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

Otto Otto;


//--SERVOS FOR FEET--
int LeftFoot = 2; // for left/first foot
int RightFoot = 3; // for right/second foot

//--SERVOS FOR LEGS-- 
int LeftLeg = 5; // for left/first leg
int RightLeg = 6; // for right/second leg

// lefts and rights are in perspective of the viewer, not robot

int distance; // in centimeters
int t; // time taken to recieve ultrasonic ray back
int danceMove; // which dance move

void setup() {
  Otto.init(LeftLeg, RightLeg, LeftFoot, RightFoot, true, 52); //setup otto robot
  Otto.home();
  //ULTRASONIC SENSOR
  pinMode(8, OUTPUT); // trigger pin
  pinMode(9, INPUT); // echo pin

}

void loop() {
  //--ULTRASONIC SENSOR DETECTION--
  digitalWrite(8, LOW); // all this
  delayMicroseconds(2); // code is
  digitalWrite(8, HIGH);// involved in
  delayMicroseconds(10);// casting an
  digitalWrite(8, LOW); // ultrasonic ray

  t = pulseIn(9, HIGH); // to read the time taken to recieve ultrasonic ray back

  distance = 0.01723 * t; // distance is in centimeters

  //--MAIN FUNCTIONALITY--
  if (distance > 25 ) { // dancing
    if (distance > 25) {
      while (distance > 25) {
          danceMove = random(1, 4);
          if (danceMove == 1) {
            Otto.moonwalker(3, 1000, 25,1);
          } if (danceMove == 2) {
            Otto.crusaito(3, 1000, 25, 1);
          } if (danceMove == 3) {
            Otto.flapping(3, 1000, 25, 1);
          } if (danceMove == 4) {
            Otto.swing(3, 1000, 25);
          }
      }
    }

  } if(distance < 25 ) { // walking
      while (distance < 25) { // double check
        Otto.walk(2, 1000, 1);
      }
  }
}