#include <stdio.h>
#include "i2c_annexe.h"
#include "Localisation.h"
#include "math.h"

#define NB_CAPTEURS 12
#define DISTANCE_OBSTACLE_IGNORE 200
#define DISTANCE_CAPTEUR_CENTRE_ROBOT_CM 4

uint8_t distance_capteur_cm[NB_CAPTEURS];

struct capteur_VL53L1X_t{
    uint8_t distance_cm;
    double angle_ref_robot;
    double angle_ref_terrain;
    uint donnee_valide;
}capteurs_VL53L1X[NB_CAPTEURS];

void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm);
void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct position_t position_robot);

void Balise_VL53L1X_gestion(){
    i2c_annexe_get_distances(distance_capteur_cm);
    for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
        actualise_VL53L1X(&(capteurs_VL53L1X[capteur]), distance_capteur_cm[capteur]);
    }
}

void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm){
    struct position_t position_robot;
    position_robot = Localisation_get();
    // Actualisation de l'angle du capteur
    capteur_VL53L1X->angle_ref_terrain = capteur_VL53L1X->angle_ref_robot + position_robot.angle_radian;
    // Maintien de l'angle entre -PI et PI
    while(capteur_VL53L1X->angle_ref_terrain > 2* M_PI){
        capteur_VL53L1X->angle_ref_terrain -= 2* M_PI;
    }
    while(capteur_VL53L1X->angle_ref_terrain < 2* -M_PI){
        capteur_VL53L1X->angle_ref_terrain += 2* M_PI;
    }
    capteur_VL53L1X->distance_cm = distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_CM;

    invalide_obstacle(capteur_VL53L1X, position_robot);

}

/// @brief Definit si l'obstable doit être pris en comptre
/// @param  
void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct position_t position_robot){
    // Positionne l'obstacle sur le terrain
    struct position_t position_obstacle;
    //printf("Angle:%.1f\n",capteur_VL53L1X->angle_ref_terrain);
    position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_cm * 10);
    position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_cm * 10);

    capteur_VL53L1X->donnee_valide=1;
    // Si la distance vaut 0, à invalider
    if(capteur_VL53L1X->distance_cm <= DISTANCE_CAPTEUR_CENTRE_ROBOT_CM){
        capteur_VL53L1X->donnee_valide=0;
    }
    // Si l'obstacle est à l'extérieur du terrain (on prend 50 mm de marge à l'intérieur du terrain, la balise faisant 100 mm)
    //printf("X:%.1f,Y:%.1f\n", position_obstacle.x_mm, position_obstacle.y_mm);
    if((position_obstacle.x_mm < 50) || (position_obstacle.y_mm < 50) || (position_obstacle.x_mm > 1950) || (position_obstacle.y_mm > 2950))
    {
        capteur_VL53L1X->donnee_valide=0;
    }

}

void Balise_VL53L1X_init(){
    capteurs_VL53L1X[9].angle_ref_robot = 0;
    capteurs_VL53L1X[10].angle_ref_robot =   M_PI/6;
    capteurs_VL53L1X[11].angle_ref_robot = 2*M_PI/6;
    capteurs_VL53L1X[0].angle_ref_robot =  3*M_PI/6;
    capteurs_VL53L1X[1].angle_ref_robot =  4*M_PI/6;
    capteurs_VL53L1X[2].angle_ref_robot =  5*M_PI/6;
    capteurs_VL53L1X[3].angle_ref_robot =  6*M_PI/6;
    capteurs_VL53L1X[4].angle_ref_robot =  7*M_PI/6;
    capteurs_VL53L1X[5].angle_ref_robot =  8*M_PI/6;
    capteurs_VL53L1X[6].angle_ref_robot =  9*M_PI/6;
    capteurs_VL53L1X[7].angle_ref_robot =  10*M_PI/6;
    capteurs_VL53L1X[8].angle_ref_robot =  11*M_PI/6;
}

uint8_t Balise_VL53L1X_get_min_distance(void){
    uint8_t min_distance = DISTANCE_OBSTACLE_IGNORE;
    for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
        if(capteurs_VL53L1X[capteur].donnee_valide){
            if(min_distance> capteurs_VL53L1X[capteur].distance_cm){
                min_distance = capteurs_VL53L1X[capteur].distance_cm;
            }
        }
    }
    return min_distance;
}

uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){
    return capteurs_VL53L1X[capteur].distance_cm;
}