#include <Servo.h> 
#include "EasingLibrary.h"
#include <NewPing.h>

#define TRIGGER_PIN  11  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     12  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 15 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

SineEase Ease;

Servo RightHipServo;  // setup servo objects 
Servo LeftHipServo;
Servo RightAnkleServo;
Servo LeftAnkleServo;

int Right = 1;
int Left = 2;
int FootUp;  // for which foot is in the air
int Distance;  //ultrasonic pulse travel time in uS
int DistanceMax = 500;  // minimum ultrasonic flight time to measure
int Duration = 199;  // time units in easing function, zero indexed
double easedPosition;  // adjustment value to servo between steps
int i; //counter
int Pos;  // servo position in microseconds
int StepDelay = 3;  // pause between servo commands
int RightHipLeft = 1210;  // servo positions in uS. left refers to the direction the body turns in relation to the leg 
int RightHipMid = 1360;
int RightHipRight = 1510;
int LeftHipLeft = 1300;  
int LeftHipMid = 1450;
int LeftHipRight = 1600;
int RightAnkleUp = 1140;  // up refers to the outer edge of the foot raising
int RightAnkleMid = 1340;
int RightAnkleDown = 1540;
int LeftAnkleUp = 1720;
int LeftAnkleMid = 1520;
int LeftAnkleDown = 1320;

int HipRangeFull = 300;  // time in uS for full travel, note full never used!
int HipRangeHalf = 150;  // all calcs use half values
int AnkleRangeFull = 400;
int AnkleRangeHalf = 200;


void setup() 
{ 
  RightHipServo.attach(9);  // attaches servos
  LeftHipServo.attach(2);
  RightAnkleServo.attach(3);
  LeftAnkleServo.attach(4);

  // Serial.begin(9600);  // setup for terminal debug

  RightHipServo.writeMicroseconds(RightHipMid);  // center servos
  LeftHipServo.writeMicroseconds(LeftHipMid);
  RightAnkleServo.writeMicroseconds(RightAnkleMid);
  LeftAnkleServo.writeMicroseconds(LeftAnkleMid);
    
  Ease.setDuration(Duration);  //  sets overall time units for easing function

  delay (5000);  // pause before the show
  StartupLean();    // first movement from standing still while facing forward
}
  

void loop() {
  // starts in base position, left foot in the air
  LeftFootStepForward();  // begin walk forward loop
  LeftFootPutDown();  //sensor looking right
  RightFootStepToBase();  // base refers to base position; facing forward, one leg in the air, hips straight; this cycle it's rt leg up
  Navigate();  // sensor check, go-no go decisions, and maneuvering sequences
  RightFootStepForward();
  RightFootPutDown();  //sensor looking left
  LeftFootStepToBase();  //lt leg up
  Navigate();
}



/*******************
movement sequences 
********************/


void StartupLean() {  // leans right
  for( i=0;i<=Duration;i++)    {
    AnkleMove1(Left);    
    AnkleMove2 (Right);
    delay (StepDelay);
  }
}
void StartupLeanLeft() {
  for( i=0;i<=Duration;i++)    {
    AnkleMove1(Right);  
    AnkleMove2 (Left);    
    delay (StepDelay);
  }
}
void RightFootStepToBase() {
  for( i=0;i<=Duration;i++)    {
    AnkleMove1(Right);    
    AnkleMove2 (Left);    
    HipMove2 (Right);
    HipMove2 (Left);    
    delay (StepDelay);
  }
  FootUp = Right;
}
void LeftFootStepToBase() {
  for( i=0;i<=Duration;i++)    {
    AnkleMove1(Left);    
    AnkleMove2 (Right);    
    HipMove4 (Right);    
    HipMove4 (Left);    
    delay (StepDelay);
  }
  FootUp = Left;
}
void RightFootStepForward() {
  for( i=0;i<=Duration;i++){
    HipMove3(Left);    
    HipMove3 (Right);    
    delay (StepDelay);
  }
}
void LeftFootStepForward() {
  for( i=0;i<=Duration;i++) {
    HipMove1(Right);    
    HipMove1 (Left);    
    delay (StepDelay);
  }
}
void RightFootPutDown() {
  for( i=0;i<=Duration;i++)    {
    AnkleMove3 (Right);
    AnkleMove4 (Left);    
    delay (StepDelay);
  }
}
void LeftFootPutDown() {
  for( i=0;i<=Duration;i++)    {
   AnkleMove4 (Right);   
   AnkleMove3 (Left);   
   delay (StepDelay);
  }
}
void BeginTurnRight() { // make following beginturnright
  for( i=0;i<=Duration;i++)    {
    HipMove3(Right);
    delay (StepDelay);
  }
}      
void BeginTurnLeft() {  // make following beginturnleft
  for( i=0;i<=Duration;i++)    {
    HipMove1(Left);
    delay(StepDelay);
  }
}
void PivotLeft() {  // make following pivotleft
  StartupLeanLeft();
  for( i=0;i<=Duration;i++)    {
    HipMove2(Left);    
    delay(StepDelay);  
  }
  RightFootPutDown();
}
void PivotRight() {  // make following pivotright
  StartupLean();
  for( i=0;i<=Duration;i++)    {
    HipMove4(Right);     
    delay(StepDelay);  
  }
  LeftFootPutDown();
}
void TurnLeft() {
  StartupLean();
  BeginTurnLeft();
  LeftFootPutDown();
  PivotLeft();
}
void TurnRight() {
  StartupLeanLeft();
  BeginTurnRight();
  RightFootPutDown();
  PivotRight();
}
  
      

