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

from __future__ import annotations

import json

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

from orisys import Sensor, configs
from orisys.signal_processing import (
    NORMAL_FORCE_CAL_OFFSET,
    NORMAL_FORCE_CAL_SLOPE,
    LowpassFilter,
    calibrate_normal_force,
    calibrate_shear_components,
    calibrate_shear_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 test_calibrate_normal_force_mini_uses_legacy_linear_file():
    raw = 10000.0
    expected = NORMAL_FORCE_CAL_SLOPE * raw + NORMAL_FORCE_CAL_OFFSET
    assert calibrate_normal_force(raw, sensor_name="mini") == pytest.approx(expected)


def test_calibrate_normal_force_box_uses_piecewise_curve():
    assert calibrate_normal_force(10000.0, sensor_name="box") == pytest.approx(0.377)
    assert calibrate_normal_force(30000.0, sensor_name="box") == pytest.approx(1.377)
    # Mid-segment interpolation between 10000→0.377 and 30000→1.377
    mid = calibrate_normal_force(20000.0, sensor_name="box")
    assert mid == pytest.approx(0.877)
    # Below first breakpoint with extrapolate=true
    assert calibrate_normal_force(0.0, sensor_name="box") == pytest.approx(0.0)


def test_calibrate_shear_force_box_piecewise():
    assert calibrate_shear_force(1000.0, sensor_name="box") == pytest.approx(0.0)
    assert calibrate_shear_force(36000.0, sensor_name="box") == pytest.approx(1.19)
    assert calibrate_shear_force(68000.0, sensor_name="box") == pytest.approx(2.06)


def test_calibrate_shear_components_box_signed():
    sx, sy = calibrate_shear_components(36000.0, -36000.0, sensor_name="box")
    assert sx == pytest.approx(1.19)
    assert sy == pytest.approx(-1.19)


def test_calibrate_shear_missing_file_returns_raw():
    assert calibrate_shear_force(1234.5, sensor_name="no-such-sensor") == pytest.approx(1234.5)
    sx, sy = calibrate_shear_components(1.0, -2.0, sensor_name="no-such-sensor")
    assert (sx, sy) == pytest.approx((1.0, -2.0))


def test_calibrate_normal_force_unknown_sensor_falls_back_to_defaults():
    raw = 5000.0
    expected = NORMAL_FORCE_CAL_SLOPE * raw + NORMAL_FORCE_CAL_OFFSET
    assert calibrate_normal_force(raw, sensor_name="no-such-sensor") == pytest.approx(expected)


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,
        )
        fn_raw, fx_raw, fy_raw = sensor.read_info(
            sensor.info.FNORMAL_RAW,
            sensor.info.FSHEARX_RAW,
            sensor.info.FSHEARY_RAW,
        )
        assert (fn_raw, fx_raw, fy_raw) == pytest.approx((1000.0, -200.0, 50.0))
        assert fn == pytest.approx(calibrate_normal_force(1000.0, sensor_name="box"))
        assert (fx, fy) == pytest.approx(
            calibrate_shear_components(-200.0, 50.0, sensor_name="box")
        )

        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 == pytest.approx(calibrate_normal_force(111.0, sensor_name="box"))
        assert (fx, fy) == pytest.approx(
            calibrate_shear_components(222.0, 333.0, sensor_name="box")
        )

        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 == pytest.approx(calibrate_normal_force(7.0, sensor_name="box"))
        assert (fx, fy) == pytest.approx(
            calibrate_shear_components(8.0, 9.0, sensor_name="box")
        )
        assert fn_raw == pytest.approx(7.0)
    finally:
        sensor.disconnect()


def test_box_read_info_applies_force_calibration_json():
    sensor = _external_sensor()
    try:
        assert sensor._force_cal_sensor_name == "box"
        sensor._force_raw = (10000.0, 36000.0, -36000.0)
        sensor._apply_force_filter()
        fn, fx, fy = sensor.read_info(
            sensor.info.FNORMAL,
            sensor.info.FSHEARX,
            sensor.info.FSHEARY,
        )
        fn_raw, fx_raw, fy_raw = sensor.read_info(
            sensor.info.FNORMAL_RAW,
            sensor.info.FSHEARX_RAW,
            sensor.info.FSHEARY_RAW,
        )
        assert (fn_raw, fx_raw, fy_raw) == pytest.approx((10000.0, 36000.0, -36000.0))
        assert fn == pytest.approx(0.377)
        assert fx == pytest.approx(1.19)
        assert fy == pytest.approx(-1.19)
    finally:
        sensor.disconnect()


