#include "vehicle.h"

#include "can-halal.h"
#include "config.h"
#include "main.h"
#include "stm32h7xx.h"
#include "stm32h7xx_hal.h"
#include "stm32h7xx_hal_fdcan.h"
#include "stm32h7xx_hal_gpio.h"
#include "tx_api.h"
#include "ui.h"
#include "vehicle_state.h"
#include <stdbool.h>

#define CHECK_WATCHDOG(param, timeout) vehicle_state.watchdog_timeout.param = tick - vehicle_state.watchdog_times.param >= timeout

// CAN filter constants
#define CAN_ID_AMS_SLAVE_PANIC 0x009
#define CAN_ID_AMS_STATUS 0x00A
#define CAN_ID_AMS_ERROR 0x00C
#define CAN_ID_AMS_DCDC 0x313

#define CAN_ID_PWM_DUTYCYLE 0x0DC
#define CAN_ID_PWM_CONFIG 0x0DD
#define CAN_ID_FTCU_TIMINGS 0x102
#define CAN_ID_FTCU_DAMPER 0x103
#define CAN_ID_FTCU_WHEELSPEED 0x104
#define CAN_ID_FTCU_BRAKE_T 0x105
#define CAN_ID_FTCU_COOLING 0x107
#define CAN_ID_FTCU_PNEUMATIK 0x110
#define CAN_ID_FTCU_DRIVER 0x111
#define CAN_ID_AS_MISSION_FB 0x410
#define CAN_ID_STW_STATUS 0x412
#define CAN_ID_FTCU_PARAM_CONFIRMED 0x413
#define CAN_ID_FTCU_TELEMETRIE1 0x719
#define CAN_ID_FTCU_TELEMETRIE2 0x720

#define CAN_ID_PDU_RESPONSE 0x0C9
#define CAN_ID_PDU_CURRENT1 0x0CA
#define CAN_ID_PDU_CURRENT2 0x0CB
#define CAN_ID_PDU_CURRENT3 0x0CC
#define CAN_ID_PDU_CURRENT4 0x0CD

#define CAN_ID_SNF_100Hz_1 0x0D3 // only for watchdog
#define CAN_ID_SNR_100Hz 0x0D4   // only for watchdog

#define CAN_ID_DASHBOARD 0x420

#define CAN_ID_SSU 0x500

#define CAN_ID_SHUNT_CURRENT 0x521
#define CAN_ID_SHUNT_VOLTAGE1 0x522
#define CAN_ID_SHUNT_VOLTAGE2 0x523
#define CAN_ID_SHUNT_TEMPERATURE 0x525

#define CAN_ID_TTS_FL 0x701
#define CAN_ID_TTS_FR 0x702
#define CAN_ID_TTS_RL 0x703
#define CAN_ID_TTS_RR 0x704

#define CAN_ID_INVERTER_VELOCITY 0x776
#define CAN_ID_INVERTER_TORQUE_WANTED 0x777
#define CAN_ID_INVERTER_TEMPERATURE 0x778
#define CAN_ID_INVERTER_TORQUE_ACTUAL 0x779
#define CAN_ID_INVERTER_ERRORS_WARNINGS 0x780

// CAN sending constants
#define CAN_ID_MISSION_SELECTED 0x400
#define CAN_ID_STW_BUTTONS 0x401
#define CAN_ID_STW_PARAM_SET 0x402

#define NUM_LV_SOC_VOLTAGE_MAP 11
struct { // percent and voltage must be monotonically decreasing
  uint8_t percent;
  float voltage;
} LV_SOC_VOLTAGE_MAP[NUM_LV_SOC_VOLTAGE_MAP] = {
    {.percent = 100, .voltage = 27.20}, //
    {.percent = 90, .voltage = 26.80},  //
    {.percent = 80, .voltage = 26.56},  //
    {.percent = 70, .voltage = 26.40},  //
    {.percent = 60, .voltage = 26.16},  //
    {.percent = 50, .voltage = 26.08},  //
    {.percent = 40, .voltage = 26.00},  //
    {.percent = 30, .voltage = 25.76},  //
    {.percent = 20, .voltage = 25.60},  //
    {.percent = 10, .voltage = 24.00},  //
    {.percent = 0, .voltage = 20.00},   //
};

