import sympy as sp
from typing import List, Iterable, Tuple

def raenderungsmatrix_G(
    x0: sp.Matrix,
    idx_X: List[int],
    idx_Y: List[int],
    idx_Z: List[int],
    mit_massstab: bool = True,
    ) -> sp.Matrix:

    u = x0.rows
    d = 7 if mit_massstab else 6
    G = sp.zeros(u, d)

    # --- Translationen ---
    for i in idx_X:
        G[i, 0] = 1
    for i in idx_Y:
        G[i, 1] = 1
    for i in idx_Z:
        G[i, 2] = 1

    # --- Rotationen ---
    # Rotation um X-Achse
    for iy, iz in zip(idx_Y, idx_Z):
        zi = x0[iz, 0]
        yi = x0[iy, 0]
        G[iy, 3] = -zi
        G[iz, 3] = yi

    # Rotation um Y-Achse
    for ix, iz in zip(idx_X, idx_Z):
        zi = x0[iz, 0]
        xi = x0[ix, 0]
        G[ix, 4] = zi
        G[iz, 4] = -xi

    # Rotation um Z-Achse
    for ix, iy in zip(idx_X, idx_Y):
        yi = x0[iy, 0]
        xi = x0[ix, 0]
        G[ix, 5] = -yi
        G[iy, 5] = xi

    # --- Maßstab ---
    if mit_massstab:
        for ix, iy, iz in zip(idx_X, idx_Y, idx_Z):
            xi = x0[ix, 0]
            yi = x0[iy, 0]
            zi = x0[iz, 0]
            G[ix, 6] = xi
            G[iy, 6] = yi
            G[iz, 6] = zi
    return G


def auswahlmatrix_E(u: int, aktive_unbekannte_indices: Iterable[int]) -> sp.Matrix:
    E = sp.zeros(u, u)
    for idx in aktive_unbekannte_indices:
        E[int(idx), int(idx)] = 1
    return E


def teilspurminimierung_Gi(G: sp.Matrix, E: sp.Matrix) -> sp.Matrix:
    Gi = E * G
    return Gi


def berechne_dx_geraendert(N: sp.Matrix, n: sp.Matrix, Gi: sp.Matrix) -> sp.Matrix:
    u = N.rows
    d = Gi.shape[1]

    K = N.row_join(Gi)
    K = K.col_join(Gi.T.row_join(sp.zeros(d, d)))

    rhs = n.col_join(sp.zeros(d, 1))

    sol = K.LUsolve(rhs)
    dx = sol[:u, :]
    return dx