// Motor A connections
int enablePin1 = 9;
int motor1Pin1 = 8; // LED 3 IN 1 FL
int motor1Pin2 = 7; // LED 4 IN 2 FR
// Motor B connections
int enablePin2 = 4;
int motor2Pin1 = 6; // LED 1 IN 3 BL
int motor2Pin2 = 5; // LED 2 IN 4 BR

int speed = 0;

void moveForward() {
	Serial.println("Forward");
  digitalWrite(motor1Pin1, HIGH); //LED 3 ON FL
  digitalWrite(motor1Pin2, HIGH); // LED 4 OFF FR
  digitalWrite(motor2Pin1, HIGH); // LED 1 ON  BL
  digitalWrite(motor2Pin2, HIGH); // LED 2 OFF BR
}

// void moveBackward() {
// 	Serial.println("Backward");
//   digitalWrite(motor1Pin1, LOW); //LED 3 OFF FL
//   digitalWrite(motor1Pin2, LOW); // LED 4 ON FR
//   digitalWrite(motor2Pin1, LOW); // LED 1 OFF BL
//   digitalWrite(motor2Pin2, LOW); // LED 2 ON BR
// }

void turnLeft() {
	Serial.println("Left");
  digitalWrite(motor1Pin1, LOW); //LED 3 OFF FL
  digitalWrite(motor1Pin2, HIGH); // LED 4 ON FR
  digitalWrite(motor2Pin1, LOW); // LED 1 ON BL
  digitalWrite(motor2Pin2, HIGH); // LED 2 OFF BR
}

void turnRight() {
	Serial.println("Right");
  digitalWrite(motor1Pin1, HIGH); //LED 3 ON FL
  digitalWrite(motor1Pin2, LOW); // LED 4 OFF FR
  digitalWrite(motor2Pin1, HIGH); // LED 1 BL
  digitalWrite(motor2Pin2, LOW); // LED 2 BR
}

void stopCar() {
	Serial.println("Stop");
  digitalWrite(motor1Pin1, LOW); //LED 3 FL
  digitalWrite(motor1Pin2, LOW); // LED 4 FR
  digitalWrite(motor2Pin1, LOW); // LED 1 BL
  digitalWrite(motor2Pin2, LOW); // LED 2 BR
}

void setup() {

	Serial.begin(9600);
  Serial.println("Welcome to the Serial Monitor!");

	// Set all the motor control pins to outputs
	pinMode(enablePin1, OUTPUT);
	pinMode(enablePin2, OUTPUT);

	pinMode(motor1Pin1, OUTPUT); //LED 3
	pinMode(motor1Pin2, OUTPUT); // LED 4
	pinMode(motor2Pin1, OUTPUT); // LED 1
	pinMode(motor2Pin2, OUTPUT); // LED 2
	
	// Turn off motors - Initial state
	// digitalWrite(motor1Pin1, LOW); //LED 3
	// digitalWrite(motor1Pin2, LOW); // LED 4
	// digitalWrite(motor2Pin1, LOW); // LED 1
	// digitalWrite(motor2Pin2, LOW); // LED 2

	// digitalWrite(enablePin1, HIGH);
	// digitalWrite(enablePin2, HIGH);
}

void loop() {
  if (Serial.available()) {
    char command = Serial.read();
    // char command_list[] = {"F","R","L","R","B","S"}
    // for(int i=0;i<sizeof(command))
    if (command == 'F') {
      moveForward();
      // digitalWrite(motor2Pin1, HIGH); // LED 1

    } else if (command == 'L') {
      turnLeft();
    } else if (command == 'R') {
      turnRight();
    } else if (command == 'S') {
      stopCar();
    }
  }
}