#include "state_machine.h"

StateHandle state;

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

void sm_update(){

  if (state.error_source) {
    state.current_state = ERROR;
  }

  switch (state.current_state) {
  case INACTIVE:
    //state.current_state = sm_update_inactive(); moniter only
    break;
  case PRECHARGE:
    //state.current_state = sm_update_precharge(); set PRECHARGE and turn on cooling at 50% or such
    break;
  case READY:
    //state.current_state = ; keep cooling at 50%, get ready to turn on powerground
    break;
  case ACTIVE:
    //state.current_state = sm_update_precharge(); set PRECHARGE and turn on cooling at 50% or such
    break;
  case DISCHARGE:
    //state.current_state = sm_update_discharge(); // open the main relay, keep PRECHARGE closed
    break;
  case CHARGING:
    //state.current_state = sm_update_charging(); // turn on cooling if needed, 
    break;
  case ERROR:
    //state.current_state = sm_update_error(); // enter the correct ERROR state 
    break;
  case WARNING:
    //state.current_state = sm_update_error(); // send warnings through CAN
    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 PRECHARGE:
      if ()
      break;
    case CHARGING:
      break;
  }
  return INACTIVE;
}

State sm_update_precharge(){
  switch (state.target_state) {
    case INACTIVE:
      break;
    case READY:
      break;
    case DISCHARGE:
      break;
  }
  return PRECHARGE;
}

State sm_update_ready(){
  switch (state.target_state) {
    case ACTIVE:
      break;
    case DISCHARGE:
      break;
  }
  return READY;
}

State sm_update_active(){
  switch (state.target_state) {
    case READY:
      break;
    case DISCHARGE:
      break;
  }
  return ACTIVE;
}

State sm_update_discharge(){
  switch (state.target_state) {
    case INACTIVE:
      break;
    case PRECHARGE:
      break;
  }
  return DISCHARGE;
}

State sm_update_charging(){
  switch (state.target_state) {
    case INACTIVE:
      break;
    case DISCHARGE:
      break;
  }
  return CHARGING;
}

State sm_update_error(){
  switch (state.target_state) {
    case INACTIVE:
      break;
    case DISCHARGE:
      break;
  }
  return ERROR;
}

void sm_set_relay_positions(State state){
  return;
}

void sm_set_relay(Relay relay, int closed){
  static int closed = 0;
  static int precharge_closed = 0;

  GPIO_PinState state = closed ? GPIO_PIN_SET : GPIO_PIN_RESET;
  switch (relay) {
  case RELAY_POS:
    ts_sm_check_close_wait(&closed, closed);
    HAL_GPIO_WritePin(RELAY_EN_GPIO_Port, RELAY_EN_Pin, state);
    break;
  case RELAY_PRECHARGE:
    ts_sm_check_close_wait(&precharge_closed, closed);
    HAL_GPIO_WritePin(PRECHARGE_EN_GPIO_Port, PRECHARGE_EN_Pin, state);
    break;
  }
}

//void sm_check_close_wait(int *is_closed, int should_close);

//void sm_handle_ams_in(const uint8_t *data);

//void sm_set_error(ErrorKind error_kind, bool is_errored);