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

import numpy as np
import pyray as rl

from iqpilot.cereal import car, custom
from iqpilot.common.constants import CV
from iqpilot.common.filter_simple import FirstOrderFilter
from iqpilot.selfdrive.ui.ui_state import ui_state
from iqpilot.selfdrive.ui.onroad.hud_renderer import COLORS, FONT_SIZES, UI_CONFIG
from iqpilot.selfdrive.ui.mici.onroad.alert_renderer import IconSide, TURN_SIGNAL_BLINK_PERIOD
from iqpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar
from iqpilot.system.ui.lib.application import gui_app, FontWeight
from iqpilot.system.ui.lib.multilang import tr
from iqpilot.system.ui.widgets import Widget
from iqpilot.system.ui.iqwidgets.lib import canvas
from iqdbc.car.volkswagen.values import VolkswagenFlags

def _feed():
  return ui_state.sm


def _speed_scale() -> float:
  return CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH


_STRIP_WIDTH = 28
_STRIP_INSET = 14
_STRIP_CEILING = 0.85
_STRIP_EMA = 5.0
_STRIP_ARC_SEGMENTS = 24
_STRIP_ACCEL = (33, 112, 115)
_STRIP_ACCEL_NEON = (94, 232, 236)
_STRIP_DECEL = (255, 0, 247)
_STRIP_DECEL_NEON = (255, 145, 251)
_STRIP_TIP_ALPHA = 235
_STRIP_ROOT_ALPHA = 55
_STRIP_CORE_DEPTH = 7.0
_STRIP_CORE_LAYERS = 8
_STRIP_CORE_ALPHA = 0.15
_STRIP_CORE_FLOOR = 0.3
_STRIP_CORE_TAIL = 22.0
_STRIP_HALO_SPREAD = 11.0
_STRIP_HALO_LAYERS = 11
_STRIP_HALO_ALPHA = 0.095
_STRIP_HALO_TAIL = 34.0
_STRIP_NEON_FULL = 2.0


class IQAccelBar:
  def __init__(self):
    self._eased = 0.0

  def _reach(self) -> float:
    mag = abs(self._eased)
    return 0.0 if mag == 0.0 else max(0.0, _STRIP_CEILING - 0.1 / mag)

  @staticmethod
  def _tint(fill, frac: float):
    return canvas.shade(*fill, int(_STRIP_TIP_ALPHA + (_STRIP_ROOT_ALPHA - _STRIP_TIP_ALPHA) * frac))

  @staticmethod
  def _halo(center, cap: float, up: bool, neon, heat: float, tail: float) -> None:
    step = int(255 * _STRIP_HALO_ALPHA * heat)
    if step < 1:
      return
    start, end = (180.0, 360.0) if up else (0.0, 180.0)
    tint = canvas.shade(*neon, step)
    faded = canvas.shade(*neon, 0)
    top, bottom = (tint, faded) if up else (faded, tint)
    for i in range(_STRIP_HALO_LAYERS):
      spread = _STRIP_HALO_SPREAD * (1.0 - i / _STRIP_HALO_LAYERS)
      if spread <= 0.0:
        continue
      canvas.annulus(center, cap, cap + spread, start, end, _STRIP_ARC_SEGMENTS, tint)
      if tail <= 0.0:
        continue
      run = min(tail, _STRIP_HALO_TAIL)
      y = center.y if up else center.y - run
      canvas.v_sweep(center.x - cap - spread, y, spread, run, top, bottom)
      canvas.v_sweep(center.x + cap, y, spread, run, top, bottom)

  @staticmethod
  def _cap(center, cap: float, up: bool, fill, neon, heat: float, tail: float) -> None:
    start, end = (180.0, 360.0) if up else (0.0, 180.0)
    canvas.annulus(center, 0.0, cap, start, end, _STRIP_ARC_SEGMENTS, fill)
    IQAccelBar._halo(center, cap, up, neon, heat, tail)
    lit = _STRIP_CORE_FLOOR + (1.0 - _STRIP_CORE_FLOOR) * heat
    core = canvas.shade(*neon, max(1, int(255 * _STRIP_CORE_ALPHA * lit)))
    faded = canvas.shade(*neon, 0)
    top, bottom = (core, faded) if up else (faded, core)
    run = min(tail, _STRIP_CORE_TAIL)
    for i in range(_STRIP_CORE_LAYERS):
      depth = _STRIP_CORE_DEPTH * (1.0 - i / _STRIP_CORE_LAYERS)
      if depth <= 0.0:
        continue
      canvas.annulus(center, max(0.0, cap - depth), cap, start, end, _STRIP_ARC_SEGMENTS, core)
      if run <= 0.0:
        continue
      y = center.y if up else center.y - run
      canvas.v_sweep(center.x - cap, y, depth, run, top, bottom)
      canvas.v_sweep(center.x + cap - depth, y, depth, run, top, bottom)

  def render(self, rect, sm) -> None:
    if not ui_state.rocket_fuel:
      return
    self._eased += (sm['carState'].aEgo - self._eased) / _STRIP_EMA
    reach = self._reach() * rect.height / 2.0
    if reach <= 0.0:
      return
    accelerating = self._eased > 0.0
    mid = rect.y + rect.height / 2.0
    top = mid - reach if accelerating else mid
    x = rect.x + _STRIP_INSET
    cap = min(_STRIP_WIDTH / 2.0, reach / 2.0)
    fill, neon = (_STRIP_ACCEL, _STRIP_ACCEL_NEON) if accelerating else (_STRIP_DECEL, _STRIP_DECEL_NEON)
    near = cap / reach
    frac_top, frac_bottom = (near, 1.0 - near) if accelerating else (1.0 - near, near)
    heat = min(abs(self._eased) / _STRIP_NEON_FULL, 1.0)

    canvas.v_sweep(x, top + cap, cap * 2.0, reach - cap * 2.0,
                   self._tint(fill, frac_top), self._tint(fill, frac_bottom))
    tail = max(0.0, reach / 2.0 - cap)
    self._cap(canvas.Pt(x + cap, top + cap), cap, True, self._tint(fill, frac_top), neon, heat, tail)
    self._cap(canvas.Pt(x + cap, top + reach - cap), cap, False, self._tint(fill, frac_bottom), neon, heat, tail)

