#include <Otto.h>
#include <Servo.h>

Otto Otto;  // Create OttoDIY object
Servo servo_10;  // Head servo

// Pins
#define PIN_YL 2   // left leg
#define PIN_YR 3   // right leg
#define PIN_RL 4   // left foot
#define PIN_RR 5   // right foot
#define PIN_LA 6   // left arm
#define PIN_RA 7   // right arm
#define PIN_Buzzer 13  // buzzer

// Head positions
int Velocidad = 30;
int Pausa = 200;
int Centro = 80;
int Cabeza_RH = 40;  // Right head
int Cabeza_LH = 120; // Left head

long temps1 = 0;

void playSimpleMelody() {
  // A small tone melody (C-D-E) using the buzzer
  Otto._tone(262, 200, PIN_Buzzer);  // C4
  delay(250);
  Otto._tone(294, 200, PIN_Buzzer);  // D4
  delay(250);
  Otto._tone(330, 200, PIN_Buzzer);  // E4
  delay(300);
}

void setup() {
  Otto.init(PIN_YL, PIN_YR, PIN_RL, PIN_RR, true, PIN_Buzzer);
  servo_10.attach(10);  // Attach head servo
}

void loop() {
  if ((millis() - temps1) >= 1000) {
    temps1 = millis();

    // Sing to indicate start
    Otto.sing(S_mode3);
    delay(500);

    // 1. Move forward 10 steps
    Otto.walk(10, 1000, 1);  // FORWARD
    playSimpleMelody();      // Play music
    delay(500);

    // 2. Move backward 10 steps
    Otto.walk(10, 1000, -1); // BACKWARD
    playSimpleMelody();      // Play music
    delay(500);

    // 3. Turn right while head turns right
    servo_10.write(Cabeza_RH);  // Turn head right
    delay(300);
    Otto.turn(5, 1000, 1);  // TURN RIGHT
    playSimpleMelody();     // Play music
    delay(500);

    // 4. Turn left while head turns left
    servo_10.write(Cabeza_LH);  // Turn head left
    delay(300);
    Otto.turn(5, 1000, -1);  // TURN LEFT
    playSimpleMelody();     // Play music
    delay(500);

    // Return head to center
    servo_10.write(Centro);
    delay(300);

    // End sound
    Otto.sing(S_mode3);
    delay(2000);  // Wait before repeating
  }
}