# ruff: noqa: T201, PLR2004
"""Visualizations for hybrid AOA+TDOA geolocation Monte Carlo validation.

Demonstrates two key scenarios:
1. Overconstrained: 3 AOA + 6 TDOA (from 4 satellites) - shows hybrid is better
   conditioned than either observable alone. Both AOA and TDOA are individually
   overconstrained, allowing meaningful comparison.
2. Underconstrained: 2 AOA + 2 TDOA - shows we can get good solutions when
   neither observable alone would suffice (2 AOA has poor range observability,
   2 TDOA is insufficient for 3D positioning)

Run with:
    python -m gri_sandbox.plots.hybrid_monte_carlo
"""

from __future__ import annotations

import numpy as np
import plotly.graph_objects as go
from gri_geosim.geolocation import (
    aoa_locator,
    tdoa_aoa_locator,
    tdoa_locator,
)
from gri_utils.observables import get_aoa_quat, get_tdoa
from plotly.subplots import make_subplots
from scipy.spatial.transform import Rotation
from scipy.stats import chi2, gaussian_kde, kstest

EARTH_RADIUS = 6378137.0
SPEED_OF_LIGHT = 299792458.0
LEO_ALTITUDE = 550_000.0  # 550 km LEO altitude (Starlink-like)
HEO_ALTITUDE = 20_000_000.0  # 20,000 km HEO altitude (GPS-like, half of GEO)

DARK_TEMPLATE = "plotly_dark"


def _satellite_position(
    lat_deg: float,
    lon_deg: float,
    altitude: float = LEO_ALTITUDE,
) -> np.ndarray:
    """Compute satellite position from lat/lon over Earth at given altitude.

    Args:
        lat_deg: Latitude in degrees (-90 to 90).
        lon_deg: Longitude in degrees (-180 to 180).
        altitude: Altitude above Earth's surface in meters.

    Returns:
        ECEF position of satellite at the specified altitude above the point.
    """
    lat = np.radians(lat_deg)
    lon = np.radians(lon_deg)
    r = EARTH_RADIUS + altitude

    return np.array(
        [
            r * np.cos(lat) * np.cos(lon),
            r * np.cos(lat) * np.sin(lon),
            r * np.sin(lat),
        ],
    )


def _leo_satellite_position(lat_deg: float, lon_deg: float) -> np.ndarray:
    """Compute LEO satellite position from lat/lon over Earth.

    Args:
        lat_deg: Latitude in degrees (-90 to 90).
        lon_deg: Longitude in degrees (-180 to 180).

    Returns:
        ECEF position of satellite at LEO_ALTITUDE above the point.
    """
    return _satellite_position(lat_deg, lon_deg, LEO_ALTITUDE)


def _heo_satellite_position(lat_deg: float, lon_deg: float) -> np.ndarray:
    """Compute HEO satellite position from lat/lon over Earth.

    HEO (High Earth Orbit) provides wider baselines for TDOA measurements,
    improving range-difference geometry. Uses ~20,000 km altitude (GPS-like).

    Args:
        lat_deg: Latitude in degrees (-90 to 90).
        lon_deg: Longitude in degrees (-180 to 180).

    Returns:
        ECEF position of satellite at HEO_ALTITUDE above the point.
    """
    return _satellite_position(lat_deg, lon_deg, HEO_ALTITUDE)


def _ground_target_position(lat_deg: float, lon_deg: float) -> np.ndarray:
    """Compute ground target position from lat/lon.

    Args:
        lat_deg: Latitude in degrees.
        lon_deg: Longitude in degrees.

    Returns:
        ECEF position on Earth's surface.
    """
    lat = np.radians(lat_deg)
    lon = np.radians(lon_deg)

    return np.array(
        [
            EARTH_RADIUS * np.cos(lat) * np.cos(lon),
            EARTH_RADIUS * np.cos(lat) * np.sin(lon),
            EARTH_RADIUS * np.sin(lat),
        ],
    )


