#!/usr/bin/env python3
from cereal import car, custom
from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS
from openpilot.selfdrive.controls.lib.events import ET

from openpilot.frogpilot.common.frogpilot_variables import NON_DRIVING_GEARS, params, params_memory

ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type

def handle_experimental_mode(conditional_experimental_mode):
  if conditional_experimental_mode:
    conditional_status = params_memory.get_int("CEStatus")

    override_value = 0 if conditional_status in {1, 2} else 1 if conditional_status >= 3 else 2

    params_memory.put_int("CEStatus", override_value)
  else:
    params.put_bool_nonblocking("ExperimentalMode", not params.get_bool("ExperimentalMode"))

class FrogPilotCard:
  def __init__(self, Car):
    self.car = Car

    self.long_press_threshold = CRUISE_LONG_PRESS * (1.5 if self.car.CP.carName == "gm" else 1)
    self.very_long_press_threshold = CRUISE_LONG_PRESS * 5

    self.accel_pressed = False
    self.decel_pressed = False
    self.force_coast = False
    self.pause_lateral = False
    self.pause_longitudinal = False
    self.prev_distance_button = False
    self.traffic_mode_enabled = False

    self.gap_counter = 0

  def update_distance_button(self, sm):
    if self.car.frogpilot_toggles.experimental_mode_via_distance and sm["carControl"].longActive:
      handle_experimental_mode(self.car.frogpilot_toggles.conditional_experimental_mode)
    elif self.car.frogpilot_toggles.force_coast_via_distance:
      self.force_coast = not self.force_coast
    elif self.car.frogpilot_toggles.pause_lateral_via_distance:
      self.pause_lateral = not self.pause_lateral
    elif self.car.frogpilot_toggles.pause_longitudinal_via_distance:
      self.pause_longitudinal = not self.pause_longitudinal
    elif self.car.frogpilot_toggles.traffic_mode_via_distance and sm["carControl"].longActive:
      self.traffic_mode_enabled = not self.traffic_mode_enabled

  def update_distance_button_long(self, sm):
    if self.car.frogpilot_toggles.experimental_mode_via_distance_long and sm["carControl"].longActive:
      handle_experimental_mode(self.car.frogpilot_toggles.conditional_experimental_mode)
    elif self.car.frogpilot_toggles.force_coast_via_distance_long:
      self.force_coast = not self.force_coast
    elif self.car.frogpilot_toggles.pause_lateral_via_distance_long:
      self.pause_lateral = not self.pause_lateral
    elif self.car.frogpilot_toggles.pause_longitudinal_via_distance_long:
      self.pause_longitudinal = not self.pause_longitudinal
    elif self.car.frogpilot_toggles.traffic_mode_via_distance_long and sm["carControl"].longActive:
      self.traffic_mode_enabled = not self.traffic_mode_enabled

  def update_distance_button_very_long(self, sm):
    self.update_distance_button_long(sm)

    if self.car.frogpilot_toggles.experimental_mode_via_distance_very_long and sm["carControl"].longActive:
      handle_experimental_mode(self.car.frogpilot_toggles.conditional_experimental_mode)
    elif self.car.frogpilot_toggles.force_coast_via_distance_very_long:
      self.force_coast = not self.force_coast
    elif self.car.frogpilot_toggles.pause_lateral_via_distance_very_long:
      self.pause_lateral = not self.pause_lateral
    elif self.car.frogpilot_toggles.pause_longitudinal_via_distance_very_long:
      self.pause_longitudinal = not self.pause_longitudinal
    elif self.car.frogpilot_toggles.traffic_mode_via_distance_very_long and sm["carControl"].longActive:
      self.traffic_mode_enabled = not self.traffic_mode_enabled

  def update_lkas_button(self, sm):
    if self.car.frogpilot_toggles.experimental_mode_via_lkas and sm["carControl"].longActive:
      handle_experimental_mode(self.car.frogpilot_toggles.conditional_experimental_mode)
    elif self.car.frogpilot_toggles.force_coast_via_lkas:
      self.force_coast = not self.force_coast
    elif self.car.frogpilot_toggles.pause_lateral_via_lkas:
      self.pause_lateral = not self.pause_lateral
    elif self.car.frogpilot_toggles.pause_longitudinal_via_lkas:
      self.pause_longitudinal = not self.pause_longitudinal
    elif self.car.frogpilot_toggles.traffic_mode_via_lkas and sm["carControl"].longActive:
      self.traffic_mode_enabled = not self.traffic_mode_enabled

  def update(self, carState, frogpilotCarState, sm):
    self.always_on_lateral_enabled = self.car.frogpilot_toggles.always_on_lateral_set

    if self.car.frogpilot_toggles.use_lkas_for_aol:
      self.always_on_lateral_enabled |= self.car.frogpilot_toggles.always_on_lateral_lkas or carState.cruiseState.enabled
      self.always_on_lateral_enabled &= frogpilotCarState.alwaysOnLateralAllowed
    else:
      self.always_on_lateral_enabled |= self.car.frogpilot_toggles.always_on_lateral_main or carState.cruiseState.enabled
      self.always_on_lateral_enabled &= carState.cruiseState.available

    self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS
    self.always_on_lateral_enabled &= sm["frogpilotPlan"].lateralCheck
    self.always_on_lateral_enabled &= sm["liveCalibration"].calPerc >= 1
    self.always_on_lateral_enabled &= sm["controlsState"].alertType != ET.IMMEDIATE_DISABLE or frogpilot_toggles.frogs_go_moo
    self.always_on_lateral_enabled &= not (carState.brakePressed and carState.vEgo < self.car.frogpilot_toggles.always_on_lateral_pause_speed) or carState.standstill

    if sm.updated["frogpilotPlan"] or any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents):
      self.accel_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in carState.buttonEvents)

    if sm.updated["frogpilotPlan"] or any(be.type == ButtonType.decelCruise for be in carState.buttonEvents):
      self.decel_pressed = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents)

    self.force_coast &= not (carState.brakePressed or carState.gasPressed)

    frogpilotCarState.distancePressed |= params_memory.get_bool("OnroadDistanceButtonPressed")

    if frogpilotCarState.distancePressed:
      self.gap_counter += 1
    elif not self.prev_distance_button:
      self.gap_counter = 0

    if not frogpilotCarState.distancePressed and 1 < self.gap_counter < self.long_press_threshold:
      self.update_distance_button(sm)
    elif self.gap_counter == self.long_press_threshold:
      self.update_distance_button_long(sm)
    elif self.gap_counter == self.very_long_press_threshold:
      self.update_distance_button_very_long(sm)

    lkas_button = any(be.pressed and be.type == FrogPilotButtonType.lkas for be in carState.buttonEvents)

    if lkas_button:
      self.update_lkas_button(sm)

    self.prev_distance_button = frogpilotCarState.distancePressed

    frogpilotCarState.accelPressed = self.accel_pressed
    frogpilotCarState.alwaysOnLateralEnabled = self.always_on_lateral_enabled
    frogpilotCarState.decelPressed = self.decel_pressed
    frogpilotCarState.distanceLongPressed = self.very_long_press_threshold > self.gap_counter >= self.long_press_threshold
    frogpilotCarState.distanceVeryLongPressed = self.gap_counter >= self.very_long_press_threshold
    frogpilotCarState.forceCoast = self.force_coast
    frogpilotCarState.pauseLateral = self.pause_lateral
    frogpilotCarState.pauseLongitudinal = self.pause_longitudinal
    frogpilotCarState.trafficModeEnabled = self.traffic_mode_enabled

    return frogpilotCarState