#include "main.h"
#include "can.h"
#include "scs_sdc.h"

TSStateHandle ts_state = {.current_state = TS_INACTIVE, .target_state = TS_INACTIVE};
IMDData imd_data;
uint8_t led_state;

void error_led_init() {
  HAL_GPIO_WritePin(AMS_ERROR_GPIO_Port, AMS_ERROR_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(IMD_ERROR_GPIO_Port, IMD_ERROR_Pin, GPIO_PIN_SET);
}

void sdc_init() {
  HAL_GPIO_WritePin(SDC_ENABLE_GPIO_Port, SDC_ENABLE_Pin, GPIO_PIN_SET); 
}

void ts_handle_ams_status(const uint8_t* data) {
  ts_state.current_state = data[0] & 0b01111111;
  imd_data.ok = data[6] & 0b10000000;
  led_state = data[7];

  if ((led_state & 0b00000010) == 0b10) {
      HAL_GPIO_WritePin(IMD_ERROR_GPIO_Port, IMD_ERROR_Pin, GPIO_PIN_SET);
  } else {
      HAL_GPIO_WritePin(IMD_ERROR_GPIO_Port, IMD_ERROR_Pin, GPIO_PIN_RESET);
  }

  if ((led_state & 0b00000100) == 0b100) {
      HAL_GPIO_WritePin(AMS_ERROR_GPIO_Port, AMS_ERROR_Pin, GPIO_PIN_SET);
  } else {
      HAL_GPIO_WritePin(AMS_ERROR_GPIO_Port, AMS_ERROR_Pin, GPIO_PIN_RESET);
  }

  if (ts_state.current_state == TS_ERROR || ts_state.current_state == TS_DISCHARGE || ((led_state & 0b110) > 0)) {
    ts_state.target_state = TS_INACTIVE;
  }
}

HAL_StatusTypeDef change_ts_state(TSState state) {
  if (state == TS_ACTIVE) {
    ts_state.target_state = TS_ACTIVE;

    return HAL_OK;
  } else if (state == TS_INACTIVE){
    ts_state.target_state = TS_INACTIVE;

    return HAL_OK;
  } else {
    return HAL_ERROR;
  }
}

HAL_StatusTypeDef check_sdc_state() {
  if (!HAL_GPIO_ReadPin(SDC_ON_GPIO_Port, SDC_ON_Pin)) {
    TRY(change_ts_state(TS_INACTIVE));
  }

  return HAL_OK;
}