#include <Servo.h>

// Define servo objects
Servo servo1;  // Leg 1
Servo servo2;  // Leg 2
Servo servo3;  // Leg 3
Servo servo4;  // Leg 4
Servo headServo; // Dog head

// Bluetooth input
char command;

void setup() {
  // Attach servos to pins
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(9);
  headServo.attach(10);

  // Initialize Bluetooth communication
  Serial.begin(9600);

  // Set initial servo positions
  servo1.write(90);
  servo2.write(90);
  servo3.write(90);
  servo4.write(90);
  headServo.write(90); // Head faces forward
}

void loop() {
  // Check for Bluetooth command
  if (Serial.available()) {
    command = Serial.read();

    switch (command) {
      case 'F': // Move forward
        moveForward();
        break;
      case 'B': // Move backward
        moveBackward();
        break;
      case 'L': // Turn left
        turnLeft();
        break;
      case 'R': // Turn right
        turnRight();
        break;
      case 'S': // Stop
        stop();
        break;
      case 'H': // Move head
        moveHead();
        break;
    }
  }
}

void moveForward() {
  servo1.write(45); // Example forward movement
  servo2.write(135);
  servo3.write(45);
  servo4.write(135);
  delay(500);
  stop();
}

void moveBackward() {
  servo1.write(135); // Example backward movement
  servo2.write(45);
  servo3.write(135);
  servo4.write(45);
  delay(500);
  stop();
}

void turnLeft() {
  servo1.write(45);
  servo2.write(45);
  servo3.write(135);
  servo4.write(135);
  delay(500);
  stop();
}

void turnRight() {
  servo1.write(135);
  servo2.write(135);
  servo3.write(45);
  servo4.write(45);
  delay(500);
  stop();
}

void stop() {
  servo1.write(90);
  servo2.write(90);
  servo3.write(90);
  servo4.write(90);
}

void moveHead() {
  headServo.write(45);  // Turn head left
  delay(500);
  headServo.write(135); // Turn head right
  delay(500);
  headServo.write(90);  // Reset to center
}