#include "Strategie.h"
#include "Geometrie.h"



enum etat_action_t Strategie_super_star(uint32_t step_ms){
	static enum{
        SSS_TEST_TOURNE,
		SSS_INIT,
		SSS_AVANCE_1,
		SSS_TOURNE,
		SSS_AVANCE_2,
		SSS_DANCE
	} etat_sss = SSS_TEST_TOURNE;
    struct trajectoire_t trajectoire;

	switch(etat_sss){
        case SSS_TEST_TOURNE:
            Localisation_set(1135, 1895, 0);
            etat_sss = SSS_TOURNE;
            break;
		case SSS_INIT:
			Localisation_set(45, 1895, 0);
			etat_sss = SSS_AVANCE_1;
			break;

		case SSS_AVANCE_1:
            Trajet_config(TRAJECT_CONFIG_STD);
            Trajectoire_droite(&trajectoire, 45, 1895, 1135, 1895, 0, 0);
            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                //etat_sss = SSS_TOURNE;
                etat_sss = SSS_DANCE;
            }
            break;

        case SSS_TOURNE:
            Trajet_config(100, 100);
            Trajectoire_circulaire(&trajectoire, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                //etat_sss = SSS_AVANCE_2;
                etat_sss = SSS_DANCE;
            }
            break;

        case SSS_AVANCE_2:
            Trajet_config(TRAJECT_CONFIG_STD);
            Trajectoire_droite(&trajectoire, 1385, 1645, 1385, 1600, 0, 0);
            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                etat_sss = SSS_DANCE;
            }
            break;

        case SSS_DANCE:
            return ACTION_TERMINEE;
	}
    return ACTION_EN_COURS;

}