#include <HX711.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <AFMotor.h>

// Weighing System
HX711 scale;
LiquidCrystal_I2C lcd(0x27, 16, 2);
const int tareButton = 6;
const float weightLimit = 1000.0; // Set your desired weight limit here (in grams)
unsigned long prevTime = 0;
long checkInterval = 1000;
bool weightExceeded = false;

// Robot Movement System
Servo Project;
#define trig 10 // Trigger pin of ultrasonic sensor
#define echo 11 // Echo pin of ultrasonic sensor
long duration; // Time
long distance; // Distance between object and sensor
AF_DCMotor front1(3);
AF_DCMotor front2(4);
AF_DCMotor back1(1);
AF_DCMotor back2(2);

// Function prototypes
int measureDistance();
void forward();
void backward();
void stop();
void turnleft();
void turnright();
void avoidObstacle();
void sendAlertMessage();

void setup() {
  // Initialize pins for ultrasonic sensor
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

  // Initialize pins for weighing system
  pinMode(tareButton, INPUT_PULLUP);
  Serial.begin(9600);

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("Garbage Robot");
  lcd.setCursor(3, 1);
  lcd.print("Initializing");
  delay(2500);
  lcd.clear();
  lcd.setCursor(3, 0);
  lcd.print("Taring");
  delay(1000);
  lcd.clear();

  // Initialize load cell
  scale.begin(9, 8);
  scale.set_scale(2280.f); // Adjust this calibration factor as needed
  scale.tare();

  // Initialize servo
  Project.attach(5);
  Project.write(90); // Set initial position to center

  // Initialize motor speeds
  front1.setSpeed(255);
  front2.setSpeed(255);
  back1.setSpeed(255);
  back2.setSpeed(255);
}

void loop() {
  // Update the load cell data
  float weight = scale.get_units(10); // Average over 10 readings for stability

  // Tare the scale if the button is pressed
  if (digitalRead(tareButton) == LOW) {
    lcd.clear();
    lcd.setCursor(3, 0);
    lcd.print("Taring");
    delay(1000);
    scale.tare();
    lcd.clear();
  }

  // Check weight and display on LCD
  unsigned long currentTime = millis();
  if (currentTime - prevTime > checkInterval) {
    if (weight > weightLimit) {
      weightExceeded = true;
      // Alert to remove garbage
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Remove Garbage");
      // Implement communication system or buzzer alert here
      // sendAlertMessage();
    } else {
      weightExceeded = false;
      lcd.setCursor(0, 0);
      lcd.print("Weight: ");
      lcd.print(weight, 2); // Display weight with 2 decimal places
      lcd.print("g   ");
      lcd.setCursor(0, 1);
      lcd.print("Limit: ");
      lcd.print(weightLimit, 0);
      lcd.print("g");
    }
    prevTime = currentTime;
  }

  // Control robot movement based on weight limit
  if (!weightExceeded) {
    // Check obstacle distance
    int distance = measureDistance();
    if (distance < 30) {
      avoidObstacle();
    } else {
      forward();
    }
  } else {
    // Weight limit exceeded, stop all movements
    stop();
  }

  // Print weight data to Serial Monitor
  Serial.print("Weight: ");
  Serial.print(weight, 2); // Print weight with 2 decimal places
  Serial.print(" g, Time: ");
  Serial.println(currentTime);
}

void forward() {
  front1.run(FORWARD);
  front2.run(FORWARD);
  back1.run(FORWARD);
  back2.run(FORWARD);
}

void backward() {
  front1.run(BACKWARD);
  front2.run(BACKWARD);
  back1.run(BACKWARD);
  back2.run(BACKWARD);
}

void stop() {
  front1.run(RELEASE);
  front2.run(RELEASE);
  back1.run(RELEASE);
  back2.run(RELEASE);
}

void turnleft() {
  front1.run(FORWARD);
  front2.run(RELEASE);
  back1.run(FORWARD);
  back2.run(RELEASE);
}

void turnright() {
  front1.run(RELEASE);
  front2.run(FORWARD);
  back1.run(RELEASE);
  back2.run(FORWARD);
}

void avoidObstacle() {
  stop(); // Stop the robot
  delay(1000);

  Project.write(180); // Rotate servo left
  delay(1000);
  int leftDis = measureDistance();

  Project.write(0); // Rotate servo right
  delay(1000);
  int rightDis = measureDistance();

  Project.write(90); // Reset servo to center
  delay(1000);

  // Determine the best direction to turn
  if (leftDis >= rightDis) {
    turnleft(); // Turn left
  } else {
    turnright(); // Turn right
  }
  delay(1000);
  stop(); // Stop after avoiding obstacle
}

int measureDistance() {
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  distance = (duration * 0.034) / 2;
  Serial.println(distance);
  return distance;
}

void sendAlertMessage() {
  // Implement your communication system here
  // Example: Send an SMS or notification
}