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


// Define Tasks PINs
const int LEDPin = 5;         // Task 2: Flashing LED
const int servoPin = 12;      // Servo control pin
Servo gateServo;
const int ld_cell=2;
const int ld_sck=4;
HX711 scale;

// Task 1: controlling the yellow LED
void controlFlashingLedTask(void *pvParameter) {
  for (;;) { // Infinite loop
    if (yelFlag == 1) {
      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: controlling sensor reading (button)
void controlSensorReadingOutTask(void *pvParameter) {
  int lastState = HIGH;           // indicate that train is not on the railway road yet
  for (;;) {
      if(scale.is_ready()){
      Serial.println("pressure reading");
      long reading=scale.get_units(1);
      scale.get_units(10);
      Serial.println(reading/419.8);
      }

  }
}



// Task 3: controlling the gate/servo
void controlServoCloseTask(void *pvParameter) {
  int startPos = 0; // 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(3000)); // Delay the task by 3 seconds (3000 milliseconds)

  for (int pos = startPos; pos <= endPos; pos += increment) {
    gateServo.write(pos); // Set the servo position
    vTaskDelay(pdMS_TO_TICKS(500)); // Delay between each step (500 milliseconds)
  }
 

}

void setup() {
  Serial.begin(115200);
  pinMode(LEDPin, OUTPUT);
  scale.begin(ld_cell,ld_sck);
  // Attach the servo on the specified pin and open the gate
  gateServo.attach(servoPin);

  // Create tasks in freeRTOS
  xTaskCreatePinnedToCore(controlFlashingLedTask, "ControlLED", 2048, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(controlSensorReadingTask, "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
}