"""Tests for finite_wordlength.py."""

import numpy as np
from scipy import signal

from finite_wordlength import (
    quantization_step,
    quantize,
    sqnr_db,
    theoretical_sqnr_db,
    quantize_coeffs,
    is_stable,
    sos_is_stable,
    first_order_zero_input,
    shift_scale,
    c_div_pow2,
    shift_scale_mean_error,
    ema_shift,
    ema_shift_final_value,
    saturate,
    wrap,
    companion_form,
    normal_form,
    block_normal_form,
    lyapunov_margin,
    zero_input_orbit,
    sustains_oscillation,
    orbit_period,
)


def test_quantization_error_bounded_by_half_step():
    # Stay inside the representable range so this tests rounding (granular) error,
    # not overload/clipping: at 4 bits the top grid value is 0.875.
    x = np.linspace(-0.85, 0.85, 5000)
    for n_bits in (4, 8, 12):
        err = np.abs(x - quantize(x, n_bits))
        # round-to-nearest: error never exceeds half a step (allow tiny float slack)
        assert np.max(err) <= quantization_step(n_bits) / 2 * (1 + 1e-9)


def test_quantized_values_lie_on_the_grid():
    c = np.array([0.371, -1.23, 0.04, 1.999])
    cq = quantize_coeffs(c, 10, c_max=2.0)
    step = quantization_step(10, 2.0)
    assert np.allclose(cq / step, np.round(cq / step))


def test_sqnr_matches_602n_plus_176():
    n = np.arange(40000)
    sine = 0.999 * np.sin(2 * np.pi * n / 200)   # full-scale sinusoid
    for n_bits in (8, 12, 16):
        measured = sqnr_db(sine, quantize(sine, n_bits))
        assert abs(measured - theoretical_sqnr_db(n_bits)) < 1.0


def test_is_stable_detects_pole_outside_unit_circle():
    assert is_stable([1.0, -0.5])           # pole at 0.5
    assert not is_stable([1.0, -1.5])       # pole at 1.5, outside


def test_direct_form_goes_unstable_while_sos_survives():
    # Manolakis Example 15.6 elliptic bandpass: the headline coefficient-
    # quantization result. Direct (b, a) form goes unstable at 10 bits; the
    # same filter as cascaded second-order sections stays stable.
    order, wn = signal.ellipord([0.3, 0.4], [0.2, 0.5], 0.1, 60)
    b, a = signal.ellip(order, 0.1, 60, wn, btype='band')
    sos = signal.ellip(order, 0.1, 60, wn, btype='band', output='sos')

    n_bits = 10
    # direct-form denominator spans a wide range, so it needs many integer bits
    c_max = 2 ** np.ceil(np.log2(np.max(np.abs(a)) + 1.0))
    a_q = quantize_coeffs(np.asarray(a), n_bits, c_max=c_max)
    sos_q = np.array([quantize_coeffs(s, n_bits, c_max=2.0) for s in sos])

    assert is_stable(a)                 # the unquantized filter is stable
    assert not is_stable(a_q)           # direct form quantized: unstable
    assert sos_is_stable(sos_q)         # SOS quantized: still stable


def test_limit_cycle_appears_near_unit_circle_and_not_far_from_it():
    # Pole close to the unit circle with a coarse quantizer locks into a
    # nonzero dead band; a pole well inside decays cleanly to zero.
    near = first_order_zero_input(a=0.9, y0=0.5, n_bits=6, n_samples=300)
    far = first_order_zero_input(a=0.5, y0=0.5, n_bits=6, n_samples=300)
    assert abs(near[-1]) > quantization_step(6)   # sustained, larger than one step
    assert far[-1] == 0.0                          # decays to exactly zero


# --- Scaling by shifts -----------------------------------------------------
#
# These pin the claims the topic page makes in prose about C integer semantics.
# The page models C in Python, which is exactly the trap that bit the bit-exact
# CUSUM model, so the arithmetic is asserted here rather than only narrated.


