"""Offline force feedback contracts; no controller or sensor is opened."""

from dataclasses import replace
import math

import pytest

from orisys_autocal.force_control import ForceControlConfig, ForceController
from orisys_autocal.reference_force import (
    NormalForceSample,
    WrenchSample,
    project_wrench,
)
from orisys_autocal.multipoint import normal_force_n, parse_rotation


def config(**overrides):
    values = dict(
        target_n=2.0,
        kp_mm_per_n_s=0.2,
        ki_mm_per_n_s2=0.1,
        kd_mm_per_n=0.0,
        max_force_n=5.0,
        max_speed_mm_s=0.5,
        max_depth_mm=1.0,
        max_sample_age_s=0.2,
        max_duration_s=10.0,
    )
    return ForceControlConfig(**(values | overrides))


def sample(force=2.0, seconds=1.0, **overrides):
    return NormalForceSample(
        force_n=force,
        t_host_mono_ns=int(seconds * 1e9),
        source_id="reference-1",
        calibration_id="tare-1",
        **overrides,
    )


def update(controller, force=2.0, seconds=1.0, depth=0.3, **overrides):
    return controller.update(
        sample(force, seconds, **overrides), depth_mm=depth, now_ns=int(seconds * 1e9)
    )


def test_ft_projection_uses_rotation_polarity_and_preserves_identity():
    # Proper rotation: sensor +X -> tool +Z; tool reaction is negative compression.
    rotation = parse_rotation([0, 0, -1, 0, 1, 0, 1, 0, 0])
    raw = WrenchSample(
        (-3.0, 7.0, 8.0), 123, "ft-1", "tare-4", "ft", torque_nm=(0.1, 0.2, 0.3)
    )
    measured = project_wrench(
        raw, rotation=rotation, expected_frame="ft", compression_sign=-1
    )
    assert measured.force_n == 3.0
    assert measured.t_host_mono_ns == 123
    assert measured.source_id == "ft-1" and measured.calibration_id == "tare-4"
    assert normal_force_n(-3, 7, 8, rotation) == -3.0
    assert raw.torque_nm == (0.1, 0.2, 0.3)
    with pytest.raises(ValueError, match="frame"):
        project_wrench(
            raw, rotation=rotation, expected_frame="platform", compression_sign=-1
        )


def test_projection_rejects_mirror_and_keeps_invalid_sample_invalid():
    raw = WrenchSample((1.0, 2.0, 3.0), 1, "ft", "tare", "ft", valid=False)
    projected = project_wrench(
        raw,
        rotation=((1, 0, 0), (0, 1, 0), (0, 0, 1)),
        expected_frame="ft",
        compression_sign=1,
    )
    assert not projected.valid
    with pytest.raises(ValueError, match="determinant"):
        project_wrench(
            raw,
            rotation=((-1, 0, 0), (0, 1, 0), (0, 0, 1)),
            expected_frame="ft",
            compression_sign=1,
        )


def test_force_drop_is_compensated_or_recorded_without_motion():
    active = ForceController(config(), mode="compensate")
    passive = ForceController(config(), mode="record_relaxation")
    for controller in (active, passive):
        assert update(controller).velocity_mm_s == 0
    compensation = update(active, force=1.8, seconds=1.1)
    observation = update(passive, force=1.8, seconds=1.1)
    assert compensation.velocity_mm_s > 0
    assert observation.velocity_mm_s == 0
    assert observation.force_rate_n_s == pytest.approx(-2.0)
    assert observation.sample.force_n == 1.8 and observation.depth_mm == 0.3
    assert observation.error_n == pytest.approx(0.2)


@pytest.mark.parametrize(
    "field,value",
    [
        ("kp_mm_per_n_s", math.nan),
        ("max_depth_mm", 0),
        ("max_sample_age_s", -1),
        ("target_n", 6),
        ("ki_mm_per_n_s2", -1),
        ("max_duration_s", math.inf),
    ],
)
def test_bad_configuration_is_rejected(field, value):
    with pytest.raises(ValueError):
        config(**{field: value})


