#include <Arduino_FreeRTOS.h>
#include <task.h>
#include <semphr.h>  // add the FreeRTOS functions for Semaphores (or Flags).
#include <FreeRTOSConfig.h>


/* Define the traceTASK_SWITCHED_IN() macro to output the voltage associated
with the task being selected to run on port 0. */
// #define traceTASK_SWITCHED_IN() vSetAnalogueOutput( 0, (int)pxCurrentTCB->pxTaskTag )
//#define traceTASK_SWITCHED_IN() analogWrite( (int)pxCurrentTCB->pxTaskTag, 255 )
//#define traceTASK_SWITCHED_OUT() analogWrite((int)pxCurrentTCB->pxTaskTag, 0 )

/* Define the traceTASK_SWITCHED_IN() macro to switch ON and OFF the led on pin 13
with the task being selected to run. */ /*modified by Aradhana Dhumal*/
/* #define traceTASK_SWITCHED_IN() digitalWrite(13, (int)pxCurrentTCB->pxTaskTag); */ 
// Declare a Mutex Semaphore Handle which we will use to manage the Serial Port.
// It will be used to ensure only only one Task is accessing this resource at any time.
SemaphoreHandle_t toggle_sem;
SemaphoreHandle_t serial_sem;

#define LEDPIN 13
#define LEDPIN2 12

QueueHandle_t analogQueue;
QueueHandle_t serialQueue;

// define two Tasks for DigitalRead & AnalogRead
void TaskAnalogRead( void *pvParameters );
void TaskAnalogWrite( void *pvParameters );
void TaskSerialWrite( void *pvParameters);
void MyIdleTask(void *pvParameters );

// the setup function runs once when you press reset or power the board
void setup() {
  pinMode(LEDPIN, OUTPUT);
  pinMode(A0, INPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);
  
  unsigned long time; 
 
  analogQueue = xQueueCreate(15,sizeof(int)); /* Create a queue */
  serialQueue = xQueueCreate(100, sizeof(int));
  
  //Setup Interrupt
  
  cli();//stop interrupts
 
  //set timer1 interrupt at 100Hz
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1  = 0;//initialize counter value to 0
  // set compare match register for 100hz increments
  OCR1A = 257; // = 10ms //155;//=1ms // = (16*10^6) / (100*1024) - 1 (must be <65536)
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS12 and CS10 bits for 1024 prescaler
  TCCR1B |= (1 << CS12) | (1 << CS10);  
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);
  
  sei();//allow interrupts

  Serial.begin(9600);
  time = micros();
  //prints time since program started
  
  // Semaphores are useful to stop a Task proceeding, where it should be paused to wait,
  // because it is sharing a resource, such as the Serial port.
  // Semaphores should only be used whilst the scheduler is running, but we can set it up here.
  if ( toggle_sem == NULL )  // Check to confirm that the Serial Semaphore has not already been created.
  {
    toggle_sem = xSemaphoreCreateBinary();  // Create a binary semaphore we will use to manage the task order
   // if ( ( xSerialSemaphore ) != NULL )
   //    xSemaphoreGive( ( xSerialSemaphore ) );  // Make the Serial Port available for use, by "Giving" the Semaphore.
  }
  if ( serial_sem == NULL )  // Check to confirm that the Serial Semaphore has not already been created.
  {
    serial_sem = xSemaphoreCreateBinary();  // Create a binary semaphore we will use to manage the task order
   // if ( ( xSerialSemaphore ) != NULL )
   //    xSemaphoreGive( ( xSerialSemaphore ) );  // Make the Serial Port available for use, by "Giving" the Semaphore.
  }
  // Now set up three Tasks to run independently.
  xTaskCreate(
    TaskAnalogRead
    ,  "AnalogRead"  // A name just for humans
    ,  128  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  3  // Priority, with 3 being the highest, and 0 being the lowest.
    ,  NULL );

  xTaskCreate(
    TaskAnalogWrite
    ,   "AnalogWrite"
    ,  128  // Stack size
    ,  NULL
    ,  1  // Priority
    ,  NULL );

  xTaskCreate(
    TaskSerialWrite
    ,  "SerialWrite"  // A name just for humans
    ,  128  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  2  // Priority, with 3 being the highest, and 0 being the lowest.
    ,  NULL );

  xTaskCreate(MyIdleTask, "IdleTask", 100, NULL, 0, NULL);
  
  // Now the Task scheduler, which takes over control of scheduling individual Tasks, is automatically started.
  vTaskStartScheduler();

  
}

