#include "Trajectoire.h"
#include "Trajectoire_bezier.h"
#include "Trajectoire_circulaire.h"
#include "Trajectoire_droite.h"
#include "Trajectoire_rotation.h"
#include "Monitoring.h"

#include "math.h"

#define NB_MAX_TRAJECTOIRES 5
#define PRECISION_ABSCISSE 0.001f


void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon,
                        float orientation_debut_rad, float orientation_fin_rad){
    trajectoire->type = TRAJECTOIRE_CIRCULAIRE;
    trajectoire->p1.x = centre_x;
    trajectoire->p1.y = centre_y;
    trajectoire->angle_debut_degre = angle_debut_degre;
    trajectoire->angle_fin_degre = angle_fin_degre;
    trajectoire->rayon = rayon;
    trajectoire->longueur = -1;
    trajectoire->orientation_debut_rad = orientation_debut_rad;
    trajectoire->orientation_fin_rad = orientation_fin_rad;
}

void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y,
                        float orientation_debut_rad, float orientation_fin_rad){
    trajectoire->type = TRAJECTOIRE_DROITE;
    trajectoire->p1.x = p1_x;
    trajectoire->p1.y = p1_y;
    trajectoire->p2.x = p2_x;
    trajectoire->p2.y = p2_y;
    trajectoire->longueur = -1;
    trajectoire->orientation_debut_rad = orientation_debut_rad;
    trajectoire->orientation_fin_rad = orientation_fin_rad;
}

void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y,
                        float orientation_debut_rad, float orientation_fin_rad){
    trajectoire->type = TRAJECTOIRE_BEZIER;
    trajectoire->p1.x = p1_x;
    trajectoire->p1.y = p1_y;
    trajectoire->p2.x = p2_x;
    trajectoire->p2.y = p2_y;
    trajectoire->p3.x = p3_x;
    trajectoire->p3.y = p3_y;
    trajectoire->p4.x = p4_x;
    trajectoire->p4.y = p4_y;
    trajectoire->longueur = -1;
    trajectoire->orientation_debut_rad = orientation_debut_rad;
    trajectoire->orientation_fin_rad = orientation_fin_rad;
}

void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad){
    trajectoire->type = TRAJECTOIRE_ROTATION;
    trajectoire->p1.x = p1_x;
    trajectoire->p1.y = p1_y;
    trajectoire->longueur = -1;
    trajectoire->orientation_debut_rad = orientation_debut_rad;
    trajectoire->orientation_fin_rad = orientation_fin_rad;
}

void Trajectoire_inverse(struct trajectoire_t * trajectoire){
    struct trajectoire_t old_trajectoire;
    old_trajectoire = *trajectoire;

    trajectoire->orientation_debut_rad = old_trajectoire.orientation_fin_rad;
    trajectoire->orientation_fin_rad = old_trajectoire.orientation_debut_rad;

    if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE){
        trajectoire->angle_debut_degre = old_trajectoire.angle_fin_degre;
        trajectoire->angle_fin_degre = old_trajectoire.angle_debut_degre;
        return;
    }
    if(trajectoire->type == TRAJECTOIRE_DROITE){
        trajectoire->p1 = old_trajectoire.p2;
        trajectoire->p2 = old_trajectoire.p1;
        return;
    }
    if(trajectoire->type == TRAJECTOIRE_BEZIER){
        trajectoire->p1 = old_trajectoire.p4;
        trajectoire->p2 = old_trajectoire.p3;
        trajectoire->p3 = old_trajectoire.p2;
        trajectoire->p4 = old_trajectoire.p1;
    }



}

/// @brief Renvoie la longueur de la trajectoire en mm, la calcule si besoin
/// @param trajectoire 
/// @return Longueur de la trajectoire
float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
    if(trajectoire->longueur > 0){
        // La longueur est déjà calculée
    }else{
        // Calculons la longueur de la trajectoire
        switch(trajectoire->type){
            case TRAJECTOIRE_DROITE:
                Trajectoire_droite_get_longueur(trajectoire);
                break;
            case TRAJECTOIRE_CIRCULAIRE:
                Trajectoire_circulaire_get_longueur(trajectoire);
                break;
            
            case TRAJECTOIRE_BEZIER:
                Trajectoire_bezier_get_longueur(trajectoire);
                break;
        }
    }
    return trajectoire->longueur;
}

/// @brief Renvoie le point d'une trajectoire à partir de son abscisse
/// @param abscisse : abscisse sur la trajectoire
/// @return point en coordonnées X/Y
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){
    struct point_xyo_t point_xyo;
    switch(trajectoire->type){
        case TRAJECTOIRE_DROITE:
            point_xyo.point_xy = Trajectoire_droite_get_point(trajectoire, abscisse);
            point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
            break;

        case TRAJECTOIRE_CIRCULAIRE:
            point_xyo.point_xy = Trajectoire_circulaire_get_point(trajectoire, abscisse);
            point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
            break;
        
        case TRAJECTOIRE_BEZIER:
            point_xyo.point_xy = Trajectoire_bezier_get_point(trajectoire, abscisse);
            point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
            break;

        case TRAJECTOIRE_ROTATION:
            point_xyo.point_xy = Trajectoire_rotation_get_point(trajectoire, abscisse);
            point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
            break;
    }
    return point_xyo;    
}

float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse){
    return (float) trajectoire->orientation_debut_rad * (1-abscisse) + (float) trajectoire->orientation_fin_rad * abscisse;
}

/// @brief Calcul la nouvelle abscisse une fois avancé de la distance indiquée
/// @param abscisse : Valeur entre 0 et 1, position actuelle du robot sur sa trajectoire
/// @param distance_mm : Distance en mm de laquelle le robot doit avancer sur la trajectoire
/// @return nouvelle abscisse
float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){
    double delta_abscisse, delta_mm, erreur_relative;

    if(distance_mm == 0){
        return abscisse;
    }
    // Ceci permet d'avoir une abscisse exact sur les trajectoires droites, les trajectoires circulaires et les rotations
    delta_abscisse = distance_mm / Trajectoire_get_longueur_mm(trajectoire);
    if(trajectoire->type == TRAJECTOIRE_ROTATION || trajectoire->type == TRAJECTOIRE_CIRCULAIRE || trajectoire->type == TRAJECTOIRE_DROITE){
        return abscisse + delta_abscisse;
    }

    delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy );

    // Sur les trajectoires de bézier, il peut être nécessaire d'affiner 
    // Les cas où l'algorythme diverge ne devraient pas se produire car distance_cm << longeur_trajectoire.
    erreur_relative = 1 - delta_mm / distance_mm;
    while(fabs(erreur_relative) > PRECISION_ABSCISSE){
        delta_abscisse = delta_abscisse * distance_mm / delta_mm;
        delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy );
        erreur_relative = 1 - delta_mm / distance_mm;
    }
    
    return abscisse + delta_abscisse;
}

double distance_points(struct point_xy_t point, struct point_xy_t point_old){
    return sqrt( pow(point.x - point_old.x, 2) + pow(point.y - point_old.y , 2));
    
}