#include <Servo.h>

Servo servoA, servoB, servoPelle, servoGrue;

const int PinPorteA = 9, PinPorteB = 10, PinPelle = 5, PinGrue = 6;
const int LedA = 13, LedB = 12;
const int BtnA = 2, BtnB = 3, BtnPelle = 4, BtnGrue = 7;

bool etatA = false, etatB = false;
int posA = 0, posB = 0; // On stocke la position actuelle

void setup() {
  servoA.attach(PinPorteA);
  servoB.attach(PinPorteB);
  servoPelle.attach(PinPelle);
  servoGrue.attach(PinGrue);

  pinMode(LedA, OUTPUT); pinMode(LedB, OUTPUT);
  pinMode(BtnA, INPUT_PULLUP); pinMode(BtnB, INPUT_PULLUP);
  pinMode(BtnPelle, INPUT_PULLUP); pinMode(BtnGrue, INPUT_PULLUP);

  servoA.write(0);
  servoB.write(0);
  servoPelle.write(0);
  servoGrue.write(0);
}

// Fonction pour déplacer un servo lentement
void bougerLentement(Servo &monServo, int angleDepart, int angleArrivee, int vitesse) {
  if (angleDepart < angleArrivee) {
    for (int pos = angleDepart; pos <= angleArrivee; pos++) {
      monServo.write(pos);
      delay(vitesse); // Plus ce nombre est grand, plus c'est lent
    }
  } else {
    for (int pos = angleDepart; pos >= angleArrivee; pos--) {
      monServo.write(pos);
      delay(vitesse);
    }
  }
}

void loop() {
  // PORTE A avec mouvement lent
  if (digitalRead(BtnA) == LOW) {
    int cible = etatA ? 0 : 90; 
    digitalWrite(LedA, !etatA); // Allume la LED avant de bouger
    bougerLentement(servoA, posA, cible, 30); // 30ms par degré = mouvement fluide
    posA = cible;
    etatA = !etatA;
    delay(300);
  }

  // PORTE B avec mouvement lent
  if (digitalRead(BtnB) == LOW) {
    int cible = etatB ? 0 : 90;
    digitalWrite(LedB, !etatB);
    bougerLentement(servoB, posB, cible, 30);
    posB = cible;
    etatB = !etatB;
    delay(300);
  }

  // PELLETEUSE (Séquence automatique 4 fois)
  if (digitalRead(BtnPelle) == LOW) {
    for (int i = 0; i < 4; i++) {
      servoPelle.write(90);
      delay(2000);
      servoPelle.write(0);
      delay(2000);
    }
    delay(500);
  }

  // GRUE A EAU (Standard)
  if (digitalRead(BtnGrue) == LOW) {
    static bool etatGrue = false;
    etatGrue = !etatGrue;
    servoGrue.write(etatGrue ? 90 : 0);
    delay(300);
  }
}