#include "math.h"
#include "Strategie.h"
#include "Geometrie_robot.h"
#include "Strategie_2024_pots.h"
#include "i2c_annexe.h"

#define DISTANCE_APPROCHE_POT_MM 250. 
#define DISTANCE_ATTRAPE_POT_MM 100.

float angle_bras[6] =
{
    180 * DEGRE_EN_RADIAN,
    120 * DEGRE_EN_RADIAN,
    60 * DEGRE_EN_RADIAN,
    0,
    -60 * DEGRE_EN_RADIAN,
    -120 * DEGRE_EN_RADIAN
};

struct position_t position_pots_dans_groupe_pot[5] =
    {
        {.x_mm = -40, .y_mm = 69.2, .angle_radian = -60 * DEGRE_EN_RADIAN}, 
        {.x_mm = 40, .y_mm = 69.2, .angle_radian = -120 * DEGRE_EN_RADIAN},
        {.x_mm = -80, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN},
        {.x_mm = 80, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN},
        {.x_mm = 0, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN}
    };

struct position_t position_groupe_pot[6] =
    {
        {.x_mm = 36.1, .y_mm = 1386.8, .angle_radian = -90 * DEGRE_EN_RADIAN}, 
        {.x_mm = 36.1, .y_mm = 616.2, .angle_radian = -90 * DEGRE_EN_RADIAN}, 
        {.x_mm = 1000, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, 
        {.x_mm = 2000, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, 
        {.x_mm = 2963.9, .y_mm = 616.2, .angle_radian = 90 * DEGRE_EN_RADIAN}, 
        {.x_mm = 2963.9, .y_mm = 1386.8, .angle_radian = 90 * DEGRE_EN_RADIAN}
    };


/// @brief renvoie la position du centre du pot ainsi que l'ange par lequel l'attraper
/// @param groupe_pot Position du groupe de pot
/// @param num_pot Pot à prendre, entre 0 et 4 (ou utiliser les macros POT_x)
struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot){
    struct position_t position_pot;
    struct position_t my_position_groupe_pot;
    
    my_position_groupe_pot = position_groupe_pot[groupe_pot];
    
    float angle_groupe_pot = my_position_groupe_pot.angle_radian;
    position_pot.x_mm = my_position_groupe_pot.x_mm + 
        cosf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].x_mm -
        sinf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].y_mm;

    position_pot.y_mm = my_position_groupe_pot.y_mm + 
        sinf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].x_mm +
        cosf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].y_mm;
    
    position_pot.angle_radian = position_pots_dans_groupe_pot[num_pot].angle_radian + angle_groupe_pot;

    return position_pot;
}


int get_bras_libre(void){
    return BRAS_1;
}

/// @brief Fonction qui déplace le robot jusqu'à la zone pour attraper les pots et qui attrape les 5 pots
enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms){
    // Parcourir la trajectoire pour aller jusqu'au premier pot
    struct position_t position_pot, position_approche_pot, position_attrape_pot;
    enum etat_action_t etat_action;

    static int bras, tempo_ms;

    // Pour le 1er pot
    position_pot = groupe_pot_get_pot(groupe_pot, POT_1);
    position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
    position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);

    static enum {
        AP_ALLER_VERS_GROUPE_POT,
        AP_APPROCHE_POT,
        AP_ATTRAPE_POT,
        AP_RETOUR_ET_LEVE_POT
    } etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;

    // Pour chaque pot
        // Baisser le bras correspondant
        // Aller jusqu'au point de prise de pot en s'orientant pour prendre le pot
        // Avancer de X cm en direction du pot
        // Reculer dans l'axe de prise et rejoindre le point de prise suivant
            // Pendant le mouvement, apres 1 sec (à confirmer) Lever le bras

    switch (etat_attrape_pot)
    {
        case AP_ALLER_VERS_GROUPE_POT:
            printf("position_pot X:%f Y:%f r:%f\n", position_pot.x_mm, position_pot.y_mm,
                position_pot.angle_radian / DEGRE_EN_RADIAN);
            printf("position_approche_pot X:%f Y:%f r:%f\n", position_approche_pot.x_mm, position_approche_pot.y_mm,
                position_approche_pot.angle_radian / DEGRE_EN_RADIAN);
            etat_attrape_pot = AP_APPROCHE_POT;
        case AP_APPROCHE_POT:
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            etat_action = Strategie_tourner_et_aller_a(
                position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[0],
                SANS_EVITEMENT, step_ms);
            if (etat_action == ACTION_TERMINEE){
                etat_attrape_pot = AP_ATTRAPE_POT;
                bras = get_bras_libre();
            }
            break;
        
        case AP_ATTRAPE_POT:
            i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT);
            etat_action = Strategie_tourner_et_aller_a(
                position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
                SANS_EVITEMENT, step_ms);
            if (etat_action == ACTION_TERMINEE){
                tempo_ms=250;
                etat_attrape_pot = AP_RETOUR_ET_LEVE_POT;
            }
            break;
        
        case AP_RETOUR_ET_LEVE_POT:
            if(tempo_ms >= 0){
                tempo_ms -= step_ms;
            }else{
                i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
            }
            etat_action = Strategie_tourner_et_aller_a(
                position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
                SANS_EVITEMENT, step_ms);
            if (etat_action == ACTION_TERMINEE){
                etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
                // TODO: pot suivant
                return ACTION_TERMINEE;
            }
            break;

        default:
            break;
    }

    return ACTION_EN_COURS;

}