def test_read_info_force_stays_raw_without_calibration_json(monkeypatch):
    monkeypatch.setattr(
        "orisys.calibration.load",
        lambda *args, **kwargs: None,
    )
    sensor = Sensor("external", config_name="box", verbose=False, backend="cpu")
    try:
        assert sensor._force_cal_sensor_name is None
        assert sensor.calibration is None
        sensor._force_raw = (111.0, -22.0, 33.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, -22.0, 33.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()


# ------------------------------------------------- bundle resolution anchors
#
# The bundle is chosen by the *running* root's folder layout, not by the root's
# name and not by whichever checkout happens to be importable. These build small
# real trees, because ``_runtime_roots`` asks the filesystem whether a directory
# carries ``examples/`` beside ``sensors/``.
_CAL_DOC = {
    "schema_version": 2,
    "unit": "N",
    "channels": {
        "normal": {"method": "piecewise_linear", "points": [[0.0, 0.0], [1000.0, 1.0]]},
        "shear_x": {
            "method": "piecewise_linear",
            "signed": True,
            "points": [[0.0, 0.0], [500.0, 2.0]],
        },
        "shear_y": {
            "method": "piecewise_linear",
            "signed": True,
            "points": [[0.0, 0.0], [500.0, 3.0]],
        },
    },
}


def _make_root(base, *, name="anything", sensor_json=True, calibration=True):
    """A runnable SDK root: ``examples/`` beside ``sensors/<name>/``.

    ``sensor.json`` is copied from the shipped bundle because it is a *sensor
    config*: a stub document would make ``_load_config`` depend on whichever
    config the machine happens to resolve, which is not what these tests are
    about.
    """
    root = base / name
    (root / "examples").mkdir(parents=True, exist_ok=True)
    (root / "examples" / "basic_pipeline.py").write_text("# demo\n", encoding="utf-8")
    bundle = root / "sensors" / "mini"
    bundle.mkdir(parents=True, exist_ok=True)
    if sensor_json:
        shipped = Path(__file__).resolve().parents[1] / "sensors" / "mini" / "sensor.json"
        (bundle / "sensor.json").write_text(
            shipped.read_text(encoding="utf-8"), encoding="utf-8"
        )
    if calibration:
        (bundle / "force_calibration.json").write_text(
            json.dumps(_CAL_DOC), encoding="utf-8"
        )
    return root


def _bare_sensor(**attrs):
    """A Sensor instance without running ``__init__``/``_load_config``.

    The note-formatting helpers touch no sensor state, so a whole sensor (and
    therefore a whole machine config) is not needed to test them.
    """
    sensor = object.__new__(Sensor)
    for key, value in attrs.items():
        setattr(sensor, key, value)
    return sensor


def test_runtime_root_anchor_keys_on_layout_not_on_the_folder_name(tmp_path):
    named = _make_root(tmp_path, name="some-random-name")
    assert configs._is_sdk_runtime_root(named) is True

    no_examples = tmp_path / "dist-like"
    (no_examples / "sensors" / "mini").mkdir(parents=True)
    assert configs._is_sdk_runtime_root(no_examples) is False

    neither = tmp_path / "plain"
    neither.mkdir()
    assert configs._is_sdk_runtime_root(neither) is False


def test_runtime_roots_walk_up_from_a_subdirectory(tmp_path, monkeypatch):
    root = _make_root(tmp_path, name="renamed-release")
    subdir = root / "examples"
    monkeypatch.setattr(configs.Path, "cwd", classmethod(lambda cls: subdir))
    roots = [str(path) for path in configs._runtime_roots()]
    assert str(root) in roots


def test_a_parent_root_never_outranks_the_root_below_it(tmp_path, monkeypatch):
    """Walking up must not let a parent's bundle shadow the real run root.

    A directory that happens to carry ``examples/`` above the launcher (a
    workspace holding several checkouts, one of which keeps an ``examples/``
    folder) used to be treated as a run root too, and its ``sensors/`` came
    first — silently shadowing the root the operator was actually running from.
    """
    parent = tmp_path / "workspace"
    inner = _make_root(parent, name="inner-release")
    (parent / "examples").mkdir(parents=True, exist_ok=True)
    assert configs._is_sdk_runtime_root(parent) is True

    monkeypatch.setattr(configs.Path, "cwd", classmethod(lambda cls: inner))
    roots = [str(path) for path in configs._runtime_roots()]
    assert roots == [str(inner)], roots


def test_the_running_root_bundle_beats_a_calibration_less_checkout(
    tmp_path, monkeypatch
):
    """The reported bug: a nearer bundle without coefficients must not win."""
    running = _make_root(tmp_path, name="running-root")
    checkout = _make_root(tmp_path, name="importable-checkout", calibration=False)
    monkeypatch.setattr(configs, "_runtime_roots", lambda: [running, checkout])
    # Pinned to real directories, so a leftover or unrelated bundle elsewhere on
    # the machine cannot decide the outcome.
    monkeypatch.setattr(
        configs,
        "_candidate_sensor_roots",
        lambda: [running / "sensors", checkout / "sensors"],
    )

    bundle = configs.resolve_sensor_bundle("mini")
    assert bundle is not None
    assert bundle.root == (running / "sensors" / "mini").resolve()
    assert bundle.force_calibration_json == (
        running / "sensors" / "mini" / "force_calibration.json"
    ).resolve()

    # The whole point: the running root's document is the one the SDK loads.
    from orisys.calibration import load as load_calibration

    cal = load_calibration(sensor_type="mini")
    assert cal is not None
    assert cal.schema_version == 2
    assert sorted(cal.channels) == ["fn", "fx", "fy"]
    assert cal.source_path == (
        running / "sensors" / "mini" / "force_calibration.json"
    ).resolve()


def test_a_nearer_bundle_wins_even_without_coefficients(tmp_path, monkeypatch):
    """Nearest bundle wins, so a calibration-less nearer one hides a farther one.

    That is the chosen precedence (the running root is authoritative), and the
    diagnostic exists precisely so this stops being silent: this is the shape of
    the original report, where a checkout's ``mini`` has only ``sensor.json``.
    """
    checkout = _make_root(tmp_path, name="checkout", calibration=False)
    running = _make_root(tmp_path, name="run-root")
    monkeypatch.setattr(configs, "_runtime_roots", lambda: [checkout, running])
    monkeypatch.setattr(
        configs,
        "_candidate_sensor_roots",
        lambda: [checkout / "sensors", running / "sensors"],
    )
    bundle = configs.resolve_sensor_bundle("mini")
    assert bundle is not None
    assert bundle.root == (checkout / "sensors" / "mini").resolve()
    assert bundle.force_calibration_json is None
    info = configs.describe_bundle_resolution("mini")
    assert info["found_root"] == (checkout / "sensors" / "mini").resolve()
    assert info["force_calibration_json"] is False


def test_describe_bundle_resolution_names_the_bundle_and_the_searched_roots(
    tmp_path, monkeypatch
):
    checkout = _make_root(tmp_path, name="checkout", calibration=False)
    running = _make_root(tmp_path, name="run-root")
    roots = [checkout / "sensors", running / "sensors"]
    # Patched at the root list, so no real bundle on this machine can satisfy
    # the lookup while the "nothing found" case is being asserted.
    monkeypatch.setattr(configs, "_candidate_sensor_roots", lambda: list(roots))

    info = configs.describe_bundle_resolution("mini")
    assert info["found_root"] == (checkout / "sensors" / "mini").resolve()
    assert info["force_calibration_json"] is False
    assert [str(path) for path in info["searched"]] == [str(p) for p in roots]

    (checkout / "sensors" / "mini" / "sensor.json").unlink()
    (running / "sensors" / "mini" / "sensor.json").unlink()
    missing = configs.describe_bundle_resolution("mini")
    assert missing["found_root"] is None
    assert missing["sensor_json"] is None
    assert missing["force_calibration_json"] is False


def test_the_calibration_note_names_the_bundle_that_was_found(tmp_path, monkeypatch):
    """The note explains a bundle with no coefficients, not a "missing file"."""
    checkout = _make_root(tmp_path, name="checkout", calibration=False)
    roots = [checkout / "sensors"]
    monkeypatch.setattr(configs, "_candidate_sensor_roots", lambda: list(roots))

    # Composed via _bare_sensor: the note reads only _force_cal_attempts, so this
    # needs no machine config and cannot fail on config resolution.
    sensor = _bare_sensor(
        _force_cal_attempts=[configs.describe_bundle_resolution("mini")],
        _force_cal_sensor_name=None,
    )
    note = sensor._force_calibration_note()
    assert "has no force_calibration.json" in note
    assert str((checkout / "sensors" / "mini").resolve()) in note
    assert "roots searched" in note

    # No bundle anywhere: say that instead of naming a root.
    (checkout / "sensors" / "mini" / "sensor.json").unlink()
    sensor = _bare_sensor(
        _force_cal_attempts=[configs.describe_bundle_resolution("mini")],
        _force_cal_sensor_name=None,
    )
    note = sensor._force_calibration_note()
    assert "no sensor bundle resolved" in note
    # A dict always keys ``found_root``, so guard its value: an unguarded copy of
    # this branch prints "bundle found at None".
    assert "None" not in note