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

SineEase Ease;

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

int Duration = 199;  // time units in easing function, zero indexed
double easedPosition;  // adjustment value to servo between steps
int i; //counter
double t = 0;  //easing time point
int Pos;  // servo position in microseconds
int StepDelay = 3;  // pause between servo commands
int ServoOne;  // distiguishes servos during simultaneous movements
int ServoTwo;
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(12);  // 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 (13000);  // pause before the show
  StartupLean();    // first movement from standing still while facing forward
}
  

void loop() {
  LeftFootStepForward();  // begin walk forward loop
  LeftFootPutDown();
  //sensor looking right
  RightFootStepToBase();  // base refers to base position; facing forward, one leg in the air, hips straight
  // rt leg up
  RightFootStepForward();
  RightFootPutDown();
  //sensor looking left
  LeftFootStepToBase();
  //lt leg up
} 


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


void StartupLean() {
  ServoOne = 2;
  ServoTwo = 1;
  t = 0;
  for( i=0;i<=Duration<=Duration;i++)    {
    t+=1;
    AnkleMove1(ServoOne);
    LeftAnkleServo.writeMicroseconds(Pos);
    AnkleMove2 (ServoTwo);
    Serial.println(Pos);
    RightAnkleServo.writeMicroseconds(Pos);
    delay (StepDelay);
  }
}
void RightFootStepToBase() {
  ServoOne = 1;
  ServoTwo = 2;
  t = 0;
  for( i=0;i<=Duration;i++)    {
    t+=1;
    AnkleMove1(ServoOne);
    RightAnkleServo.writeMicroseconds(Pos);
    AnkleMove2 (ServoTwo);
    LeftAnkleServo.writeMicroseconds(Pos);
    LeftHipServo.writeMicroseconds(Pos);
    HipMove2 (ServoOne);
    RightHipServo.writeMicroseconds(Pos);
    delay (StepDelay);
  }
}
void LeftFootStepToBase() {
  ServoOne = 2;
  ServoTwo = 1;
  t = 0;
  for( i=0;i<=Duration;i++)    {
    t+=1;
    AnkleMove1(ServoOne);
    LeftAnkleServo.writeMicroseconds(Pos);
    AnkleMove2 (ServoTwo);
    RightAnkleServo.writeMicroseconds(Pos);
    HipMove4 (ServoTwo);
    RightHipServo.writeMicroseconds(Pos);
    HipMove4 (ServoOne);
    LeftHipServo.writeMicroseconds(Pos);
    delay (StepDelay);
  }
}
void RightFootStepForward() {
  ServoOne = 2;
  ServoTwo = 1;
  t = 0;
  for( i=0;i<=Duration;i++){
    t+=1;
    HipMove3(ServoOne);
    LeftHipServo.writeMicroseconds(Pos);
    HipMove3 (ServoTwo);
    RightHipServo.writeMicroseconds(Pos);
    delay (StepDelay);
  }
}
void LeftFootStepForward() {
  ServoOne = 1;
  ServoTwo = 2;
  t = 0;
  for( i=0;i<=Duration;i++)    {
    t+=1;
    HipMove1(ServoOne);
    RightHipServo.writeMicroseconds(Pos);
    HipMove1 (ServoTwo);
    LeftHipServo.writeMicroseconds(Pos);
    delay (StepDelay);
  }
}
void RightFootPutDown() {
  ServoOne = 2;
  ServoTwo = 1;
  t = 0;
  for( i=0;i<=Duration;i++)    {
    t+=1;
    AnkleMove3 (ServoTwo);
    RightAnkleServo.writeMicroseconds(Pos);
    AnkleMove4 (ServoOne);
    LeftAnkleServo.writeMicroseconds(Pos);
    delay (StepDelay);
  }
}
void LeftFootPutDown() {
  ServoOne = 1;
  ServoTwo = 2;
  t = 0;
  for( i=0;i<=Duration;i++)    {
   t+=1;
   AnkleMove4 (ServoOne);
   RightAnkleServo.writeMicroseconds(Pos);
   AnkleMove3 (ServoTwo);
   LeftAnkleServo.writeMicroseconds(Pos);
   delay (StepDelay);
  }
}

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


void HipMove1 (int WhichServo) {
  Ease.setTotalChangeInPosition(HipRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeIn(i);
      Pos = RightHipMid + easedPosition;
      break;
    case 2:  // left
      easedPosition=Ease.easeIn(i);
      Pos = LeftHipMid + easedPosition;
      break;
    }
}
void HipMove2 (int WhichServo) {
  Ease.setTotalChangeInPosition(HipRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeInOut(i);
      Pos = RightHipRight - easedPosition;
      break;
    case 2:  // left
      easedPosition=Ease.easeInOut(i);
      Pos = LeftHipRight - easedPosition;
      break;
    }
}
void HipMove3 (int WhichServo) {
  Ease.setTotalChangeInPosition(HipRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeIn(i);
      Pos = RightHipMid - easedPosition;
      break;
    case 2:  // left
      easedPosition=Ease.easeIn(i);
      Pos = LeftHipMid - easedPosition;
      break;
    }
}
void HipMove4 (int WhichServo) {
  Ease.setTotalChangeInPosition(HipRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeInOut(i);
      Pos = RightHipLeft + easedPosition;
      break;
    case 2:  // left
      easedPosition=Ease.easeInOut(i);
      Pos = LeftHipLeft + easedPosition;
      break;
    }
}
void AnkleMove1 (int WhichServo) {
  Ease.setTotalChangeInPosition(AnkleRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeInOut(i);
      Pos = RightAnkleMid + easedPosition;
      break;
    case 2:  // left
      easedPosition=Ease.easeInOut(i);
      Pos = LeftAnkleMid - easedPosition;
      break;
    }
}
void AnkleMove2 (int WhichServo) {
  Ease.setTotalChangeInPosition(AnkleRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeInOut(i);
      Pos = RightAnkleMid - easedPosition;
      break;
    case 2:  // left
      easedPosition=Ease.easeInOut(i);
      Pos = LeftAnkleMid + easedPosition;
      break;
  }
}
void AnkleMove3 (int WhichServo) {
  Ease.setTotalChangeInPosition(AnkleRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeOut(i);
      Pos = RightAnkleDown - easedPosition;
      break;
    case 2:  // left
      easedPosition=Ease.easeOut(i);
      Pos = LeftAnkleDown + easedPosition;
      break;
  }
}
void AnkleMove4 (int WhichServo) {
  Ease.setTotalChangeInPosition(AnkleRangeHalf); 
  switch (WhichServo) {
    case 1:  //right
      easedPosition=Ease.easeOut(i);
      Pos = RightAnkleUp + easedPosition;
      break;
    case 2:  // left
      easedPosition=Ease.easeOut(i);
      Pos = LeftAnkleUp - easedPosition;
      break;
  }
}