/*
 *  state_machine.h
 *
 *  Created on: 07.07.2024
 *      Author: Hamza
 */
 
#include "state_machine.h"
#include "PWM_control.h"
#include "eeprom.h"
#include "main.h"
#include <stdint.h>

// Time to wait after reaching 95% of battery voltage before exiting precharge
// Set this to 1000 in scruti to demonstrate the voltage on the multimeter
#define PRECHARGE_DURATION 5000 // ms
// Time to wait for discharge
#define DISCHARGE_DURATION 5000 // ms
// Time to wait for charger voltage before going to TS_ERROR
#define MAX_CHARGING_CHECK_DURATION 2000 // ms
// Time to wait between closing relays
#define RELAY_CLOSE_WAIT 10 // ms
// Max time to wait for CAN messages. If we reach it then we emergency shutdown.
#define CAN_TIMEOUT 1000
// waiting time between to eeprom writes
#define EEPROM_WRITE_FREQ_INACTIVE  10000   
#define EEPROM_WRITE_FREQ_ACTIVE    1000

StateHandle state;
int32_t RELAY_BAT_SIDE_VOLTAGE;
int32_t RELAY_ESC_SIDE_VOLTAGE;
int32_t CURRENT_MEASUREMENT;
bool CURRENT_MEASUREMENT_ON;
float base_offset = 0;

uint32_t precharge_timer;
uint32_t discharge_timer;
uint32_t CAN_timer;
uint32_t EEPROM_timer;

uint32_t powerground_softstart_timer;
uint32_t powerground_calibration_timer;
uint8_t powerground_calibration_stage;

uint8_t current_powerground_status;
uint8_t target_powerground_status;

static uint32_t timestamp;

void sm_init(){
  state.current_state = STATE_INACTIVE;
  state.target_state = STATE_INACTIVE;
  state.error_source = 0;
  precharge_timer = discharge_timer = powerground_calibration_timer;
  CAN_timer = HAL_GetTick() + 5000;
}

#warning change amsState here
void sm_update(){
  sm_check_errors();
  sm_precharge_discharge_manager();
  sm_calibrate_powerground();
  sm_powerground_manager();
  if (CAN_timer < HAL_GetTick())
    state.current_state = state.target_state = STATE_ERROR;

  if (state.current_state == STATE_INACTIVE || state.current_state == STATE_ERROR){
    CURRENT_MEASUREMENT = (module.auxVoltages[0] > 2494) ? (module.auxVoltages[0] - (2494.0)) * (300.0) : 0;
  } else {
    CURRENT_MEASUREMENT = (module.auxVoltages[0] > 2477) ? (module.auxVoltages[0] - (2477.0)) * (4600.0) : 0;
  }
  CURRENT_MEASUREMENT_ON = (module.auxVoltages[1] > 2400);
  RELAY_ESC_SIDE_VOLTAGE = module.auxVoltages[2] * 15.98;
  RELAY_BAT_SIDE_VOLTAGE = module.auxVoltages[3] * 15.98;     // the calculation says the factor is 11. 11.711 yields the better result
 

  switch (state.current_state) {
    case STATE_INACTIVE:
      state.current_state = sm_update_inactive();             // monitor only
      break;
    case STATE_PRECHARGE:
      state.current_state = sm_update_precharge();            // set PRECHARGE and turn on cooling at 50% or such
      break;
    case STATE_READY:
      state.current_state = sm_update_ready();                // keep cooling at 50%, get ready to turn on powerground
      break;
    case STATE_ACTIVE:
      state.current_state = sm_update_active();               // set PRECHARGE and turn on cooling at 50% or such
      break;
    case STATE_DISCHARGE:
      state.current_state = sm_update_discharge();            // open the main relay, keep PRECHARGE closed
      break;
    case STATE_CHARGING_PRECHARGE:
      state.current_state = sm_update_charging_precharge();   
      break;
    case STATE_CHARGING:
      state.current_state = sm_update_charging();             // monitor and turn on cooling if needed. 
      break;
    case STATE_ERROR:
      state.current_state = sm_update_error();                // enter the correct ERROR state 
      break;
  }

  sm_set_relay_positions(state.current_state);
  state.target_state = state.current_state;
}

