#include "state_machine.h"
#include "AMS_HighLevel.h"
#include "TMP1075.h"
#include "errors.h"
#include "main.h"
#include "stm32f3xx_hal.h"
#include <stdint.h>

StateHandle state;
int16_t RELAY_BAT_SIDE_VOLTAGE;
int16_t RELAY_ESC_SIDE_VOLTAGE;
int16_t CURRENT_MEASUREMENT;
uint8_t powerground_status;

static uint32_t timestamp;

void sm_init(){
  state.current_state = STATE_INACTIVE;
  state.target_state = STATE_INACTIVE;
  state.error_source = 0;
}

void sm_update(){
  sm_check_errors();

  RELAY_BAT_SIDE_VOLTAGE = module.auxVoltages[0] * 12.42;     // the calculation says the factor is 11.989. 12.42 yields the better result
  RELAY_ESC_SIDE_VOLTAGE = module.auxVoltages[1] * 12.42;
  CURRENT_MEASUREMENT = (module.auxVoltages[2] - 2496) * 300;
  
  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;
}

State sm_update_inactive(){
  switch (state.target_state) {
    case STATE_PRECHARGE:
      return STATE_PRECHARGE;
    case STATE_CHARGING_PRECHARGE:
      return STATE_CHARGING_PRECHARGE;
    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_PRECHARGE:
      if (RELAY_BAT_SIDE_VOLTAGE-RELAY_ESC_SIDE_VOLTAGE < 100){
        PWM_set_throttle();
        return STATE_READY;
      }
      return STATE_PRECHARGE;
    case STATE_DISCHARGE:
      return STATE_DISCHARGE;
    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_DISCHARGE:
      return (RELAY_ESC_SIDE_VOLTAGE < 5000) ? STATE_INACTIVE : STATE_DISCHARGE;
    case STATE_PRECHARGE:       // if CAN Signal 1000 0000 then get ready
      return STATE_PRECHARGE;
    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;
    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, 0);
      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);
      relay_closed = closed;
      break;
    case RELAY_PRECHARGE:
      HAL_GPIO_WritePin(PRECHARGE_ENABLE_GPIO_Port, PRECHARGE_ENABLE_Pin, state);
      precharge_closed = closed;
      break;
  }
}

void sm_check_charging(){
  if (RELAY_BAT_SIDE_VOLTAGE < RELAY_ESC_SIDE_VOLTAGE && timestamp == 0)  
    timestamp = HAL_GetTick() + 5000;
  if (timestamp < HAL_GetTick())
    state.target_state = STATE_CHARGING_PRECHARGE;
}

/* 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_handle_ams_in(const uint8_t *data){  
  switch (data[0]) {
    case 0x00:
      if (state.current_state != STATE_INACTIVE){
        state.target_state = STATE_DISCHARGE;
      }
      break;
    case 0x01:
      if (state.target_state == STATE_INACTIVE || state.target_state == STATE_DISCHARGE){
        state.target_state = STATE_PRECHARGE;
      } else if (state.target_state == STATE_ACTIVE){
        state.target_state = STATE_READY;
      }
      break;
    case 0x02:
      if (state.current_state == STATE_READY || state.current_state == STATE_ACTIVE){
        PWM_powerground_control(data[1]);
        state.target_state = STATE_ACTIVE;        // READY -> ACTIVE
      }
      break;
  }
}

void sm_set_error(ErrorKind error_kind, bool is_errored){}

#warning TODO: add error checking for everything here
void sm_check_errors(){
  switch (error_data.data_kind) {
    case SEK_OVERTEMP:
    case SEK_UNDERTEMP:
    case SEK_TOO_FEW_TEMPS:
      state.error_type.temperature_error = 1;
      break;
    case SEK_OVERVOLT:
    case SEK_UNDERVOLT:
    case SEK_OPENWIRE:
      state.error_type.voltage_error = 1;
      break;
    case SEK_EEPROM_ERR:
      state.error_type.eeprom_error = 1;
      break;
    case SEK_INTERNAL_BMS_TIMEOUT:
      state.error_type.bms_timeout = 1;
      break;
    case SEK_INTERNAL_BMS_CHECKSUM_FAIL:
    case SEK_INTERNAL_BMS_OVERTEMP:
    case SEK_INTERNAL_BMS_FAULT:
      state.error_type.bms_fault = 1;
      break;
  }

  if (1){
    state.error_type.current_error = 1;
  }

  if (1){
    state.error_type.current_sensor_missing = 1;
  }

  if (RELAY_BAT_SIDE_VOLTAGE < 30000){
    state.error_type.voltage_error = 1;
  }

  if (1){
    state.error_type.voltage_missing = 1;
  }
}  

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(1);
      break;
    case STATE_DISCHARGE:
      state.current_state = STATE_INACTIVE;
      timestamp = HAL_GetTick() + 10000;
      break;
  }

  state.target_state = state.current_state;
}