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

#include "CAN_Communication.h"

#include "SoC_Estimation.h"

#include "stm32g4xx_hal_fdcan.h"

canFrame framebuffer[CANFRAMEBUFFERSIZE] = {0};
uint8_t framebufferwritepointer;
uint8_t framebufferreadpointer;

int32_t shuntcurrent = 0;
int32_t shuntvoltage1 = 0;
int32_t shuntvoltage2 = 0;
int32_t shuntvoltage3 = 0;
int32_t shuntbusbartemp = 0;
int32_t shuntpower = 0;
int32_t shuntampereseconds = 0;
int32_t shuntenergy = 0;

uint32_t shuntlastmessage = 0;

uint8_t currentlap = 0;
uint8_t TSTargetState = 0;

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; // Range start
  fdfilter.FilterID2 = 0x000; // Range stop
  fdfilter.FilterIndex = 0;

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

  HAL_FDCAN_ConfigFilter(hcan, &fdfilter);

  HAL_FDCAN_Start(hcan);

  HAL_FDCAN_ActivateNotification(hcan, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
  // hcan->Instance->CCCR |= FDCAN_CCCR_ASM;
}

uint8_t CAN_Receive(FDCAN_HandleTypeDef* hcan) {
  uint32_t ecount = hcan->Instance->ECR;
  while (framebufferreadpointer != framebufferwritepointer) {
    framebufferreadpointer++;

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

    canFrame rxFrame = framebuffer[framebufferreadpointer];

    switch (rxFrame.FrameID) {
    case SHUNT_CURRENT:
      shuntcurrent = (rxFrame.data[2] << 24) | (rxFrame.data[3] << 16) |
                     (rxFrame.data[4] << 8) | (rxFrame.data[5]);
      shuntlastmessage = framebuffer[framebufferreadpointer].timestamp;
      break;
    case SHUNT_VOLTAGE_1:
      shuntvoltage1 = (rxFrame.data[2] << 24) | (rxFrame.data[3] << 16) |
                      (rxFrame.data[4] << 8) | (rxFrame.data[5]);
      shuntlastmessage = framebuffer[framebufferreadpointer].timestamp;
      break;
    case SHUNT_VOLTAGE_2:
      shuntvoltage2 = (rxFrame.data[2] << 24) | (rxFrame.data[3] << 16) |
                      (rxFrame.data[4] << 8) | (rxFrame.data[5]);
      shuntlastmessage = framebuffer[framebufferreadpointer].timestamp;
      break;
    case SHUNT_VOLTAGE_3:
      shuntvoltage3 = (rxFrame.data[2] << 24) | (rxFrame.data[3] << 16) |
                      (rxFrame.data[4] << 8) | (rxFrame.data[5]);
      shuntlastmessage = framebuffer[framebufferreadpointer].timestamp;
      break;
    case SHUNT_BUSBAR_TEMP:
      shuntbusbartemp = (rxFrame.data[2] << 24) | (rxFrame.data[3] << 16) |
                        (rxFrame.data[4] << 8) | (rxFrame.data[5]);
      shuntlastmessage = framebuffer[framebufferreadpointer].timestamp;
      break;
    case SHUNT_POWER:
      shuntpower = (rxFrame.data[2] << 24) | (rxFrame.data[3] << 16) |
                   (rxFrame.data[4] << 8) | (rxFrame.data[5]);
      shuntlastmessage = framebuffer[framebufferreadpointer].timestamp;
      break;
    case SHUNT_ENERGY:
      shuntenergy = (rxFrame.data[2] << 24) | (rxFrame.data[3] << 16) |
                    (rxFrame.data[4] << 8) | (rxFrame.data[5]);
      shuntlastmessage = framebuffer[framebufferreadpointer].timestamp;
      break;
    case SHUNT_AMPERE_SECONDS:
      shuntampereseconds = (rxFrame.data[2] << 24) | (rxFrame.data[3] << 16) |
                           (rxFrame.data[4] << 8) | (rxFrame.data[5]);
      shuntlastmessage = framebuffer[framebufferreadpointer].timestamp;
      break;

    case AUTOBOX_INFO:
      currentlap = rxFrame.data[0] >> 2;
      TSTargetState = rxFrame.data[0] & 0x01;
      break;
    }
  }

  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;
  if (HAL_FDCAN_GetTxFifoFreeLevel(hcan) > 0) {
    HAL_FDCAN_AddMessageToTxFifoQ(hcan, &txheader, buffer);
    return 0;
  }

  return 1;
}

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 < framebuffer[framebufferwritepointer].length; i++) {
    framebuffer[framebufferwritepointer].data[i] = data[i];
  }

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