def _create_overconstrained_geometry() -> dict:
    """Create geometry for overconstrained scenario (3 AOA + 6 TDOA).

    Uses realistic orbit assignments:
    - AOA from LEO (~550 km): Closer satellites provide better angular resolution
    - TDOA from HEO (~20,000 km): Higher orbits provide wider baselines for
      range-difference measurements

    Target on ground at lat=30, lon=0.

    TDOA constraint counting:
    - 4 satellites → C(4,2) = 6 pairs
    - 3 unknowns (position) → 6 TDOA measurements is overconstrained
    - Combined with 3 AOA (6 measurements) → 12 total for 3 unknowns

    Returns:
        Dictionary with all geometry parameters.
    """
    # Target on Earth's surface
    target_xyz = _ground_target_position(lat_deg=30.0, lon_deg=0.0)

    # 3 AOA LEO satellites - spread around target with lat/lon diversity
    # LEO provides good angular resolution for direction-finding
    aoa_collector_xyz = np.array(
        [
            _leo_satellite_position(lat_deg=40.0, lon_deg=-10.0),
            _leo_satellite_position(lat_deg=25.0, lon_deg=15.0),
            _leo_satellite_position(lat_deg=35.0, lon_deg=5.0),
        ],
    )
    aoa_rotations = Rotation.identity(3)

    # 4 TDOA HEO satellites with north/south diversity
    # HEO provides wide baselines for range-difference geometry
    tdoa_satellites = np.array(
        [
            _heo_satellite_position(lat_deg=45.0, lon_deg=-5.0),  # North
            _heo_satellite_position(lat_deg=-20.0, lon_deg=-15.0),  # South
            _heo_satellite_position(lat_deg=30.0, lon_deg=20.0),  # Near equator
            _heo_satellite_position(lat_deg=-35.0, lon_deg=5.0),  # South
        ],
    )

    # All 6 TDOA pairs from 4 satellites: C(4,2) = 6
    # This makes TDOA truly overconstrained (6 measurements for 3 unknowns)
    pairs = [(0, 1), (0, 2), (0, 3), (1, 2), (1, 3), (2, 3)]
    tdoa_collector1_xyz = np.array([tdoa_satellites[i] for i, _ in pairs])
    tdoa_collector2_xyz = np.array([tdoa_satellites[j] for _, j in pairs])

    return {
        "target_xyz": target_xyz,
        "aoa_collector_xyz": aoa_collector_xyz,
        "aoa_rotations": aoa_rotations,
        "tdoa_collector1_xyz": tdoa_collector1_xyz,
        "tdoa_collector2_xyz": tdoa_collector2_xyz,
    }


def _create_underconstrained_geometry_2aoa_2tdoa() -> dict:
    """Create geometry for underconstrained scenario: 2 AOA + 2 TDOA.

    Uses realistic orbit assignments with geometry designed so TDOA hyperboloids
    and AOA bearings are NOT aligned:

    - AOA from LEO (~550 km): Closer satellites for better angular resolution
    - TDOA from HEO (~20,000 km): Higher orbits for wider baselines

    - Target at equator/prime meridian (lat=0, lon=0)
    - TDOA HEO satellites:
      - Sat 1: directly overhead
      - Sat 2: offset east
      - Sat 3: offset north (with some south for N/S diversity)
      - TDOAs (1-2) and (1-3) create hyperboloids intersecting in a curved
        line with ambiguous "altitude" (radial distance from Earth center)
    - AOA LEO satellites:
      - South of overhead: provides bearing from south
      - West of overhead: provides bearing from west
      - These resolve the altitude ambiguity that TDOA alone cannot fix

    Neither 2 AOA nor 2 TDOA alone provides 3D observability, but combined
    they do because the constraints are geometrically independent.

    Returns:
        Dictionary with all geometry parameters.
    """
    # Target at equator, prime meridian
    target_xyz = _ground_target_position(lat_deg=0.0, lon_deg=0.0)

    # Angular offset for satellite positions
    # HEO satellites need larger angular separation due to higher altitude
    heo_offset_deg = 15.0
    leo_offset_deg = 5.0

    # TDOA HEO satellites: overhead, east, north (with south diversity)
    # These create hyperboloids that intersect along a line with ambiguous altitude
    tdoa_sat1 = _heo_satellite_position(lat_deg=0.0, lon_deg=0.0)  # Overhead
    tdoa_sat2 = _heo_satellite_position(lat_deg=0.0, lon_deg=heo_offset_deg)  # East
    tdoa_sat3 = _heo_satellite_position(lat_deg=-heo_offset_deg, lon_deg=0.0)  # South

    # TDOA pairs: (1,2) and (1,3) - both use overhead as reference
    tdoa_collector1_xyz = np.array([tdoa_sat1, tdoa_sat1])
    tdoa_collector2_xyz = np.array([tdoa_sat2, tdoa_sat3])

    # AOA LEO satellites: south and west of overhead
    # These provide bearing constraints orthogonal to the TDOA ambiguity
    # LEO provides better angular resolution for direction-finding
    aoa_collector_xyz = np.array(
        [
            _leo_satellite_position(lat_deg=-leo_offset_deg, lon_deg=0.0),  # South
            _leo_satellite_position(lat_deg=0.0, lon_deg=-leo_offset_deg),  # West
        ],
    )
    aoa_rotations = Rotation.identity(2)

    return {
        "target_xyz": target_xyz,
        "aoa_collector_xyz": aoa_collector_xyz,
        "aoa_rotations": aoa_rotations,
        "tdoa_collector1_xyz": tdoa_collector1_xyz,
        "tdoa_collector2_xyz": tdoa_collector2_xyz,
    }


