"""Tests for terrain ray intersection."""

import numpy as np

from gri_utils.conversion import lla_to_xyz, xyz_to_lla
from gri_utils.terrain import (
    lla_to_xyz_sheet,
    ray_intersects_sheet_bounds,
    ray_sheet_intersect,
)


def test_ray_sheet_intersect_vertical_hit() -> None:
    """Test vertical ray hitting flat terrain."""
    # Create flat terrain at 1000m altitude
    lats = np.linspace(39.0, 40.0, 50)
    lons = np.linspace(-105.0, -104.0, 50)
    alts = np.full((50, 50), 1000.0)

    sheet = lla_to_xyz_sheet(lats, lons, alts)

    # Ray from 10km above center, pointing down
    center_lla = np.array([39.5, -104.5, 10000.0])
    origin = lla_to_xyz(center_lla)
    direction = -origin / np.linalg.norm(origin)  # Toward Earth center

    hit = ray_sheet_intersect(sheet, origin, direction)

    assert hit is not None
    # Hit should be near the terrain surface (1000m altitude)
    hit_lla = xyz_to_lla(hit)
    assert 900 < hit_lla[2] < 1100  # Within 100m of 1000m surface


def test_ray_sheet_intersect_miss_outside_bounds() -> None:
    """Test ray that misses sheet geographically."""
    lats = np.linspace(39.0, 40.0, 20)
    lons = np.linspace(-105.0, -104.0, 20)
    alts = np.zeros((20, 20))

    sheet = lla_to_xyz_sheet(lats, lons, alts)

    # Ray from north pole pointing down - far from sheet
    origin = np.array([0.0, 0.0, 7e6])
    direction = np.array([0.0, 0.0, -1.0])

    hit = ray_sheet_intersect(sheet, origin, direction)

    assert hit is None


def test_ray_sheet_intersect_miss_parallel() -> None:
    """Test ray parallel to terrain surface."""
    lats = np.linspace(39.0, 40.0, 20)
    lons = np.linspace(-105.0, -104.0, 20)
    alts = np.zeros((20, 20))

    sheet = lla_to_xyz_sheet(lats, lons, alts)

    # Ray from high altitude, pointing tangentially
    origin_lla = np.array([39.5, -104.5, 100000.0])  # 100km up
    origin = lla_to_xyz(origin_lla)

    # Direction perpendicular to radial (roughly east)
    _radial = origin / np.linalg.norm(origin)
    east = np.array([-origin[1], origin[0], 0])
    east = east / np.linalg.norm(east)

    hit = ray_sheet_intersect(sheet, origin, east)

    # Should miss - ray is too high and parallel
    assert hit is None


def test_ray_sheet_intersect_with_peak() -> None:
    """Test ray hitting terrain with a peak."""
    lats = np.linspace(39.0, 40.0, 50)
    lons = np.linspace(-105.0, -104.0, 50)
    alts = np.zeros((50, 50))
    # Add a 3000m peak in the center
    alts[25, 25] = 3000.0
    alts[24:27, 24:27] = 2000.0

    sheet = lla_to_xyz_sheet(lats, lons, alts)

    # Ray from 5km altitude above peak, pointing down
    peak_lla = np.array([39.5, -104.5, 5000.0])
    origin = lla_to_xyz(peak_lla)
    direction = -origin / np.linalg.norm(origin)

    hit = ray_sheet_intersect(sheet, origin, direction)

    assert hit is not None
    hit_lla = xyz_to_lla(hit)
    # Should hit somewhere on the peak (>1000m)
    assert hit_lla[2] > 500


def test_ray_sheet_intersect_zero_direction() -> None:
    """Test with zero direction vector."""
    lats = np.linspace(39.0, 40.0, 10)
    lons = np.linspace(-105.0, -104.0, 10)
    alts = np.zeros((10, 10))

    sheet = lla_to_xyz_sheet(lats, lons, alts)
    origin = np.array([0.0, 0.0, 7e6])
    direction = np.array([0.0, 0.0, 0.0])

    hit = ray_sheet_intersect(sheet, origin, direction)

    assert hit is None


def test_ray_intersects_sheet_bounds_wrapper() -> None:
    """Test convenience wrapper function."""
    lats = np.linspace(39.0, 40.0, 10)
    lons = np.linspace(-105.0, -104.0, 10)
    alts = np.zeros((10, 10))

    sheet = lla_to_xyz_sheet(lats, lons, alts)

    # Ray toward sheet center
    center_lla = np.array([39.5, -104.5, 100000.0])
    origin = lla_to_xyz(center_lla)
    direction = -origin

    assert ray_intersects_sheet_bounds(sheet, origin, direction)

    # Ray away from sheet
    origin_far = np.array([0.0, 0.0, 7e6])
    direction_away = np.array([1.0, 0.0, 0.0])

    assert not ray_intersects_sheet_bounds(sheet, origin_far, direction_away)


def test_ray_sheet_intersect_altitude_epsilon() -> None:
    """Test altitude epsilon parameter."""
    lats = np.linspace(39.0, 40.0, 50)
    lons = np.linspace(-105.0, -104.0, 50)
    alts = np.full((50, 50), 1000.0)

    sheet = lla_to_xyz_sheet(lats, lons, alts)

    # Ray from above
    center_lla = np.array([39.5, -104.5, 10000.0])
    origin = lla_to_xyz(center_lla)
    direction = -origin / np.linalg.norm(origin)

    # Hit with 0 epsilon
    hit0 = ray_sheet_intersect(sheet, origin, direction, altitude_epsilon_m=0.0)

    # Hit with 100m epsilon (should be higher)
    hit100 = ray_sheet_intersect(sheet, origin, direction, altitude_epsilon_m=100.0)

    assert hit0 is not None
    assert hit100 is not None

    alt0 = xyz_to_lla(hit0)[2]
    alt100 = xyz_to_lla(hit100)[2]

    # With epsilon, hit should be higher
    assert alt100 > alt0 + 50  # At least 50m higher
