#include "Geometrie.h"
#include "math.h"


/// @brief Retourne l'angle entre -PI et +PI
/// @param angle 
/// @return 
float Geometrie_get_angle_normalisee(float angle){
    while(angle > M_PI){
        angle -= 2* M_PI;
    }
    while(angle < -M_PI){
        angle += 2* M_PI;
    }
    return angle;
}

/// @brief Indique si un angle est compris entre deux angles. Les angles doivent être entre -PI et PI.
/// @param angle : angle à comparer
/// @param angle_min : début de la fourchette
/// @param angle_max : fin de la fourchette
/// @return 1 si l'angle est compris entre min et max, 0 sinon
unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max){
    if(angle_min > angle_max){
        // cas où la fourchette comprend -PI.
        if( (angle > angle_min) || (angle < angle_max)){
            return 1;
        }
        return 0;
    }else{
        // Cas normal
        if( (angle > angle_min) && (angle < angle_max)){
            return 1;
        }
        return 0;
    }
}

/// @brief A partir de l'orientation actuelle du robot et de l'orientation souhaitée, 
/// donne l'angle consigne pour limiter les rotations inutiles.
/// Tous les angles sont en radian
/// @param angle_depart 
/// @param angle_souhaite 
/// @return angle_optimal en radian
float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite){
    while((angle_depart - angle_souhaite) > M_PI){
        angle_souhaite += 2* M_PI;
    }
    while((angle_depart - angle_souhaite) < -M_PI){
        angle_souhaite -= 2* M_PI;
    }
    return angle_souhaite;
}

/// @brief Indique si les deux plages d'angle se recoupent
/// @param angle1_min Début de la première plage
/// @param angle1_max Fin de la première plage
/// @param angle2_min Début de la seconde plage
/// @param angle2_max Fin de la seconde plage
/// @return 1 si les deux plages s'intersectent, 0 sinon
unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max){
    // Pour que les plages s'intersectent, soit :
    // * angle1_min est compris entre angle2_min et angle2_max
    // * angle1_max est compris entre angle2_min et angle2_max
    // * angle2_min et angle2_max sont compris entre angle1_min et angle1_max (tester angle2_min ou angle2_max est suffisant)
    if(Geometrie_compare_angle(angle1_min, angle2_min, angle2_max) ||
        Geometrie_compare_angle(angle1_max, angle2_min, angle2_max) ||
        Geometrie_compare_angle(angle2_min, angle1_min, angle1_max)){
            return 1;
    }
    return 0;
}

/// @brief Déplace un point de la distance indiquée en se servant de l'angle de la position donnée.
struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm){
    struct position_t position_arrivée;
    position_arrivée.angle_radian = position_depart.angle_radian;
    position_arrivée.x_mm = position_depart.x_mm + cosf(position_depart.angle_radian) * distance_mm;
    position_arrivée.y_mm = position_depart.y_mm + sinf(position_depart.angle_radian) * distance_mm;
    
    return position_arrivée;
}