"""
End-to-End Tests für die Pipeline gegen echte Daten.

Diese Tests rufen die Pipeline direkt auf und validieren,
dass sinnvolle Ergebnisse erzeugt werden.
"""
import json
from pathlib import Path

import pytest

from scripts import estimate_from_dir

DATA = Path(__file__).parent / "data"
SCENE_9A = DATA / "Scene9a"
ROBOT_JSON = DATA / "robot.json"
REFERENCE_POSE = SCENE_9A / "pose.json"


def _normalize_angle(angle: float) -> float:
    """Normalisiere Winkel auf [-180, 180]."""
    while angle > 180:
        angle -= 360
    while angle <= -180:
        angle += 360
    return angle


def _angle_distance(a1: float, a2: float) -> float:
    """Berechne die kürzeste Winkel-Differenz in Grad."""
    a1_norm = _normalize_angle(a1)
    a2_norm = _normalize_angle(a2)
    diff = abs(a1_norm - a2_norm)
    if diff > 180:
        diff = 360 - diff
    return diff


@pytest.mark.skipif(
    not SCENE_9A.exists(),
    reason="Scene9a Testdaten nicht vorhanden"
)
def test_estimate_from_scene9a():
    """
    E2E-Test: Scene9a-Bilder mit allen Kameras a-g,
    dazugehörige Intrinsiken und robot.json auswerten.
    
    Validiert, dass:
    - Pipeline ohne Fehler läuft
    - Alle 7 Gelenke Werte erhalten
    - Confidence-Werte für alle Gelenke vorhanden
    - Plausible numerische Ergebnisse erzeugt werden
    - Ergebnisse mit Referenzhaltung übereinstimmen (±5mm, ±5°)
    """
    result = estimate_from_dir(
        SCENE_9A,
        robot_json=ROBOT_JSON,
    )
    
    # [*] Basis-Validierungen
    assert result is not None
    assert isinstance(result.joints, dict)
    assert isinstance(result.confidence, dict)
    
    # [*] Gelenke prüfen
    expected_joints = {"x", "y", "z", "a", "b", "c", "e"}
    assert set(result.joints.keys()) == expected_joints, \
        f"Erwartet {expected_joints}, aber erhalten {set(result.joints.keys())}"
    
    # [*] Alle Werte sollten numerisch sein und nicht NaN
    for joint, value in result.joints.items():
        assert isinstance(value, (int, float)), \
            f"Gelenk {joint} sollte numerisch sein, ist aber {type(value)}"
        assert not (isinstance(value, float) and value != value), \
            f"Gelenk {joint} ist NaN"
    
    # [*] Confidence-Werte prüfen
    confidence_levels = {"high", "medium", "low", "none"}
    for joint, conf in result.confidence.items():
        assert conf in confidence_levels, \
            f"Gelenk {joint} hat ungültiges Confidence {conf}"
    
    # [*] Marker und Residuum
    assert result.n_markers > 0, "Sollte mindestens einen Marker haben"
    assert result.residual_rms >= 0, "RMS-Residuum sollte nicht negativ sein"
    assert result.processing_ms > 0, "Verarbeitung sollte Zeit brauchen"
    
    # [*] Fehlerbehandlung
    assert isinstance(result.errors, list)
    
    # [*] Vergleich mit Referenzwerten aus pose.json
    if REFERENCE_POSE.exists():
        ref_data = json.loads(REFERENCE_POSE.read_text())
        ref_position = ref_data.get("position", {})
        
        # Toleranzen: ±2mm für Linear, ±2° für Rotation
        LINEAR_TOLERANCE = 2.0  # mm
        ANGULAR_TOLERANCE = 2.0  # Grad
        
        for joint in expected_joints:
            estimated = result.joints[joint]
            reference = ref_position.get(joint)
            
            if reference is not None:
                if joint in {"x", "y", "z"}:
                    # Linear-Achsen: Toleranz ±5mm
                    diff = abs(estimated - reference)
                    assert diff <= LINEAR_TOLERANCE, \
                        f"Gelenk {joint}: Abweichung {diff:.2f}mm > {LINEAR_TOLERANCE}mm " \
                        f"(Referenz: {reference}, Gemessen: {estimated:.2f})"
                else:
                    # Rotations-Achsen: Toleranz ±5°
                    angle_diff = _angle_distance(estimated, reference)
                    assert angle_diff <= ANGULAR_TOLERANCE, \
                        f"Gelenk {joint}: Abweichung {angle_diff:.2f}° > {ANGULAR_TOLERANCE}° " \
                        f"(Referenz: {reference}°, Gemessen: {estimated:.2f}°)"
    
    # [OK] Ausgeben zur manuellen Validierung
    print(f"\n[OK] Scene9a erfolgreich verarbeitet")
    print(f"  Gelenke: {result.joints}")
    print(f"  Confidence: {result.confidence}")
    print(f"  Marker: {result.n_markers}, RMS: {result.residual_rms:.3f} mm")
    print(f"  Zeit: {result.processing_ms} ms")