#include "includes.h"
#include <yfuns.h>

#define CAN1          0x1
#define CAN2          0x2
#define BAUD_RATE     115200
#define LPC_APB0_BASE 0x40000000UL
#define LPC_CAN1      (LPC_APB0_BASE + 0x44000)
#define AFRAM         0x40038000

#define STD_ID_FORMAT 0
#define EXT_ID_FORMAT 1

#define DATA_FRAME   0
#define REMOTE_FRAME 1

#define RECEIVING    0
#define TRANSMITTING 1

typedef struct
{
    uint32_t id;
    uint8_t dataA[4];
    uint8_t dataB[4];
    uint8_t len;
    uint8_t format;
    uint8_t type;
} CAN_MSG_Type;

CAN_MSG_Type transMessage, recMessage;

void initCAN();
void setCANBaudRate();
void testCAN();
void configCANMessage(uint32_t id, uint8_t dataA[4], uint8_t dataB[4],
                      uint8_t len, uint8_t format, uint8_t type, uint8_t dir);
void CAN_IRQHandler(void);
uint8_t checkMessage(CAN_MSG_Type* transMessage, CAN_MSG_Type* recMessage);
void receiveCANMessage(CAN_MSG_Type* recMessage);
void sendCANMessage(CAN_MSG_Type* transMessage);

/** external data **/
#pragma section=".intvec"
/** public functions **/
/*************************************************************************
 * Function Name: InitClock
 * Parameters: void
 * Return: void
 *
  * Description: Initialize PLL0 and clocks' dividers. PLL = 96MHz,
 *               CPU - 96MHz, PCLK - 48 MHz
 *
 *************************************************************************/
void InitClock(void)
{
  /*Sys Clock Select as CPU clock
    divider 1:1*/
  CLK_SetCpuClk(CPUSEL_CLKSYS,1);
  /*Select IRC oscilator as Sys clock*/
  CLK_SetSysClk(SYSSEL_IRCOSC);
  /*Enable Main oscilator*/
  CLK_MainOscSet(CLK_ENABLE,MOSCRNG_1_20MHZ);
  /*Select Main oscilator as Sys clock*/
  CLK_SetSysClk(SYSSEL_MOSC);
  /*Enable set PLL 96Hz (M = 8; P = 1)*/
  CLK_SetMainPll(CLK_ENABLE, 8-1, 0);
  /*Set peripheral divider 1:2. Peripheral clock 48MHz*/
  CLK_SetPeriphClk(2);
  /*PLL0 out is used as the input to the CPU clock divider
  divider 1:1. CPU runs at 96MHz*/
  CLK_SetCpuClk(CPUSEL_CLKPLL,1);
}

/*************************************************************************
 * Function Name: __low_level_init
 * Parameters: void
 * Return:
 *
 * Description: Low level system init (clock, flash accelerator,
 *              SDRAM and vector table address)
 *
 *
 *************************************************************************/
int __low_level_init (void)
{
  /*if debug is in SDRAM
    clock and SDRAM initialization is made
    in mac file*/
#ifndef SDRAM_DEBUG
  int cpuclk;
  /* Flash accelerator save value*/
  FLASHCFG = (6UL<<12) | 0x3AUL;
  /*Clock Init*/
  InitClock();
  /*Flash accelerator init*/
  cpuclk = CLK_GetClock(CLK_CPU);

  if( cpuclk < 20000000 ){
    FLASHCFG = (0x0UL<<12) | 0x3AUL;
  } else if( cpuclk < 40000000 ){
    FLASHCFG = (0x1UL<<12) | 0x3AUL;
  } else if( cpuclk< 60000000 ){
    FLASHCFG = (0x2UL<<12) | 0x3AUL;
  } else if( cpuclk < 80000000 ){
    FLASHCFG = (0x3UL<<12) | 0x3AUL;
  } else if( cpuclk < 100000000 ){
    FLASHCFG = (0x4UL<<12) | 0x3AUL;
  }
  /*SDRAM init*/
  SDRAM_Init();
#endif
  /*Set vector table location*/
  VTOR  = (unsigned int)__segment_begin(".intvec");

  return  1;
}

