#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#define TRIG_PIN 13
#define ECHO_PIN 12
#define MAX_DISTANCE 400  // cm (empty basket)
#define MIN_DISTANCE 1    // cm (full basket)

// LCD1: displays basket fullness (I2C address 0x27)
// LCD2: displays action message (I2C address 0x26)
LiquidCrystal_I2C lcd1(0x27, 16, 2);
LiquidCrystal_I2C lcd2(0x26, 16, 2);

long getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  lcd1.init(); lcd1.backlight();
  lcd2.init(); lcd2.backlight();
}

void loop() {
  long d = getDistance();
  int fullness = constrain((int)(((MAX_DISTANCE - d) * 100.0) / (MAX_DISTANCE - MIN_DISTANCE)), 0, 100);
  
  // LCD1: Display basket fullness status
  lcd1.setCursor(0, 0);
  lcd1.print("Rover Status: ");
  
  lcd1.setCursor(0, 1);
  if(fullness >= 80) {
    lcd1.print("Collecting Waste");
  } else if(fullness >= 10) {
    lcd1.print("Roaming...      ");
  } else {
    lcd1.print("Roaming...      ");
  }
  
  // LCD2: Display action message
  lcd2.clear();
  lcd2.setCursor(0, 0);
  if(fullness >= 80) {
    lcd2.print("Going to Collect");
    lcd2.setCursor(0, 1);
    lcd2.print("Waste          ");
  } else {
    lcd2.print("Roaming...      ");
  }
  delay(500);
}