#include "Asser_Moteurs.h"
#include "Geometrie_robot.h"
#include "Commande_vitesse.h"



/// @brief Commande une rotation autour d'un point dans le référentiel du robot.
/// Cette fonction déplace le torseur cinétique du centre de rotation vers le centre du robot
/// pour obtenir la vitesse de ce dernier.
/// @param rotation_rad_s : Rotation en rad/s
/// @param centre_x : centre de rotation (coordonnée X)
/// @param centre_y : centre de rotation (coordonnée Y)
void commande_rotation(float rotation_rad_s, float centre_x, float centre_y){
    float vitesse_x_mm_s, vitesse_y_mm_s;
    vitesse_x_mm_s = centre_y * rotation_rad_s;
    vitesse_y_mm_s = -centre_x * rotation_rad_s;

    commande_vitesse(vitesse_x_mm_s, vitesse_y_mm_s, rotation_rad_s);
}


/// @brief Commande de la vitesse dans le référentiel du robot
/// tel que décrit ici : http://poivron-robotique.fr/Robot-holonome-lois-de-commande.html
/// @param vitesse_x_mm_s : Vitesse x en mm/s dans le référentiel du robot
/// @param vitesse_y_mm_s : Vitesse y en mm/s dans le référentiel du robot
/// @param orientation_radian_s : Rotation en radian/s dans le référentiel du robot
void commande_vitesse(float vitesse_x_mm_s, float vitesse_y_mm_s, float orientation_radian_s){
    float vitesse_roue_a, vitesse_roue_b, vitesse_roue_c;

    vitesse_roue_a = vitesse_x_mm_s / 2.0 - vitesse_y_mm_s * RACINE_DE_3 / 2.0 - DISTANCE_ROUES_CENTRE_MM * orientation_radian_s;
    vitesse_roue_b = vitesse_x_mm_s / 2.0 + vitesse_y_mm_s * RACINE_DE_3 / 2.0 - DISTANCE_ROUES_CENTRE_MM * orientation_radian_s;
    vitesse_roue_c = -vitesse_x_mm_s - DISTANCE_ROUES_CENTRE_MM * orientation_radian_s;

    AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_roue_a);
    AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_roue_b);
    AsserMoteur_setConsigne_mm_s(MOTEUR_C, vitesse_roue_c);

}

/// @brief Arrête le robot.
void commande_vitesse_stop(){
    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0);
    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0);
}