def run_hybrid_monte_carlo(
    geometry: dict,
    aoa_sigma: float = 0.0005,  # 0.5 mrad (balanced with 1μs TDOA for ~equal RMS)
    tdoa_sigma: float = 1e-6,  # 1 μs (matches tdoa_monte_carlo)
    n_trials: int = 1000,
    seed: int = 42,
) -> dict:
    """Run Monte Carlo simulation for hybrid geolocation.

    Args:
        geometry: Dictionary with collector positions and target.
        aoa_sigma: AOA measurement noise (1-sigma, radians).
        tdoa_sigma: TDOA measurement noise (1-sigma, seconds).
        n_trials: Number of Monte Carlo trials.
        seed: Random seed for reproducibility.

    Returns:
        Dictionary with simulation results.
    """
    rng = np.random.default_rng(seed)

    target_true = geometry["target_xyz"]
    aoa_collector_xyz = geometry["aoa_collector_xyz"]
    aoa_rotations = geometry["aoa_rotations"]
    tdoa_collector1_xyz = geometry["tdoa_collector1_xyz"]
    tdoa_collector2_xyz = geometry["tdoa_collector2_xyz"]

    n_aoa = len(aoa_collector_xyz)
    n_tdoa = len(tdoa_collector1_xyz)

    # Compute true measurements
    true_aoa = get_aoa_quat(aoa_collector_xyz, target_true, aoa_rotations)
    true_tdoa = np.array(
        [
            get_tdoa(target_true, tdoa_collector1_xyz[i], tdoa_collector2_xyz[i])
            for i in range(n_tdoa)
        ],
    )

    # Covariances
    aoa_cov = np.eye(2) * aoa_sigma**2
    aoa_covariances = np.tile(aoa_cov, (n_aoa, 1, 1))
    tdoa_variances = np.full(n_tdoa, tdoa_sigma**2)

    normalized_miss_sq = []
    position_errors = []
    covariance_traces = []
    singular_count = 0

    for _ in range(n_trials):
        # Add noise
        aoa_noise = rng.standard_normal((n_aoa, 2)) * aoa_sigma
        noisy_aoa = true_aoa + aoa_noise

        tdoa_noise = rng.standard_normal(n_tdoa) * tdoa_sigma
        noisy_tdoa = true_tdoa + tdoa_noise

        # Solve
        result = tdoa_aoa_locator(
            aoa_collector_xyz=aoa_collector_xyz,
            aoa_rotations=aoa_rotations,
            aoa_measurements=noisy_aoa,
            aoa_covariances=aoa_covariances,
            tdoa_collector1_xyz=tdoa_collector1_xyz,
            tdoa_collector2_xyz=tdoa_collector2_xyz,
            tdoa_measurements=noisy_tdoa,
            tdoa_variances=tdoa_variances,
        )

        delta = result.position - target_true
        position_errors.append(delta)
        covariance_traces.append(np.trace(result.covariance))

        # Check condition number to detect ill-conditioned covariances
        eigvals = np.linalg.eigvalsh(result.covariance)
        if eigvals.min() < 1e-10 * eigvals.max():
            # Near-singular covariance - skip d² calculation
            singular_count += 1
            normalized_miss_sq.append(np.nan)
        else:
            cov_inv = np.linalg.inv(result.covariance)
            d_sq = delta @ cov_inv @ delta
            normalized_miss_sq.append(d_sq)

    return {
        "normalized_miss_sq": np.array(normalized_miss_sq),
        "position_errors": np.array(position_errors),
        "covariance_traces": np.array(covariance_traces),
        "singular_count": singular_count,
        "target_true": target_true,
        "n_aoa": n_aoa,
        "n_tdoa": n_tdoa,
    }


