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

void sensorPollingTask(void * pvParameters) {
  TickType_t xLastWakeTime;
  const TickType_t xPeriod = pdMS_TO_TICKS(20);

  xLastWakeTime = xTaskGetTickCount();

  while(1){
    int sensor1 = analogRead(A0);
    int sensor2 = analogRead(A1);
    int sensor3 = analogRead(A2);
    
    int avg = (sensor1+ sensor2+ sensor3)/3;

    int processingTime = random(2,9);

    Serial.print("Tick:");
    Serial.print(xTaskGetTickCount());

    Serial.print(" |A0=");
    Serial.print(sensor1);

    Serial.print(" A1=");
    Serial.print(sensor2);

    Serial.print(" A=2");
    Serial.print(sensor3);

    Serial.print(" Average=");
    Serial.print(avg);

    Serial.print(" |Processing Delay=");
    Serial.print(processingTime);
    Serial.println("ms");

    vTaskDelay(pdMS_TO_TICKS(processingTime));

    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }

}

void setup(){
  Serial.begin(115200);

  xTaskCreate(
    sensorPollingTask,
    "PollingTask",
    configMINIMAL_STACK_SIZE,
    NULL,
    2,
    NULL
  );

  vTaskStartScheduler();

}

void loop(){
  
}