#include <Adafruit_PWMServoDriver.h>
#define NUM_MOTORS 4
#define MAX_NUM_STEPS 10
#define rampTime 500

byte m = 0;
int out_PWM = 0;

enum States {IDLE, RAMP, SPEED, REBOOT};
unsigned long startTime;

byte pwmValue;

//MOTORS
typedef struct {
  byte size;
  byte index;
  const char *name;
  States state;
  unsigned long timeStep;
  int vel[MAX_NUM_STEPS];
  unsigned long tempo[MAX_NUM_STEPS];
} Motors;

Motors motors[] = {
  { 2, 0, "M1", IDLE, 0, { 50, 0 }, { 2000, 2000 }},
  { 2, 0, "M2", IDLE, 0, { 50, -50 }, { 2000, 2000 } },
  { 2, 0, "M3", SPEED, 0, { 75, -75 }, { 2000, 2000 } },
  { 2, 0, "M4", IDLE, 0, { 100, -100 }, { 2000, 2000 } }
};

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Start setup!");
  for (int i = 0; i <= NUM_MOTORS; i++) {
    motors[i].timeStep =  millis();
  };
  Serial.println("End setup!");
}

void loop() {
  // put your main code here, to run repeatedly:

  if (m < NUM_MOTORS) {
    pwmValue = updateMotor(motors[m], m * 10);
  }
  if (motors[m].name == "M1") {
    // Serial.print(" Sup:");
    // Serial.print(60);
    // Serial.print(",");
    // Serial.print(" Mid:");
    // Serial.print(0);
    // Serial.print(",");
    // Serial.print(" Inf:");
    // Serial.print(-60);
    // Serial.print(",");
    Serial.print(" Speed:");
    Serial.print(out_PWM);
    Serial.print(",");
    Serial.print(" Index:");
    Serial.println(motors[m].index * 10);
  }
  m += 1;
  if (m >= NUM_MOTORS) {
    m = 0;
  }
}

byte updateMotor(Motors &motor, byte num) {

  unsigned long now = millis();
  unsigned long elapsedTime = now - motor.timeStep;
  byte index = motor.index;

  if (motor.index >= motor.size) {
    motor.state = REBOOT;
  }

  switch (motor.state) {
    case IDLE:
      out_PWM = 0;
      motor.state = RAMP;
      motor.timeStep = millis();
      break;
    case RAMP:
      if (elapsedTime <= rampTime) {
        if (index == 0) {
          out_PWM = motor.vel[index] * (float(elapsedTime) / float(rampTime));
        } else {
          out_PWM = motor.vel[index - 1] + (motor.vel[index] - motor.vel[index - 1]) * (float(elapsedTime) / float(rampTime));
        }
        // if (motor.name == "M1") {
        //   Serial.print(" Sup:");
        //   Serial.print(60);
        //   Serial.print(",");
        //   Serial.print(" Mid:");
        //   Serial.print(0);
        //   Serial.print(",");
        //   Serial.print(" Inf:");
        //   Serial.print(-60);
        //   Serial.print(",");
        //   Serial.print(" Speed:");
        //   Serial.print(out_PWM);
        //   Serial.print(",");
        //   Serial.print(" Index:");
        //   Serial.println(index * 10);
        // }
      } else {
        motor.state = SPEED;
        motor.timeStep = millis();
      }
      break;
    case SPEED:
      if (elapsedTime <= motor.tempo[index]) {
        out_PWM = motor.vel[index];
        // if (motor.name == "M1") {
        //   Serial.print(" Sup:");
        //   Serial.print(60);
        //   Serial.print(",");
        //   Serial.print(" Mid:");
        //   Serial.print(0);
        //   Serial.print(" Inf:");
        //   Serial.print(-60);
        //   Serial.print(",");
        //   Serial.print(" Speed:");
        //   Serial.print(out_PWM);
        //   Serial.print(",");
        //   Serial.print(" Index:");
        //   Serial.println(index * 10);
        // }
      } else {
        motor.index += 1;
        motor.state = RAMP;
        motor.timeStep = millis();
      }
      break;
    case REBOOT:
      if (elapsedTime <= rampTime) {
        out_PWM = motor.vel[motor.size - 1] + (motor.vel[0] - motor.vel[motor.size - 1]) * (float(elapsedTime) / float(rampTime));
        // if (motor.name == "M1") {
        //   Serial.print(" Sup:");
        //   Serial.print(60);
        //   Serial.print(",");
        //   Serial.print(" Mid:");
        //   Serial.print(0);
        //   Serial.print(" Inf:");
        //   Serial.print(-60);
        //   Serial.print(",");
        //   Serial.print(" Speed:");
        //   Serial.print(out_PWM);
        //   Serial.print(",");
        //   Serial.print(" Index:");
        //   Serial.println(index * 10);
        // }
      } else {
        motor.index = 0;
        motor.timeStep = millis();
        motor.state = SPEED;
      }
  }
  return out_PWM;
}