/*** INCLUDES *******************************************************/
#include "HardwareProfile.h"

/* FreeRTOS.org includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "croutine.h"
#include "list.h"
#include "semphr.h"
#include "queue.h"
#include "timers.h"

/* Standard includes. */
#include <stdio.h>

void vInitApp(void);

void vLED1Task( void *pvParameters );
void vLED2Task( void *pvParameters );
void vLED3Task( void *pvParameters );
void vLED4Task( void *pvParameters );

/* Initialization of Change notification service. */
void vEnableChangeNotification(void);

QueueHandle_t xLedQueue1;
QueueHandle_t xLedQueue2;
QueueHandle_t xLedQueue3;
QueueHandle_t xLedQueue4;

const TickType_t turnOnTime = 400 / portTICK_RATE_MS;
const TickType_t turnOffTime = 200 / portTICK_RATE_MS;

int main(void)
{
    vInitApp();

    xLedQueue1 = xQueueCreate( 5, sizeof( portBASE_TYPE ) );
    xLedQueue2 = xQueueCreate( 5, sizeof( portBASE_TYPE ) );
    xLedQueue3 = xQueueCreate( 5, sizeof( portBASE_TYPE ) );
    xLedQueue4 = xQueueCreate( 5, sizeof( portBASE_TYPE ) );

    xTaskCreate( vLED1Task, "LED 1", 256, NULL, 2, NULL );
    xTaskCreate( vLED2Task, "LED 2", 256, NULL, 2, NULL );
    xTaskCreate( vLED3Task, "LED 3", 256, NULL, 2, NULL );
    xTaskCreate( vLED4Task, "LED 4", 256, NULL, 2, NULL );

    vTaskStartScheduler();

    for( ;; );

    return 0;
}//end main

void vLED1Task( void *pvParameters )
{
    portBASE_TYPE xReceivedValue;
    portBASE_TYPE xTransmittedValue;

    for( ;; )
    {
        if( xQueueReceive(xLedQueue1, &(xReceivedValue), portMAX_DELAY ) == pdPASS )
        {
            if(xReceivedValue == 1)
            {
                xTransmittedValue = 1;
                vTaskDelay(turnOnTime);
                mTurnOnLED(0);
                xQueueSend(xLedQueue2, (void * ) & xTransmittedValue, portMAX_DELAY);
            }
            else
            {
                vTaskDelay(turnOffTime);
                mTurnOffLED(0);
            }
        }
    }
}

void vLED2Task( void *pvParameters )
{
    portBASE_TYPE xReceivedValue;
    portBASE_TYPE xTransmittedValue;

    for( ;; )
    {
        if( xQueueReceive(xLedQueue2, &(xReceivedValue), portMAX_DELAY ) == pdPASS )
        {
            if(xReceivedValue == 1)
            {
                xTransmittedValue = 1;
                vTaskDelay(turnOnTime);
                mTurnOnLED(1);
                xQueueSend(xLedQueue3, (void * ) & xTransmittedValue, portMAX_DELAY);
            }
            else
            {
                xTransmittedValue = 0;
                vTaskDelay(turnOffTime);
                mTurnOffLED(1);
                xQueueSend(xLedQueue1, (void * ) & xTransmittedValue, portMAX_DELAY);
            }
        }
    }
}

void vLED3Task( void *pvParameters )
{
    portBASE_TYPE xReceivedValue;
    portBASE_TYPE xTransmittedValue;

    for( ;; )
    {
        if( xQueueReceive(xLedQueue3, &(xReceivedValue), portMAX_DELAY ) == pdPASS )
        {
            if(xReceivedValue == 1)
            {
                xTransmittedValue = 1;
                vTaskDelay(turnOnTime);
                mTurnOnLED(2);
                xQueueSend(xLedQueue4, (void * ) & xTransmittedValue, portMAX_DELAY);
            }
            else
            {
                xTransmittedValue = 0;
                vTaskDelay(turnOffTime);
                mTurnOffLED(2);
                xQueueSend(xLedQueue2, (void * ) & xTransmittedValue, portMAX_DELAY);
            }
        }
    }
}


void vLED4Task( void *pvParameters )
{
    portBASE_TYPE xReceivedValue;
    portBASE_TYPE xTransmittedValue;

    for( ;; )
    {
        if( xQueueReceive(xLedQueue4, &(xReceivedValue), portMAX_DELAY ) == pdPASS )
        {
            if(xReceivedValue == 1)
            {
                vTaskDelay(turnOnTime);
                mTurnOnLED(3);
            }
            else
            {
                xTransmittedValue = 0;
                vTaskDelay(turnOffTime);
                mTurnOffLED(3);
                xQueueSend(xLedQueue3, (void * ) & xTransmittedValue, portMAX_DELAY);
            }
        }
    }
}

void vApplicationIdleHook( void )
{
}

void vInitApp(void)
{
    mInitLEDs(); /* Initialisation of LEDs */
    mInitButtons(); /* Initialisation of Button */
    vEnableChangeNotification(); /* Initialisation of CN intrrrupt */
}

void vEnableChangeNotification(void)
{
    CNENAbits.CNIEA2 = 1; // Pin : RA2 ( S1 Button )
    CNENAbits.CNIEA3 = 1; // Pin : RA3 ( S2 Button )
    IEC1bits.CNIE = 1; // Enable CNI interrupt
    __builtin_enable_interrupts(); // Enable global Interrupt
}

/* Interrupt service routine for the CNI interrupt. */
void __attribute__ (( interrupt, no_auto_psv )) _CNInterrupt ( void )
{
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    portBASE_TYPE xTransmittedValue = 0xf;

    if(IFS1bits.CNIF == 1)
    {
        IFS1bits.CNIF = 0;
        if(mGetButtonStatePB1())
        {
            xTransmittedValue = 1;
            xQueueSendFromISR(xLedQueue1, (void*) &xTransmittedValue, &xHigherPriorityTaskWoken);
        }
        if(mGetButtonStatePB2())
        {
            xTransmittedValue = 0;
            xQueueSendFromISR(xLedQueue4, (void*) &xTransmittedValue, &xHigherPriorityTaskWoken);
        }
        if( xHigherPriorityTaskWoken )
        {
            taskYIELD ();
        }

    }
}