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

TSStateHandle ts_state;
IMDData imd_data;

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;

  if (ts_state.current_state != TS_ERROR) {
      HAL_GPIO_WritePin(AMS_ERROR_GPIO_Port, AMS_ERROR_Pin, GPIO_PIN_RESET);
  } else {
      HAL_GPIO_WritePin(AMS_ERROR_GPIO_Port, AMS_ERROR_Pin, GPIO_PIN_SET);
  }

  if (imd_data.ok) {
      HAL_GPIO_WritePin(IMD_ERROR_GPIO_Port, IMD_ERROR_Pin, GPIO_PIN_RESET);
  } else {
      HAL_GPIO_WritePin(IMD_ERROR_GPIO_Port, IMD_ERROR_Pin, GPIO_PIN_SET);
  }
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if(GPIO_Pin == TS_ON_Pin) {
    can_change_ts_state(TS_ACTIVE);
  } else if (GPIO_Pin == ENC_PUSH_ON_Pin){
    can_change_ts_state(TS_INACTIVE);
  } else {
    __NOP();
  }
}

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

  return HAL_OK;
}