#include "Localisation.h"
#include "Temps.h"
#include "QEI.h"
#include "math.h"
#include "Geometrie_robot.h"

struct position_t position;

void Localisation_init(int id){
    Temps_init();
    QEI_init(id);
    position.x_mm = 0;
    position.y_mm = 0;
    position.angle_radian = 0;
}

void Localisation_set(float x_mm, float y_mm, float angle_radian){
    position.x_mm = x_mm;
    position.y_mm = y_mm;
    position.angle_radian = angle_radian;
}

void Localisation_set_x(float x_mm){
    position.x_mm = x_mm;
}

void Localisation_set_y(float y_mm){
    position.y_mm = y_mm;
}

void Localisation_set_angle(float angle_radian){
    position.angle_radian = angle_radian;
}

void Localisation_gestion(){
    float distance_roue_gauche_mm = QEI_get_mm(QEI_B_NAME);
    float distance_roue_droite_mm = QEI_get_mm(QEI_A_NAME);
    
    float delta_avance_mm, delta_orientation_rad;

    delta_avance_mm = (distance_roue_droite_mm + distance_roue_gauche_mm)/2;
    delta_orientation_rad = (distance_roue_droite_mm - distance_roue_gauche_mm) / (DISTANCE_ROUES_CENTRE_MM*2);

    position.angle_radian += delta_orientation_rad;

    // Projection dans le référentiel de la table
    position.x_mm += delta_avance_mm * cosf(position.angle_radian);
    position.y_mm += delta_avance_mm * sinf(position.angle_radian);

}

struct position_t Localisation_get(void){
    return position;
}