#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

#define PCA9685_ADDRESS  0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PCA9685_ADDRESS);

#define SERVOMIN1  92  // pour wokwi
#define SERVOMAX1  497 
#define SERVOMIN2  92 
#define SERVOMAX2  497
//#define SERVOMIN1 66   // This is the 'minimum' pulse length count (out of 4096)
//#define SERVOMAX1 476  // This is the 'maximum' pulse length count (out of 4096)
//#define SERVOMIN2 76   // This is the 'minimum' pulse length count (out of 4096)
//#define SERVOMAX2 496  // This is the 'maximum' pulse length count (out of 4096)
#define SERVO_FREQ 50  // Analog servos run at ~50 Hz updates
#define MOTORS 8

struct MotorData {
  int model;
  int angle;
  int animation;
  int step;
  unsigned long startTime;
};
unsigned long currentTime;

// initialisation des moteurs
MotorData Motors[MOTORS] = {{180,90,-1}, {180,90,-1}, {180,90,-1}, {180,90,-1}, {270,135,-1}, {270,135,-1}, {270,135,-1}, {270,135,-1}};

#define LINEAR    0
#define INVERSE   1
#define POWER     2
#define CUBIC     3
#define R_INVERSE 4
#define R_POWER   5
#define R_CUBIC   6

struct AnimationData {
  int angle;
  float duration;
  int curve;
};

// initialisation des animations
// le format c'est {nombre d'étapes dans l'anim}, {angle1, durée de transition, type d'accélération}, {angle2, durée, accélération}, etc...
// les type d'accélération sont : LINEAR, bah vitesse linéaire
// INVERSE, POWER, CUBIC, commencent lentement puis accélèrent de + en +, avec INVERSE qui est plus doux et CUBIC plus abrupt
// R_INVERSE, R_POWER, R_CUBIC, commencent rapidement puis décélèrent de + en +
AnimationData Animation01[] = {{3}, {0,0,LINEAR}, {180,5000,LINEAR}, {0,5000,LINEAR}};
AnimationData Animation02[] = {{3}, {0,0,LINEAR}, {180,5000,INVERSE}, {0,5000,INVERSE}};
AnimationData Animation03[] = {{3}, {0,0,LINEAR}, {180,5000,POWER}, {0,5000,POWER}};
AnimationData Animation04[] = {{3}, {0,0,LINEAR}, {180,5000,CUBIC}, {0,5000,CUBIC}};
AnimationData Animation05[] = {{3}, {0,0,LINEAR}, {270,5000,LINEAR}, {0,5000,LINEAR}};
AnimationData Animation06[] = {{3}, {0,0,LINEAR}, {270,5000,R_INVERSE}, {0,5000,R_INVERSE}};
AnimationData Animation07[] = {{3}, {0,0,LINEAR}, {270,5000,R_POWER}, {0,5000,R_POWER}};
AnimationData Animation08[] = {{3}, {0,0,LINEAR}, {270,5000,R_CUBIC}, {0,5000,R_CUBIC}};

AnimationData *Animations[] = {Animation01, Animation02, Animation03, Animation04, Animation05, Animation06, Animation07, Animation08};

void setup() {
  Serial.begin(9600);
  pwm.begin();  // Initialize PWM instance
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
}

void loop() {
  updateAnimations();
  delay(50); // possible de changer la fréquence des updates si ça rame
}

int getAnglePulse(int channel, int angle) {
  if(Motors[channel].model == 180)
    return map(angle, 0, 180, SERVOMIN1, SERVOMAX1);
  else
    return map(angle, 0, 270, SERVOMIN2, SERVOMAX2);
}
  
void updateAnimations() {
  currentTime = millis();
  for(int channel=0; channel < MOTORS ; channel++) {
    MotorData &data = Motors[channel];
    if(data.animation == -1) randomizeAnimation(channel);
    AnimationData &animation = Animations[data.animation][data.step];
    float x = animation.duration > 0 ? min(1.0f, (currentTime - data.startTime) / animation.duration) : 1.0f;
    switch(animation.curve) {
      case INVERSE:   x /= 2-x;                   break;
      case POWER:     x *= x  ;                   break;
      case CUBIC:     x *= x*x;                   break;
      case R_INVERSE: x = 1-x; x /= 2-x; x = 1-x; break;
      case R_POWER:   x = 1-x; x *= x  ; x = 1-x; break;
      case R_CUBIC:   x = 1-x; x *= x*x; x = 1-x; break;
    }
    int angle0 = data.step==1 ? data.angle : Animations[data.animation][data.step-1].angle;
    data.angle = angle0 + (animation.angle - angle0) * x;
    pwm.setPWM(channel, 0, getAnglePulse(channel, data.angle));
    if(x < 1.0f) continue;
    data.step++;
    data.startTime = currentTime;
    if(data.step > Animations[data.animation][0].angle) data.animation = -1;
  }
}

void randomizeAnimation(int channel) {
  int animation;
	switch(channel) {// ici tu peux mettre des anim random, j'ai mis des nombre fixe juste pour montrer les différentes accélérations dans wokwi
    case 0: animation = 0; break;
    case 1: animation = 1; break;
    case 2: animation = 2; break;
    case 3: animation = 3; break;
    case 4: animation = 4; break;
    case 5: animation = 5; break;
    case 6: animation = 6; break;
    case 7: animation = 7; break;
  }
  Motors[channel].animation = animation;
  Motors[channel].step = 1;
  Motors[channel].startTime = currentTime;
}