#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)

// Define Motor 1 (Servo Motor) pins
#define EN1 33   // Enable pin for Motor 1
#define IN1 34   // Input pin 1 for Motor 1
#define IN2 35   // Input pin 2 for Motor 1

// Define Motor 2 (Linear Actuator) pins
#define EN2 38   // Enable pin for Motor 2
#define IN3 36   // Input pin 1 for Motor 2
#define IN4 37   // Input pin 2 for Motor 2

// Define LEDs
#define YELLOW_LED 12   // LED for Inductive Sensor 1
#define GREEN_LED 13    // LED for Inductive Sensor 2
#define WHITE_LED 46    // LED for Limit Switch

#define CW_LED1 47    // Violet LED for Motor 1 (Clockwise)
#define CCW_LED1 48   // LightBlue LED for Motor 1 (Counterclockwise)

// Define buttons
#define YELLOW_BUTTON 31  // Button for Inductive Sensor 1
#define GREEN_BUTTON 32   // Button for Inductive Sensor 2
#define WHITE_BUTTON 45   // Button for Limit Switch

// 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

// Blinking interval (milliseconds)
const unsigned long BLINK_INTERVAL = 500;

// Variables to store the last blink time for each LED
unsigned long previousYellowTime = 0;
unsigned long previousGreenTime = 0;
unsigned long previousWhiteTime = 0;

// Variables to track the current LED states
bool yellowState = false;
bool greenState = false;
bool whiteState = false;

// State variables
bool yellowActive = false;
bool greenActive = false;
bool whiteActive = false;


// --- 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();

   // Set motor control pins as outputs
  pinMode(EN1, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(EN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // Set LED pins as outputs
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(WHITE_LED, OUTPUT);
  pinMode(CW_LED1, OUTPUT);
  pinMode(CCW_LED1, OUTPUT);


  // Set button pins as inputs with pull-up resistors
  pinMode(YELLOW_BUTTON, INPUT_PULLUP);
  pinMode(GREEN_BUTTON, INPUT_PULLUP);
  pinMode(WHITE_BUTTON, INPUT_PULLUP);

  // Initialize all outputs to LOW
  resetOutputs();
}

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();
  }

  // Read button states
  bool yellowButton = digitalRead(YELLOW_BUTTON) == LOW;
  bool greenButton = digitalRead(GREEN_BUTTON) == LOW;
  bool whiteButton = digitalRead(WHITE_BUTTON) == LOW;

  unsigned long currentTime = millis(); // Get the current time

  // Sequence logic
  if (yellowButton && !yellowActive) {
    resetOutputs();         // Turn off all outputs before activating yellow
    yellowActive = true;
    greenActive = false;
    whiteActive = false;
  } else if (greenButton && !greenActive) {
    resetOutputs();         // Turn off all outputs before activating green
    yellowActive = false;
    greenActive = true;
    whiteActive = false;
  } else if (whiteButton && !whiteActive) {
    resetOutputs();         // Turn off all outputs before activating white
    yellowActive = false;
    greenActive = false;
    whiteActive = true;
  }

  // Execute the active function
  if (yellowActive) {
    controlYellow(currentTime);
  } else if (greenActive) {
    controlGreen(currentTime);
  } else if (whiteActive) {
    controlWhite(currentTime);
  }
}

// === 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();
}

void resetOutputs() {
  // Turn off all LEDs
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(WHITE_LED, LOW);
  digitalWrite(CW_LED1, LOW);
  digitalWrite(CCW_LED1, LOW);


  // Disable both motors
  digitalWrite(EN1, LOW);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(EN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

void controlYellow(unsigned long currentTime) {
  // Blink the Yellow LED at the specified interval
  if (currentTime - previousYellowTime >= BLINK_INTERVAL) {
    previousYellowTime = currentTime;
    yellowState = !yellowState; // Toggle LED state
    digitalWrite(YELLOW_LED, yellowState);
  }
  digitalWrite(EN1, HIGH);        // Enable Motor 1
  digitalWrite(IN1, HIGH);        // Rotate Motor 1 Clockwise
  digitalWrite(IN2, LOW);
  digitalWrite(CW_LED1, HIGH);    // Turn on CW LED for Motor 1
  digitalWrite(CCW_LED1, LOW);    // Turn off CCW LED for Motor 1
}

void controlGreen(unsigned long currentTime) {
  // Blink the Green LED at the specified interval
  if (currentTime - previousGreenTime >= BLINK_INTERVAL) {
    previousGreenTime = currentTime;
    greenState = !greenState; // Toggle LED state
    digitalWrite(GREEN_LED, greenState);
  }
  digitalWrite(EN2, HIGH);        // Enable Motor 2
  digitalWrite(IN3, HIGH);        // Rotate Motor 2 Clockwise
  digitalWrite(IN4, LOW);
  digitalWrite(CW_LED1, HIGH);    // Turn on CW LED for Motor 2
  digitalWrite(CCW_LED1, LOW);    // Turn off CCW LED for Motor 2
}

void controlWhite(unsigned long currentTime) {
  // Blink the White LED at the specified interval
  if (currentTime - previousWhiteTime >= BLINK_INTERVAL) {
    previousWhiteTime = currentTime;
    whiteState = !whiteState; // Toggle LED state
    digitalWrite(WHITE_LED, whiteState);
  }
  digitalWrite(EN2, HIGH);        // Enable Motor 2
  digitalWrite(IN3, LOW);         // Rotate Motor 2 Counterclockwise
  digitalWrite(IN4, HIGH);
  digitalWrite(CW_LED1, LOW);     // Turn off CW LED for Motor 2
  digitalWrite(CCW_LED1, HIGH);   // Turn on CCW LED for Motor 2
}