void loop()
{
  // Empty. Things are done in Tasks.
}

/*--------------------------------------------------*/
/*---------------------- Tasks ---------------------*/
/*--------------------------------------------------*/
 
void TaskAnalogRead( void *pvParameters __attribute__((unused)) )  // This is a Task.
{
  
  
  //Serial.println(**bufferTemp);
  unsigned long time; 
  UBaseType_t msgs;
  UBaseType_t reading;

  vTaskSetApplicationTaskTag( NULL, ( void * )12);
  for (;;) // A Task shall never return or exit.
  {
    
    // See if we can obtain or "Take" the Analog Semaphore.
    // If the semaphore is not available, wait 10 ticks of the Scheduler to see if it becomes free.
    if ( xSemaphoreTake( toggle_sem, ( TickType_t ) 10 ) == pdTRUE )
    {
      // this will release the highest importance task only when the ISR gives the semaphore
      //Read A0:
      reading = (UBaseType_t) analogRead(A0);
      xQueueSend(analogQueue, (int) reading, (TickType_t) 1);
     
      //this will fill the serial queue and when full block the task
      
      xQueueSend(serialQueue, (int) reading, (TickType_t) 1);
      msgs = uxQueueMessagesWaiting( serialQueue);
      if( msgs >= 40){
        //Serial.print("Serial Semaphore Given ");
        //Serial.println(msgs);
        xSemaphoreGive(serial_sem);
      }
    }
  }
}
void TaskAnalogWrite( void *pvParameters __attribute__((unused)) )  // This is a Task.
{
  unsigned long time; 
  int intensity = 0;
  int value = 0;
  
  vTaskSetApplicationTaskTag( NULL, ( void * )11);   
  for (;;)
  {
        if(xQueueReceive(analogQueue, &intensity, (TickType_t) 1)==pdPASS)
         {
          if(intensity >= 128) {
          digitalWrite(LEDPIN, HIGH);
          }
          else{
          digitalWrite(LEDPIN, LOW);
          }
         }
  }
}

void TaskSerialWrite( void *pvParameters __attribute__((unused)) )  // This is a Task.
{
  unsigned long time; 

  int value = 0;
  int msgs=0;
  
  vTaskSetApplicationTaskTag( NULL, ( void * )10);   
  for (;;)
  {
        
      if ( xSemaphoreTake( serial_sem, ( TickType_t ) 10 ) == pdTRUE ){
        //vTaskSuspend((TaskHandle_t)(TaskAnalogRead));
        msgs= uxQueueMessagesWaiting (serialQueue);
        for(int d= msgs; d>=0; d--)
        {
          xQueueReceive(serialQueue, &value,( TickType_t ) 10);
          Serial.print((int) value);
          Serial.print(" ");
          Serial.print((int) d);
          Serial.println();
        }
        //vTaskResume((TaskHandle_t)(TaskAnalogRead));
        
        
      }
  }
}
  /* Idle Task with priority Zero */ 
static void MyIdleTask(void* pvParameters)
{
  TickType_t xLastWakeTime;
  const TickType_t xFrequency = 2; //wait for at least 1 tick
  unsigned long time; 
     
  vTaskSetApplicationTaskTag( NULL, ( void * )9);
     for( ;; )
     {
      //digitalWrite(LEDPIN, !digitalRead(LEDPIN));
      delay(1);
     }
}

ISR(TIMER1_COMPA_vect){//timer1 interrupt 100Hz gives semaphore for Toggle Task
 BaseType_t  xHigherPriorityTaskWoken  pdFALSE;
 digitalWrite(8, !digitalRead(8));
 xSemaphoreGiveFromISR(toggle_sem, &xHigherPriorityTaskWoken); 
 
 if (xHigherPriorityTaskWoken) {
    taskYIELD();      // if this is left out, the scheduler will not be invoked directly and the task only switched in on the next tick!!!
  }

}