"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.volkswagen import pqcan


class PQRadarHandler:
  GRA_STEP = 3                            # GRA_Neu cadence (~33 Hz), matches stock stalk module
  SPOOF_STEP = 2                         # Motor_2 / Motor_5 spoof cadence (50 Hz)

  ENGAGE_FLOOR = 1.0 * CV.KPH_TO_MS      # never SET at/below 1 kph
  REENGAGE_FLOOR = 2.0 * CV.KPH_TO_MS    # hysteresis: only (re)engage above 2 kph

  SETSPEED_TOL_KPH = 1.0                 # don't chase set-speed within this band
  SHORT_STEP_KPH = 1.0 * CV.MPH_TO_KPH   # GRA_*_kurz ~= 1 mph
  LONG_STEP_KPH = 5.0 * CV.MPH_TO_KPH    # GRA_*_lang ~= 5 mph
  TAP_RELEASE_CYCLES = 2                 # GRA cycles to release between set-speed taps

  def __init__(self, CAN):
    self.bus = CAN.ext                   # radar lives on bus 2 (ext/cam)
    self.counter = 0
    self.failed = False                  # ACS_Fehler / irrev_Fehler -> radar dead for the drive
    self.want_engaged = False            # our belief the radar cruise should be on
    self._press_phase = 0                # alternate press/release for SET/cancel discrete edges
    self._tap_cooldown = 0               # set-speed tap rate limiter

  def reset(self):
    self.want_engaged = False
    self._press_phase = 0
    self._tap_cooldown = 0

  @staticmethod
  def _map_gap_bars(gap_bars):
    if not gap_bars:
      return None
    return int(min(3, max(1, gap_bars)))

  def update(self, packer, frame, CS, *, blend_active, engage_req, cancel_req,
             set_speed_kph, gap_bars, v_ego):
    can_sends = []

    if not blend_active:
      self.reset()
      return can_sends

    if CS.acc_radar_fehler or CS.acc_radar_sta_adr == 3:
      self.failed = True

    radar_active = (CS.acc_radar_sta_adr == 1) and not self.failed

    if self.failed:
      self.want_engaged = False
    elif cancel_req:
      self.want_engaged = False
    elif engage_req and v_ego > self.REENGAGE_FLOOR:
      self.want_engaged = True

    if (frame % self.SPOOF_STEP) == 0:
      # Mirror the stock engage handshake ORDER: the radar leads (SET -> ADR active) and only THEN
      # does the engine report GRA regulating. Asserting MO2_Sta_GRA=1 before the radar's ADR is
      # active presents an impossible state ("engine regulating but ADR didn't initiate it") and the
      # radar latches an irreversible fault. So only assert cruise-active once ADR is actually active;
      # relay Motor_5 (main switch) as verbatim OEM passthrough throughout.
      hold_engaged = self.want_engaged and radar_active
      can_sends.append(pqcan.filter_motor2(packer, self.bus, CS.motor2_stock, gra_active=hold_engaged))
      can_sends.append(pqcan.filter_motor5(packer, self.bus, CS.motor5_stock, gra_active=False))

    if (frame % self.GRA_STEP) == 0:
      set_btn = resume_btn = cancel = up_s = down_s = up_l = down_l = False
      self._press_phase ^= 1
      pressing = self._press_phase == 0

      if self.failed:
        pass
      elif cancel_req:
        # Only actually press cancel when the radar is engaged (or overdriven) -- otherwise it's a
        # no-op against a passive/off radar and just spams GRA_Abbrechen at ~16 Hz. Fires while
        # active and stops once the radar leaves the active state.
        cancel = pressing and radar_active
      elif self.want_engaged and not radar_active and v_ego > self.ENGAGE_FLOOR:
        # Engage with RESUME (GRA_Recall), not SET. Ground-truth from an OEM ACC engage (radar leads:
        # RESUME -> ACS_Sta_ADR 2->1 in ~90ms -> engine MO2_Sta_GRA 0->1 ~80ms later) shows this radar
        # engages from passive on GRA_Recall; it ignores GRA_Neu_Setzen from that state.
        resume_btn = pressing
      elif self.want_engaged and radar_active:
        if self._tap_cooldown > 0:
          self._tap_cooldown -= 1
        elif set_speed_kph > 0:
          delta = set_speed_kph - CS.acc_radar_v_wunsch
          if abs(delta) >= self.SETSPEED_TOL_KPH:
            big = abs(delta) >= self.LONG_STEP_KPH
            if delta > 0:
              up_l, up_s = big, not big
            else:
              down_l, down_s = big, not big
            self._tap_cooldown = self.TAP_RELEASE_CYCLES

      self.counter = (self.counter + 1) % 16
      can_sends.append(pqcan.create_radar_gra(
        packer, self.bus, CS.gra_stock_values, self.counter,
        set_btn=set_btn, resume=resume_btn, cancel=cancel, up_short=up_s, down_short=down_s,
        up_long=up_l, down_long=down_l, zeitluecke=self._map_gap_bars(gap_bars),
      ))

    return can_sends