_BS_INSET = 20
_BS_DROP = 100
_BS_MIN = 0.01


class _BlindSide:
  def __init__(self, name: str):
    self.texture = gui_app.texture(f'icons_mici/onroad/blind_spot_{name}.png', 108, 128)
    self.glow = FirstOrderFilter(0, 0.15, 1 / gui_app.target_fps)
    self.on_left = name == "left"

  def feed(self, present: bool):
    self.glow.update(1.0 if present else 0.0)

  def lit(self) -> bool:
    return self.glow.x > _BS_MIN

  def place(self, rect):
    tex = self.texture
    x = rect.x + _BS_INSET if self.on_left else rect.x + rect.width - _BS_INSET - tex.width
    canvas.stamp(tex, x, rect.y + _BS_DROP, canvas.shade(255, 255, 255, int(255 * self.glow.x)))


class IQBlindSpotOverlay:
  def __init__(self):
    self._left = _BlindSide("left")
    self._right = _BlindSide("right")

  def update(self) -> None:
    cs = _feed()['carState']
    self._left.feed(cs.leftBlindspot)
    self._right.feed(cs.rightBlindspot)

  @property
  def detected(self) -> bool:
    return self._left.lit() or self._right.lit()

  def render(self, rect) -> None:
    if not ui_state.blindspot:
      return
    for side in (self._left, self._right):
      if side.lit():
        side.place(rect)

_MQB_CLUSTER_EXEMPT = (VolkswagenFlags.PQ | VolkswagenFlags.MLB | VolkswagenFlags.MEB | VolkswagenFlags.MEB_GEN2 | VolkswagenFlags.MQB_EVO)

class IQSpeedOverlay:
  def __init__(self):
    self.speed: float = 0.0
    self._cluster_ever_live: bool = False
    self._heavy = gui_app.font(FontWeight.BOLD)
    self._mid = gui_app.font(FontWeight.MEDIUM)

  def _source_speed(self, cs) -> float:
    cp = ui_state.CP
    if cp is not None and cp.brand == "volkswagen" and not (cp.flags & _MQB_CLUSTER_EXEMPT):
      return cs.vEgoCluster
    self._cluster_ever_live = self._cluster_ever_live or cs.vEgoCluster != 0.0
    return cs.vEgoCluster if self._cluster_ever_live else cs.vEgo

  def update(self) -> None:
    self.speed = max(0.0, self._source_speed(_feed()['carState']) * _speed_scale())

  def _stack(self, face, text: str, size: int, rect, top: float, color) -> float:
    extent = canvas.span(face, text, size)
    canvas.glyphs(face, text, canvas.Pt(rect.x + (rect.width - extent.x) / 2, top), size, color)
    return extent.y

  def render(self, rect) -> None:
    top = rect.y + 52
    number_h = self._stack(self._heavy, str(round(self.speed)), FONT_SIZES.current_speed, rect, top, COLORS.WHITE)
    unit = tr("km/h") if ui_state.is_metric else tr("mph")
    self._stack(self._mid, unit, FONT_SIZES.speed_unit, rect, top + number_h - 10, COLORS.WHITE_TRANSLUCENT)

def clip_to_width(font, words: str, size: int, limit: float) -> str:
  if canvas.span(font, words, size).x <= limit:
    return words
  trimmed = words
  while len(trimmed) > 3 and canvas.span(font, trimmed + "...", size).x > limit:
    trimmed = trimmed[:-1]
  return trimmed + "..."


class RoadNameBanner(Widget):
  TYPE_SIZE = 46
  MARGIN = 40
  GAP = 10
  TORQUE_SCALE = 3.0

  def __init__(self):
    super().__init__()
    self.road_name = ""
    self._face = gui_app.font(FontWeight.BOLD)

  def update(self):
    sm = _feed()
    if sm.recv_frame["carState"] < ui_state.started_frame:
      return
    if sm.updated["iqLiveData"]:
      self.road_name = sm["iqLiveData"].roadName

  def _render(self, rect):
    if not self.road_name or not ui_state.road_name_toggle:
      return
    label = clip_to_width(self._face, self.road_name, self.TYPE_SIZE, rect.width - self.MARGIN)
    extent = canvas.span(self._face, label, self.TYPE_SIZE)
    top = min(TorqueBar.resting_bottom(rect, self.TORQUE_SCALE) + self.GAP,
              rect.y + rect.height - extent.y)
    canvas.glyphs(self._face, label,
                  canvas.Pt(rect.x + (rect.width - extent.x) / 2, top),
                  self.TYPE_SIZE, COLORS.WHITE)