/************************************************
individual moves                                *
*************************************************/


void HipMove1 (int WhichServo) {
  Ease.setTotalChangeInPosition(HipRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeIn(i);
      Pos = RightHipMid + easedPosition;
      RightHipServo.writeMicroseconds(Pos);
      break;
    case 2:  // left
      easedPosition=Ease.easeIn(i);
      Pos = LeftHipMid + easedPosition;
      LeftHipServo.writeMicroseconds(Pos);
      break;
    }
}
void HipMove2 (int WhichServo) {
  Ease.setTotalChangeInPosition(HipRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeInOut(i);
      Pos = RightHipRight - easedPosition;
      RightHipServo.writeMicroseconds(Pos);
      break;
    case 2:  // left
      easedPosition=Ease.easeInOut(i);
      Pos = LeftHipRight - easedPosition;
      LeftHipServo.writeMicroseconds(Pos);
      break;
    }
}
void HipMove3 (int WhichServo) {
  Ease.setTotalChangeInPosition(HipRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeIn(i);
      Pos = RightHipMid - easedPosition;
      RightHipServo.writeMicroseconds(Pos);
      break;
    case 2:  // left
      easedPosition=Ease.easeIn(i);
      Pos = LeftHipMid - easedPosition;
      LeftHipServo.writeMicroseconds(Pos);
      break;
    }
}
void HipMove4 (int WhichServo) {
  Ease.setTotalChangeInPosition(HipRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeInOut(i);
      Pos = RightHipLeft + easedPosition;
      RightHipServo.writeMicroseconds(Pos);
      break;
    case 2:  // left
      easedPosition=Ease.easeInOut(i);
      Pos = LeftHipLeft + easedPosition;
      LeftHipServo.writeMicroseconds(Pos);
      break;
    }
}
void AnkleMove1 (int WhichServo) {
  Ease.setTotalChangeInPosition(AnkleRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeInOut(i);
      Pos = RightAnkleMid + easedPosition;
      RightAnkleServo.writeMicroseconds(Pos);
      break;
    case 2:  // left
      easedPosition=Ease.easeInOut(i);
      Pos = LeftAnkleMid - easedPosition;
      LeftAnkleServo.writeMicroseconds(Pos);
      break;
    }
}
void AnkleMove2 (int WhichServo) {
  Ease.setTotalChangeInPosition(AnkleRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeInOut(i);
      Pos = RightAnkleMid - easedPosition;
      RightAnkleServo.writeMicroseconds(Pos);
      break;
    case 2:  // left
      easedPosition=Ease.easeInOut(i);
      Pos = LeftAnkleMid + easedPosition;
      LeftAnkleServo.writeMicroseconds(Pos);
      break;
  }
}
void AnkleMove3 (int WhichServo) {
  Ease.setTotalChangeInPosition(AnkleRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeOut(i);
      Pos = RightAnkleDown - easedPosition;
      RightAnkleServo.writeMicroseconds(Pos);
      break;
    case 2:  // left
      easedPosition=Ease.easeOut(i);
      Pos = LeftAnkleDown + easedPosition;
      LeftAnkleServo.writeMicroseconds(Pos);
      break;
  }
}
void AnkleMove4 (int WhichServo) {
  Ease.setTotalChangeInPosition(AnkleRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeOut(i);
      Pos = RightAnkleUp + easedPosition;
      RightAnkleServo.writeMicroseconds(Pos);
      break;
    case 2:  // left
      easedPosition=Ease.easeOut(i);
      Pos = LeftAnkleUp - easedPosition;
      LeftAnkleServo.writeMicroseconds(Pos);
      break;
  }
}
void Navigate() {
  GetDistance();
  if (Distance < DistanceMax && Distance > 0) {  
    switch (FootUp) {  // shift to start standing position, aka position A
      case 1:  // right foot up, will turn left      
      RightFootPutDown();
      TurnLeft();
      GetDistance();
      GoNoGo();
      StartupLean();     // restart with StartupLean
      LeftFootStepForward();  
      LeftFootPutDown();  
      RightFootStepToBase();  // returns to position before avoidance loop
      break;
    
    case 2:
      LeftFootPutDown();
      TurnRight();
      GetDistance();
      GoNoGo();
      StartupLeanLeft();
      RightFootStepForward();
      RightFootPutDown(); 
      LeftFootStepToBase();  // restarts and returns to position before avoidance loop
break;
    }
  }
}
void GetDistance() {
  Distance = sonar.ping(); // Send ping, get ping time in microseconds (uS).
}
void GoNoGo() {
      if (Distance < DistanceMax && Distance > 0) {
        do {
          switch (FootUp) {
            case 1:
              TurnLeft();
              break;
            case 2:
              TurnRight();
              break;
          }        
          GetDistance();  // read distance again
        } while (Distance < DistanceMax && Distance > 0);
      }   
}