#include "vehicle.h"

#include "main.h"
#include "stm32h7xx.h"
#include "stm32h7xx_hal.h"
#include "stm32h7xx_hal_fdcan.h"
#include "stm32h7xx_hal_gpio.h"
#include "tx_api.h"
#include "ui.h"

FDCAN_HandleTypeDef *hcan;

void vehicle_thread_entry(ULONG hfdcan_addr) {
  hcan = (void *)hfdcan_addr;

  FDCAN_FilterTypeDef filter;
  filter.IdType = FDCAN_STANDARD_ID;
  filter.FilterIndex = 0;
  filter.FilterType = FDCAN_FILTER_MASK;
  filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
  filter.FilterID1 = 0x123;
  filter.FilterID2 = 0x7FF;

  if (HAL_FDCAN_ConfigFilter(hcan, &filter) != HAL_OK) {
    Error_Handler();
  }
  // Reject non-matching messages
  if (HAL_FDCAN_ConfigGlobalFilter(hcan, FDCAN_REJECT, FDCAN_REJECT,
                                   FDCAN_REJECT_REMOTE,
                                   FDCAN_REJECT_REMOTE) != HAL_OK) {
    Error_Handler();
  }
  if (HAL_FDCAN_Start(hcan)) {
    Error_Handler();
  }
  if (HAL_FDCAN_ActivateNotification(hcan, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) !=
      HAL_OK) {
    Error_Handler();
  }

  FDCAN_TxHeaderTypeDef tx_header;
  tx_header.IdType = FDCAN_STANDARD_ID;
  tx_header.TxFrameType = FDCAN_DATA_FRAME;
  tx_header.ErrorStateIndicator = FDCAN_ESI_PASSIVE;
  tx_header.BitRateSwitch = FDCAN_BRS_OFF;
  tx_header.FDFormat = FDCAN_CLASSIC_CAN;
  tx_header.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
  tx_header.MessageMarker = 0;
  while (1) {
    tx_header.Identifier = 0x456;
    tx_header.DataLength = FDCAN_DLC_BYTES_2;
    uint8_t data[] = {0xFF, 0xEE};
    if (HAL_FDCAN_GetTxFifoFreeLevel(hcan) != 0) {
      HAL_FDCAN_AddMessageToTxFifoQ(hcan, &tx_header, data);
    }
    tx_thread_sleep(10);
  }
}

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan,
                               uint32_t RxFifo0ITs) {
  if (hfdcan != hcan || (RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) == RESET) {
    return;
  }

  static FDCAN_RxHeaderTypeDef header;
  static uint8_t data[8];
  if (HAL_FDCAN_GetRxMessage(hcan, FDCAN_RX_FIFO0, &header, data) != HAL_OK) {
    Error_Handler();
  }

  if (header.IdType != FDCAN_STANDARD_ID || header.Identifier == 0x123) {
    HAL_GPIO_TogglePin(STATUS2_GPIO_Port, STATUS2_Pin);
  }
}