//
// FreeRTOS Example
// By PEK '2006
//
// MCU: AT91SAM7S256
// Experiment board: AT91SAM7S-EK
//

#include "Board.h"
#include "FreeRTOS.h"
#include "task.h"

// Definitions
#define TASK_PRIO_T1    (tskIDLE_PRIORITY + 1)
#define TASK_PRIO_T2    (tskIDLE_PRIORITY + 1)

void t1_task(void *pvParameters)
{
    portTickType xLastWakeTime;
    const portTickType xFrequency = 200;

    xLastWakeTime = xTaskGetTickCount();    // Initialise with current time

    while(1)
    {
        vTaskDelayUntil(&xLastWakeTime, xFrequency);    // Delay

        if(AT91C_BASE_PIOA->PIO_ODSR & LED1)    // LED1 is set?
        {
            AT91C_BASE_PIOA->PIO_CODR = LED1;   // Clear LED1
        }
        else
        {
            AT91C_BASE_PIOA->PIO_SODR = LED1;   // Set LED1
        }
    }
}

void t2_task(void *pvParameters)
{
    portTickType xLastWakeTime;
    const portTickType xFrequency = 400;

    xLastWakeTime = xTaskGetTickCount();    // Initialise with current time

    while(1)
    {
        vTaskDelayUntil(&xLastWakeTime, xFrequency);    // Delay

        if(AT91C_BASE_PIOA->PIO_ODSR & LED2)    // LED1 is set?
        {
            AT91C_BASE_PIOA->PIO_CODR = LED2;   // Clear LED1
        }
        else
        {
            AT91C_BASE_PIOA->PIO_SODR = LED2;   // Set LED1
        }
    }
}

int main()
{
    xTaskHandle xT1;
    xTaskHandle xT2;

    // Setup PIO
    AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_PIOA;  // Enable clock to PIO
    AT91C_BASE_PIOA->PIO_OER = LED_MASK;    // Output on PA0-PA3
    AT91C_BASE_PIOA->PIO_SODR = LED_MASK;   // Clear leds

    // Create tasks
    xTaskCreate(t1_task, "T1", configMINIMAL_STACK_SIZE, NULL, TASK_PRIO_T1, &xT1);
    xTaskCreate(t2_task, "T2", configMINIMAL_STACK_SIZE, NULL, TASK_PRIO_T2, &xT2);

    vTaskStartScheduler();  // Start scheduler

    return 1; // Should never return here
}