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

#include "CAN_Communication.h"

#include "AIR_State_Maschine.h"
#include "Check_Shunt_Limits.h"
#include "Error_Check.h"
#include "SoC_Estimation.h"

#include "stm32g4xx_hal_fdcan.h"

#include <stdint.h>
#include <string.h>

const uint16_t slave_CAN_id_to_slave_index[10] = {
    0, 1, 2, 3, 4, 5, 6, 7, 255, 8}; // TODO: Make this pretty pls
canFrame framebuffer[CANFRAMEBUFFERSIZE] = {0};
uint32_t framebufferwritepointer = 0;
uint32_t framebufferreadpointer = 0;
uint32_t frames_read = 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; // 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) {
  frames_read = 0;
  while (framebufferreadpointer != framebufferwritepointer) {
    framebufferreadpointer++;

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

    canFrame* rxFrame = &framebuffer[framebufferreadpointer];
    frames_read++;

    switch (rxFrame->FrameID) {
    case CAN_ID_SLAVE_EMERGENCY:
      CAN_HandleSlaveEmergency(rxFrame);
      break;
    default:
      if ((rxFrame->FrameID & CAN_MASK_SLAVE_STATUS) ==
          CAN_ID_SLAVE_STATUS_BASE) {
        CAN_HandleSlaveStatus(rxFrame);
      } else if ((rxFrame->FrameID & CAN_MASK_SHUNT) == CAN_ID_SHUNT_BASE) {
        CAN_HandleShuntMsg(rxFrame);
      }
      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;

  HAL_FDCAN_AddMessageToTxFifoQ(hcan, &txheader, buffer);

  return 0;
}

void CAN_SendAbxStatus(FDCAN_HandleTypeDef* hcan) {
  uint8_t buffer[4];
  buffer[0] = airstate.currentTSState | (1 << 7);
  buffer[1] = current_soc;
  buffer[2] = (uint8_t)(min_voltage >> 8);
  buffer[3] = (int8_t)(max_temp >> 4);
  CAN_Transmit(hcan, CAN_ID_AMS_STATUS, buffer, 4);
}

void CAN_SendAMSPanic(FDCAN_HandleTypeDef* hcan, AMSErrorHandle* error) {
  uint8_t buffer[8];
  buffer[0] = error->errorcode;
  memcpy(&buffer[1], error->errorarg, 7);
  CAN_Transmit(hcan, CAN_ID_AMS_PANIC, buffer, 8);
}

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 CAN_HandleSlaveStatus(canFrame* rxFrame) {
  uint16_t msg = rxFrame->FrameID - CAN_ID_SLAVE_STATUS_BASE;
  uint8_t slaveID = (msg & 0x0F0) >> 4;
  slaveID = slave_CAN_id_to_slave_index[slaveID];
  uint8_t messageID = msg & 0x00F;

  if (slaveID < N_SLAVES) {
    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;
    slaves[slaveID].frame_timestamps[messageID] = rxFrame->timestamp;
  }
}

void CAN_HandleSlaveEmergency(canFrame* rxFrame) {
  uint8_t slave_id = rxFrame->data[0];
  slaves[slave_id].error = 1;
  memcpy(slaves[slave_id].error_frame, rxFrame->data, 8);
}

void CAN_HandleShuntMsg(canFrame* rxFrame) {
  switch (rxFrame->FrameID) {
  case CAN_ID_SHUNT_CURRENT:
    shunt_data.current = (rxFrame->data[2] << 24) | (rxFrame->data[3] << 16) |
                         (rxFrame->data[4] << 8) | (rxFrame->data[5]);
    airstate.shuntCurrent = shunt_data.current;
    break;
  case CAN_ID_SHUNT_VOLTAGE_1:
    shunt_data.voltage1 = (rxFrame->data[2] << 24) | (rxFrame->data[3] << 16) |
                          (rxFrame->data[4] << 8) | (rxFrame->data[5]);
    break;
  case CAN_ID_SHUNT_VOLTAGE_2:
    shunt_data.voltage2 = (rxFrame->data[2] << 24) | (rxFrame->data[3] << 16) |
                          (rxFrame->data[4] << 8) | (rxFrame->data[5]);
    airstate.BatteryVoltageVehicleSide = shunt_data.voltage2;
    break;
  case CAN_ID_SHUNT_VOLTAGE_3:
    shunt_data.voltage3 = (rxFrame->data[2] << 24) | (rxFrame->data[3] << 16) |
                          (rxFrame->data[4] << 8) | (rxFrame->data[5]);
    airstate.BatteryVoltageBatterySide = shunt_data.voltage3;
    break;
  case CAN_ID_SHUNT_BUSBAR_TEMP:
    shunt_data.busbartemp = (rxFrame->data[2] << 24) |
                            (rxFrame->data[3] << 16) | (rxFrame->data[4] << 8) |
                            (rxFrame->data[5]);
    break;
  case CAN_ID_SHUNT_POWER:
    shunt_data.power = (rxFrame->data[2] << 24) | (rxFrame->data[3] << 16) |
                       (rxFrame->data[4] << 8) | (rxFrame->data[5]);
    break;
  case CAN_ID_SHUNT_ENERGY:
    shunt_data.energy = (rxFrame->data[2] << 24) | (rxFrame->data[3] << 16) |
                        (rxFrame->data[4] << 8) | (rxFrame->data[5]);
    break;
  case CAN_ID_SHUNT_AMPERE_SECONDS:
    shunt_data.ampere_seconds = (rxFrame->data[2] << 24) |
                                (rxFrame->data[3] << 16) |
                                (rxFrame->data[4] << 8) | (rxFrame->data[5]);
    break;
  }
  shunt_data.last_message = framebuffer[framebufferreadpointer].timestamp;
}