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

#include "AMS_CAN.h"

#include "BQ_Abstraction_Layer.h"
#include "ClockSync.h"
#include "TMP144.h"
#include "common_defs.h"
#include "main.h"

#include "stm32f4xx.h"
#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_can.h"

#include <stdint.h>

CAN_HandleTypeDef* ams_can_handle;

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

  // Start peripheral
  if (HAL_CAN_Start(ams_can_handle) != HAL_OK) {
    ams_can_handle = car_handle;
    if (HAL_CAN_Start(ams_can_handle) != HAL_OK) {
      Error_Handler();
    }
  }

  // Config filter
  CAN_FilterTypeDef can_filter;
  can_filter.FilterActivation = CAN_FILTER_ENABLE;
  can_filter.FilterBank = 0;
  can_filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
  /* Message ID is in the MSBs of the FilterId register */
  can_filter.FilterIdHigh = CAN_ID_CLOCK_SYNC << (16 - 11);
  can_filter.FilterIdLow = 0;
  /* Filter the 11 MSBs (i.e. a StdId) */
  can_filter.FilterMaskIdHigh = 0xFFE0;
  can_filter.FilterMaskIdLow = 0;
  can_filter.FilterMode = CAN_FILTERMODE_IDMASK;
  can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
  // If we use CAN1, the slave filter should start after our filter bank. If we
  // use CAN2, it should start at our filter bank.
  if (ams_can_handle == ams_handle) {
    can_filter.SlaveStartFilterBank = 14;
  } else {
    can_filter.SlaveStartFilterBank = 0;
  }
  if (HAL_CAN_ConfigFilter(ams_can_handle, &can_filter) != HAL_OK) {
    Error_Handler();
  }
  can_filter.FilterBank++;
  can_filter.FilterIdHigh = CAN_ID_MASTER_HEARTBEAT << (16 - 11);
  can_filter.FilterIdLow = 0;
  if (HAL_CAN_ConfigFilter(ams_can_handle, &can_filter) != HAL_OK) {
    Error_Handler();
  }

  // Activate RX notifications
  if (HAL_CAN_ActivateNotification(ams_can_handle,
                                   CAN_IT_RX_FIFO0_MSG_PENDING) != 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 == ams_can_handle) {
    ams_can_handle_ams_msg(&header, data);
  } else {
    Error_Handler();
  }
}

void ams_can_handle_ams_msg(CAN_RxHeaderTypeDef* header, uint8_t* data) {
  if (header->IDE != CAN_ID_STD) {
    return;
  }

  switch (header->StdId) {
  case CAN_ID_CLOCK_SYNC:
    clock_sync_handle_clock_sync_frame(data[0]);
    break;
  case CAN_ID_MASTER_HEARTBEAT:
    clock_sync_handle_master_heartbeat();
    break;
  }
}

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

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

  // Send voltages
  for (int msg_id = 0; msg_id < 3; msg_id++) {
    header.StdId = CAN_ID_AMS_SLAVE_HEARTBEAT_BASE | (slave_id << 4) | msg_id;
    for (int i = 0; i < 4; i++) {
      int cell = msg_id * 4 + i;
      uint16_t v = (cell < N_CELLS) ? cell_voltages[cell] : 0;
      data[2 * i + 0] = v & 0xFF;
      data[2 * i + 1] = v >> 8;
    }
    if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1,
                                        CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
      uint32_t mailbox;
      HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
    }
  }

  // Send temperatures
  for (int temp_msg_id = 0; temp_msg_id < 8; temp_msg_id++) {
    int msg_id = temp_msg_id + 3;
    header.StdId = CAN_ID_AMS_SLAVE_HEARTBEAT_BASE | (slave_id << 4) | msg_id;
    for (int i = 0; i < 4; i++) {
      int sensor = temp_msg_id * 4 + i;
      uint16_t temp = temperatures[sensor];
      data[2 * i + 0] = temp & 0xFF;
      data[2 * i + 1] = temp >> 8;
    }
    if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1,
                                        CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
      uint32_t mailbox;
      HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
    }
  }
}

void ams_can_send_error(AMS_ErrorCode error_code,
                        uint32_t transmission_timeout) {
  static CAN_TxHeaderTypeDef header;
  header.IDE = CAN_ID_STD;
  header.DLC = 8;
  header.RTR = CAN_RTR_DATA;
  header.TransmitGlobalTime = DISABLE;
  header.StdId = CAN_ID_SLAVE_ERROR;

  static uint8_t data[8];
  data[0] = slave_id;
  data[1] = error_code;

  HAL_CAN_AbortTxRequest(ams_can_handle,
                         CAN_TX_MAILBOX0 | CAN_TX_MAILBOX1 | CAN_TX_MAILBOX2);
  uint32_t mailbox;
  HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
  ams_can_wait_for_free_mailboxes(ams_can_handle, 3, transmission_timeout);
}

HAL_StatusTypeDef ams_can_wait_for_free_mailboxes(CAN_HandleTypeDef* handle,
                                                  int num_mailboxes,
                                                  uint32_t timeout) {
  uint32_t end = HAL_GetTick() + timeout;
  while (HAL_GetTick() < end) {
    if (HAL_CAN_GetTxMailboxesFreeLevel(handle) >= num_mailboxes) {
      return HAL_OK;
    }
  }
  return HAL_TIMEOUT;
}