#include <Arduino_FreeRTOS.h> 
#include "semphr.h"              //including semaphore library

int distance =0, duration =0, motion_detection=0;

SemaphoreHandle_t xBinarySemaphore;

void Task3_Distance_Tracker(void *pvParameters);
void Task2_Vehicle_Motion_Tracker(void *pvParameters);
void Task1_Warning_Alert(void *pvParameters);

void setup()
{
  Serial.begin(9600); //setting the baud rate
  xBinarySemaphore = xSemaphoreCreateBinary();    //API for creating the Semaphore
  // API for Task Creation
  xTaskCreate(Task3_Distance_Tracker, "Distance_Tracker", 128, NULL, 1, NULL);
  xTaskCreate(Task2_Vehicle_Motion_Tracker, "Motion_Tracker", 128, NULL, 1, NULL);
  xTaskCreate(Task1_Warning_Alert, "Warning_Alert", 128, NULL, 1, NULL);
  xSemaphoreGive(xBinarySemaphore);              //SemaphoreGive API
}
void loop()
{}

void Task3_Distance_Tracker(void* pvParameters)
{
  const int TRIG_PIN = 9;          // choose the trig pin (for Ultrasonic distance sensor)
  const int ECHO_PIN = 10;         // choose the echo pin (for Ultrasonic distance sensor)
  pinMode(TRIG_PIN, OUTPUT);       // declare trig pin as output
  pinMode(ECHO_PIN, INPUT);        // declare echo pin as output
  while(1)
  {
    //To start a new distance measurement set the TRIG pin to high for 10uS or more
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    duration = pulseIn(ECHO_PIN, HIGH);
    //using table given to convert the ECHO pulse length in microseconds into centimeters/meters
    distance = duration / 5800;            //in meters
    xSemaphoreTake(xBinarySemaphore,portMAX_DELAY);   //SemaphoreTake API
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println("m");
    xSemaphoreGive(xBinarySemaphore);                 //SemaphoreGive API
    vTaskDelay(5);
  }
}
 
void Task2_Vehicle_Motion_Tracker(void *pvParameters)
{
  const int INPUT_PIN = 2;               // choose the input pin (for PIR sensor)                 
  int PIR_STATE = LOW;                   // we start, assuming no motion detected
  int Value = 0;                         // variable for reading the pin status

  pinMode(INPUT_PIN, INPUT);     // declare sensor as input

  while(1)   
  {
    Value = digitalRead(INPUT_PIN);  // read input value    
    xSemaphoreTake(xBinarySemaphore,portMAX_DELAY);   //SemaphoreTake API
      if (Value == HIGH) // check if the input is HIGH
      {            
        if (PIR_STATE == LOW) 
        {
          // we have just turned on
          Serial.println("Vehicle Motion detected!");
          motion_detection =1;
        } 
        else if (PIR_STATE == HIGH) 
        {
          // we have just turned off
          Serial.println("No Motion detected!"); 
          motion_detection =0;
        }
      }
    xSemaphoreGive(xBinarySemaphore);                 //SemaphoreGive API
    vTaskDelay(5);
  }
}

void Task1_Warning_Alert(void* pvParameters)
{  
  const int LED = 11;          // choose the LED pin
  pinMode(LED, OUTPUT);        // declare LED as output
  for(;;)
  {
    xSemaphoreTake(xBinarySemaphore,portMAX_DELAY);   //SemaphoreTake API
    //set LED High... ON/OFF rate 
    if (distance<=2 && motion_detection ==1)
    {
      if (distance<=1)
      {
        analogWrite(LED,255);
        Serial.println("COLLISION WARNING !!");
        vTaskDelay(6);
        analogWrite(LED,0);
      }
      else
      {
        analogWrite(LED,127);
      }
    }
    else
    {
      analogWrite(LED,0);
    }
    xSemaphoreGive(xBinarySemaphore);                 //SemaphoreGive API
    vTaskDelay(100);
  }
}