// LOW-LEVEL INITIALISATION - END



// Params: baudrate - the baud rate of the CAN peripheral.
// Returns: None
// Description: Sets the baud rate for the CAN
void setCANBaudRate(int baudrate)
{
  uint32_t result = 0;
  uint8_t NT, TSEG1, TSEG2;
  uint32_t CANPclk = 0;
  uint32_t BRP;

  CANPclk = CLK_GetClock(CLK_PERIPH);

  result = CANPclk / baudrate;

  // Calculate suitable nominal time value.
  // NT = TSEG1 + TSEG2 + 3
  // NTEG <= 24
  // TSEG1 >= TSEG2*3
  for(NT=24; NT > 0; NT -= 2)
  {
    if(!(result % NT))
    {
      BRP = result / NT - 1;
      NT--;
      TSEG2 = NT/3 - 1;
      TSEG1 = NT - NT/3 - 1;
      break;
    }
  }

  // Enter reset mode.
  CAN1MOD = 0x01;

  // Set bit timing.
  // Default: SAM = 0x00;
  //          SJW = 0x03;
  CAN1BTR = (TSEG2 << 20) | (TSEG1 << 16) | (3 << 14) | BRP;

  // Enter normal operating mode.
  CAN1MOD = 0;
}

// Params: None
// Returns: None
// Description: Initialises the CAN peripheral.
void initCAN()
{
  // Enable power and clock.
  PCONP_bit.PCAN1 = 1;

  // Configure pins.
  IOCON_P0_00 &= ~0x7; IOCON_P0_00 |= 0x1;
  IOCON_P0_01 &= ~0x7; IOCON_P0_01 |= 0x1;

  IOCON_P0_04 &= ~0x7; IOCON_P0_04 |= 0x2;
  IOCON_P0_05 &= ~0x7; IOCON_P0_05 |= 0x2;

  IOCON_P0_21 &= ~0x7; IOCON_P0_21 |= 0x4;
  IOCON_P0_22 &= ~0x7; IOCON_P0_22 |= 0x4;

  IOCON_P2_07 &= ~0x7; IOCON_P2_07 |= 0x1;
  IOCON_P0_08 &= ~0x7; IOCON_P2_08 |= 0x1;

  // Triggers reset mode - all (writable) registers can be written to.
  CAN1MOD = 1;
  CAN1IER = 0;
  CAN1GSR = 0;

  // Aborts transmissions, releases receive buffer and clears data overrun.
  CAN1CMR_bit.AT  = 1;
  CAN1CMR_bit.RRB = 1;
  CAN1CMR_bit.CDO = 1;

  // Read to clear register.
  uint16_t i = CAN1ICR;

  AFMR = 0x01;

  SFF_SA     = 0;
  SFF_GRP_SA = 0;
  EFF_SA     = 0;
  EFF_GRP_SA = 0;
  ENDOFTABLE = 0;

  AFMR  = 0;

  // Set baud rate.
  setCANBaudRate(BAUD_RATE);
}

