#include <MobaTools.h>
#include <Wire.h>
#include <HCPCA9685.h>
#define  I2CAdd 0x40
HCPCA9685 HCPCA9685(I2CAdd);

// Servo Paramiters
// ------NOTE-------\\
// Odd Legs - Normal 0-180 
// Even Legs - Reversed 180-0
// Base legs (e.g 1&2) adds to 180 
//------------------\\
// Servo leg numbers 
  int buttonPin;
  int run;

void setup(){ 
  HCPCA9685.Init(SERVO_MODE);
  delay(1000);
  Serial.begin(9600);
  Serial.println("Loading Full Motion Sequence...");
  Serial.print("Initilising Servo Driver... ");
  HCPCA9685.Sleep(false);
  unsigned int Pos;
  Serial.println("Done");
  Serial.print("Homeing Servos...           ");
  Serial.println("3");
  delay(1000);
  Serial.println("2");
  delay(1000);
  Serial.println(1);
  delay(1000);
  Serial.print("Homing Servos Now...      ");
  HCPCA9685.Servo(0,0); 
  HCPCA9685.Servo(1,180); 
  HCPCA9685.Servo(2,0);
  HCPCA9685.Servo(3,180);
  HCPCA9685.Servo(4,0);
  HCPCA9685.Servo(5,180);
  delay(2000);
  Serial.println("Done");
  Serial.print("Loading Button...           ");
  run = 0; // starts stopped
  buttonPin = 2;
  pinMode(buttonPin, INPUT_PULLUP);
  attachInterrupt(0, buttonPress, LOW);
  delay(1000);
  Serial.println("Done");
  Serial.print("Loading Complete");
  Serial.println();
  Serial.println();
  }
  
void loop(){ 
while(run == 0)
// Motion Code
{
  delay(1);
  Serial.print("Press Button To Begin");
  Serial.println();
  HCPCA9685.Sleep(false);
}
  // Load 2 
    if(run>0){
      delay(1);
      HCPCA9685.Sleep(true);
      Serial.println("DOWN");
      HCPCA9685.Servo(0,0); 
      HCPCA9685.Servo(1,180); 
      HCPCA9685.Servo(2,0);
      HCPCA9685.Servo(3,180);
      HCPCA9685.Servo(4,0);
      HCPCA9685.Servo(5,180);
      delay(4000);
      Serial.println();
    }

  // Load 2 
    if(run>0){
      delay(1);
      Serial.println("UP");
      HCPCA9685.Servo(0,180); 
      HCPCA9685.Servo(1,0); 
      HCPCA9685.Servo(2,180);
      HCPCA9685.Servo(3,0);
      HCPCA9685.Servo(4,180);
      HCPCA9685.Servo(5,0);
      delay(4000);
      Serial.println();
    }

  // Load 2 
    if(run>0){
      delay(1);
      Serial.println("Level");
      HCPCA9685.Servo(0,90); 
      HCPCA9685.Servo(1,90); 
      HCPCA9685.Servo(2,90);
      HCPCA9685.Servo(3,90);
      HCPCA9685.Servo(4,90);
      HCPCA9685.Servo(5,90);
      delay(4000);
      Serial.println();
    }

}
//Interrupt
  void buttonPress(void) 
  {
    static unsigned long last_interrupt_time = 0;
    unsigned long interrupt_time = millis();
    if (interrupt_time - last_interrupt_time > 200) 
    {
      if(run==0)
      {run=225;}
      else{run=0;} 
    }
  last_interrupt_time = interrupt_time;
  }