from types import SimpleNamespace

import pytest

from iqdbc.car.honda.values import CAR
from iqpilot.selfdrive.controls.radard import (SCAN_CHALLENGER_STALE_CYCLES, SCAN_DISTANCE_STALE_CYCLES,
                                               SCAN_LEAD_MIN_CYCLES, SCAN_LEAD_PROB, SCAN_SWEEP_DT,
                                               KalmanParams, RadarD, Track, get_lead,
                                               scan_low_speed_candidate, track_agrees_with_model,
                                               uses_scan_radar)

DT_MDL = 0.05


def honda_cp(fingerprint=CAR.HONDA_CIVIC_BOSCH, radar_unavailable=False, brand="honda"):
  return SimpleNamespace(brand=brand, carFingerprint=fingerprint, radarUnavailable=radar_unavailable, flags=0)


def model_lead(x=30.0, y=0.0, v=10.0, prob=0.9, x_std=2.0, y_std=0.5, v_std=1.0):
  return SimpleNamespace(x=[x], y=[y], v=[v], a=[0.0], prob=prob, xStd=[x_std], yStd=[y_std], vStd=[v_std])


def make_track(identifier, d_rel, y_rel=0.0, v_rel=0.0, v_ego=10.0, cycles=SCAN_LEAD_MIN_CYCLES, measured=True):
  track = Track(identifier, v_rel + v_ego, KalmanParams(SCAN_SWEEP_DT))
  for _ in range(cycles):
    track.update(d_rel, y_rel, v_rel, v_rel + v_ego, measured)
  return track


class TestScanRadarGating:
  def test_capable_verified_car_uses_scan(self):
    assert uses_scan_radar(honda_cp())

  def test_radar_unavailable_disables(self):
    assert not uses_scan_radar(honda_cp(radar_unavailable=True))

  def test_non_family_car_never_scans(self):
    assert not uses_scan_radar(honda_cp(fingerprint=CAR.HONDA_CIVIC))
    assert not uses_scan_radar(honda_cp(fingerprint=CAR.HONDA_CIVIC_2022))

  def test_other_brand_never_scans(self):
    assert not uses_scan_radar(honda_cp(brand="toyota"))


class TestTrackMeasurementGating:
  def test_repeated_payload_not_absorbed_twice(self):
    kp = KalmanParams(SCAN_SWEEP_DT)
    absorbed = Track(1, 10.0, kp)
    starved = Track(1, 10.0, kp)
    absorbed.update(30.0, 0.0, 5.0, 15.0, True)
    starved.update(30.0, 0.0, 5.0, 15.0, True)
    v_after_first = starved.vLeadK

    for _ in range(5):
      absorbed.update(30.0, 0.0, 5.0, 15.0, True, absorb_measurement=True)
      starved.update(30.0, 0.0, 5.0, 15.0, False, absorb_measurement=False)

    assert starved.vLeadK == v_after_first
    assert absorbed.vLeadK != v_after_first
    assert starved.cnt == absorbed.cnt


class TestLowSpeedCandidate:
  def test_needs_minimum_cycles(self):
    young = make_track(1, 10.0, v_ego=2.0, cycles=SCAN_LEAD_MIN_CYCLES - 1)
    mature = make_track(2, 10.0, v_ego=2.0)
    assert not scan_low_speed_candidate(young, 2.0)
    assert scan_low_speed_candidate(mature, 2.0)

  def test_geometry_still_applies(self):
    offset = make_track(3, 10.0, y_rel=2.0, v_ego=2.0)
    assert not scan_low_speed_candidate(offset, 2.0)


class TestModelAgreement:
  def test_strict_tighter_than_relaxed(self):
    track = make_track(1, 40.0, v_ego=10.0)
    # ~15m disagreement: beyond the strict 25% envelope, inside the relaxed 40% one
    lead = model_lead(x=56.5)
    assert not track_agrees_with_model(track, lead, 10.0, strict=True)
    assert track_agrees_with_model(track, lead, 10.0, strict=False)