def test_shift_is_not_negation_symmetric():
    # The reader-reported identity: ((-n) >> 1) != -(n >> 1) for odd n.
    n = np.array([1, 3, 5, 7], dtype=np.int64)
    assert not np.array_equal(shift_scale(-n, 1), -shift_scale(n, 1))
    # Concretely, for n = 1: (-1) >> 1 == -1 while -(1 >> 1) == 0.
    assert shift_scale(np.array([-1]), 1)[0] == -1
    assert -shift_scale(np.array([1]), 1)[0] == 0


def test_shift_floors_while_c_division_truncates():
    x = np.array([-5, -3, -1, 0, 1, 3, 5], dtype=np.int64)
    # Shift floors toward minus infinity.
    assert list(shift_scale(x, 1)) == [-3, -2, -1, 0, 0, 1, 2]
    # C's '/' truncates toward zero, so it differs on every negative odd value.
    assert list(c_div_pow2(x, 1)) == [-2, -1, 0, 0, 0, 1, 2]
    assert not np.array_equal(shift_scale(x, 1), c_div_pow2(x, 1))


def test_shift_error_has_a_dc_offset_but_rounding_noise_variance():
    rng = np.random.default_rng(0)
    x = rng.integers(-(1 << 20), 1 << 20, size=1 << 18).astype(np.int64)
    for k in (1, 4, 8):
        exact = x / (1 << k)
        err = shift_scale(x, k) - exact
        # The mean matches the closed form: a DC offset approaching -1/2 LSB.
        assert abs(err.mean() - shift_scale_mean_error(k)) < 0.01
        # The half-LSB bias leaves a residual that vanishes with k.
        err_r = shift_scale(x, k, rounded=True) - exact
        assert abs(err_r.mean() - shift_scale_mean_error(k, rounded=True)) < 0.01
        # Both carry the same Delta^2/12 noise power (Delta = 1 output LSB).
        if k >= 4:
            assert abs(err.var() - 1 / 12) < 0.005
            assert abs(err_r.var() - 1 / 12) < 0.005


def test_c_division_is_unbiased_on_symmetric_data():
    # Truncation toward zero is unbiased on a symmetric distribution, but it is
    # nonlinear near zero (a dead zone), which is the opposite failure to the shift.
    x = np.arange(-(1 << 16), 1 << 16, dtype=np.int64)
    err = c_div_pow2(x, 4) - x / 16
    assert abs(err.mean()) < 1e-9


def test_shift_ema_settles_short_of_its_target():
    for k, target in ((4, 1000), (6, 1000), (4, 10)):
        y = ema_shift(np.full(600, target, dtype=np.int64), k)
        assert y[-1] == ema_shift_final_value(target, k)
    # The dead band is 2**k - 1 counts wide.
    assert ema_shift_final_value(1000, 4) == 1000 - 15
    assert ema_shift_final_value(1000, 6) == 1000 - 63


def test_shift_ema_ignores_a_step_smaller_than_its_dead_band():
    # A step of 10 with k = 4 never moves the filter off zero at all.
    y = ema_shift(np.full(600, 10, dtype=np.int64), 4)
    assert y[-1] == 0
    # Adding the rounding bias gets it moving, though still not to the target.
    y_r = ema_shift(np.full(600, 10, dtype=np.int64), 4, rounded=True)
    assert 0 < y_r[-1] < 10


def test_c_div_pow2_matches_c_at_the_most_negative_int64():
    # abs(INT64_MIN) is not representable, so a sign-and-magnitude implementation
    # wraps and returns the wrong SIGN here. Verified against compiled C:
    # INT64_MIN / 2 == -4611686018427387904.
    lo = np.iinfo(np.int64).min
    assert c_div_pow2(np.array([lo]), 1)[0] == -4611686018427387904
    assert shift_scale(np.array([lo]), 1)[0] == -4611686018427387904
    # One above the floor, where the magnitude IS representable.
    assert c_div_pow2(np.array([lo + 1]), 1)[0] == -4611686018427387903


