#include "math.h"
#include "Trajectoire.h"


void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){
    float distance_angulaire;
    if(trajectoire->angle_debut_rad > trajectoire->angle_fin_rad){
        distance_angulaire = trajectoire->angle_debut_rad - trajectoire->angle_fin_rad;
    }else{
        distance_angulaire = trajectoire->angle_fin_rad - trajectoire->angle_debut_rad;
    }
    trajectoire->longueur = trajectoire->rayon * distance_angulaire;
}

/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse
/// @param abscisse : compris entre 0 et 1
struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){
    struct point_xy_t point;
    float angle_rad;

    angle_rad = (float) trajectoire->angle_debut_rad * (1-abscisse) + (float) trajectoire->angle_fin_rad * abscisse;
    point.x = trajectoire->p1.x + cos(angle_rad) * trajectoire->rayon;
    point.y = trajectoire->p1.y + sin(angle_rad) * trajectoire->rayon;

    return point;
}