RoadNameRenderer = RoadNameBanner
ellipsize = clip_to_width

from dataclasses import dataclass, field

_ARROW = 'signal'
_WARN = 'blind_spot'


@dataclass(frozen=True)
class TurnSignalConfig:
  left_x: int = 80
  left_y: int = 190
  right_x: int = 80
  right_y: int = 190
  size: int = 150

class _IndicatorLamp(Widget):
  def __init__(self, direction: IconSide):
    super().__init__()
    self.mode: str | None = None
    self._epoch = 0.0
    self._glow = FirstOrderFilter(0.0, 0.3, 1 / gui_app.target_fps)
    self._art = {
      _ARROW: gui_app.texture(f'icons_mici/onroad/turn_signal_{direction}.png', 120, 109),
      _WARN: gui_app.texture(f'icons_mici/onroad/blind_spot_{direction}.png', 120, 109),
    }

  def set_mode(self, mode: str | None):
    if mode != self.mode or mode is None:
      self._epoch = 0.0
    self.mode = mode

  def _pulse(self) -> int:
    self._glow.dt = 1 / gui_app.target_fps
    self._glow.update_alpha(0.3)
    if time.monotonic() - self._epoch > TURN_SIGNAL_BLINK_PERIOD:
      self._epoch = time.monotonic()
      self._glow.x = 255 * 2
    else:
      self._glow.update(255 * 0.2)
    return int(min(self._glow.x, 255))

  def _render(self, _):
    if self.mode is None:
      return
    alpha = self._pulse() if self.mode == _ARROW else 255
    tex = self._art[self.mode]
    canvas.stamp(tex, self._rect.x + (self._rect.width - tex.width) / 2,
                 self._rect.y + (self._rect.height - tex.height) / 2, canvas.shade(255, 255, 255, alpha))


def _lamp_modes(event_name: str, cs, remembered):
  if event_name == 'preLaneChangeLeft':
    return _ARROW, None, IconSide.left
  if event_name == 'preLaneChangeRight':
    return None, _ARROW, IconSide.right
  if event_name == 'laneChange':
    if remembered == IconSide.left:
      return _ARROW, None, remembered
    if remembered == IconSide.right:
      return None, _ARROW, remembered
    return None, None, remembered
  if event_name == 'laneChangeBlocked':
    side = IconSide.left if cs.leftBlinker else IconSide.right if cs.rightBlinker else remembered
    if side == IconSide.left:
      return _WARN, None, remembered
    if side == IconSide.right:
      return None, _WARN, remembered
    return None, None, remembered
  left = _WARN if cs.leftBlindspot else _ARROW if cs.leftBlinker else None
  right = _WARN if cs.rightBlindspot else _ARROW if cs.rightBlinker else None
  return left, right, None


class IQTurnSignalOverlay:
  def __init__(self, config: TurnSignalConfig | None = None):
    self._config = config or TurnSignalConfig()
    self._lamps = {IconSide.left: _IndicatorLamp(IconSide.left),
                   IconSide.right: _IndicatorLamp(IconSide.right)}
    self._remembered: IconSide | None = None

  def update(self):
    sm = _feed()
    alert = sm['selfdriveState'].alertType
    event_name = alert.split('/')[0] if alert else ''
    left, right, self._remembered = _lamp_modes(event_name, sm['carState'], self._remembered)
    self._lamps[IconSide.left].set_mode(left)
    self._lamps[IconSide.right].set_mode(right)

  def render(self, rect):
    if not ui_state.turn_signals:
      return
    c = self._config
    mid_x = rect.x + rect.width / 2
    spots = {
      IconSide.left: canvas.Box(mid_x - c.left_x - c.size, rect.y + c.left_y, c.size, c.size),
      IconSide.right: canvas.Box(mid_x + c.right_x, rect.y + c.right_y, c.size, c.size),
    }
    for side, lamp in self._lamps.items():
      if lamp.mode is not None:
        lamp.render(spots[side])

  @property
  def config(self) -> TurnSignalConfig:
    return self._config

  @config.setter
  def config(self, new_config: TurnSignalConfig):
    self._config = new_config

_PROVIDER_TAGS = {0: "", 1: "NAV", 2: "MBX", 3: "VIS", 4: "OSM"}
_NAV_TEX_W, _NAV_TEX_H = 256, 128
_NAV_BADGE_W = 160
_NAV_FONT = 36
_NAV_SHIFT = -260

