// Hey there! This is an RC Car (Well Almost!)
// Use arrow keys to drive
// Press slide switch or spacebar for headlights
const int DIR_PIN_LEFT = 3;
const int STEP_PIN_LEFT = 4;
const int DIR_PIN_RIGHT = 5;
const int STEP_PIN_RIGHT = 6;
const int ENABLE_PIN = 7;
const int LED_LEFT = 7;   // LED for turning left
const int LED_RIGHT = 2;  // LED for turning right
const int BUTTON_PIN_1 = 8;
const int BUTTON_PIN_2 = 9;
const int BUTTON_PIN_3 = 10;
const int BUTTON_PIN_4 = 11;
const int Headlights = 12;
const int Headlightspin = A0;
const int motorDelay = 1000;

bool lastButtonState = HIGH;
bool ledState = LOW;

void setup() {
  pinMode(DIR_PIN_LEFT, OUTPUT);
  pinMode(STEP_PIN_LEFT, OUTPUT);
  pinMode(DIR_PIN_RIGHT, OUTPUT);
  pinMode(STEP_PIN_RIGHT, OUTPUT);
  pinMode(ENABLE_PIN, OUTPUT);
  pinMode(LED_LEFT, OUTPUT);
  pinMode(LED_RIGHT, OUTPUT);
  pinMode(Headlightspin, OUTPUT);

  pinMode(BUTTON_PIN_1, INPUT_PULLUP);
  pinMode(BUTTON_PIN_2, INPUT_PULLUP);
  pinMode(BUTTON_PIN_3, INPUT_PULLUP);
  pinMode(BUTTON_PIN_4, INPUT_PULLUP);
  pinMode(Headlights, INPUT_PULLUP);

  digitalWrite(ENABLE_PIN, LOW);
  digitalWrite(LED_LEFT, LOW); // LED initially off
  digitalWrite(LED_RIGHT, LOW); // LED initially off
}

void loop() {
  if (digitalRead(BUTTON_PIN_1) == LOW) {
    moveForward();
  } else if (digitalRead(BUTTON_PIN_2) == LOW) {
    moveBackward();
  } else if (digitalRead(BUTTON_PIN_3) == LOW) {
    turnRight();
  } else if (digitalRead(BUTTON_PIN_4) == LOW) {
    turnLeft();
  } else {
    digitalWrite(LED_LEFT, LOW);
    digitalWrite(LED_RIGHT, LOW);
  }

  bool buttonState = digitalRead(Headlights);

  if (buttonState == LOW && lastButtonState == HIGH) {
    ledState = !ledState;
    digitalWrite(Headlightspin, ledState ? HIGH : LOW);
    delay(50);
  }

  lastButtonState = buttonState;
}

void moveForward() {
  digitalWrite(DIR_PIN_LEFT, HIGH);
  digitalWrite(DIR_PIN_RIGHT, HIGH);
  stepMotors();
}

void moveBackward() {
  digitalWrite(DIR_PIN_LEFT, LOW);
  digitalWrite(DIR_PIN_RIGHT, LOW);
  stepMotors();
}

void turnRight() {
  digitalWrite(DIR_PIN_LEFT, HIGH);
  digitalWrite(DIR_PIN_RIGHT, LOW);
  digitalWrite(LED_RIGHT, HIGH); // LED for right turn
  digitalWrite(LED_LEFT, LOW);   // Ensure left LED is off
  stepMotors();
}

void turnLeft() {
  digitalWrite(DIR_PIN_LEFT, LOW);
  digitalWrite(DIR_PIN_RIGHT, HIGH);
  digitalWrite(LED_LEFT, HIGH);  // LED for left turn
  digitalWrite(LED_RIGHT, LOW);  // Ensure right LED is off
  stepMotors();
}

void stepMotors() {
  for (int i = 0; i < 200; i++) {
    digitalWrite(STEP_PIN_LEFT, HIGH);
    digitalWrite(STEP_PIN_RIGHT, HIGH);
    delayMicroseconds(motorDelay);
    digitalWrite(STEP_PIN_LEFT, LOW);
    digitalWrite(STEP_PIN_RIGHT, LOW);
    delayMicroseconds(motorDelay);
  }
}