text: import os
  import math
  
  import cereal.messaging as messaging
  from common.numpy_fast import clip, interp
  from selfdrive.swaglog import cloudlog
  from common.realtime import sec_since_boot
  from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
  from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
  from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
  from selfdrive.controls.lib.dynamic_follow import DynamicFollow
  
  STOPPING_DISTANCE = 1.5
  
  LOG_MPC = os.environ.get('LOG_MPC', False)
  
  
  class LongitudinalMpc():
    def __init__(self, mpc_id):
      self.mpc_id = mpc_id
      self.dynamic_follow = DynamicFollow(mpc_id)
      self.setup_mpc()
      self.v_mpc = 0.0
      self.v_mpc_future = 0.0
      self.a_mpc = 0.0
      self.v_cruise = 0.0
      self.prev_lead_status = False
      self.prev_lead_x = 0.0
      self.new_lead = False
      self.v_rel = 0
  
      self.last_cloudlog_t = 0.0
      self.n_its = 0
      self.duration = 0
      self.cruise_gap = 0	
  
    def publish(self, pm):
      if LOG_MPC:
        qp_iterations = max(0, self.n_its)
        dat = messaging.new_message('liveLongitudinalMpc')
        dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
        dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
        dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
        dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
        dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
        dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
        dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
        dat.liveLongitudinalMpc.qpIterations = qp_iterations
        dat.liveLongitudinalMpc.mpcId = self.mpc_id
        dat.liveLongitudinalMpc.calculationTime = self.duration
        pm.send('liveLongitudinalMpc', dat)
  
    def setup_mpc(self):
      ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
      self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
                       MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
  
      self.mpc_solution = ffi.new("log_t *")
      self.cur_state = ffi.new("state_t *")
      self.cur_state[0].v_ego = 0
      self.cur_state[0].a_ego = 0
      self.a_lead_tau = _LEAD_ACCEL_TAU
  
    def set_cur_state(self, v, a):
      self.cur_state[0].v_ego = v
      self.cur_state[0].a_ego = a
  
    def update(self, CS, lead):
      v_ego = CS.vEgo
      v_lead = 0
  
      # Setup current mpc state
      self.cur_state[0].x_ego = 0.0
  
      if lead is not None and lead.status:
        x_lead = max(0, lead.dRel - STOPPING_DISTANCE)
        v_lead = max(0.0, lead.vLead)
        a_lead = lead.aLeadK
  
        if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
          v_lead = 0.0
          a_lead = 0.0
  
        self.a_lead_tau = lead.aLeadTau
        self.new_lead = False
        if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
          self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
          self.new_lead = True
  
        self.dynamic_follow.update_lead(v_lead, a_lead, x_lead, lead.status, self.new_lead)
        self.prev_lead_status = True
        self.prev_lead_x = x_lead
        self.cur_state[0].x_l = x_lead
        self.cur_state[0].v_l = v_lead
      else:
        self.prev_lead_status = False
        # Fake a fast lead car, so mpc keeps running
        self.cur_state[0].x_l = 50.0
        self.cur_state[0].v_l = v_ego + 10.0
        a_lead = 0.0
        v_lead = 0.0
        self.a_lead_tau = _LEAD_ACCEL_TAU
      # Calculate conditions
      self.v_rel = v_lead - v_ego   # calculate relative velocity vs lead car
  
      # Calculate mpc
      t = sec_since_boot()
      cruise_gap = int(clip(CS.cruiseGap, 1., 3.))
      baseTR = interp(float(cruise_gap), [1., 2., 3.], [1.0, 1.3, 1.8])
      if v_ego <= 20.0:
        if cruise_gap == 1:
          TR = self.dynamic_follow.update(CS, self.libmpc)
        else:
          TR = interp(-self.v_rel, [-0.1, 2.0 + cruise_gap*0.25], [baseTR, 2.1])
      else:
        TR = interp(-self.v_rel, [0, 3.5], [baseTR, 1.8])
        
      if self.cruise_gap != cruise_gap:
        self.cruise_gap = cruise_gap
        self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
  
      self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR)
      self.duration = int((sec_since_boot() - t) * 1e9)
  
      # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
      self.v_mpc = self.mpc_solution[0].v_ego[1]
      self.a_mpc = self.mpc_solution[0].a_ego[1]
      self.v_mpc_future = self.mpc_solution[0].v_ego[10]
  
      # Reset if NaN or goes through lead car
      crashing = any(lead - ego < -50 for (lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego))
      nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
      backwards = min(self.mpc_solution[0].v_ego) < -0.01
  
      if ((backwards or crashing) and self.prev_lead_status) or nans:
        if t > self.last_cloudlog_t + 5.0:
          self.last_cloudlog_t = t
          cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
                            self.mpc_id, backwards, crashing, nans))
  
        self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
                         MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
        self.cur_state[0].v_ego = v_ego
        self.cur_state[0].a_ego = 0.0
        self.v_mpc = v_ego
        self.a_mpc = CS.aEgo
        self.prev_lead_status = False
  