import math

import pytest

from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
import iqdbc.car.hyundai.hyundaicanfd as hyundaicanfd
import iqdbc.car.hyundai.radar_interface as radar_interface_module
from iqdbc.car.hyundai.radar_interface import (
  CORNER_OBJECT_STABLE_TRACK_ID_START,
  RADAR_MSG_COUNT3,
  RADAR_MSG_COUNT4,
  RADAR_START_ADDR_CANFD3,
  CornerObjectTrackIdManager,
  RadarInterface,
  corner_object_position_valid,
)
from iqdbc.car.hyundai.values import CAR, HyundaiExtFlags, HyundaiFlags


class TestDensoRadar:
  @staticmethod
  def parse(addr, dat):
    name = f"RADAR_TRACK_{addr:x}"
    parser = CANParser("hyundai_kia_denso_front_radar_generated", [(name, 20)], 1)
    parser.update([0, [(addr, bytes.fromhex(dat), 1)]])
    return parser.vl[name]

  def test_active_track_signals(self):
    # Person walking toward the parked car, left of the camera center.
    track = self.parse(0x503, "bc047efcc1fe8b00")

    assert track["LONG_DIST"] == pytest.approx(7.1875)
    assert track["LAT_DIST"] == pytest.approx(-1.625)
    assert track["REL_SPEED"] == pytest.approx(-0.734375)
    assert track["OBJECT_STATE"] == 3

  def test_empty_track(self):
    track = self.parse(0x507, "53fff80000000081")

    assert track["LONG_DIST"] == pytest.approx(409.55)
    assert track["LAT_DIST"] == 0
    assert track["REL_SPEED"] == 0
    assert track["OBJECT_STATE"] == 0

  def test_long_range_lateral_distance(self):
    # Real driving sample: treating the signed field as -12 degrees would put
    # this target about 34 m sideways at 161 m. It is instead -3.0 m lateral.
    track = self.parse(0x506, "b664eafa00cd230b")

    assert track["LONG_DIST"] == pytest.approx(161.4625)
    assert track["LAT_DIST"] == pytest.approx(-3.0)
    assert track["OBJECT_STATE"] == 3

  def test_parser_selection_and_point_conversion(self, monkeypatch):
    class FakeParams:
      def get_int(self, key):
        return 1 if key == "EnableRadarTracks" else 0

    monkeypatch.setattr(radar_interface_module, "Params", FakeParams)
    cp = structs.CarParams()
    cp.carFingerprint = CAR.KIA_SORENTO
    cp.flags = 0
    cp.extFlags = HyundaiExtFlags.RADAR_GROUP4.value
    cp.radarUnavailable = False
    cp.safetyConfigs = [structs.CarParams.SafetyConfig()]

    radar_interface = RadarInterface(cp)

    assert radar_interface.radar_group4
    assert RADAR_MSG_COUNT4 == 8
    assert radar_interface.radar_msg_count == RADAR_MSG_COUNT4
    assert radar_interface.trigger_msg_tracks == 0x507

    active_dat = bytes.fromhex("bc047efcc1fe8b00")
    empty_dat = bytes.fromhex("bcfff80000000081")
    packets = [(addr, active_dat if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
    radar_data = radar_interface.update([0, packets])
    point = next(point for point in radar_data.points if point.trackId == 35)

    assert point.measured
    assert point.dRel == pytest.approx(7.1875)
    assert point.yRel == pytest.approx(1.625)
    assert point.vRel == pytest.approx(-0.734375)
    assert math.isnan(point.aRel)

    # EN: Confirm that the long-range sample survives the filter and converts
    #     radar-left-negative to openpilot-left-positive coordinates.
    # KO: 장거리 샘플의 필터 통과와 레이더 좌측 음수 좌표가 openpilot 좌측
    #     양수 좌표로 변환되는지 확인함.
    long_range_dat = bytes.fromhex("b664eafa00cd230b")
    packets = [(addr, long_range_dat if addr == 0x506 else empty_dat, 1) for addr in range(0x500, 0x508)]
    radar_data = radar_interface.update([0, packets])
    point = next(point for point in radar_data.points if point.trackId == 38)

    assert point.dRel == pytest.approx(161.4625)
    assert point.yRel == pytest.approx(3.0)

    # EN: A state-0 raw detection must not enter a stable tracked-object slot.
    # KO: 상태 0인 raw detection이 안정적인 추적 객체 슬롯에 들어오지 않음을 확인함.
    raw_detection = bytes.fromhex("d702f4fc200000e4")
    packets = [(addr, raw_detection if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
    radar_data = radar_interface.update([0, packets])
    assert not radar_data.points

    # EN: A real confirmed track beyond the former 205 m limit remains valid.
    # KO: 기존 205m 상한을 넘는 실제 확정 트랙도 유효하게 유지됨.
    confirmed_213m_track = bytes.fromhex("35854c0780f163e0")
    packets = [(addr, confirmed_213m_track if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
    radar_data = radar_interface.update([0, packets])
    point = next(point for point in radar_data.points if point.trackId == 35)
    assert point.dRel == pytest.approx(213.275)
    assert point.yRel == pytest.approx(-3.75)

    # EN: The 325 m boundary is rejected, leaving ample separation from the
    #     409.55 m empty-slot sentinel.
    # KO: 325m 경계값을 제외해 409.55m 빈 슬롯 값과 충분한 간격을 확보함.
    boundary_track = bytes.fromhex("bccb200000000300")
    packets = [(addr, boundary_track if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
    radar_data = radar_interface.update([0, packets])
    assert not radar_data.points

    # EN: The wider profile keeps a real stable track at 4.875 m, covering more
    #     of the outer adjacent lane than the conservative 4.5 m profile.
    # KO: 넓어진 필터에서 4.875m의 실제 안정 트랙을 유지해 보수적인 4.5m
    #     설정보다 바깥쪽 인접 차선을 더 넓게 포함함.
    outer_lane_track = bytes.fromhex("d80b66f640000300")
    packets = [(addr, outer_lane_track if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
    radar_data = radar_interface.update([0, packets])
    point = next(point for point in radar_data.points if point.trackId == 35)
    assert point.yRel == pytest.approx(4.875)

    # EN: Tracks beyond the widened envelope are rejected as roadside clutter;
    #     this payload differs only in lateral distance (-7.0 m).
    # KO: 넓어진 범위를 벗어난 트랙은 도로변 잡음으로 제외함. 이 payload는
    #     횡방향 거리(-7.0m)만 다름.
    far_side_reflection = bytes.fromhex("d80b66f200000300")
    packets = [(addr, far_side_reflection if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
    radar_data = radar_interface.update([0, packets])
    assert not radar_data.points


class TestRadarGroup3:
  @staticmethod
  def parse(addr, dat):
    name = f"RADAR_TRACK_{addr:x}"
    parser = CANParser("hyundai_canfd_radar_generated", [(name, 20)], 1)
    parser.update([0, [(addr, bytes.fromhex(dat), 1)]])
    return parser.vl[name]

  def test_group3_active_track(self):
    track = self.parse(0x406, "e1043b0f02590e692a227e16f80fe00f28fcc753a20a0000")

    assert track["OBJECT_LENGTH"] == pytest.approx(4.4)
    assert track["LONG_DIST"] == pytest.approx(55.4)
    assert track["LAT_DIST"] == pytest.approx(-3.0)
    assert track["REL_SPEED"] == pytest.approx(4.4)

  def test_group3_empty_track(self):
    track = self.parse(0x407, "c03d3b0000000000ff0700000000000000d0020000000000")

    assert track["OBJECT_LENGTH"] == 0
    assert track["LONG_DIST"] == pytest.approx(204.7)
    assert track["LAT_DIST"] == 0
    assert track["REL_SPEED"] == 0

  def test_group3_parser_selection(self, monkeypatch):
    class FakeParams:
      def get_int(self, key):
        return 1 if key == "EnableRadarTracks" else 0

    monkeypatch.setattr(radar_interface_module, "Params", FakeParams)
    monkeypatch.setattr(hyundaicanfd, "Params", FakeParams)
    cp = structs.CarParams()
    cp.carFingerprint = next(car for car, dbc in radar_interface_module.DBC.items() if "hyundai_canfd" in dbc[Bus.pt])
    cp.flags = HyundaiFlags.CANFD.value
    cp.extFlags = HyundaiExtFlags.RADAR_GROUP3.value
    cp.radarUnavailable = False
    cp.safetyConfigs = [structs.CarParams.SafetyConfig()]

    radar_interface = RadarInterface(cp)

    assert radar_interface.radar_group3
    assert radar_interface.radar_start_addr == RADAR_START_ADDR_CANFD3
    assert radar_interface.radar_msg_count == RADAR_MSG_COUNT3
    assert radar_interface.trigger_msg_tracks == 0x41D

    active_dat = bytes.fromhex("e1043b0f02590e692a227e16f80fe00f28fcc753a20a0000")
    empty_dat = bytes.fromhex("c03d3b0000000000ff0700000000000000d0020000000000")
    packets = [(addr, active_dat if addr == 0x406 else empty_dat, 1) for addr in range(0x400, 0x41E)]
    radar_data = radar_interface.update([0, packets])
    point = next(point for point in radar_data.points if point.trackId == 38)

    assert point.measured
    assert point.dRel == pytest.approx(53.1)
    assert point.yRel == pytest.approx(-3.0)
    assert point.vRel == pytest.approx(4.4)


class TestCornerRadarObjectIdentity:
  @staticmethod
  def set_bits(data, start, size, value):
    for offset in range(size):
      bit = start + offset
      data[bit // 8] |= ((value >> offset) & 1) << (bit % 8)

  @pytest.mark.parametrize(
    "dbc,msg_name,addr,age_signal,id_signal,age_start,id_start",
    (
      ("hyundai_canfd_corner_radar_180_generated", "CORNER_RADAR_180_OBJECTS_180", 0x180,
       "SLOT1_AGE", "SLOT1_OBJECT_ID", 32, 44),
      ("hyundai_canfd_corner_radar_235_generated", "CORNER_RADAR_235_OBJECTS_235", 0x235,
       "OBJ_AGE", "OBJ_OBJECT_ID", 32, 44),
    ),
  )
  def test_object_identity_signals(self, dbc, msg_name, addr, age_signal, id_signal, age_start, id_start):
    data = bytearray(32)
    self.set_bits(data, age_start, 8, 23)
    self.set_bits(data, id_start, 7, 46)
    parser = CANParser(dbc, [(msg_name, 33)], 1)
    parser.update([0, [(addr, bytes(data), 1)]])

    assert parser.vl[msg_name][age_signal] == 23
    assert parser.vl[msg_name][id_signal] == 46

  def test_track_id_survives_slot_move_and_resets_with_age(self):
    manager = CornerObjectTrackIdManager()
    first_id = manager.get_track_id("corner180", object_id=108, age=240)

    assert first_id == CORNER_OBJECT_STABLE_TRACK_ID_START
    assert manager.get_track_id("corner180", object_id=108, age=241) == first_id
    assert manager.get_track_id("corner235", object_id=108, age=241) != first_id
    assert manager.get_track_id("corner180", object_id=108, age=2) != first_id

  def test_clipped_side_object_position_is_valid(self):
    assert corner_object_position_valid(0.0, 2.8)
    assert corner_object_position_valid(25.0, 0.2)
    assert not corner_object_position_valid(0.0, 0.0)
    assert not corner_object_position_valid(0.0, 5.0)


class TestCornerRadar430CandidateFilter:
  @staticmethod
  def slot_word(distance_raw, meta13=0, b2=10, b3=2):
    return distance_raw | (meta13 << 13) | (b2 << 16) | (b3 << 24)

  @classmethod
  def message(cls, slots):
    words = [0x010d1f40] * 7
    for slot, word in slots.items():
      words[slot - 1] = word

    dat = bytearray(32)
    for idx, word in enumerate(words):
      dat[4 + idx * 4:8 + idx * 4] = int(word).to_bytes(4, "little")
    return bytes(dat)

  @staticmethod
  def build_interface(monkeypatch):
    class FakeParams:
      def get_int(self, key):
        return 1 if key == "EnableCornerRadar" else 0

    monkeypatch.setattr(radar_interface_module, "Params", FakeParams)
    monkeypatch.setattr(hyundaicanfd, "Params", FakeParams)
    cp = structs.CarParams()
    cp.carFingerprint = next(car for car, dbc in radar_interface_module.DBC.items() if "hyundai_canfd" in dbc[Bus.pt])
    cp.flags = HyundaiFlags.CANFD.value
    cp.extFlags = HyundaiExtFlags.CORNER_RADAR_OBJECTS_430.value
    cp.radarUnavailable = True
    cp.safetyConfigs = [structs.CarParams.SafetyConfig()]
    return RadarInterface(cp)

  @staticmethod
  def update_frames(radar_interface, packets, frames=5):
    radar_data = None
    for _ in range(frames):
      radar_data = radar_interface.update([0, packets])
    return radar_data

  def test_430_promotes_supported_neighbor_bins(self, monkeypatch):
    radar_interface = self.build_interface(monkeypatch)
    empty = self.message({})
    supported_bins = self.message({
      6: self.slot_word(1000),
      7: self.slot_word(1004),
    })
    packets = [(addr, supported_bins if addr == 0x436 else empty, 1) for addr in range(0x430, 0x438)]
    packets += [(addr, empty, 1) for addr in range(0x440, 0x448)]

    radar_data = self.update_frames(radar_interface, packets)
    points = {point.trackId: point for point in radar_data.points}

    assert points[300].measured
    assert points[300].dRel == pytest.approx(50.1)
    assert points[300].yRel == pytest.approx(2.0)
    assert points[300].yvRel == 0.0

  def test_430_expires_noncenter_inward_yvrel(self, monkeypatch):
    radar_interface = self.build_interface(monkeypatch)
    empty = self.message({})
    frame_defs = (
      (0x431, 4, 5),
      (0x433, 3, 4),
      (0x435, 2, 3),
      (0x430, 5, 6),
      (0x432, 4, 5),
      (0x434, 3, 4),
      (0x436, 2, 3),
    )

    radar_data = None
    for addr, first_slot, second_slot in frame_defs:
      msg = self.message({
        first_slot: self.slot_word(1000),
        second_slot: self.slot_word(1004),
      })
      packets = [(a, msg if a == addr else empty, 1) for a in range(0x430, 0x438)]
      packets += [(a, empty, 1) for a in range(0x440, 0x448)]
      radar_data = radar_interface.update([0, packets])
    radar_data = self.update_frames(radar_interface, packets, frames=3)
    points = {point.trackId: point for point in radar_data.points}

    assert points[300].measured
    assert points[300].yRel == pytest.approx(2.0)
    assert points[300].yvRel == 0.0