#include "Strategie.h"
#include "Asser_Position.h"
#include "Geometrie.h"
#include "Servomoteur.h"



enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t couleur){
	static enum{
		NINJA_INIT,
		NINJA_GRENIER1,
        NINJA_GRENIER2A,
        NINJA_GRENIER2B,
        NINJA_GRENIER2C,
		NINJA_FIN
	} etat_sss = NINJA_INIT;
    static struct trajectoire_t trajectoire_composee;
    static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;

	switch(etat_sss){
		case NINJA_INIT:
            if(couleur == COULEUR_JAUNE){
                Localisation_set(645, 1870, 0);
                Trajectoire_bezier(&trajectoire1, 
                    645, 1870,
                    800, 1870,
                    890, 1730,
                    990, 1730, 0., 0.);
                Trajectoire_droite(&trajectoire2, 
                    990, 1730,
                    1150, 1730, 0, 0);
                
            }else{
                Localisation_set(3000 - 45, 1870, M_PI);
                Trajectoire_bezier(&trajectoire1, 
                    3000 - 45, 1870, 3000 - 800, 1870,
                    3000 - 890, 1730, 3000 - 990, 1730, 0., 0.);
                Trajectoire_droite(&trajectoire2, 3000 - 990, 1730, 3000 - 1150, 1730, 0, 0);
            }

            Trajectoire_composee_init(&trajectoire_composee);
            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
			etat_sss = NINJA_GRENIER1;
			break;

		case NINJA_GRENIER1:
            Trajet_config(TRAJECT_CONFIG_RAPIDE);
            if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                etat_sss = NINJA_GRENIER2A;
                if(couleur == COULEUR_JAUNE){
                    Trajectoire_bezier(&trajectoire1, 
                        1150, 1730,
                        1030, 1730,
                        980, 1850,
                        980, 1970, 0., 0.);
                }
            }
            break;

        case NINJA_GRENIER2A:
            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                etat_sss = NINJA_GRENIER2B;
                if(couleur == COULEUR_JAUNE){
                    Trajectoire_bezier(&trajectoire1, 
                        980, 1970,
                        980, 1850,
                        1160, 1925,
                        1420, 1925, 0., 0.);
                }
            }
            break;
        
        case NINJA_GRENIER2B:
            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                etat_sss = NINJA_GRENIER2C;
                if(couleur == COULEUR_JAUNE){
                    Trajet_config(TRAJECT_CONFIG_STD);
                    Trajectoire_bezier(&trajectoire1, 
                        1420, 1925,
                        1377, 1925,
                        1354, 1945,
                        1354, 1980, 0., 0.);
                }
            }
            break;

        case NINJA_GRENIER2C:
            if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                etat_sss = NINJA_FIN;
            }
            break;

        case NINJA_FIN:
            //Asser_Position_maintien();
            return ACTION_TERMINEE;
	}
    return ACTION_EN_COURS;

}

enum etat_action_t Strategie_test(uint32_t step_ms){
    static enum{
		TEST_INIT,
        TEST_EN_COURS,
        TEST_TERMINE
	} etat_test = TEST_INIT;

    switch(etat_test){
        case TEST_INIT:
            if(Strategie_recale(0, step_ms) == ACTION_TERMINEE){
                etat_test = TEST_TERMINE;
            }
            break;

        case TEST_TERMINE:
            return ACTION_TERMINEE;
    }
    

    return ACTION_EN_COURS;
}



void PAMI_dance(int identifiant){
    switch (identifiant)
    {
    case 0:
        sleep_ms(500);
        Servomoteur_set(DOIGT_AVANCE);
        sleep_ms(250);
        Servomoteur_set(DOIGT_RETRAIT);
        sleep_ms(250); // t = 1,0 s
        sleep_ms(750); // t = 1,75 s
        Servomoteur_set(DOIGT_AVANCE);
        sleep_ms(250);
        Servomoteur_set(DOIGT_RETRAIT);
        sleep_ms(250); // t = 2,25 s
        sleep_ms(950); // t = 3,2 s
        for(int i =0; i<3;i++){
            Servomoteur_set(DOIGT_AVANCE);
            sleep_ms(200);
            Servomoteur_set(DOIGT_RETRAIT);
            sleep_ms(200);
        }// t = 4,4 s
        sleep_ms(1400); // t = 5,8
        break;
    case 1:
    case 2:
    case 3:
        sleep_ms(1100); // t = 1,1 s
        Servomoteur_set(DOIGT_AVANCE);
        sleep_ms(250);
        Servomoteur_set(DOIGT_RETRAIT);
        sleep_ms(250); // t = 1,6 s
        sleep_ms(800); // t = 2,4 s
        Servomoteur_set(DOIGT_AVANCE);
        sleep_ms(250);
        Servomoteur_set(DOIGT_RETRAIT);
        sleep_ms(250); // t = 2,9 s
        sleep_ms(1600); // t = 4,5 s
        for(int i =0; i<3;i++){
            Servomoteur_set(DOIGT_AVANCE);
            sleep_ms(150);
            Servomoteur_set(DOIGT_RETRAIT);
            sleep_ms(150);
        }// t = 5,4 s
        sleep_ms(400); // t = 5,8 s
        break;
    
    default:
        break;
    }

}