// Params: None
// Returns: None
// Description: Performs a self-test on the CAN peripheral.
void testCAN()
{
  uint32_t *ptr;

  // Enter reset mode.
  CAN1MOD_bit.RM  = 1;

  // Enable bypassing.
  AFMR_bit.ACCBP = 1;

  // Clear acceptance filter RAM.
  for(uint32_t i=0; i < 512; i++)
  {
    ptr  = (uint32_t*)(AFRAM+i);
    *ptr = 0;
  }

  /*
  // Set up acceptance filter values; reserve space in SFF_sa for one
  // standard format group id.
  SFF_SA     = (0x00 << 1);
  SFF_GRP_SA = (0x00 << 1);
  EFF_SA     = (0x20 << 1);
  EFF_GRP_SA = (0x20 << 1);
  ENDOFTABLE = (0x20 << 1);

  ptr = (uint32_t*)AFRAM;
  // [    Valid ID 1   ] [    Valid ID 2   ]
  // 0010 0111 1111 1111 0010 0000 0000 0000
  // *ptr = 0x2EFF2000;

  // 0010 0000 0000 0000 0010 0111 1111 1111
  *ptr = 0x20002EFF;
  */

  // Enable self-testing mode.
  CAN1MOD_bit.STM = 1;
  CAN1CMR_bit.SRR = 1;

  // Re-enter normal operational mode.
  CAN1MOD_bit.RM  = 0;

  // Enable interrupts on transmitting and receiving messages.
  CAN1IER_bit.RIE      = 1;
  CAN1IER_bit.TIE1     = 1;
  CAN1IER_bit.IDIE     = 1;
  SETENA0_bit.SETENA25 = 1;

  // [(En)/Dis]able AF bypassing.
  AFMR_bit.ACCBP  = 1;
  //AFMR_bit.ACCOFF = 0;
  //AFMR_bit.EFCAN  = 1;


  uint8_t dataA[] = { 0x12, 0x12, 0x12, 0x12 };
  uint8_t dataB[] = { 0x34, 0x34, 0x34, 0x34 };
  uint8_t dataZ[] = { 0, 0, 0, 0};
  configCANMessage(0xEFF, dataA, dataB, 8, STD_ID_FORMAT, DATA_FRAME,
                   TRANSMITTING);

  configCANMessage(0, dataZ, dataZ, 0, 0, 0, RECEIVING);

  printf("Sending message...\n");

  sendCANMessage(&transMessage);
}

// Params: id       - the id of the message (if format = STD_ID_FORMAT, then id
//                    should be 11 bits long, else if format = EXT_ID_FORMAT,
//                    then id should be 29 bits long).
//         dataA[4] - data field A.
//         dataB[4] - data field B.
//         len      - the length of the data field in bytes
//                    (0000b - 0111b: 0-7 bytes, 1xxxb - 8 bytes).
//         format   - the format of the id
//                    (STD_ID_FORMAT - 11 bit format,
//                    EXT_ID_FORMAT - 29 bit format).
//         type     - remote transmission request
//                  - (DATA_FRAME - the number of data bytes called out by the
//                    DLC field are sent from the CANxTDA and CANxTDB
//                    registers,
//                    REMOTE FRAME - a remote frame is sent).
//         dir      - specifies if the message format is for messages being
//                    sent or received (0 - receiving, 1 - transmitting).
// Returns: None
// Description: Configures the format for transmitting and receiving messages.
void configCANMessage(uint32_t id, uint8_t dataA[4], uint8_t dataB[4],
                               uint8_t len, uint8_t format, uint8_t type,
                               uint8_t dir)
{
  if(dir)
  {
    transMessage.id     = id;

    for(int i=0; i < 4; i++)
    {
      transMessage.dataA[i]  = dataA[i];
      transMessage.dataB[i]  = dataB[i];
    }

    transMessage.len    = len;
    transMessage.format = format;
    transMessage.type   = type;
  }
  else
  {
    recMessage.id     = id;

    for(int i=0; i < 4; i++)
    {
      recMessage.dataA[i]  = dataA[i];
      recMessage.dataB[i]  = dataB[i];
    }

    recMessage.len    = len;
    recMessage.format = format;
    recMessage.type   = type;
  }
}

// Params: None
// Returns: None
// Description: Handles messages received over CAN.
void CAN_IRQHandler(void)
{
  printf("IRQ\n");

  /*
  // Check receive buffer status.
  if(CAN1GSR_bit.RBS)
  {
    printf("You have mail\n");
  }
  else
  {
    printf("You don't have mail\n");
  }
  */
  CAN1ICR_bit;

  if(CAN1SR_bit.RBS)
  {
    receiveCANMessage(&recMessage);

    // Validate received and transmitted information.
    if(checkMessage(&transMessage, &recMessage))
    {
      printf("Self test is successful\n");
    }
    else
    {
      printf("Self test is not successful\n");
    }
  }
}

