/*
 * FreeRTOS Kernel V10.4.3
 * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * https://www.FreeRTOS.org
 * https://github.com/FreeRTOS
 *
 */

/* Standard includes. */
#include <string.h>
#include <stdio.h>
#include <stdarg.h>
#include <stdbool.h>

/* Kernel includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "timers.h"
#include "semphr.h"
#include "event_groups.h"
#include "fsl_mcan.h"
#include "sample_can_interface.h"

/* TODO Add any manufacture supplied header files necessary for CMSIS functions
to be available here. */
/* Freescale includes. */
#include "fsl_device_registers.h"
#include "fsl_debug_console.h"
#include "pin_mux.h"
#include "board.h"

#include "FreeRTOS_CLI.h"
#include "UARTCommandConsole.h"
#include "SPItoFPGAOperation.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
//static BaseType_t prvTaskStatsCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
static BaseType_t prvDispVersionCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
static BaseType_t prvFpgaStatusCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
static BaseType_t prvSomResetSignalCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
static BaseType_t prvFpgaWriteCAN0RxData( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
static BaseType_t prvFpgaWriteCAN1RxData( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
//static BaseType_t prvParameterEchoCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );

/*
 * The queue send and receive tasks as described in the comments at the top of
 * this file.
 */
static void prvFPGAInterfaceTask(void *pvParameters);
static void prvCANxRxBufferProcess(int can0or1, UBaseType_t uxNumberOfItems);
static void prvCANxTxEventProcess(int can0or1, mcan_tx_event_buffer_frame_t* pulTxEventBuff);

/*
 * The callback function assigned to the example software timer as described at
 * the top of this file.
 */
static void vExampleTimerCallback(TimerHandle_t xTimer);

/*
 * The event semaphore task as described at the top of this file.
 */
static void prvEventSemaphoreTask(void *pvParameters);

/*******************************************************************************
 * Globals
 ******************************************************************************/
/* The queue used by the queue send and queue receive tasks. */
//static QueueHandle_t xQueue = NULL;
QueueHandle_t xQueueFPGAtoCAN0 = NULL;
QueueHandle_t xQueueFPGAtoCAN1 = NULL;
QueueHandle_t xQueueCAN0toFPGA = NULL;
QueueHandle_t xQueueCAN1toFPGA = NULL;

/* The semaphore (in this case binary) that is used by the FreeRTOS tick hook
 * function and the event semaphore task.
 */
static SemaphoreHandle_t xEventSemaphore = NULL;

/* The counters used by the various examples.  The usage is described in the
 * comments at the top of this file.
 */
static volatile uint32_t ulCountOfTimerCallbackExecutions = 0;
//static volatile uint32_t ulCountOfItemsReceivedOnQueue    = 0;
static volatile uint32_t ulCountOfReceivedSemaphores      = 0;

/*******************************************************************************
 * Code
 ******************************************************************************/
#if 0
/* Structure that defines the "task-stats" command line command.  This generates
a table that gives information on each task in the system. */
static const CLI_Command_Definition_t xTaskStats =
{
	"task-stats", /* The command string to type. */
	"task-stats:\r\n\t Displays a table showing the state of each FreeRTOS task.\r\n\r\n",
	prvTaskStatsCommand, /* The function to run. */
	0 /* No parameters are expected. */
};
#endif

static const CLI_Command_Definition_t xDispVersion =
{
    "version",             /* The command string to type. */
    "\r\n  \"version\": Display Firmware version and build date.\r\n",
    prvDispVersionCommand,   /* The function to run. */
    0                       /* No parameters are expected. */
};

static const CLI_Command_Definition_t xFpgaStatus =
{
    "fpga-sts",             /* The command string to type. */
    "\r\n  \"fpga-sts\": Display FPGA status (nSTATUS, CONFDONE, INITDONE)\r\n",
    prvFpgaStatusCommand,   /* The function to run. */
    0                       /* No parameters are expected. */
};

static const CLI_Command_Definition_t xSomResetSignal =
{
    "som-rst",              /* The command string to type. */
    "\r\n  \"som-rst\": Control SOM reset\r\n"
    "    < Usage: som-rst {assert/deassert/execute} >\r\n",
    prvSomResetSignalCommand, /* The function to run. */
    1                       /* No parameters are expected. */
};

static const CLI_Command_Definition_t xTestCAN0Rx =
{
	"can0-rx", /* The command string to type. */
	"\r\n  \"can0-rx\": Send the CAN0 RX data to the FPGA I/F for testing.\r\n"
    "    < Usage: can0-rx {send data string} >\r\n",
	prvFpgaWriteCAN0RxData, /* The function to run. */
	-1 /* No parameters are expected. */
};

static const CLI_Command_Definition_t xTestCAN1Rx =
{
	"can1-rx", /* The command string to type. */
	"\r\n  \"can1-rx\": Send the CAN1 RX data to the FPGA I/F for testing.\r\n"
    "    < Usage: can1-rx {send data string} >\r\n",
	prvFpgaWriteCAN1RxData, /* The function to run. */
	-1 /* No parameters are expected. */
};

/* @TODO: It is better to added CAN ID change command. */
/* @TOTO: It is better to added CAN Monitor mode enable/disable Select command. */

#if 0
static BaseType_t prvTaskStatsCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
{
	const char *const pcHeader = "     State   Priority  Stack    #\r\n************************************************\r\n";
	BaseType_t xSpacePadding;

	/* Remove compile time warnings about unused parameters, and check the
	write buffer is not NULL.  NOTE - for simplicity, this example assumes the
	write buffer length is adequate, so does not check for buffer overflows. */
	( void ) pcCommandString;
	( void ) xWriteBufferLen;
	configASSERT( pcWriteBuffer );

	/* Generate a table of task stats. */
	strcpy( pcWriteBuffer, "Task" );
	pcWriteBuffer += strlen( pcWriteBuffer );

	/* Minus three for the null terminator and half the number of characters in
	"Task" so the column lines up with the centre of the heading. */
	configASSERT( configMAX_TASK_NAME_LEN > 3 );
	for( xSpacePadding = strlen( "Task" ); xSpacePadding < ( configMAX_TASK_NAME_LEN - 3 ); xSpacePadding++ )
	{
		/* Add a space to align columns after the task's name. */
		*pcWriteBuffer = ' ';
		pcWriteBuffer++;

		/* Ensure always terminated. */
		*pcWriteBuffer = 0x00;
	}
	strcpy( pcWriteBuffer, pcHeader );
	vTaskList( pcWriteBuffer + strlen( pcHeader ) );

	/* There is no more data to return after this single string, so return
	pdFALSE. */
	return pdFALSE;
}
#endif

static BaseType_t prvDispVersionCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
{
	const char *const pcHeader = "\r\n<< Firmware version and build date >>\r\n";

	/* Remove compile time warnings about unused parameters, and check the
	write buffer is not NULL.  NOTE - for simplicity, this example assumes the
	write buffer length is adequate, so does not check for buffer overflows. */
	( void ) pcCommandString;
	( void ) xWriteBufferLen;
	configASSERT( pcWriteBuffer );

	strcpy( pcWriteBuffer, pcHeader );
	pcWriteBuffer += strlen( pcHeader );
	sprintf( pcWriteBuffer, "%s\r\n\r\n\0", cVerInfo );

	/* There is no more data to return after this single string, so return
	pdFALSE. */
	return pdFALSE;
}

static BaseType_t prvFpgaStatusCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
{
	const char *const pcHeader = "\r\n<< FPGA Status >>\r\n";

	/* Remove compile time warnings about unused parameters, and check the
	write buffer is not NULL.  NOTE - for simplicity, this example assumes the
	write buffer length is adequate, so does not check for buffer overflows. */
	( void ) pcCommandString;
	( void ) xWriteBufferLen;
	configASSERT( pcWriteBuffer );

	strcpy( pcWriteBuffer, pcHeader );
	pcWriteBuffer += strlen( pcHeader );
	sprintf( pcWriteBuffer, "SDM_NSTATUS:   %d\r\nSDM_CONF_DONE: %d\r\nSDM_INIT_DONE: %d\r\n\r\n\0",
			(int)NSTATUS_READ(), (int)CONFDONE_READ(), (int)INITDONE_READ());

	/* There is no more data to return after this single string, so return
	pdFALSE. */
	return pdFALSE;
}

static BaseType_t prvSomResetSignalCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
{
	const char *pcParameter;
	BaseType_t lParameterStringLength;
	const TickType_t delay = 100 / portTICK_PERIOD_MS;

	/* Remove compile time warnings about unused parameters, and check the
	write buffer is not NULL.  NOTE - for simplicity, this example assumes the
	write buffer length is adequate, so does not check for buffer overflows. */
	( void ) pcCommandString;
	( void ) xWriteBufferLen;
	configASSERT( pcWriteBuffer );

	/* Obtain the parameter string. */
	pcParameter = FreeRTOS_CLIGetParameter
					(
						pcCommandString,		/* The command string itself. */
						1,						/* Return the first parameter. */
						&lParameterStringLength	/* Store the parameter string length. */
					);

	/* Sanity check something was returned. */
	configASSERT( pcParameter );

	/* There are only two valid parameter values. */
	if( strncmp( pcParameter, "assert", strlen( "assert" ) ) == 0 )
	{
		sprintf( pcWriteBuffer, "\r\nAssert SOM_RESET_B signal.\r\n\r\n\0");
		SOMRESET_ON();
	}
	else if( strncmp( pcParameter, "deassert", strlen( "deassert" ) ) == 0 )
	{
		sprintf( pcWriteBuffer, "\r\nDeassert SOM_RESET_B signal.\r\n\r\n\0");
		SOMRESET_OFF();
	}
	else if( strncmp( pcParameter, "execute", strlen( "execute" ) ) == 0 )
	{
		sprintf( pcWriteBuffer, "\r\nExecute SOM_RESET_B. (Assert and wait 100ms, and Deassert)\r\n\r\n\0");
		SOMRESET_ON();
		vTaskDelay(delay);	/* wait 100ms */
		SOMRESET_OFF();
	}
	else
	{
		sprintf( pcWriteBuffer, "\r\nInvalid parameter!! <Usage: som-rst {assert/deassert/execute}>\r\n\r\n\0");
	}

	/* There is no more data to return after this single string, so return
	pdFALSE. */
	return pdFALSE;
}

static BaseType_t prvFpgaWriteCAN0RxData( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
{
	const char *pcParameter;
	BaseType_t lParameterStringLength;
	const TickType_t delay = 100 / portTICK_PERIOD_MS;
    static uint32_t ulQueueValue[mainQUEUE_IFTASKS_BYTES/sizeof(uint32_t)];

	/* Remove compile time warnings about unused parameters, and check the
	write buffer is not NULL.  NOTE - for simplicity, this example assumes the
	write buffer length is adequate, so does not check for buffer overflows. */
	( void ) pcCommandString;
	( void ) xWriteBufferLen;
	configASSERT( pcWriteBuffer );

	/* Obtain the parameter string. */
	pcParameter = FreeRTOS_CLIGetParameter
					(
						pcCommandString,		/* The command string itself. */
						1,						/* Return the first parameter. */
						&lParameterStringLength	/* Store the parameter string length. */
					);

	/* Sanity check something was returned. */
	configASSERT( pcParameter );

	sprintf( pcWriteBuffer, "\r\nSend to the CAN0-RX Queue: %s [Len=%d] \r\n\r\n\0", pcParameter, lParameterStringLength);

    /* Send to the queue - causing the queue receive task to unblock and
    increment its counter.  0 is used as the block time so the sending
    operation will not block - it shouldn't need to block as the queue
    should always be empty at this point in the code. */
	ulQueueValue[0] = lParameterStringLength;
	memcpy((uint8_t*)&ulQueueValue[1], (uint8_t*)pcParameter, lParameterStringLength);
    xQueueSend(xQueueCAN0toFPGA, &ulQueueValue, 0);

	/* There is no more data to return after this single string, so return
	pdFALSE. */
	return pdFALSE;
}

static BaseType_t prvFpgaWriteCAN1RxData( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
{
	const char *pcParameter;
	BaseType_t lParameterStringLength;
	const TickType_t delay = 100 / portTICK_PERIOD_MS;
    static uint32_t ulQueueValue[mainQUEUE_IFTASKS_BYTES/sizeof(uint32_t)];

	/* Remove compile time warnings about unused parameters, and check the
	write buffer is not NULL.  NOTE - for simplicity, this example assumes the
	write buffer length is adequate, so does not check for buffer overflows. */
	( void ) pcCommandString;
	( void ) xWriteBufferLen;
	configASSERT( pcWriteBuffer );

	/* Obtain the parameter string. */
	pcParameter = FreeRTOS_CLIGetParameter
					(
						pcCommandString,		/* The command string itself. */
						1,						/* Return the first parameter. */
						&lParameterStringLength	/* Store the parameter string length. */
					);

	/* Sanity check something was returned. */
	configASSERT( pcParameter );

	sprintf( pcWriteBuffer, "\r\nSend to the CAN1-RX Queue: %s [Len=%d] \r\n\r\n\0", pcParameter, lParameterStringLength);

    /* Send to the queue - causing the queue receive task to unblock and
    increment its counter.  0 is used as the block time so the sending
    operation will not block - it shouldn't need to block as the queue
    should always be empty at this point in the code. */
	ulQueueValue[0] = lParameterStringLength;
	memcpy((uint8_t*)&ulQueueValue[1], (uint8_t*)pcParameter, lParameterStringLength);
    xQueueSend(xQueueCAN1toFPGA, &ulQueueValue, 0);

	/* There is no more data to return after this single string, so return
	pdFALSE. */
	return pdFALSE;
}

#if 0
static BaseType_t prvParameterEchoCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
{
	const char *pcParameter;
	BaseType_t xParameterStringLength, xReturn;
	static UBaseType_t uxParameterNumber = 0;

	/* Remove compile time warnings about unused parameters, and check the
	write buffer is not NULL.  NOTE - for simplicity, this example assumes the
	write buffer length is adequate, so does not check for buffer overflows. */
	( void ) pcCommandString;
	( void ) xWriteBufferLen;
	configASSERT( pcWriteBuffer );

	if( uxParameterNumber == 0 )
	{
		/* The first time the function is called after the command has been
		entered just a header string is returned. */
		sprintf( pcWriteBuffer, "The parameters were:\r\n" );

		/* Next time the function is called the first parameter will be echoed
		back. */
		uxParameterNumber = 1U;

		/* There is more data to be returned as no parameters have been echoed
		back yet. */
		xReturn = pdPASS;
	}
	else
	{
		/* Obtain the parameter string. */
		pcParameter = FreeRTOS_CLIGetParameter
						(
							pcCommandString,		/* The command string itself. */
							uxParameterNumber,		/* Return the next parameter. */
							&xParameterStringLength	/* Store the parameter string length. */
						);

		if( pcParameter != NULL )
		{
			/* Return the parameter string. */
			memset( pcWriteBuffer, 0x00, xWriteBufferLen );
			sprintf( pcWriteBuffer, "%d: ", ( int ) uxParameterNumber );
			strncat( pcWriteBuffer, ( char * ) pcParameter, ( size_t ) xParameterStringLength );
			strncat( pcWriteBuffer, "\r\n", strlen( "\r\n" ) );

			/* There might be more parameters to return after this one. */
			xReturn = pdTRUE;
			uxParameterNumber++;
		}
		else
		{
			/* No more parameters were found.  Make sure the write buffer does
			not contain a valid string. */
			pcWriteBuffer[ 0 ] = 0x00;

			/* No more data to return. */
			xReturn = pdFALSE;

			/* Start over the next time this command is executed. */
			uxParameterNumber = 0;
		}
	}

	return xReturn;
}
#endif

/*!
 * @brief Main function
 */
int main(void)
{
    TimerHandle_t xExampleSoftwareTimer = NULL;

    /* Init board hardware. */
    /* attach 12 MHz clock to FLEXCOMM0 (debug console) */
    CLOCK_AttachClk(BOARD_DEBUG_UART_CLK_ATTACH);
    /* attach 12 MHz clock to FLEXCOMM1 (I2C master) */
    CLOCK_AttachClk(kFRO12M_to_FLEXCOMM1);
    /* attach FRO_HF clock (48MHz) to FLEXCOMM2 (SPI master) */
    CLOCK_AttachClk(kFRO_HF_to_FLEXCOMM2);
    /* reset FLEXCOMM for I2C */
    RESET_PeripheralReset(kFC1_RST_SHIFT_RSTn);
    /* reset FLEXCOMM2 for SPI */
    RESET_PeripheralReset(kFC2_RST_SHIFT_RSTn);

    /* Set MCAN clock 180Mhz/3=60MHz. */
    CLOCK_SetClkDiv(kCLOCK_DivCan0Clk, 3U, true);
    CLOCK_SetClkDiv(kCLOCK_DivCan1Clk, 3U, true);

    BOARD_InitBootPins();
    BOARD_InitBootClocks();
    BOARD_InitDebugConsole();

	/* Initialize FPGA Status */
    NSTATUS_INIT();
    INITDONE_INIT();
    CONFDONE_INIT();
	/* Initialize SOM Reset */
    SOMRESET_INIT(LOGIC_SOMRESET_DEASSERT);
	/* Initialize MCU LED0-LED2 (All ON)*/
	LED0_INIT(LOGIC_LED_ON);
	LED1_INIT(LOGIC_LED_ON);
	LED2_INIT(LOGIC_LED_ON);
	/* Initialize SPIM for FPGA I/F */
	SPIM_INIT();

	/* Register all the command line commands defined immediately above. */
#if USE_MCU_CAN_UNITTEST
    FreeRTOS_CLIRegisterCommand( &xCan0to1 );   /* CAN0 to CAN1 loopback test command */
    FreeRTOS_CLIRegisterCommand( &xCan1to0 );   /* CAN1 to CAN0 loopback test command */
	FreeRTOS_CLIRegisterCommand( &xTestCAN0Rx );
	FreeRTOS_CLIRegisterCommand( &xTestCAN1Rx );
#endif // USE_MCU_CAN_UNITTEST
	//FreeRTOS_CLIRegisterCommand( &xTaskStats );
    FreeRTOS_CLIRegisterCommand( &xDispVersion );
	FreeRTOS_CLIRegisterCommand( &xFpgaStatus );
	FreeRTOS_CLIRegisterCommand( &xSomResetSignal );
	/* Start the task that manages the command console for FreeRTOS+CLI. */
	vUARTCommandConsoleStart( ( configMINIMAL_STACK_SIZE * 3 ), tskIDLE_PRIORITY );

	/* Create the queue used for FPGA I/F Task --> CAN0 I/F Task. */
    xQueueFPGAtoCAN0 = xQueueCreate(mainQUEUE_IFTASKS_LENGTH, mainQUEUE_IFTASKS_BYTES);
    /* Enable queue view in MCUX IDE FreeRTOS TAD plugin. */
    if (xQueueFPGAtoCAN0 != NULL)	vQueueAddToRegistry(xQueueFPGAtoCAN0, "xQueueF2C0");

	/* Create the queue used for FPGA I/F Task --> CAN1 I/F Task. */
    xQueueFPGAtoCAN1 = xQueueCreate(mainQUEUE_IFTASKS_LENGTH, mainQUEUE_IFTASKS_BYTES);
    /* Enable queue view in MCUX IDE FreeRTOS TAD plugin. */
    if (xQueueFPGAtoCAN1 != NULL)	vQueueAddToRegistry(xQueueFPGAtoCAN1, "xQueueF2C1");

	/* Create the queue used for CAN0 I/F Task --> FPGA I/F Task. */
    xQueueCAN0toFPGA = xQueueCreate(mainQUEUE_IFTASKS_LENGTH, mainQUEUE_IFTASKS_BYTES);
    /* Enable queue view in MCUX IDE FreeRTOS TAD plugin. */
    if (xQueueCAN0toFPGA != NULL)	vQueueAddToRegistry(xQueueCAN0toFPGA, "xQueueC02F");

	/* Create the queue used for CAN1 I/F Task --> FPGA I/F Task. */
    xQueueCAN1toFPGA = xQueueCreate(mainQUEUE_IFTASKS_LENGTH, mainQUEUE_IFTASKS_BYTES);
    /* Enable queue view in MCUX IDE FreeRTOS TAD plugin. */
    if (xQueueCAN1toFPGA != NULL)	vQueueAddToRegistry(xQueueCAN1toFPGA, "xQueueC12F");

    /* Create the semaphore used by the FreeRTOS tick hook function and the
    event semaphore task. */
    vSemaphoreCreateBinary(xEventSemaphore);

#if USE_MCU_CAN_UNITTEST
	MCAN_MCULoopback_Init();
#else
	MCAN_TXRXviaFPGA_Init();
#endif // USE_MCU_CAN_UNITTEST

    if (xTaskCreate(prvFPGAInterfaceTask, "FPGA", configMINIMAL_STACK_SIZE * 4, NULL, mainFPGAIF_TASK_PRIORITY, NULL) != pdPASS)
    {
    	vOutputString("Task creation failed!.\r\n");
        while (1)
            ;
    }

    /* Create the task that is synchronised with an interrupt using the
    xEventSemaphore semaphore. */
    if (xTaskCreate(prvEventSemaphoreTask, "Sem", configMINIMAL_STACK_SIZE, NULL,
                    mainEVENT_SEMAPHORE_TASK_PRIORITY, NULL) != pdPASS)
    {
    	vOutputString("Task creation failed!.\r\n");
        while (1)
            ;
    }

    /* Create the software timer as described in the comments at the top of
    this file. */
    xExampleSoftwareTimer = xTimerCreate(/* A text name, purely to help debugging. */
                                         "LEDTimer",
                                         /* The timer period, in this case 1000ms (1s). */
                                         mainSOFTWARE_TIMER_PERIOD_MS,
                                         /* This is a periodic timer, so xAutoReload is set to pdTRUE. */
                                         pdTRUE,
                                         /* The ID is not used, so can be set to anything. */
                                         (void *)0,
                                         /* The callback function that switches the LED off. */
                                         vExampleTimerCallback);

    /* Start the created timer.  A block time of zero is used as the timer
    command queue cannot possibly be full here (this is the first timer to
    be created, and it is not yet running). */
    xTimerStart(xExampleSoftwareTimer, 0);

    /* Start the tasks and timer running. */
    vTaskStartScheduler();

    /* If all is well, the scheduler will now be running, and the following line
    will never be reached.  If the following line does execute, then there was
    insufficient FreeRTOS heap memory available for the idle and/or timer tasks
    to be created.  See the memory management section on the FreeRTOS web site
    for more details.  */
    for (;;)
        ;
}

/*!
 * @brief Timer callback.
 */
static void vExampleTimerCallback(TimerHandle_t xTimer)
{
    /* The timer has expired.  Count the number of times this happens.  The
    timer that calls this function is an auto re-load timer, so it will
    execute periodically. */
    ulCountOfTimerCallbackExecutions++;
    
    /* Toggle Heart Beat LED */
    if(ulCountOfTimerCallbackExecutions & 1)	LED1_ON();
    else	LED1_OFF();
    
    /* FPGA Status Monitor1: CONF_DONE */
    if(CONFDONE_READ())		LED0_ON();
    else	LED0_OFF();
    
    /* FPGA Status Monitor2: INIT_DONE */
    if(INITDONE_READ())		LED2_ON();
    else	LED2_OFF();
}

/*!
 * @brief Task prvFPGAInterfaceTask interface with the FPGA by controlling the SPI-to-AVMM Bridge.
 */
static void prvFPGAInterfaceTask(void *pvParameters)
{
    static uint32_t ulTxEventTemp[sizeof(mcan_tx_event_buffer_frame_t)/sizeof(uint32_t)];
    static uint32_t ulTxEventBuff[eIndexCanMAX][sizeof(mcan_tx_event_buffer_frame_t)/sizeof(uint32_t)];
    static bool bDet1stEventWord[eIndexCanMAX] = {false, false};
    static uint32_t ulFiFoLevel = 0;
    static uint32_t ulFiFoStatus = 0;
    static TickType_t xNextWakeTime;
    static EventBits_t event_bits;
    static uint32_t cnt = 0;
    UBaseType_t uxNumberOfItems;

    /* Initialise xNextWakeTime - this only needs to be done once. */
    xNextWakeTime = xTaskGetTickCount();

    vOutputPrintf( "\r\n== prvFPGAInterfaceTask() - Start ==\r\n" );

#if !USE_MCU_CAN_UNITTEST
    /* Sets the initialization complete flag. */
    xEventGroupSetBits(xEventGroupFpgaInterface, B0);
    vOutputPrintf( "\r\n== prvFPGAInterfaceTask() - Init ==\r\n" );

    /* Synchronize the execution of each task: FPGA Interface, CAN0 Interface, CAN1 Interface */
    xEventGroupSync(xEventGroupSyncTasks, FPGATASK_BIT, ALL_TASKS_BITS, portMAX_DELAY);
    vOutputPrintf( "\r\n== prvFPGAInterfaceTask() - Sync ==\r\n" );
#endif

    /* Self Check for FPGA SPI Interface. */
    for(cnt = 0; cnt < 5; cnt++)
    {
        if(CONFDONE_READ() && SW4_2_READ())
        {
            vOutputPrintf( "\r\n== prvFPGAInterfaceTask() - Start Self Check for SPI I/F ==\r\n" );
            {
                char cTestBuff[160];
                memset(cTestBuff, 0, sizeof(cTestBuff));
                fpga_spi_bridge_sequential_write( FPGA_OCRAM_BASE, cVerInfo, strlen(cVerInfo) );
                vOutputPrintf( "fpga_spi_bridge_write(OCRAM) Finish: %s\r\n", cVerInfo );
                fpga_spi_bridge_sequential_read( FPGA_OCRAM_BASE, cTestBuff, sizeof(cTestBuff) );
                vOutputPrintf( "fpga_spi_bridge_read(OCRAM) Finish: %s\r\n", cTestBuff );
                vOutputPrintf( "@@ Self Check for SPI I/F: result = " );
                if(memcmp(cTestBuff, cVerInfo, sizeof(cTestBuff)) == 0)
                {
                    vOutputPrintf( "SUCCEEDED.\r\n" );
                    break;
                } else 
                {
                    vOutputPrintf( "FAILED!!!!!!!!! \r\n" );
                    /* Task Abort & Error Indicate via LED Brink. */
                    while(1)
                    {
                        vTaskDelay(200/portTICK_PERIOD_MS);	/* wait 200ms */
                        LED0_TOGGLE();
                        LED1_TOGGLE();
                        LED2_TOGGLE();
                    }
                }
            }
        }
#if USE_DEBUG_PRINT_L1
        vOutputPrintf("\r\nFPGA is not ready, or SW4.2 is on. [%s:%d]\r\n", __FILE__, __LINE__);
#endif /* #if USE_DEBUG_PRINT_L2 */
        vTaskDelay(2000/portTICK_PERIOD_MS);	/* wait 2s */
    }

    for (;;)
    {
        /* Place this task in the blocked state until it is time to run again.
        The block time is specified in ticks, the constant used converts ticks
        to ms.  While in the Blocked state this task will not consume any CPU
        time. */
        vTaskDelayUntil(&xNextWakeTime, mainFPGAIF_TASK_PERIOD_MS);

        /*******************/
        /*  CAN0 to FPGA   */
        /*******************/
        // Wait for frames (messages) to be loaded in the xQueueCan0ToFpga queue.
        if( xQueueCAN0toFPGA != NULL )
        {
            if ( uxNumberOfItems = uxQueueMessagesWaiting( xQueueCAN0toFPGA ) )
            {
                /* Receive Queue message and write to FPGA RxBuffer0 FIFO. */
                prvCANxRxBufferProcess(eIndexCan0, uxNumberOfItems);
            }
        }

        /*******************/
        /*  CAN1 to FPGA   */
        /*******************/
        // Wait for frames (messages) to be loaded in the xQueueCan1ToFpga queue.
        if( xQueueCAN1toFPGA != NULL )
        {
            if ( uxNumberOfItems = uxQueueMessagesWaiting( xQueueCAN1toFPGA ) )
            {
                /* Receive Queue message and write to FPGA RxBuffer1 FIFO. */
                prvCANxRxBufferProcess(eIndexCan1, uxNumberOfItems);
            }
        }

		vTaskDelay(10/portTICK_PERIOD_MS);	/* wait 10ms */

        /*******************/
        /*  FPGA to CAN0/1 */
        /*******************/
        if(!CONFDONE_READ() || !SW4_2_READ())
        {
#if USE_DEBUG_PRINT_L1
            vOutputPrintf("\r\nFPGA is not ready, or SW4.2 is on. [%s:%d]\r\n", __FILE__, __LINE__);
#endif /* #if USE_DEBUG_PRINT_L2 */
            vTaskDelay(5000/portTICK_PERIOD_MS);	/* wait 5s */
            continue;
        }

        // Read CAN01 EVENT FIFO via SPI-to-AVMM.
        fpga_spi_bridge_read( FIFO_TXEVENT01_CSR_BASE + FIFO_LEVEL_REG, 
                              (uint8_t*)&ulFiFoLevel, sizeof(uint32_t) );

        while ( ulFiFoLevel > 0 )
        {
#if USE_DEBUG_PRINT_L2
            vOutputPrintf( "Fpga_Task - FIFO_TxEv01 Level=%d\r\n", ulFiFoLevel );
#endif
            // #1. Check the status of the CAN01 EVENT FIFO 
            fpga_spi_bridge_read( FIFO_TXEVENT01_CSR_BASE + FIFO_STATUS_REG, 
                                  (uint8_t*)&ulFiFoStatus, sizeof(uint32_t) );
            if ( !( ulFiFoStatus & (FIFO_STATUS_E_MSK | FIFO_STATUS_UDF_MSK)) )
            {
                mcan_tx_event_buffer_frame_t* pTxEventFrame = (mcan_tx_event_buffer_frame_t*)&ulTxEventTemp[0];
                int can0or1;
            
                // #2. If data has arrived in FIFO (not EMPTY), read one data 
                //     Read from CAN01TX EVENT FIFO to ulTxEventTemp via AMVM-to-SPI. 
                fpga_spi_bridge_read( FIFO_TXEVENT01_BASE, (uint8_t*)&ulTxEventTemp[0], sizeof(uint32_t) );
#if USE_DEBUG_PRINT_L1
                vOutputPrintf( "Fpga_Task - fpga_spi_bridge_read(FIFO_TxEv01)\r\n" );
#endif

                can0or1 = pTxEventFrame->num1;
                if(pTxEventFrame->fdt1)
                {   /* ---- 1st tx event word ---- */
                    bDet1stEventWord[can0or1] = true;
                    ulTxEventBuff[can0or1][0] = ulTxEventTemp[0];
                } else 
                {   /* ---- 2nd tx event word ---- */
                    if(bDet1stEventWord[can0or1] == true)
                    {
                        bDet1stEventWord[can0or1] = false;
                        ulTxEventBuff[can0or1][1] = ulTxEventTemp[0];

#if USE_DEBUG_PRINT_L2
                        vOutputPrintf( "Fpga_Task - fpga_spi_bridge_read(FIFO_TxEv01) can0or1=%d Data:\r\n", can0or1 );
                        for ( cnt = 0; cnt < sizeof(mcan_tx_event_buffer_frame_t)/sizeof(uint32_t); cnt++ )
                        {
                            vOutputPrintf( "0x%08x\r\n", ulTxEventBuff[can0or1][cnt] );
                        }
                        vOutputPrintf( "\r\n" );
#endif
                        /* Read payload data and send Queue message to CANx task. */
                        prvCANxTxEventProcess(can0or1, (mcan_tx_event_buffer_frame_t*)ulTxEventBuff[can0or1]);
                        /* Break from the while loop after processing a send event. */
                        break;

                    } else 
                    {
                        vOutputPrintf( "Fpga_Task - Irregular Tx EVENT FIFO data sequence detected!! can0or1=%d \r\n", can0or1 );
                    }
                }
            } else 
            {
                vOutputPrintf( "Fpga_Task - FIFO_TxEv01 Empty or Underflow!! Status=0x%08X \r\n", ulFiFoStatus );
            }
            ulFiFoLevel--;
        }
    }
}

/*!
 * @brief Receive Queue message and write to FPGA RxBuffer FIFO. 
 */
static void prvCANxRxBufferProcess(int can0or1, UBaseType_t uxNumberOfItems)
{
    uint32_t rxbuffer_fifo_csr_addr;
    uint32_t rxbuffer_fifo_addr;
    QueueHandle_t* pxQueueCANxtoFPGA;
    static uint32_t ulQueueValue[mainQUEUE_IFTASKS_BYTES/sizeof(uint32_t)];
    size_t txSize;
    static uint32_t ulFiFoLevel = 0;
    static uint32_t ulFiFoStatus = 0;
    static uint32_t cnt = 0;
    mcan_rx_buffer_frame_t *rxBuffer;
    size_t PayloadSize;

    if(can0or1 == eIndexCan0)
    {
        rxbuffer_fifo_csr_addr = FIFO_RXBUFF0_CSR_BASE;
        rxbuffer_fifo_addr = FIFO_RXBUFF0_BASE;
        pxQueueCANxtoFPGA = &xQueueCAN0toFPGA;
    } else /* if(can0or1 == eIndexCan1) */
    {
        rxbuffer_fifo_csr_addr = FIFO_RXBUFF1_CSR_BASE;
        rxbuffer_fifo_addr = FIFO_RXBUFF1_BASE;
        pxQueueCANxtoFPGA = &xQueueCAN1toFPGA;
    }

    /*******************/
    /*  CANx to FPGA   */
    /*******************/
#if USE_DEBUG_PRINT_L2
    vOutputPrintf( "Fpga_Task - Number of messages C%dToFpga Queue=%d\r\n", can0or1, uxNumberOfItems );
#endif
    // Load xQueueCan0ToFpga queue messages into ulQueueValue. 
    if ( xQueueReceive( *pxQueueCANxtoFPGA, &ulQueueValue, 0 ) != pdPASS )
    { 
        vOutputPrintf( "Fpga_Task - Failed to receive message!\r\n" );
    }

    rxBuffer = (mcan_rx_buffer_frame_t*)&ulQueueValue[0];
    PayloadSize = DATA_LENGTH_BYTES[rxBuffer->dlc];
    txSize = CAN_FRAMEHEADER_BYTES + PayloadSize;
    // Rounded up to 4 bytes (FPGA-side FIFO data width). 
    if(txSize & (FPGAFIFO_WIDTH_BYTES-1))
    {
        txSize = (txSize & ~(FPGAFIFO_WIDTH_BYTES-1)) + FPGAFIFO_WIDTH_BYTES;
    }

#if USE_DEBUG_PRINT_L1
    vOutputPrintf( "Fpga_Task - Q_Recv(C%dToFpga)\r\n", can0or1 );
#endif
#if USE_DEBUG_PRINT_L2
    vOutputPrintf( "Fpga_Task - C%dToFpga Q_Recv Data:\r\n", can0or1 );
    for ( cnt = 0; cnt < txSize/sizeof(uint32_t); cnt++ )
    {
        vOutputPrintf( "0x%08x\r\n", ulQueueValue[cnt] );
    }
    vOutputPrintf( "\r\n" );
#endif
    
    // Note: Since Queue does not support receiving variable data lengths, you may want to use Stream Buffer for data reception.
    //  For now, store data in Queue. The Stream Buffer should be configured one-to-one for each Send/Receive Task.
    //  In the case of Queue, it has the advantage that it can be configured many-to-many because it includes exclusive control, 
    //  but it may be heavy. 

    if(CONFDONE_READ() && SW4_2_READ())
    {
        // Write CAN0 RX FIFO via SPI-to-AVMM.
        fpga_spi_bridge_read( (uint32_t)(rxbuffer_fifo_csr_addr + FIFO_STATUS_REG), 
                            (uint8_t*)&ulFiFoStatus, sizeof(uint32_t) );
        // FIFOFull otherwise write to CAN0RX FIFO via SPI-to-AVMM. 
        if ( !( ulFiFoStatus & (FIFO_STATUS_F_MSK | FIFO_STATUS_AF_MSK | FIFO_STATUS_OVF_MSK)) )
        {
            fpga_spi_bridge_write( rxbuffer_fifo_addr, (uint8_t*)&ulQueueValue[0], txSize);
#if USE_DEBUG_PRINT_L1
            vOutputPrintf( "Fpga_Task - fpga_spi_bridge_write(FIFO_RxC%d)\r\n", can0or1 );
#endif
#if USE_DEBUG_PRINT_L2
            fpga_spi_bridge_read( (uint32_t)(rxbuffer_fifo_csr_addr + FIFO_LEVEL_REG), 
                                (uint8_t*)&ulFiFoLevel, sizeof(uint32_t) );
            vOutputPrintf( "Fpga_Task - FIFO_RxC%d Level=%d\r\n", can0or1, ulFiFoLevel );
#endif
        } else 
        {
#if USE_DEBUG_PRINT_L1
            vOutputPrintf( "Fpga_Task - FIFO_RxC%d Full or Overflow!! Status=%0x08X \r\n", can0or1, ulFiFoStatus );
#endif
        }
    } else 
    {
#if USE_DEBUG_PRINT_L1
        vOutputPrintf("\r\nFPGA is not ready, or SW4.2 is on. [%s:%d]\r\n", __FILE__, __LINE__);
#endif /* #if USE_DEBUG_PRINT_L2 */
        vTaskDelay(5000/portTICK_PERIOD_MS);	/* wait 5s */
    }

    return;
}

/*!
 * @brief Read payload data and send Queue message to CANx task.
 */
static void prvCANxTxEventProcess(int can0or1, mcan_tx_event_buffer_frame_t* pulTxEventBuff)
{
    uint32_t txdata_fifo_csr_addr;
    uint32_t txdata_fifo_addr;
    QueueHandle_t* pxQueueFPGAtoCANx;
    static uint32_t ulQueueValue[mainQUEUE_IFTASKS_BYTES/sizeof(uint32_t)];
    size_t rxSize;
    static uint32_t ulFiFoLevel = 0;
    static uint32_t ulFiFoStatus = 0;
    static uint32_t cnt = 0;
    mcan_tx_buffer_frame_t* txEvent;
    size_t PayloadSize;

    if(can0or1 == eIndexCan0)
    {
        txdata_fifo_csr_addr = FIFO_TXDATA0_CSR_BASE;
        txdata_fifo_addr = FIFO_TXDATA0_BASE;
        pxQueueFPGAtoCANx = &xQueueFPGAtoCAN0;
    } else /* if(can0or1 == eIndexCan1) */
    {
        txdata_fifo_csr_addr = FIFO_TXDATA1_CSR_BASE;
        txdata_fifo_addr = FIFO_TXDATA1_BASE;
        pxQueueFPGAtoCANx = &xQueueFPGAtoCAN1;
    }

    /* Copy while converting to CAN controller transmit FIFO format */
    txEvent = (mcan_tx_buffer_frame_t*)ulQueueValue;
    txEvent->esi = pulTxEventBuff->esi;
    txEvent->xtd = pulTxEventBuff->xtd;
    txEvent->rtr = pulTxEventBuff->rtr;
    txEvent->id  = pulTxEventBuff->id;
    txEvent->mm = 0;
    txEvent->efc = pulTxEventBuff->et >> 1;
    txEvent->fdf = pulTxEventBuff->fdf;
    txEvent->brs = pulTxEventBuff->brs;
    txEvent->dlc = pulTxEventBuff->dlc;

    PayloadSize = DATA_LENGTH_BYTES[pulTxEventBuff->dlc];
    rxSize = PayloadSize;
    // Rounded up to 4 bytes (FPGA-side FIFO data width). 
    if(rxSize & (FPGAFIFO_WIDTH_BYTES-1))
    {
        rxSize = (rxSize & ~(FPGAFIFO_WIDTH_BYTES-1)) + FPGAFIFO_WIDTH_BYTES;
    }

    // #4. When the number of data items is non-zero, data is read from the CAN0TX DATA FIFO. 
    fpga_spi_bridge_read( txdata_fifo_csr_addr + FIFO_LEVEL_REG, 
                            (uint8_t*)&ulFiFoLevel, sizeof(uint32_t) );
    if ( ulFiFoLevel >= rxSize/sizeof(uint32_t) )
    {
#if USE_DEBUG_PRINT_L2
        vOutputPrintf( "Fpga_Task - FIFO_TxC%d Level=%d\r\n", can0or1, ulFiFoLevel );
#endif
        fpga_spi_bridge_read( txdata_fifo_csr_addr + FIFO_STATUS_REG, 
                                (uint8_t*)&ulFiFoStatus, sizeof(uint32_t) );
        if ( !( ulFiFoStatus & (FIFO_STATUS_E_MSK | FIFO_STATUS_UDF_MSK)) )
        {
            fpga_spi_bridge_read( txdata_fifo_addr, (uint8_t*)&ulQueueValue[CAN_FRAMEHEADER_BYTES/FPGAFIFO_WIDTH_BYTES], rxSize );
#if USE_DEBUG_PRINT_L1
            vOutputPrintf( "Fpga_Task - fpga_spi_bridge_read(FIFO_TxC%d)\r\n", can0or1 );
#endif
        } else 
        {
#if USE_DEBUG_PRINT_L1
            vOutputPrintf( "Fpga_Task - FIFO_TxC%d Empty or Underflow!! Status=0x%08X \r\n", can0or1, ulFiFoStatus );
#endif
        }

#if USE_DEBUG_PRINT_L2
        vOutputPrintf( "Fpga_Task - Q_Send(FpgaToC%d) Data:\r\n", can0or1 );
        for ( cnt = 0; cnt < ((sizeof(mcan_tx_event_buffer_frame_t) + rxSize)/sizeof(uint32_t)); cnt++ )
        {
            vOutputPrintf( "0x%08x\r\n", ulQueueValue[cnt] );
        }
        vOutputPrintf( "\r\n" );
#endif
        // Send the frame set to ulQueueValue to the xQueueFpgaToCan0 queue. 
        if ( xQueueSend( *pxQueueFPGAtoCANx, &ulQueueValue, 0 ) != pdPASS )
        { 
            vOutputPrintf( "Fpga_Task - Failed to send message!\r\n" );
        }
#if USE_DEBUG_PRINT_L1
        vOutputPrintf( "Fpga_Task - Q_Send(FpgaToC%d)\r\n", can0or1 );
#endif

    } else 
    {
        vOutputPrintf( "Fpga_Task - FIFO_TxC%d data is insufficient!! Level=%d \r\n", can0or1, ulFiFoLevel );

        /* Empty FIFO by reading all data. */
        fpga_spi_bridge_read( txdata_fifo_addr, (uint8_t*)&ulQueueValue[4], ulFiFoLevel*sizeof(uint32_t));
    }

    return;
}

/*!
 * @brief task prvEventSemaphoreTask is waiting for semaphore.
 */
static void prvEventSemaphoreTask(void *pvParameters)
{
    for (;;)
    {
        /* Block until the semaphore is 'given'. */
        if (xSemaphoreTake(xEventSemaphore, portMAX_DELAY) != pdTRUE)
        {
        	vOutputString("Failed to take semaphore.\r\n");
        }

        /* Count the number of times the semaphore is received. */
        ulCountOfReceivedSemaphores++;

        //vOutputString("Event task is running.\r\n");
        vOutputString("");
    }
}

/*!
 * @brief tick hook is executed every tick.
 */
void vApplicationTickHook(void)
{
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    static uint32_t ulCount             = 0;

    /* The RTOS tick hook function is enabled by setting configUSE_TICK_HOOK to
    1 in FreeRTOSConfig.h.

    "Give" the semaphore on every 500th tick interrupt. */
    ulCount++;
    if (ulCount >= 500UL)
    {
        /* This function is called from an interrupt context (the RTOS tick
        interrupt),    so only ISR safe API functions can be used (those that end
        in "FromISR()".

        xHigherPriorityTaskWoken was initialised to pdFALSE, and will be set to
        pdTRUE by xSemaphoreGiveFromISR() if giving the semaphore unblocked a
        task that has equal or higher priority than the interrupted task. */
        xSemaphoreGiveFromISR(xEventSemaphore, &xHigherPriorityTaskWoken);
        ulCount = 0UL;
    }

    /* If xHigherPriorityTaskWoken is pdTRUE then a context switch should
    normally be performed before leaving the interrupt (because during the
    execution of the interrupt a task of equal or higher priority than the
    running task was unblocked).  The syntax required to context switch from
    an interrupt is port dependent, so check the documentation of the port you
    are using.

    In this case, the function is running in the context of the tick interrupt,
    which will automatically check for the higher priority task to run anyway,
    so no further action is required. */
}

/*!
 * @brief Malloc failed hook.
 */
void vApplicationMallocFailedHook(void)
{
    /* The malloc failed hook is enabled by setting
    configUSE_MALLOC_FAILED_HOOK to 1 in FreeRTOSConfig.h.

    Called if a call to pvPortMalloc() fails because there is insufficient
    free memory available in the FreeRTOS heap.  pvPortMalloc() is called
    internally by FreeRTOS API functions that create tasks, queues, software
    timers, and semaphores.  The size of the FreeRTOS heap is set by the
    configTOTAL_HEAP_SIZE configuration constant in FreeRTOSConfig.h. */
    for (;;)
        ;
}

/*!
 * @brief Stack overflow hook.
 */
void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName)
{
    (void)pcTaskName;
    (void)xTask;

    /* Run time stack overflow checking is performed if
    configconfigCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2.  This hook
    function is called if a stack overflow is detected.  pxCurrentTCB can be
    inspected in the debugger if the task name passed into this function is
    corrupt. */
    for (;;)
        ;
}

/*!
 * @brief Idle hook.
 */
void vApplicationIdleHook(void)
{
    volatile size_t xFreeStackSpace;

    /* The idle task hook is enabled by setting configUSE_IDLE_HOOK to 1 in
    FreeRTOSConfig.h.

    This function is called on each cycle of the idle task.  In this case it
    does nothing useful, other than report the amount of FreeRTOS heap that
    remains unallocated. */
    xFreeStackSpace = xPortGetFreeHeapSize();

    if (xFreeStackSpace > 100)
    {
        /* By now, the kernel has allocated everything it is going to, so
        if there is a lot of heap remaining unallocated then
        the value of configTOTAL_HEAP_SIZE in FreeRTOSConfig.h can be
        reduced accordingly. */
    }
}


