/*
 * AIR_State_Maschine.c
 *
 *  Created on: Jun 15, 2022
 *      Author: max
 */

#include "AIR_State_Maschine.h"

#include "main.h"

#include "stm32g4xx_hal.h"
#include "stm32g4xx_hal_gpio.h"

AIRStateHandler airstate;

DMA_HandleTypeDef* air_current_dma = {0};
DMA_HandleTypeDef* sdc_voltage_dma = {0};

uint8_t air_adc_complete = 0;
uint8_t sdc_adc_complete = 0;

static uint32_t pos_air_change_timestamp, neg_air_change_timestamp,
    precharge_change_timestamp;
static GPIO_PinState neg_air_state, pos_air_state, precharge_state;

void init_AIR_State_Maschine() {
  airstate.targetTSState = TS_INACTIVE;
  airstate.currentTSState = TS_INACTIVE;
  airstate.BatteryVoltageBatterySide = 0;
  airstate.BatteryVoltageVehicleSide = 0;
}

uint8_t Update_AIR_State() {
  //--------------------------------------------------State Transition
  // Rules----------------------------------------------------------

  /*if(airstate->currentTSState == airstate->targetTSState)	//Target TS
  State is Equal to actual TS State
  {
          return airstate->currentTSState;
  }*/

  if (airstate.currentTSState == TS_ERROR) // No Escape from TS Error State
  {
    // Don't change anything, but prevent any other if from being entered
  }

  else if (airstate.targetTSState ==
           TS_ERROR) // Error State is Entered if Target State is Error State
  {
    airstate.currentTSState = TS_ERROR;
  }

  else if ((airstate.currentTSState == TS_INACTIVE) &&
           (airstate.targetTSState ==
            TS_ACTIVE)) // Transition from Inactive to Active via Precharge
  {
    airstate.currentTSState = TS_PRECHARGE;
    airstate.precharge95ReachedTimestamp = 0;
  }

  else if ((airstate.currentTSState == TS_INACTIVE) &&
           (airstate.targetTSState == TS_CHARGING)) {
    airstate.currentTSState = TS_CHARGING_CHECK;
    airstate.chargingCheckTimestamp = HAL_GetTick();
  }

  // TODO: Is it correct that we also go from precharge to discharge?
  else if ((airstate.currentTSState == TS_ACTIVE ||
            airstate.currentTSState == TS_PRECHARGE ||
            airstate.currentTSState == TS_CHARGING_CHECK ||
            airstate.currentTSState == TS_CHARGING) &&
           (airstate.targetTSState ==
            TS_INACTIVE)) // Transition from Active to Inactive via Discharge
  {
    airstate.currentTSState = TS_DISCHARGE;
  }

  else if ((airstate.targetTSState == TS_CHARGING) &&
           (airstate.currentTSState == TS_CHARGING_CHECK)) {
    if (airstate.BatteryVoltageVehicleSide >
        airstate.BatteryVoltageBatterySide) {
      airstate.currentTSState = TS_CHARGING;
    } else if (HAL_GetTick() > airstate.chargingCheckTimestamp + 2000) {
      airstate.currentTSState = TS_ERROR;
    }
  }

  else if (airstate.currentTSState == TS_CHARGING) {
    if (airstate.shuntCurrent < 0) {
      airstate.currentTSState = TS_ERROR;
    }
  }

  else if (airstate.currentTSState ==
           TS_PRECHARGE) // Change from Precharge to Active at 95% TS Voltage at
                         // Vehicle Side
  {
    if ((airstate.BatteryVoltageVehicleSide >
         LOWER_VEHICLE_SIDE_VOLTAGE_LIMIT)) {
      if (airstate.BatteryVoltageVehicleSide >
          (airstate.BatteryVoltageBatterySide * 0.95)) {
        if (airstate.precharge95ReachedTimestamp == 0) {
          airstate.precharge95ReachedTimestamp = HAL_GetTick();
        } else if (HAL_GetTick() - airstate.precharge95ReachedTimestamp >=
                   PRECHARGE_95_DURATION) {
          airstate.currentTSState = TS_ACTIVE;
        }
      }
    }
  }

  else if (airstate.currentTSState ==
           TS_DISCHARGE) // Change from Discharge to Inactive at 95% TS
                         // Voltage at Vehicle Side
  {
    airstate.currentTSState = TS_INACTIVE;
  }

  //_-----------------------------------------------AIR
  // Positions--------------------------------------------------------

  if (airstate.currentTSState == TS_PRECHARGE) {
    AIR_Precharge_Position();
  }

  if (airstate.currentTSState == TS_DISCHARGE) {
    AIR_Discharge_Position();
  }

  if (airstate.currentTSState == TS_CHARGING_CHECK) {
    AIR_Precharge_Position();
  }

  if (airstate.currentTSState == TS_CHARGING) {
    AIR_Active_Position();
  }

  if (airstate.currentTSState == TS_ACTIVE) {
    AIR_Active_Position();
  }

  if (airstate.currentTSState == TS_INACTIVE) {
    AIR_Inactive_Position();
  }

  if (airstate.currentTSState == TS_ERROR) {
    AIR_Error_Position();
  }

  return airstate.currentTSState;
}