// Params: transMessage - the message being transmitted.
//         recMessage - the message being received.
// Returns: Whether the two messages are the same (0 - false, 1 - true).
// Description: Compares the two messages to each other and determines if
//              they are identical
uint8_t checkMessage(CAN_MSG_Type* transMessage, CAN_MSG_Type* recMessage)
{
  if(transMessage->format != recMessage->format) return FALSE;
  if(transMessage->id != recMessage->id) return FALSE;
  if(transMessage->len != recMessage->len) return FALSE;
  if(transMessage->type != recMessage->type) return FALSE;

  for(int i=0; i < 4; i++)
  {
    if(transMessage->dataA[i] != recMessage->dataA[i]
       || transMessage->dataB[i] != recMessage->dataB[i]) return FALSE;
  }

  return TRUE;
}

// Params: recMessage - a message loaded with the information of the incoming
//         data.
// Returns: None
// Description: loads data received over CAN into a message format.
void receiveCANMessage(CAN_MSG_Type* recMessage)
{
  uint32_t data;

  // Check status of receive buffer.
  if(CAN1SR_bit.RBS)
  {
    recMessage->format = CAN1RFS_bit.FF;
    recMessage->type   = CAN1RFS_bit.RTR;
    recMessage->len    = CAN1RFS_bit.DLC;
    recMessage->id     = CAN1RID;

    if(recMessage->type == DATA_FRAME)
    {
      // Read first four bytes of data.
      data = CAN1RDA;

      recMessage->dataA[0] = (data & 0x000000FF) >> 0;
      recMessage->dataA[1] = (data & 0x0000FF00) >> 8;
      recMessage->dataA[2] = (data & 0x00FF0000) >> 16;
      recMessage->dataA[3] = (data & 0xFF000000) >> 24;

      // Read second four bytes of data.
      data = CAN1RDB;

      recMessage->dataB[0] = (data & 0x000000FF) >> 0;
      recMessage->dataB[1] = (data & 0x0000FF00) >> 8;
      recMessage->dataB[2] = (data & 0x00FF0000) >> 16;
      recMessage->dataB[3] = (data & 0xFF000000) >> 24;

      // Release receive buffer.
      CAN1CMR_bit.RRB = 1;
    }
    // If the received frame is just a Remote Frame, then we do not have data
    // and we release the receive buffer.
    else
    {
      CAN1CMR_bit.RRB = 1;
    }
  }
}

// Params: transMessage - the message to be transmitted.
// Returns: None
// Description: Prepares a message to be transmitted over CAN.
void sendCANMessage(CAN_MSG_Type* transMessage)
{
  uint32_t data;

  // Check status of transmit buffer 1.
  if(CAN1SR_bit.TBS1)
  {
    CAN1TFI1_bit.DLC = transMessage->len;

    if(transMessage->type == REMOTE_FRAME)
    {
      CAN1TFI1_bit.RTR = 1;
    }
    else
    {
      CAN1TFI1_bit.RTR = 0;
    }

    if(transMessage->format == EXT_ID_FORMAT)
    {
      CAN1TFI1_bit.FF = 1;
    }
    else
    {
      CAN1TFI1_bit.FF = 0;
    }

    // Write CAN ID.
    CAN1TID1 = transMessage->id;

    data = ((transMessage->dataA[0]) << 0)
         | ((transMessage->dataA[1]) << 8)
         | ((transMessage->dataA[2]) << 16)
         | ((transMessage->dataA[3]) << 24);

    // Write first four bytes of data.
    CAN1TDA1 = data;

    data = ((transMessage->dataB[0]) << 0)
         | ((transMessage->dataB[1]) << 8)
         | ((transMessage->dataB[2]) << 16)
         | ((transMessage->dataB[3]) << 24);

    // Write second four bytes of data.
    CAN1TDB1 = data;

    // Select transfer buffer 1.
    CAN1CMR_bit.STB1 = 1;

    // Write transmission request.
    CAN1CMR_bit.TR = 1;

    while(!CAN1SR_bit.TCS1)
    {
      printf("Waiting...\n");
    };

    //printf("Got this far\n");
  }
}

int main()
{
  // Initialise CAN.
  initCAN();

  // Enter self-test mode.
  testCAN();

  printf("In main\n");

  while(1);

  return 0;
}