"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""

import copy
from enum import StrEnum

from iqdbc.car import Bus, structs, DT_CTRL
from iqdbc.car.can_definitions import CanData
from iqdbc.car.interfaces import CarStateBase
from iqdbc.car.subaru.values import SubaruFlags

from iqdbc.lvbs.car.subaru import subarucan_ext
from iqdbc.lvbs.car.subaru.values_ext import SubaruFlagsIQ
from iqdbc.can.parser import CANParser

_SNG_ACC_MIN_DIST = 3
_SNG_ACC_MAX_DIST = 4.5


class IQStopAndGoController:
  def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
    self.CP = CP
    self.CP_IQ = CP_IQ
    self.enabled = CP_IQ.flags & (SubaruFlagsIQ.STOP_AND_GO | SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE)
    self.manual_parking_brake = CP_IQ.flags & SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE

    self.last_standstill_frame = 0
    self.epb_resume_frames_remaining = -1
    self.prev_close_distance = 0.0

  def update_epb_resume_sequence(self, should_resume: bool) -> bool:
    if self.manual_parking_brake:
      return False

    if should_resume:
      self.epb_resume_frames_remaining = 15

    send_resume = self.epb_resume_frames_remaining > 0
    if self.epb_resume_frames_remaining > 0:
      self.epb_resume_frames_remaining -= 1

    return send_resume

  def update_stop_and_go(self, CC: structs.CarControl, CS: CarStateBase, frame: int) -> bool:
    """
    Manages stop-and-go functionality for adaptive cruise control (ACC).

    Args:
        CC: Car control data
        CS: Car state data
        frame: Current frame number

    Returns:
        bool: True if resume command should be sent, False otherwise
    """

    if not CC.enabled or not CC.hudControl.leadVisible:
      return False

    close_distance = CS.es_distance_msg["Close_Distance"]
    in_standstill = CS.out.standstill

    if not in_standstill:
      self.last_standstill_frame = frame

    # Check if we've been in standstill long enough
    mpb_standstill_timers = (0.75, 0.8) if self.CP.flags & SubaruFlags.PREGLOBAL else (0.5, 0.55)
    standstill_duration = (frame - self.last_standstill_frame) * DT_CTRL
    in_standstill_hold = standstill_duration > mpb_standstill_timers[0]
    if (frame - self.last_standstill_frame) * DT_CTRL >= mpb_standstill_timers[1]:
      self.last_standstill_frame = frame

    # Car state distance-based conditions (EPB only)
    in_resume_distance = _SNG_ACC_MIN_DIST < close_distance < _SNG_ACC_MAX_DIST
    distance_increasing = close_distance > self.prev_close_distance
    distance_resume_allowed = in_resume_distance and distance_increasing

    if self.manual_parking_brake:
      # Manual parking brake: Direct resume when the standstill hold threshold is reached to prevent ACC fault
      send_resume = in_standstill_hold
    else:
      # EPB: Resume sequence with trigger on distance with lead car increasing
      should_resume = CS.out.standstill and distance_resume_allowed
      send_resume = self.update_epb_resume_sequence(should_resume)

    self.prev_close_distance = close_distance

    return send_resume

  def create_creep_assist(self, packer, CC: structs.CarControl, CS: CarStateBase, frame: int) -> list[CanData]:
    can_sends = []

    if not self.enabled:
      return can_sends

    send_resume = self.update_stop_and_go(CC, CS, frame)

    can_sends.append(subarucan_ext.create_throttle(packer, self.CP, CS.throttle_msg, send_resume and not self.manual_parking_brake))

    if frame % 2 == 0:
      can_sends.append(subarucan_ext.create_brake_pedal(packer, self.CP, CS.brake_pedal_msg, send_resume and self.manual_parking_brake))

    return can_sends


class IQStopAndGoState:
  def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
    self.CP = CP
    self.CP_IQ = CP_IQ

    self.brake_pedal_msg: dict[str, float] = {}
    self.throttle_msg: dict[str, float] = {}

  def update(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
    cp = can_parsers[Bus.pt]

    self.brake_pedal_msg = copy.copy(cp.vl["Brake_Pedal"])

    if not self.CP.flags & SubaruFlags.HYBRID:
      self.throttle_msg = copy.copy(cp.vl["Throttle"])