#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Initialize the LCD with I2C address 0x27
LiquidCrystal_I2C lcd(0x27, 20, 4);

// --- 1x5 Keypad Button Pins ---
#define BUTTON_UP 26
#define BUTTON_DOWN 27
#define BUTTON_LEFT 28
#define BUTTON_RIGHT 29
#define BUTTON_ENTER 30

// --- Seed Dispenser Level Pins ---
#define trigPin2 22  // Trigger Pin for Seed Level Sensor
#define echoPin2 23  // Echo Pin for Seed Level Sensor

// Seed Dispenser Level LEDs
#define fullLED 8         // Green LED (100% Full)
#define seventyFiveLED 9  // Yellow LED (75% Full)
#define fiftyLED 10       // Blue LED (50% Full)
#define twentyFiveLED 11  // Red LED (25% Full or less)

// --- Obstacle Detection Pins ---
#define trigPin 24  // Ultrasonic Sensor Trigger
#define echoPin 25  // Ultrasonic Sensor Echo

// Obstacle Detection LEDs
#define stopLED 2    // Red LED (Brake)
#define forwardLED 3 // Green LED (Forward)

// Variables for Seed Dispenser Sensor
long duration2;
int distance2;

// Variables for Obstacle Detection Sensor
long duration;
int distance;

// Variables for LCD and Editing
int currentIndex = 0;         // Current variable index
int currentDigit = 0;         // Active digit (0 = first, 1 = second)
int values[4] = {0, 0, 0, 0}; // Array for Length, Width, Rotational Range, Distance
int inputDigits[2] = {0, 0};  // Temporary input digits for active variable

// --- Setup ---
void setup() {
  // Initialize LCD
  lcd.init();
  lcd.backlight();

  // Initialize Buttons
  pinMode(BUTTON_UP, INPUT_PULLUP);
  pinMode(BUTTON_DOWN, INPUT_PULLUP);
  pinMode(BUTTON_LEFT, INPUT_PULLUP);
  pinMode(BUTTON_RIGHT, INPUT_PULLUP);
  pinMode(BUTTON_ENTER, INPUT_PULLUP);

  // Initialize Seed Dispenser & Level Sensor
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(fullLED, OUTPUT);
  pinMode(seventyFiveLED, OUTPUT);
  pinMode(fiftyLED, OUTPUT);
  pinMode(twentyFiveLED, OUTPUT);

  // Initialize Obstacle Detection Sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(stopLED, OUTPUT);
  pinMode(forwardLED, OUTPUT);

  // Load Initial Values for LCD
  loadCurrentValue();
  updateLCD();
}

void loop() {
  // === Sensor Monitoring ===
  monitorSensors();

  // === Handle Button Inputs ===
  if (digitalRead(BUTTON_UP) == LOW) {
    inputDigits[currentDigit] = (inputDigits[currentDigit] + 1) % 10;
    delay(200); // Debounce
    updateLCD();
  }

  if (digitalRead(BUTTON_DOWN) == LOW) {
    inputDigits[currentDigit] = (inputDigits[currentDigit] - 1 + 10) % 10;
    delay(200); // Debounce
    updateLCD();
  }

  if (digitalRead(BUTTON_LEFT) == LOW) {
    saveCurrentValue();
    currentIndex = (currentIndex - 1 + 4) % 4; // Wrap around
    loadCurrentValue();
    delay(200); // Debounce
    updateLCD();
  }

  if (digitalRead(BUTTON_RIGHT) == LOW) {
    saveCurrentValue();
    currentIndex = (currentIndex + 1) % 4; // Wrap around
    loadCurrentValue();
    delay(200); // Debounce
    updateLCD();
  }

  if (digitalRead(BUTTON_ENTER) == LOW) {
    if (currentDigit == 0) {
      currentDigit = 1; // Move to the second digit
    } else {
      saveCurrentValue();
      currentDigit = 0;   // Reset to the first digit
      currentIndex = (currentIndex + 1) % 4; // Move to next variable
      loadCurrentValue();
    }
    delay(200); // Debounce
    updateLCD();
  }
}

// === Sensor Monitoring Function ===
void monitorSensors() {
  // --- Seed Dispenser Level Sensor ---
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = duration2 * 0.034 / 2;

  digitalWrite(fullLED, LOW);
  digitalWrite(seventyFiveLED, LOW);
  digitalWrite(fiftyLED, LOW);
  digitalWrite(twentyFiveLED, LOW);

  if (distance2 <= 100) {
    digitalWrite(fullLED, HIGH);
  } else if (distance2 > 100 && distance2 <= 200) {
    digitalWrite(seventyFiveLED, HIGH);
  } else if (distance2 > 200 && distance2 <= 300) {
    digitalWrite(fiftyLED, HIGH);
  } else {
    digitalWrite(twentyFiveLED, HIGH);
  }

  // --- Obstacle Detection Sensor ---
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

  if (distance <= 10 && distance > 0) {
    digitalWrite(stopLED, HIGH);
    digitalWrite(forwardLED, LOW);
  } else {
    digitalWrite(stopLED, LOW);
    digitalWrite(forwardLED, HIGH);
  }
}

// === LCD Update Functions ===
void saveCurrentValue() {
  values[currentIndex] = inputDigits[0] * 10 + inputDigits[1];
}

void loadCurrentValue() {
  inputDigits[0] = values[currentIndex] / 10;
  inputDigits[1] = values[currentIndex] % 10;
}

void updateLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Length: ");
  lcd.print(values[0]);
  lcd.setCursor(0, 1);
  lcd.print("Width: ");
  lcd.print(values[1]);
  lcd.setCursor(0, 2);
  lcd.print("Rotation: ");
  lcd.print(values[2]);
  lcd.setCursor(0, 3);
  lcd.print("Distance: ");
  lcd.print(values[3]);

  lcd.setCursor(12, currentIndex);
  lcd.print("<");
  lcd.print(inputDigits[0]);
  lcd.print(inputDigits[1]);
  lcd.print(">");

  if (currentDigit == 0) {
    lcd.setCursor(13, currentIndex);
  } else {
    lcd.setCursor(14, currentIndex);
  }
  lcd.blink();
}