"""Line of sight visibility check through WGS84 Earth ellipsoid.

Determines if the straight line between two ECEF points intersects (is blocked by)
the WGS84 Earth ellipsoid.

This is useful for:
- Satellite-to-satellite visibility checks
- Satellite-to-ground visibility checks
- Any geometry where Earth occlusion matters

The algorithm scales coordinates to transform the WGS84 ellipsoid into a unit sphere,
then performs a simple ray-sphere intersection test. This is both fast and accurate
for the ellipsoid geometry.
"""

from __future__ import annotations

from typing import TYPE_CHECKING

import numpy as np

from gri_utils.constants import ER_M, PR_M

# Tolerance for detecting degenerate case (same point)
_SAME_POINT_TOL = 1e-30
# Tolerance for t-parameter bounds (handles floating point at surface endpoints)
_T_EPSILON = 1e-9

if TYPE_CHECKING:
    from collections.abc import Sequence

    from numpy.typing import NDArray


def line_of_sight_clear(
    xyz1_m: NDArray[np.floating] | Sequence,
    xyz2_m: NDArray[np.floating] | Sequence,
) -> bool:
    """Check if line of sight between two ECEF points clears the WGS84 ellipsoid.

    Determines whether the straight line segment from xyz1 to xyz2 intersects
    the WGS84 Earth ellipsoid. Returns True if the path is clear (no intersection),
    False if the Earth blocks the line of sight.

    The test transforms coordinates to a unit sphere space (scaling x,y by 1/ER_M
    and z by 1/PR_M), then solves the quadratic ray-sphere intersection equation.

    Args:
        xyz1_m: First ECEF position in meters. Shape (3,).
        xyz2_m: Second ECEF position in meters. Shape (3,).

    Returns:
        True if line of sight is clear (no Earth intersection).
        False if Earth blocks the line of sight.

    Example:
        >>> import numpy as np
        >>> # Two satellites on opposite sides of Earth - blocked
        >>> sat1 = np.array([42164e3, 0.0, 0.0])  # GEO satellite
        >>> sat2 = np.array([-42164e3, 0.0, 0.0])  # Opposite side
        >>> line_of_sight_clear(sat1, sat2)
        False

        >>> # Satellite and ground station with clear view
        >>> sat = np.array([0.0, 0.0, 42164e3])  # Over north pole
        >>> ground = np.array([0.0, 0.0, 6378137.0])  # North pole surface
        >>> line_of_sight_clear(sat, ground)
        True
    """
    # Convert to numpy arrays
    p1 = np.asarray(xyz1_m, dtype=np.float64)
    p2 = np.asarray(xyz2_m, dtype=np.float64)

    # Scale factors to transform WGS84 ellipsoid to unit sphere
    # After scaling: x**2/ER**2 + y**2/ER**2 + z**2/PR**2 = 1
    # becomes x**2 + y**2 + z**2 = 1
    scale = np.array([1.0 / ER_M, 1.0 / ER_M, 1.0 / PR_M])

    # Transform points to unit sphere space
    p1_scaled = p1 * scale
    p2_scaled = p2 * scale

    # Ray from p1 to p2: P(t) = p1 + t * d, where t in [0, 1]
    d = p2_scaled - p1_scaled

    # Solve ||p1 + t*d||**2 = 1 for t
    # Expands to: (d.d)*t**2 + 2*(p1.d)*t + (p1.p1 - 1) = 0
    # Standard form: a*t**2 + b*t + c = 0
    a = np.dot(d, d)
    b = 2.0 * np.dot(p1_scaled, d)
    c = np.dot(p1_scaled, p1_scaled) - 1.0

    # Discriminant determines if ray intersects sphere
    discriminant = b * b - 4.0 * a * c

    if discriminant < 0:
        # No intersection with sphere at all - clear
        return True

    # Handle degenerate case: same point or nearly same point
    if a < _SAME_POINT_TOL:
        # Points are the same (or extremely close) - no path to block
        return True

    # Two potential intersection points at t values
    sqrt_disc = np.sqrt(discriminant)
    t1 = (-b - sqrt_disc) / (2.0 * a)
    t2 = (-b + sqrt_disc) / (2.0 * a)

    # Check if either intersection is strictly within the segment (0, 1)
    # We use exclusive bounds because endpoints on the surface are allowed -
    # we only care if the Earth blocks the interior of the path.
    # Use small epsilon for numerical stability when endpoints are on/near surface.
    return t2 <= _T_EPSILON or t1 >= 1.0 - _T_EPSILON
