#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

#define RAYON_ZONE_PLANTE_MM 180

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(enum zone_plante_t zone_plante, uint32_t step_ms){
    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.y_mm - position_robot.y_mm, position_zone_plante.x_mm - position_robot.x_mm);
    return Strategie_tourner_a(angle - ANGLE_PINCE, step_ms);
}
/// @brief Indique si la plante se trouve dans la zone de récole (en comptant une marge)
/// @param position_plante : position de la plante
/// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante
/// @return 1 si la plante est dans la zone, 0 si le robot ne trouve pas la plante ou qu'elle est hors zone.
bool est_dans_zone(struct position_t position_plante, enum zone_plante_t zone_plante){
    struct position_t position_zone_plante;
    float distance_plante_zone;

    position_zone_plante = liste_zone_plante[zone_plante];
    
    distance_plante_zone = sqrtf( powf(position_zone_plante.x_mm - position_zone_plante.x_mm, 2) + 
                                powf(position_zone_plante.y_mm - position_zone_plante.y_mm, 2));
    
    if(distance_plante_zone < RAYON_ZONE_PLANTE_MM){
        return 1;
    }
    return 0;   
}

/// @brief Déplace le robot vers une plante, vérifie que la plante est bien dans la zone plante qu'on vise
/// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante
/// @return ACTION_SUCCESS si le robot est prêt à attraper la plante, ACTION_ECHEC si la plante 
enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
    static enum {
        SAAP_INIT_DETECTION,
        SAAP_ASSERV
    } etat_aller_a_plante = SAAP_INIT_DETECTION;
    enum validite_vl53l8_t validite;
    float angle_rad, distance_mm, distance_obstacle, commande_vitesse_plante;
    float distance_contrainte_obstacle, vitesse_max_contrainte_obstacle;
    const float acceleration_mm_ss_obstacle=500;
    static float distance_min_mm;
    static int tempo_ms, tempo_asserv;
    static bool entree_dans_zone;

    switch(etat_aller_a_plante){
        case SAAP_INIT_DETECTION:
            i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
            i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
            i2c_annexe_mi_ferme_doigt_plante();
            i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
            tempo_ms = 2000;
            distance_min_mm = 2000;
            tempo_asserv = 500;
            entree_dans_zone=false;
            etat_aller_a_plante = SAAP_ASSERV;
            break;

        case SAAP_ASSERV:
            i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm);
            if(validite == VL53L8_PLANTE){
                tempo_ms = 2000;
                // 1 on s'assure que la plante est dans la zone qu'on recherche !
                if(zone_plante != ZONE_PLANTE_AUCUNE){
                    // Cas où on checher une plante dans une zone
                    struct position_t position_robot, position_plante;
                    bool robot_dans_zone;
                    position_robot = Localisation_get();
                    position_robot.angle_radian += ANGLE_PINCE + angle_rad;
                    position_plante = Geometrie_deplace(position_robot, distance_mm);
                    if( !est_dans_zone(position_plante, zone_plante)){
                        return ACTION_ECHEC;
                    }
                    robot_dans_zone = est_dans_zone(Localisation_get(), zone_plante);
                    if(entree_dans_zone == true){
                        if(robot_dans_zone == false){
                            // Le robot est sorti de la zone
                            return ACTION_ECHEC;
                        }
                    }
                    if(robot_dans_zone == true){
                        entree_dans_zone = true;
                    }
                }

                // 2 on s'assure qu'il n'y a pas de robot en face (TODO)
                distance_contrainte_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(ANGLE_PINCE);
                if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
                    vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
                }

                // 2 bis, on s'assure que le robot se rapproche de la plante. Si ce n'est pas le cas, on arrête
                if(distance_mm < distance_min_mm){
                    distance_min_mm = distance_mm;
                    tempo_asserv = 500;
                }else{
                    tempo_asserv--;
                    if(tempo_asserv <= 0){
                        commande_vitesse(0, 0, 0);
                        return ACTION_ECHEC;
                    }
                }

                // 3 on asservi
                commande_vitesse_plante = (distance_mm - 83) * 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;
                }
                // On limite la vitesse avec l'obstacle
                if (commande_vitesse_plante > vitesse_max_contrainte_obstacle){
                    commande_vitesse_plante = vitesse_max_contrainte_obstacle;
                }
                
                commande_vitesse(cosf(ANGLE_PINCE + 0.04) * commande_vitesse_plante ,
                    sinf(ANGLE_PINCE + 0.04) * commande_vitesse_plante , (angle_rad - 0.04) * ASSER_ANGLE_GAIN_PLANTE_P);
            }else{
                tempo_ms--;
                if(tempo_ms <= 0){
                    return ACTION_ECHEC;
                }
            }
            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 ou ACTION_ECHEC (plante tombée devant le robot)
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot, enum zone_plante_t zone_plante){
    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;
    enum validite_vl53l8_t validite;
    enum etat_action_t etat_action;
    float angle_rad, distance_mm;

    switch (etat_plante_dans_pot)
    {
        case PDP_ALLER_PLANTE:
            etat_action = Strat_2024_aller_a_plante(zone_plante);
            if (etat_action == ACTION_TERMINEE){
                etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
            }else if (etat_action == ACTION_ECHEC){
                return ACTION_ECHEC;
            }
            break;

        case PDP_PRE_PRISE_PLANTE:
            i2c_annexe_set_mode_VL53L8(VL53L8_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){
                tempo_ms=500;
                etat_plante_dans_pot=PDP_PRE_PRISE_RECULE;
            }
            break;
        
        case PDP_PRE_PRISE_RECULE:
            i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm);
            tempo_ms--;
            if(tempo_ms <= 0){
                i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
                i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
            }
            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){
                if(validite == VL53L8_PLANTE){
                    if(distance_mm > 90){
                        // Nous n'avons pas attrapé la plante avec le doigt, on recommence:
                        etat_plante_dans_pot = PDP_ALLER_PLANTE;
                        i2c_annexe_ouvre_doigt_plante();
                        i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                        break;
                    }
                }
                commande_vitesse_stop();
                i2c_annexe_ouvre_doigt_plante();
                i2c_annexe_attrape_plante(bras_depose_pot);
                tempo_ms = 5000;
                i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                etat_plante_dans_pot=PDP_ATTRAPE_PLANTE;
            }
            break;


        case PDP_ATTRAPE_PLANTE:
            tempo_ms--;
            if(tempo_ms <= 0){
                etat_plante_dans_pot=PDP_TEMPO;
                if(bras_depose_pot == PLANTE_BRAS_1){
                    i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
                }else{
                    i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
                }
                tempo_ms = 250;
            }
            break;

        case PDP_TEMPO:
            tempo_ms--;
            if(tempo_ms <= 0){
                etat_plante_dans_pot=PDP_RECULE;
            }
            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_ALLER_PLANTE;
                return ACTION_TERMINEE;
            }
            break;
         
        default:
            break;
    }
    return ACTION_EN_COURS;
}