/*
 * AMS_CAN.c
 *
 *  Created on: Mar 19, 2022
 *      Author: jasper
 */

#include "AMS_CAN.h"

#include "BQ_Abstraction_Layer.h"
#include "TMP144.h"

static CAN_HandleTypeDef* handle_ams;
static CAN_HandleTypeDef* handle_car;

void ams_can_init(CAN_HandleTypeDef* ams_handle,
                  CAN_HandleTypeDef* car_handle) {
  handle_ams = ams_handle;
  handle_car = car_handle;

  // Configure filters
  CAN_FilterTypeDef filter_car;
  filter_car.FilterBank = 14;
  filter_car.FilterMode = CAN_FILTERMODE_IDMASK;
  filter_car.FilterScale = CAN_FILTERSCALE_32BIT;
  filter_car.FilterIdHigh = CAN_ID_AMS_SLAVE_HEARTBEAT_BASE >> 16;
  filter_car.FilterIdLow = CAN_ID_AMS_SLAVE_HEARTBEAT_BASE & 0xFFFF;
  filter_car.FilterMaskIdHigh = 0xFFFF;
  filter_car.FilterMaskIdLow = 0xFF00;
  filter_car.FilterFIFOAssignment = CAN_RX_FIFO0;
  filter_car.FilterActivation = ENABLE;
  filter_car.SlaveStartFilterBank = 14;
  if (HAL_CAN_ConfigFilter(handle_car, &filter_car) != HAL_OK) {
    Error_Handler();
  }

  // Start peripheral
  if (HAL_CAN_Start(handle_ams) != HAL_OK) {
    Error_Handler();
  }
  if (HAL_CAN_Start(handle_car) != HAL_OK) {
    Error_Handler();
  }

  // Activate RX notifications
  if (HAL_CAN_ActivateNotification(handle_ams, CAN_IT_RX_FIFO0_MSG_PENDING) !=
      HAL_OK) {
    Error_Handler();
  }
  if (HAL_CAN_ActivateNotification(handle_car,
                                   CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_ERROR |
                                       CAN_IT_LAST_ERROR_CODE) != HAL_OK) {
    Error_Handler();
  }
}

static int cb_triggered = 0;

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* handle) {
  static CAN_RxHeaderTypeDef header;
  static uint8_t data[8];
  cb_triggered = 1;

  if (HAL_CAN_GetRxMessage(handle, CAN_RX_FIFO0, &header, data) != HAL_OK) {
    Error_Handler();
  }

  if (handle == handle_ams) {
    ams_can_handle_ams_msg(&header, data);
  } else if (handle == handle_car) {
    ams_can_handle_car_msg(&header, data);
  } else {
    Error_Handler();
  }
}

void ams_can_handle_ams_msg(CAN_RxHeaderTypeDef* header, uint8_t* data) {}

void ams_can_handle_car_msg(CAN_RxHeaderTypeDef* header, uint8_t* data) {}

void ams_can_send_heartbeat() {
  static CAN_TxHeaderTypeDef header;
  static uint8_t data[8];
  static uint8_t SLAVE_ID = 0;

  header.IDE = CAN_ID_STD;
  header.DLC = 8;
  header.RTR = CAN_RTR_DATA;
  header.TransmitGlobalTime = DISABLE;

  for (int i = 0; i < N_CELLS / 2; i++) {
    header.StdId = CAN_ID_AMS_SLAVE_HEARTBEAT_BASE | (SLAVE_ID) << 4 | i;
    uint16_t v1 = cell_voltages[i * 2];
    uint16_t v2 = cell_voltages[i * 2 + 1];
    data[0] = v1 >> 8;
    data[1] = v1 & 0xFF;
    data[2] = v2 >> 8;
    data[3] = v2 & 0xFF;
    uint16_t t1 = temperatures[i * 2];
    uint16_t t2 = temperatures[i * 2 + 1];
    data[4] = t1 >> 8;
    data[5] = t1 & 0xFF;
    data[6] = t2 >> 8;
    data[7] = t2 & 0xFF;
    uint32_t mailbox;
    HAL_StatusTypeDef status;
    if ((status = HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox)) !=
        HAL_OK) {
      Error_Handler();
    } else {
      HAL_Delay(100);
    }
  }
}