void sm_handle_ams_in(const uint8_t *data){  
  CAN_timer = HAL_GetTick() + CAN_TIMEOUT;
  switch (data[0]) {
    case 0x00:
      if (state.current_state != STATE_INACTIVE){
        state.target_state = STATE_DISCHARGE;
        PWM_powerground_control(255);
      }
      break;
    case 0x01:
      if (state.target_state == STATE_INACTIVE || state.target_state == STATE_DISCHARGE){
        state.target_state = STATE_PRECHARGE;
        PWM_powerground_control(0);
      } else if (state.target_state == STATE_ACTIVE){
        state.target_state = STATE_READY;
        PWM_powerground_control(0);
      }
      break;
    case 0x02:
      if (state.current_state == STATE_READY || state.current_state == STATE_ACTIVE){
        target_powerground_status = data[1];
        state.target_state = STATE_ACTIVE;        // READY -> ACTIVE
      }
      break;
    case 0xF0:
      if (state.current_state == STATE_INACTIVE){
        state.target_state = STATE_CHARGING_PRECHARGE;
      }
      break;
    #warning implement this
    case 0xF1:                                    // EEPROM
      break;
    case 0xFF:                                    // EMERGENCY SHUTDOWN
      state.current_state = STATE_DISCHARGE;
      state.target_state = STATE_ERROR;
      break;
  }
}

void sm_precharge_discharge_manager(){

  if (state.current_state != STATE_PRECHARGE && state.target_state == STATE_PRECHARGE){
    precharge_timer = HAL_GetTick() + PRECHARGE_DURATION;
  } else if (state.current_state == STATE_PRECHARGE && precharge_timer < HAL_GetTick()) {
    state.target_state = STATE_READY;
    precharge_timer = 0;
  }

  if (state.current_state != STATE_CHARGING_PRECHARGE && state.target_state == STATE_CHARGING_PRECHARGE){
    precharge_timer = HAL_GetTick() + PRECHARGE_DURATION;
  } else if (state.current_state == STATE_CHARGING_PRECHARGE && precharge_timer < HAL_GetTick()) {
    state.target_state = STATE_CHARGING;
    precharge_timer = 0;
  }

  if (state.current_state != STATE_DISCHARGE && state.target_state == STATE_DISCHARGE){
    discharge_timer = HAL_GetTick() + DISCHARGE_DURATION;
  } else if (state.current_state == STATE_DISCHARGE && discharge_timer < HAL_GetTick()) {
    state.target_state = STATE_INACTIVE;
    discharge_timer = 0;
  }

}

void sm_powerground_manager(){
  if (current_powerground_status == target_powerground_status)
    return;
  if ( current_powerground_status > 100 || target_powerground_status > 100){ //something went wrong
    PWM_powerground_control(255);
    current_powerground_status = target_powerground_status= 0;
    return;
  }

  if (powerground_softstart_timer < HAL_GetTick()){
    if (current_powerground_status < target_powerground_status ){
      current_powerground_status++;
      PWM_powerground_softcontrol();
      powerground_softstart_timer = HAL_GetTick() + 10;
    } else if (current_powerground_status > target_powerground_status) {
      current_powerground_status--;
      PWM_powerground_softcontrol();
      powerground_softstart_timer = HAL_GetTick() + 10;
    }
  }
}

void sm_calibrate_powerground(){
  if (powerground_calibration_stage != 4 && state.current_state == STATE_PRECHARGE){
    switch (powerground_calibration_stage) {
      case 0:
        powerground_calibration_timer = HAL_GetTick() + 0;
        powerground_calibration_stage = 1;
        return;
      case 1:
        if (powerground_calibration_timer < HAL_GetTick()){
          powerground_calibration_timer = HAL_GetTick() + 2000;
          powerground_calibration_stage = 2;
          PWM_powerground_control(100);
        }
        return;
      case 2:
        if (powerground_calibration_timer < HAL_GetTick()){
          powerground_calibration_timer = HAL_GetTick() + 1000;
          powerground_calibration_stage = 3;
          PWM_powerground_control(0);
        }
        return;
      case 3:
        if (powerground_calibration_timer < HAL_GetTick()){
          powerground_calibration_stage = 4;
        }
        return;
    }
  }
}

void sm_program_powerground(){
  powerground_calibration_timer = HAL_GetTick() + 5000;
  while (powerground_calibration_timer > HAL_GetTick()){}
  state.current_state = STATE_PRECHARGE;
  sm_update();  

  powerground_calibration_timer = HAL_GetTick() + PRECHARGE_DURATION + 1000;
  PWM_powerground_control(100);
  while (powerground_calibration_timer > HAL_GetTick()){}
  sm_update();
  while (1) {
    
  }
}

void sm_balancing(){
  for (int i = 0; i < 13; i++) {
    amsConfigBalancing((1 << i), 0xF);
  }
  amsStartBalancing(0);
}