@pytest.mark.parametrize(
    "fault",
    [
        "invalid",
        "stale",
        "future",
        "duplicate",
        "gap",
        "overforce",
        "depth",
        "nonfinite",
        "identity",
        "timeout",
    ],
)
def test_faults_latch_zero_output_until_explicit_reset(fault):
    controller = ForceController(config())
    update(controller)
    item, now, depth = sample(1.0, 1.1), 1_100_000_000, 0.3
    if fault == "invalid":
        item = replace(item, valid=False)
    elif fault == "stale":
        now = 1_400_000_000
    elif fault == "future":
        now = 1_000_000_000
    elif fault == "duplicate":
        item = sample(1.0, 1.0)
    elif fault == "gap":
        item, now = sample(1.0, 1.4), 1_400_000_000
    elif fault == "overforce":
        item = sample(-6.0, 1.1)
    elif fault == "depth":
        depth = 1.1
    elif fault == "nonfinite":
        item = sample(math.nan, 1.1)
    elif fault == "identity":
        item = replace(item, calibration_id="new-tare")
    elif fault == "timeout":
        controller = ForceController(config(max_duration_s=0.05))
        update(controller)
    result = controller.update(item, depth_mm=depth, now_ns=now)
    assert result.fault and result.velocity_mm_s == 0
    assert update(controller, seconds=1.2).fault == result.fault
    controller.reset()
    assert not update(controller, seconds=1.3).fault


def test_derivative_is_on_measurement_and_uses_sample_time():
    controller = ForceController(
        config(kp_mm_per_n_s=0, ki_mm_per_n_s2=0, kd_mm_per_n=0.1), mode="compensate"
    )
    update(controller)
    assert update(controller, force=1.9, seconds=1.1).velocity_mm_s == pytest.approx(
        0.1
    )


def test_saturation_does_not_accumulate_integral():
    controller = ForceController(
        config(kp_mm_per_n_s=1, ki_mm_per_n_s2=1), mode="compensate"
    )
    for index in range(10):
        assert update(controller, force=0, seconds=1 + index * 0.1).velocity_mm_s == 0.5
    assert update(controller, force=2.1, seconds=2).velocity_mm_s < 0


def test_pure_integral_can_saturate_without_losing_its_output():
    controller = ForceController(
        config(kp_mm_per_n_s=0, ki_mm_per_n_s2=10), mode="compensate"
    )
    update(controller, force=0)
    assert update(controller, force=0, seconds=1.1).velocity_mm_s == 0.5
    assert update(controller, force=2.1, seconds=1.2).velocity_mm_s < 0


def test_numerical_overflow_faults_instead_of_returning_a_velocity():
    controller = ForceController(config(kp_mm_per_n_s=1e308), mode="compensate")
    assert update(controller, force=-2).fault == "numerical_failure"


def test_depth_horizon_limits_velocity_and_boundary_faults():
    controller = ForceController(config(), mode="compensate")
    result = update(controller, force=0, depth=0.99)
    assert result.velocity_mm_s <= 0.05 + 1e-12
    assert update(controller, force=0, seconds=1.1, depth=1.0).fault


def test_relaxing_rubber_model_tracks_force_without_unbounded_depth():
    # Standard linear solid: F = k_inf*x + q, q' = k_relax*x' - q/tau.
    controller = ForceController(
        config(max_sample_age_s=0.1, kp_mm_per_n_s=0.25, ki_mm_per_n_s2=0.25),
        mode="compensate",
    )
    depth, transient = 0.4, 1.2
    initial_force = 2 * depth + transient
    peak = initial_force
    for index in range(400):
        force = 2 * depth + transient
        decision = update(
            controller, force=force, seconds=1 + index * 0.02, depth=depth
        )
        assert not decision.fault
        change = decision.velocity_mm_s * 0.02
        depth += change
        transient += 3 * change - transient * 0.02 / 0.5
        peak = max(peak, force)
    assert abs(force - 2.0) < 0.08
    assert 0.4 < depth <= 1.0
    assert peak < 2.3