#include <Arduino_FreeRTOS.h>
#include <queue.h>

// Define variables
int task1time = 1500;
int task2time = 3000;
int commandvalue;
int receivedvalue;

void task1( void *pvParameters );
void task2( void *pvParameters );

QueueHandle_t queue;

void setup() 
{
  
  pinMode(A0, OUTPUT);
  pinMode(A1, OUTPUT);
  pinMode(A2, OUTPUT);

  Serial.begin(9600);

  queue = xQueueCreate(1, sizeof (int));

  xTaskCreate(
    task1
    ,  "Read Input Data"
    ,  128   
    ,  NULL      
    ,  2         
    ,  NULL );

  xTaskCreate(
    task2
    ,  "LED Cmd" 
    ,  128            
    ,  NULL           
    ,  0          
    ,  NULL );
}

void loop()
{
  // Nothing to do here. Everything is implemented inside the tasks
}

void task1( void *pvParameters __attribute__((unused)) )
{
  for (;;)
  {
    commandvalue = analogRead(A3);
    xQueueSend(queue, &commandvalue, portMAX_DELAY);
    vTaskDelay(task1time/15); 
  }
}

void task2( void *pvParameters __attribute__((unused)) )
{
  for (;;)
  {
    if(commandvalue < 512)
    {
      digitalWrite(A0, HIGH);
      digitalWrite(A1, LOW);
      digitalWrite(A2, LOW);
    }
    else if ((commandvalue >= 512) && (commandvalue < 768))
    {
      digitalWrite(A0, HIGH);
      digitalWrite(A1, HIGH);
      digitalWrite(A2, LOW);
    }
    else
    {
      digitalWrite(A0, HIGH);
      digitalWrite(A1, HIGH);
      digitalWrite(A2, HIGH);
    }

    xQueueReceive(queue, &receivedvalue, portMAX_DELAY);
    Serial.print("Command value : "); Serial.println(receivedvalue);
    vTaskDelay(task2time/15); 
  }
}