#ifndef CAR_CONTROL_H
#define CAR_CONTROL_H

#include "kx_11_can_frame/asdm_sim_canfd_frame.h"
#include <thread>


enum class StsMach
{
  kStandby = 0,
  kApaStartup = 1,
  kApaShutDown = 2,
  kApaActive = 3,
  kFailure = 4,
  kHwaPreActive = 5,
  kHwaActive = 6,
  kHwaShutDown = 7
};

enum class ChassisErrCode
{
  kNoFailure = 0,
  kSteeringFailure = 1,
  kBrakeFailure = 2, 
  kPropulsionFailure = 3,
  kRbuFailure = 4,
  kCommunicationFailure = 5,
  kVDSWInternalFailure = 6,
  kRWSFailure = 7,
  kSteerWheelServiousErr = 8,
  kHandshakeTimeout = 9
};

class CarControl
{
public:
  CarControl();
  ~CarControl();


  void set_lat_lgt_ctrl_req(bool req);
  void set_target_gear(GearPrkgAssistReq tar);
  bool is_lat_lgt_ctrl_active();
  void set_target_pinion_ag_in_deg(float32_t ang_req);
  void set_target_acc_mps2(float32_t tar_acc);
  void set_turn_light_status(TurnLightIndicReq req);
  void set_ctrl_mode_switch_spd(float32_t spd_kmh);
  void set_debug_log_para(bool enable, uint16_t print_cycle_ms);
  ChassisErrCode get_chassis_err_state();
  StsMach get_chassis_statemachine_sts();
  float32_t get_current_steer_ang_in_deg();
  float32_t get_current_vehicle_spd_in_ms();
  GearPrkgAssistReq get_setted_tar_gear();
  GearLevelIndicate get_cur_disp_gear();
private:
  void sm_task_20ms();
  void sm_task_thread();
  bool is_chassis_system_failure();
  bool is_hwa_switched();
  void apa_active_ctrl();
  void hwa_active_ctrl();
  bool is_target_move_gear_changed();
  bool is_vehicle_standstill_sts();
  bool is_hwa_active();
  void chassis_handshake_off();
  void apa_startup_action();
  void hwa_pre_active();
  bool is_car_vin_matched();
private:
  std::unique_ptr<ASDM_Sim_Process>  asdm_obj_ptr_{nullptr};
  StsMach sm_{StsMach::kStandby};
  std::unique_ptr<std::thread> thread_sm_task_{nullptr};
  bool is_chassis_handshake_timeout{false};

  bool is_lat_lgt_ctrl_requested_{false};
  GearPrkgAssistReq tar_gear_{GearPrkgAssistReq::kNoRequest};
  float32_t tar_pinion_ang_in_deg_{0};
  float32_t tar_acc_meter_per_second2_{0};

  uint16_t time_startup_ = 0;
  uint16_t timeout_apa_startup = 0;
  uint16_t timeout_apa_shutdown = 0;
  uint16_t timeout_hwa_startup = 0;
  uint16_t timeout_hwa_shutdown = 0;
  uint32_t time_vin_check = 0;
  const uint16_t kMaxStartupDlyTime = (3000 / 20); // uinit:ms
  const uint16_t kMaxApaStartupTime = (3000 / 20); // uinit:ms
  const uint16_t kMaxApaShutdownTime = (10000 / 20); // uinit:ms
  const uint16_t kMaxHwaStartupTime = (5000 / 20); // uinit:ms
  const uint16_t kMaxHwaShutdownTime = (10000 / 20); // uinit:ms
  const uint32_t kMaxVinCheckTime = (5 * 60 * 1000 / 20); // uinit:ms,  5minites
  float32_t kSpeedForCtrlModSwitch = CONVERT_KPH_2_MPS(6);   // the APA longitude speed uppper limit is 10km/h
  const float32_t kSteerDiffMax = 90;
  const float32_t kSteerDiffMin = -90;
  const float32_t kSteerMaxAng = 450;
  const float32_t kSteerMinAng = -450;
  const float32_t kApaShutDownDecel = -1.28; //uinit: m/s^2, apa max deceleration.
  const float32_t kHwaShutDownDecel = -5; //uinit: m/s^2, hwa shutdown deceleration value.

  const float32_t kApaAccelerationMax = 1.27; //uinit: m/s^2
  const float32_t kApaAccelerationMin = -1.27; //uinit: m/s^2
  const float32_t kHwaAccelerationMax = 11; //uinit: m/s^2
  const float32_t kHwaAccelerationMin = -11; //uinit: m/s^2

  // debug parameter:
  bool debug_enable_{false};
  uint16_t print_period_ms_{20};
};

#endif