def run_comparison_monte_carlo(
    geometry: dict,
    aoa_sigma: float = 0.0005,  # 0.5 mrad (balanced with 1μs TDOA for ~equal RMS)
    tdoa_sigma: float = 1e-6,  # 1 μs (matches tdoa_monte_carlo)
    n_trials: int = 500,
    seed: int = 42,
) -> dict:
    """Run Monte Carlo comparing hybrid vs AOA-only vs TDOA-only.

    Only valid for overconstrained case where both standalone solvers work.

    Args:
        geometry: Dictionary with collector positions and target.
        aoa_sigma: AOA measurement noise (1-sigma, radians).
        tdoa_sigma: TDOA measurement noise (1-sigma, seconds).
        n_trials: Number of Monte Carlo trials.
        seed: Random seed for reproducibility.

    Returns:
        Dictionary with comparison results.
    """
    rng = np.random.default_rng(seed)

    target_true = geometry["target_xyz"]
    aoa_collector_xyz = geometry["aoa_collector_xyz"]
    aoa_rotations = geometry["aoa_rotations"]
    tdoa_collector1_xyz = geometry["tdoa_collector1_xyz"]
    tdoa_collector2_xyz = geometry["tdoa_collector2_xyz"]

    n_aoa = len(aoa_collector_xyz)
    n_tdoa = len(tdoa_collector1_xyz)

    # Compute true measurements
    true_aoa = get_aoa_quat(aoa_collector_xyz, target_true, aoa_rotations)
    true_tdoa = np.array(
        [
            get_tdoa(target_true, tdoa_collector1_xyz[i], tdoa_collector2_xyz[i])
            for i in range(n_tdoa)
        ],
    )

    # Covariances
    aoa_cov = np.eye(2) * aoa_sigma**2
    aoa_covariances = np.tile(aoa_cov, (n_aoa, 1, 1))
    tdoa_variances = np.full(n_tdoa, tdoa_sigma**2)

    results = {
        "hybrid": {"errors": [], "traces": []},
        "aoa_only": {"errors": [], "traces": []},
        "tdoa_only": {"errors": [], "traces": []},
    }

    for _ in range(n_trials):
        # Add noise
        aoa_noise = rng.standard_normal((n_aoa, 2)) * aoa_sigma
        noisy_aoa = true_aoa + aoa_noise

        tdoa_noise = rng.standard_normal(n_tdoa) * tdoa_sigma
        noisy_tdoa = true_tdoa + tdoa_noise

        # Hybrid
        result_hybrid = tdoa_aoa_locator(
            aoa_collector_xyz=aoa_collector_xyz,
            aoa_rotations=aoa_rotations,
            aoa_measurements=noisy_aoa,
            aoa_covariances=aoa_covariances,
            tdoa_collector1_xyz=tdoa_collector1_xyz,
            tdoa_collector2_xyz=tdoa_collector2_xyz,
            tdoa_measurements=noisy_tdoa,
            tdoa_variances=tdoa_variances,
        )
        results["hybrid"]["errors"].append(
            np.linalg.norm(result_hybrid.position - target_true),
        )
        results["hybrid"]["traces"].append(np.trace(result_hybrid.covariance))

        # AOA only (need at least 2 collectors)
        if n_aoa >= 2:
            result_aoa = aoa_locator(
                aoa_collector_xyz,
                aoa_rotations,
                noisy_aoa,
                aoa_covariances,
            )
            results["aoa_only"]["errors"].append(
                np.linalg.norm(result_aoa.position - target_true),
            )
            results["aoa_only"]["traces"].append(np.trace(result_aoa.covariance))

        # TDOA only (need at least 3 measurements)
        if n_tdoa >= 3:
            result_tdoa = tdoa_locator(
                tdoa_collector1_xyz,
                tdoa_collector2_xyz,
                noisy_tdoa,
                tdoa_variances,
            )
            results["tdoa_only"]["errors"].append(
                np.linalg.norm(result_tdoa.position - target_true),
            )
            results["tdoa_only"]["traces"].append(np.trace(result_tdoa.covariance))

    # Convert to arrays
    for value in results.values():
        value["errors"] = list(np.array(value["errors"]))
        value["traces"] = list(np.array(value["traces"]))

    return results


