#include "Commande_vitesse.h"
#include "Strategie_2024_plante.h"
#include "Geometrie_robot.h"
#include "i2c_annexe.h"
#include "math.h"
#include <stdio.h>

#define ASSER_ANGLE_GAIN_PLANTE_P 1.5
#define ASSER_DISTANCE_GAIN_PLANTE_P 10

enum etat_action_t Strat_2024_aller_a_plante(){
    static enum {
        SAAP_INIT_DETECTION,
        SAAP_ASSERV,
        SAAP_ATTRAPE,
        SAAP_ATTRAPE_TEMPO,
    } etat_aller_a_plante = SAAP_INIT_DETECTION;
    enum validite_vl53l8_t validite;
    float angle, distance, commande_vitesse_plante;
    static int tempo;

    switch(etat_aller_a_plante){
        case SAAP_INIT_DETECTION:
            i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
            etat_aller_a_plante = SAAP_ASSERV;
            break;

        case SAAP_ASSERV:
            i2c_annexe_get_VL53L8(&validite, &angle, &distance);
            if(validite == VL53L8_PLANTE){
                commande_vitesse_plante = (distance - 70) * ASSER_DISTANCE_GAIN_PLANTE_P;
                if(commande_vitesse_plante > 300){
                    commande_vitesse_plante = 300;
                }
                if(commande_vitesse_plante <= 0){
                    commande_vitesse_stop();
                    i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                    etat_aller_a_plante = SAAP_ATTRAPE;
                    break;
                }
                
                commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
                    sinf(ANGLE_PINCE) * commande_vitesse_plante , angle * ASSER_ANGLE_GAIN_PLANTE_P);
            }else{
            }
            break;
        
        case SAAP_ATTRAPE:
            i2c_annexe_attrape_plante(PLANTE_BRAS_1);
            etat_aller_a_plante = SAAP_ATTRAPE_TEMPO;
            tempo = 2500;
            break;

        case SAAP_ATTRAPE_TEMPO:
            tempo--;
            if(tempo <= 0){
                etat_aller_a_plante = SAAP_INIT_DETECTION;
                return ACTION_TERMINEE;
            }
            break;


    }

    return ACTION_EN_COURS;
}