#include <Servo.h>

// ---------- Pin setup ----------
int trigger_pin = 8;
int echo_pin = 9;
int led1 = 11;
int led2 = 12;
Servo ServoHead;
int AservoPin = 2;
int time;
int distance;
const int servoPins[4] = {10, 5, 3, 4};
const int speakerPin = 13;  // connected to a piezzo speaker

// -------- Servo offsets --------
const int leftHipOffset  = 10;
const int rightHipOffset = 5;
const int leftFootOffset  = 4;
const int rightFootOffset = 3;

//const char *melody = "4C42D44E42F46G46C56A46C56G46R06F44F42G46E46E46D44D42E46C46R07C52A44G42G46F46E46D46C46R0"; // I'm a little teapot
const char *melody = "4E52B42C54D52C52B44A42A42C54E52D52C56B42C54D54E54C54A42A42A42B42C56D52F54A52G52F56E52C54E52D52C54B42B42C54D54E54C54A44A44R0"; // Tetris
#include "move.h"
#include "player.h"

Player player(speakerPin);
long bpm = 120L;

void sync()
{
  // Given set BPM, delay until the next quarter of a beat
  static unsigned long lastMillis = 0;
  unsigned long duration = 60000L / (bpm * 4);
  long leftMillis = lastMillis + duration - millis();
  if (leftMillis > 0) delay(leftMillis);
  lastMillis = millis();

  // Update player
  player.sync(bpm);
}


void setup ( ) {
  Serial.begin (9600);
  pinMode (trigger_pin, OUTPUT);
  pinMode (echo_pin, INPUT);
  pinMode (speakerPin, OUTPUT);
  pinMode (led1, OUTPUT);
  pinMode (led2, OUTPUT);
  pinMode( AservoPin, OUTPUT);
  ServoHead.attach(AservoPin);
  // Init legs
  initialize();
  delay(4000);
}

void loop()
{
  ServoHead.write(90);
  digitalWrite (trigger_pin, HIGH);
  delayMicroseconds (10);
  digitalWrite (trigger_pin, LOW);
  time = pulseIn (echo_pin, HIGH);
  digitalWrite (led1, LOW);
  digitalWrite (led2, LOW);
  digitalWrite (speakerPin, LOW);
  delay(1000);
  distance = (time * 0.034) / 2;
  if (distance < 10)
  {
    digitalWrite (speakerPin, HIGH);
    digitalWrite (led1, HIGH);
    ServoHead.write(0);
    delay (1000);
  }
  else ((distance < 20));
  delay(1000);
  distance = (time * 0.034) / 2;
  {
    digitalWrite (speakerPin, LOW);
    digitalWrite (led2, HIGH);
    delay (500);
    ServoHead.write(180);
  }
  delay (20000);
  player.play(melody);
  stand_rythm(2);
  side_left(2);
  hello_left(1);
  walk(2);

  player.play(melody);
  stand_rythm(2);
  side_right(2);
  hello_right(1);
  walk(2);
}