#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

struct position_t liste_zone_plante[]=
{
  {.x_mm = 1500, .y_mm = 1500 },
  {.x_mm = 1000, .y_mm = 1300 },
  {.x_mm = 1000, .y_mm = 700 },
  {.x_mm = 1500, .y_mm = 500 },
  {.x_mm = 2000, .y_mm = 700 },
  {.x_mm = 2000, .y_mm = 1300 }
};



enum etat_action_t Strat_2024_aller_zone_plante(uint32_t step_ms){
    int zone_plante = 2;
    struct position_t position_robot, position_zone_plante;
    float angle;

    position_zone_plante = liste_zone_plante[zone_plante];
    position_robot = Localisation_get(); 
    angle = atan2f(position_zone_plante.x_mm - position_robot.x_mm, position_zone_plante.y_mm - position_robot.y_mm);
    return Strategie_tourner_a(angle, step_ms);
}

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_PRE_PRISE_PLANTE,
        PDP_PRE_PRISE_TEMPO,
        PDP_PRE_PRISE_RECULE,
        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){
                etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
            }
            break;

        case PDP_PRE_PRISE_PLANTE:
            i2c_annexe_ferme_doigt_plante();
            tempo_ms = 250;
            etat_plante_dans_pot=PDP_PRE_PRISE_TEMPO;
            break;

        case PDP_PRE_PRISE_TEMPO:
            tempo_ms--;
            if(tempo_ms <= 0){
                etat_plante_dans_pot=PDP_PRE_PRISE_RECULE;
            }
            break;
        
        case PDP_PRE_PRISE_RECULE:
            position_initiale = Localisation_get();
            position_initiale.angle_radian += ANGLE_PINCE;
            position_recule = Geometrie_deplace(position_initiale, -50);
            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();
                i2c_annexe_ouvre_doigt_plante();
                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;
}