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

#include "AMS_CAN.h"

#include "BQ_Abstraction_Layer.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>

static CAN_HandleTypeDef* handle_ams;

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

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

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

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

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(handle_ams, 1,
                                        CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
      uint32_t mailbox;
      HAL_CAN_AddTxMessage(handle_ams, &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(handle_ams, 1,
                                        CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
      uint32_t mailbox;
      HAL_CAN_AddTxMessage(handle_ams, &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(handle_ams,
                         CAN_TX_MAILBOX0 | CAN_TX_MAILBOX1 | CAN_TX_MAILBOX2);
  uint32_t mailbox;
  HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox);
  ams_can_wait_for_free_mailboxes(handle_ams, 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;
}