void vehicle_thread_entry(ULONG hfdcan_addr) {
  memset(&vehicle_state, 0, sizeof(vehicle_state));

  ftcan_init((void *)hfdcan_addr);
  ftcan_add_filter(CAN_ID_AMS_SLAVE_PANIC, 0x7FF);
  ftcan_add_filter(CAN_ID_AMS_STATUS, 0x7FF);
  ftcan_add_filter(CAN_ID_AMS_ERROR, 0x7FF);
  ftcan_add_filter(CAN_ID_AMS_DCDC, 0x7FF);
  ftcan_add_filter(CAN_ID_PWM_DUTYCYLE, 0x7FF);
  ftcan_add_filter(CAN_ID_PWM_CONFIG, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_TIMINGS, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_DAMPER, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_WHEELSPEED, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_BRAKE_T, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_COOLING, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_PNEUMATIK, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_DRIVER, 0x7FF);
  ftcan_add_filter(CAN_ID_AS_MISSION_FB, 0x7FF);
  ftcan_add_filter(CAN_ID_STW_STATUS, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_PARAM_CONFIRMED, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_TELEMETRIE1, 0x7FF);
  ftcan_add_filter(CAN_ID_FTCU_TELEMETRIE2, 0x7FF);
  ftcan_add_filter(CAN_ID_PDU_RESPONSE, 0x7FF);
  ftcan_add_filter(CAN_ID_PDU_CURRENT1, 0x7FF);
  ftcan_add_filter(CAN_ID_PDU_CURRENT2, 0x7FF);
  ftcan_add_filter(CAN_ID_PDU_CURRENT3, 0x7FF);
  ftcan_add_filter(CAN_ID_PDU_CURRENT4, 0x7FF);
  ftcan_add_filter(CAN_ID_SNF_100Hz_1, 0x7FF);
  ftcan_add_filter(CAN_ID_SNR_100Hz, 0x7FF);
  ftcan_add_filter(CAN_ID_DASHBOARD, 0x7FF);
  ftcan_add_filter(CAN_ID_SSU, 0x7FF);
  ftcan_add_filter(CAN_ID_SHUNT_CURRENT, 0x7FF);
  ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE1, 0x7FF);
  ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE2, 0x7FF);
  ftcan_add_filter(CAN_ID_SHUNT_TEMPERATURE, 0x7FF);
  ftcan_add_filter(CAN_ID_TTS_FL, 0x7FF);
  ftcan_add_filter(CAN_ID_TTS_FR, 0x7FF);
  ftcan_add_filter(CAN_ID_TTS_RL, 0x7FF);
  ftcan_add_filter(CAN_ID_TTS_RL, 0x7FF);
  ftcan_add_filter(CAN_ID_INVERTER_VELOCITY, 0x7FF);
  ftcan_add_filter(CAN_ID_INVERTER_TORQUE_WANTED, 0x7FF);
  ftcan_add_filter(CAN_ID_INVERTER_TEMPERATURE, 0x7FF);
  ftcan_add_filter(CAN_ID_INVERTER_TORQUE_ACTUAL, 0x7FF);
  ftcan_add_filter(CAN_ID_INVERTER_ERRORS_WARNINGS, 0x7FF);

  while (1) {
    tx_thread_sleep(10);

#ifdef DEMO_MODE
    double tick = HAL_GetTick();

    vehicle_state.speed = (sin(tick * 0.001) * 8 + 10 + cos(tick * 0.003) * 8) * 4;
    if (vehicle_state.speed <= 0) {
      vehicle_state.speed = 0;
    }
    tx_event_flags_set(&gui_update_events, GUI_UPDATE_VEHICLE_STATE, TX_OR);
#endif

    uint32_t tick = HAL_GetTick();
    CHECK_WATCHDOG(snf, 300);
    CHECK_WATCHDOG(db, 600);
    CHECK_WATCHDOG(ftcu, 300);
    CHECK_WATCHDOG(pdu, 300);
    CHECK_WATCHDOG(snr, 300);
    CHECK_WATCHDOG(ams, 300);
    CHECK_WATCHDOG(shunt, 300);
    tx_event_flags_set(&gui_update_events, GUI_UPDATE_VEHICLE_STATE, TX_OR);
  }
}

