#include <Servo.h>

Servo servo1; 
Servo servo2; 

int buttonState0 = 0;
int buttonState1 = 0;
int buttonState2 = 0;
int buttonState3 = 0;
int buttonState4 = 0;
int lastButtonState3 = HIGH;
int buttonPin0 = A0;
int buttonPin1 = A1;
int buttonPin2 = A2;
int buttonPin3 = A3;
int buttonPin4 = A4;
int ledPin = 13;
int speakerPin = 8;
bool ledState = false;

unsigned long lastToggle = 0;
int speakerState = LOW;

void setup() {
  servo1.attach(9);
  servo2.attach(10);
  pinMode(buttonPin0, INPUT_PULLUP);
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(buttonPin3, INPUT_PULLUP);
  pinMode(buttonPin4, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);
  pinMode(speakerPin, OUTPUT);
}

void loop() {
  buttonState0 = digitalRead(buttonPin0);
  buttonState1 = digitalRead(buttonPin1);
  buttonState2 = digitalRead(buttonPin2);
  buttonState3 = digitalRead(buttonPin3);
  buttonState4 = digitalRead(buttonPin4);

  if (buttonState0 == LOW) { //forward
    servo1.write(180);
    servo2.write(0);
  } else if (buttonState1 == LOW) { //left
    servo1.write(90);
    servo2.write(0);
  } else if (buttonState2 == LOW) { //right
    servo1.write(180);
    servo2.write(90);
// Uncomment the section you want to use and comment out the other this changes button A3 to either reverse or LED on and off

  // Reverse
    /*
  } else if (buttonState3 == LOW) { 
    servo1.write(0);
    servo2.write(180);
    digitalWrite(ledPin, LOW); // Ensure LED is off
    */

  // LED
  } else if (buttonState3 == LOW && lastButtonState3 == HIGH) {
    servo1.write(90);
    servo2.write(90);
    ledState = !ledState; // Toggle LED state
    digitalWrite(ledPin, ledState ? HIGH : LOW); // Update LED state
    delay(50);

  } else if (buttonState4 == LOW) { //play tone
    if (micros() - lastToggle >= 1136) { // 1136 microseconds is approximately the period of a 440 Hz tone
      speakerState = !speakerState;
      digitalWrite(speakerPin, speakerState);
      lastToggle = micros();
    }
  } else {
    servo1.write(90); // Stops the servo
    servo2.write(90); // Stops the servo
    digitalWrite(speakerPin, LOW); // Ensure the tone stops
  }

  lastButtonState3 = buttonState3; // Update the last button state for LED switching
}