#include "Commande_vitesse.h"
#include "Strategie_2024_plante.h"
#include "Geometrie_robot.h"
#include "Localisation.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(void){
    static enum {
        SAAP_INIT_DETECTION,
        SAAP_ASSERV
    } 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_INIT_DETECTION;
                    return ACTION_TERMINEE;
                }
                
                commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
                    sinf(ANGLE_PINCE) * commande_vitesse_plante , angle * ASSER_ANGLE_GAIN_PLANTE_P);
            }else{
            }
            break;
    }

    return ACTION_EN_COURS;
}

/// @brief S'approche d'une plante et la dépose dans un pot déjà attrapé
/// @param step_ms 
/// @param bras_depose_pot : PLANTE_BRAS_1 ou PLANTE_BRAS_6
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot){
    static enum{
        PDP_ALLER_PLANTE,
        PDP_ATTRAPE_PLANTE,
        PDP_RECULE,
        PDP_TEMPO,
    } etat_plante_dans_pot = PDP_ALLER_PLANTE;
    static int tempo_ms;
    struct position_t position_recule, position_initiale;

    switch (etat_plante_dans_pot)
    {
        case PDP_ALLER_PLANTE:
            if (Strat_2024_aller_a_plante() == ACTION_TERMINEE){
                i2c_annexe_attrape_plante(bras_depose_pot);
                tempo_ms = 2000;

                etat_plante_dans_pot=PDP_ATTRAPE_PLANTE;
            }
            break;

        case PDP_ATTRAPE_PLANTE:
            tempo_ms--;
            if(tempo_ms <= 0){
                etat_plante_dans_pot=PDP_RECULE;
                tempo_ms = 2000;
            }
            break;

        case PDP_RECULE:
            tempo_ms--;
            position_initiale = Localisation_get();
            position_initiale.angle_radian += ANGLE_PINCE;
            position_recule = Geometrie_deplace(position_initiale, -100);
            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
            if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, step_ms) == ACTION_TERMINEE){
                commande_vitesse_stop();
                etat_plante_dans_pot=PDP_TEMPO;
            }
            break;

        case PDP_TEMPO:
            tempo_ms--;
            if(tempo_ms <= 0){
                etat_plante_dans_pot=PDP_ALLER_PLANTE;
                return ACTION_TERMINEE;
            }            
        default:
            break;
    }
    return ACTION_EN_COURS;
}