#include "can.h"

#include "main.h"
#include "shunt_monitoring.h"
#include "slave_monitoring.h"
#include "soc_estimation.h"
#include "stm32f3xx_hal_can.h"
#include "stm32f3xx_hal_gpio.h"
#include "ts_state_machine.h"
#include "util.h"
#include <stdint.h>

static CAN_HandleTypeDef *hcan;

void can_init(CAN_HandleTypeDef *handle) {
  hcan = handle;

  CAN_FilterTypeDef filter;
  filter.FilterIdHigh = CAN_ID_SHUNT_BASE & 0xFF0;
  filter.FilterMaskIdHigh = 0xFF0;
  filter.FilterIdLow = CAN_ID_AMS_IN;
  filter.FilterMaskIdLow = 0xFFF;
  filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
  filter.FilterBank = 0;
  filter.FilterMode = CAN_FILTERMODE_IDMASK;
  filter.FilterScale = CAN_FILTERSCALE_16BIT;
  if (HAL_CAN_ConfigFilter(hcan, &filter) != HAL_OK) {
    Error_Handler();
  }
  // TODO: Slave status?
  filter.FilterIdHigh = CAN_ID_SLAVE_PANIC;
  filter.FilterMaskIdHigh = 0xFFF;
  filter.FilterIdLow = CAN_ID_SLAVE_LOG;
  filter.FilterMaskIdLow = 0xFFF;
  filter.FilterBank = 1;
  if (HAL_CAN_ConfigFilter(hcan, &filter) != HAL_OK) {
    Error_Handler();
  }

  if (HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING) !=
      HAL_OK) {
    Error_Handler();
  }
  if (HAL_CAN_Start(hcan) != HAL_OK) {
    Error_Handler();
  }
}

HAL_StatusTypeDef can_transmit(uint8_t id, uint8_t *data, size_t datalen) {
  static CAN_TxHeaderTypeDef header;
  header.StdId = id;
  header.IDE = CAN_ID_STD;
  header.RTR = CAN_RTR_DATA;
  header.DLC = datalen;

  uint32_t mailbox;
  return HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox);
}

HAL_StatusTypeDef can_send_status() {
  uint8_t data[4];
  data[0] = ts_state.current_state;
  data[1] = current_soc;
  data[2] = min_voltage >> 8;
  data[3] = max_temp >> 8;
  return can_transmit(CAN_ID_AMS_STATUS, data, 4);
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *handle) {
  if (handle != hcan) {
    return;
  }

  CAN_RxHeaderTypeDef header;
  uint8_t data[8];
  if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &header, data) != HAL_OK) {
    set_error_led();
    return;
  }

  if (header.IDE != CAN_ID_STD) {
    return;
  }

  if ((header.StdId & 0xFF0) == CAN_ID_SHUNT_BASE) {
    shunt_handle_can_msg(&header, data);
    return;
  }
  switch (header.StdId) {
  case CAN_ID_SLAVE_PANIC:
    slaves_handle_panic(data);
    break;
  case CAN_ID_SLAVE_LOG:
    slaves_handle_log(data);
    break;
  case CAN_ID_AMS_IN:
    ts_sm_handle_ams_in(data);
    break;
  }
}