#include <Arduino.h>
#include <ESP32Servo.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "HX711.h"

// T1 --> T2
//    --> T3
//    --> T4 --> T5

// Define Tasks PINs
const int Entering_ld_cell=17;  // Task 1: sensing approaching train
const int LEDPin = 39;          // Task 2: Flashing LED
const int servoPin = 15;        // Task 3&5: Closing/Opining railway road gate 
const int Leaving_ld_cell=2;    // Task 4: sensing leaving train
const int ld_sck=12;            // Task 1&4 : sensor Clock

Servo gateServo;
HX711 Enter_sensor;
HX711 Leave_sensor;

int LedTaskDelete = 0;
int ReadingOutTaskDelete = 0;
int ServoTaskDelete = 0;
int trainApproach = 0;


// Task 1: sensing approaching train -- 500 ms
void sensingTrainEnteringTask(void *pvParameter) {
  long current_reading = 0.0;
  long prev_reading = 0.0;
  for (;;) {
    if(Enter_sensor.is_ready()){
      current_reading = Enter_sensor.read()/419.8;
      if(current_reading > 0.0 && prev_reading == 0.0 && trainApproach == 0){
        Serial.println("Train approaching the railway road");
        trainApproach = 1;
        xTaskCreatePinnedToCore(controlLedTask, "T2:Flashing LED", 2048, NULL, 3, NULL, 0);
        xTaskCreatePinnedToCore(controlServoCloseTask, "T3:Closing railway road gate", 2048, NULL, 2, NULL, 0);
        xTaskCreatePinnedToCore(sensingTrainLeavingTask, "T4:Sensing train is leaving ", 2048, NULL, 5, NULL, 0);
        LedTaskDelete=0;
      }
      else{
        prev_reading = current_reading;
      }
    }
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

// Task 2: controlling the flashing LED -- 230 ms
void controlLedTask(void *pvParameter) {
  for (;;) { // Infinite loop
    if(LedTaskDelete == 1){
      vTaskDelete(NULL);
    }
      digitalWrite(LEDPin, HIGH); // Turn the LED on
      vTaskDelay(pdMS_TO_TICKS(230)); // Adjust delay for LED blinking frequency

      digitalWrite(LEDPin, LOW); // Turn the LED off
      vTaskDelay(pdMS_TO_TICKS(230)); // Adjust delay for LED blinking frequency
  }
}

// Task 3: controlling the gate to close -- 210 ms
void controlServoCloseTask(void *pvParameter) {
  int startPos = 10;   
  int endPos = 90;    
  int increment = 5;
  gateServo.write(startPos); 
  vTaskDelay(pdMS_TO_TICKS(1500)); // Delay the task by 1.5 second
  Serial.println("Start closing the gate");

  for (int pos = startPos; pos <= endPos; pos += increment) {
    gateServo.write(pos); // Set the servo position
    if(pos == endPos){
      Serial.println("Gate is closed");
      vTaskDelete(NULL);
    }
    else{
      vTaskDelay(pdMS_TO_TICKS(210));
    }
  }

}

// Task 4: sensing leaving train -- 1000 ms
void sensingTrainLeavingTask(void *pvParameter) {
  long current_reading = 0.0;
  long prev_reading = 0.0;
  for (;;) {
    if(Leave_sensor.is_ready()){
      current_reading = Leave_sensor.read()/419.8;
      if(current_reading == 0.0 && prev_reading > 0.0){
        Serial.println("Train passed the railway road");
        LedTaskDelete = 1;
        trainApproach = 0;
        xTaskCreatePinnedToCore(controlServoOpenTask, "T5:Opeining railway road gate", 2048, NULL, 1, NULL, 0);
        vTaskDelete(NULL);
      }
      else{
        prev_reading = current_reading;
      }
    }
    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}

// Task 5: controlling the gate to open -- 210 ms
void controlServoOpenTask(void *pvParameter) {
  int startPos = 90;
  int endPos = 10;    
  int decrement = 5;  
  Serial.println("Start opining the gate");
  for (int pos = startPos; pos >= endPos; pos -= decrement) {
    gateServo.write(pos);
    if(pos == endPos){
      Serial.println("Gate is open");
      vTaskDelete(NULL);
    }
    else{
      vTaskDelay(pdMS_TO_TICKS(210));
    }
  }

}

void setup() {
  Serial.begin(115200);
  pinMode(LEDPin, OUTPUT);
  Leave_sensor.begin(Leaving_ld_cell,ld_sck);
  Enter_sensor.begin(Entering_ld_cell,ld_sck);
  gateServo.attach(servoPin);

  // Create tasks in freeRTOS
  xTaskCreatePinnedToCore(
    sensingTrainEnteringTask,   // Task Function
    "T1:Sensing train entring", // Task Name
    2048,                       // Size of the task's stack in words (not bytes)
    NULL,                       // Pass task parameters
    4,                          // Task priorities: based on the rate of task accuracy
    NULL,                       // Task handle
    0                           // Core
  );
}

void loop() {
  // Empty loop - tasks are running independently
}