#!/usr/bin/env python3
"""
Debug-Skript für Netzausgleichung
Prüft Punktevergleich und findet Fehler
"""

import sys
import math
sys.path.insert(0, '/home/ubuntu/trimble_geodesy')

from modules.jxl_parser import JXLParser
from modules.network_adjustment import NetworkAdjustment

def main():
    # Lade JXL
    jxl_path = "/home/ubuntu/trimble_geodesy/test_data/Baumschulenstr_93.jxl"
    parser = JXLParser()
    parser.parse(jxl_path)
    
    print("=" * 80)
    print("DEBUG: NETZAUSGLEICHUNG")
    print("=" * 80)
    
    # Zeige Winkeleinheit
    print(f"\n### JXL Einstellungen:")
    print(f"  Winkeleinheit: {parser.angle_units}")
    
    # NetworkAdjustment initialisieren
    adj = NetworkAdjustment(parser)
    
    # Beobachtungen extrahieren
    adj.extract_observations()
    adj.initialize_points()
    
    # Debug: Zeige Beobachtungen
    print(f"\n### Beobachtungen ({len(adj.observations)} total):")
    dir_obs = [o for o in adj.observations if o.obs_type == 'direction']
    dist_obs = [o for o in adj.observations if o.obs_type == 'distance']
    zen_obs = [o for o in adj.observations if o.obs_type == 'zenith']
    
    print(f"  Richtungen: {len(dir_obs)}")
    print(f"  Strecken: {len(dist_obs)}")
    print(f"  Zenitwinkel: {len(zen_obs)}")
    
    print("\n### Beispiel-Beobachtungen (erste 5 Richtungen):")
    for obs in dir_obs[:5]:
        print(f"  {obs.from_station} -> {obs.to_point}: {obs.value:.6f} gon (std: {obs.std_dev:.6f})")
    
    print("\n### Beispiel-Beobachtungen (erste 5 Strecken):")
    for obs in dist_obs[:5]:
        print(f"  {obs.from_station} -> {obs.to_point}: {obs.value:.4f} m (std: {obs.std_dev:.6f})")
    
    # Koordinaten vor Ausgleichung speichern
    coords_before = {}
    for name, pt in adj.points.items():
        coords_before[name] = (pt.x, pt.y, pt.z)
    
    print(f"\n### Punkte ({len(adj.points)} total):")
    for name, pt in sorted(adj.points.items()):
        print(f"  {name}: X={pt.x:.4f}, Y={pt.y:.4f}, Z={pt.z:.4f}")
    
    # Festpunkte setzen
    ref_points = parser.get_reference_points()
    print(f"\n### Referenzpunkte (5xxx): {ref_points}")
    
    for name in ref_points:
        adj.set_fixed_point(name)
    
    # Falls keine Referenzpunkte, erste Station als Festpunkt
    if not adj.fixed_points:
        print("WARNUNG: Keine Referenzpunkte gefunden!")
        stations = list(parser.stations.values())
        if stations:
            first_station = min(stations, key=lambda s: s.timestamp)
            adj.set_fixed_point(first_station.name)
            print(f"  -> Setze {first_station.name} als Festpunkt")
    
    print(f"\n### Festpunkte: {adj.fixed_points}")
    
    # Zeige berechnete Orientierungen
    print(f"\n### Berechnete Orientierungen pro Station:")
    for station_name, orientation in sorted(adj.orientations.items()):
        print(f"   {station_name}: {orientation:.4f} gon")
    
    # Konsistenzprüfung
    print("\n" + "=" * 80)
    print("KONSISTENZPRÜFUNG")
    print("=" * 80)
    
    consistency = adj.check_consistency()
    print(f"\n### Ergebnis: {'✅ KONSISTENT' if consistency['consistent'] else '❌ INKONSISTENT'}")
    
    if not consistency['consistent']:
        print("\n### Probleme gefunden:")
        for issue in consistency['issues']:
            print(f"   ⚠️ {issue}")
        
        print("\n### WICHTIG:")
        print("   Die Koordinaten in der JXL-Datei wurden bereits von Trimble Access")
        print("   berechnet und sind fertig. Die Beobachtungen (Kreislesungen) sind")
        print("   rohe Messwerte, die nicht mit den berechneten Koordinaten konsistent")
        print("   verglichen werden können, da die Orientierungen stark variieren.")
        print("")
        print("   Eine Netzausgleichung mit diesen Daten ist nicht sinnvoll, da sie")
        print("   die bereits korrekten Koordinaten verschlechtern würde.")
        print("")
        print("   Empfehlung: Verwenden Sie die ausgeglichenen Koordinaten aus der JXL")
        print("   direkt, ohne weitere Ausgleichung.")
    
    # Ausgleichung durchführen (nur Residuen, keine Koordinatenänderung)
    print("\n### Starte Residuenanalyse (keine Koordinatenänderung)...")
    try:
        result = adj.adjust(mode="residuals_only")
        
        print(f"\n### Ergebnis:")
        print(f"  Konvergiert: {result.converged}")
        print(f"  Iterationen: {result.iterations}")
        print(f"  Sigma-0 posteriori: {result.sigma_0_posteriori:.6f}")
        print(f"  Redundanz: {result.redundancy}")
        
        # PUNKTEVERGLEICH
        print("\n" + "=" * 100)
        print("PUNKTEVERGLEICH: VORHER vs. NACHHER")
        print("=" * 100)
        print(f"{'Punkt':<10} {'X_vorher':>12} {'Y_vorher':>12} {'Z_vorher':>10} | {'X_nachher':>12} {'Y_nachher':>12} {'Z_nachher':>10} | {'ΔX':>10} {'ΔY':>10} {'ΔZ':>10} {'Δ3D':>10}")
        print("-" * 120)
        
        deviations = []
        for name, pt in sorted(adj.points.items()):
            x_before, y_before, z_before = coords_before[name]
            x_after, y_after, z_after = pt.x, pt.y, pt.z
            
            dx = x_after - x_before
            dy = y_after - y_before
            dz = z_after - z_before
            d3d = math.sqrt(dx**2 + dy**2 + dz**2)
            
            deviations.append((name, dx, dy, dz, d3d, pt.is_fixed))
            
            fixed_marker = "*" if pt.is_fixed else " "
            print(f"{name:<10}{fixed_marker} {x_before:>12.4f} {y_before:>12.4f} {z_before:>10.4f} | "
                  f"{x_after:>12.4f} {y_after:>12.4f} {z_after:>10.4f} | "
                  f"{dx:>10.4f} {dy:>10.4f} {dz:>10.4f} {d3d:>10.4f}")
        
        # PLAUSIBILITÄTSPRÜFUNG
        print("\n" + "=" * 80)
        print("PLAUSIBILITÄTSPRÜFUNG")
        print("=" * 80)
        
        # Sortiert nach größter Abweichung
        deviations_sorted = sorted(deviations, key=lambda x: x[4], reverse=True)
        
        # Warnungen
        critical_errors = []
        warnings = []
        for name, dx, dy, dz, d3d, is_fixed in deviations_sorted:
            if not is_fixed:
                if d3d > 1.0:
                    critical_errors.append((name, d3d))
                elif d3d > 0.1:
                    warnings.append((name, d3d))
        
        if critical_errors:
            print("\n🚨 KRITISCHE FEHLER (Abweichung > 1m):")
            for name, d3d in critical_errors:
                print(f"  ❌ {name}: {d3d:.4f} m")
        
        if warnings:
            print("\n⚠️ WARNUNGEN (Abweichung > 10cm):")
            for name, d3d in warnings:
                print(f"  ⚠ {name}: {d3d:.4f} m")
        
        # Statistik
        non_fixed_deviations = [d for d in deviations if not d[5]]
        if non_fixed_deviations:
            d3d_values = [d[4] for d in non_fixed_deviations]
            dx_values = [abs(d[1]) for d in non_fixed_deviations]
            dy_values = [abs(d[2]) for d in non_fixed_deviations]
            
            print("\n### Statistik (nur Neupunkte):")
            print(f"  Anzahl Punkte: {len(non_fixed_deviations)}")
            print(f"  Min Δ3D: {min(d3d_values):.6f} m")
            print(f"  Max Δ3D: {max(d3d_values):.6f} m")
            print(f"  Mittel Δ3D: {sum(d3d_values)/len(d3d_values):.6f} m")
            print(f"  Max |ΔX|: {max(dx_values):.6f} m")
            print(f"  Max |ΔY|: {max(dy_values):.6f} m")
        
        if not critical_errors and not warnings:
            print("\n✅ Alle Koordinaten unverändert (wie erwartet bei residuals_only)")
        
        # RESIDUEN-ANALYSE
        print("\n" + "=" * 80)
        print("RESIDUENANALYSE")
        print("=" * 80)
        
        dir_obs = [o for o in adj.observations if o.obs_type == 'direction']
        dist_obs = [o for o in adj.observations if o.obs_type == 'distance']
        
        dir_residuals = [o.residual * 1000 for o in dir_obs]  # in mgon
        dist_residuals = [o.residual * 1000 for o in dist_obs]  # in mm
        
        if dir_residuals:
            print(f"\n### Richtungsresiduen (in mgon):")
            print(f"   Anzahl: {len(dir_residuals)}")
            print(f"   Min: {min(dir_residuals):.2f} mgon")
            print(f"   Max: {max(dir_residuals):.2f} mgon")
            print(f"   RMSE: {math.sqrt(sum(r**2 for r in dir_residuals)/len(dir_residuals)):.2f} mgon")
        
        if dist_residuals:
            print(f"\n### Streckenresiduen (in mm):")
            print(f"   Anzahl: {len(dist_residuals)}")
            print(f"   Min: {min(dist_residuals):.2f} mm")
            print(f"   Max: {max(dist_residuals):.2f} mm")
            print(f"   RMSE: {math.sqrt(sum(r**2 for r in dist_residuals)/len(dist_residuals)):.2f} mm")
        
        # Zeige größte Residuen
        print(f"\n### Größte Richtungsresiduen:")
        sorted_dir = sorted(dir_obs, key=lambda x: abs(x.residual), reverse=True)[:5]
        for obs in sorted_dir:
            print(f"   {obs.from_station} -> {obs.to_point}: {obs.residual*1000:.2f} mgon")
        
        print(f"\n### Größte Streckenresiduen:")
        sorted_dist = sorted(dist_obs, key=lambda x: abs(x.residual), reverse=True)[:5]
        for obs in sorted_dist:
            print(f"   {obs.from_station} -> {obs.to_point}: {obs.residual*1000:.2f} mm")
        
        # Qualitätsbewertung
        rmse_dir = math.sqrt(sum(r**2 for r in dir_residuals)/len(dir_residuals)) if dir_residuals else 0
        rmse_dist = math.sqrt(sum(r**2 for r in dist_residuals)/len(dist_residuals)) if dist_residuals else 0
        
        print("\n### Qualitätsbewertung:")
        if rmse_dir < 10:
            print(f"   ✅ Richtungen: SEHR GUT (RMSE < 10 mgon)")
        elif rmse_dir < 50:
            print(f"   ✅ Richtungen: GUT (RMSE < 50 mgon)")
        elif rmse_dir < 100:
            print(f"   ⚠️ Richtungen: AKZEPTABEL (RMSE < 100 mgon)")
        else:
            print(f"   ❌ Richtungen: SCHLECHT (RMSE = {rmse_dir:.2f} mgon)")
        
        if rmse_dist < 5:
            print(f"   ✅ Strecken: SEHR GUT (RMSE < 5 mm)")
        elif rmse_dist < 20:
            print(f"   ✅ Strecken: GUT (RMSE < 20 mm)")
        elif rmse_dist < 50:
            print(f"   ⚠️ Strecken: AKZEPTABEL (RMSE < 50 mm)")
        else:
            print(f"   ❌ Strecken: SCHLECHT (RMSE = {rmse_dist:.2f} mm)")
        
        # Prüfe ob Problem besteht
        max_d3d = max(d[4] for d in non_fixed_deviations) if non_fixed_deviations else 0
        if max_d3d > 1.0:
            print("\n" + "=" * 80)
            print("🔍 FEHLERANALYSE")
            print("=" * 80)
            
            # Prüfe berechnete vs. beobachtete Richtungen
            print("\n### Prüfung Richtungsbeobachtungen:")
            for obs in dir_obs[:3]:
                from_pt = adj.points.get(obs.from_station)
                to_pt = adj.points.get(obs.to_point)
                if from_pt and to_pt:
                    dx = to_pt.x - from_pt.x
                    dy = to_pt.y - from_pt.y
                    # Berechne Azimut in Gon (aus X,Y)
                    azimuth_calc = math.atan2(dx, dy) * 200.0 / math.pi
                    if azimuth_calc < 0:
                        azimuth_calc += 400.0
                    
                    diff = obs.value - azimuth_calc
                    while diff > 200:
                        diff -= 400
                    while diff < -200:
                        diff += 400
                    
                    print(f"  {obs.from_station} -> {obs.to_point}:")
                    print(f"    Beobachtet: {obs.value:.6f} gon")
                    print(f"    Berechnet:  {azimuth_calc:.6f} gon")
                    print(f"    Differenz:  {diff:.6f} gon ({diff*10000:.2f} mgon)")
            
            # Prüfe berechnete vs. beobachtete Strecken
            print("\n### Prüfung Streckenbeobachtungen:")
            for obs in dist_obs[:3]:
                from_pt = adj.points.get(obs.from_station)
                to_pt = adj.points.get(obs.to_point)
                if from_pt and to_pt:
                    dx = to_pt.x - from_pt.x
                    dy = to_pt.y - from_pt.y
                    dz = to_pt.z - from_pt.z
                    dist_calc_3d = math.sqrt(dx**2 + dy**2 + dz**2)
                    dist_calc_2d = math.sqrt(dx**2 + dy**2)
                    
                    diff_3d = obs.value - dist_calc_3d
                    diff_2d = obs.value - dist_calc_2d
                    
                    print(f"  {obs.from_station} -> {obs.to_point}:")
                    print(f"    Beobachtet: {obs.value:.6f} m")
                    print(f"    Berechnet 3D: {dist_calc_3d:.6f} m (Diff: {diff_3d*1000:.2f} mm)")
                    print(f"    Berechnet 2D: {dist_calc_2d:.6f} m (Diff: {diff_2d*1000:.2f} mm)")
        
    except Exception as e:
        print(f"FEHLER: {e}")
        import traceback
        traceback.print_exc()

if __name__ == "__main__":
    main()