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

ModeType = Literal['acc', 'blended']
IQ_DYNAMIC_MODE_PARAM = "IQDynamicMode"
IQ_DYNAMIC_CONDITIONAL_CURVES_PARAM = "IQDynamicConditionalCurves"
IQ_DYNAMIC_CONDITIONAL_SLOWER_LEAD_PARAM = "IQDynamicConditionalSlowerLead"
IQ_DYNAMIC_CONDITIONAL_STOPPED_LEAD_PARAM = "IQDynamicConditionalStoppedLead"
IQ_DYNAMIC_CONDITIONAL_MODEL_STOPS_PARAM = "IQDynamicConditionalModelStops"
IQ_DYNAMIC_CONDITIONAL_SLC_FALLBACK_PARAM = "IQDynamicConditionalSLCFallback"
IQ_DYNAMIC_CONDITIONAL_SPEED_PARAM = "IQDynamicConditionalSpeed"
IQ_DYNAMIC_CONDITIONAL_LEAD_SPEED_PARAM = "IQDynamicConditionalLeadSpeed"
IQ_DYNAMIC_MODEL_STOP_TIME_PARAM = "IQDynamicModelStopTime"
IQ_DYNAMIC_MINIMUM_FORCE_STOP_LENGTH_PARAM = "IQDynamicMinimumForceStopLength"
IQ_FORCE_STOPS_PARAM = "IQForceStops"


class IQConstants:
  CRUISING_SPEED = 3.0
  SIGNAL_QUEUE_DEPTH = 6
  LEAD_LOCK_GATE = 0.45
  BRAKE_CURVE_QUEUE_DEPTH = 5
  BRAKE_CURVE_GATE = 0.3
  BRAKE_CURVE_SPEED_AXIS = [0., 10., 20., 30., 40., 50., 55., 60.]
  BRAKE_CURVE_DISTANCE_AXIS = [32., 46., 64., 86., 108., 130., 145., 165.]
  CRUISE_LAG_QUEUE_DEPTH = 10
  CRUISE_LAG_GATE = 0.55
  CRUISE_LAG_RATIO_GATE = 1.025
  CONDITIONAL_SPEED_DEFAULT = 18.0
  CONDITIONAL_LEAD_SPEED_DEFAULT = 24.0
  MODEL_STOP_TIME_DEFAULT = 3.0
  MODEL_STOP_THRESHOLD = 0.55
  SLOW_LEAD_THRESHOLD = 0.55
  FORCE_STOP_PLANNER_TIME = 3.0
  MINIMUM_FORCE_STOP_LENGTH_DEFAULT = 0.0


def compute_slowdown_need(kph: float, horizon_dist: float, desired_dist: float) -> float:
  if horizon_dist >= desired_dist or desired_dist <= 0.0:
    return 0.0

  shortage = desired_dist - horizon_dist
  shortage_ratio = shortage / desired_dist
  need = min(1.0, shortage_ratio * 2.0)

  if horizon_dist < desired_dist * 0.3:
    need = min(1.0, need * 2.0)

  if kph > 25.0:
    need = min(1.0, need * (1.0 + (kph - 25.0) / 80.0))

  return need


class IQFilterEngine:
  def __init__(self, initial=0.0, measurement_noise=0.1, process_noise=0.01, process_decay=1.0, smoothing_floor=0.85):
    self._value = initial
    self._variance = 1.0
    self._measurement_noise = measurement_noise
    self._process_noise = process_noise
    self._process_decay = process_decay
    self._smoothing_floor = smoothing_floor
    self._initialized = False
    self._samples = []
    self._sample_limit = 10
    self._confidence = 0.0

  def push(self, measurement: float) -> None:
    if len(self._samples) >= self._sample_limit:
      self._samples.pop(0)
    self._samples.append(measurement)

    if not self._initialized:
      self._value = measurement
      self._initialized = True
      self._confidence = 0.1
      return

    self._variance = self._process_decay * self._variance + self._process_noise
    gain = self._variance / (self._variance + self._measurement_noise)
    effective_gain = gain * (1.0 - self._smoothing_floor) + self._smoothing_floor * 0.1

    innovation = measurement - self._value
    self._value = self._value + effective_gain * innovation
    self._variance = (1.0 - effective_gain) * self._variance

    if abs(innovation) < 0.1:
      self._confidence = min(1.0, self._confidence + 0.05)
    else:
      self._confidence = max(0.1, self._confidence - 0.02)

  def value(self):
    return self._value if self._initialized else None

  def confidence(self) -> float:
    return self._confidence

  def reset(self) -> None:
    self._initialized = False
    self._samples = []
    self._confidence = 0.0


class IQModeEngine:
  def __init__(self):
    self._state: ModeType = 'acc'
    self._scores = {'acc': 1.0, 'blended': 0.0}
    self._switching_timer = 0
    self._mode_age = 0
    self._forced_takeover = False

  def request(self, mode: ModeType, urgency: float = 1.0, emergency: bool = False) -> None:
    if emergency:
      self._forced_takeover = True
      self._state = mode
      self._switching_timer = 15
      self._mode_age = 0
      return

    self._scores[mode] = min(1.0, self._scores[mode] + 0.1 * urgency)
    for key in self._scores:
      if key != mode:
        self._scores[key] = max(0.0, self._scores[key] - 0.05)

    if self._mode_age < 10 and not self._forced_takeover:
      return

    threshold = 0.6 if mode != self._state else 0.3
    if self._scores[mode] > threshold and mode != self._state and self._switching_timer == 0:
      self._switching_timer = 15
      self._state = mode
      self._mode_age = 0

  def update(self) -> None:
    if self._switching_timer > 0:
      self._switching_timer -= 1

    self._mode_age += 1
    if self._forced_takeover and self._mode_age > 20:
      self._forced_takeover = False

    for key in self._scores:
      self._scores[key] *= 0.98

  def get_mode(self) -> ModeType:
    return self._state