#include "ts_state_machine.h"

#include "main.h"
#include "shunt_monitoring.h"
#include "stm32f3xx_hal.h"
#include "stm32f3xx_hal_gpio.h"
#include <stdint.h>

TSStateHandle ts_state;

static uint32_t precharge_95_reached_timestamp = 0;
static uint32_t charging_check_timestamp = 0;
static uint32_t discharge_begin_timestamp = 0;

static int sdc_is_closed() {
  return HAL_GPIO_ReadPin(SDC_VOLTAGE_GPIO_Port, SDC_VOLTAGE_Pin) ==
         GPIO_PIN_SET;
}

void ts_sm_init() {
  ts_state.current_state = TS_INACTIVE;
  ts_state.target_state = TS_INACTIVE;
  ts_state.error = 0;
}

void ts_sm_update() {
  if (ts_state.error) {
    ts_state.current_state = TS_ERROR;
  }

  switch (ts_state.current_state) {
  case TS_INACTIVE:
    ts_state.current_state = ts_sm_update_inactive();
    break;
  case TS_ACTIVE:
    ts_state.current_state = ts_sm_update_active();
    break;
  case TS_PRECHARGE:
    ts_state.current_state = ts_sm_update_precharge();
    break;
  case TS_DISCHARGE:
    ts_state.current_state = ts_sm_update_discharge();
    break;
  case TS_ERROR:
    ts_state.current_state = ts_sm_update_error();
    break;
  case TS_CHARGING_CHECK:
    ts_state.current_state = ts_sm_update_charging_check();
    break;
  case TS_CHARGING:
    ts_state.current_state = ts_sm_update_charging();
    break;
  }

  ts_sm_set_relay_positions(ts_state.current_state);
}

TSState ts_sm_update_inactive() {
  if (ts_state.target_state == TS_ACTIVE) {
    precharge_95_reached_timestamp = 0;
    return TS_PRECHARGE;
  } else if (ts_state.target_state == TS_CHARGING) {
    charging_check_timestamp = HAL_GetTick();
    return TS_CHARGING_CHECK;
  }

  return TS_INACTIVE;
}

TSState ts_sm_update_active() {
  if (ts_state.target_state == TS_INACTIVE || !sdc_is_closed()) {
    discharge_begin_timestamp = HAL_GetTick();
    return TS_DISCHARGE;
  }

  return TS_ACTIVE;
}

TSState ts_sm_update_precharge() {
  if (ts_state.target_state == TS_INACTIVE || !sdc_is_closed()) {
    discharge_begin_timestamp = HAL_GetTick();
    return TS_DISCHARGE;
  }
  if (shunt_data.voltage_veh > MIN_VEHICLE_SIDE_VOLTAGE &&
      shunt_data.voltage_veh > 0.95 * shunt_data.voltage_bat) {
    uint32_t now = HAL_GetTick();
    if (precharge_95_reached_timestamp == 0) {
      precharge_95_reached_timestamp = now;
    } else if (now - precharge_95_reached_timestamp >= PRECHARGE_95_DURATION) {
      precharge_95_reached_timestamp = 0;
      return TS_ACTIVE;
    }
  }

  return TS_PRECHARGE;
}

TSState ts_sm_update_discharge() {
  if (HAL_GetTick() - discharge_begin_timestamp >= DISCHARGE_DURATION) {
    return TS_INACTIVE;
  } else {
    return TS_DISCHARGE;
  }
}

TSState ts_sm_update_error() {
  static uint32_t no_error_since = 0;
  if (ts_state.error == 0) {
    uint32_t now = HAL_GetTick();
    if (no_error_since == 0) {
      no_error_since = now;
    } else if (now - no_error_since > NO_ERROR_TIME) {
      no_error_since = 0;
      return TS_INACTIVE;
    }
  }

  return TS_ERROR;
}

TSState ts_sm_update_charging_check() {
  if (ts_state.target_state == TS_INACTIVE || !sdc_is_closed()) {
    discharge_begin_timestamp = HAL_GetTick();
    return TS_DISCHARGE;
  }

  if (shunt_data.voltage_veh > shunt_data.voltage_bat) {
    return TS_CHARGING;
  } else if (HAL_GetTick() - charging_check_timestamp >
             MAX_CHARGING_CHECK_DURATION) {
    return TS_ERROR;
  }

  return TS_CHARGING_CHECK;
}

TSState ts_sm_update_charging() {
  if (ts_state.target_state == TS_INACTIVE || !sdc_is_closed()) {
    discharge_begin_timestamp = HAL_GetTick();
    return TS_DISCHARGE;
  }
  if (shunt_data.current < 0) {
    return TS_ERROR;
  }

  return TS_CHARGING;
}

void ts_sm_set_relay_positions(TSState state) {
  switch (state) {
  case TS_INACTIVE:
  case TS_DISCHARGE:
  case TS_ERROR:
    ts_sm_set_relay_position(RELAY_NEG, 0);
    ts_sm_set_relay_position(RELAY_POS, 0);
    ts_sm_set_relay_position(RELAY_PRECHARGE, 0);
    break;
  case TS_ACTIVE:
  case TS_CHARGING:
    ts_sm_set_relay_position(RELAY_NEG, 1);
    ts_sm_set_relay_position(RELAY_POS, 1);
    ts_sm_set_relay_position(RELAY_PRECHARGE, 1);
    // TODO: Open precharge relay after a while
    break;
  case TS_PRECHARGE:
  case TS_CHARGING_CHECK:
    ts_sm_set_relay_position(RELAY_NEG, 1);
    ts_sm_set_relay_position(RELAY_POS, 0);
    ts_sm_set_relay_position(RELAY_PRECHARGE, 1);
    break;
  }
}

void ts_sm_set_relay_position(Relay relay, int closed) {
  static int neg_closed = 0;
  static int pos_closed = 0;
  static int precharge_closed = 0;

  GPIO_PinState state = closed ? GPIO_PIN_SET : GPIO_PIN_RESET;
  switch (relay) {
  case RELAY_NEG:
    ts_sm_check_close_wait(&neg_closed, closed);
    HAL_GPIO_WritePin(NEG_AIR_CTRL_GPIO_Port, NEG_AIR_CTRL_Pin, state);
    break;
  case RELAY_POS:
    ts_sm_check_close_wait(&pos_closed, closed);
    HAL_GPIO_WritePin(POS_AIR_CTRL_GPIO_Port, POS_AIR_CTRL_Pin, state);
    break;
  case RELAY_PRECHARGE:
    ts_sm_check_close_wait(&precharge_closed, closed);
    HAL_GPIO_WritePin(PRECHARGE_CTRL_GPIO_Port, PRECHARGE_CTRL_Pin, state);
    break;
  }
}

void ts_sm_check_close_wait(int *is_closed, int should_close) {
  static uint32_t last_close_timestamp = 0;
  if (should_close != *is_closed) {
    *is_closed = should_close;
    if (should_close) {
      uint32_t dt = HAL_GetTick() - last_close_timestamp;
      if (dt < RELAY_CLOSE_WAIT) {
        HAL_Delay(RELAY_CLOSE_WAIT - dt);
      }
      last_close_timestamp = HAL_GetTick();
    }
  }
}

void ts_sm_handle_ams_in(const uint8_t *data) {
  if (data[0] & 0x01) {
    ts_state.target_state = TS_ACTIVE;
  } else {
    ts_state.target_state = TS_INACTIVE;
  }
}

void ts_sm_set_error_source(uint32_t flag, int state) {
  if (state) {
    ts_state.error |= flag;
  } else {
    ts_state.error &= ~flag;
  }
}