class NavInfluenceRenderer(Widget):
  def __init__(self):
    super().__init__()
    self.engaged = False
    self.valid = False
    self.provider = 0
    self.long_override = False
    self._streak = 0
    self.font = gui_app.font(FontWeight.BOLD)
    self._offscreen = rl.load_render_texture(_NAV_TEX_W, _NAV_TEX_H)

  def update(self):
    sm = _feed()
    if sm.updated["iqPlan"]:
      nav = sm["iqPlan"].iqNavState.nav
      self.engaged = nav.engaged
      self.valid = nav.valid
      self.provider = getattr(nav.provider, "raw", nav.provider)
    if sm.updated["carControl"]:
      self.long_override = sm["carControl"].cruiseControl.override
    self._streak = self._streak + 1 if (self.engaged and self.valid) else 0

  def _blinked_off(self) -> bool:
    fps = gui_app.target_fps
    return self.engaged and (self._streak % fps) < (fps / 2.5)

  def _bake(self, label: str):
    extent = canvas.span(self.font, label, _NAV_FONT)
    badge = canvas.Box((_NAV_TEX_W - _NAV_BADGE_W) // 2, (_NAV_TEX_H - extent.y - 10) // 2,
                       _NAV_BADGE_W, int(extent.y + 10))
    rl.begin_texture_mode(self._offscreen)
    rl.clear_background(canvas.CLEAR)
    canvas.panel(badge, 0.2, 10, COLORS.OVERRIDE if self.long_override else canvas.shade(0, 255, 0, 255))
    rl.rl_set_blend_factors(rl.RL_ZERO, rl.RL_ONE_MINUS_SRC_ALPHA, 0x8006)
    rl.rl_set_blend_mode(rl.BLEND_CUSTOM)
    canvas.glyphs(self.font, label,
                  canvas.Pt(badge.x + (badge.width - extent.x) / 2, badge.y + (badge.height - extent.y) / 2),
                  _NAV_FONT, canvas.WHITE)
    rl.rl_set_blend_mode(rl.BLEND_ALPHA)
    rl.end_texture_mode()

  def _render(self, rect):
    if not self.valid or self._blinked_off():
      return
    label = _PROVIDER_TAGS.get(int(self.provider), "NAV")
    if not label:
      return
    self._bake(label)
    ax = rect.x + rect.width / 2 + _NAV_SHIFT - _NAV_TEX_W / 2
    ay = (rect.height / 4 - 40) - _NAV_TEX_H / 2
    canvas.stamp_scaled(self._offscreen.texture, canvas.Box(0, 0, _NAV_TEX_W, -_NAV_TEX_H),
                        canvas.Box(ax, ay, _NAV_TEX_W, _NAV_TEX_H), canvas.Pt(0, 0), 0, canvas.WHITE)

class ChevronOptions:
  OFF = 0
  DISTANCE_ONLY = 1
  SPEED_ONLY = 2
  TTC_ONLY = 3
  ALL = 4

_CH_FONT = 40
_CH_LINE = 50
_CH_MARGIN = 20
_CH_FADE_DOWN = 0.05
_CH_FADE_UP = 0.1
_CH_DEDUP = 3.0

def _gap_label(d_rel: float, _v_abs: float) -> str:
  val = max(0.0, d_rel)
  return f"{val:.0f} m" if ui_state.is_metric else f"{val * 3.28084:.0f} ft"

def _pace_label(_d_rel: float, v_abs: float) -> str:
  unit = "km/h" if ui_state.is_metric else "mph"
  return f"{max(0.0, v_abs * _speed_scale()):.0f} {unit}"

def _ttc_label(d_rel: float, _v_abs: float, v_ego: float) -> str:
  ttc = (d_rel / v_ego) if (d_rel > 0 and v_ego > 0) else 0.0
  return f"{ttc:.1f} s" if 0 < ttc < 200 else "---"

_CH_METRICS = (
  ((ChevronOptions.DISTANCE_ONLY, ChevronOptions.ALL), lambda d, va, ve: _gap_label(d, va)),
  ((ChevronOptions.SPEED_ONLY, ChevronOptions.ALL), lambda d, va, ve: _pace_label(d, va)),
  ((ChevronOptions.TTC_ONLY, ChevronOptions.ALL), _ttc_label),
)

class ChevronMetrics:
  def __init__(self):
    self._alpha: float = 0.0
    self._font = gui_app.font(FontWeight.SEMI_BOLD)

  def update_alpha(self, has_lead: bool):
    self._alpha = float(np.clip(self._alpha + (_CH_FADE_UP if has_lead else -_CH_FADE_DOWN), 0.0, 1.0))

  def should_render(self) -> bool:
    return ui_state.chevron_metrics != ChevronOptions.OFF and self._alpha > 0.0

  @staticmethod
  def _marker_size(d_rel: float) -> float:
    return float(np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0)) * 2.35

  def _labels(self, d_rel: float, v_rel: float, v_ego: float) -> list[str]:
    mode = ui_state.chevron_metrics
    return [fn(d_rel, v_rel + v_ego, v_ego) for modes, fn in _CH_METRICS if mode in modes]

  def _top_y(self, anchor_y: float, size: float, n: int, rect) -> float:
    y = anchor_y + size + 15
    block = n * _CH_LINE
    floor = rect.y + rect.height - _CH_MARGIN
    if y + block > floor:
      y = max(rect.y + _CH_MARGIN, min(anchor_y, floor) - 15 - block)
    return y

  def _stack(self, lines, cx: float, top: float, rect):
    a = self._alpha
    fg = canvas.shade(255, 255, 255, int(255 * a))
    shadow = canvas.shade(0, 0, 0, int(200 * a))
    floor = rect.y + rect.height - _CH_MARGIN
    for i, line in enumerate(lines):
      y = int(top + i * _CH_LINE)
      if y + _CH_LINE > floor:
        break
      w = canvas.span(self._font, line, _CH_FONT).x
      x = int(np.clip(cx - w / 2, rect.x + _CH_MARGIN, rect.x + rect.width - w - _CH_MARGIN))
      canvas.glyphs(self._font, line, canvas.Pt(x + 2, y + 2), _CH_FONT, shadow)
      canvas.glyphs(self._font, line, canvas.Pt(x, y), _CH_FONT, fg)

  def _one_lead(self, lead, marker, v_ego: float, rect):
    if not self.should_render() or marker.center is None:
      return
    lines = self._labels(lead.dRel, lead.vRel, v_ego)
    if not lines:
      return
    self._stack(lines, marker.center[0], self._top_y(marker.center[1], self._marker_size(lead.dRel), len(lines), rect), rect)

  @staticmethod
  def _active_leads(radar_state, markers):
    tracked = []
    for lead, marker in zip((radar_state.leadOne, radar_state.leadTwo), markers, strict=False):
      if lead and lead.status and marker.center is not None:
        tracked.append((lead, marker))
    if len(tracked) == 2 and abs(tracked[0][0].dRel - tracked[1][0].dRel) <= _CH_DEDUP:
      tracked.pop()
    return tracked

  def draw_lead_status(self, sm, radar_state, rect, lead_vehicles):
    present = [radar_state.leadOne, radar_state.leadTwo]
    self.update_alpha(any(bool(x) and x.status for x in present))
    if not self.should_render():
      return
    v_ego = sm['carState'].vEgo
    for lead, marker in self._active_leads(radar_state, lead_vehicles):
      self._one_lead(lead, marker, v_ego, rect)