void HAL_FDCAN_ErrorCallback(FDCAN_HandleTypeDef* hcan) {}

void CAN_SendAbxStatus(FDCAN_HandleTypeDef* hcan) {
  uint8_t buffer[4];
  buffer[0] = ctrltsstate.currentTSState | (1 << 7);
  buffer[1] = 160;
  buffer[2] = (uint8_t)(shuntvoltage1 / 2000);
  buffer[3] = current_soc;
  CAN_Transmit(hcan, AMS_STATUS_ID, buffer, 4);
}

void CAN_SendAMSPanic(FDCAN_HandleTypeDef* hcan) {
  uint8_t buffer[8];
  buffer[0] = errorflags.errorcode;
  buffer[1] = errorflags.errorargs[0];
  buffer[2] = 0;
  buffer[3] = 0;
  buffer[4] = 0;
  buffer[5] = 0;
  buffer[6] = 0;
  buffer[7] = 0;
  CAN_Transmit(hcan, AMS_PANIC_ID, buffer, 8);
}

uint8_t slavelognum = 0;
uint8_t framelognum = 0;

void CAN_SendLoggingFrame(FDCAN_HandleTypeDef* hcan) {

  uint8_t buffer[8];

  buffer[0] = ((slavelognum << 4) | framelognum);

  framelognum++;
  if ((framelognum > 6)) {
    framelognum = 0;
    slavelognum++;
  }
  if ((slavelognum > NUMBEROFSLAVES)) {
    slavelognum = 0;
    framelognum = 0;
  }

  switch (framelognum) {
  case 0:
    buffer[1] = CAN_convert_logval(slaves[slavelognum].cellVoltages[0],
                                   BATTERY_VOLTAGE_TYPE);
    buffer[2] = CAN_convert_logval(slaves[slavelognum].cellVoltages[1],
                                   BATTERY_VOLTAGE_TYPE);
    buffer[3] = CAN_convert_logval(slaves[slavelognum].cellVoltages[2],
                                   BATTERY_VOLTAGE_TYPE);
    buffer[4] = CAN_convert_logval(slaves[slavelognum].cellVoltages[3],
                                   BATTERY_VOLTAGE_TYPE);
    buffer[5] = CAN_convert_logval(slaves[slavelognum].cellVoltages[4],
                                   BATTERY_VOLTAGE_TYPE);
    buffer[6] = CAN_convert_logval(slaves[slavelognum].cellVoltages[5],
                                   BATTERY_VOLTAGE_TYPE);
    buffer[7] = CAN_convert_logval(slaves[slavelognum].cellVoltages[6],
                                   BATTERY_VOLTAGE_TYPE);
    break;

  case 1:
    buffer[1] = CAN_convert_logval(slaves[slavelognum].cellVoltages[7],
                                   BATTERY_VOLTAGE_TYPE);
    buffer[2] = CAN_convert_logval(slaves[slavelognum].cellVoltages[8],
                                   BATTERY_VOLTAGE_TYPE);
    buffer[3] = CAN_convert_logval(slaves[slavelognum].cellVoltages[9],
                                   BATTERY_VOLTAGE_TYPE);
    buffer[4] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[0], BATTERY_TEMP_TYPE);
    buffer[5] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[1], BATTERY_TEMP_TYPE);
    buffer[6] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[2], BATTERY_TEMP_TYPE);
    buffer[7] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[3], BATTERY_TEMP_TYPE);
    break;

  case 2:
    buffer[1] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[4], BATTERY_TEMP_TYPE);
    buffer[2] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[5], BATTERY_TEMP_TYPE);
    buffer[3] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[6], BATTERY_TEMP_TYPE);
    buffer[4] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[7], BATTERY_TEMP_TYPE);
    buffer[5] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[8], BATTERY_TEMP_TYPE);
    buffer[6] =
        CAN_convert_logval(slaves[slavelognum].cellTemps[9], BATTERY_TEMP_TYPE);
    buffer[7] = CAN_convert_logval(slaves[slavelognum].cellTemps[10],
                                   BATTERY_TEMP_TYPE);
    break;

  case 3:
    buffer[1] = CAN_convert_logval(slaves[slavelognum].cellTemps[11],
                                   BATTERY_TEMP_TYPE);
    buffer[2] = CAN_convert_logval(slaves[slavelognum].cellTemps[12],
                                   BATTERY_TEMP_TYPE);
    buffer[3] = CAN_convert_logval(slaves[slavelognum].cellTemps[13],
                                   BATTERY_TEMP_TYPE);
    buffer[4] = CAN_convert_logval(slaves[slavelognum].cellTemps[14],
                                   BATTERY_TEMP_TYPE);
    buffer[5] = CAN_convert_logval(slaves[slavelognum].cellTemps[15],
                                   BATTERY_TEMP_TYPE);
    buffer[6] = CAN_convert_logval(slaves[slavelognum].cellTemps[16],
                                   BATTERY_TEMP_TYPE);
    buffer[7] = CAN_convert_logval(slaves[slavelognum].cellTemps[17],
                                   BATTERY_TEMP_TYPE);
    break;

  case 4:
    buffer[1] = CAN_convert_logval(slaves[slavelognum].cellTemps[18],
                                   BATTERY_TEMP_TYPE);
    buffer[2] = CAN_convert_logval(slaves[slavelognum].cellTemps[19],
                                   BATTERY_TEMP_TYPE);
    buffer[3] = CAN_convert_logval(slaves[slavelognum].cellTemps[20],
                                   BATTERY_TEMP_TYPE);
    buffer[4] = CAN_convert_logval(slaves[slavelognum].cellTemps[21],
                                   BATTERY_TEMP_TYPE);
    buffer[5] = CAN_convert_logval(slaves[slavelognum].cellTemps[22],
                                   BATTERY_TEMP_TYPE);
    buffer[6] = CAN_convert_logval(slaves[slavelognum].cellTemps[23],
                                   BATTERY_TEMP_TYPE);
    buffer[7] = CAN_convert_logval(slaves[slavelognum].cellTemps[24],
                                   BATTERY_TEMP_TYPE);
    break;

  case 5:
    buffer[1] = CAN_convert_logval(slaves[slavelognum].cellTemps[25],
                                   BATTERY_TEMP_TYPE);
    buffer[2] = CAN_convert_logval(slaves[slavelognum].cellTemps[26],
                                   BATTERY_TEMP_TYPE);
    buffer[3] = CAN_convert_logval(slaves[slavelognum].cellTemps[27],
                                   BATTERY_TEMP_TYPE);
    buffer[4] = CAN_convert_logval(slaves[slavelognum].cellTemps[28],
                                   BATTERY_TEMP_TYPE);
    buffer[5] = CAN_convert_logval(slaves[slavelognum].cellTemps[29],
                                   BATTERY_TEMP_TYPE);
    buffer[6] = CAN_convert_logval(slaves[slavelognum].cellTemps[30],
                                   BATTERY_TEMP_TYPE);
    buffer[7] = CAN_convert_logval(slaves[slavelognum].cellTemps[31],
                                   BATTERY_TEMP_TYPE);
    break;
  }

  CAN_Transmit(hcan, AMS_LOGGING_ID, buffer, 8);
}

uint8_t CAN_convert_logval(uint16_t value, uint8_t type) {
  if (type == BATTERY_VOLTAGE_TYPE) {
    return (uint8_t)value >> 8;
  } else if (type == BATTERY_TEMP_TYPE) {
    return (uint8_t)value >> 4;
  }
}