"""Calculate slant range from observer along azimuth/elevation to target altitude.

Given an observer position and a look direction (azimuth, elevation), find the
range at which the line of sight intersects a specified altitude above the
WGS84 ellipsoid.

This is useful for:
- Determining ground range from an elevated platform (satellite, aircraft)
- Placing targets at specific altitudes along a look direction
- Converting az/el observations to range for geolocation scenarios
"""

from __future__ import annotations

from typing import TYPE_CHECKING

import numpy as np

from gri_utils.constants import ER_M, PR_M
from gri_utils.conversion.xyz_aer import aer_to_xyz

if TYPE_CHECKING:
    from collections.abc import Sequence

    from numpy.typing import NDArray


def slant_range(
    observer_xyz: NDArray[np.floating] | Sequence,
    azimuth_deg: float,
    elevation_deg: float,
    target_altitude_m: float = 0.0,
) -> float | None:
    """Calculate slant range from observer to intersection with target altitude.

    Given an observer position and look direction, find the range where the
    line of sight intersects the specified altitude above the WGS84 ellipsoid.

    The algorithm works by:
    1. Creating a unit direction vector in the az/el direction
    2. Solving the ray-ellipsoid intersection for WGS84 + target altitude
    3. Returning the range to the nearest valid intersection

    Args:
        observer_xyz: Observer position in ECEF XYZ coordinates (meters). Shape (3,).
        azimuth_deg: Azimuth angle in degrees, clockwise from north (0-360).
        elevation_deg: Elevation angle in degrees, up from local horizontal (-90 to 90).
        target_altitude_m: Target altitude above WGS84 ellipsoid in meters.
            Defaults to 0.0 (ground level).

    Returns:
        Slant range in meters to the intersection point, or None if the ray
        does not intersect the target altitude surface (e.g., looking up into
        space when target altitude is ground level).

    Example:
        >>> from gri_utils.conversion import lla_to_xyz
        >>> # Satellite at 500km looking down at 45 deg elevation
        >>> observer = lla_to_xyz([0.0, 0.0, 500_000.0])
        >>> slant_range(observer, azimuth_deg=0.0, elevation_deg=-45.0)
        707106...  # approximately 707 km

        >>> # Ground station looking up - no intersection with ground
        >>> observer = lla_to_xyz([45.0, -75.0, 100.0])
        >>> slant_range(observer, azimuth_deg=180.0, elevation_deg=45.0)
        None
    """
    observer_xyz = np.asarray(observer_xyz, dtype=np.float64)

    # Create unit direction vector in az/el direction
    # aer_to_xyz expects (az, el, range) and returns XYZ offset vector
    aer = np.array([azimuth_deg, elevation_deg, 1.0])
    direction = aer_to_xyz(observer_xyz, aer)
    direction = direction / np.linalg.norm(direction)

    # Scale factors for the target altitude ellipsoid
    # WGS84 ellipsoid scaled to target altitude: (x/a)^2 + (y/a)^2 + (z/b)^2 = 1
    # where a = ER_M + alt, b = PR_M + alt (approximately, for small altitudes)
    #
    # For accuracy with WGS84, we scale by the semi-axes plus altitude
    a = ER_M + target_altitude_m  # equatorial radius + altitude
    b = PR_M + target_altitude_m  # polar radius + altitude

    # Ray: P(t) = observer_xyz + t * direction
    # Ellipsoid: (x/a)^2 + (y/a)^2 + (z/b)^2 = 1
    #
    # Substituting ray into ellipsoid equation:
    # ((ox + t*dx)/a)^2 + ((oy + t*dy)/a)^2 + ((oz + t*dz)/b)^2 = 1
    #
    # This expands to: A*t^2 + B*t + C = 0
    ox, oy, oz = observer_xyz
    dx, dy, dz = direction

    # Coefficients for quadratic equation
    # A = (dx^2 + dy^2)/a^2 + dz^2/b^2
    # B = 2 * ((ox*dx + oy*dy)/a^2 + oz*dz/b^2)
    # C = (ox^2 + oy^2)/a^2 + oz^2/b^2 - 1
    a_coef = (dx**2 + dy**2) / a**2 + dz**2 / b**2
    b_coef = 2 * ((ox * dx + oy * dy) / a**2 + oz * dz / b**2)
    c_coef = (ox**2 + oy**2) / a**2 + oz**2 / b**2 - 1

    discriminant = b_coef**2 - 4 * a_coef * c_coef

    if discriminant < 0:
        # No intersection with the target altitude surface
        return None

    sqrt_disc = np.sqrt(discriminant)
    t1 = (-b_coef - sqrt_disc) / (2 * a_coef)
    t2 = (-b_coef + sqrt_disc) / (2 * a_coef)

    # We want the nearest positive intersection (in front of observer)
    # t1 <= t2 always due to how we compute them
    if t1 > 0:
        return float(t1)
    if t2 > 0:
        return float(t2)

    # Both intersections are behind the observer
    return None
