#include "Localisation.h"
#include "Strategie_PAMI.h"
#include "Geometrie.h"
#include "Trajet.h"


#define COULEUR_BLEU 1
#define COULEUR_JAUNE 0

void configure_trajet(int identifiant, int couleur){
	
	struct trajectoire_t trajectoire;
	Trajet_config(TRAJECT_CONFIG_RAPIDE);

	switch(couleur){
		case COULEUR_JAUNE:
			switch (identifiant)
			{
				case 0:
					Localisation_set(3000-1249, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
					3000-750, 1400, 3000-750, 2100, 0, 0);
					break;
				case 1:
					Localisation_set(3000-1249, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
					3000-750, 1400, 3000-750, 2100, 0, 0);
					break;
				case 2:
					Localisation_set(3000-1245, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63, 
					3000-540, 1400, 3100, 1400, M_PI, M_PI);
					break;
				case 3:
					Localisation_set(3000-1130, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, 
					3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
					break;
				case 4:
					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
					Localisation_set(3000-1364, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63, 
					2700-900, 600, 2700-0, 0, 0, 0);
					break;
				case 5:
					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
					Localisation_set(3000-1450, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63, 
					3000-930, 970, 0, 1200, -M_PI / 2., M_PI);
					break;
				case 6:
					break;
				case 7:
                    Trajet_config(200, 200);
					Localisation_set(100, 0, M_PI/2.);
					Trajectoire_circulaire(&trajectoire, 0, 0, 0, 360, 100, M_PI / 2., M_PI / 2.);
					break;
				
				default:
					break;
			}
			break;

		case COULEUR_BLEU:
			switch (identifiant)
			{
				case 0:
					Localisation_set(1249, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
					750, 1400, 750, 2100, M_PI, M_PI);
					break;
				case 1:
					Localisation_set(1249, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
					750, 1400, 750, 2100, M_PI, M_PI);
					break;
				case 2:
					Localisation_set(1245, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63, 
					540, 1400, -100, 1400, M_PI, M_PI);
					break;
				case 3:
					Localisation_set(1121, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
					606, 2000-590, 225, 2000-225, M_PI, M_PI);
					break;
				case 4:
					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
					Localisation_set(1364, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63, 
					900, 600, 0, 0, -M_PI / 2., M_PI);
					break;
				case 5:
					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
					Localisation_set(1450, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63, 
					930, 970, 3000, 1200, -M_PI / 2., M_PI);
					break;
				case 6:
					break;
				case 7:
                    Trajet_config(200, 200);
					Localisation_set(100, 0, M_PI/2.);
					Trajectoire_circulaire(&trajectoire, 0, 0, 0, 360, 100, M_PI / 2., M_PI / 2.);
					
					break;
				
				default:
					break;
			}
			break;
	}
	
	Trajet_debut_trajectoire(trajectoire);

}