def test_shift_ema_dead_band_is_one_sided():
    # The dead band exists only when climbing. Falling, the residual is negative,
    # and an arithmetic shift of a negative residual is -1 and never 0, so the
    # filter converges exactly. This is the page's own asymmetry applied to itself.
    for k in (2, 4, 6):
        rising = ema_shift(np.full(4000, 1000, dtype=np.int64), k, y0=0)
        assert rising[-1] == 1000 - ((1 << k) - 1)
        falling = ema_shift(np.full(4000, 0, dtype=np.int64), k, y0=1000)
        assert falling[-1] == 0                      # exact, no shortfall
        assert ema_shift_final_value(0, k, y0=1000) == 0


def test_ema_final_value_predicts_both_directions_and_both_modes():
    rng = np.random.default_rng(7)
    for _ in range(60):
        k = int(rng.integers(1, 8))
        y0 = int(rng.integers(-800, 800))
        target = int(rng.integers(-800, 800))
        rounded = bool(rng.integers(0, 2))
        y = ema_shift(np.full(9000, target, dtype=np.int64), k,
                      rounded=rounded, y0=y0)
        assert y[-1] == ema_shift_final_value(target, k, y0, rounded)


# --- Overflow oscillations in state space -----------------------------------
#
# The filter used throughout: a stable 3rd-order lowpass-ish set of poles whose
# DIRECT-FORM realization sustains a full-scale overflow oscillation under
# SATURATION arithmetic. That combination is the interesting one, because
# saturation is exactly what is supposed to make overflow safe -- and does, for
# second-order sections (Ebert, Mazo and Taylor 1969), which is why the
# counterexample has to be third order.
A3 = [2.4364, -2.3099, 0.8597]


def _poles_of(a):
    return np.roots(np.concatenate(([1.0], -np.asarray(a, dtype=float))))


def test_both_overflow_rules_obey_the_bound_the_lyapunov_argument_needs():
    # This test exists because the plausible story is wrong. It is tempting to
    # say wraparound oscillates "because it can make a value bigger" -- it
    # cannot. Its output is bounded by the range and it is the identity inside
    # it, so |Q(v)| <= |v| holds for wraparound exactly as for saturation, and
    # the state-space condition therefore covers two's-complement arithmetic.
    v = np.linspace(-4, 4, 4001)
    assert np.all(np.abs(saturate(v)) <= np.abs(v) + 1e-15)
    assert np.all(np.abs(wrap(v)) <= np.abs(v) + 1e-12)
    assert np.max(np.abs(wrap(v))) <= 1.0
    # What wraparound does destroy is sign and continuity: a state just over the
    # top of the range comes back at the bottom of it.
    assert wrap(1.01) < 0 < 1.01
    assert np.allclose(wrap(np.linspace(-0.99, 0.99, 51)), np.linspace(-0.99, 0.99, 51))


def test_normal_form_has_norm_equal_to_the_pole_radius():
    for r in (0.5, 0.9, 0.99):
        for theta in (0.1, np.pi / 4, np.pi / 2, 3.0):
            A = normal_form(r, theta)
            assert np.allclose(A.T @ A, r ** 2 * np.eye(2))
            assert abs(np.linalg.norm(A, 2) - r) < 1e-12
            assert np.allclose(sorted(np.abs(np.linalg.eigvals(A))), [r, r])


def test_direct_form_norm_is_never_below_one_from_order_two_up():
    # From order 2 the delay line puts a row of the identity into A, so
    # ||A||_2 >= 1 however deep inside the unit circle the poles are.
    for a in ([0.9, -0.2], A3, [0.1, 0.1, 0.1, 0.1]):
        assert np.linalg.norm(companion_form(a), 2) >= 1.0 - 1e-12
    # Order 1 is the exception, and it is not a rounding-error edge case: there
    # is no delay row at all, so a stable first-order direct form MEETS the
    # condition and provably cannot sustain an overflow oscillation.
    assert np.linalg.norm(companion_form([0.5]), 2) == 0.5
    assert lyapunov_margin(companion_form([0.5])) < 0


def test_block_normal_form_is_the_same_filter_with_a_smaller_norm():
    poles = _poles_of(A3)
    Ac, Ab = companion_form(A3), block_normal_form(poles)
    assert np.allclose(np.sort_complex(np.linalg.eigvals(Ac)),
                       np.sort_complex(np.linalg.eigvals(Ab)))
    assert abs(np.linalg.norm(Ab, 2) - np.max(np.abs(poles))) < 1e-9
    assert np.linalg.norm(Ac, 2) > 3.0


