#define SERVO_PIN0      12 
#define SERVO_PIN1      13 

#define SERVO_PIN0      2 
#define SERVO_PIN1      3 
#define SERVO_PIN2      4   

#define SERVO_PIN0      9 
#define SERVO_PIN1      10 
#define SERVO_PIN2      11   

#define SERVO_PIN0      5 
#define SERVO_PIN1      6 
#define SERVO_PIN2      7   
#define SERVO_PIN2      8   

#define JOYSTICK_PIN  A6                // Joystick analog input
#define SPEED         100                // degree/s

#include <Servo.h>
//#include <Adafruit_PWMServoDriver.h>

#define NUM_SERVOS 12
Servo servoArr[NUM_SERVOS];

//Servo servoArr[] = {servo0,servo1,servo2};                          // create servo object to control a servo

#include <Ramp.h>

unsigned long runTime;

int servoInitPos[] = {180,180,180,
                      180,180,180,
                      180,180,180,
                      180,180,180,180
};

ramp s0=180, // neckupdown
     s1=180, //neckleftright
     s2=180, //jaw
     s3=180, //llshoulder
     s4=180, //llelbow
     s5=180, //llwrist
     s6=180, //rlshoulder
     s7=180, //rlelbow
     s8=180, //rlwrist
     s9=180, //neck nw
     s10=180, //neck ne
     s11=180, //neck se
     s12=180; //neck sw

ramp servoPos[] = {s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12};   

int lls[] = {60,90};
int llsstate = 0;

int lle[] = {150,90};
int llestate = 0;

int llw[] = {0,60};
int llwstate = 0;

int rls[] = {130,60};
int rlsstate = 0;

int rle[] = {40,150};
int rlestate = 0;

int rlw[] = {0,70};
int rlwstate = 0;


void updateServo() {

  for(int i=0; i<12; i++){
    servoPos[i].update();
    //Serial.print(i);
    //Serial.print(": ");
    //Serial.println(servoPos[i].getValue()); 
    servoArr[i].write(servoPos[i].getValue());
    //delay(10);
  }

}

void moveInitPosition(){

  Serial.println("moveINIT"); 
  for(int i=0;i<12;i++){
      //uint32_t duration = 1000.* (float)abs(servoPos[i].getValue()-servoInitPos[i])/SPEED;
      servoPos[i].go(servoInitPos[i],2000);
      //Serial.print(i + ": ");
      //Serial.println(servoInitPos[i]); 
      //updateServo();
      //Serial.println("init done");
  } 

}

void updatePositions(bool init = false) {
   
    // MOVE LEFT LEG
    if(servoPos[3].isFinished()){
      servoPos[3].go(lls[llsstate],1000); // LEFT SHOULDER
      llsstate == 0 ? llsstate = 1 : llsstate = 0;
    }
    if(servoPos[4].isFinished()){
      servoPos[4].go(lle[llestate],1000); // LEFT ELBOW
      llestate == 0 ? llestate = 1 : llestate = 0;
    }
    if(servoPos[5].isFinished()){
      servoPos[5].go(llw[llwstate],1000); // LEFT WRIST
      llwstate == 0 ? llwstate = 1 : llwstate = 0;
    }
/*
    // MOVE RIGHT LEG
    if(servoPos[7].isFinished()){
      servoPos[7].go(rls[rlsstate],1000); // RIGHT SHOULDER
      rlsstate == 0 ? rlsstate = 1 : rlsstate = 0;
    }
    if(servoPos[8].isFinished()){
      servoPos[8].go(rle[rlestate],1000); // RIGHT ELBOW
      rlestate == 0 ? rlestate = 1 : rlestate = 0;
    }
    if(servoPos[9].isFinished()){
      servoPos[9].go(rlw[rlwstate],1000); // RIGHT WRIST
      rlwstate == 0 ? rlwstate = 1 : rlwstate = 0;
    }
*/
    // NECK


}

void setup() {
  Serial.begin(115200);
   for( int i=0; i<NUM_SERVOS; i++)
   {
      servoArr[i].attach(i + 2);      
   }
   //delay(5000);           // time to get to the first position
   moveInitPosition();
   
}

int step=0;

void tiltHeadLeft(){
  servoPos[8].go(90,1000); //NW
  servoPos[9].go(180,1000); // NE
  servoPos[10].go(180,1000); //SE
  servoPos[11].go(90,1000); //SW
}