def plot_chi2_validation(results: dict, title_suffix: str = "") -> go.Figure:
    """Plot chi-squared validation for hybrid Monte Carlo.

    Args:
        results: Results from run_hybrid_monte_carlo.
        title_suffix: Additional text for plot title.

    Returns:
        Plotly figure with histogram and Q-Q plot.
    """
    # Filter out NaN values
    normalized_miss_sq = results["normalized_miss_sq"]
    normalized_miss_sq = normalized_miss_sq[~np.isnan(normalized_miss_sq)]
    n_aoa = results["n_aoa"]
    n_tdoa = results["n_tdoa"]

    fig = make_subplots(
        rows=1,
        cols=2,
        subplot_titles=["Histogram vs χ²(3)", "Q-Q Plot"],
    )

    # Histogram
    fig.add_trace(
        go.Histogram(
            x=normalized_miss_sq,
            nbinsx=40,
            histnorm="probability density",
            name="Empirical",
            marker_color="rgba(99, 110, 250, 0.7)",
        ),
        row=1,
        col=1,
    )

    x_theory = np.linspace(0, 15, 200)
    pdf_theory = chi2.pdf(x_theory, df=3)
    fig.add_trace(
        go.Scatter(
            x=x_theory,
            y=pdf_theory,
            mode="lines",
            name="χ²(3)",
            line={"color": "#EF553B", "width": 2},
        ),
        row=1,
        col=1,
    )

    # Q-Q Plot
    sorted_data = np.sort(normalized_miss_sq)
    n = len(sorted_data)
    percentiles = (np.arange(1, n + 1) - 0.5) / n
    theoretical_quantiles = chi2.ppf(percentiles, df=3)

    fig.add_trace(
        go.Scatter(
            x=theoretical_quantiles,
            y=sorted_data,
            mode="markers",
            name="Q-Q",
            marker={"color": "#636EFA", "size": 3, "opacity": 0.5},
            showlegend=False,
        ),
        row=1,
        col=2,
    )

    max_val = max(theoretical_quantiles[-1], sorted_data[-1])
    fig.add_trace(
        go.Scatter(
            x=[0, max_val],
            y=[0, max_val],
            mode="lines",
            name="y=x",
            line={"color": "#EF553B", "dash": "dash"},
            showlegend=False,
        ),
        row=1,
        col=2,
    )

    # Statistics
    mean_d_sq = np.mean(normalized_miss_sq)
    var_d_sq = np.var(normalized_miss_sq)
    _, p_value = kstest(normalized_miss_sq, chi2(3).cdf)

    fig.add_annotation(
        x=0.98,
        y=0.98,
        xref="paper",
        yref="paper",
        text=(
            f"<b>Stats:</b><br>"
            f"Mean: {mean_d_sq:.2f} (theory: 3.0)<br>"
            f"Var: {var_d_sq:.2f} (theory: 6.0)<br>"
            f"KS p: {p_value:.3f}"
        ),
        showarrow=False,
        font={"size": 11},
        align="left",
        bgcolor="rgba(0,0,0,0.5)",
        xanchor="right",
        yanchor="top",
    )

    title = f"Hybrid Monte Carlo ({n_aoa} AOA + {n_tdoa} TDOA)"
    if title_suffix:
        title += f" - {title_suffix}"

    fig.update_layout(
        template=DARK_TEMPLATE,
        title=title,
        height=400,
    )

    fig.update_xaxes(title_text="d²", row=1, col=1)
    fig.update_yaxes(title_text="Density", row=1, col=1)
    fig.update_xaxes(title_text="Theoretical", row=1, col=2)
    fig.update_yaxes(title_text="Empirical", row=1, col=2)

    return fig


