text: from random import randint
  
  from cereal import car
  from common.params import Params
  from common.realtime import DT_CTRL
  from common.numpy_fast import clip, interp
  from selfdrive.config import Conversions as CV
  from selfdrive.car import apply_std_steer_torque_limits
  from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_scc11, create_scc12, create_scc13, create_scc14, \
                                               create_mdps12, create_lfahda_mfc, create_hda_mfc
  from selfdrive.car.hyundai.values import Buttons, FEATURES, CarControllerParams
  from opendbc.can.packer import CANPacker
  from selfdrive.car.hyundai.scc_smoother import SccSmoother
  from selfdrive.road_speed_limiter import road_speed_limiter_get_active
  
  VisualAlert = car.CarControl.HUDControl.VisualAlert
  LongCtrlState = car.CarControl.Actuators.LongControlState
  
  min_set_speed = 30 * CV.KPH_TO_MS
  
  def process_hud_alert(enabled, fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart):
  
    sys_warning = (visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw))
  
    # initialize to no line visible
    sys_state = 1
    if left_lane and right_lane or sys_warning:  # HUD alert only display when LKAS status is active
      sys_state = 3 if enabled or sys_warning else 4
    elif left_lane:
      sys_state = 5
    elif right_lane:
      sys_state = 6
  
    # initialize to no warnings
    left_lane_warning = 0
    right_lane_warning = 0
    if left_lane_depart:
      left_lane_warning = 1
    if right_lane_depart:
      right_lane_warning = 1
  
    return sys_warning, sys_state, left_lane_warning, right_lane_warning
  
  
  class CarController():
    def __init__(self, dbc_name, CP, VM):
      self.car_fingerprint = CP.carFingerprint
      self.packer = CANPacker(dbc_name)
      self.apply_steer_last = 0
      self.accel = 0
      self.lkas11_cnt = 0
      self.scc12_cnt = -1
      self.resume_cnt = 0
      self.last_lead_distance = 0
      self.resume_wait_timer = 0
      self.turning_signal_timer = 0
      self.longcontrol = CP.openpilotLongitudinalControl
      self.scc_live = not CP.radarOffCan
      self.turning_indicator_alert = False
  
      params = Params()
      self.lfamfc = params.get("MfcSelect", encoding='utf8') == "2"
      self.mad_mode_enabled = params.get("LongControlSelect", encoding='utf8') == "0" or \
                              params.get("LongControlSelect", encoding='utf8') == "1"
      self.haptic_feedback_speed_camera = params.get_bool('HapticFeedbackWhenSpeedCamera')
      self.scc_smoother = SccSmoother()
      self.last_blinker_frame = 0
      self.prev_active_cam = False
      self.active_cam_timer = 0
  
    def update(self, c, enabled, CS, frame, CC, actuators, pcm_cancel_cmd, visual_alert,
               left_lane, right_lane, left_lane_depart, right_lane_depart, set_speed, lead_visible, controls):
  
      # Steering Torque
      new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
      apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, CarControllerParams)
  
      # disable when temp fault is active, or below LKA minimum speed
      lkas_active = c.active and not CS.out.steerWarning and abs(CS.out.steeringAngleDeg) < CS.CP.maxSteeringAngleDeg
  
      # Disable steering while turning blinker on and speed below 60 kph
      if CS.out.leftBlinker or CS.out.rightBlinker:
        self.turning_signal_timer = 0.5 / DT_CTRL  # Disable for 0.5 Seconds after blinker turned off
      if self.turning_indicator_alert: # set and clear by interface
        lkas_active = 0
      if self.turning_signal_timer > 0:
        self.turning_signal_timer -= 1
      if not lkas_active:
        apply_steer = 0
  
      self.apply_steer_last = apply_steer
  
      sys_warning, sys_state, left_lane_warning, right_lane_warning = \
        process_hud_alert(enabled, self.car_fingerprint, visual_alert, left_lane, right_lane, left_lane_depart, right_lane_depart)
  
      if self.haptic_feedback_speed_camera:
        if self.prev_active_cam != self.scc_smoother.active_cam:
          self.prev_active_cam = self.scc_smoother.active_cam
          if self.scc_smoother.active_cam:
            self.active_cam_timer = int(1.5 / DT_CTRL)
  
        if self.active_cam_timer > 0:
          self.active_cam_timer -= 1
          left_lane_warning = right_lane_warning = 1
  
      clu11_speed = CS.clu11["CF_Clu_Vanz"]
      enabled_speed = 38 if CS.is_set_speed_in_mph else 60
      if clu11_speed > enabled_speed or not lkas_active:
        enabled_speed = clu11_speed
  
      if not (min_set_speed < set_speed < 255 * CV.KPH_TO_MS):
        set_speed = min_set_speed
      set_speed *= CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH
  
      if frame == 0:  # initialize counts from last received count signals
        self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"]
  
        # TODO: fix this
        # self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
        # self.scc_update_frame = frame
  
      # check if SCC is alive
      # if frame % 7 == 0:
      # if CS.scc11["AliveCounterACC"] == self.prev_scc_cnt:
      # if frame - self.scc_update_frame > 20 and self.scc_live:
      # self.scc_live = False
      # else:
      # self.scc_live = True
      # self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
      # self.scc_update_frame = frame
  
      self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
      self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10
  
      can_sends = []
      can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
                                     CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane,
                                     left_lane_warning, right_lane_warning, 0))
  
      if CS.mdps_bus or CS.scc_bus == 1:  # send lkas11 bus 1 if mdps or scc is on bus 1
        can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
                                       CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane,
                                       left_lane_warning, right_lane_warning, 1))
  
      if frame % 2 and CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
        can_sends.append(create_clu11(self.packer, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed))
  
      if pcm_cancel_cmd and (self.longcontrol and not self.mad_mode_enabled):
        can_sends.append(create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
      else:
        can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
  
      # fix auto resume - by neokii
      if CS.out.cruiseState.standstill and not CS.out.gasPressed:
  
        if self.last_lead_distance == 0:
          self.last_lead_distance = CS.lead_distance
          self.resume_cnt = 0
          self.resume_wait_timer = 0
  
        # scc smoother
        elif self.scc_smoother.is_active(frame):
          pass
  
        elif self.resume_wait_timer > 0:
          self.resume_wait_timer -= 1
  
        elif abs(CS.lead_distance - self.last_lead_distance) > 0.1:
          can_sends.append(create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
          self.resume_cnt += 1
  
          if self.resume_cnt >= randint(6, 8):
            self.resume_cnt = 0
            self.resume_wait_timer = randint(30, 36)
  
      # reset lead distnce after the car starts moving
      elif self.last_lead_distance != 0:
        self.last_lead_distance = 0
  
      # scc smoother
      self.scc_smoother.update(enabled, can_sends, self.packer, CC, CS, frame, controls)
  
      # send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
      if self.longcontrol and CS.cruiseState_enabled and (CS.scc_bus or not self.scc_live):
        if frame % 2 == 0:
          stopping = (actuators.longControlState == LongCtrlState.stopping)
          apply_accel = clip(actuators.accel if c.active else 0,
                             CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
          apply_accel = self.scc_smoother.get_apply_accel(CS, controls.sm, apply_accel, stopping)
          self.accel = apply_accel
  
          controls.apply_accel = apply_accel
          aReqValue = CS.scc12["aReqValue"]
          controls.aReqValue = aReqValue
  
          if aReqValue < controls.aReqValueMin:
            controls.aReqValueMin = controls.aReqValue
          if aReqValue > controls.aReqValueMax:
            controls.aReqValueMax = controls.aReqValue
  
          if self.scc12_cnt < 0:
            self.scc12_cnt = CS.scc12["CR_VSM_Alive"] if not CS.no_radar else 0
  
          self.scc12_cnt += 1
          self.scc12_cnt %= 0xF
  
          can_sends.append(create_scc11(self.packer, frame, enabled, set_speed, lead_visible, self.scc_live, CS.scc11,
                                        self.scc_smoother.active_cam))
  
          can_sends.append(create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt, self.scc_live, CS.scc12,
                                        CS.out.gasPressed, CS.out.brakePressed, CS.out.cruiseState.standstill,
                                        self.car_fingerprint))
  
          if frame % 20 == 0 and CS.has_scc13:
            can_sends.append(create_scc13(self.packer, CS.scc13))
  
          if CS.has_scc14:
            acc_standstill = stopping if CS.out.vEgo < 2. else False
            lead = self.scc_smoother.get_lead(controls.sm)
  
            if lead is not None:
              d = lead.dRel
              obj_gap = 1 if d < 25 else 2 if d < 40 else 3 if d < 60 else 4 if d < 80 else 5
            else:
              obj_gap = 0
  
            can_sends.append(create_scc14(self.packer, enabled, CS.out.vEgo, acc_standstill, apply_accel, CS.out.gasPressed,
                                          obj_gap, CS.scc14))
      else:
        self.scc12_cnt = -1
  
      # 20 Hz LFA MFA message
      if frame % 5 == 0:
        activated_hda = road_speed_limiter_get_active()
        # activated_hda: 0 - off, 1 - main road, 2 - highway
        if self.lfamfc:
          can_sends.append(create_lfahda_mfc(self.packer, enabled, activated_hda))
        elif CS.has_lfa_hda:
          can_sends.append(create_hda_mfc(self.packer, activated_hda, CS, left_lane, right_lane))
  
      # 5 Hz ACC options
      #if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:
      #  can_sends.extend(create_acc_opt(self.packer))
  
      # 2 Hz front radar options
      #if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
      #  can_sends.append(create_frt_radar_opt(self.packer))
  
      new_actuators = actuators.copy()
      new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
      new_actuators.accel = self.accel
  
      return new_actuators, can_sends
  