class TestScanLeadSelection:
  def make_cp(self):
    return honda_cp(), SimpleNamespace()

  def test_held_lead_survives_probability_dip(self):
    CP, CP_IQ = self.make_cp()
    track = make_track(7, 10.0, v_ego=2.0)
    lead = model_lead(x=11.5, v=2.0, prob=0.05)
    # raw prob is below threshold, the filtered prob is passed in above it
    result = get_lead(2.0, True, {7: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
                      scan_radar=True, filtered_prob=0.6, held_track_id=7)
    assert result['status'] and result['radarTrackId'] == 7

  def test_unrelated_closer_point_cannot_usurp_without_model_evidence(self):
    CP, CP_IQ = self.make_cp()
    held = make_track(7, 10.0, v_ego=2.0)
    interloper = make_track(9, 4.0, y_rel=0.5, v_ego=2.0)
    lead = model_lead(x=11.5, v=2.0, prob=0.9)
    result = get_lead(2.0, True, {7: held, 9: interloper}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
                      scan_radar=True, filtered_prob=0.9, held_track_id=7)
    assert result['radarTrackId'] == 7

  def test_matching_closer_point_takes_over(self):
    CP, CP_IQ = self.make_cp()
    held = make_track(7, 10.0, v_ego=2.0)
    closer = make_track(9, 4.0, v_ego=2.0)
    lead = model_lead(x=5.5, v=2.0, prob=0.9)  # model agrees with the closer car
    result = get_lead(2.0, True, {7: held, 9: closer}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
                      scan_radar=True, filtered_prob=0.9, held_track_id=7)
    assert result['radarTrackId'] == 9

  def test_radar_only_takeover_when_no_model_lead(self):
    CP, CP_IQ = self.make_cp()
    track = make_track(4, 8.0, v_ego=2.0)
    lead = model_lead(prob=0.0)
    result = get_lead(2.0, True, {4: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
                      scan_radar=True, filtered_prob=0.0, held_track_id=-1)
    assert result['status'] and result['radarTrackId'] == 4

  def test_young_track_cannot_lead_alone(self):
    CP, CP_IQ = self.make_cp()
    track = make_track(4, 8.0, v_ego=2.0, cycles=1)
    lead = model_lead(prob=0.0)
    result = get_lead(2.0, True, {4: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True,
                      scan_radar=True, filtered_prob=0.0, held_track_id=-1)
    assert not result['status']

  def test_non_scan_behavior_unchanged(self):
    CP, CP_IQ = self.make_cp()
    track = make_track(4, 8.0, v_ego=2.0, cycles=1)
    lead = model_lead(prob=0.0)
    result = get_lead(2.0, True, {4: track}, lead, 2.0, CP, CP_IQ, low_speed_override=True)
    assert result['status']  # legacy path has no maturity gate


class TestHeldLeadStaleness:
  def make_radard(self, tracks):
    rd = object.__new__(RadarD)
    rd.tracks = tracks
    rd.ready = True
    rd.v_ego = 10.0
    rd.held_lead_ids = [7, -1]
    rd._held_evidence_ids = [7, -1]
    rd._challenger_stale_counts = [0, 0]
    rd._distance_stale_counts = [0, 0]
    return rd

  def test_relaxed_match_keeps_hold(self):
    held = make_track(7, 30.0, v_ego=10.0)
    rd = self.make_radard({7: held})
    for _ in range(5):
      rd._refresh_held_lead_evidence(0, model_lead(x=32.0), 0.9)
    assert rd.held_lead_ids[0] == 7

  def test_better_challenger_releases_hold(self):
    held = make_track(7, 80.0, v_ego=10.0)
    challenger = make_track(9, 30.0, v_ego=10.0)
    rd = self.make_radard({7: held, 9: challenger})
    lead = model_lead(x=31.5)
    for _ in range(SCAN_CHALLENGER_STALE_CYCLES):
      rd._refresh_held_lead_evidence(0, lead, 0.9)
    assert rd.held_lead_ids[0] == -1

  def test_gross_distance_disagreement_releases_hold(self):
    held = make_track(7, 80.0, v_ego=10.0)
    rd = self.make_radard({7: held})
    lead = model_lead(x=31.5, x_std=60.0, y_std=30.0, v_std=30.0)  # huge model uncertainty
    for _ in range(SCAN_DISTANCE_STALE_CYCLES):
      rd._refresh_held_lead_evidence(0, lead, 0.9)
    assert rd.held_lead_ids[0] == -1

  def test_low_probability_resets_evidence_without_release(self):
    held = make_track(7, 80.0, v_ego=10.0)
    rd = self.make_radard({7: held})
    for _ in range(10):
      rd._refresh_held_lead_evidence(0, model_lead(x=31.5), SCAN_LEAD_PROB)
    assert rd.held_lead_ids[0] == 7
    assert rd._distance_stale_counts[0] == 0


class TestProbFilterTiming:
  def test_rises_instantly_decays_slowly(self):
    from iqpilot.common.filter_simple import FirstOrderFilter
    f = FirstOrderFilter(0.0, 0.2, DT_MDL)
    f.x = max(f.x, 0.9)
    assert f.x == pytest.approx(0.9)
    f.update(0.0)
    assert f.x > 0.6