def plot_conditioning_comparison(comparison: dict) -> go.Figure:
    """Plot comparison of hybrid vs standalone solvers.

    Args:
        comparison: Results from run_comparison_monte_carlo.

    Returns:
        Plotly figure comparing error distributions and covariance traces.
    """
    fig = make_subplots(
        rows=1,
        cols=2,
        subplot_titles=["Position Error Distribution", "Covariance Trace (RMS)"],
    )

    colors = {
        "hybrid": "#00CC96",
        "aoa_only": "#636EFA",
        "tdoa_only": "#EF553B",
    }
    labels = {
        "hybrid": "Hybrid AOA+TDOA",
        "aoa_only": "AOA Only",
        "tdoa_only": "TDOA Only",
    }

    # Error histograms
    for key in ["hybrid", "aoa_only", "tdoa_only"]:
        if len(comparison[key]["errors"]) > 0:
            fig.add_trace(
                go.Histogram(
                    x=comparison[key]["errors"],
                    nbinsx=30,
                    name=labels[key],
                    marker_color=colors[key],
                    opacity=0.6,
                ),
                row=1,
                col=1,
            )

    # RMS uncertainty comparison (bar chart)
    rms_values = []
    names = []
    bar_colors = []
    for key in ["hybrid", "aoa_only", "tdoa_only"]:
        if len(comparison[key]["traces"]) > 0:
            rms = np.sqrt(np.mean(comparison[key]["traces"]))
            rms_values.append(rms)
            names.append(labels[key])
            bar_colors.append(colors[key])

    fig.add_trace(
        go.Bar(
            x=names,
            y=rms_values,
            marker_color=bar_colors,
            showlegend=False,
        ),
        row=1,
        col=2,
    )

    # Add improvement annotation
    if len(comparison["hybrid"]["errors"]) > 0:
        hybrid_mean = np.mean(comparison["hybrid"]["errors"])
        improvements = []
        for key in ["aoa_only", "tdoa_only"]:
            if len(comparison[key]["errors"]) > 0:
                other_mean = np.mean(comparison[key]["errors"])
                improvement = (other_mean - hybrid_mean) / other_mean * 100
                improvements.append(f"{labels[key]}: {improvement:.1f}% better")

        if improvements:
            fig.add_annotation(
                x=0.02,
                y=0.98,
                xref="paper",
                yref="paper",
                text="<b>Hybrid Improvement:</b><br>" + "<br>".join(improvements),
                showarrow=False,
                font={"size": 10},
                align="left",
                bgcolor="rgba(0,0,0,0.5)",
                xanchor="left",
                yanchor="top",
            )

    fig.update_layout(
        template=DARK_TEMPLATE,
        title="Overconstrained: Hybrid vs Standalone Solvers (3 AOA + 6 TDOA)",
        height=400,
        barmode="overlay",
    )

    fig.update_xaxes(title_text="Position Error (m)", row=1, col=1)
    fig.update_yaxes(title_text="Count", row=1, col=1)
    fig.update_yaxes(title_text="RMS Position Uncertainty (m)", row=1, col=2)

    return fig


