from cereal import car
from common.numpy_fast import clip, interp
from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                           create_accel_command, create_acc_cancel_command, \
                                           create_fcw_command, create_lta_steer_command
from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
                                        MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
from opendbc.can.packer import CANPacker
from common.op_params import opParams

VisualAlert = car.CarControl.HUDControl.VisualAlert


class CarController():
  def __init__(self, dbc_name, CP, VM):
    self.last_steer = 0
    self.alert_active = False
    self.last_standstill = False
    self.standstill_req = False
    self.steer_rate_limited = False
    self.standstill_hack = opParams().get('standstill_hack')

    self.packer = CANPacker(dbc_name)
    self.gas = 0
    self.accel = 0

  def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
             left_line, right_line, lead, left_lane_depart, right_lane_depart):

    # gas and brake
    if CS.CP.enableGasInterceptor and enabled:
      MAX_INTERCEPTOR_GAS = 0.5
      # RAV4 has very sensitive gas pedal
      if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]:
        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
      elif CS.CP.carFingerprint in [CAR.COROLLA]:
        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
      else:
        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
      # offset for creep and windbrake
      pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
      pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
      interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
    else:
      interceptor_gas_cmd = 0.
    pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)

    # steer torque
    new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
    self.steer_rate_limited = new_steer != apply_steer

    # Cut steering while we're in a known fault state (2s)
    if not enabled or CS.steer_state in [9, 25] or abs(CS.out.steeringRateDeg) > 100:
      apply_steer = 0
      apply_steer_req = 0
    else:
      apply_steer_req = 1

    # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
    # than CS.cruiseState.enabled. confirm they're not meaningfully different
    if not enabled and CS.pcm_acc_status:
      pcm_cancel_cmd = 1

    # on entering standstill, send standstill request
    if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR and not self.standstill_hack:
      self.standstill_req = True
    if CS.pcm_acc_status != 8:
      # pcm entered standstill or it's disabled
      self.standstill_req = False

    self.last_steer = apply_steer
    self.last_standstill = CS.out.standstill

    can_sends = []

    #*** control msgs ***
    #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)

    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
    can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
    if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
      can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))

    # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
    # if frame % 2 == 0:
    #   can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
    #   can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))

    # we can spam can to cancel the system even if we are using lat only control
    if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
      lead = lead or CS.out.vEgo < 12.    # at low speed we always assume the lead is present so ACC can be engaged

      # Lexus IS uses a different cancellation message
      if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
        can_sends.append(create_acc_cancel_command(self.packer))
      elif CS.CP.openpilotLongitudinalControl:
        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, CS.distance_btn))
        self.accel = pcm_accel_cmd
      else:
        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, CS.distance_btn))

    if frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
      # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
      # This prevents unexpected pedal range rescaling
      can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
      self.gas = interceptor_gas_cmd

    # ui mesg is at 100Hz but we send asap if:
    # - there is something to display
    # - there is something to stop displaying
    fcw_alert = hud_alert == VisualAlert.fcw
    steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]

    send_ui = False
    if ((fcw_alert or steer_alert) and not self.alert_active) or \
       (not (fcw_alert or steer_alert) and self.alert_active):
      send_ui = True
      self.alert_active = not self.alert_active
    elif pcm_cancel_cmd:
      # forcing the pcm to disengage causes a bad fault sound so play a good sound instead
      send_ui = True

    if (frame % 100 == 0 or send_ui):
      can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))

    if frame % 100 == 0 and CS.CP.enableDsu:
      can_sends.append(create_fcw_command(self.packer, fcw_alert))

    # *** static msgs ***
    for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
      if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
        can_sends.append(make_can_msg(addr, vl, bus))

    new_actuators = actuators.copy()
    new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
    new_actuators.accel = self.accel
    new_actuators.gas = self.gas

    return new_actuators, can_sends

