#include <Servo.h>
#include <Stepper.h>

// Pines de los botones
const int buttonPin1 = 2;
const int buttonPin2 = 3;
const int buttonPin3 = 4;

// Variables para almacenar el estado de los botones
volatile bool buttonState1 = false;
volatile bool buttonState2 = false;
volatile bool buttonState3 = false;

// Variables para controlar las acciones del brazo robótico
volatile bool pickObject = false;
volatile bool rotateObject = false;
volatile bool releaseObject = false;
volatile bool returnToInitial = false;

// Instancias de los servomotores
Servo servomotor3; // Servomotor pinza
Servo servomotor2; // Servomotor en medio
Servo servomotor1; // Servomotor abajo

// Instancia del motor paso a paso
Stepper motor(2048, 4, 6, 5, 7);

void setup() {
  // Configurar pines de los botones como entrada
  pinMode(buttonPin1, INPUT);
  pinMode(buttonPin2, INPUT);
  pinMode(buttonPin3, INPUT);
  
  // Habilitar las resistencias pull-up internas para los botones
  digitalWrite(buttonPin1, HIGH);
  digitalWrite(buttonPin2, HIGH);
  digitalWrite(buttonPin3, HIGH);

  // Configurar interrupciones para los botones
  attachInterrupt(digitalPinToInterrupt(buttonPin1), button1Interrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(buttonPin2), button2Interrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(buttonPin3), button3Interrupt, CHANGE);

  // Inicializar los servomotores
  servomotor3.attach(11);
  servomotor2.attach(10);
  servomotor1.attach(9);

  // Configurar el motor paso a paso
  motor.setSpeed(5);
}

void loop() {
  // Controlar las acciones del brazo robótico según los botones presionados
  if (pickObject) {
    pickObject = false;
    pickObjectAction();
  }

  if (rotateObject) {
    rotateObject = false;
    rotateObjectAction();
  }

  if (releaseObject) {
    releaseObject = false;
    releaseObjectAction();
  }

  if (returnToInitial) {
    returnToInitial = false;
    returnToInitialAction();
  }
}

void pickObjectAction() {
  // Mover los servomotores para recoger el objeto
  for (int i = 0; i <= 45; i++) {
    servomotor3.write(i);
    delay(25);
  }
  delay(1000);
  for (int i = 0; i <= 90; i++) {
    servomotor2.write(i);
    delay(25);
  }
  delay(1000);
  for (int i = 0; i <= 90; i++) {
    servomotor1.write(i);
    delay(25);
  }
  delay(1000);
  for (int i = 45; i >= 0; i--) {
    servomotor3.write(i);
    delay(25);
  }
  delay(1000);
  for (int i = 90; i >= 0; i--) {
    servomotor1.write(i);
    delay(25);
  }
  delay(1000);
  for (int i = 90; i >= 0; i--) {
    servomotor2.write(i);
    delay(25);
  }
  delay(1000);
}

void rotateObjectAction() {
  // Girar el motor paso a paso con el objeto
  motor.step(512);
  delay(1000);
}

void releaseObjectAction() {
  // Soltar el objeto
  for (int i = 0; i <= 90; i++) {
    servomotor2.write(i);
    delay(25);
  }
  delay(1000);
  for (int i = 0; i <= 90; i++) {
    servomotor1.write(i);
    delay(25);
  }
  delay(1000);
  for (int i = 0; i <= 45; i++) {
    servomotor3.write(i);
    delay(25);
  }
  delay(1000);
}

void returnToInitialAction() {
  // Volver a la posición inicial
  servomotor1.write(0);
  servomotor2.write(0);
  motor.step(-512);
  delay(3000);
}

void button1Interrupt() {
  // Actualizar el estado del botón 1
  buttonState1 = !buttonState1;
  if (buttonState1) {
    pickObject = true;
  }
}

void button2Interrupt() {
  // Actualizar el estado del botón 2
  buttonState2 = !buttonState2;
  if (buttonState2) {
    rotateObject = true;
  }
}

void button3Interrupt() {
  // Actualizar el estado del botón 3
  buttonState3 = !buttonState3;
  if (buttonState3) {
    releaseObject = true;
    returnToInitial = true;
  }
}