#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 );
void emergencyButtonTask(void *pvParameters);
void flowmeterTask(void *pvParameters);
void servoTask(void *pvParameters);
void displayTask(void *pvParameters);

Servo myservo;

const int buttonPin = 7;
const int relayPin = 3;
const int servoPin = 4;
const int flowMeterPin = A0;

bool valveClosed = false;
float flowrate = 0.0;
int servoPosition = 0;

// 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(servoPin);
  
  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
//    ,  2  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
//    ,  NULL ); //Task Handle
//
//  xTaskCreate(
//    TaskAnalogRead
//    ,  "AnalogRead" // A name just for humans
//    ,  128  // Stack size
//    ,  NULL //Parameters for the task
//    ,  1  // Priority
//    ,  NULL ); //Task Handle

      

      xTaskCreate(emergencyButtonTask, "EmergencyButtonTask", 100, NULL, 1, NULL);
      xTaskCreate(flowmeterTask, "FlowmeterTask", 128, NULL, 2, NULL);
      xTaskCreate(servoTask, "ServoTask", 128, NULL, 3, NULL);
      xTaskCreate(displayTask, "DisplayTask", 128, NULL, 4, 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.
//{
//  /*
//    DigitalReadSerial
//    Reads a digital input on pin 2, prints the result to the serial monitor
//
//    This example code is in the public domain.
//  */
//
//  // digital pin 2 has a pushbutton attached to it. Give it a name:
//  uint8_t pushButton = 2;
//
//  // make the pushbutton's pin an input:
//  pinMode(pushButton, INPUT);
//
//  for (;;) // A Task shall never return or exit.
//  {
//    // read the input pin:
//    int buttonState = digitalRead(pushButton);
//
//    // See if we can obtain or "Take" the Serial Semaphore.
//    // If the semaphore is not available, wait 5 ticks of the Scheduler to see if it becomes free.
//    if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
//    {
//      // We were able to obtain or "Take" the semaphore and can now access the shared resource.
//      // We want to have the Serial Port for us alone, as it takes some time to print,
//      // so we don't want it getting stolen during the middle of a conversion.
//      // print out the state of the button:
//      Serial.println(buttonState);
//
//      xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
//    }
//
//    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:
//    int sensorValue = analogRead(A0);
//
//    // See if we can obtain or "Take" the Serial Semaphore.
//    // If the semaphore is not available, wait 5 ticks of the Scheduler to see if it becomes free.
//    if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
//    {
//      // We were able to obtain or "Take" the semaphore and can now access the shared resource.
//      // We want to have the Serial Port for us alone, as it takes some time to print,
//      // so we don't want it getting stolen during the middle of a conversion.
//      // print out the value you read:
//      Serial.println(sensorValue);
//
//      xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
//    }
//
//    vTaskDelay(1);  // one tick delay (15ms) in between reads for stability
//  }
//}

void emergencyButtonTask( void *pvParameters __attribute__((unused)) )  // This is a Task.
{
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(relayPin, OUTPUT);
  for(;;)
  {
    if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
    {
      if (digitalRead(buttonPin) == LOW) {
        valveClosed = !valveClosed;
        digitalWrite(relayPin, valveClosed ? HIGH : LOW);
        vTaskDelay(pdMS_TO_TICKS(1000)); 
      }
      xSemaphoreGive( xSerialSemaphore ); 
    }
    vTaskDelay(pdMS_TO_TICKS(100));  
  }
}

void flowmeterTask( void *pvParameters __attribute__((unused)) )  // This is a Task.
{
  for(;;)
  {
    if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
    {
      flowrate = random(0, 100);
      if(valveClosed == true){
        flowrate = 0;
      }
      xSemaphoreGive( xSerialSemaphore ); 
    vTaskDelay(pdMS_TO_TICKS(1000));  
    }
  }
}

void servoTask( void *pvParameters __attribute__((unused)) )  // This is a Task.
{
  for(;;)
  {
    if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
    {
      if(flowrate == 0 && valveClosed == true){
        servoPosition = 0;
      }else if (flowrate == 50){
        servoPosition = 90;
      }else if(flowrate <= 50){
        servoPosition = 180;
      }else{
        servoPosition = 45;
      }

      myservo.write(servoPosition);
      
      xSemaphoreGive( xSerialSemaphore ); 
    }
    vTaskDelay(pdMS_TO_TICKS(1000));  
  }
}

void displayTask( void *pvParameters __attribute__((unused)) )  // This is a Task.
{
  for(;;)
  {
    if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
    {
      if (!valveClosed){
        Serial.print("Flow Rate: ");
        Serial.print(flowrate);
        Serial.print(" | Servo Position: ");
        Serial.print(servoPosition);
        Serial.print(" | Solenoid State: ");
        Serial.println("Open");
      }else{
        Serial.println("WARNING: EMERGENCY, PLEASE CHECK SOON");
      }
            
      xSemaphoreGive( xSerialSemaphore ); 
    }
    vTaskDelay(pdMS_TO_TICKS(1000));  
  }
}