"""Tests for Hermite interpolation of orbital state vectors."""

import math

import numpy as np
import pytest

from gri_utils.orbit import OrbitalElements, elements_to_state, hermite_interpolate


def _generate_circular_ephemeris(
    n_points: int,
    radius_m: float = 7_000_000.0,
    dt_s: float = 60.0,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Generate ephemeris from a circular equatorial orbit for testing.

    Returns:
        Tuple of (times, positions, velocities) arrays.
    """
    elements = OrbitalElements(
        semi_major_axis_m=radius_m,
        eccentricity=0.0,
        inclination_rad=0.0,
        raan_rad=0.0,
        arg_periapsis_rad=0.0,
        mean_anomaly_rad=0.0,
        epoch_unix_s=0.0,
    )

    times = np.arange(n_points, dtype=np.float64) * dt_s
    states = elements_to_state(elements, times)
    assert isinstance(states, list)

    positions = np.array([s.position_eci_m for s in states])
    velocities = np.array([s.velocity_eci_ms for s in states])

    return times, positions, velocities


def test_exact_at_knots() -> None:
    """Hermite interpolation reproduces knot values exactly."""
    times, positions, velocities = _generate_circular_ephemeris(10)

    states = hermite_interpolate(times, positions, velocities, times)
    assert isinstance(states, list)
    assert len(states) == 10

    for i, state in enumerate(states):
        np.testing.assert_allclose(
            state.position_eci_m,
            positions[i],
            atol=1e-6,
        )
        np.testing.assert_allclose(
            state.velocity_eci_ms,
            velocities[i],
            atol=1e-6,
        )


def test_midpoint_accuracy_circular_orbit() -> None:
    """Midpoint interpolation is close to true Keplerian state."""
    radius_m = 7_000_000.0
    times, positions, velocities = _generate_circular_ephemeris(
        20,
        radius_m=radius_m,
        dt_s=60.0,
    )

    # Query at midpoints between knots
    midpoints = (times[:-1] + times[1:]) / 2.0

    # Get Hermite-interpolated states
    hermite_states = hermite_interpolate(
        times,
        positions,
        velocities,
        midpoints,
    )
    assert isinstance(hermite_states, list)

    # Get true Keplerian states at same times
    elements = OrbitalElements(
        semi_major_axis_m=radius_m,
        eccentricity=0.0,
        inclination_rad=0.0,
        raan_rad=0.0,
        arg_periapsis_rad=0.0,
        mean_anomaly_rad=0.0,
        epoch_unix_s=0.0,
    )
    true_states = elements_to_state(elements, midpoints)
    assert isinstance(true_states, list)

    for h_state, t_state in zip(hermite_states, true_states, strict=True):
        pos_err = np.linalg.norm(h_state.position_eci_m - t_state.position_eci_m)
        vel_err = np.linalg.norm(h_state.velocity_eci_ms - t_state.velocity_eci_ms)

        # With 60s spacing on a ~7000 km orbit, Hermite should be sub-meter
        assert pos_err < 1.0, f"Position error {pos_err:.3f} m exceeds 1 m"
        assert vel_err < 0.01, f"Velocity error {vel_err:.6f} m/s exceeds 0.01 m/s"


def test_eccentric_orbit_accuracy() -> None:
    """Hermite interpolation works well for eccentric orbits."""
    elements = OrbitalElements(
        semi_major_axis_m=10_000_000.0,
        eccentricity=0.3,
        inclination_rad=math.radians(45.0),
        raan_rad=math.radians(30.0),
        arg_periapsis_rad=math.radians(60.0),
        mean_anomaly_rad=0.0,
        epoch_unix_s=0.0,
    )

    # Generate knots at 30-second intervals
    times = np.arange(20, dtype=np.float64) * 30.0
    states = elements_to_state(elements, times)
    assert isinstance(states, list)
    positions = np.array([s.position_eci_m for s in states])
    velocities = np.array([s.velocity_eci_ms for s in states])

    # Query at midpoints
    midpoints = (times[:-1] + times[1:]) / 2.0
    hermite_states = hermite_interpolate(
        times,
        positions,
        velocities,
        midpoints,
    )
    assert isinstance(hermite_states, list)

    true_states = elements_to_state(elements, midpoints)
    assert isinstance(true_states, list)

    for h_state, t_state in zip(hermite_states, true_states, strict=True):
        pos_err = np.linalg.norm(h_state.position_eci_m - t_state.position_eci_m)
        # Eccentric orbits have more curvature, allow slightly more error
        assert pos_err < 5.0, f"Position error {pos_err:.3f} m exceeds 5 m"


def test_scalar_query_returns_single_state() -> None:
    """Scalar query time returns a single OrbitState, not a list."""
    times, positions, velocities = _generate_circular_ephemeris(5)

    state = hermite_interpolate(times, positions, velocities, 30.0)
    assert not isinstance(state, list)
    assert state.position_eci_m.shape == (3,)
    assert state.velocity_eci_ms.shape == (3,)
    assert state.acceleration_eci_ms2.shape == (3,)


def test_array_query_returns_list() -> None:
    """Array query times return a list of OrbitState objects."""
    times, positions, velocities = _generate_circular_ephemeris(5)

    states = hermite_interpolate(
        times,
        positions,
        velocities,
        [30.0, 90.0, 150.0],
    )
    assert isinstance(states, list)
    assert len(states) == 3


def test_velocity_is_position_derivative() -> None:
    """Returned velocity is consistent with position differences."""
    times, positions, velocities = _generate_circular_ephemeris(10, dt_s=60.0)

    t_query = 150.0
    dt = 0.001  # 1 ms step for numerical derivative

    state_before = hermite_interpolate(
        times,
        positions,
        velocities,
        t_query - dt / 2,
    )
    state_after = hermite_interpolate(
        times,
        positions,
        velocities,
        t_query + dt / 2,
    )
    state_center = hermite_interpolate(
        times,
        positions,
        velocities,
        t_query,
    )
    assert not isinstance(state_before, list)
    assert not isinstance(state_after, list)
    assert not isinstance(state_center, list)

    numerical_vel = (state_after.position_eci_m - state_before.position_eci_m) / dt
    np.testing.assert_allclose(
        state_center.velocity_eci_ms,
        numerical_vel,
        rtol=1e-6,
    )


def test_conservation_of_radius_circular() -> None:
    """Hermite-interpolated circular orbit stays near constant radius."""
    radius_m = 7_000_000.0
    times, positions, velocities = _generate_circular_ephemeris(
        20,
        radius_m=radius_m,
        dt_s=60.0,
    )

    query_times = np.linspace(times[0], times[-1], 100)
    states = hermite_interpolate(times, positions, velocities, query_times)
    assert isinstance(states, list)

    radii = np.array([np.linalg.norm(s.position_eci_m) for s in states])
    # Radius should stay within a few meters of nominal for 60s spacing
    assert np.all(np.abs(radii - radius_m) < 1.0)


def test_minimum_two_knots_required() -> None:
    """Raises ValueError with fewer than 2 knot points."""
    with pytest.raises(ValueError, match="at least 2"):
        hermite_interpolate(
            np.array([0.0]),
            np.array([[1.0, 0.0, 0.0]]),
            np.array([[0.0, 1.0, 0.0]]),
            0.0,
        )


def test_non_increasing_times_rejected() -> None:
    """Raises ValueError if knot times are not strictly increasing."""
    with pytest.raises(ValueError, match="strictly increasing"):
        hermite_interpolate(
            np.array([0.0, 0.0, 1.0]),
            np.zeros((3, 3)),
            np.zeros((3, 3)),
            0.5,
        )


def test_shape_mismatch_rejected() -> None:
    """Raises ValueError if positions/velocities shape doesn't match times."""
    with pytest.raises(ValueError, match="positions shape"):
        hermite_interpolate(
            np.array([0.0, 1.0, 2.0]),
            np.zeros((2, 3)),  # wrong length
            np.zeros((3, 3)),
            0.5,
        )
