#include "stdio.h"

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

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

// Translation en mm/s pour aspirer les cerises
#define TRANSLATION_CERISE 15

void commande_rotation_contacteur_longer_A();
void commande_rotation_contacteur_longer_C();
void commande_translation_longer_vers_A();
void commande_translation_longer_vers_C();


double vitesse_accostage_mm_s=100;

enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    static enum {
        LONGE_INIT,
        LONGE_VERS_A,
        LONGE_VERS_C,
        LONGE_COLLE
    } etat_longer_bord=LONGE_INIT;

    switch  (etat_longer_bord){
        case LONGE_INIT:
            if(longer_direction==LONGER_VERS_A){
                etat_longer_bord=LONGE_VERS_A;
            }else{
                // longer_direction==LONGER_VERS_C
                etat_longer_bord=LONGE_VERS_C;
            }
            break;

        case LONGE_VERS_A:
            commande_translation_longer_vers_A();
            if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || 
                (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
                    etat_longer_bord = LONGE_COLLE;
                    printf("Longer colle\n");
            }
            break;
        
        case LONGE_VERS_C:
            commande_translation_longer_vers_C();
            if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || 
                (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
                    etat_longer_bord = LONGE_COLLE;
                    printf("Longer colle\n");
            }
            break;

        case LONGE_COLLE:
            if(cerise_accostage() == ACTION_TERMINEE){
                etat_longer_bord = LONGE_INIT;
                printf("Longer INIT\n");
            }
    }
    return etat_action;

}

enum etat_action_t cerise_accostage(void){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    double rotation;

    static enum {
        CERISE_AVANCE_DROIT,
        CERISE_TOURNE_CONTACTEUR_LONGER_A,
        CERISE_TOURNE_CONTACTEUR_LONGER_C,
        CERISE_ACCOSTE
    } etat_accostage=CERISE_AVANCE_DROIT;

    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);
        if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
            etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_A;
            printf("D ACCOSTE_TOURNE_A\n");
        }
        if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
            etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_C;
            printf("D ACCOSTE_TOURNE_C\n");
        }
        if (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF && i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
            etat_accostage=CERISE_ACCOSTE;
            printf("D ACCOSTE\n");
        }
        break;
    
    case CERISE_TOURNE_CONTACTEUR_LONGER_A:
        commande_rotation_contacteur_longer_A();
        if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
            if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
                etat_accostage = CERISE_AVANCE_DROIT;
                printf("A ACCOSTE_AVANCE\n");
            }else{
                etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_A;
                printf("A ACCOSTE_TOURNE_A\n");
            }
        }else if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
            etat_accostage = CERISE_ACCOSTE;
            printf("A ACCOSTE\n");
        }
        break;
    
    case CERISE_TOURNE_CONTACTEUR_LONGER_C:
        commande_rotation_contacteur_longer_C();
        if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
            if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
                etat_accostage = CERISE_AVANCE_DROIT;
                printf("C ACCOSTE_AVANCE\n");
            }else{
                etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_C;
                printf("C ACCOSTE_TOURNE_C\n");
            }
        }else if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
            etat_accostage = CERISE_ACCOSTE;
            printf("C ACCOSTE\n");
        }
        break;

    case CERISE_ACCOSTE:
        commande_vitesse_stop();
        etat_accostage = CERISE_AVANCE_DROIT;
        etat_action = ACTION_TERMINEE;
        break;
    
    default:
        break;
    }

    return etat_action;
}

void commande_rotation_contacteur_longer_A(){
    commande_rotation(-ROTATION_CERISE, RAYON_ROBOT, 0);
}

void commande_rotation_contacteur_longer_C(){
    commande_rotation(ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0);
}

void commande_translation_longer_vers_A(){
    // V_x : V * cos (60°) = V / 2
    // V_y : V * sin (60°) = V * RACINE(3) / 2
    commande_vitesse(TRANSLATION_CERISE/2., TRANSLATION_CERISE / 2. * RACINE_DE_3, 0);
}

void commande_translation_longer_vers_C(){
    // V_x : -V * cos (60°) = -V / 2
    // V_y : -V * sin (60°) = -V * RACINE(3) / 2
    commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 2. * RACINE_DE_3, 0);
}