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


class Datumsfestlegung:

    @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


    @staticmethod
    def make_index(unbekannten_liste):
        names = [str(s).strip() for s in unbekannten_liste]
        return names, {n: i for i, n in enumerate(names)}


    def build_G_from_names(x0, unbekannten_liste, liste_punktnummern, mit_massstab=True):
        x0 = np.asarray(x0, float).reshape(-1)
        names, idx = Datumsfestlegung.make_index(unbekannten_liste)

        u = len(names)
        d = 7 if mit_massstab else 6
        G = np.zeros((u, d), dtype=float)

        for pid in liste_punktnummern:
            sx, sy, sz = f"X{pid}", f"Y{pid}", f"Z{pid}"
            if sx not in idx or sy not in idx or sz not in idx:
                # Punkt nicht als voller XYZ-Unbekannter vorhanden -> skip
                continue

            ix, iy, iz = idx[sx], idx[sy], idx[sz]
            xi, yi, zi = x0[ix], x0[iy], x0[iz]

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

            # Rotationen (δr = ω × r)
            # Rx: δY=-Z, δZ=+Y
            G[iy, 3] = -zi; G[iz, 3] =  yi
            # Ry: δX=+Z, δZ=-X
            G[ix, 4] =  zi; G[iz, 4] = -xi
            # Rz: δX=-Y, δY=+X
            G[ix, 5] = -yi; G[iy, 5] =  xi

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


    def aktive_indices_from_selection(auswahl, unbekannten_liste):
        names, idx = Datumsfestlegung.make_index(unbekannten_liste)
        aktive = []
        for pid, comp in auswahl:
            key = f"{comp.upper()}{str(pid)}"
            if key not in idx:
                raise KeyError(f"{key} nicht im Unbekanntenvektor.")
            aktive.append(idx[key])
        # unique
        out = []
        seen = set()
        for i in aktive:
            if i not in seen:
                seen.add(i)
                out.append(i)
        return out


    def auswahlmatrix_E(u, aktive_indices):
        E = np.zeros((u, u), dtype=float)
        for i in aktive_indices:
            E[int(i), int(i)] = 1.0
        return E


    def berechne_dx_geraendert(N, n, G):
        N = np.asarray(N, float)
        n = np.asarray(n, float).reshape(-1, 1)
        G = np.asarray(G, float)

        u = N.shape[0]
        d = G.shape[1]

        K = np.block([
            [N, G],
            [G.T, np.zeros((d, d))]
        ])
        rhs = np.vstack([n, np.zeros((d, 1))])

        sol = np.linalg.solve(K, rhs)
        dx = sol[:u]
        k = sol[u:]
        return dx, k