#include <Servo.h>

// Définition des pins des joysticks
const int joy1X = A0;  // Axe X du premier joystick
const int joy1Y = A1;  // Axe Y du premier joystick
const int joy2X = A2;  // Axe X du deuxième joystick
const int joy2Y = A3;  // Axe Y du deuxième joystick

// Définition des pins des servos
const int servoPin1 = 3;
const int servoPin2 = 5;
const int servoPin3 = 6;
const int servoPin4 = 9;

// Création des objets Servo
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

// Paramètres des servos
const int servoId1 = 1,  neutralPos1 = 50,   maxPos1 = 180,  minPos1 = 0,   maxSpeed1 = 1,  direction1 = -1; // Limites pour servo1
const int servoId2 = 2,  neutralPos2 = 60,   maxPos2 = 160,  minPos2 = 20,  maxSpeed2 = 1,  direction2 = 1; // Limites pour servo2
const int servoId3 = 3,  neutralPos3 = 70,   maxPos3 = 170,  minPos3 = 10,  maxSpeed3 = 1,  direction3 = 1; // Limites pour servo3
const int servoId4 = 4,  neutralPos4 = 80,   maxPos4 = 150,  minPos4 = 30,  maxSpeed4 = 1,  direction4 = 1; // Limites pour servo4

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

  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);

  servoMove(servo1, neutralPos1, minPos1, maxPos1);
  servoMove(servo2, neutralPos2, minPos2, maxPos2);
  servoMove(servo3, neutralPos3, minPos3, maxPos3);
  servoMove(servo4, neutralPos4, minPos4, maxPos4);

  pinMode(joy1X, INPUT);
  pinMode(joy1Y, INPUT);
  pinMode(joy2X, INPUT);
  pinMode(joy2Y, INPUT);
}



void loop() {
  int joy1XVal = analogRead(joy1X);
  int joy1YVal = analogRead(joy1Y);
  int joy2XVal = analogRead(joy2X);
  int joy2YVal = analogRead(joy2Y);

  updateServo(servoId1, servo1, joy1XVal, minPos1, maxPos1, maxSpeed1, direction1);
  updateServo(servoId2, servo2, joy1YVal, minPos2, maxPos2, maxSpeed2, direction2);
  updateServo(servoId3, servo3, joy2XVal, minPos3, maxPos3, maxSpeed3, direction3);
  updateServo(servoId4, servo4, joy2YVal, minPos4, maxPos4, maxSpeed4, direction4);

  delay(15); // Délai pour la fluidité des mouvements

}

void servoMove(Servo myServo, int value, int minVal, int maxVal){
  int newPosition = constrain(value, minVal, maxVal);
  myServo.write(newPosition);
}

void updateServo(int servoID, Servo myServo, int joyVal, int minPos, int maxPos, int maxSpeed, int directionSwitch) {
  int currentPos = myServo.read();
  int centerValue = 512;  // Valeur centrale pour les joysticks

  // Serial.print(" | servoID: ");
  // Serial.print(servoID);
  // Serial.print(" | joyVal ");
  // Serial.print(joyVal);

  // Calcul de la différence entre la position actuelle du joystick et le centre
  int offset = joyVal - centerValue;

  // Serial.print(" | offset: ");
  // Serial.print(abs(offset));


  int speed = map(abs(offset), 0, 512, 0, maxSpeed);  // Calculez la vitesse en fonction de l'amplitude du joystick
  if (abs(offset) > 0 && speed == 0) {
    speed = maxSpeed;
  }

  // Serial.print(" | speed: ");
  // Serial.print(speed);


  // Déterminer la direction de mouvement basée sur l'offset
  int direction = offset > 0 ? 1 : -1;
  
  // ajuste la direction par rapport au switch
  direction = direction * directionSwitch;

  // Serial.print("| direction: ");
  // Serial.print(direction);

  // Ajuster la nouvelle position basée sur la direction et la vitesse
  int newPos = currentPos + (direction * speed);

  // Serial.print("| newPos: ");
  // Serial.print(newPos);

  // Vérification et ajustement pour s'assurer que la nouvelle position ne dépasse pas les limites
  newPos = constrain(newPos, minPos, maxPos);

  // Appliquez la nouvelle position au servo
  myServo.write(newPos);

  // Serial.println("");

}