#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Arduino.h>
#include <XPT2046_Touchscreen.h>
#include<ESP32Servo.h>

//***************** PAQUETE A *****************
Servo myservo_1;
#define SERVO_1 17
#define ENABLE_1 18
#define DIR_1 8           
#define STEP_1 3          
#define pulsador1_1 10     
#define pulsador2_1 9      

//***************** PAQUETE B *****************
Servo myservo_2;
#define SERVO_2 36
#define ENABLE_2 37
#define DIR_2 38           
#define STEP_2 39          
#define pulsador1_2 35      
#define pulsador2_2 48      

// Parámetros de calibración (puedes ajustarlos según el motor)
const int DIST_AGARRE = 1500;    
const int DIST_RETROCESO = 1500; 
const int DIST_INICIO = 2000;    

//Sentido del motor
bool HORARIO = HIGH;
bool ANTIHORARIO = LOW;

volatile bool pulsador1PresionadoA = false;
volatile bool pulsador2PresionadoA = false;
volatile bool pulsador1PresionadoB = false;
volatile bool pulsador2PresionadoB = false;


void setup() {
  Serial.begin(115200);
  //***************** PAQUETE A *****************
  pinMode(DIR_1, OUTPUT);  
  pinMode(STEP_1, OUTPUT); 
  pinMode(ENABLE_1, OUTPUT);
  pinMode(pulsador1_1, INPUT_PULLUP); 
  pinMode(pulsador2_1, INPUT_PULLUP); 
  // interrupción para los fines de carrera 
  attachInterrupt(pulsador1_1, pul1A_ISR, FALLING);
  attachInterrupt(pulsador2_1, pul2A_ISR, FALLING);
  digitalWrite(ENABLE_1, LOW);
  myservo_1.attach(SERVO_1, 600, 2400);

  //***************** PAQUETE B *****************
  pinMode(DIR_2, OUTPUT);  
  pinMode(STEP_2, OUTPUT); 
  pinMode(ENABLE_2, OUTPUT);
  pinMode(pulsador1_2, INPUT_PULLUP); 
  pinMode(pulsador2_2, INPUT_PULLUP);
  // interrupción para los fines de carrera 
  attachInterrupt(pulsador1_2, pul1B_ISR, FALLING);
  attachInterrupt(pulsador2_2, pul2B_ISR, FALLING);
  digitalWrite(ENABLE_2, LOW);
  myservo_2.attach(SERVO_2, 600, 2400);
}

//INTERRUPCIONES
void ARDUINO_ISR_ATTR pul1A_ISR(void) { pulsador1PresionadoA = true; }
void ARDUINO_ISR_ATTR pul2A_ISR(void) { pulsador2PresionadoA = true; }
void ARDUINO_ISR_ATTR pul1B_ISR(void) { pulsador1PresionadoB = true; }
void ARDUINO_ISR_ATTR pul2B_ISR(void) { pulsador2PresionadoB = true; }

void GirarMotores(int DIST, bool SENTIDO, int PIN_DIR_A, int PIN_STEP_A, int PIN_EN_A, int PIN_DIR_B, int PIN_STEP_B, int PIN_EN_B){
  // Habilitar ambos motores
  digitalWrite(PIN_EN_A, LOW);
  digitalWrite(PIN_EN_B, LOW);  

  // Establecer direcciones
  digitalWrite(PIN_DIR_A, SENTIDO);
  digitalWrite(PIN_DIR_B, SENTIDO);

  // Generar pulsos para ambos motores
  for (int i = 0; i < DIST; i++) {
    digitalWrite(PIN_STEP_A, HIGH);
    digitalWrite(PIN_STEP_B, HIGH);
    delayMicroseconds(1000);  // Controla la velocidad de los motores
    digitalWrite(PIN_STEP_A, LOW);
    digitalWrite(PIN_STEP_B, LOW);
    delayMicroseconds(1000);
  }

  // Deshabilitar ambos motores
  digitalWrite(PIN_EN_A, HIGH);
  digitalWrite(PIN_EN_B, HIGH);

  delay(1000); // Pequeña pausa después del movimiento
}

void loop() {
  // Respuesta a los pulsadores de fin de carrera
  if (pulsador1PresionadoA || pulsador1PresionadoB) {
    GirarMotores(DIST_INICIO, ANTIHORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2);
    pulsador1PresionadoA = pulsador1PresionadoB = false;
    Serial.println("Motores A y B: Retrocediendo por fin de carrera 1");
  }
  if (pulsador2PresionadoA || pulsador2PresionadoB) {
    GirarMotores(DIST_INICIO, HORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2);
    pulsador2PresionadoA = pulsador2PresionadoB = false;
    Serial.println("Motores A y B: Avanzando por fin de carrera 2");
  }

  // Movimiento coordinado de ambos motores
  GirarMotores(DIST_AGARRE, HORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2);
  myservo_1.write(90);
  myservo_2.write(90);
  delay(500);

  GirarMotores(DIST_RETROCESO, ANTIHORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2);
  myservo_1.write(30);
  myservo_2.write(30);
  delay(500);
}