"""Tests for state_to_elements (state vector to orbital elements conversion)."""

import math

import numpy as np
import pytest

from gri_utils.constants import GM_EARTH
from gri_utils.orbit import (
    OrbitalElements,
    OrbitState,
    elements_to_state,
    position_velocity_to_elements,
    state_to_elements,
)


def _make_circular_state(
    radius_m: float,
    inclination_rad: float = 0.0,
) -> OrbitState:
    """Create state vector for circular orbit at given radius and inclination."""
    v = math.sqrt(GM_EARTH / radius_m)

    if inclination_rad == 0:
        # Equatorial orbit
        position = np.array([radius_m, 0.0, 0.0])
        velocity = np.array([0.0, v, 0.0])
    else:
        # Inclined orbit - position at ascending node, velocity tilted
        position = np.array([radius_m, 0.0, 0.0])
        velocity = np.array(
            [0.0, v * math.cos(inclination_rad), v * math.sin(inclination_rad)],
        )

    return OrbitState(
        position_eci_m=position,
        velocity_eci_ms=velocity,
        acceleration_eci_ms2=np.zeros(3),
    )


def test_circular_equatorial_orbit() -> None:
    """Test conversion of circular equatorial orbit."""
    radius = 7_000_000
    state = _make_circular_state(radius)

    elements = state_to_elements(state, unix_s=0.0)

    assert elements.semi_major_axis_m == pytest.approx(radius, rel=1e-6)
    assert elements.eccentricity == pytest.approx(0.0, abs=1e-10)
    assert elements.inclination_rad == pytest.approx(0.0, abs=1e-10)


def test_circular_inclined_orbit() -> None:
    """Test conversion of circular inclined orbit."""
    radius = 7_500_000
    inclination = math.radians(45)
    state = _make_circular_state(radius, inclination)

    elements = state_to_elements(state, unix_s=0.0)

    assert elements.semi_major_axis_m == pytest.approx(radius, rel=1e-6)
    assert elements.eccentricity == pytest.approx(0.0, abs=1e-6)
    assert elements.inclination_rad == pytest.approx(inclination, rel=1e-6)


def test_round_trip_conversion() -> None:
    """Test that elements -> state -> elements preserves values."""
    original = OrbitalElements(
        semi_major_axis_m=8_000_000,
        eccentricity=0.1,
        inclination_rad=0.8,
        raan_rad=1.5,
        arg_periapsis_rad=2.0,
        mean_anomaly_rad=0.5,
        epoch_unix_s=1000.0,
    )

    # Convert to state and back
    state = elements_to_state(original, original.epoch_unix_s)
    assert not isinstance(state, list)
    recovered = state_to_elements(state, original.epoch_unix_s)

    assert recovered.semi_major_axis_m == pytest.approx(
        original.semi_major_axis_m,
        rel=1e-6,
    )
    assert recovered.eccentricity == pytest.approx(original.eccentricity, abs=1e-8)
    assert recovered.inclination_rad == pytest.approx(
        original.inclination_rad,
        rel=1e-6,
    )
    # RAAN and arg_periapsis might differ by 2*pi or have sign ambiguity for edge cases
    # but for this well-defined case they should match
    assert recovered.raan_rad == pytest.approx(original.raan_rad, rel=1e-6)
    assert recovered.arg_periapsis_rad == pytest.approx(
        original.arg_periapsis_rad,
        rel=1e-6,
    )
    # Mean anomaly should match at the same epoch
    assert recovered.mean_anomaly_rad == pytest.approx(
        original.mean_anomaly_rad,
        rel=1e-6,
    )