def test_the_filter_is_genuinely_stable_before_any_quantizer():
    # Without this the headline test below proves nothing: an unstable filter
    # would "oscillate" for reasons that have nothing to do with overflow.
    assert np.max(np.abs(_poles_of(A3))) < 1.0
    assert is_stable(np.concatenate(([1.0], -np.asarray(A3))))
    x = np.ones(3)
    A = companion_form(A3)
    for _ in range(2000):
        x = A @ x
    assert np.linalg.norm(x) < 1e-20


def test_direct_form_sustains_an_overflow_oscillation_under_saturation():
    found, orbit = sustains_oscillation(companion_form(A3), saturate, seed=11)
    assert found
    assert np.max(np.abs(orbit)) > 0.99          # full scale, not a small wobble
    # exactly periodic, not a slow decay that the watch window is too short to see
    assert np.allclose(orbit[-1], orbit[-7], atol=1e-12)


def test_block_normal_form_provably_cannot_oscillate_and_does_not():
    Ab = block_normal_form(_poles_of(A3))
    # The proof: a negative margin means V(x) = x.T x strictly decreases.
    assert lyapunov_margin(Ab) < 0
    assert not sustains_oscillation(Ab, saturate, seed=11)[0]
    assert not sustains_oscillation(Ab, saturate, seed=5)[0]


def test_an_oscillation_rules_out_every_diagonal_lyapunov_certificate():
    # Contrapositive, and the reason the condition is worth stating: the direct
    # form oscillates, so no diagonal D can satisfy it. Checked here for D = I.
    Ac = companion_form(A3)
    assert sustains_oscillation(Ac, saturate, seed=11)[0]
    assert lyapunov_margin(Ac) > 0


def test_second_order_direct_form_needs_only_saturation():
    # Ebert, Mazo and Taylor: for a SECOND-order section, saturation alone
    # removes overflow oscillations -- no change of structure required. Both
    # halves are asserted, because the point is the contrast between them, and
    # neither is available from the norm condition: this A fails that condition
    # under both arithmetics, yet oscillates under only one of them.
    r, theta = 0.95, np.pi / 4
    a = [2 * r * np.cos(theta), -r ** 2]
    A = companion_form(a)
    assert lyapunov_margin(A) > 0
    assert sustains_oscillation(A, wrap, seed=3)[0]
    assert not sustains_oscillation(A, saturate, seed=3)[0]


def test_the_normal_form_is_safe_under_wraparound_too():
    # The payoff of getting the wraparound bound right: a negative margin covers
    # two's-complement arithmetic as well, so the normal form needs no saturating
    # adder to be overflow-safe. Same poles as the direct form above, which does
    # oscillate under wraparound.
    r, theta = 0.95, np.pi / 4
    A = normal_form(r, theta)
    assert lyapunov_margin(A) < 0
    assert not sustains_oscillation(A, wrap, seed=3)[0]
    assert not sustains_oscillation(A, saturate, seed=3)[0]


def test_zero_input_orbit_settles_before_it_reports():
    A = block_normal_form(_poles_of(A3))
    # The slowest pole is 0.9671, so 800 settling steps leave 0.9671**800 ~ 1e-11.
    late = zero_input_orbit(A, [1.0, 1.0, 1.0], saturate, n_samples=50, n_settle=800)
    assert np.max(np.abs(late)) < 1e-9


def test_a_second_order_direct_form_can_carry_a_diagonal_certificate():
    # Regression guard. The page once claimed a direct form of order >= 2 "can
    # never satisfy" the condition, which is true only for D = I. The condition
    # quantifies over diagonal D, and here is one that works.
    A = companion_form([0.3, 0.3])
    assert np.max(np.abs(np.linalg.eigvals(A))) < 1
    assert lyapunov_margin(A) > 0                    # fails at D = I
    assert lyapunov_margin(A, [1.0, 0.5]) < 0        # passes at D = diag(1, 0.5)
    # and having a certificate, it cannot oscillate under either arithmetic
    assert not sustains_oscillation(A, wrap, seed=2)[0]
    assert not sustains_oscillation(A, saturate, seed=2)[0]


