#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C address 0x3F, 16 column and 2 rows
#define TRIG_PIN 26 // ESP32 pin GIOP26 connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN 25 // ESP32 pin GIOP25 connected to Ultrasonic Sensor's ECHO pin

void Task1( void *pvParameters );
void Task2( void *pvParameters );
void Task3( void *pvParameters );
void Task4( void *pvParameters );
void xDelay(int ms);
SemaphoreHandle_t xSemaphore;

double duration_us, distance_cm;
double dis1[2] = {0,0};
float var = 1;
float Speed;

void setup() {

 xSemaphore = xSemaphoreCreateMutex();
 xTaskCreatePinnedToCore( Task1, "sensor", 1024, NULL ,3 , NULL, 0 );
 xTaskCreatePinnedToCore( Task2, "calculation", 1024, NULL ,2 , NULL, 0 );
 xTaskCreatePinnedToCore( Task3, "display", 2000, NULL ,1 , NULL, 0 );
 xTaskCreatePinnedToCore( Task4, "buzzer", 1024, NULL ,1 , NULL, 0 );
   
  Serial.begin(115200); // Starts the serial communication

  lcd.init();               // initialize the lcd
  lcd.backlight();          // open the backlight
  pinMode(TRIG_PIN, OUTPUT); // config trigger pin to output mode
  pinMode(ECHO_PIN, INPUT);  // config echo pin to input mode
  pinMode(14, OUTPUT);
}

void loop() {
  
}

void Task1(void *pvParameters) // This is a task.
{
 (void) pvParameters;
 while(1) // A Task shall never return or exit.
 {
   // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(100);
  digitalWrite(TRIG_PIN, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(ECHO_PIN, HIGH);

  // calculate the distance
  distance_cm = 0.017 * duration_us;
  vTaskDelay(490 / portTICK_PERIOD_MS);

 }
}
 void Task2(void *pvParameters) // This is a task.
{
 (void) pvParameters;
 while(1)
 {
 dis1[0]=dis1[1];
  dis1[1]=distance_cm;
  Speed = (dis1[1]-dis1[0])*2;
  if (Speed <0)
  Speed = -1*Speed;
   vTaskDelay(490 / portTICK_PERIOD_MS);
 }
}

void Task3(void *pvParameters) // This is a task.
{
 (void) pvParameters;
 while(1)
 {
  xSemaphoreTake( xSemaphore, portMAX_DELAY );
  lcd.backlight();          // open the backlight
  lcd.clear();
  lcd.setCursor(0, 0); // start to print at the first row
  lcd.print("speed: ");
  lcd.print(Speed);
  lcd.print(" cm/s");
  Serial.print("the speed is: ");
  Serial.println(Speed);
   xDelay(445);
   xSemaphoreGive(xSemaphore);
 }
}

 void Task4(void *pvParameters) // This is a task.
{
 (void) pvParameters;
 while(1)
 {     
  if(Speed > 10)
  {  

    digitalWrite(14, HIGH);  // turn the LED on (HIGH is the voltage level)
       xDelay(300);
    digitalWrite(14, LOW);  // turn the LED on (HIGH is the voltage level)

  }
  else
  {      
        vTaskDelay(500 / portTICK_PERIOD_MS);
  }
 }

}

void xDelay(int ms)
{
 long T = millis();
 while( (millis() - T) < ms);
}