#include "strategie.h"
#include "odometrie.h"
#include "asservissement.h"
/*#include "CommandGoTo.h"
#include "CommandAvancerToutDroit.h"
#include "CommandTourner.h"
#include "Bras.h"
*/


Strategie* Strategie::strategie = NULL;

Strategie::Strategie(bool is_blue, Odometrie* odometrie)
    : collision_detected(false)//, listeActions(NULL)
{
    this->is_blue = is_blue;
    this->odometrie = odometrie;
    Strategie::strategie = this;
    Position positionDeDepart(POS_DEPART_X,POS_DEPART_Y);
    Angle angleDeDepart(ANGLE_DEPART);

     positionDeDepart.setY(is_blue ? positionDeDepart.getY() : -positionDeDepart.getY());

    angleDeDepart = (is_blue ? angleDeDepart : -angleDeDepart);

    //command = new Asservissement(PositionPlusAngle(Position(335, 400), Angle(M_PI_2)), roueCodeuseGauche, roueCodeuseDroite);

    odometrie->setPos(PositionPlusAngle(positionDeDepart,angleDeDepart));

    //command = new Asservissement(PositionPlusAngle(positionDeDepart, angleDeDepart), roueCodeuseGauche, roueCodeuseDroite);
    //command = new Asservissement(PositionPlusAngle(Position(0, 0), Angle(0)), roueCodeuseGauche, roueCodeuseDroite);
    //command->strategie = this;
 //   instruction_nb=1;
  //  doNthInstruction(instruction_nb);
   listeActions= new ListeActions(odometrie,is_blue);

    listeActions->creerPremiereAction();

}

void Strategie::update()
{
 //   int cote = (is_blue ? 1:-1);

if (listeActions->getActionActuelle())
{
    if(listeActions->getActionActuelle()->executer())
    {
        listeActions->supprimerPremiereAction();
        listeActions->creerPremiereAction();
    }
}
else
{
    Command::freinageDUrgence(true);
    Asservissement::finMatch();
}


/*   bool dejaVu = false;
   if (Asservissement::asservissement->command == NULL)
    {
        new CommandGoTo(Position(1000,cote*400));
     //   int i =0; i++;
    // new CommandTourner(Angle(M_PI_2));
    //    new CommandAvancerToutDroit(1000);
    }
    if ((odometrie->getPos().getPosition()-Position(1000,cote*400)).getNorme()< DISTANCE_ARRET && !dejaVu)
    {
        dejaVu = true;
        new CommandGoTo(Position(1000,cote*1000));
        //new CommandTourner(M_PI_2);
    }
   if ((odometrie->getPos().getPosition()-Position(1000,cote*1000)).getNorme()< DISTANCE_ARRET)
    {
        Command::freinageDUrgence(true);
    }
    //Si tout est ok, on passe à la suite
    if(!collision_detected){
        instruction_nb++;
        doNthInstruction(instruction_nb);
    }
    else{   //Sinon on fait les instructions au dessus de 128 (Pas utilisé ici)
        instruction_collision_nb++;
        doNthInstruction(INSTRUCTION_COLLISION+instruction_collision_nb);
    }
*/
}

void Strategie::collisionDetected(){
    collision_detected = true;
    instruction_collision_nb=0;
    doNthInstruction(INSTRUCTION_COLLISION);
}

void Strategie::doNthInstruction(uint16_t n){

}

