import sympy as sp
import numpy as np
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(Q_ll: np.ndarray, Q_AA: np.ndarray) -> np.ndarray:
        if Q_ll.ndim != 2 or Q_ll.shape[0] != Q_ll.shape[1]:
            raise ValueError("Q_ll muss quadratisch sein.")
        if Q_AA.ndim != 2 or Q_AA.shape[0] != Q_AA.shape[1]:
            raise ValueError("Q_AA muss quadratisch sein.")
        Q_ext = np.block([[Q_ll, np.zeros((Q_ll.shape[0], Q_AA.shape[0]))],[np.zeros((Q_AA.shape[0], Q_ll.shape[0])), Q_AA]])
        return Q_ext