void sm_eeprom_write_status(){
  if (EEPROM_timer < HAL_GetTick()){
    eeprom_write_status();
    EEPROM_timer = HAL_GetTick() + EEPROM_WRITE_FREQ_INACTIVE;
  }
}


#warning TODO: add error checking for everything here
void sm_check_errors(){  
  state.error_type.temperature_error = (error_data.error_sources & (1 << 0) || error_data.error_sources & (1 << 1) || error_data.error_sources & (1 << 4)) ? 1 : 0;
  state.error_type.voltage_error = (error_data.error_sources & (1 << 2)|| error_data.error_sources & (1 << 3)|| error_data.error_sources & (1 << 5) || RELAY_BAT_SIDE_VOLTAGE < 30000) ? 1 : 0;
  state.error_type.bms_timeout = (error_data.error_sources & (1 << 7)) ? 1 : 0;
  state.error_type.bms_fault = (error_data.error_sources & (1 << 8) || error_data.error_sources & (1 << 10) || error_data.error_sources & (1 << 9)) ? 1 : 0;
  //SEK_EEPROM_ERR: state.error_type.eeprom_error = 1;
  //state.error_type.current_error = (powerground_status > 10 && CURRENT_MEASUREMENT < 500) ? 1 : 0;
  state.error_type.current_sensor_missing = (!CURRENT_MEASUREMENT_ON) ? 1 : 0;
  state.error_type.voltage_missing = (RELAY_BAT_SIDE_VOLTAGE < 1000) ? 1 : 0;
  

  if (  state.error_type.current_error == 1 || state.error_type.current_sensor_missing == 1 || //state.error_type.eeprom_error == 1 ||
        state.error_type.state_transition_fail == 1 || state.error_type.temperature_error == 1 || state.error_type.voltage_error == 1 ||
        state.error_type.voltage_missing == 1 || state.error_type.bms_fault == 1 || state.error_type.bms_timeout == 1){
    if (state.current_state != STATE_INACTIVE && state.current_state != STATE_ERROR)
      state.current_state = STATE_DISCHARGE;
    state.target_state = STATE_ERROR;
    PWM_powerground_control(255);
  } else if (state.current_state == STATE_ERROR){
      state.target_state = STATE_INACTIVE;
  }
  sm_set_error_source();
}  

void sm_set_error_source(){
  state.error_source = 0;
  state.error_source |= (state.error_type.bms_timeout << 0);
  state.error_source |= (state.error_type.bms_fault << 1);
  state.error_source |= (state.error_type.temperature_error << 2);
  state.error_source |= (state.error_type.current_error << 3);
  
  state.error_source |= (state.error_type.current_sensor_missing << 4);
  state.error_source |= (state.error_type.voltage_error << 5);
  state.error_source |= (state.error_type.voltage_missing << 6);
  state.error_source |= (state.error_type.state_transition_fail << 7);
}

State sm_update_inactive(){
  switch (state.target_state) {
    case STATE_PRECHARGE:
      return STATE_PRECHARGE;
    case STATE_CHARGING_PRECHARGE:
      return STATE_CHARGING_PRECHARGE;
    case STATE_ERROR:
      return STATE_ERROR;
    default: 
      return STATE_INACTIVE;
  }
}

State sm_update_precharge(){
  switch (state.target_state) {
    case STATE_INACTIVE:          // if CAN Signal 0000 0000 then immidiete shutdown
      return STATE_DISCHARGE;
    case STATE_READY:
      return STATE_READY;
    default:
      return STATE_PRECHARGE;
  }
}

State sm_update_ready(){
  switch (state.target_state) {
    case STATE_ACTIVE:            // if CAN Signal 1100 0000 then turn on powerground
      return STATE_ACTIVE;
    case STATE_DISCHARGE:         // if CAN Signal 0000 0000 then shutdown
      return STATE_DISCHARGE;
    default:
      return STATE_READY;
  }
}

State sm_update_active(){
  switch (state.target_state) {
    case STATE_READY:            // if CAN Signal 1000 0000 then turn oof powerground but stay ready
      return STATE_READY;       
    case STATE_DISCHARGE:        // if CAN Signal 0000 0000 then shutdown
      return STATE_DISCHARGE;
    default:
      return STATE_ACTIVE;
  }
}

State sm_update_discharge(){
  switch (state.target_state) {
    case STATE_INACTIVE:
      return STATE_INACTIVE;
    case STATE_ERROR:
      return STATE_ERROR;
    default: 
      return STATE_DISCHARGE;
  }
}