def test_the_diagonal_condition_reproduces_the_classical_second_order_boundary():
    # The reason the condition is worth stating at all: for the second-order
    # direct form, "a diagonal D exists" is exactly |a1| + |a2| < 1, the
    # classical overflow-oscillation condition, recovered from a matrix
    # inequality that knows nothing about biquads.
    grid = np.logspace(-4, 4, 401)
    rng = np.random.default_rng(0)
    checked = 0
    while checked < 150:
        a1, a2 = rng.uniform(-2.2, 2.2), rng.uniform(-1.05, 1.05)
        if abs(a2) >= 1 or abs(a1) >= 1 + a2:        # stability triangle
            continue
        A = companion_form([a1, a2])
        has_cert = any(lyapunov_margin(A, [1.0, t]) < 0 for t in grid)
        assert has_cert == (abs(a1) + abs(a2) < 1), (a1, a2)
        checked += 1


def test_saturation_does_not_save_every_second_order_structure():
    # Ebert, Mazo and Taylor is about the second-order DIRECT FORM. Stated for
    # "any second-order structure" it is false, and this is the counterexample:
    # a stable 2x2 realization with a period-2 orbit pinned at the rails.
    A = np.array([[-1.3516, -0.3035], [2.2327, 0.8368]])
    assert np.max(np.abs(np.linalg.eigvals(A))) < 1
    orbit = zero_input_orbit(A, [1.0, -1.0], saturate, n_samples=20, n_settle=50000)
    assert orbit_period(orbit) == 2
    assert np.max(np.abs(orbit)) == 1.0
    x = np.array([1.0, -1.0])                        # the linear part decays
    for _ in range(2000):
        x = A @ x
    assert np.linalg.norm(x) < 1e-15


def test_orbit_period_rejects_a_decaying_transient():
    # The detector's own defect, guarded. Amplitude after settling is not
    # evidence: a filter with poles at 0.999 is still large after 300 steps and
    # decays to nothing given 100000. Only exact periodicity separates them.
    r, theta = 0.999, 0.3
    A = companion_form([2 * r * np.cos(theta), -r**2])
    slow = zero_input_orbit(A, [0.9, -0.6], saturate, n_samples=200, n_settle=300)
    assert np.max(np.abs(slow)) > 0.3                # would once have been "found"
    assert orbit_period(slow) == 0                   # but it is not periodic
    assert not sustains_oscillation(A, saturate, seed=0)[0]
    gone = zero_input_orbit(A, [0.9, -0.6], saturate, n_samples=10, n_settle=100000)
    assert np.max(np.abs(gone)) < 1e-30              # it was only decaying


def test_block_normal_form_refuses_repeated_poles():
    # It would otherwise return a realization of a different filter, and pass an
    # eigenvalue check while doing it.
    import pytest
    with pytest.raises(ValueError, match="repeated pole"):
        block_normal_form([0.6, 0.6])


def test_second_order_sections_fix_the_third_order_counterexample():
    # The practical point, and the one a reader should act on: the cascade this
    # page recommends from its first paragraph removes the oscillation with no
    # state-space argument at all.
    sos = signal.tf2sos([1.0, 0, 0, 0], np.concatenate(([1.0], -np.asarray(A3))))
    rng = np.random.default_rng(0)
    n = 400
    st = rng.uniform(-1, 1, (sos.shape[0], 2, n))
    for _ in range(3000):
        v = np.zeros(n)
        for i in range(sos.shape[0]):
            b0, b1, b2, _, a1_, a2_ = sos[i]
            y = saturate(b0 * v + st[i, 0])
            st[i, 0] = saturate(b1 * v - a1_ * y + st[i, 1])
            st[i, 1] = saturate(b2 * v - a2_ * y)
            v = y
    assert np.max(np.abs(st)) < 1e-40
    assert sustains_oscillation(companion_form(A3), saturate, seed=11)[0]