def plot_underconstrained_validation(results: dict) -> go.Figure:
    """Plot χ² validation for underconstrained 2 AOA + 2 TDOA scenario.

    Args:
        results: Results from run_hybrid_monte_carlo for 2 AOA + 2 TDOA.

    Returns:
        Plotly figure with χ² histogram, Q-Q plot, and position errors.
    """
    fig = make_subplots(
        rows=1,
        cols=3,
        subplot_titles=[
            "χ² Validation",
            "Q-Q Plot",
            "Position Errors (Y vs Z)",
        ],
    )

    nms = results["normalized_miss_sq"]
    nms = nms[~np.isnan(nms)]
    errors = results["position_errors"]

    # Chi-squared histogram
    fig.add_trace(
        go.Histogram(
            x=nms,
            nbinsx=40,
            histnorm="probability density",
            name="Empirical",
            marker_color="rgba(171, 99, 250, 0.7)",
        ),
        row=1,
        col=1,
    )

    x_theory = np.linspace(0, 15, 200)
    pdf_theory = chi2.pdf(x_theory, df=3)
    fig.add_trace(
        go.Scatter(
            x=x_theory,
            y=pdf_theory,
            mode="lines",
            name="χ²(3)",
            line={"color": "#EF553B", "width": 2},
        ),
        row=1,
        col=1,
    )

    # Q-Q Plot
    sorted_data = np.sort(nms)
    n = len(sorted_data)
    percentiles = (np.arange(1, n + 1) - 0.5) / n
    theoretical_quantiles = chi2.ppf(percentiles, df=3)

    fig.add_trace(
        go.Scatter(
            x=theoretical_quantiles,
            y=sorted_data,
            mode="markers",
            name="Q-Q",
            marker={"color": "#AB63FA", "size": 3, "opacity": 0.5},
            showlegend=False,
        ),
        row=1,
        col=2,
    )

    max_val = max(theoretical_quantiles[-1], sorted_data[-1])
    fig.add_trace(
        go.Scatter(
            x=[0, max_val],
            y=[0, max_val],
            mode="lines",
            name="y=x",
            line={"color": "#EF553B", "dash": "dash"},
            showlegend=False,
        ),
        row=1,
        col=2,
    )

    # Position error scatter (Y vs Z)
    error_magnitudes = np.linalg.norm(errors, axis=1)
    fig.add_trace(
        go.Scatter(
            x=errors[:, 1],
            y=errors[:, 2],
            mode="markers",
            name="Errors",
            marker={
                "color": error_magnitudes,
                "colorscale": "Viridis",
                "size": 3,
                "opacity": 0.5,
                "colorbar": {"title": "Error (m)", "x": 1.02},
            },
            showlegend=False,
        ),
        row=1,
        col=3,
    )

    # Statistics
    mean_d_sq = np.mean(nms)
    var_d_sq = np.var(nms)
    _, p_value = kstest(nms, chi2(3).cdf)
    mean_error = np.mean(error_magnitudes)

    fig.add_annotation(
        x=0.98,
        y=0.98,
        xref="paper",
        yref="paper",
        text=(
            f"<b>Stats:</b><br>"
            f"Mean d²: {mean_d_sq:.2f} (theory: 3.0)<br>"
            f"Var d²: {var_d_sq:.2f} (theory: 6.0)<br>"
            f"KS p: {p_value:.3f}<br>"
            f"Mean error: {mean_error:.0f}m"
        ),
        showarrow=False,
        font={"size": 11},
        align="left",
        bgcolor="rgba(0,0,0,0.5)",
        xanchor="right",
        yanchor="top",
    )

    fig.update_layout(
        template=DARK_TEMPLATE,
        title="Underconstrained (2 AOA + 2 TDOA): Hybrid Enables 3D Solution",
        height=400,
    )

    fig.update_xaxes(title_text="d²", row=1, col=1)
    fig.update_yaxes(title_text="Density", row=1, col=1)
    fig.update_xaxes(title_text="Theoretical", row=1, col=2)
    fig.update_yaxes(title_text="Empirical", row=1, col=2)
    fig.update_xaxes(title_text="Y Error (m)", row=1, col=3)
    fig.update_yaxes(title_text="Z Error (m)", row=1, col=3)

    return fig


def _kde_line(data: np.ndarray, n_points: int = 200) -> tuple[np.ndarray, np.ndarray]:
    """Compute KDE for smooth density estimation.

    Args:
        data: 1D array of samples.
        n_points: Number of points for the KDE curve.

    Returns:
        Tuple of (x_values, density_values).
    """
    kde = gaussian_kde(data)
    x_min, x_max = data.min(), data.max()
    padding = (x_max - x_min) * 0.1
    x = np.linspace(x_min - padding, x_max + padding, n_points)
    return x, kde(x)


def plot_error_comparison(n_trials: int = 1000) -> go.Figure:
    """Create error comparison plot showing all solver configurations.

    Shows position error distributions as smooth KDE lines for:
    - Hybrid 3A+6T (overconstrained)
    - AOA only (3 collectors)
    - TDOA only (6 pairs from 4 satellites)
    - Hybrid 2A+2T (underconstrained)

    Args:
        n_trials: Number of Monte Carlo trials per scenario.

    Returns:
        Plotly figure with error comparison.
    """
    # Run all simulations
    print("Running overconstrained Monte Carlo (3 AOA + 6 TDOA)...")
    geom_over = _create_overconstrained_geometry()
    comparison = run_comparison_monte_carlo(geom_over, n_trials=n_trials)

    print("Running underconstrained Monte Carlo (2 AOA + 2 TDOA)...")
    geom_2a2t = _create_underconstrained_geometry_2aoa_2tdoa()
    results_2a2t = run_hybrid_monte_carlo(geom_2a2t, n_trials=n_trials)

    fig = go.Figure()

    # Define colors and labels for each configuration
    configs = [
        ("hybrid", "Hybrid 3A+6T", "#00CC96", comparison["hybrid"]["errors"]),
        ("aoa_only", "AOA only (3)", "#636EFA", comparison["aoa_only"]["errors"]),
        ("tdoa_only", "TDOA only (6)", "#EF553B", comparison["tdoa_only"]["errors"]),
    ]

    # Add underconstrained hybrid
    errors_2a2t = np.linalg.norm(results_2a2t["position_errors"], axis=1)
    configs.append(("hybrid_2a2t", "Hybrid 2A+2T", "#AB63FA", errors_2a2t))

    # Add KDE lines for each configuration
    for _key, label, color, errors in configs:
        if len(errors) > 0:
            x, density = _kde_line(errors)
            mean_err = np.mean(errors)
            fig.add_trace(
                go.Scatter(
                    x=x,
                    y=density,
                    mode="lines",
                    name=f"{label} (μ={mean_err:.0f}m)",
                    line={"color": color, "width": 2.5},
                ),
            )

    fig.update_layout(
        template=DARK_TEMPLATE,
        title="Position Error Comparison: Hybrid vs Standalone Solvers",
        xaxis_title="Position Error (m)",
        yaxis_title="Density",
        height=450,
        legend={
            "x": 0.98,
            "y": 0.98,
            "xanchor": "right",
            "yanchor": "top",
            "bgcolor": "rgba(0,0,0,0.5)",
        },
    )

    return fig