void vehicle_select_mission(Mission mission) {
  uint8_t mission_int = mission;
  ftcan_transmit(CAN_ID_MISSION_SELECTED, &mission_int, 1);
}

void vehicle_broadcast_param(ParamType param, int32_t value) {
  uint8_t data[5];
  uint8_t *ptr = data;
  ptr = ftcan_marshal_unsigned(ptr, param, 1);
  ptr = ftcan_marshal_signed(ptr, value, 4);
  ftcan_transmit(CAN_ID_STW_PARAM_SET, data, 5);
}

void vehicle_broadcast_buttons(GPIO_PinState *button_states) {
  uint8_t data = (button_states[DRS_BUTTON_IDX] << 0) | (button_states[6] << 1) | (button_states[7] << 2);
  ftcan_transmit(CAN_ID_STW_BUTTONS, &data, 1);
}

float calculate_lv_soc(float lv_voltage) {
  // check if lv_voltage is in range of voltage map
  if (lv_voltage >= LV_SOC_VOLTAGE_MAP[0].voltage) {
    return LV_SOC_VOLTAGE_MAP[0].percent;
  }
  if (lv_voltage <= LV_SOC_VOLTAGE_MAP[NUM_LV_SOC_VOLTAGE_MAP - 1].voltage) {
    return LV_SOC_VOLTAGE_MAP[NUM_LV_SOC_VOLTAGE_MAP - 1].percent;
  }

  size_t index_upper_volt = 0;
  size_t index_lower_volt = 1;

  while (index_lower_volt < NUM_LV_SOC_VOLTAGE_MAP && !(lv_voltage <= LV_SOC_VOLTAGE_MAP[index_upper_volt].voltage &&
                                                        lv_voltage >= LV_SOC_VOLTAGE_MAP[index_lower_volt].voltage)) {
    index_upper_volt++;
    index_lower_volt++;
  }

  // calculate f value between 0 and 1 for linear interpolation
  float f = (lv_voltage - LV_SOC_VOLTAGE_MAP[index_lower_volt].voltage) /
            (LV_SOC_VOLTAGE_MAP[index_upper_volt].voltage - LV_SOC_VOLTAGE_MAP[index_lower_volt].voltage);

  return LV_SOC_VOLTAGE_MAP[index_lower_volt].percent * (1.0 - f) + LV_SOC_VOLTAGE_MAP[index_upper_volt].percent * f;
}

