#include "Localisation.h"
#include "Commande_vitesse.h"
#include "math.h"

#define GAIN_P_POSITION 15
#define GAIN_P_ORIENTATION 10

#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN)

struct position_t position_maintien;

/// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot
/// C'est à la consigne d'être défini avant pour être atteignable.
/// Nécessite l'appel des fonctions QEI_update(); Localisation_gestion(); AsserMoteur_Gestion(_step_ms);
/// @param position_consigne : position à atteindre dans le référentiel de la table.
float delta_x_mm, delta_y_mm, delta_orientation_radian;
float delta_orientation_radian_tmp;
void Asser_Position(struct position_t position_consigne){
    float delta_avance_mm;
    float avance_mm_s, rotation_radian_s;
    //float delta_x_mm, delta_y_mm, delta_orientation_radian;
    struct position_t position_actuelle;
    float delta_orientation_radian_tmp;

    position_actuelle = Localisation_get();

    // Calcul de l'erreur
    delta_x_mm = position_consigne.x_mm - position_actuelle.x_mm;
    delta_y_mm = position_consigne.y_mm - position_actuelle.y_mm;
    delta_avance_mm = sqrtf(delta_x_mm * delta_x_mm + delta_y_mm * delta_y_mm);
    delta_orientation_radian_tmp = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian;
    delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian_tmp);
    
    // On asservi sur +PI/2 / -PI/2
    if(delta_orientation_radian > (M_PI/2)){
        delta_orientation_radian -= M_PI;
        delta_avance_mm = -delta_avance_mm;
    }
    if(delta_orientation_radian < -(M_PI/2)){
        delta_orientation_radian += M_PI;
        delta_avance_mm = -delta_avance_mm;
    }
    


    // Asservissement
    avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
    rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;

    if(delta_avance_mm < 10){
        rotation_radian_s=0;
    }
    
    // Commande en vitesse
    commande_vitesse(avance_mm_s, rotation_radian_s);

}

void Asser_Position_set_Pos_Maintien(struct position_t position){
    position_maintien=position;
}

void Asser_Position_maintien(){
    Asser_Position(position_maintien);
}

float Asser_Position_get_erreur_angle(){
    return delta_orientation_radian;
}

/// @brief Renvoi 1 si l'erreur d'angle supérieur au seuil
/// @return 1 si panic, 0 si nominal
int Asser_Position_panic_angle(){
    if(delta_orientation_radian > MAX_ERREUR_ANGLE){
        return 1;
    }
    return 0;
}