#include <Servo.h>
#include <LiquidCrystal.h>

LiquidCrystal lcd(8, 7, 6, 5, 4, 3);

const int buttonPin1 = 2;
const int buttonPin2 = 3;
const int buttonPin3 = 4;
const int buttonPin4 = 5;

volatile bool buttonState1 = false;
volatile bool buttonState2 = false;
volatile bool buttonState3 = false;
volatile bool buttonState4 = false;

volatile bool recoger = false;
volatile bool rotar = false;
volatile bool dejar = false;
volatile bool regresar = false;

Servo servomotor4;
Servo servomotor3;
Servo servomotor2;
Servo servomotor1;


void setup() {
  pinMode(buttonPin1, INPUT);
  pinMode(buttonPin2, INPUT);
  pinMode(buttonPin3, INPUT);
  pinMode(buttonPin4, INPUT);

  digitalWrite(buttonPin1, HIGH);
  digitalWrite(buttonPin2, HIGH);
  digitalWrite(buttonPin3, HIGH);
  digitalWrite(buttonPin4, HIGH);

  attachInterrupt(digitalPinToInterrupt(buttonPin1), button1Interrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(buttonPin2), button2Interrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(buttonPin3), button3Interrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(buttonPin4), button4Interrupt, CHANGE);

  servomotor4.attach(12);
  servomotor3.attach(11);
  servomotor2.attach(10);
  servomotor1.attach(9);

  lcd.begin(16, 2);

}

void loop() {
  if(recoger) {
    recoger = false;
    recogerAction();
  }
  
  if(rotar) {
    rotar = false;
    rotarAction();
  }

  if(dejar) {
    dejar = false;
    dejarAction();
  }

  if(regresar) {
    regresar = false;
    regresarAction();
  }

  //Programamos el LCD para que le de instrucciones al usuario
  lcd.clear();
  lcd.print("Hola");
  delay(2000);
  lcd.clear();
  lcd.print("Presione el");
  lcd.setCursor(0,1);
  lcd.print("pulsador");
  delay(2000);
  lcd.clear();
  lcd.print("Para iniciar");
  delay(3000);
  lcd.clear();
}

void recogerAction() {
  for(int i = 0; i <= 45; i++) {
    servomotor3.write(i);
    delay(20);
  }
  delay(1000);
  for(int i = 0; i <= 90; i++) {
    servomotor2.write(i);
    delay(20);
  }
  delay(1000);
  for(int i = 0; i <= 90; i++) {
    servomotor1.write(i);
    delay(20);
  }
  delay(1000);
  for(int i = 45; i >= 0; i--){
    servomotor3.write(i);
    delay(20);
  }
  delay(1000);
  for(int i = 90; i >= 0; i--) {
    servomotor1.write(i);
    delay(20);
  }
  delay(1000);
  for(int i = 90; i >= 0; i--) {
    servomotor2.write(i);
    delay(20);
  }
  delay(1000);
}

void rotarAction() {
  for(int i = 0; i <= 180; i++); {
    servomotor4.write(i);
    delay(20);
  }
  delay(1000);
}

void dejarAction() {
  for(int i = 0; i <= 90; i++) {
    servomotor2.write(i);
    delay(20);
  }
  delay(1000);
  for(int i = 0; i <= 90; i++) {
    servomotor1.write(i);
    delay(20);
  }
  delay(1000);
  for(int i = 0; i <= 45; i++) {
    servomotor3.write(i);
    delay(25);
  }
  delay(1000);
}

void regresarAction() {
  servomotor1.write(0);
  servomotor2.write(0);
  servomotor4.write(0);
  delay(2000);
}


void button1Interrupt() {
  buttonState1 = !buttonState1;
  if(buttonState1) {
    recoger = true;
  }
}

void button2Interrupt() {
  buttonState2 = !buttonState2;
  if(buttonState2) {
    rotar = true;
  }
}

void button3Interrupt() {
  buttonState3 = !buttonState3;
  if(buttonState3) {
    dejar = true;
  }
}

void button4Interrupt() {
  buttonState4 = !buttonState4;
  if(buttonState3) {
    regresar = true;
  }
}