#include "Strategie.h"
#include "Rotation.h"
#include "Localisation.h"
#include "Asser_Moteurs.h"
#include "Geometrie_robot.h"
#include "math.h"

float rotation_angle_cible;
float rotation_angle_sens;
float rotation_vitesse_rad_s;
float rotation_vitesse_max_rad_s;
float rotation_acceleration_rad_ss;

float Rotation_calcul_vitesse(float angle_actuel, float pas_de_temps_s);

void rotation_init(float angle_cible){
    struct position_t position = Localisation_get();
    rotation_angle_cible = Geometrie_get_angle_optimal(position.angle_radian, angle_cible);
    rotation_vitesse_rad_s = 0;
    rotation_vitesse_max_rad_s = 5;
    rotation_acceleration_rad_ss = 25;

    if(rotation_angle_cible - position.angle_radian > 0){
        rotation_angle_sens = 1;
    }else{
        rotation_angle_sens = -1;
    }

}

void rotation_set_vitesse(float vitesse_rad_s){
    float vitesse_mm_s = vitesse_rad_s * DISTANCE_ROUES_CENTRE_MM;
    if(rotation_angle_sens == 1){
        AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_mm_s);
        AsserMoteur_setConsigne_mm_s(MOTEUR_B, -vitesse_mm_s);
    }else{
        AsserMoteur_setConsigne_mm_s(MOTEUR_A, -vitesse_mm_s);
        AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_mm_s);
    }
}
enum etat_action_t rotation_gestion(float pas_de_temps_s){
    struct position_t position = Localisation_get();
    rotation_vitesse_rad_s = Rotation_calcul_vitesse(position.angle_radian, pas_de_temps_s);

    if (rotation_angle_sens == 1 ){
        rotation_set_vitesse(rotation_vitesse_rad_s);
        if (position.angle_radian >  rotation_angle_cible){
            return ACTION_TERMINEE;
        }

    }else{
        if (position.angle_radian <  rotation_angle_cible){
            return ACTION_TERMINEE;
        }
    }
    return ACTION_EN_COURS;

}

float Rotation_calcul_vitesse(float angle_actuel, float pas_de_temps_s){
    // Phase d'accélération
    rotation_vitesse_rad_s = rotation_vitesse_rad_s + pas_de_temps_s * rotation_acceleration_rad_ss;

    // Saturation par vitesse max
    if(rotation_vitesse_rad_s > rotation_vitesse_max_rad_s){
        rotation_vitesse_rad_s = rotation_vitesse_max_rad_s;
    }

    // Saturation par la courbe de décélération
    float vitesse_décélération_rad_s;
    // distance restante
    float angle_restant;
    if (rotation_angle_sens == 1){
        angle_restant = rotation_angle_cible - angle_actuel;
    }else{
        angle_restant = angle_actuel - rotation_angle_cible;
    }

    if(angle_restant < 0){
        vitesse_décélération_rad_s = 0;
    }else{
        vitesse_décélération_rad_s = sqrtf(2 * rotation_acceleration_rad_ss * angle_restant);
    }
    if(rotation_vitesse_rad_s > vitesse_décélération_rad_s){
        rotation_vitesse_rad_s = vitesse_décélération_rad_s;
    }  
    return rotation_vitesse_rad_s;  
}