#include <ESP32Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

int POS_INICIAL = 0;
int POS_INTERMEDIA = 90;
int POS_FINAL = 180;

const int pinServo1 = 19;
const int pinServo2 = 18;
const int pinServo3 = 5;
const int pinServo4 = 17;

const int pinVert = 34;  // Pin del eje vertical del joystick
const int pinHorz = 35;  // Pin del eje horizontal del joystick
const int pinBoton = 32; // Pin del boton del joystick

bool boton = true;


void setup(){
  Serial.begin(9600);

  servo1.setPeriodHertz(50); // Configura el periodo de PWM en 50Hz
  servo2.setPeriodHertz(50);
  servo3.setPeriodHertz(50);
  servo4.setPeriodHertz(50);

  servo1.attach(pinServo1); // Configura los pines de los servos y los rangos de pulsos
  servo2.attach(pinServo2);
  servo3.attach(pinServo3);
  servo4.attach(pinServo4);

  pinMode(pinBoton, INPUT_PULLUP); // Configura el pin del boton como entreda de pull-up interno

  servo1.write(POS_INICIAL);
  servo2.write(POS_INICIAL);
  servo3.write(POS_INICIAL);
  servo4.write(POS_INICIAL);

}



void imprimir_angulos(int angle1, int angle2, int angle3, int angle4){
   // Imprime los ángulos que se van a enviar a los servos
  Serial.print("Angle1: ");
  Serial.print(angle1);
  Serial.print("  Angle2: ");
  Serial.print(angle2);
  Serial.print("  Angle3: ");
  Serial.print(angle3);
  Serial.print("  Angle4: ");
  Serial.println(angle4);

}

void imprimir_valores(int vertValue, int horzValue, int buttonState){
   // Imprime los valores en el monitor serial
  Serial.print("Vertical: ");
  Serial.print(vertValue);
  Serial.print("  Horizontal: ");
  Serial.print(horzValue);
  Serial.print("  Boton: ");
  Serial.println(buttonState);
  Serial.print("  variableboton: ");
  Serial.println(boton);
}

void aplicar_angulos_a_los_servos(int angle1, int angle2, int angle3, int angle4){
  // Aplica los ángulos calculados a los servos
  servo1.write(angle1);
  servo2.write(angle2);
  servo3.write(angle3);
  servo4.write(angle4);
}

void loop(){

  int vertValue = analogRead(pinVert); // eje vertical
  int horzValue = analogRead(pinHorz); // eje horizontal
  int buttonState = digitalRead(pinBoton);

  imprimir_valores(vertValue, horzValue, buttonState);

  // Mapea los valores del joystick a rangos de angulos para los servos
  int angle1 = map(horzValue, 0, 4095, 0, 180); // Mapea el eje horizontal del joystick al rango de ángulos (0-180 grados)
  int angle2 = map(vertValue, 0, 4095, 0, 180); // Mapea el eje vertical del joystick al rango de ángulos (0-180 grados)
  int angle3 = map(horzValue, 0, 4095, 0, 180);
  int angle4 = map(vertValue, 0, 4095, 0, 180);

  

  if (buttonState == LOW && boton) {
    aplicar_angulos_a_los_servos(POS_INTERMEDIA, POS_INTERMEDIA, POS_INTERMEDIA, POS_INTERMEDIA);
    boton = !boton;
  }
  else if (buttonState == LOW && !boton) {
    aplicar_angulos_a_los_servos(POS_INICIAL, POS_INICIAL, POS_INICIAL, POS_INICIAL);
    boton = !boton;
  }


  if (vertValue > 2048){
    aplicar_angulos_a_los_servos(90, 90, 180, 180);
    }
  else if (vertValue < 200){ // Si el joystick esta hacia abajo
    aplicar_angulos_a_los_servos(180, 180, 90, 90);
    }
  if (horzValue > 2048){ // Si el joystick esta hacia la izquierda
    aplicar_angulos_a_los_servos(90, 180, 90, 180);
    }
  else if (horzValue < 200){ // Si el joystick esta hacia la derecha
    aplicar_angulos_a_los_servos(180, 90, 180, 90);
    }
  
  imprimir_angulos(angle1, angle2, angle3, angle4);






  delay(400); // Pequeña pausa para estabilidad
}