#include <VarSpeedServo.h>

int input = A0;

VarSpeedServo servo1;
VarSpeedServo servo2;
VarSpeedServo servo3;
VarSpeedServo servo4;
VarSpeedServo servo5;
VarSpeedServo servo6;

int servo1_pin = 3;
int servo2_pin = 5;
int servo3_pin = 6;
int servo4_pin = 9;
int servo5_pin = 10;
int servo6_pin = 11;

void setup() {
  Serial.begin(9600);
  pinMode(servo1_pin, OUTPUT);
  pinMode(servo2_pin, OUTPUT);
  pinMode(servo3_pin, OUTPUT);
  pinMode(servo4_pin, OUTPUT);
  pinMode(servo5_pin, OUTPUT);
  pinMode(servo6_pin, OUTPUT);
  pinMode(input, INPUT);

  servo1.attach(servo1_pin);
  servo2.attach(servo2_pin);
  servo3.attach(servo3_pin);
  servo4.attach(servo4_pin);
  servo5.attach(servo5_pin);
  servo6.attach(servo6_pin);

  // Set robot to home
  home();
}

void home() {
  // To be able to set it to home, first set servo4 to 20
  // Then, servo6 to 155, to fully close it
  servo1.write(90);
  servo2.write(90);
  servo3.write(90);
  servo5.write(90);
  servo4.write(20, 0, true); // blocking
  servo6.write(155, 0, true); // blocking
  delay(500);
}

void pos1() {
  // rotate
  servo5.write(86, 150);

  servo6.write(100, 150);
  servo4.write(70, 150);

  servo3.write(70, 150, true);

  delay(50);

  servo6.write(70, 150);
  servo4.write(75, 150);
  servo3.write(100, 150, true);

  delay(50);

  servo6.write(80, 150);
  servo4.write(60, 150);
  servo3.write(48, 150, true); // 46?

  delay(50);

  //servo3.write(50, 50, true);

  /*
   * old
  servo4.write(64, 150, true);
  delay(100);
  servo6.write(42, 150);
  */
  servo4.write(64, 50, true);
  delay(100);
  servo6.write(42, 50);
}

void pos2() {
  servo6.write(70, 4);
  servo4.write(50, 4, true);

  delay(500);

  servo4.write(40, 30);
  servo3.write(22, 30, true);

  delay(500);

  servo2.write(60, 50);
  servo5.write(147, 50, true);

  delay(500);

  servo4.write(98, 30);
  servo6.write(50, 30);
  servo3.write(85, 30, true);
}

void pos2_new() {
  servo6.write(70, 4);
  servo4.write(50, 4, true);

  delay(500);

  servo4.write(40, 30);
  servo3.write(22, 30, true);

  delay(500);

  // new begins
  servo4.write(20, 100);
  servo6.write(90, 100);
  servo3.write(0, 100);
  servo5.write(170, 100, true);
}

void pos22_new() {
  servo5.write(90, 50);
  servo6.write(50, 50, true);
  servo4.write(50, 50);
  servo3.write(150, 50, true);

  delay(200);
}

void pos3_home() {
  servo4.write(40, 30);
  servo6.write(70, 30);
  servo3.write(95, 30, true);

  delay(400);

  home();
}

/*
void pos1_audio_jack() {
  servo4.write(60, 150);
  servo3.write(40, 150);
  servo6.write(46, 150);
  servo5.write(83, 150);
  servo1.write(85, 150, true);
  delay(500);
  servo3.write(48, 150, true);
  delay(500);
  servo4.write(65, 150, true); // BAJAR
  delay(500);
}


void pos2_audio_jack() {
  servo4.write(60, 5); // Subir
  servo6.write(52, 5, true); // Subir
  delay(300);

  servo4.write(50, 100);
  servo6.write(70, 150);
  servo3.write(30, 150, true);

  delay(300);

  servo5.write(170, 150, true); // girar

  delay(300);

  // Bajar en drop zone
  servo6.write(50, 150);
  servo4.write(95, 150);
  servo3.write(80, 150, true);

  delay(300);
}
*/

void loop() {
  int input_val = analogRead(input);
  while(input_val < 500) {
    input_val = analogRead(input);
    delay(100);
  }

  pos1();
  input_val = analogRead(input);
  while(input_val < 500) {
    input_val = analogRead(input);
    delay(100);
  }

  pos2();
  input_val = analogRead(input);
  while(input_val < 500) {
    input_val = analogRead(input);
    delay(100);
  }

  pos3_home();





  /*
  int input_val = analogRead(input);
  while(input_val < 500) {
    input_val = analogRead(input);
    delay(100);
  }

  pos1();
  input_val = analogRead(input);
  while(input_val < 500) {
    input_val = analogRead(input);
    delay(100);
  }

  pos2_new();
  input_val = analogRead(input);
  while(input_val < 500) {
    input_val = analogRead(input);
    delay(100);
  }

  pos22_new();
  input_val = analogRead(input);
  while(input_val < 500) {
    input_val = analogRead(input);
    delay(100);
  }

  home();
  */



}