// Standard includes
#include <string.h>

// TI C2000/CGT includes
#include "board.h"
#include "mcan.h"
#include "sysctl.h"

void CAN_localInit(void) {
  SysCtl_setMCANClk(SYSCTL_MCANA, SYSCTL_MCANCLK_DIV_5);
}

uint32_t CAN_readIdField(const uint32_t &id) {
  uint32_t decodedId = 0U;
  if (((id >> 30U) & 0b1U) == 0U) {
    // 11-bit ID, stored in 28:18
    decodedId = (id >> 18U) & 0x7FFU;
  } else {
    // 29-bit ID, stored in 28:0
    decodedId = (id & 0x1FFFFFFFU);
  }
  return decodedId;
}

uint32_t CAN_setIdField(const uint32_t id) { return (id & 0x7FFU) << 18U; }

void CAN_sendMessage(const uint32_t canId, const uint16_t msgData[8U],
                     const uint16_t msgLen) {
  MCAN_TxBufElement txMsg;
  memset(&txMsg, 0, sizeof(txMsg)); // Zero out the struct
  txMsg.id = CAN_setIdField(canId);
  txMsg.dlc = msgLen;
  memcpy(txMsg.data, msgData, msgLen);
  MCAN_writeMsgRam(MCAN0_BASE, MCAN_MEM_TYPE_FIFO, 0U, &txMsg);
  MCAN_TxFIFOStatus txfifoStatus;
  MCAN_getTxFIFOQueStatus(MCAN0_BASE, &txfifoStatus);

  const int32_t state = MCAN_txBufAddReq(MCAN0_BASE, txfifoStatus.putIdx);
}

bool CAN_readMessageNonBlocking(uint32_t &canId, uint16_t msgData[8U],
                                uint16_t &msgLen) {
  const uint32_t mcanBase = MCAN0_BASE;
  const MCAN_RxFIFONum fifoNumber = MCAN_RX_FIFO_NUM_0;

  canId = 0U;
  msgLen = 0U;

  MCAN_RxFIFOStatus fifoStatus = {0};
  MCAN_getRxFIFOStatus(mcanBase, &fifoStatus);
    MCAN_RxNewDataStatus newData;

  //  Check whether there are actually any messages to read
  if (fifoStatus.fillLvl != 0) {
    uint16_t getIndex = fifoStatus.getIdx;

    MCAN_RxBufElement rxMsg;
    // Read the message from the CAN RX fifo
    MCAN_readMsgRam(mcanBase, MCAN_MEM_TYPE_FIFO, 0U, fifoNumber, &rxMsg);
    // Increment the fifo get head in the HW
    MCAN_writeRxFIFOAck(mcanBase, fifoNumber, getIndex);
    MCAN_clearNewDataStatus(MCANA_DRIVER_BASE, &newData);

    
    if (rxMsg.dlc <= 8U) {
      canId = CAN_readIdField(rxMsg.id);
      msgLen = rxMsg.dlc;
      memcpy(msgData, rxMsg.data, msgLen);
    } else {
      ; // Error, message is too large
      msgLen = 0U;
    }
  } else {
    // No data to get
    msgLen = 0U;
  }

  bool received = false;
  if (msgLen != 0U) {
    received = true;
  } else {
    received = false;
  }
  return received;
}



bool CAN_ISR0(void) {
  uint32_t intrStatus;

  intrStatus = MCAN_getIntrStatus(MCAN0_BASE);

  bool state = false;

  //
  // Clear the interrupt Status.
  //
  MCAN_clearIntrStatus(MCAN0_BASE, intrStatus);

  //
  //  Clearing the interrupt lineNum
  //
  MCAN_clearInterrupt(MCAN0_BASE, 1);

  //
  //  Check to see if the interrupt is caused by a message being
  //  received in dedicated RX Buffers
  //
  if ((MCAN_INTR_SRC_RX_FIFO0_NEW_MSG & intrStatus) ==
      MCAN_INTR_SRC_RX_FIFO0_NEW_MSG) {

    uint32_t id;
    uint16_t dlc;
    uint16_t data[8];

    const bool hasData = CAN_readMessageNonBlocking(id, data, dlc);
    if ((hasData == true) && (dlc > 0U) && (id == 0x321)) {
      state = static_cast<bool>(0b1 & data[0]);
    }
  }

  // Acknowledge this interrupt located in group 9
  Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP9);
  return state;
}