#include "vehicle.h"

#include "main.h"
#include "state.h"

#include "stm32g4xx_hal_def.h"
#include "stm32g4xx_hal_fdcan.h"

FDCAN_HandleTypeDef* can;

void handle_as_mission_fb(FDCAN_RxHeaderTypeDef* header, uint8_t* data);

void vehicle_init(FDCAN_HandleTypeDef* handle) {
  can = handle;

  // RX Filter
  FDCAN_FilterTypeDef filter;
  filter.IdType = FDCAN_STANDARD_ID;
  filter.FilterIndex = 0;
  filter.FilterType = FDCAN_FILTER_MASK;
  filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
  filter.FilterID1 = CAN_ID_AS_MISSION_FB;
  filter.FilterID2 = filter.FilterID1;
  if (HAL_FDCAN_ConfigFilter(can, &filter) != HAL_OK) {
    Error_Handler();
  }

  // Global filter: Filter all remote frames with STD & EXT ID, reject non
  // matching frames with STD & EXT ID

  // TODO: What does that mean?
  if (HAL_FDCAN_ConfigGlobalFilter(can, FDCAN_REJECT, FDCAN_REJECT,
                                   FDCAN_FILTER_REMOTE,
                                   FDCAN_FILTER_REMOTE) != HAL_OK) {
    Error_Handler();
  }

  // Start FDCAN module
  if (HAL_FDCAN_Start(can) != HAL_OK) {
    Error_Handler();
  }

  // Enable RX interrupt
  if (HAL_FDCAN_ActivateNotification(can, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) !=
      HAL_OK) {
    Error_Handler();
  }
}

void vehicle_send_mission_select(Mission mission) {
  // TODO: Automatically resend this until it's acknowledged
  FDCAN_TxHeaderTypeDef header;
  header.Identifier = CAN_ID_MISSION_SELECT;
  header.IdType = FDCAN_STANDARD_ID;
  header.TxFrameType = FDCAN_DATA_FRAME;
  header.DataLength = FDCAN_DLC_BYTES_1;
  header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
  header.BitRateSwitch = FDCAN_BRS_OFF;
  header.FDFormat = FDCAN_CLASSIC_CAN;
  header.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
  header.MessageMarker = 0;

  uint8_t data[1];
  data[0] = mission;
  if (HAL_FDCAN_AddMessageToTxFifoQ(can, &header, data) != HAL_OK) {
    Error_Handler();
  }
}

/* BEGIN CAN MESSAGE HANDLERS { */

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef* handle,
                               uint32_t interrupt_flags) {
  if (!(interrupt_flags & FDCAN_IT_RX_FIFO0_NEW_MESSAGE)) {
    return;
  }

  static FDCAN_RxHeaderTypeDef header;
  static uint8_t data[64];

  if (HAL_FDCAN_GetRxMessage(can, FDCAN_RX_FIFO0, &header, data) != HAL_OK) {
    Error_Handler();
  }

  if (header.IdType != FDCAN_STANDARD_ID) {
    return;
  }

  switch (header.Identifier) {
  case CAN_ID_AS_MISSION_FB:
    handle_as_mission_fb(&header, data);
    break;
  }
}

void handle_as_mission_fb(FDCAN_RxHeaderTypeDef* header, uint8_t* data) {
  Mission mission_fb = data[0];
  if (mission_fb == MISSION_NONE) {
    return;
  }

  stw_state.view_state.ami.current_mission = mission_fb;

  stw_state.view = VIEW_AMI;
}

/* } END CAN MESSAGE HANDLERS */