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


// Define Tasks PINs
const int LEDPin = 39;         // Task 2: Flashing LED
const int servoPin = 15;      // Servo control pin
const int Leaving_ld_cell=2;
const int ld_sck=12;
const int Entering_ld_cell=17;
Servo gateServo;
HX711 Leave_sensor;
HX711 Enter_sensor;

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

// Task 1: controlling the flashing LED
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 2: Sesnsor to read if the train is passing the railway road
void controlReadingOutTask(void *pvParameter) {
  long current_reading = 0.0;
  long prev_reaing = 0.0;
  for (;;) {
    if(Leave_sensor.is_ready()){
      // Serial.println("pressure reading");
      current_reading = Leave_sensor.read()/419.8;
      if(current_reading == 0.0 && prev_reaing > 0.0){
        Serial.println("Train passed the railway road");
        LedTaskDelete = 1;
        xTaskCreatePinnedToCore(controlServoOpenTask, "ControlGateOpen", 2048, NULL, 1, NULL, 0);
        vTaskDelete(NULL);

      }
      else{
        prev_reaing = current_reading;
      }
      // Serial.println(current_reading);
    }
    vTaskDelay(pdMS_TO_TICKS(230));
  }
}

// Task 3: controlling the gate/servo close
void controlServoCloseTask(void *pvParameter) {
  int startPos = 10;   // Starting position of the servo (0 degrees)
  int endPos = 90;    // Ending position of the servo (180 degrees)
  int increment = 5;  // Step increment size (adjust as needed)
  //gateServo.write(0); 
  vTaskDelay(pdMS_TO_TICKS(1500)); // Delay the task by 1.5 second
  for (int pos = startPos; pos <= endPos; pos += increment) {
    gateServo.write(pos); // Set the servo position
    if(pos == endPos){
      vTaskDelete(NULL);
    }
    else{
        Serial.println("Gate is closed");
      vTaskDelay(pdMS_TO_TICKS(210)); // Delay between each step (500 milliseconds)
    }
  }

}

// Task 4: controlling the gate/servo open
void controlServoOpenTask(void *pvParameter) {
  int startPos = 90;
  int endPos = 10;    // Ending position of the servo (180 degrees)
  int decrement = 5;  // Step increment size (adjust as needed)
  
  for (int pos = startPos; pos >= endPos; pos -= decrement) {
    gateServo.write(pos); // Set the servo position
    if(pos == endPos){
      vTaskDelete(NULL);
    }
    else{
      Serial.println("Gate is open");
      vTaskDelay(pdMS_TO_TICKS(210)); // Delay between each step (500 milliseconds)
    }
  }

}

void setup() {
  Serial.begin(115200);
  pinMode(LEDPin, OUTPUT);
  Leave_sensor.begin(Leaving_ld_cell,ld_sck);
  Enter_sensor.begin(Entering_ld_cell,ld_sck);
  // Attach the servo on the specified pin and open the gate
  gateServo.attach(servoPin);
  //gateServo.write(90);
  // Create tasks in freeRTOS
  xTaskCreatePinnedToCore(controlLedTask, "ControlLED", 2048, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(controlReadingOutTask, "ControlReading", 2048, NULL, 3, NULL, 0);
  xTaskCreatePinnedToCore(controlServoCloseTask, "ControlGateClose", 2048, NULL, 1, NULL, 0);
  //xTaskCreatePinnedToCore(A, B, C, D, E, F, G);
  // A: Task Function
  // B: Task Name
  // C: Size of the task's stack in words (not bytes)
  // D: Pass task parameters
  // E: Task priorities range (from 0 - task range)
  // F:  Task handle
  // G: Core
}

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