#include "state_machine.h"
#include "ADBMS_LL_Driver.h"
#include "AMS_HighLevel.h"
#include "stm32f3xx_hal.h"
#include "ADBMS_Abstraction.h"
#include "main.h"

StateHandle state;
static bool relay_closed = 0;
static bool precharge_closed = 0;
static int16_t RELAY_BAT_SIDE = 0;
static int16_t RELAY_ESC_SIDE = 0;
static int16_t CURRENT_MEASUREMENT = 0;

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

void sm_update(){

  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:
      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);
  //status_led_state(state.current_state, (ErrorKind) state.error_type);
}

State sm_update_inactive(){
  switch (state.target_state) {
    case STATE_PRECHARGE:
      //close precharge relay, wait until both sides are similar
      sm_set_relay_positions(STATE_PRECHARGE);
      return STATE_PRECHARGE;
    case STATE_CHARGING:
      return STATE_CHARGING;
    default:
      return STATE_INACTIVE;
  }
}

State sm_update_precharge(){
  switch (state.target_state) {
    case STATE_INACTIVE:
      return STATE_INACTIVE;
    case STATE_READY:
      return STATE_READY;
    case STATE_DISCHARGE:
      return STATE_DISCHARGE;
    default:
      return STATE_PRECHARGE;
  }
}

State sm_update_ready(){
  switch (state.target_state) {
    case STATE_ACTIVE:
      return STATE_ACTIVE;
    case STATE_DISCHARGE:
      return STATE_DISCHARGE;
    default:
      return STATE_READY;
  }
}

State sm_update_active(){
  switch (state.target_state) {
    case STATE_READY:
      return STATE_READY;
    case STATE_DISCHARGE:
      return STATE_DISCHARGE;
    default:
      return STATE_ACTIVE;
  }
}

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

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

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

void sm_set_relay_positions(State current_state){
  switch (state.target_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);
    case STATE_DISCHARGE:
      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, 1);
      break;
    case STATE_ERROR:
      sm_set_relay(RELAY_MAIN, 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_EN_GPIO_Port, RELAY_EN_Pin, state);
      relay_closed = closed;
      break;
    case RELAY_PRECHARGE:
      HAL_GPIO_WritePin(PRECHARGE_EN_GPIO_Port, PRECHARGE_EN_Pin, state);
      precharge_closed = closed;
      break;
  }
}


void sm_check_precharge_discharge(bool *is_closed, bool should_close){}
  // compare RELAY_BATT_SIDE and RELAY_ESC_SIDE
  // if (state.current_state == STATE_PRECHARGE && (RELAY_ESC_SIDE < RELAY_BAT_SIDE)) //-> don't switch from PRECHARGE to READY
  // if (state.current_state == STATE_DISCHARGE && (RELAY_ESC_SIDE > 12V)) -> don't switch from DISCHARGE to INACTIVE

void sm_handle_ams_in(const uint8_t *data){}

void sm_set_error(ErrorKind error_kind, bool is_errored);

void sm_check_errors(){}