from __future__ import annotations

from dataclasses import dataclass

import numpy as np


@dataclass(frozen=True)
class TriangulationResult:
    module_a: np.ndarray
    module_b: np.ndarray
    direction_a: np.ndarray
    direction_b: np.ndarray
    closest_a: np.ndarray
    closest_b: np.ndarray
    estimated_position: np.ndarray
    residual_m: float
    range_a_m: float
    range_b_m: float


def direction_from_az_el(azimuth_deg: float, elevation_deg: float) -> np.ndarray:
    """Convert ENU azimuth/elevation angles to a unit direction vector."""
    az = np.deg2rad(azimuth_deg)
    el = np.deg2rad(elevation_deg)
    direction = np.array(
        [
            np.cos(el) * np.sin(az),
            np.cos(el) * np.cos(az),
            np.sin(el),
        ],
        dtype=np.float64,
    )
    return direction / np.linalg.norm(direction)


def az_el_from_points(origin: np.ndarray, target: np.ndarray) -> tuple[float, float]:
    """Return ENU azimuth/elevation from origin to target."""
    delta = np.asarray(target, dtype=np.float64) - np.asarray(origin, dtype=np.float64)
    horizontal = np.hypot(delta[0], delta[1])
    azimuth = (np.rad2deg(np.arctan2(delta[0], delta[1])) + 360.0) % 360.0
    elevation = np.rad2deg(np.arctan2(delta[2], horizontal))
    return float(azimuth), float(elevation)


def closest_points_between_rays(
    origin_a: np.ndarray,
    direction_a: np.ndarray,
    origin_b: np.ndarray,
    direction_b: np.ndarray,
) -> tuple[np.ndarray, np.ndarray, float, float]:
    """Find the closest points between two 3D rays."""
    p = np.asarray(origin_a, dtype=np.float64)
    q = np.asarray(origin_b, dtype=np.float64)
    u = np.asarray(direction_a, dtype=np.float64)
    v = np.asarray(direction_b, dtype=np.float64)
    u = u / np.linalg.norm(u)
    v = v / np.linalg.norm(v)

    w = p - q
    a = float(np.dot(u, u))
    b = float(np.dot(u, v))
    c = float(np.dot(v, v))
    d = float(np.dot(u, w))
    e = float(np.dot(v, w))
    denom = a * c - b * b

    if abs(denom) < 1e-9:
        t = 0.0
        s = max(0.0, e / c)
    else:
        t = (b * e - c * d) / denom
        s = (a * e - b * d) / denom
        t = max(0.0, t)
        s = max(0.0, s)

    closest_a = p + t * u
    closest_b = q + s * v
    return closest_a, closest_b, float(t), float(s)


def triangulate_from_az_el(
    module_a: np.ndarray,
    azimuth_a_deg: float,
    elevation_a_deg: float,
    module_b: np.ndarray,
    azimuth_b_deg: float,
    elevation_b_deg: float,
) -> TriangulationResult:
    """Triangulate a 3D position from two module rays."""
    direction_a = direction_from_az_el(azimuth_a_deg, elevation_a_deg)
    direction_b = direction_from_az_el(azimuth_b_deg, elevation_b_deg)
    closest_a, closest_b, range_a_m, range_b_m = closest_points_between_rays(
        module_a,
        direction_a,
        module_b,
        direction_b,
    )
    estimated = (closest_a + closest_b) / 2.0
    residual = float(np.linalg.norm(closest_a - closest_b))
    return TriangulationResult(
        module_a=np.asarray(module_a, dtype=np.float64),
        module_b=np.asarray(module_b, dtype=np.float64),
        direction_a=direction_a,
        direction_b=direction_b,
        closest_a=closest_a,
        closest_b=closest_b,
        estimated_position=estimated,
        residual_m=residual,
        range_a_m=range_a_m,
        range_b_m=range_b_m,
    )