/*
  ESP32 Robot Manual Control with L298N Motor Driver and Buttons
  
  Hardware:
  - ESP32 Development Board
  - L298N Motor Driver
  - 2 DC Motors
  - 5 Push Buttons
  
  Connections:
  L298N IN1 -> ESP32 Pin 14
  L298N IN2 -> ESP32 Pin 15
  L298N IN3 -> ESP32 Pin 13
  L298N IN4 -> ESP32 Pin 12
  
  Button Forward -> ESP32 Pin 2
  Button Stop -> ESP32 Pin 17
  Button Backward -> ESP32 Pin 4
  Button Right -> ESP32 Pin 5
  Button Left -> ESP32 Pin 16
  
  Note: Buttons should be connected between pin and GND (using internal pullup)
*/

// Motor control pins
#define IN1 14  // L298N IN1
#define IN2 15  // L298N IN2
#define IN3 13  // L298N IN3
#define IN4 12  // L298N IN4

// Button pins
#define BTN_FORWARD  2
#define BTN_STOP     17
#define BTN_BACKWARD 4
#define BTN_RIGHT    5
#define BTN_LEFT     16

// Variables for button debouncing
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;
String currentAction = "STOPPED";

void setup() {
  // Initialize Serial Monitor
  Serial.begin(115200);
  Serial.println("ESP32 Robot Manual Control Starting...");
  
  // Set motor control pins as outputs
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  
  // Set button pins as inputs with internal pullup resistors
  pinMode(BTN_FORWARD, INPUT_PULLUP);
  pinMode(BTN_STOP, INPUT_PULLUP);
  pinMode(BTN_BACKWARD, INPUT_PULLUP);
  pinMode(BTN_RIGHT, INPUT_PULLUP);
  pinMode(BTN_LEFT, INPUT_PULLUP);
  
  // Ensure motors are stopped initially
  stopMotors();
  
  Serial.println("Robot initialized. Ready for manual control!");
  Serial.println("Button Controls:");
  Serial.println("- Forward Button (Pin 2): Move Forward");
  Serial.println("- Stop Button (Pin 17): Stop Robot");
  Serial.println("- Backward Button (Pin 4): Move Backward");
  Serial.println("- Right Button (Pin 5): Turn Right");
  Serial.println("- Left Button (Pin 16): Turn Left");
  Serial.println("----------------------------------------");
}

void loop() {
  // Read button states (LOW = pressed due to pullup)
  bool forwardPressed = !digitalRead(BTN_FORWARD);
  bool stopPressed = !digitalRead(BTN_STOP);
  bool backwardPressed = !digitalRead(BTN_BACKWARD);
  bool rightPressed = !digitalRead(BTN_RIGHT);
  bool leftPressed = !digitalRead(BTN_LEFT);
  
  // Simple debouncing
  if (millis() - lastDebounceTime > debounceDelay) {
    String newAction = currentAction;
    
    // Check which button is pressed (priority order)
    if (stopPressed) {
      stopMotors();
      newAction = "STOPPED";
    }
    else if (forwardPressed) {
      moveForward();
      newAction = "MOVING FORWARD";
    }
    else if (backwardPressed) {
      moveBackward();
      newAction = "MOVING BACKWARD";
    }
    else if (leftPressed) {
      turnLeft();
      newAction = "TURNING LEFT";
    }
    else if (rightPressed) {
      turnRight();
      newAction = "TURNING RIGHT";
    }
    else if (!forwardPressed && !backwardPressed && !leftPressed && !rightPressed && !stopPressed) {
      // No button pressed, stop the robot
      stopMotors();
      newAction = "STOPPED";
    }
    
    // Display action change
    if (newAction != currentAction) {
      Serial.println("Action: " + newAction);
      currentAction = newAction;
      lastDebounceTime = millis();
    }
  }
  
  // Small delay to prevent excessive CPU usage
  delay(10);
}

// Function to move robot forward
void moveForward() {
  digitalWrite(IN1, HIGH);  // Left motor forward
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);  // Right motor forward
  digitalWrite(IN4, LOW);
}

// Function to move robot backward
void moveBackward() {
  digitalWrite(IN1, LOW);   // Left motor backward
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);   // Right motor backward
  digitalWrite(IN4, HIGH);
}

// Function to turn robot left
void turnLeft() {
  digitalWrite(IN1, LOW);   // Left motor backward
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);  // Right motor forward
  digitalWrite(IN4, LOW);
}

// Function to turn robot right
void turnRight() {
  digitalWrite(IN1, HIGH);  // Left motor forward
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);   // Right motor backward
  digitalWrite(IN4, HIGH);
}

// Function to stop all motors
void stopMotors() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}