#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <ESP32Servo.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

// Pin definitions
#define HX711_DATA_PIN 19
#define HX711_CLK_PIN 18
#define TARE_BUTTON_PIN 4
#define SERVO_PIN 17  // Assuming GPIO 17 for the servo
// #define LED_PIN 2
#define MOTOR_PIN 23
// LCD Setup
LiquidCrystal_I2C lcd(0x27, 16, 2);  // Updated to 16x2 LCD

// Servo Setup
Servo myServo;

// Keypad setup
const byte ROWS = 4;
const byte COLS = 3;
char keys[ROWS][COLS] = {
  {'1','2','3'},
  {'4','5','6'},
  {'7','8','9'},
  {'*','0','#'}
};
byte rowPins[ROWS] = {32, 33, 25, 26}; // Connect to the row pinouts of the keypad
byte colPins[COLS] = {27, 14, 12}; // Connect to the column pinouts of the keypad
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Global Variables
volatile float weight_threshold = 0.0;
volatile long tare_offset = 0;
float calibration_factor = 420;

void setup() {
  Serial.begin(115200);
  Serial.println("Starting setup...");
  
  pinMode(HX711_CLK_PIN, OUTPUT);
  pinMode(HX711_DATA_PIN, INPUT);
  pinMode(MOTOR_PIN, OUTPUT);
  pinMode(TARE_BUTTON_PIN, INPUT_PULLUP);  // Set tare button pin with pull-up
  myServo.attach(SERVO_PIN);
  myServo.write(0); // Assume 0 is closed

  Wire.begin(13, 16);
  lcd.begin(16, 2);  // Updated to 16x2 LCD initialization
  delay(100); 
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Setup...");

  // Create tasks
  xTaskCreate(WeightMeasurementTask, "WeightMeasurementTask", 4096, NULL, 3, NULL);
  xTaskCreate(KeypadTask, "KeypadTask", 4096, NULL, 2, NULL);
  xTaskCreate(TareButtonTask, "TareButtonTask", 4096, NULL, 1, NULL);  // Tare button task
  xTaskCreate(ServoControlTask, "ServoControlTask", 4096, NULL, 1, NULL);  // Create the Servo task

  Serial.println("ALL TASKS Created successfully");
}

// Define tasks for FreeRTOS
void WeightMeasurementTask(void *parameter) {
  while (1) {
    long raw_value = hx711_read();
    float weight = (float)(raw_value - tare_offset) / calibration_factor; // Assuming calibration factor is defined

    // Display weight on the second row
    lcd.setCursor(0, 1);  // Second row, first character
    lcd.print("Weight: ");
    lcd.print(weight, 2);
    lcd.print("kg    ");  // Extra spaces to clear previous characters

    Serial.print("Weight: ");
    Serial.print(weight, 2);
    Serial.println(" kg");

    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

void KeypadTask(void *parameter) {
  String input = "";  // Temporary string to accumulate input
  while (1) {
    char key = keypad.getKey();
    if (key != NO_KEY) {
      if (key >= '0' && key <= '9') {
        // Append the numeric key to the input
        input += key;
        lcd.setCursor(0, 0);  // Use second row for input display
        lcd.print("Input: ");
        lcd.print(input);
        lcd.print("      ");  // Clear extra characters
      } else if (key == '#') {
        // Finalize the input when # is pressed
        weight_threshold = input.toFloat() / 1000.0;  // Convert to float, input in grams
        lcd.setCursor(0, 0);  // Second row
        lcd.print("Threshold: ");
        lcd.print(weight_threshold, 2);  // Print as kg
        input = "";  // Clear the input for future entries
      } else if (key == '*') {
        digitalWrite(MOTOR_PIN,LOW);
        myServo.write(0);
        // Optionally clear the input when * is pressed
        input = "";
        lcd.setCursor(0, 0);  // Second row
        lcd.print("Cleared        ");  // Clear the display for feedback
      }
    }
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

// Tare button task to update the tare offset
void TareButtonTask(void *parameter) {
  while (1) {
    if (digitalRead(TARE_BUTTON_PIN) == LOW) {
      // Button pressed, capture current raw value as tare offset
      tare_offset = hx711_read();
      lcd.setCursor(0, 0);  // First row for tare status
      lcd.print("Tare Set       ");
      Serial.println("Tare set!");
      delay(1000);  // Simple debounce delay
      lcd.clear(); // Clear the LCD after displaying tare status
    }
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}

// Servo control task to open/close based on the weight threshold
void ServoControlTask(void *parameter) {
  // Ensure the servo is initially at 0 degrees (closed)
  myServo.write(0);
  // digitalWrite(LED_BUILTIN, LOW);
  digitalWrite(MOTOR_PIN, LOW);

  while (1) {
    // Check if threshold is set, if not, skip the task
    if (weight_threshold <= 0) {
      Serial.println("Threshold not set, skipping task...");
      vTaskDelay(pdMS_TO_TICKS(1000));  // Wait before rechecking
      continue;  // Skip to the next loop iteration
    }

    // Read the weight from the sensor
    long raw_value = hx711_read();
    float weight = (float)(raw_value - tare_offset) / calibration_factor;

    // If weight is below the threshold
    if (weight < weight_threshold) {
      Serial.println("Weight below threshold, opening servo...");
      myServo.write(90);  // Open the servo to 90 degrees
      // digitalWrite(LED_BUILTIN, LOW);  // Turn on an LED (or other indicator)
      digitalWrite(MOTOR_PIN, HIGH);    // Activate motor or another peripheral
    } 
    // If weight reaches or exceeds the threshold
    else {
      Serial.println("Threshold reached, closing servo...");
      myServo.write(0);  // Close the servo to 0 degrees
      // digitalWrite(LED_BUILTIN, HIGH);   // Turn off the LED
      digitalWrite(MOTOR_PIN, LOW);     // Deactivate motor
    }

    vTaskDelay(pdMS_TO_TICKS(1000));  // Wait before the next check
  }
}


long hx711_read() {
  long count = 0;
  unsigned char i;

  digitalWrite(HX711_CLK_PIN, LOW);
  while (digitalRead(HX711_DATA_PIN));
  for (i = 0; i < 24; i++) {
    digitalWrite(HX711_CLK_PIN, HIGH);
    delayMicroseconds(1);
    digitalWrite(HX711_CLK_PIN, LOW);
    count = count << 1;
    if (digitalRead(HX711_DATA_PIN)) {
      count++;
    }
  }
  digitalWrite(HX711_CLK_PIN, HIGH);
  count ^= 0x800000;
  delayMicroseconds(1);
  digitalWrite(HX711_CLK_PIN, LOW);
  return count;
}

void loop() {
  // Intentionally empty. Tasks are managed by FreeRTOS.
}