void tiltHeadRight(){
  servoPos[8].go(180,1000); //NW
  servoPos[9].go(90,1000); // NE
  servoPos[10].go(90,1000); //SE
  servoPos[11].go(180,1000); //SW
}

void tiltHeadCenter(){
  servoPos[8].go(180,1000); //NW
  servoPos[9].go(180,1000); // NE
  servoPos[10].go(180,1000); //SE
  servoPos[11].go(180,1000); //SW
}

void turnHeadCenter(){ servoPos[1].go(90,1000); }
void turnHeadLeft(){ servoPos[1].go(180,1000); }
void turnHeadRight(){ servoPos[1].go(0,1000); }

void liftHeadCenter(){ servoPos[0].go(90,1000); }
void liftHeadDown(){ servoPos[0].go(180,1000); }
void liftHeadUp(){ servoPos[0].go(0,1000); }

int barkStatus=0;

void bark(){
    
    for(int i=0;i<12;i++){

      if(i < 3 || i > 8){
        Serial.print("test: ");
        Serial.println(i);
      }

    }

    if(servoPos[2].isFinished() && barkStatus == 0){
      Serial.println("bark");
      servoPos[2].go(90,500,SINUSOIDAL_INOUT,ONCEBACKWARD);
      //barkStatus=1;
    }

/*
    if(servoPos[2].isFinished() && barkStatus == 1){
      Serial.println("bark stop");
      servoPos[2].go(180,500);
      barkStatus=0;
    }
    */
}



void loop() {

  runTime = millis();


 // Serial.println(runTime);
  //if(runTime >= 5000){
  //  updatePositions();               // update joystick value
  //}

 if(runTime >= (1000 * 5) && step == 0){
    //tiltHeadCenter();
    //turnHeadCenter();
    bark();
    step++;
  }

  if(runTime >= (1000 * 10) && step == 1){
    //tiltHeadLeft();
    //turnHeadLeft();
        bark();

    step++;
  }
  
  if(runTime >= (1000 * 15) && step == 2){
    //tiltHeadRight();
    //turnHeadLeft();
    bark();
    step++;
  }

  if(runTime>= (1000 * 20) && step == 3){
    //tiltHeadCenter();
    //turnHeadCenter();
    step++;
  }

  if(barkStatus == 1){
    bark();
  }



  updateServo();                  // update servo according to joystick value
  delay(10);                      // lazy way to limit update to 100 Hz
}



void tiltNeckPos(int dir, int pos, int ms) {
  pos = (pos < 100) ? 100 : pos;
  ms = (ms < 500) ? 500 : ms;

  int positions[5][4] = {
    {pos, pos, 180, 180},    // dir == 12
    {180, pos, pos, 180},    // dir == 3
    {180, 180, pos, pos},    // dir == 6
    {pos, 180, 180, pos},    // dir == 9
    {180, 180, 180, 180}     // default
  };

  int* p = positions[4];  // default

  switch (dir) {
    case 12: p = positions[0]; break;
    case 3:  p = positions[1]; break;
    case 6:  p = positions[2]; break;
    case 9:  p = positions[3]; break;
  }

  for (int i = 0; i < 4; i++) {
    // REMEMBER TO UPDATE SERVOPOS TO CORRECT!
    servoPos[8 + i].go(p[i], ms, SINUSOIDAL_INOUT);
  }
}

void rotateHeadPos(int pos, int ms, bool center=false) {

  ms = (ms < 500) ? 500 : ms;
  pos = (pos < 10) ? 10 : pos; //RIGHT
  pos = (pos < 170) ? 170 : pos; // LEFT
  if(center == true){ pos = 90; }
  // CHECK SERVO POS
  servoPos[1].go(pos,ms, SINUSOIDAL_INOUT);

}


void tiltHeadPos(int pos, int ms, bool center=false){

    ms = (ms < 500) ? 500 : ms;
    pos = (pos =< 60) ? 60 : pos; // UP
    pos = (pos >= 100) ? 100 : pos; // DOWN
    if(center == true){ pos = 80; }
    // CHECK SERVO POS
    servoPos[0].go(pos,ms, SINUSOIDAL_INOUT);

}

void bark(){

  if(servoPos[2].isFinished()){
      servoPos[2].go(110,500,SINUSOIDAL_INOUT,ONCEBACKWARD);
      Serial.println("BARK");
  }

}