// Project: Grain Levelizer Robot
// ESP32 LED Control via Serial Monitor

// Define LED Pins
#define led1Pin 4
#define led2Pin 23
#define led3Pin 18
#define led4Pin 19

void setup() {
  // Initialize Serial Communication
  Serial.begin(115200);
  Serial.println("Grain Levelizer Robot - Serial Control Initialized");

  // Set LED Pins as Output
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  pinMode(led3Pin, OUTPUT);
  pinMode(led4Pin, OUTPUT);

  // Ensure all LEDs are OFF initially
  digitalWrite(led1Pin, LOW);
  digitalWrite(led2Pin, LOW);
  digitalWrite(led3Pin, LOW);
  digitalWrite(led4Pin, LOW);
}

void loop() {
  // Check if data is available in Serial Monitor
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');  // Read input until newline
    command.trim();  // Remove any extra spaces or newline characters

    // Turn off all LEDs before processing a new command
    digitalWrite(led1Pin, LOW);
    digitalWrite(led2Pin, LOW);
    digitalWrite(led3Pin, LOW);
    digitalWrite(led4Pin, LOW);

    // Execute Commands
    if (command == "f") {  // Move Forward
      digitalWrite(led1Pin, HIGH);
      digitalWrite(led2Pin, HIGH);
      Serial.println("Action: Forward - LED1 & LED2 ON");
    }
    else if (command == "b") {  // Move Backward
      digitalWrite(led3Pin, HIGH);
      digitalWrite(led4Pin, HIGH);
      Serial.println("Action: Backward - LED3 & LED4 ON");
    }
    else if (command == "r") {  // Turn Right
      digitalWrite(led1Pin, HIGH);
      digitalWrite(led4Pin, HIGH);
      Serial.println("Action: Right Turn - LED1 & LED4 ON");
    }
    else if (command == "l") {  // Turn Left
      digitalWrite(led2Pin, HIGH);
      digitalWrite(led3Pin, HIGH);
      Serial.println("Action: Left Turn - LED2 & LED3 ON");
    }
    else if (command == "s") {  // Stop (All LEDs OFF)
      Serial.println("Action: Stop - All LEDs OFF");
    }
    else {
      Serial.println("Invalid Command! Use: f (Forward), b (Backward), r (Right), l (Left), s (Stop)");
    }
  }
}