"""Tests for SDK signal helpers and Sensor.configure runtime settings."""

from __future__ import annotations

import numpy as np
import pytest
from scipy.signal import butter, filtfilt

from orisys import Sensor
from orisys.signal_processing import (
    NORMAL_FORCE_CAL_OFFSET,
    NORMAL_FORCE_CAL_SLOPE,
    LowpassFilter,
    calibrate_normal_force,
    lowpass_filter,
)


def _legacy_viewer_lowpass(data, cutoff_hz, sample_rate_hz=30.0, order=2):
    """Reference copy of the previous viewer2d Butterworth + filtfilt path."""
    y = np.asarray(data, dtype=np.float64)
    if cutoff_hz == 0:
        return y.copy()
    if y.size == 0:
        return y
    nyquist = 0.5 * sample_rate_hz
    b, a = butter(order, cutoff_hz / nyquist, btype="low")
    padlen = 3 * (max(len(b), len(a)) - 1)
    if y.size <= padlen:
        return y.copy()
    return filtfilt(b, a, y)


def test_batch_lowpass_matches_legacy_viewer_filter():
    rng = np.random.default_rng(0)
    t = np.arange(180, dtype=np.float64) / 30.0
    signal = np.sin(2 * np.pi * 1.0 * t) + 0.4 * rng.normal(size=t.size)
    cutoff = 5.0
    actual = lowpass_filter(signal, cutoff_hz=cutoff, sample_rate_hz=30.0, order=2)
    expected = _legacy_viewer_lowpass(signal, cutoff_hz=cutoff, sample_rate_hz=30.0, order=2)
    np.testing.assert_allclose(actual, expected, rtol=1e-10, atol=1e-10)


def test_batch_lowpass_cutoff_zero_is_noop():
    data = np.array([1.0, 2.0, 3.0])
    out = lowpass_filter(data, cutoff_hz=0)
    np.testing.assert_array_equal(out, data)
    assert out is not data


def test_stateful_lowpass_reduces_high_frequency_noise():
    rng = np.random.default_rng(1)
    n = 240
    t = np.arange(n, dtype=np.float64) / 30.0
    clean = np.sin(2 * np.pi * 0.5 * t)
    noisy = clean + 0.8 * rng.normal(size=n)

    filt = LowpassFilter(cutoff_hz=3.0, sample_rate_hz=30.0, order=2, n_channels=1)
    filtered = np.array([float(filt.process([v])[0]) for v in noisy])

    assert np.isfinite(filtered).all()
    noisy_hf = float(np.std(np.diff(noisy)))
    filtered_hf = float(np.std(np.diff(filtered)))
    assert filtered_hf < 0.5 * noisy_hf
    # Steady-state init on the first sample should not start at zero.
    assert filtered[0] == pytest.approx(noisy[0])


def test_calibrate_normal_force_preserves_linear_curve():
    raw = 12345.0
    expected = NORMAL_FORCE_CAL_SLOPE * raw + NORMAL_FORCE_CAL_OFFSET
    assert calibrate_normal_force(raw) == pytest.approx(expected)
    assert calibrate_normal_force(0.0) == pytest.approx(NORMAL_FORCE_CAL_OFFSET)


def _external_sensor():
    return Sensor("external", config_name="box", verbose=False, backend="cpu")


def test_configure_lowpass_filters_read_info_and_keeps_raw():
    sensor = _external_sensor()
    try:
        sensor._force_raw = (1000.0, -200.0, 50.0)
        sensor._apply_force_filter()
        fn, fx, fy = sensor.read_info(
            sensor.info.FNORMAL,
            sensor.info.FSHEARX,
            sensor.info.FSHEARY,
        )
        assert (fn, fx, fy) == pytest.approx((1000.0, -200.0, 50.0))

        sensor.configure(lowpass_cutoff_hz=5.0, lowpass_sample_rate_hz=30.0)
        noisy = [(1000.0 + (200.0 if i % 2 else -200.0), -200.0, 50.0) for i in range(40)]
        filtered_fn = []
        raw_fn = []
        for sample in noisy:
            sensor._force_raw = sample
            sensor._apply_force_filter()
            filtered_fn.append(sensor.read_info(sensor.info.FNORMAL)[0])
            raw_fn.append(sensor.read_info(sensor.info.FNORMAL_RAW)[0])

        assert raw_fn[-1] == pytest.approx(noisy[-1][0])
        assert filtered_fn[-1] != pytest.approx(raw_fn[-1])
        assert float(np.std(np.diff(filtered_fn))) < float(np.std(np.diff(raw_fn)))
        fx_raw, fy_raw = sensor.read_info(sensor.info.FSHEARX_RAW, sensor.info.FSHEARY_RAW)
        assert fx_raw == pytest.approx(-200.0)
        assert fy_raw == pytest.approx(50.0)
    finally:
        sensor.disconnect()


def test_configure_lowpass_disabled_or_zero_cutoff_returns_raw():
    sensor = _external_sensor()
    try:
        sensor.configure(lowpass_cutoff_hz=5.0)
        sensor._force_raw = (10.0, 20.0, 30.0)
        sensor._apply_force_filter()

        sensor.configure(lowpass_cutoff_hz=0)
        sensor._force_raw = (111.0, 222.0, 333.0)
        sensor._apply_force_filter()
        fn, fx, fy = sensor.read_info(
            sensor.info.FNORMAL,
            sensor.info.FSHEARX,
            sensor.info.FSHEARY,
        )
        assert (fn, fx, fy) == pytest.approx((111.0, 222.0, 333.0))

        sensor.configure(lowpass_cutoff_hz=5.0, lowpass_enabled=False)
        sensor._force_raw = (7.0, 8.0, 9.0)
        sensor._apply_force_filter()
        fn, fx, fy = sensor.read_info(
            sensor.info.FNORMAL,
            sensor.info.FSHEARX,
            sensor.info.FSHEARY,
        )
        fn_raw = sensor.read_info(sensor.info.FNORMAL_RAW)[0]
        assert (fn, fx, fy) == pytest.approx((7.0, 8.0, 9.0))
        assert fn_raw == pytest.approx(7.0)
    finally:
        sensor.disconnect()


def test_configure_hhd_downsample_invalidates_solver_cache():
    sensor = _external_sensor()
    try:
        sensor._hhd_downsample = 1
        sensor._Sensor__hhd = object()
        sensor._hhd_grid_shape = (40, 40)
        sensor.valid_mask = np.ones((90, 90), dtype=bool)
        sensor.valid_mask_hhd = sensor.valid_mask

        sensor.configure(hhd_downsample=3)
        assert sensor._hhd_downsample == 3
        assert sensor._Sensor__hhd is None
        assert sensor._hhd_grid_shape is None
        assert sensor.valid_mask_hhd is not None
        assert sensor.valid_mask_hhd.shape == (30, 30)

        with pytest.raises(ValueError):
            sensor.configure(hhd_downsample="nope")
    finally:
        sensor.disconnect()