void Activate_TS() { airstate.targetTSState = TS_ACTIVE; }

void Deactivate_TS() { airstate.targetTSState = TS_INACTIVE; }

void AIR_Precharge_Position() {
  Set_Relay_Position(RELAY_PRECHARGE, GPIO_PIN_SET);
  Set_Relay_Position(RELAY_AIR_NEG, GPIO_PIN_SET);
  Set_Relay_Position(RELAY_AIR_POS, GPIO_PIN_RESET);
}

void AIR_Inactive_Position() {
  Set_Relay_Position(RELAY_PRECHARGE, GPIO_PIN_RESET);
  Set_Relay_Position(RELAY_AIR_NEG, GPIO_PIN_RESET);
  Set_Relay_Position(RELAY_AIR_POS, GPIO_PIN_RESET);
}

void AIR_Discharge_Position() {
  Set_Relay_Position(RELAY_PRECHARGE, GPIO_PIN_SET);
  Set_Relay_Position(RELAY_AIR_NEG, GPIO_PIN_SET);
  Set_Relay_Position(RELAY_AIR_POS, GPIO_PIN_RESET);
}

void AIR_Active_Position() {
  if (pos_air_state == GPIO_PIN_SET &&
      HAL_GetTick() - pos_air_change_timestamp > PRECHARGE_OPEN_AFTER) {
    Set_Relay_Position(RELAY_PRECHARGE, GPIO_PIN_RESET);
  } else {
    Set_Relay_Position(RELAY_PRECHARGE, GPIO_PIN_SET);
  }
  Set_Relay_Position(RELAY_AIR_NEG, GPIO_PIN_SET);
  Set_Relay_Position(RELAY_AIR_POS, GPIO_PIN_SET);
}

void AIR_Error_Position() {
  Set_Relay_Position(RELAY_PRECHARGE, GPIO_PIN_RESET);
  Set_Relay_Position(RELAY_AIR_NEG, GPIO_PIN_RESET);
  Set_Relay_Position(RELAY_AIR_POS, GPIO_PIN_RESET);
}

void Set_Relay_Position(Relay relay, GPIO_PinState position) {
  // Add a small delay after closing relays in order to not draw too much
  // current
  switch (relay) {
  case RELAY_AIR_NEG:
    HAL_GPIO_WritePin(NEG_AIR_CTRL_GPIO_Port, NEG_AIR_CTRL_Pin, position);
    if (position != neg_air_state) {
      neg_air_change_timestamp = HAL_GetTick();
    }
    if (position == GPIO_PIN_SET && neg_air_state == GPIO_PIN_RESET) {
      HAL_Delay(10);
    }
    neg_air_state = position;
    break;
  case RELAY_AIR_POS:
    HAL_GPIO_WritePin(POS_AIR_CTRL_GPIO_Port, POS_AIR_CTRL_Pin, position);
    if (position != pos_air_state) {
      pos_air_change_timestamp = HAL_GetTick();
    }
    if (position == GPIO_PIN_SET && pos_air_state == GPIO_PIN_RESET) {
      HAL_Delay(10);
    }
    pos_air_state = position;
    break;
  case RELAY_PRECHARGE:
    HAL_GPIO_WritePin(PRECHARGE_CTRL_GPIO_Port, PRECHARGE_CTRL_Pin, position);
    if (position != precharge_state) {
      precharge_change_timestamp = HAL_GetTick();
    }
    if (position == GPIO_PIN_SET && precharge_state == GPIO_PIN_RESET) {
      HAL_Delay(10);
    }
    precharge_state = position;
    break;
  }
}