[((24, 18, 24, 37), 'opendbc.can.packer.CANPacker', 'CANPacker', ({(24, 28, 24, 36): 'dbc_name'}, {}), '(dbc_name)', False, 'from opendbc.can.packer import CANPacker\n'), ((47, 20, 47, 103), 'common.numpy_fast.clip', 'clip', ({(47, 25, 47, 40): 'actuators.accel', (47, 42, 47, 71): 'CarControllerParams.ACCEL_MIN', (47, 73, 47, 102): 'CarControllerParams.ACCEL_MAX'}, {}), '(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.\n    ACCEL_MAX)', False, 'from common.numpy_fast import clip, interp\n'), ((51, 18, 51, 125), 'selfdrive.car.apply_toyota_steer_torque_limits', 'apply_toyota_steer_torque_limits', ({(51, 51, 51, 60): 'new_steer', (51, 62, 51, 77): 'self.last_steer', (51, 79, 51, 103): 'CS.out.steeringTorqueEps', (51, 105, 51, 124): 'CarControllerParams'}, {}), '(new_steer, self.last_steer, CS.out.\n    steeringTorqueEps, CarControllerParams)', False, 'from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg\n'), ((42, 21, 42, 103), 'common.numpy_fast.interp', 'interp', ({(42, 28, 42, 39): 'CS.out.vEgo', (42, 41, 42, 85): '[0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION]', (42, 87, 42, 102): '[-0.4, 0.0, 0.2]'}, {}), '(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-0.4, \n    0.0, 0.2])', False, 'from common.numpy_fast import clip, interp\n'), ((44, 28, 44, 72), 'common.numpy_fast.clip', 'clip', ({(44, 33, 44, 46): 'pedal_command', (44, 48, 44, 50): '0.0', (44, 52, 44, 71): 'MAX_INTERCEPTOR_GAS'}, {}), '(pedal_command, 0.0, MAX_INTERCEPTOR_GAS)', False, 'from common.numpy_fast import clip, interp\n'), ((84, 21, 84, 91), 'selfdrive.car.toyota.toyotacan.create_steer_command', 'create_steer_command', ({(84, 42, 84, 53): 'self.packer', (84, 55, 84, 66): 'apply_steer', (84, 68, 84, 83): 'apply_steer_req', (84, 85, 84, 90): 'frame'}, {}), '(self.packer, apply_steer, apply_steer_req, frame)', False, 'from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, create_accel_command, create_acc_cancel_command, create_fcw_command, create_lta_steer_command\n'), ((22, 27, 22, 37), 'common.op_params.opParams', 'opParams', ({}, {}), '()', False, 'from common.op_params import opParams\n'), ((36, 22, 36, 115), 'common.numpy_fast.interp', 'interp', ({(36, 29, 36, 40): 'CS.out.vEgo', (36, 42, 36, 96): '[0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION]', (36, 98, 36, 114): '[0.15, 0.3, 0.0]'}, {}), '(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION],\n    [0.15, 0.3, 0.0])', False, 'from common.numpy_fast import clip, interp\n'), ((86, 23, 86, 78), 'selfdrive.car.toyota.toyotacan.create_lta_steer_command', 'create_lta_steer_command', ({(86, 48, 86, 59): 'self.packer', (86, 61, 86, 62): '(0)', (86, 64, 86, 65): '(0)', (86, 67, 86, 77): '(frame // 2)'}, {}), '(self.packer, 0, 0, frame // 2)', False, 'from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, create_accel_command, create_acc_cancel_command, create_fcw_command, create_lta_steer_command\n'), ((109, 23, 109, 99), 'selfdrive.car.create_gas_interceptor_command', 'create_gas_interceptor_command', ({(109, 54, 109, 65): 'self.packer', (109, 67, 109, 86): 'interceptor_gas_cmd', (109, 88, 109, 98): '(frame // 2)'}, {}), '(self.packer, interceptor_gas_cmd, frame // 2)', False, 'from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg\n'), ((128, 23, 128, 151), 'selfdrive.car.toyota.toyotacan.create_ui_command', 'create_ui_command', ({(128, 41, 128, 52): 'self.packer', (128, 54, 128, 65): 'steer_alert', (128, 67, 128, 81): 'pcm_cancel_cmd', (128, 83, 128, 92): 'left_line', (128, 94, 128, 104): 'right_line', (128, 106, 128, 122): 'left_lane_depart', (128, 124, 128, 141): 'right_lane_depart', (128, 143, 128, 150): 'enabled'}, {}), '(self.packer, steer_alert, pcm_cancel_cmd, left_line,\n    right_line, left_lane_depart, right_lane_depart, enabled)', False, 'from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, create_accel_command, create_acc_cancel_command, create_fcw_command, create_lta_steer_command\n'), ((131, 23, 131, 65), 'selfdrive.car.toyota.toyotacan.create_fcw_command', 'create_fcw_command', ({(131, 42, 131, 53): 'self.packer', (131, 55, 131, 64): 'fcw_alert'}, {}), '(self.packer, fcw_alert)', False, 'from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, create_accel_command, create_acc_cancel_command, create_fcw_command, create_lta_steer_command\n'), ((38, 22, 38, 114), 'common.numpy_fast.interp', 'interp', ({(38, 29, 38, 40): 'CS.out.vEgo', (38, 42, 38, 96): '[0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION]', (38, 98, 38, 113): '[0.3, 0.4, 0.0]'}, {}), '(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION],\n    [0.3, 0.4, 0.0])', False, 'from common.numpy_fast import clip, interp\n'), ((40, 22, 40, 114), 'common.numpy_fast.interp', 'interp', ({(40, 29, 40, 40): 'CS.out.vEgo', (40, 42, 40, 96): '[0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION]', (40, 98, 40, 113): '[0.4, 0.5, 0.0]'}, {}), '(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION],\n    [0.4, 0.5, 0.0])', False, 'from common.numpy_fast import clip, interp\n'), ((99, 25, 99, 63), 'selfdrive.car.toyota.toyotacan.create_acc_cancel_command', 'create_acc_cancel_command', ({(99, 51, 99, 62): 'self.packer'}, {}), '(self.packer)', False, 'from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, create_accel_command, create_acc_cancel_command, create_fcw_command, create_lta_steer_command\n'), ((136, 25, 136, 52), 'selfdrive.car.make_can_msg', 'make_can_msg', ({(136, 38, 136, 42): 'addr', (136, 44, 136, 46): 'vl', (136, 48, 136, 51): 'bus'}, {}), '(addr, vl, bus)', False, 'from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg\n'), ((101, 25, 101, 146), 'selfdrive.car.toyota.toyotacan.create_accel_command', 'create_accel_command', ({(101, 46, 101, 57): 'self.packer', (101, 59, 101, 72): 'pcm_accel_cmd', (101, 74, 101, 88): 'pcm_cancel_cmd', (101, 90, 101, 109): 'self.standstill_req', (101, 111, 101, 115): 'lead', (101, 117, 101, 128): 'CS.acc_type', (101, 130, 101, 145): 'CS.distance_btn'}, {}), '(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.\n    standstill_req, lead, CS.acc_type, CS.distance_btn)', False, 'from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, create_accel_command, create_acc_cancel_command, create_fcw_command, create_lta_steer_command\n'), ((104, 25, 104, 120), 'selfdrive.car.toyota.toyotacan.create_accel_command', 'create_accel_command', ({(104, 46, 104, 57): 'self.packer', (104, 59, 104, 60): '(0)', (104, 62, 104, 76): 'pcm_cancel_cmd', (104, 78, 104, 83): '(False)', (104, 85, 104, 89): 'lead', (104, 91, 104, 102): 'CS.acc_type', (104, 104, 104, 119): 'CS.distance_btn'}, {}), '(self.packer, 0, pcm_cancel_cmd, False, lead, CS.\n    acc_type, CS.distance_btn)', False, 'from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, create_accel_command, create_acc_cancel_command, create_fcw_command, create_lta_steer_command\n')]