_TEAL = canvas.shade(0x0C, 0x94, 0x96, 0xFF)
_AMBER = canvas.shade(255, 188, 0, 255)
_GREEN = canvas.shade(0, 255, 0, 255)
_GREY = canvas.shade(145, 155, 149, 255)
_G = 9.81
_BAR_FONT = 38
_ANGLE_TYPES = (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED)


@dataclass
class Readout:
  tag: str
  value: str
  unit: str = ""
  color: object = field(default_factory=lambda: canvas.WHITE)
  tag_text: str = ""
  value_text: str = ""
  unit_text: str = ""
  tag_w: float = 0.0
  value_w: float = 0.0
  unit_w: float = 0.0
  span: float = 0.0

  def size_up(self, font, px: int):
    self.tag_text = f"{self.tag} "
    self.value_text = self.value
    self.unit_text = f" {self.unit}" if self.unit else ""
    self.tag_w = canvas.span(font, self.tag_text, px, 0).x
    self.value_w = canvas.span(font, self.value_text, px, 0).x
    self.unit_w = canvas.span(font, self.unit_text, px, 0).x if self.unit else 0
    self.span = self.tag_w + self.value_w + self.unit_w

  @property
  def total_width(self):
    return self.span

  def measure(self, font, px):
    self.size_up(font, px)


UiElement = Readout

def _banded(magnitude, warn, crit, ok):
  if magnitude > crit:
    return canvas.RED
  return _AMBER if magnitude > warn else ok

def _closing(v_rel):
  return _banded(-v_rel if v_rel < 0 else 0.0, 0.0, 4.4704, canvas.WHITE)

def _following(d_rel):
  if d_rel < 5:
    return canvas.RED
  return _AMBER if d_rel < 15 else canvas.WHITE

def _steer_tint(sm):
  if not sm['carControl'].latActive:
    return canvas.WHITE
  return _GREY if sm['carState'].steeringPressed else _TEAL

def _angle_tint(sm, deg):
  floor = _steer_tint(sm) if sm['carControl'].latActive else canvas.WHITE
  return _banded(abs(deg), 90.0, 180.0, floor)

def _yaw_offset(sm):
  return sm['vehicleParameters'].angleOffsetAverageDeg if sm.valid['vehicleParameters'] else 0.0

def _bank(sm):
  return sm['vehicleParameters'].roll if sm.valid['vehicleParameters'] else 0.0

def _units(is_metric):
  return (CV.MS_TO_KPH, "km/h") if is_metric else (CV.MS_TO_MPH, "mph")

def _fix(sm):
  for svc in ('gpsLocationExternal', 'gpsLocation'):
    if sm.valid[svc]:
      return sm[svc], svc
  return None, None

def steering_angle(sm, is_metric):
  deg = sm['carState'].steeringAngleDeg - _yaw_offset(sm)
  return Readout("R.S.", f"{deg:.1f}°", color=_angle_tint(sm, deg))

def desired_steering_angle(sm, is_metric):
  live = sm['carControl'].latActive
  off = _yaw_offset(sm)
  lat = sm['controlsState'].lateralControlState
  if lat.which() == 'angleState':
    want = lat.angleState.steeringAngleDesiredDeg - off
  else:
    want = sm['carControl'].actuators.steeringAngleDeg - off
  seen = sm['carState'].steeringAngleDeg - off
  tint = _banded(abs(seen), 90.0, 180.0, _TEAL) if live else canvas.WHITE
  return Readout("D.S.", f"{want:.1f}°" if live else "-", color=tint)

def desired_steering_pid(sm, is_metric):
  live = sm['carControl'].latActive
  off = _yaw_offset(sm)
  want = sm['controlsState'].lateralControlState.pidState.steeringAngleDesiredDeg - off
  seen = sm['carState'].steeringAngleDeg - off
  tint = _banded(abs(seen), 90.0, 180.0, _TEAL) if live else canvas.WHITE
  return Readout("D.S.", f"{want:.1f}°" if live else "-", color=tint)

