import cereal.messaging as messaging
import numpy as np

from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip, interp
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, V_CRUISE_MAX
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, A_CRUISE_MAX_VALS, A_CRUISE_MAX_BP, get_max_accel
from openpilot.selfdrive.modeld.constants import ModelConstants

from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController

# class FrogPilotPlanner:
#   def __init__(self, params, params_memory):
#     self.params_memory = params_memory
#     self.cem = ConditionalExperimentalMode()
#     self.mtsc = MapTurnSpeedController()

#     self.override_slc = False

#     self.overridden_speed = 0
#     self.mtsc_target = 0
#     self.slc_target = 0
#     self.v_cruise = 0
#     self.vtsc_target = 0

#     self.x_desired_trajectory = np.zeros(CONTROL_N)

#     self.update_frogpilot_params(params, params_memory)

#   def update(self, sm, mpc):
#     carState, controlsState, modelData = sm['carState'], sm['controlsState'], sm['modelV2']

#     enabled = controlsState.enabled

#     v_cruise_kph = min(controlsState.vCruise, V_CRUISE_MAX)
#     v_cruise = v_cruise_kph * CV.KPH_TO_MS
#     v_ego = carState.vEgo