#include <Servo.h>

// Definición de pines
const int joyA_X = A4;
const int joyA_Y = A5;
const int joyB_X = 20;
const int joyB_Y = 21;
const int buttonPin = A1;
const int servoXPin = A2;
const int servoYPin = A3;

// Variables para los servos
Servo servoX;
Servo servoY;

// Variables para el estado del botón
int buttonState = 0;
int lastButtonState = 0;
bool mode = false; // false: ambos joysticks controlan ambos servos, true: cada joystick controla un servo

void setup() {
  // Inicialización de los pines
  pinMode(buttonPin, INPUT);
  servoX.attach(servoXPin);
  servoY.attach(servoYPin);
  Serial.begin(9600); // Inicializar el monitor serie para depuración
}

void loop() {
  // Leer el estado del botón
  buttonState = digitalRead(buttonPin);

  // Cambio de modo al presionar el botón
  if (buttonState != lastButtonState) {
    if (buttonState == HIGH) {
      mode = !mode;
    }
    delay(50); // Debounce
  }
  lastButtonState = buttonState;

  // Leer los valores de los joysticks
  int joyA_X_val = analogRead(joyA_X);
  int joyA_Y_val = analogRead(joyA_Y);
  int joyB_X_val = analogRead(joyB_X);
  int joyB_Y_val = analogRead(joyB_Y);

// Mapear los valores de los joysticks a los ángulos de los servos
  int servoX_val, servoY_val;

  if (mode) {
    // Modo 1: cada joystick controla un servo
    servoX_val = map(joyA_X_val, 0, 1023, 0, 180);
    servoY_val = map(joyB_Y_val, 0, 1023, 0, 180);
  } else {
    // Modo 0: ambos joysticks controlan ambos servos
    int maxJoyX = (abs(joyA_X_val - 512) > abs(joyB_X_val - 512)) ? joyA_X_val : joyB_X_val;
    int maxJoyY = (abs(joyA_Y_val - 512) > abs(joyB_Y_val - 512)) ? joyA_Y_val : joyB_Y_val;
    servoX_val = map(maxJoyX, 0, 1023, 0, 180);
    servoY_val = map(maxJoyY, 0, 1023, 0, 180);
  }

  // Mover los servos
  servoX.write(servoX_val);
  servoY.write(servoY_val);

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