def test_multiple_round_trips() -> None:
    """Test round-trip for various orbital configurations."""
    test_cases = [
        # (a, e, i, raan, omega, M)
        (7_000_000, 0.001, 0.1, 0.5, 1.0, 0.0),  # Nearly circular, low inclination
        (10_000_000, 0.3, 1.0, 2.0, 3.0, 1.5),  # Elliptical, high inclination
        (42_164_000, 0.001, 0.0, 0.0, 0.0, 0.0),  # GEO-like
        (6_800_000, 0.0001, math.radians(51.6), 1.0, 0.5, 2.0),  # ISS-like
    ]

    for a, e, i, raan, omega, m0 in test_cases:
        original = OrbitalElements(
            semi_major_axis_m=a,
            eccentricity=e,
            inclination_rad=i,
            raan_rad=raan,
            arg_periapsis_rad=omega,
            mean_anomaly_rad=m0,
            epoch_unix_s=0.0,
        )

        state = elements_to_state(original, 0.0)
        assert not isinstance(state, list)
        recovered = state_to_elements(state, 0.0)

        assert recovered.semi_major_axis_m == pytest.approx(a, rel=1e-6)
        assert recovered.eccentricity == pytest.approx(e, abs=1e-8)
        assert recovered.inclination_rad == pytest.approx(i, rel=1e-6)


def test_position_velocity_to_elements() -> None:
    """Test convenience function with raw vectors."""
    position = np.array([7_000_000.0, 0.0, 0.0])
    velocity = np.array([0.0, 7546.0, 0.0])

    elements = position_velocity_to_elements(position, velocity, unix_s=0.0)

    # Should be nearly circular
    assert elements.eccentricity < 0.01
    # Inclination should be equatorial
    assert elements.inclination_rad == pytest.approx(0.0, abs=0.01)


def test_degenerate_orbit_raises() -> None:
    """Test that degenerate orbits raise ValueError."""
    # Zero angular momentum (radial trajectory)
    state = OrbitState(
        position_eci_m=np.array([7_000_000.0, 0.0, 0.0]),
        velocity_eci_ms=np.array([1000.0, 0.0, 0.0]),  # Radial velocity
        acceleration_eci_ms2=np.zeros(3),
    )

    with pytest.raises(ValueError, match="angular momentum"):
        state_to_elements(state, 0.0)


def test_hyperbolic_orbit_raises() -> None:
    """Test that hyperbolic orbits raise ValueError."""
    # Very high velocity -> escape trajectory
    state = OrbitState(
        position_eci_m=np.array([7_000_000.0, 0.0, 0.0]),
        velocity_eci_ms=np.array([0.0, 20000.0, 0.0]),  # Escape velocity
        acceleration_eci_ms2=np.zeros(3),
    )

    with pytest.raises(ValueError, match="eccentricity"):
        state_to_elements(state, 0.0)


def test_epoch_is_preserved() -> None:
    """Test that epoch is set correctly in output."""
    state = _make_circular_state(7_000_000)
    epoch = 1704067200.0

    elements = state_to_elements(state, epoch)

    assert elements.epoch_unix_s == epoch


def test_energy_conservation() -> None:
    """Test that specific orbital energy is consistent."""
    state = _make_circular_state(8_000_000, math.radians(30))
    elements = state_to_elements(state, 0.0)

    # Specific energy from state: v^2/2 - mu/r
    r = np.linalg.norm(state.position_eci_m)
    v = np.linalg.norm(state.velocity_eci_ms)
    energy_from_state = v**2 / 2 - GM_EARTH / r

    # Specific energy from elements: -mu/(2a)
    energy_from_elements = -GM_EARTH / (2 * elements.semi_major_axis_m)

    assert energy_from_state == pytest.approx(energy_from_elements, rel=1e-6)


def test_angular_momentum_conservation() -> None:
    """Test that specific angular momentum is consistent."""
    state = _make_circular_state(7_500_000, math.radians(45))
    elements = state_to_elements(state, 0.0)

    # Angular momentum from state: |r x v|
    h_from_state = np.linalg.norm(
        np.cross(state.position_eci_m, state.velocity_eci_ms),
    )

    # Angular momentum from elements: sqrt(mu * a * (1 - e^2))
    a = elements.semi_major_axis_m
    e = elements.eccentricity
    h_from_elements = math.sqrt(GM_EARTH * a * (1 - e**2))

    assert h_from_state == pytest.approx(h_from_elements, rel=1e-6)
