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


class Datumsfestlegung:

    @staticmethod
    def datumskomponenten(
        auswahl: Iterable[Tuple[str, str]],
        liste_punktnummern: Sequence[str],
        *,
        layout: str = "XYZ"
    ) -> List[int]:
        punkt2pos = {str(p): i for i, p in enumerate(liste_punktnummern)}

        layout = layout.upper()
        if layout != "XYZ":
            raise ValueError("Nur layout='XYZ' unterstützt (wie bei euch).")
        comp2off = {"X": 0, "Y": 1, "Z": 2}

        aktive: List[int] = []
        for pt, comp in auswahl:
            spt = str(pt)
            c = comp.upper()
            if spt not in punkt2pos:
                raise KeyError(f"Punkt '{pt}' nicht in liste_punktnummern.")
            if c not in comp2off:
                raise ValueError(f"Komponente '{comp}' ungültig. Nur X,Y,Z.")
            p = punkt2pos[spt]
            aktive.append(3 * p + comp2off[c])

        # Duplikate entfernen
        out, seen = [], set()
        for i in aktive:
            if i not in seen:
                seen.add(i)
                out.append(i)
        return out

    @staticmethod
    def auswahlmatrix_E(u: int, aktive_unbekannte_indices: Iterable[int]) -> sp.Matrix:
        E = sp.zeros(u, u)
        for idx in aktive_unbekannte_indices:
            i = int(idx)
            if not (0 <= i < u):
                raise IndexError(f"Aktiver Index {i} außerhalb [0,{u-1}]")
            E[i, i] = 1
        return E

    @staticmethod
    def raenderungsmatrix_G(
        x0: sp.Matrix,
        liste_punktnummern: Sequence[str],
        *,
        mit_massstab: bool = True,
        layout: str = "XYZ",
    ) -> sp.Matrix:
        if x0.cols != 1:
            raise ValueError("x0 muss Spaltenvektor sein.")
        layout = layout.upper()
        if layout != "XYZ":
            raise ValueError("Nur layout='XYZ' unterstützt (wie bei euch).")

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

        for p in range(nP):
            ix, iy, iz = 3*p, 3*p+1, 3*p+2
            xi, yi, zi = x0[ix, 0], x0[iy, 0], x0[iz, 0]

            # Translationen
            G[ix, 0] = 1
            G[iy, 1] = 1
            G[iz, 2] = 1

            # Rotationen
            G[iy, 3] = -zi; G[iz, 3] =  yi     # Rx
            G[ix, 4] =  zi; G[iz, 4] = -xi     # Ry
            G[ix, 5] = -yi; G[iy, 5] =  xi     # Rz

            # Maßstab
            if mit_massstab:
                G[ix, 6] = xi
                G[iy, 6] = yi
                G[iz, 6] = zi

        return G

    @staticmethod
    def berechne_dx_geraendert(N: sp.Matrix, n: sp.Matrix, Gi: sp.Matrix) -> sp.Matrix:
        if N.rows != N.cols:
            raise ValueError("N muss quadratisch sein.")
        if n.cols != 1:
            raise ValueError("n muss Spaltenvektor sein.")
        if Gi.rows != N.rows:
            raise ValueError("Gi hat falsche Zeilenzahl.")

        u = N.rows
        d = Gi.cols
        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)
        return sol[:u, :]

    @staticmethod
    def weiches_datum(
        A: sp.Matrix,
        dl: sp.Matrix,
        Q_ll: sp.Matrix,
        x0: sp.Matrix,
        anschluss_indices: Sequence[int],
        anschluss_werte: sp.Matrix,
        Sigma_AA: Optional[sp.Matrix] = None,
    ) -> Tuple[sp.Matrix, sp.Matrix, sp.Matrix]:
        if dl.cols != 1 or x0.cols != 1:
            raise ValueError("dl und x0 müssen Spaltenvektoren sein.")
        if A.rows != dl.rows:
            raise ValueError("A.rows muss dl.rows entsprechen.")
        if A.cols != x0.rows:
            raise ValueError("A.cols muss x0.rows entsprechen.")
        if Q_ll.rows != Q_ll.cols or Q_ll.rows != A.rows:
            raise ValueError("Q_ll muss (n×n) sein und zu A.rows passen.")

        u = A.cols
        idx = [int(i) for i in anschluss_indices]
        m = len(idx)

        if anschluss_werte.cols != 1 or anschluss_werte.rows != m:
            raise ValueError("anschluss_werte muss (m×1) sein.")
        if Sigma_AA is None:
            Sigma_AA = sp.eye(m)
        if Sigma_AA.rows != m or Sigma_AA.cols != m:
            raise ValueError("Sigma_AA muss (m×m) sein.")

        A_A = sp.zeros(m, u)
        for r, j in enumerate(idx):
            if not (0 <= j < u):
                raise IndexError(f"Anschluss-Index {j} außerhalb [0,{u-1}]")
            A_A[r, j] = 1

        x0_A = sp.Matrix([[x0[j, 0]] for j in idx])
        dl_A = anschluss_werte - x0_A

        A_ext = A.col_join(A_A)
        dl_ext = dl.col_join(dl_A)

        Q_ext = sp.zeros(Q_ll.rows + m, Q_ll.cols + m)
        Q_ext[:Q_ll.rows, :Q_ll.cols] = Q_ll
        Q_ext[Q_ll.rows:, Q_ll.cols:] = Sigma_AA

        return A_ext, dl_ext, Q_ext