"""Tests for Frequency Difference of Arrival (FDOA) observable computation.

Tests verify that get_fdoa correctly computes the frequency difference of signal
arrival at two collectors due to Doppler shift from emitter motion.
"""

import numpy as np

from gri_utils.constants import C
from gri_utils.observables import get_fdoa

# Basic computation


def test_fdoa_basic_stationary_emitter() -> None:
    """Verify FDOA is zero when emitter is stationary."""
    emit_xyz = np.array([6378137.0, 0.0, 0.0])
    emit_vel = np.array([0.0, 0.0, 0.0])
    c1_xyz = np.array([6378137.0, -1000.0, 0.0])
    c2_xyz = np.array([6378137.0, 1000.0, 0.0])
    freq_hz = 1e9  # 1 GHz

    fdoa = get_fdoa(emit_xyz, emit_vel, c1_xyz, c2_xyz, freq_hz)

    assert np.isclose(fdoa, 0.0, atol=1e-10)


def test_fdoa_perpendicular_velocity() -> None:
    """Verify FDOA is zero when velocity is perpendicular to both collectors."""
    # Emitter at origin, collectors on Y axis, velocity along Z
    emit_xyz = np.array([6378137.0, 0.0, 0.0])
    emit_vel = np.array([0.0, 0.0, 100.0])  # 100 m/s in Z direction
    c1_xyz = np.array([6378137.0, -1000.0, 0.0])
    c2_xyz = np.array([6378137.0, 1000.0, 0.0])
    freq_hz = 1e9

    fdoa = get_fdoa(emit_xyz, emit_vel, c1_xyz, c2_xyz, freq_hz)

    # Perpendicular velocity gives equal Doppler at both collectors
    assert np.isclose(fdoa, 0.0, atol=1e-10)


def test_fdoa_positive_when_moving_toward_c1() -> None:
    """Verify FDOA is positive when emitter moves toward collector1."""
    emit_xyz = np.array([6378137.0, 0.0, 0.0])
    emit_vel = np.array([0.0, -100.0, 0.0])  # Moving toward c1 (negative Y)
    c1_xyz = np.array([6378137.0, -1000.0, 0.0])
    c2_xyz = np.array([6378137.0, 1000.0, 0.0])
    freq_hz = 1e9

    fdoa = get_fdoa(emit_xyz, emit_vel, c1_xyz, c2_xyz, freq_hz)

    # Positive FDOA means higher Doppler at collector1
    assert fdoa > 0


def test_fdoa_negative_when_moving_toward_c2() -> None:
    """Verify FDOA is negative when emitter moves toward collector2."""
    emit_xyz = np.array([6378137.0, 0.0, 0.0])
    emit_vel = np.array([0.0, 100.0, 0.0])  # Moving toward c2 (positive Y)
    c1_xyz = np.array([6378137.0, -1000.0, 0.0])
    c2_xyz = np.array([6378137.0, 1000.0, 0.0])
    freq_hz = 1e9

    fdoa = get_fdoa(emit_xyz, emit_vel, c1_xyz, c2_xyz, freq_hz)

    # Negative FDOA means higher Doppler at collector2
    assert fdoa < 0


def test_fdoa_magnitude_calculation() -> None:
    """Verify FDOA magnitude matches expected Doppler formula."""
    # Emitter equidistant from collectors, moving directly toward c1
    emit_xyz = np.array([6378137.0, 0.0, 0.0])
    emit_vel = np.array([0.0, -100.0, 0.0])  # 100 m/s toward c1
    c1_xyz = np.array([6378137.0, -1000.0, 0.0])
    c2_xyz = np.array([6378137.0, 1000.0, 0.0])
    freq_hz = 1e9

    fdoa = get_fdoa(emit_xyz, emit_vel, c1_xyz, c2_xyz, freq_hz)

    # Unit vectors from emitter to collectors are [0, -1, 0] and [0, 1, 0]
    # v_radial1 = [0, -100, 0] · [0, -1, 0] = 100 m/s
    # v_radial2 = [0, -100, 0] · [0, 1, 0] = -100 m/s
    # FDOA = f0/c * (100 - (-100)) = f0/c * 200
    expected_fdoa = freq_hz / C * 200.0
    assert np.isclose(fdoa, expected_fdoa, rtol=1e-10)