def actual_lat_accel(sm, is_metric):
  a = sm['controlsState'].curvature * sm['carState'].vEgo ** 2 - _bank(sm) * _G
  return Readout("A.L.A.", f"{a:.2f}", "m/s^2", _steer_tint(sm))

def desired_lat_accel(sm, is_metric):
  live = sm['carControl'].latActive
  a = sm['controlsState'].desiredCurvature * sm['carState'].vEgo ** 2 - _bank(sm) * _G
  return Readout("D.L.A.", f"{a:.2f}" if live else "-", "m/s^2", _steer_tint(sm))

def a_ego(sm, is_metric):
  return Readout("L.ACC.", f"{sm['carState'].aEgo:.1f}", "m/s^2")

def lead_distance(sm, is_metric):
  lead = sm['radarState'].leadOne
  return Readout("REL DIST", "-", "m") if not lead.status else Readout("REL DIST", f"{lead.dRel:.0f}", "m", _following(lead.dRel))

def lead_rel_speed(sm, is_metric):
  lead = sm['radarState'].leadOne
  k, unit = _units(is_metric)
  return Readout("REL SPEED", "-", unit) if not lead.status else Readout("REL SPEED", f"{lead.vRel * k:.0f}", unit, _closing(lead.vRel))

def lead_speed(sm, is_metric):
  lead = sm['radarState'].leadOne
  k, unit = _units(is_metric)
  if not lead.status:
    return Readout("L.S.", "-", unit)
  return Readout("L.S.", f"{(lead.vRel + sm['carState'].vEgo) * k:.0f}", unit, _closing(lead.vRel))

def friction_coefficient(sm, is_metric):
  ltp = sm['lateralTorqueParameters']
  return Readout("FRIC.", f"{ltp.frictionCoefficientFiltered:.3f}", color=_GREEN if ltp.valid else canvas.WHITE)

def lat_accel_factor(sm, is_metric):
  ltp = sm['lateralTorqueParameters']
  return Readout("L.A.F.", f"{ltp.latAccelFactorFiltered:.3f}", color=_GREEN if ltp.valid else canvas.WHITE)

def eps_torque(sm, is_metric):
  return Readout("E.T.", f"{abs(sm['carState'].steeringTorqueEps):.1f}", "N·dm")

_COMPASS = ("N", "NE", "E", "SE", "S", "SW", "W", "NW")

