#define DECEL 40.0
#define ACCEL 20.0
#define VITESSE 100.0 

#define DISTANCE 1000.0
double x = 0, v = 0, vr = 0, posr = 0; //distance, vitesse, vitesse réelle (mesurée par variation de distance)
double tu = 0, td = 0, tt = 0; // temps1 (accel), temps2 (Vconst), temps3 (decel)

double t_accel_end = 0;
double t_decel_start = 0;
double t_const = 0;
double t_total = 0;

long old_time = 0;
double old_time_2 = 0;
double old_x = 0;

double t_accel = VITESSE/ACCEL;
double t_decel = VITESSE/DECEL;

double t_start;

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

  double dist_accel = (ACCEL/2)*t_accel*t_accel;
  double dist_decel = (DECEL/2)*t_decel*t_decel;
  double dist_const = DISTANCE - dist_accel - dist_decel;
  
  if (dist_const < 0){    
    float rapport = ACCEL/DECEL;
    dist_accel = DISTANCE/(1+rapport);
    dist_decel = DISTANCE - dist_accel;
    Serial.print("rapport ");
    Serial.print(rapport);
    Serial.print(" dist accel ");
    Serial.print(dist_accel);
    Serial.print(" dist decel ");
    Serial.println(dist_decel);
    dist_const = 0;
    t_accel = sqrt((2.0*dist_accel)/ACCEL);
    t_decel = sqrt((2.0*dist_decel)/DECEL);
    Serial.println("Valeurs rectifiées pour courbe triangle");
  }

  t_accel_end = t_accel;
  t_const = dist_const/VITESSE;
  t_decel_start = t_accel + t_const;
  t_total = t_accel + t_const + t_decel;
  
  
  
  Serial.print(t_accel_end); Serial.print("\t");Serial.print(t_const); Serial.print("\t");Serial.print(t_decel_start); Serial.println();
  Serial.print("================== Start ==================");
  delay(2000);
  t_start = micros();
}

void loop() {
  double t = (micros() - t_start) / 1000000.0;

  //verifier les exclusitivés des comparaisons
  if (t <= t_accel_end) tu = t; else tu = t_accel;
  if (t >= t_accel_end && t <= t_decel_start) td = (t - tu); else if (t >= t_accel_end) td = t_const;
  if (t >= t_decel_start && t <= t_total) tt = (t - tu - td); else if (t >= t_decel_start) tt = t_decel;

  //si possible, verifier pendant la decel que l'objectif sera bien atteint et ajuster si besoin (backlog)
  // position acquise au cours du mouvement :
  //   phase accel              phase v constante   phase décel  
  x = ((ACCEL/2.0) * tu * tu) + (ACCEL * tu * td) + ((ACCEL * tu * tt) - ((DECEL/2.0) * tt* tt));

  v = (ACCEL * tu) - (DECEL * tt);
  if (v < 0 || x == DISTANCE) v = 0;

  //sample de la vitesse, pour verifier
  if (micros() - old_time > 100){ // intervalle de sample, us
    double t_int = ((micros() - old_time)/1000000.0);
    vr = (x- old_x) / t_int;
    posr += vr * t_int;

    old_time = micros();
    old_x = x;
  }

  //Serial.print(t); Serial.print("\t");
  //Serial.print(tu); Serial.print("\t");Serial.print(td); Serial.print("\t"); Serial.print(tt); Serial.print("\t");
  Serial.print(x); Serial.print("\t"); 
  Serial.print(v); Serial.print("\t");
  Serial.print(vr); Serial.print("\t");
  Serial.print(posr); Serial.print("\t");


  // steps
  double mm_per_step = 5.0; // arbitrary, test purposes
  int64_t main_steps = (int64_t)((double)posr/(double)mm_per_step); // int takes care of truncating decimals
  // classic binary floating point arithmetics problem : doesn't quite always reach the full travel distance, as floating point numbers are bigger than they should be
  // problem is invisible using arduino's shitty print function, can use this if motivated enough https://forum.locoduino.org/index.php?topic=1340.0


  Serial.print((int32_t)main_steps); Serial.print("\t"); // bro doesn't know how to print in64s ...
  Serial.print((double)(main_steps) * mm_per_step); Serial.print("\t"); 

  //now that we know oh many steps we should have taken by this point, we can control the step timer's step count to this data, without using the timer's speed 
  // as the way to control the robot's speed, as the robot's speed is already included in the steps generation rate
  // so, from now on, we don't have to worry about who actually generated the step, so long as the motor is at the correct position, speed is controlled by the process generatig steps

  // CAREFUL TO DISABLE RELOAD WAIT (or whatever it is called, the thingamajig that makes the timer ARR update only after going to 0), AS THIS COULD INTRODUCE OSCILLATIONS OVER TIME

  // let's try it out, generating constant reverse control signal
  //int64_t control_steps = (int64_t)(-100.0 * (millis() / 1000.0));

  //what about a variable and (almost) random one ?
  static double control_steps = 0;
  control_steps += (((float)(analogRead(A0) - 512)/10.0) * (micros() - old_time_2) / 1000000.0); // 4 in the morning is NOT the right time for this kind of shit
  old_time_2 = micros();

  int64_t actual_steps = main_steps + (int64_t)control_steps;
  //Serial.print((int32_t)control_steps); Serial.print("\t"); 
  Serial.print((int32_t)actual_steps); Serial.print("\t"); 

  // seems to work like a charm
  // with the only downside being increased CPU load, since we can't just input a variable speed (PID'd to error) in there, and have to integrate speed to get the actual position that the robot should have at any instant to correct the error

  Serial.println();
}