def test_fdoa_scales_with_frequency() -> None:
    """Verify FDOA scales linearly with carrier frequency."""
    emit_xyz = np.array([6378137.0, 0.0, 0.0])
    emit_vel = np.array([0.0, -100.0, 0.0])
    c1_xyz = np.array([6378137.0, -1000.0, 0.0])
    c2_xyz = np.array([6378137.0, 1000.0, 0.0])

    fdoa_1ghz = get_fdoa(emit_xyz, emit_vel, c1_xyz, c2_xyz, 1e9)
    fdoa_2ghz = get_fdoa(emit_xyz, emit_vel, c1_xyz, c2_xyz, 2e9)

    assert np.isclose(fdoa_2ghz, 2 * fdoa_1ghz, rtol=1e-10)


def test_fdoa_scales_with_velocity() -> None:
    """Verify FDOA scales linearly with velocity magnitude."""
    emit_xyz = np.array([6378137.0, 0.0, 0.0])
    c1_xyz = np.array([6378137.0, -1000.0, 0.0])
    c2_xyz = np.array([6378137.0, 1000.0, 0.0])
    freq_hz = 1e9

    emit_vel_100 = np.array([0.0, -100.0, 0.0])
    emit_vel_200 = np.array([0.0, -200.0, 0.0])

    fdoa_100 = get_fdoa(emit_xyz, emit_vel_100, c1_xyz, c2_xyz, freq_hz)
    fdoa_200 = get_fdoa(emit_xyz, emit_vel_200, c1_xyz, c2_xyz, freq_hz)

    assert np.isclose(fdoa_200, 2 * fdoa_100, rtol=1e-10)


# Batch processing


def test_fdoa_batch_emitters() -> None:
    """Verify batch processing with multiple emitters."""
    emits_xyz = np.array(
        [
            [6378137.0, 0.0, 0.0],
            [6378137.0, 500.0, 0.0],
            [6378137.0, -500.0, 0.0],
        ],
    )
    emits_vel = np.array(
        [
            [0.0, -100.0, 0.0],  # Moving toward c1
            [0.0, 100.0, 0.0],  # Moving toward c2
            [0.0, 0.0, 100.0],  # Perpendicular (roughly)
        ],
    )
    c1_xyz = np.array([6378137.0, -1000.0, 0.0])
    c2_xyz = np.array([6378137.0, 1000.0, 0.0])
    freq_hz = 1e9

    fdoas = get_fdoa(emits_xyz, emits_vel, c1_xyz, c2_xyz, freq_hz)

    assert fdoas.shape == (3,)
    assert fdoas[0] > 0  # Moving toward c1
    assert fdoas[1] < 0  # Moving toward c2


def test_fdoa_single_returns_scalar() -> None:
    """Verify single emitter returns scalar-like value."""
    emit_xyz = np.array([6378137.0, 0.0, 0.0])
    emit_vel = np.array([0.0, -100.0, 0.0])
    c1_xyz = np.array([6378137.0, -1000.0, 0.0])
    c2_xyz = np.array([6378137.0, 1000.0, 0.0])
    freq_hz = 1e9

    fdoa = get_fdoa(emit_xyz, emit_vel, c1_xyz, c2_xyz, freq_hz)

    # Should be 0-dimensional or scalar
    assert fdoa.ndim == 0 or fdoa.shape == ()


# Input format flexibility


def test_accepts_lists() -> None:
    """Verify function accepts Python lists."""
    emit_xyz = [6378137.0, 0.0, 0.0]
    emit_vel = [0.0, 0.0, 0.0]
    c1_xyz = [6378137.0, -1000.0, 0.0]
    c2_xyz = [6378137.0, 1000.0, 0.0]
    freq_hz = 1e9

    fdoa = get_fdoa(emit_xyz, emit_vel, c1_xyz, c2_xyz, freq_hz)

    assert np.isclose(fdoa, 0.0, atol=1e-10)