State sm_update_charging_precharge(){
  switch (state.target_state) {
    case STATE_CHARGING:
      return STATE_CHARGING;
    case STATE_DISCHARGE:
      return STATE_DISCHARGE;
    default:
      return STATE_CHARGING_PRECHARGE;
  }
}

State sm_update_charging(){
  switch (state.target_state) {
    case STATE_DISCHARGE:
      return STATE_DISCHARGE;
    default:

      return STATE_CHARGING;
  }
}

State sm_update_error(){
  switch (state.target_state) {
    case STATE_DISCHARGE:
      return STATE_DISCHARGE;
    case STATE_INACTIVE:
      return STATE_INACTIVE;
    default:
      return STATE_ERROR;
  }
}

void sm_set_relay_positions(State current_state){
  switch (state.current_state) {
    case STATE_INACTIVE:
      sm_set_relay(RELAY_MAIN, 0);
      sm_set_relay(RELAY_PRECHARGE, 0);
      break;
    case STATE_PRECHARGE:
      sm_set_relay(RELAY_MAIN, 0);
      sm_set_relay(RELAY_PRECHARGE, 1);
      break;
    case STATE_READY:
      sm_set_relay(RELAY_MAIN, 1);
      sm_set_relay(RELAY_PRECHARGE, 0);
      break;
    case STATE_ACTIVE:
      sm_set_relay(RELAY_MAIN, 1);
      sm_set_relay(RELAY_PRECHARGE, 0);
      break;
    case STATE_DISCHARGE:
      sm_set_relay(RELAY_MAIN, 0);
      sm_set_relay(RELAY_PRECHARGE, 1);
      break;
    case STATE_CHARGING_PRECHARGE:
      sm_set_relay(RELAY_MAIN, 0);
      sm_set_relay(RELAY_PRECHARGE, 1);
      break;
    case STATE_CHARGING:
      sm_set_relay(RELAY_MAIN, 1);
      sm_set_relay(RELAY_PRECHARGE, 0);
      break;
    case STATE_ERROR:
      sm_set_relay(RELAY_MAIN, 0);
      sm_set_relay(RELAY_PRECHARGE, 0);
      break;
  }
}

void sm_set_relay(Relay relay, bool closed){
  GPIO_PinState state = closed ? GPIO_PIN_SET : GPIO_PIN_RESET;
  switch (relay) {
    case RELAY_MAIN:
      HAL_GPIO_WritePin(RELAY_ENABLE_GPIO_Port, RELAY_ENABLE_Pin, state);
      break;
    case RELAY_PRECHARGE:
      HAL_GPIO_WritePin(PRECHARGE_ENABLE_GPIO_Port, PRECHARGE_ENABLE_Pin, state);
      break;
  }
}

/* returns the ID and temperature of the hottest cell */
void sm_check_battery_temperature(int8_t *id, int16_t *temp){
  for (int i = 0; i < N_TEMP_SENSORS; i++) {
    if (tmp1075_temps[i] > *temp){
      *id = i;
      *temp = tmp1075_temps[i];
    }
  }
}

int16_t sm_return_cell_temperature(int id){ return tmp1075_temps[id]; }

int16_t sm_return_cell_voltage(int id){ return module.cellVoltages[id]; }

void sm_test_cycle_states(){
  RELAY_BAT_SIDE_VOLTAGE = module.auxVoltages[0];
  RELAY_ESC_SIDE_VOLTAGE = module.auxVoltages[1];
  CURRENT_MEASUREMENT = module.auxVoltages[2];
  sm_set_relay_positions(state.current_state);
  
  if (timestamp > HAL_GetTick())
    return;
  switch (state.current_state) {
    case STATE_INACTIVE:
      state.current_state = STATE_PRECHARGE;
      timestamp = HAL_GetTick() + 30000;
      PWM_powerground_control(0);
      break;
    case STATE_PRECHARGE:
      state.current_state = STATE_READY;
      timestamp = HAL_GetTick() + 10000;
      break;
    case STATE_READY:
      state.current_state = STATE_ACTIVE;
      timestamp = HAL_GetTick() + 10000;
      break;
    case STATE_ACTIVE:
      state.current_state = STATE_DISCHARGE;
      timestamp = HAL_GetTick() + 10000;
      PWM_powerground_control(10);
      break;
    case STATE_DISCHARGE:
      state.current_state = STATE_INACTIVE;
      timestamp = HAL_GetTick() + 10000;
      break;
    case STATE_CHARGING_PRECHARGE:
    case STATE_CHARGING:
    case STATE_ERROR:
      break;
    }

  state.target_state = state.current_state;
}