import numpy as np

from galileo_rx.fixed_point.fixed_correlator import FixedCorrelator
from galileo_rx.fixed_point.fixed_nco import FixedNco
from galileo_rx.fixed_point.fixed_loop_filter import FixedSecondOrderLoopFilter
from galileo_rx.fixed_point.formats import FixedFormat
from galileo_rx.fixed_point.quantizer import Quantizer


def test_saturation_and_overflow_counter() -> None:
    quantizer = Quantizer(FixedFormat(4, 2), overflow="saturate", rounding="nearest")
    result = quantizer.quantize([-3.0, -1.0, 0.0, 1.9, 3.0])
    np.testing.assert_array_equal(result, [-8, -4, 0, 7, 7])
    assert quantizer.statistics.overflow_count == 3


def test_twos_complement_wrap() -> None:
    quantizer = Quantizer(FixedFormat(4, 0), overflow="wrap", rounding="truncate")
    np.testing.assert_array_equal(quantizer.quantize([8, 9, -9]), [-8, -7, 7])


def test_convergent_rounding_ties_to_even() -> None:
    quantizer = Quantizer(FixedFormat(8, 1), rounding="convergent")
    np.testing.assert_array_equal(quantizer.quantize([0.25, 0.75, 1.25]), [0, 2, 2])


def test_fixed_nco_phase_continuity_and_frequency_word() -> None:
    whole = FixedNco(10e6, 1e6)
    split = FixedNco(10e6, 1e6)
    c1, s1 = whole.generate(50)
    ca, sa = split.generate(17)
    cb, sb = split.generate(33)
    np.testing.assert_array_equal(c1, np.concatenate([ca, cb]))
    np.testing.assert_array_equal(s1, np.concatenate([sa, sb]))
    assert abs(whole.frequency_control_word * 10e6 / 2**32 - 1e6) < 10e6 / 2**33


def test_fixed_correlator_close_to_float() -> None:
    fs = 1e6
    count = 1000
    time_s = np.arange(count) / fs
    replica = np.where(np.arange(count) % 3, 1, -1)
    samples = 0.5 * replica * np.exp(2j * np.pi * 125e3 * time_s)
    model = FixedCorrelator(fs, 125e3)
    result = model.correlate_prompt(samples, replica)
    assert abs(result.complex_value.real - 500.0) < 5.0
    assert abs(result.complex_value.imag) < 5.0
    assert result.overflow_count == 0


def test_fixed_loop_filter_tracks_double_reference() -> None:
    from galileo_rx.tracking.loop_filters import SecondOrderLoopFilter

    fixed = FixedSecondOrderLoopFilter(2.0, 0.004)
    reference = SecondOrderLoopFilter(2.0)
    fixed_output = np.asarray([fixed.update(0.02) for _ in range(100)])
    reference_output = np.asarray([reference.update(0.02, 0.004) for _ in range(100)])
    assert np.sqrt(np.mean((fixed_output - reference_output) ** 2)) < 1e-3
    assert fixed.overflow_count == 0