#include "Strategie_prise_cerises.h"
#include "Commande_vitesse.h"
#include "Geometrie.h"
#include "Geometrie_robot.h"
#include "math.h"

// Rotation en rad/s pour accoster les cerises
#define ROTATION_CERISE 0.5f

double vitesse_accostage_mm_s=100;

enum etat_action_t cerise_accostage(void){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    struct position_t contacteur_gauche, contacteur_droit;
    double rotation;

    static enum {
        CERISE_AVANCE_DROIT,
        CERISE_TOURNE_CONTACTEUR_GAUCHE,
        CERISE_TOURNE_CONTACTEUR_DROIT,
        CERISE_ACCOSTE
    } etat_accostage=CERISE_AVANCE_DROIT;


    // Position des contacteurs 
    contacteur_gauche.x_mm = RAYON_ROBOT;
    contacteur_gauche.y_mm = 0;

    contacteur_droit.x_mm = RAYON_ROBOT * cos(-M_PI/6);
    contacteur_droit.y_mm = RAYON_ROBOT * sin(-M_PI/6);

    switch (etat_accostage)
    {
    case CERISE_AVANCE_DROIT:
        commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0);
        break;
    
    case CERISE_TOURNE_CONTACTEUR_GAUCHE:
        rotation = ROTATION_CERISE;
        commande_rotation(ROTATION_CERISE, contacteur_gauche.x_mm, contacteur_gauche.y_mm);
        break;
    
    case CERISE_TOURNE_CONTACTEUR_DROIT:
        rotation = ROTATION_CERISE;
        commande_rotation(-ROTATION_CERISE, contacteur_droit.x_mm, contacteur_droit.y_mm);
        break;
    
    default:
        break;
    }

    return etat_action;
}