void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
  uint32_t tick = HAL_GetTick();
  const uint8_t *ptr;
  switch (id) {
    case CAN_ID_AMS_SLAVE_PANIC:
      vehicle_state.watchdog_times.ams = tick;
      vehicle_state.last_ams_slave_panic.id = data[0];
      vehicle_state.last_ams_slave_panic.kind = data[1];
      ptr = &data[2];
      vehicle_state.last_ams_slave_panic.arg = ftcan_unmarshal_unsigned(&ptr, 4);
      break;
    case CAN_ID_AMS_STATUS:
      vehicle_state.watchdog_times.ams = tick;
      vehicle_state.ts_state = data[0] & 0x7F;
      vehicle_state.sdc_closed = (data[0] & 0x80) >> 7;
      vehicle_state.soc_ts = data[1];
      ptr = &data[2];
      vehicle_state.min_cell_volt = ftcan_unmarshal_unsigned(&ptr, 2) * 1e-3;
      vehicle_state.max_cell_temp = ftcan_unmarshal_signed(&ptr, 2) * 1e-2;
      // vehicle_state.imd_state = data[6] & 0x7F;  // does not provide useful
      // data
      vehicle_state.imd_ok = (data[6] & 0x80) >> 7;
      vehicle_state.tsal_green = data[7] & 0x01;
      vehicle_state.imd_error = (data[7] & 0x02) >> 1;
      vehicle_state.ams_error = (data[7] & 0x04) >> 2;
      break;
    case CAN_ID_AMS_ERROR:
      vehicle_state.watchdog_times.ams = tick;
      vehicle_state.last_ams_error.kind = data[0];
      vehicle_state.last_ams_error.arg = data[1];
      break;
    case CAN_ID_AMS_DCDC:
      vehicle_state.watchdog_times.ams = tick;
      ptr = &data[0];
      vehicle_state.dcdc_temp = ftcan_unmarshal_signed(&ptr, 2) * 0.01f;
      vehicle_state.dcdc_current = ftcan_unmarshal_signed(&ptr, 2) * 0.001f;
      break;
    case CAN_ID_PWM_DUTYCYLE:
      vehicle_state.watchdog_times.ftcu = tick;
      // TODO
      break;
    case CAN_ID_PWM_CONFIG:
      vehicle_state.watchdog_times.ftcu = tick;
      // TODO
      break;
    case CAN_ID_FTCU_TIMINGS:
      vehicle_state.watchdog_times.ftcu = tick;
      vehicle_state.lap_time_best = (data[0] | (data[1] << 8)) * 0.01f;
      vehicle_state.lap_time_last = (data[2] | (data[3] << 8)) * 0.01f;
      vehicle_state.sector_time_best = (data[4] | (data[5] << 8)) * 0.01f;
      vehicle_state.sector_time_last = (data[6] | (data[7] << 8)) * 0.01f;
      break;
    case CAN_ID_FTCU_DAMPER:
      vehicle_state.watchdog_times.ftcu = tick;
      // TODO
      break;
    case CAN_ID_FTCU_WHEELSPEED:
      vehicle_state.watchdog_times.ftcu = tick;
      vehicle_state.wheel_speeds.wss_fl = (data[0] | ((data[1] & 0x0F) << 8)) * 0.05;
      vehicle_state.wheel_speeds.wss_fr = (((data[1] >> 4) & 0x0F) | (data[2] << 4)) * 0.05;
      vehicle_state.wheel_speeds.wss_rl = (data[3] | ((data[4] & 0x0F) << 8)) * 0.05;
      vehicle_state.wheel_speeds.wss_rr = (((data[4] >> 4) & 0x0F) | (data[5] << 4)) * 0.05;
      vehicle_state.distance_session = data[6] | (data[7] << 8);
      break;
    case CAN_ID_FTCU_BRAKE_T:
      vehicle_state.watchdog_times.ftcu = tick;
      vehicle_state.temps.brake_fl = (data[0] | (data[1] << 8)) * 0.01f;
      vehicle_state.temps.brake_fr = (data[2] | (data[3] << 8)) * 0.01f;
      vehicle_state.temps.brake_rl = (data[4] | (data[5] << 8)) * 0.01f;
      vehicle_state.temps.brake_rr = (data[6] | (data[7] << 8)) * 0.01f;
      break;
    case CAN_ID_FTCU_COOLING:
      vehicle_state.watchdog_times.ftcu = tick;
      // TODO
      break;
    case CAN_ID_FTCU_PNEUMATIK:
      vehicle_state.watchdog_times.ftcu = tick;
      vehicle_state.tank_pressure_1 = data[0] | ((data[1] & 0x0F) << 8);
      vehicle_state.tank_pressure_2 = data[2] | ((data[3] & 0x0F) << 8);
      break;
    case CAN_ID_FTCU_DRIVER:
      vehicle_state.watchdog_times.ftcu = tick;
      vehicle_state.apps_percent = data[0];
      vehicle_state.brake_pressure_f = (data[1] | ((data[2] & 0x0F) << 8)) * 0.1f;
      vehicle_state.brake_pressure_r = (((data[2] >> 4) & 0x0F) | (data[3] << 4)) * 0.1f;
      vehicle_state.steering_angle = data[4];
      vehicle_state.speed = data[5] * 0.2f;
      vehicle_state.lap_count = data[6];
      vehicle_state.sector_count = data[7];
      break;
    case CAN_ID_AS_MISSION_FB:
      vehicle_state.watchdog_times.ftcu = tick;
      vehicle_state.active_mission = data[0] & 0b111;
      break;
    case CAN_ID_STW_STATUS:
      vehicle_state.watchdog_times.ftcu = tick;
      // vehicle_state.lap_count = data[0] & 0b111111;
      vehicle_state.errors.err_pdu = (data[0] >> 6) & 0b1;
      vehicle_state.errors.err_res = (data[0] >> 7) & 0b1;
      vehicle_state.r2d_progress = (data[1] >> 0) & 0b1111;
      vehicle_state.as_state = (data[1] >> 4) & 0b111;
      vehicle_state.errors.err_as = (data[1] >> 7) & 0b1;
      vehicle_state.errors.err_apps_plausible = (data[2] >> 0) & 0b1;
      vehicle_state.errors.err_soft_bspd = (data[2] >> 1) & 0b1;
      vehicle_state.errors.err_scs = (data[2] >> 2) & 0b1;
      vehicle_state.errors.err_con_mon = (data[2] >> 3) & 0b1;
      vehicle_state.errors.err_initial_checkup = (data[2] >> 4) & 0b1;
      vehicle_state.errors.err_inv_2 = (data[2] >> 5) & 0b1;
      vehicle_state.errors.err_inv_1 = (data[2] >> 6) & 0b1;
      vehicle_state.errors.err_ams = (data[2] >> 7) & 0b1;
      vehicle_state.errors.err_sdc = (data[3] >> 0) & 0b1;
      vehicle_state.sdc_status = (data[3] >> 1) & 0b1111;
      vehicle_state.inv_2_ready = (data[3] >> 5) & 0b1;
      vehicle_state.inv_1_ready = (data[3] >> 6) & 0b1;
      vehicle_state.energy_per_lap = data[4] | (data[5] << 8);
      vehicle_state.initial_checkup_state = data[6];
      break;
    case CAN_ID_FTCU_PARAM_CONFIRMED:
      vehicle_state.watchdog_times.ftcu = tick;
      vehicle_state.last_param_confirmed = data[0];
      tx_event_flags_set(&gui_update_events, GUI_UPDATE_PARAM_CONFIRMED, TX_OR);
      break;
    case CAN_ID_FTCU_TELEMETRIE1:
      vehicle_state.watchdog_times.ftcu = tick;
      // TODO
      break;
    case CAN_ID_FTCU_TELEMETRIE2:
      vehicle_state.watchdog_times.ftcu = tick;
      // TODO
      break;
    case CAN_ID_PDU_RESPONSE:
      vehicle_state.watchdog_times.pdu = tick;
      // TODO
      break;
    case CAN_ID_PDU_CURRENT1:
      vehicle_state.watchdog_times.pdu = tick;
      // TODO
      break;
    case CAN_ID_PDU_CURRENT2:
      vehicle_state.watchdog_times.pdu = tick;
      // TODO
      break;
    case CAN_ID_PDU_CURRENT3:
      vehicle_state.watchdog_times.pdu = tick;
      // TODO
      break;
    case CAN_ID_PDU_CURRENT4:
      vehicle_state.watchdog_times.pdu = tick;
      ptr = &data[2];
      vehicle_state.lv_voltage = ftcan_unmarshal_signed(&ptr, 2) * 0.001f;
      vehicle_state.lv_soc = calculate_lv_soc(vehicle_state.lv_voltage);
      break;
    case CAN_ID_SNF_100Hz_1:
      vehicle_state.watchdog_times.snf = tick;
      break;
    case CAN_ID_SNR_100Hz:
      vehicle_state.watchdog_times.snr = tick;
      break;
    case CAN_ID_DASHBOARD:
      vehicle_state.watchdog_times.db = tick;
      // TODO
      break;
    case CAN_ID_SSU:
      // TODO
      break;
    case CAN_ID_SHUNT_CURRENT:
      vehicle_state.watchdog_times.shunt = tick;
      // The first two bytes of shunt result messages are metadata
      ptr = &data[2];
      vehicle_state.ts_current = ftcan_unmarshal_signed(&ptr, 4) * 0.001f;
      break;
    case CAN_ID_SHUNT_VOLTAGE1:
      vehicle_state.watchdog_times.shunt = tick;
      ptr = &data[2];
      vehicle_state.ts_voltage_bat = ftcan_unmarshal_signed(&ptr, 4) * -0.001f;
      break;
    case CAN_ID_SHUNT_VOLTAGE2:
      vehicle_state.watchdog_times.shunt = tick;
      ptr = &data[2];
      vehicle_state.ts_voltage_veh = vehicle_state.ts_voltage_bat - (ftcan_unmarshal_signed(&ptr, 4) * -0.001f);
      break;
    case CAN_ID_SHUNT_TEMPERATURE:
      vehicle_state.watchdog_times.shunt = tick;
      ptr = &data[2];
      vehicle_state.shunt_temperature = ftcan_unmarshal_signed(&ptr, 4) * 0.1f;
      break;
    case CAN_ID_TTS_FL:
      // TODO
      break;
    case CAN_ID_TTS_FR:
      // TODO
      break;
    case CAN_ID_TTS_RL:
      // TODO
      break;
    case CAN_ID_TTS_RR:
      // TODO
      break;
    case CAN_ID_INVERTER_VELOCITY:
      vehicle_state.inv_velocity_1 = (data[0] | (data[1] << 8) | (data[2] << 16) | (data[3] << 24)) * 0.001f;
      vehicle_state.inv_velocity_2 = (data[4] | (data[5] << 8) | (data[6] << 16) | (data[7] << 24)) * 0.001f;
      break;
    case CAN_ID_INVERTER_TORQUE_WANTED:
      vehicle_state.inv_torque_demanded_1 = data[0] | (data[1] << 8);
      vehicle_state.inv_torque_demanded_2 = data[2] | (data[3] << 8);
      vehicle_state.inv_torque_desired_1 = data[4] | (data[5] << 8);
      vehicle_state.inv_torque_desired_2 = data[6] | (data[7] << 8);
      break;
    case CAN_ID_INVERTER_TEMPERATURE:
      vehicle_state.temps.inv_1 = data[0] | (data[1] << 8);
      vehicle_state.temps.mot_1 = data[2] | (data[3] << 8);
      vehicle_state.temps.inv_2 = data[4] | (data[5] << 8);
      vehicle_state.temps.mot_2 = data[6] | (data[7] << 8);
      break;
    case CAN_ID_INVERTER_TORQUE_ACTUAL:
      vehicle_state.inv_control_word_1 = data[0] | (data[1] << 8);
      vehicle_state.inv_torque_actual_1 = data[2] | (data[3] << 8);
      vehicle_state.inv_control_word_2 = data[4] | (data[5] << 8);
      vehicle_state.inv_torque_actual_2 = data[6] | (data[7] << 8);
      break;
    case CAN_ID_INVERTER_ERRORS_WARNINGS:
      vehicle_state.inv_errors_1 = data[0] | (data[1] << 8);
      vehicle_state.inv_warnings_1 = data[2] | (data[3] << 8);
      vehicle_state.inv_errors_2 = data[4] | (data[5] << 8);
      vehicle_state.inv_warnings_2 = data[6] | (data[7] << 8);
      break;
  }
  tx_event_flags_set(&gui_update_events, GUI_UPDATE_VEHICLE_STATE, TX_OR);
}