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

// Declare a mutex Semaphore Handle which we will use to manage the Serial Port.
// It will be used to ensure only one Task is accessing this resource at any time.
SemaphoreHandle_t xSerialSemaphore;

// define two Tasks for DigitalRead & AnalogRead
void TaskDigitalRead( void *pvParameters );
void TaskAnalogRead( void *pvParameters );
int eStop;
int sensorValue;
Servo myservo;  // create servo object to control a servo
int potpin = 1;  // analog pin used to connect the potentiometer
int val;    // variable to read the value from the analog pin

// the setup function runs once when you press reset or power the board
void setup() {

  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);
   myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB, on LEONARDO, MICRO, YUN, and other 32u4 based boards.
  }

  // 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 ( xSerialSemaphore == NULL )  // Check to confirm that the Serial Semaphore has not already been created.
  {
    xSerialSemaphore = xSemaphoreCreateMutex();  // Create a mutex semaphore we will use to manage the Serial Port
    if ( ( xSerialSemaphore ) != NULL )
      xSemaphoreGive( ( xSerialSemaphore ) );  // Make the Serial Port available for use, by "Giving" the Semaphore.
  }

  // Now set up two Tasks to run independently.
  xTaskCreate(
    TaskDigitalRead
    ,  "DigitalRead"  // A name just for humans
    ,  128  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL //Parameters for the task
    ,  1  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  NULL ); //Task Handle

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

  xTaskCreate(
    TaskAnalogRead
    ,  "AnalogRead" // A name just for humans
    ,  128  // Stack size
    ,  NULL //Parameters for the task
    ,  3  // Priority
    ,  NULL ); //Task Handle

  xTaskCreate(
    OutputServo
    ,  "OutputServo" // A name just for humans
    ,  128  // Stack size
    ,  NULL //Parameters for the task
    ,  2  // Priority
    ,  NULL );
  // Now the Task scheduler, which takes over control of scheduling individual Tasks, is automatically started.
}

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

/*--------------------------------------------------*/
/*---------------------- Tasks ---------------------*/
/*--------------------------------------------------*/

void TaskDigitalRead( void *pvParameters __attribute__((unused)) )  // This is a Task.
{
  // digital pin 2 has a pushbutton attached to it. Give it a name:
  uint8_t pushButton = 2;
  pinMode(pushButton, INPUT);
  for (;;) // A Task shall never return or exit.
  {
    int buttonState = digitalRead(pushButton);
    if (buttonState == true){
      eStop = 1;
    } else{
      eStop = 0;
    }
    vTaskDelay(1);  // one tick delay (15ms) in between reads for stability
  }
}

void TaskAnalogRead( void *pvParameters __attribute__((unused)) )  // This is a Task.
{

  for (;;)
  {
    // read the input on analog pin 0:
    sensorValue = analogRead(A0);
    vTaskDelay(1);  // one tick delay (15ms) in between reads for stability
  }
}

void OutputValve( void *pvParameters __attribute__((unused)) )  // This is a Task.
{

  for (;;)
  {
    if (eStop == 1){
      digitalWrite(7, HIGH);
    }else{
      digitalWrite(7, LOW);
    };
    vTaskDelay(1);  // one tick delay (15ms) in between reads for stability
  }
}

void OutputServo( void *pvParameters __attribute__((unused)) )  // This is a Task.
{
  for (;;)
  {
    if (eStop == 1){
      val = sensorValue;            // reads the value of the potentiometer (value between 0 and 1023)
      val = map(val, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
      myservo.write(val); 
    };
    vTaskDelay(1);  // one tick delay (15ms) in between reads for stability
  }
}