// Motor front-left
int motorFrontLeft_A = 16;
int motorFrontLeft_B = 17;

// Motor front-right
int motorFrontRight_A = 18;
int motorFrontRight_B = 19;

// Motor back-left
int motorBackLeft_A = 12;
int motorBackLeft_B = 14;

// Motor back-right
int motorBackRight_A = 32;
int motorBackRight_B = 33;

int speedMotor = 25; // This should be the PWM pin for controlling speed

int number;

void setup() {
  Serial.begin(115200);
  Serial.println("Board Ready !!!");

  // Set motor pins as outputs
  pinMode(motorFrontLeft_A, OUTPUT);
  pinMode(motorFrontLeft_B, OUTPUT);
  pinMode(motorFrontRight_A, OUTPUT);
  pinMode(motorFrontRight_B, OUTPUT);
  pinMode(motorBackLeft_A, OUTPUT);
  pinMode(motorBackLeft_B, OUTPUT);
  pinMode(motorBackRight_A, OUTPUT);
  pinMode(motorBackRight_B, OUTPUT);

  // Initialize motor speed
  pinMode(speedMotor, OUTPUT);
  analogWrite(speedMotor, 100); // 100 is the initial speed, you can adjust this
}

void loop() {
  if (Serial.available() > 0) {
    // Read the incoming byte
    char incomingByte = Serial.read();
    
    if (incomingByte >= '0' && incomingByte <= '9') {
      // Convert the ASCII value to an integer
      number = incomingByte - '0';
      
      // Print the received number
      Serial.print("Received number: ");
      Serial.println(number);
    }

    // Execute the corresponding function based on the received number
    switch (number) {
      case 1:
        setForward();
        break;
      case 2:
        setBackward();
        break;
      case 3:
        setGoleft();
        break;
      case 4:
        setGoright();
        break;
      case 5:
        setTurnleft();
        break;
      case 6:
        setTurnright();
        break;
      default:
        Serial.println("Invalid input");
        break;
    }
  }

  delay(10); // Small delay for stability
}

void setForward() {
  digitalWrite(motorFrontLeft_A, HIGH);
  digitalWrite(motorFrontLeft_B, LOW);
  digitalWrite(motorFrontRight_A, HIGH);
  digitalWrite(motorFrontRight_B, LOW);
  digitalWrite(motorBackLeft_A, HIGH);
  digitalWrite(motorBackLeft_B, LOW);
  digitalWrite(motorBackRight_A, HIGH);
  digitalWrite(motorBackRight_B, LOW);
  Serial.println("Moving Forward");
}

void setBackward() {
  digitalWrite(motorFrontLeft_A, LOW);
  digitalWrite(motorFrontLeft_B, HIGH);
  digitalWrite(motorFrontRight_A, LOW);
  digitalWrite(motorFrontRight_B, HIGH);
  digitalWrite(motorBackLeft_A, LOW);
  digitalWrite(motorBackLeft_B, HIGH);
  digitalWrite(motorBackRight_A, LOW);
  digitalWrite(motorBackRight_B, HIGH);
  Serial.println("Moving Backward");
}

void setGoleft() {
  digitalWrite(motorFrontLeft_A, HIGH);
  digitalWrite(motorFrontLeft_B, LOW);
  digitalWrite(motorFrontRight_A, LOW);
  digitalWrite(motorFrontRight_B, HIGH);
  digitalWrite(motorBackLeft_A, LOW);
  digitalWrite(motorBackLeft_B, HIGH);
  digitalWrite(motorBackRight_A, HIGH);
  digitalWrite(motorBackRight_B, LOW);
  Serial.println("Go Left");
}

void setGoright() {
  digitalWrite(motorFrontLeft_A, LOW);
  digitalWrite(motorFrontLeft_B, HIGH);
  digitalWrite(motorFrontRight_A, HIGH);
  digitalWrite(motorFrontRight_B, LOW);
  digitalWrite(motorBackLeft_A, HIGH);
  digitalWrite(motorBackLeft_B, LOW);
  digitalWrite(motorBackRight_A, LOW);
  digitalWrite(motorBackRight_B, HIGH);
  Serial.println("Go Right");
}

void setTurnleft() {
  digitalWrite(motorFrontLeft_A, LOW);
  digitalWrite(motorFrontLeft_B, HIGH);
  digitalWrite(motorFrontRight_A, HIGH);
  digitalWrite(motorFrontRight_B, LOW);
  digitalWrite(motorBackLeft_A, LOW);
  digitalWrite(motorBackLeft_B, HIGH);
  digitalWrite(motorBackRight_A, HIGH);
  digitalWrite(motorBackRight_B, LOW);
  Serial.println("Turning Left");
}

void setTurnright() {
  digitalWrite(motorFrontLeft_A, HIGH);
  digitalWrite(motorFrontLeft_B, LOW);
  digitalWrite(motorFrontRight_A, LOW);
  digitalWrite(motorFrontRight_B, HIGH);
  digitalWrite(motorBackLeft_A, HIGH);
  digitalWrite(motorBackLeft_B, LOW);
  digitalWrite(motorBackRight_A, LOW);
  digitalWrite(motorBackRight_B, HIGH);
  Serial.println("Turning Right");
}