#define LED_DEVICE_ID  XPAR_GPIO_DEVICE_ID
#define LED_CHANNEL   1

#define CAN_DEVICE_ID  XPAR_PS7_CAN_0_DEVICE_ID

#define TEST_BTR_SYNCJUMPWIDTH    3
#define TEST_BTR_SECOND_TIMESEGMENT  2
#define TEST_BTR_FIRST_TIMESEGMENT  15


XGpio  Gpio;

static  XCanPs  Can0;

uint32_t  TxFrame[10];
uint8_t   *FramePtr;
uint8_t    Index;


volatile int i = 0;

int main()
{
  int status;
  XCanPs *CanInstPtr = &Can0 ;
  XCanPs_Config *ConfigPtr;


    init_platform();
    xil_printf("Platform Init done\n");

    // GPIO (Led) Init
    status  = XGpio_Initialize(&Gpio, LED_DEVICE_ID);
    if (XST_SUCCESS != status) {
      xil_printf("XGpio Init (LED) failed\n");
      //return XST_FAILURE;
    }

    XGpio_SetDataDirection(&Gpio, LED_CHANNEL, 0x00);
    status  = XGpio_SelfTest(&Gpio);
    if (XST_SUCCESS != status) {
      xil_printf("XGpio Selftest failed\n");
      //return XST_FAILURE;
    }

    // CAN0 Init
    ConfigPtr  = XCanPs_LookupConfig(CAN_DEVICE_ID);
    if (NULL == ConfigPtr) {
      xil_printf("CAN0 LookUpConfig failed\n");
      //return XST_FAILURE;
    }
    else {
      xil_printf("CAN0 LookUpConfig success\n");
      xil_printf("ConfigPtr: %d\n", ConfigPtr);
    }

    status  = XCanPs_CfgInitialize(&Can0, ConfigPtr, ConfigPtr->BaseAddr);
    if (status != XST_SUCCESS) {
      xil_printf("CAN0 CfgInitialize failed\n");
      //return XST_FAILURE;
    }
    else {
      xil_printf("CAN0 CfgInitialize success\n");
    }

    // Selftest
    status = XCanPs_SelfTest(&Can0);
  if (status != XST_SUCCESS) {
    xil_printf("CAN0 SelfTest failed\n");
    //return XST_FAILURE;
  }

  //Enter Configuration Mode so we can setup Baud Rate Prescaler
  // Register (BRPR) and Bit Timing Register (BTR).
  XCanPs_EnterMode(CanInstPtr, XCANPS_MODE_CONFIG);
  while(XCANPS_MODE_CONFIG != XCanPs_GetMode(CanInstPtr) );

  // Setup Baud Rate Prescaler Register (BRPR) and
  // Bit Timing Register (BTR).
  XCanPs_SetBaudRatePrescaler(CanInstPtr, 199);
  XCanPs_SetBitTiming(CanInstPtr, TEST_BTR_SYNCJUMPWIDTH,
        TEST_BTR_SECOND_TIMESEGMENT,
        TEST_BTR_FIRST_TIMESEGMENT);

  // Enter Loop Back Mode
  XCanPs_EnterMode(CanInstPtr, XCANPS_MODE_NORMAL);
  while(XCANPS_MODE_NORMAL != XCanPs_GetMode(CanInstPtr) );


    while(1)
    {
      print("Hello World 2\n\r");
      for (i = 0; i < 10000000; i++) { ; }
      XGpio_DiscreteWrite(&Gpio, LED_CHANNEL, 0x00);
      for (i = 0; i < 10000000; i++) { ; }
      XGpio_DiscreteWrite(&Gpio, LED_CHANNEL, 0x01);

      TxFrame[0] = (u32)XCanPs_CreateIdValue((uint32_t)1000, 0, 0, 0, 0);
      TxFrame[1] = (u32)XCanPs_CreateDlcValue((uint8_t)8);

      FramePtr = (u8 *)(&TxFrame[2]);
    for (Index = 0; Index < 8; Index++) {
      *FramePtr++ = (u8)Index;
    }

      status  = XCanPs_Send(&Can0, TxFrame);
      if (XST_SUCCESS != status) {
        xil_printf("Can0 send failed\n");
      }
      else {
        xil_printf("Can0 send succeeded\n");
      }
    }


    cleanup_platform();
    return 0;
}