def main() -> None:
    """Run all hybrid Monte Carlo visualizations."""
    print("=" * 60)
    print("Hybrid AOA+TDOA Monte Carlo Validation")
    print("=" * 60)

    # Overconstrained case
    print("\n1. OVERCONSTRAINED CASE (3 AOA + 6 TDOA from 4 satellites)")
    print("-" * 40)
    geom_over = _create_overconstrained_geometry()
    results_over = run_hybrid_monte_carlo(geom_over, n_trials=1000)

    mean_d_sq = np.mean(results_over["normalized_miss_sq"])
    _, p_value = kstest(results_over["normalized_miss_sq"], chi2(3).cdf)
    mean_error = np.mean(np.linalg.norm(results_over["position_errors"], axis=1))

    print(f"  Mean d²: {mean_d_sq:.3f} (theory: 3.0)")
    print(f"  KS p-value: {p_value:.4f}")
    print(f"  Mean position error: {mean_error:.1f} m")

    # Comparison with standalone solvers
    print("\n  Comparison with standalone solvers:")
    comparison = run_comparison_monte_carlo(geom_over, n_trials=500)

    for key, label in [
        ("hybrid", "Hybrid"),
        ("aoa_only", "AOA only"),
        ("tdoa_only", "TDOA only"),
    ]:
        if len(comparison[key]["errors"]) > 0:
            mean_err = np.mean(comparison[key]["errors"])
            rms_unc = np.sqrt(np.mean(comparison[key]["traces"]))
            print(f"    {label}: mean err={mean_err:.1f}m, RMS unc={rms_unc:.1f}m")

    # Underconstrained case - neither AOA nor TDOA alone is sufficient
    print("\n2. UNDERCONSTRAINED CASE (2 AOA + 2 TDOA)")
    print("-" * 40)
    print("  Geometry: TDOA satellites (overhead, east, north)")
    print("            AOA satellites (south, west)")
    print("  TDOA hyperboloids and AOA bearings are orthogonal")
    geom_2a2t = _create_underconstrained_geometry_2aoa_2tdoa()
    results_2a2t = run_hybrid_monte_carlo(geom_2a2t, n_trials=1000)

    # Filter out NaN values for statistics
    valid_d_sq_2a2t = results_2a2t["normalized_miss_sq"][
        ~np.isnan(results_2a2t["normalized_miss_sq"])
    ]
    mean_error = np.mean(np.linalg.norm(results_2a2t["position_errors"], axis=1))

    print(f"  Singular covariances: {results_2a2t['singular_count']}/1000")
    if len(valid_d_sq_2a2t) > 10:
        mean_d_sq = np.mean(valid_d_sq_2a2t)
        _, p_value = kstest(valid_d_sq_2a2t, chi2(3).cdf)
        print(f"  Mean d²: {mean_d_sq:.3f} (theory: 3.0)")
        print(f"  KS p-value: {p_value:.4f}")
    else:
        print("  Too few valid covariances for χ² statistics")
    print(f"  Mean position error: {mean_error:.1f} m")
    print("  (2 AOA has poor range; 2 TDOA insufficient alone)")

    # Show plots
    print("\nGenerating plots...")

    fig1 = plot_chi2_validation(results_over, "Overconstrained")
    fig1.show()

    fig2 = plot_conditioning_comparison(comparison)
    fig2.show()

    fig3 = plot_underconstrained_validation(results_2a2t)
    fig3.show()

    fig4 = plot_error_comparison(n_trials=1000)
    fig4.show()


if __name__ == "__main__":
    main()
