#include <math.h>
#include "Geometrie.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "Asser_Position.h"

double Trajet_calcul_vitesse(double temps_s);
int Trajet_terminee(double abscisse);

double abscisse;    // Position entre 0 et 1 sur la trajectoire
double position_mm; // Position en mm sur la trajectoire
double vitesse_mm_s;
double vitesse_max_trajet_mm_s=500;
double acceleration_mm_ss;
struct trajectoire_t trajet_trajectoire;
struct position_t position_consigne;

/// @brief Initialise le module Trajet. A appeler en phase d'initilisation
void Trajet_init(){
    abscisse = 0;
    vitesse_mm_s = 0;
    acceleration_mm_ss = 500;
    position_mm = 0;
}

/// @brief Configure la vitesse maximale et l'acceleration pour les futurs trajets
/// @param _vitesse_max_trajet_mm_s 
/// @param _acceleration_mm_ss 
void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss){
    vitesse_max_trajet_mm_s = _vitesse_max_trajet_mm_s;
    acceleration_mm_ss = _acceleration_mm_ss;
}

void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){
    abscisse = 0;
    vitesse_mm_s = 0;
    position_mm = 0;
    trajet_trajectoire = trajectoire;
}

/// @brief Avance la consigne de position sur la trajectoire
/// @param pas_de_temps_s : temps écoulé depuis le dernier appel en seconde 
enum etat_trajet_t Trajet_avance(double pas_de_temps_s){
    double distance_mm;
    enum etat_trajet_t trajet_etat = TRAJET_EN_COURS;
    struct point_xyo_t point;
    struct position_t position;

    // Calcul de la vitesse 
    vitesse_mm_s = Trajet_calcul_vitesse(pas_de_temps_s);

    // Calcul de l'avancement en mm
    distance_mm = vitesse_mm_s * pas_de_temps_s;
    position_mm += distance_mm;

    // Calcul de l'abscisse sur la trajectoire
    abscisse = Trajectoire_avance(&trajet_trajectoire, abscisse, distance_mm);
    
    // Obtention du point consigne
    point = Trajectoire_get_point(&trajet_trajectoire, abscisse);

    position.x_mm = point.point_xy.x;
    position.y_mm = point.point_xy.y;
    position.angle_radian = point.orientation;

    position_consigne=position;
    Asser_Position(position);

    if(Trajet_terminee(abscisse)){
        trajet_etat = TRAJET_TERMINE;
    }
    return trajet_etat;

}

/// @brief Savoir si un trajet est terminé est trivial sauf pour les courbes de Bézier
/// où les approximations font que l'abscisse peut ne pas atteindre 1.
/// @param abscisse : abscisse sur la trajectoire
/// @return 1 si le trajet est terminé, 0 sinon
int Trajet_terminee(double abscisse){
    if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){
        if(abscisse >= 1 ){
            return 1;
        }
    }else{
        if(abscisse >= 0.99 ){
            return 1;
        }
    }
    return 0;
}

/// @brief Envoie la consigne de position calculée par le module trajet. Principalement pour le débug/réglage asservissement.
struct position_t Trajet_get_consigne(){
    return position_consigne;
}

/// @brief Calcule la vitesse à partir de l’accélération du robot, de la vitesse maximale et de la contrainte en fin de trajectoire
/// @param pas_de_temps_s : temps écoulé en ms
/// @return vitesse déterminée en m/s
double Trajet_calcul_vitesse(double pas_de_temps_s){
    double vitesse_max_contrainte;
    double distance_contrainte;
    double vitesse;
    // Calcul de la vitesse avec acceleration
    vitesse = vitesse_mm_s + acceleration_mm_ss * pas_de_temps_s;

    // Calcul de la vitesse maximale due à la contrainte en fin de trajectoire (0 mm/s)
    // https://poivron-robotique.fr/Consigne-de-vitesse.html
    distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm;
    // En cas de dépassement, on veut garder la contrainte, pour l'instant
    if(distance_contrainte > 0){
        vitesse_max_contrainte = sqrt(2 * acceleration_mm_ss * distance_contrainte);
    }else{
        vitesse_max_contrainte = 0;
    }

    // Selection de la vitesse la plus faible
    if(vitesse > vitesse_max_contrainte){
        vitesse = vitesse_max_contrainte;
    }
    if(vitesse > vitesse_max_trajet_mm_s){
        vitesse = vitesse_max_trajet_mm_s;
    }
    return vitesse;
}