def bearing(sm, is_metric):
  fix, _ = _fix(sm)
  if fix is None or fix.bearingAccuracyDeg == 180.0:
    return Readout("B.D.", "OFF | -")
  heading = _COMPASS[int(((fix.bearingDeg + 22.5) % 360) // 45)]
  return Readout("B.D.", f"{heading} | {fix.bearingDeg:.0f}°")

def altitude(sm, is_metric):
  fix, svc = _fix(sm)
  if fix is None:
    return Readout("ALT.", "-", "m")
  acc = fix.horizontalAccuracy if svc == 'gpsLocationExternal' else 1.0
  return Readout("ALT.", f"{fix.altitude:.1f}" if acc != 0.0 else "-", "m")

def _desired_probe(sm):
  if sm['controlsState'].lateralControlState.which() == 'angleState':
    return desired_steering_angle
  if ui_state.CP is not None and ui_state.CP.steerControlType in _ANGLE_TYPES:
    return desired_steering_angle
  if sm['controlsState'].lateralControlState.which() == 'pidState':
    return desired_steering_pid
  return desired_lat_accel

class IQDevMetricsOverlay(Widget):
  DEV_UI_OFF = 0
  DEV_UI_RIGHT = 1
  DEV_UI_BOTTOM = 2
  DEV_UI_BOTH = 3
  BOTTOM_BAR_HEIGHT = 61

  def __init__(self):
    super().__init__()
    self._face = gui_app.font(FontWeight.BOLD)
    self.dev_ui_mode = self.DEV_UI_OFF

  @staticmethod
  def get_bottom_dev_ui_offset():
    return IQDevMetricsOverlay.BOTTOM_BAR_HEIGHT if ui_state.developer_ui != IQDevMetricsOverlay.DEV_UI_OFF else 0

  def _update_state(self) -> None:
    self.dev_ui_mode = ui_state.developer_ui

  def _render(self, rect) -> None:
    if self.dev_ui_mode == self.DEV_UI_OFF:
      return
    sm = ui_state.sm
    if sm.recv_frame["carState"] < ui_state.started_frame:
      return
    self._paint_bar(rect)

  def _gather(self, sm):
    probes = (_desired_probe(sm), actual_lat_accel, steering_angle, a_ego, lead_speed)
    cells = [probe(sm, ui_state.is_metric) for probe in probes]
    for cell in cells:
      cell.size_up(self._face, _BAR_FONT)
    return cells

  def _paint_bar(self, rect) -> None:
    height = self.BOTTOM_BAR_HEIGHT
    top = int(rect.y + rect.height - height)
    canvas.slab(rect.x, top, rect.width, height, canvas.shade(0, 0, 0, 100))

    cells = self._gather(ui_state.sm)
    slack = (rect.width - sum(c.span for c in cells)) / (len(cells) + 1)
    baseline = top + height // 2 - _BAR_FONT // 2

    cursor = rect.x + slack
    for cell in cells:
      self._paint_cell(cursor, baseline, cell)
      cursor += cell.span + slack

  def _paint_cell(self, x, y, cell) -> None:
    canvas.glyphs(self._face, cell.tag_text, canvas.Pt(x, y), _BAR_FONT, canvas.WHITE)
    canvas.glyphs(self._face, cell.value_text, canvas.Pt(x + cell.tag_w, y), _BAR_FONT, cell.color)
    if cell.unit:
      canvas.glyphs(self._face, cell.unit_text, canvas.Pt(x + cell.tag_w + cell.value_w, y), _BAR_FONT, canvas.WHITE)

_SL_M_TO_FT = 3.28084
_SL_M_TO_MI = 0.000621371
_SL_AHEAD_STEPS = 5
_SL_ASSIST = custom.IQPlan.SpeedLimit.AssistState
_SL_SOURCE = custom.IQPlan.SpeedLimit.Source
_SL_GREY = canvas.shade(145, 155, 149, 255)
_SL_DARK = canvas.shade(77, 77, 77, 255)
_SL_PANEL_BG = canvas.shade(0, 0, 0, 180)
_SL_PANEL_EDGE = canvas.shade(255, 255, 255, 100)

def _dim(color, alpha: float):
  return canvas.with_opacity(color, 255 * alpha)

class IQSpeedLimitOverlay(Widget):
  def __init__(self):
    super().__init__()
    self.speed_limit = 0.0
    self.speed_limit_last = 0.0
    self.speed_limit_offset = 0.0
    self.speed_limit_valid = False
    self.speed_limit_last_valid = False
    self.speed_limit_final_last = 0.0
    self.speed_limit_source = _SL_SOURCE.none
    self.assist_state = _SL_ASSIST.disabled

    self.ahead_limit = 0.0
    self.ahead_dist = 0.0
    self._ahead_prev = 0.0
    self.ahead_valid = False
    self._ahead_streak = 0

    self.assist_frame = 0
    self.speed = 0.0
    self.set_speed = 0.0

    self._bold = gui_app.font(FontWeight.BOLD)
    self._demi = gui_app.font(FontWeight.SEMI_BOLD)
    self._norm = gui_app.font(FontWeight.NORMAL)
    self._pulse_ema = FirstOrderFilter(1.0, 0.5, 1 / gui_app.target_fps)

    px = 90
    self._up = gui_app.texture("img_plus_arrow_up.png", px, px)
    self._down = gui_app.texture("img_minus_arrow_down.png", px, px)

  @property
  def speed_limit_assist_state(self):
    return self.assist_state

  @property
  def _scale(self):
    return CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH

  def _take_plan(self, lp_iq):
    k = self._scale
    r = lp_iq.speedLimit.resolver
    self.speed_limit = r.speedLimit * k
    self.speed_limit_last = r.speedLimitLast * k
    self.speed_limit_offset = r.speedLimitOffset * k
    self.speed_limit_valid = r.speedLimitValid
    self.speed_limit_last_valid = r.speedLimitLastValid
    self.speed_limit_final_last = r.speedLimitFinalLast * k
    self.speed_limit_source = r.source
    self.assist_state = lp_iq.speedLimit.assist.state

  def _take_ahead(self, lmd):
    self.ahead_valid = lmd.speedLimitAheadValid
    self.ahead_limit = lmd.speedLimitAhead * self._scale
    self.ahead_dist = lmd.speedLimitAheadDistance
    if self.ahead_dist < self._ahead_prev:
      self._ahead_streak = min(_SL_AHEAD_STEPS, self._ahead_streak + 1)
    elif self.ahead_dist > self._ahead_prev:
      self._ahead_streak = max(0, self._ahead_streak - 1)
    self._ahead_prev = self.ahead_dist

  def update(self):
    sm = _feed()
    if sm.recv_frame["carState"] < ui_state.started_frame:
      return
    if sm.updated["iqPlan"]:
      self._take_plan(sm["iqPlan"])
    if sm.updated["iqLiveData"]:
      self._take_ahead(sm["iqLiveData"])
    cs = sm["carState"]
    self.set_speed = cs.cruiseState.speed * self._scale
    v_ego = cs.vEgoCluster if cs.vEgoCluster != 0.0 else cs.vEgo
    self.speed = max(0.0, v_ego * self._scale)

  def _spec(self):
    has_limit = self.speed_limit_valid or self.speed_limit_last_valid
    value = str(round(self.speed_limit_last)) if has_limit else "---"
    badge = ""
    if self.speed_limit_offset != 0:
      badge = f"{'' if self.speed_limit_offset > 0 else '-'}{round(abs(self.speed_limit_offset))}"
    warn = ui_state.speed_limit_mode >= 2
    over = has_limit and round(self.speed_limit_final_last) < round(self.speed)
    tint = canvas.RED if (warn and over) else (_SL_GREY if not self.speed_limit_valid else canvas.BLACK)
    return value, badge, tint, has_limit

  def _render(self, rect):
    if ui_state.speed_limit_mode == 0:
      return
    w = UI_CONFIG.set_speed_width_metric if ui_state.is_metric else UI_CONFIG.set_speed_width_imperial
    sign = canvas.Box(rect.x + 60 - 6, rect.y + 45 + UI_CONFIG.set_speed_height + 12, w + 12, 160)
    if self.assist_state == _SL_ASSIST.preActive:
      self.assist_frame += 1
      pulse = 0.65 + 0.35 * math.sin(self.assist_frame * math.pi / gui_app.target_fps)
      self._sign(sign, self._pulse_ema.update(pulse))
      self._nudge_arrow(sign)
    else:
      self.assist_frame = 0
      self._pulse_ema.update(1.0)
      self._sign(sign)
      self._ahead(sign)

  def _sign(self, rect, alpha=1.0):
    value, badge, tint, has_limit = self._spec()
    (self._vienna if ui_state.is_metric else self._mutcd)(rect, value, badge, tint, has_limit, alpha)

  def _nudge_arrow(self, sign):
    delta = round(self.speed_limit_final_last) - round(self.set_speed)
    if delta == 0:
      return
    arrow = self._up if delta > 0 else self._down
    bounce = int(20 * math.sin(self.assist_frame * 2.0 * math.pi / (gui_app.target_fps * 2.5)))
    x = sign.x + (sign.width - arrow.width) / 2
    y = sign.y + (sign.height - arrow.height) / 2 + (bounce if delta > 0 else -bounce)
    canvas.stamp(arrow, x, y, canvas.WHITE)

  def _vienna(self, rect, value, badge, tint, has_limit, alpha=1.0):
    hub = canvas.Pt(rect.x + rect.width / 2, rect.y + rect.height / 2)
    radius = (rect.width + 18) / 2
    canvas.disc_at(hub, radius, _dim(canvas.WHITE, alpha))
    canvas.annulus(hub, radius * 0.80, radius, 0, 360, 36, _dim(canvas.RED, alpha))
    canvas.glyphs_centered(self._bold, value, 70 if len(value) >= 3 else 85, hub, _dim(tint, alpha))
    if badge and has_limit:
      br = radius * 0.4
      bc = canvas.Pt(rect.x + rect.width - br / 2, rect.y + br / 2)
      canvas.disc_at(bc, br, _dim(canvas.BLACK, alpha))
      canvas.annulus(bc, br - 3, br, 0, 360, 36, _dim(_SL_DARK, alpha))
      canvas.glyphs_centered(self._bold, badge, int(br * 2 * (0.5 if len(badge) < 3 else 0.45)), bc, _dim(canvas.WHITE, alpha))

  def _mutcd(self, rect, value, badge, tint, has_limit, alpha=1.0):
    canvas.panel(rect, 0.35, 10, _dim(canvas.WHITE, alpha))
    inner = canvas.Box(rect.x + 10, rect.y + 10, rect.width - 20, rect.height - 20)
    canvas.panel_outline(inner, 0.35, 10, 4, _dim(canvas.BLACK, alpha))
    mid = rect.x + rect.width / 2
    canvas.glyphs_centered(self._demi, "SPEED", 40, canvas.Pt(mid, rect.y + 40), _dim(canvas.BLACK, alpha))
    canvas.glyphs_centered(self._demi, "LIMIT", 40, canvas.Pt(mid, rect.y + 80), _dim(canvas.BLACK, alpha))
    canvas.glyphs_centered(self._bold, value, 90, canvas.Pt(mid, rect.y + 150), _dim(tint, alpha))
    if badge and has_limit:
      side = rect.width * 0.3
      overlap = side * 0.2
      chip = canvas.Box(rect.x + rect.width - side / 1.5 + overlap, rect.y - side / 1.25 + overlap, side, side)
      canvas.panel(chip, 0.35, 10, _dim(canvas.BLACK, alpha))
      canvas.panel_outline(chip, 0.35, 10, 6, _dim(_SL_DARK, alpha))
      canvas.glyphs_centered(self._bold, badge, int(side * (0.6 if len(badge) < 3 else 0.475)),
                             canvas.Pt(chip.x + side / 2, chip.y + side / 2), _dim(canvas.WHITE, alpha))

  def _ahead(self, sign):
    if not (self.ahead_valid and self.ahead_limit > 0 and self.ahead_limit != self.speed_limit_last and self._ahead_streak > 0):
      return
    panel = canvas.Box(sign.x + (sign.width - 170) / 2, sign.y + sign.height + 10, 170, 160)
    canvas.panel(panel, 0.35, 10, _SL_PANEL_BG)
    canvas.panel_outline(panel, 0.35, 10, 3, _SL_PANEL_EDGE)
    mid = panel.x + panel.width / 2
    canvas.glyphs_centered(self._demi, "AHEAD", 40, canvas.Pt(mid, panel.y + 28), _SL_GREY)
    canvas.glyphs_centered(self._bold, str(round(self.ahead_limit)), 70, canvas.Pt(mid, panel.y + 82), canvas.WHITE)
    canvas.glyphs_centered(self._norm, self._dist(self.ahead_dist), 36, canvas.Pt(mid, panel.y + 134), _SL_GREY)

  @staticmethod
  def _dist(d):
    if ui_state.is_metric:
      if d < 50:
        return tr("Near")
      if d >= 1000:
        return f"{d / 1000:.1f} km"
      return f"{int(round(d, -1) if d < 200 else round(d, -2))} m"
    ft = d * _SL_M_TO_FT
    if ft < 100:
      return tr("Near")
    if ft >= 900:
      return f"{d * _SL_M_TO_MI:.1f} mi"
    step = 50 if ft < 500 else 100
    return f"{int(round(ft / step) * step)} ft"