/*
 * CAN_Communication.c
 *
 *  Created on: Apr 26, 2022
 *      Author: max
 */

#include "CAN_Communication.h"

#include "Error_Check.h"

#include "stm32g4xx_hal_fdcan.h"

// 3 should be programmed with CAN id 2
const uint16_t slave_CAN_id_to_slave_index[7] = {
    0, 1, 2, 3, 4, 5, 255}; // TODO: Make this pretty pls
canFrame framebuffer[CANFRAMEBUFFERSIZE] = {0};
uint32_t framebufferwritepointer;
uint32_t framebufferreadpointer;

void CAN_Init(FDCAN_HandleTypeDef* hcan) {
  HAL_FDCAN_Stop(hcan);

  framebufferreadpointer = 0;
  framebufferwritepointer = 0;

  FDCAN_FilterTypeDef fdfilter = {0};

  fdfilter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
  fdfilter.FilterID1 = 0x000; // ID
  fdfilter.FilterID2 = 0x000; // Mask
  fdfilter.FilterIndex = 0;

  fdfilter.FilterType = FDCAN_FILTER_MASK;
  fdfilter.IdType = FDCAN_STANDARD_ID;

  HAL_FDCAN_ConfigFilter(hcan, &fdfilter);

  HAL_StatusTypeDef status = HAL_FDCAN_Start(hcan);

  if (status) {
    return;
  }

  status =
      HAL_FDCAN_ActivateNotification(hcan, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
}

uint8_t CAN_Receive(FDCAN_HandleTypeDef* hcan) {
  while (framebufferreadpointer != framebufferwritepointer) {
    framebufferreadpointer++;

    if (framebufferreadpointer >= CANFRAMEBUFFERSIZE) {
      framebufferreadpointer = 0;
    }

    canFrame rxFrame = framebuffer[framebufferreadpointer];

    if ((rxFrame.FrameID & SLAVE_STATUS_BASE_ADDRESS) ==
        SLAVE_STATUS_BASE_ADDRESS) {
      uint16_t msg = rxFrame.FrameID - SLAVE_STATUS_BASE_ADDRESS;
      uint8_t slaveID = (msg & 0x0F0) >> 4;
      slaveID = slave_CAN_id_to_slave_index[slaveID];
      uint8_t messageID = msg & 0x00F;
      updateSlaveInfo(slaveID, messageID, rxFrame);
    } else if (rxFrame.FrameID == SLAVE_EMERGENCY_ADDRESS) {
      AMSErrorHandle errorframe = {0};
      errorframe.errorcode = SlavesErrorFrameError;
      errorframe.errorarg[0] = rxFrame.data[0];
      errorframe.errorarg[1] = rxFrame.data[1];
      errorframe.errorarg[2] = rxFrame.data[2];
      errorframe.errorarg[3] = rxFrame.data[3];
      errorframe.errorarg[4] = rxFrame.data[4];
      errorframe.errorarg[5] = rxFrame.data[5];
      errorframe.errorarg[6] = rxFrame.data[6];
      errorframe.errorarg[7] = rxFrame.data[7];

      AMS_Error_Handler(errorframe);
    }
  }

  return 0;
}

uint8_t CAN_Transmit(FDCAN_HandleTypeDef* hcan, uint16_t frameid,
                     uint8_t* buffer, uint8_t datalen) {
  FDCAN_TxHeaderTypeDef txheader = {0};

  txheader.Identifier = frameid;
  txheader.IdType = FDCAN_STANDARD_ID;
  txheader.TxFrameType = FDCAN_FRAME_CLASSIC;
  txheader.DataLength = ((uint32_t)datalen) << 16;
  txheader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
  txheader.BitRateSwitch = FDCAN_BRS_OFF;
  txheader.FDFormat = FDCAN_CLASSIC_CAN;
  txheader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
  txheader.MessageMarker = 0;

  HAL_FDCAN_AddMessageToTxFifoQ(hcan, &txheader, buffer);

  return 0;
}
void HAL_FDCAN_ErrorCallback(FDCAN_HandleTypeDef* hcan) {}

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef* handle,
                               uint32_t interrupt_flags) {

  FDCAN_RxHeaderTypeDef rxFrameHeader;
  uint8_t data[8];
  framebufferwritepointer++;

  if (framebufferwritepointer >= CANFRAMEBUFFERSIZE) {
    framebufferwritepointer = 0;
  }

  if (!(interrupt_flags & FDCAN_IT_RX_FIFO0_NEW_MESSAGE)) {
    return;
  }

  if (HAL_FDCAN_GetRxMessage(handle, FDCAN_RX_FIFO0, &rxFrameHeader, data) !=
      HAL_OK) {
    framebuffer[framebufferwritepointer].error = 1;
  } else {
    framebuffer[framebufferwritepointer].error = 0;
  }

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

  framebuffer[framebufferwritepointer].FrameID =
      (int16_t)rxFrameHeader.Identifier;
  framebuffer[framebufferwritepointer].length =
      (uint8_t)rxFrameHeader.DataLength >> 16;

  for (int i = 0; i < (rxFrameHeader.DataLength >> 16); i++) {
    framebuffer[framebufferwritepointer].data[i] = data[i];
  }

  framebuffer[framebufferwritepointer].timestamp = HAL_GetTick();
}

void updateSlaveInfo(uint8_t slaveID, uint8_t MessageID, canFrame rxFrame) {
  if (slaveID < NUMBEROFSLAVES) {
    switch (MessageID) {
    case 0x00:
      slaves[slaveID].cellVoltages[0] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellVoltages[1] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellVoltages[2] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellVoltages[3] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    case 0x01:
      slaves[slaveID].cellVoltages[4] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellVoltages[5] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellVoltages[6] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellVoltages[7] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    case 0x02:
      slaves[slaveID].cellVoltages[8] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellVoltages[9] = rxFrame.data[2] | rxFrame.data[3] << 8;
      break;
    case 0x03:
      slaves[slaveID].cellTemps[0] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellTemps[1] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellTemps[2] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellTemps[3] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    case 0x04:
      slaves[slaveID].cellTemps[4] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellTemps[5] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellTemps[6] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellTemps[7] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    case 0x05:
      slaves[slaveID].cellTemps[8] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellTemps[9] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellTemps[10] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellTemps[11] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    case 0x06:
      slaves[slaveID].cellTemps[12] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellTemps[13] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellTemps[14] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellTemps[15] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    case 0x07:
      slaves[slaveID].cellTemps[16] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellTemps[17] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellTemps[18] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellTemps[19] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    case 0x08:
      slaves[slaveID].cellTemps[20] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellTemps[21] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellTemps[22] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellTemps[23] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    case 0x09:
      slaves[slaveID].cellTemps[24] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellTemps[25] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellTemps[26] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellTemps[27] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    case 0x0A:
      slaves[slaveID].cellTemps[28] = rxFrame.data[0] | rxFrame.data[1] << 8;
      slaves[slaveID].cellTemps[29] = rxFrame.data[2] | rxFrame.data[3] << 8;
      slaves[slaveID].cellTemps[30] = rxFrame.data[4] | rxFrame.data[5] << 8;
      slaves[slaveID].cellTemps[31] = rxFrame.data[6] | rxFrame.data[7] << 8;
      break;
    }
    slaves[slaveID].timestamp = rxFrame.timestamp;
  }
}