#include "LZA_Thread.h"
#include <string>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <localization/lza.hpp>
#include <pid/rpath.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>

#include <string>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include "Time_management.h"
#include "Formatings.h"
#include "Geometrie.h"

struct LZACorel{
  localization::ROBLOC pos;
  double corel;
};
bool CompLZA(LZACorel me, LZACorel is){
  return (me.corel<is.corel);
}

//void Recomput_Weeldist(euclid_position Oldstate,euclid_position R_state, double &ssg, double &ssd);
//void compute_Fenetre_Error_Odo(euclid_position R_state, double distLeft, double distRight, Eigen::MatrixXd &P, double &x1, double &x2, double &y1, double &y2);
//void SimuOdo(euclid_position StartPos,euclid_position &SimPos, double DistRG, double DistRD);
void Format_LAS(laser_4m_record Las1, laser_4m_record Las2, double DIST[]);
bool isLZAResult(localization::ROBLOC LZAres,euclid_position Odosource, double tetTol, bool Trusted,euclid_position LastRecal);
localization::ROBLOC MultiresLZA(localization::LZALocalization lzamod, double Las[57], double dist, P3D_Robot_State Rob_State);
//void FusionLZA(std::vector<double> ixOdo,std::vector<double> iyOdo,std::vector<double> itOdo, localization::ROBLOC LZAres, int Mode, localization::ROBLOC &RelocVal));
bool FusionLZA(double Iodo[6],int Mode, euclid_position LZARes,euclid_position OdoEstime, euclid_position &RelocVal);

bool isLZAusable(euclid_position odoposP3D);

double GetRTY(std::vector<struct polar_coordinate> Mesures);

void LZAthread(SafeShared<euclid_position> &RecalageLoc, SafeShared<laser_4m_record> &las1, SafeShared<laser_4m_record> &las2, SafeShared<euclid_position> &odom, SafeShared<int> &Mode, SafeShared<int> &ModeCompas, SafeShared<euclid_position> &LZA_res, SafeShared<double> &Compas_res, bool verbose, bool Logging){
  bool running=true;
  bool new_estime_flag=false;
  bool followSimStateOld, followSimState=false;
  int presentMode, presentModeCompas;
  struct timespec StartTime, Time, TLas1, TLas2, Todo, Toldodo, LauncheTime, TimeLocOld, TimeEstime;
  laser_4m_record relevel1, relevel2;
  euclid_position odoposP3D, odopos, OdoPosOld, offset, oldOdosim, OdoposSim, OdoEstime, Val_Recalage, LZA_estime;
  double distLeft, distRight;
  double datas_loc[57];
  double Dsleeped;
  double EstimeCompas, LastCompas, LZACompas;
  double Total_Reloc=0;
  std::vector<struct polar_coordinate> Telemetries;
  std::vector<struct polar_coordinate> Mesures_Droite;
  std::vector<struct polar_coordinate> OldLas1,OldLas2;
  struct timespec Las1Old, Las2Old, Las1Time, Las2Time;
  struct timespec TimeFormat,TimeLZA;
  struct timespec TimeCompas,TimeLog;
  int LZArun=0;

  std::vector<double> Logs;
  std::vector<double> LZA_LAS;
  std::vector<struct timespec> LogTime;
  std::vector<int> LogMode;
  std::vector<double> Ixinit(2),Iyinit(2),Itinit(2),Ix(2),Iy(2),It(2);
  double drecalage,thetad;
  Ixinit=initInterval(-0.58675,0.48832);
  Iyinit=initInterval(-0.52998,0.54509);
  Itinit=initInterval(-0.53878,0.53629);
  euclid_position LastLoc, EstimeInit, LoopDestect_pose;
  double Ds_loop_detect;
  int consfail=0;
  int logline=0;
  int Reloc_Flag, SleepReloc_Flag=0;

  OdoPosOld.pos.x=0;
  OdoPosOld.pos.y=1.1;
  OdoPosOld.pos.z=0;
  OdoPosOld.dir.phi=0;
  OdoPosOld.dir.psy=0;
  OdoPosOld.dir.theta=0;
  OdoposSim=OdoPosOld;
  Val_Recalage.pos.x=999;
  Val_Recalage.pos.y=999;
  Val_Recalage.pos.z=0;
  Val_Recalage.dir.phi=0;
  Val_Recalage.dir.psy=0;
  Val_Recalage.dir.theta=999;
  LZA_estime=Val_Recalage;
  LastLoc=Val_Recalage;
  double IOdo[6]={0,0,0,0,0,0};

  bool simuon=false;
  double x1,x2,y1,y2,t1,t2;
  double x1sim,x2sim,y1sim,y2sim=0;
  bool verif, logneed;
  std::vector<euclid_position> Histo_Odo;
  std::vector<localization::ROBLOC> Histo_LZA;
  int Nloc, Nlocoffset, icoherence;
  double dtol;
  int lsleep;

  std::string mapfolder =PID_PATH("LZA_files");
  std::cout<<"Map folder : "<<mapfolder<<std::endl;
  localization::LZALocalization lzamod (mapfolder.c_str(),0,0,0);//LZALocalization map exactly on Experimental Map. on a observe un ecart de 0.4 rad pout le theta en offset...
  lzamod.set_Fenetre(-2,-2,4,4);
  lzamod.set_Position(0,1.1,0);
  P3D_Robot_State etat_robot;
  localization::ROBLOC loclza;

  Eigen::MatrixXd PLZA(3,3);
  bool Monoparticular;
/**************************************************************************
//  PLZA << pow(0.038383,2),0,0,0,pow(0.037831,2),0,0,0,pow(0.023118,2);
PLZA << pow(0.1554,2),0,0,0,pow(0.1736,2),0,0,0,pow(0.2353,2);
  int Repet_Lost=19;//20;//11;
  int Repet_maintain=14;//10;
  double dist_reobserve_m=0.4*1.7605;//1.5941;//1.5754;
  double Ttol=(3.44*M_PI)/2;//3.0156;//2*M_PI/57;
  int Failtol=21;//19;
    Monoparticular=true;
/*****************************************************************/
  Monoparticular=false;
//  PLZA << pow(0.3186,2),0,0,0,pow(0.1727,2),0,0,0,pow(0.1671,2);
  PLZA << pow(0.1182,2),0,0,0,pow(0.1432,2),0,0,0,pow(0.1622,2);//Original
//  PLZA << pow(0.1182,2),0,0,0,pow(0.1432,2),0,0,0,pow(0.13,2);
  int Repet_Lost=19;//6;
  int Repet_maintain=14;//6;
  double dist_reobserve_m=0.4*0.55;//0.467;//2.4966;
  double Ttol=M_PI/4;//2.31;//3.8983/3;//4.066;//
  int Failtol=19;//18;//19;
  lzamod.set_DXteCoef(2.57);
  lzamod.set_DXpourCoef(3.46);
/****************************************************************/

  int Fail_Count=0;
  Eigen::MatrixXd P(3,3);
  P=PLZA;
  Eigen::MatrixXd PSim(3,3);
  PSim=PLZA;
  double Ttolecrase;
  bool Loc_requis=true;
  bool Heated=false;
  bool Localised_once=false;
  euclid_position HeatedPos=OdoPosOld;
  euclid_position NewHeatedPos=OdoPosOld;

	clock_gettime(CLOCK_REALTIME, &Time);
  StartTime=Time;
  TLas1=Time;
  TLas2=Time;
  Todo=Time;
  Toldodo=Time;
  LauncheTime=Time;
  TimeLocOld=Time;
  TimeEstime=Time;

  std::string filename = PID_PATH("+PANORAMA_Log/"+std::to_string(LauncheTime.tv_sec)+"/LZA_LOG.txt");
  std::ofstream savefile;
  if(Logging){
    savefile.open(filename,std::ios::out);
  }
  std::string filenameRl = PID_PATH("+PANORAMA_Log/"+std::to_string(LauncheTime.tv_sec)+"/Right_Las_LOG.txt");
  std::ofstream savefileRl;
  if(Logging){
    savefileRl.open(filenameRl,std::ios::out);
  }

  LauncheTime=Mode.Get(presentMode);
  Mode.Set(LZA_IDLEMODE,LauncheTime);
  presentMode=LZA_IDLEMODE;
  Las2Old=LauncheTime;
  Las1Old=LauncheTime;

  Nlocoffset=0;
  LZACompas=999;

  struct timespec OtherRelocTime, OldOtherRelocTime, P3DPosTime;
  euclid_position OtherRelocPos, P3DPos;
  double YRT=999;

  while(running){
    ModeCompas.Get(presentModeCompas);
    Mode.Get(presentMode);
    clock_gettime(CLOCK_REALTIME, &StartTime);
    logneed=false;
    switch (presentModeCompas) {
      case COMPAS_IDLEMODE:
        EstimeCompas=999;
        LastCompas=EstimeCompas;
        break;
      case COMPAS_OBSERVEMODE:
      case COMPAS_RUNNINGMODE:
        Todo=odom.Get(odoposP3D);
        Las2Time=las2.Get(relevel2);
        Las1Time=las1.Get(relevel1);
        if (isafter(Las2Time,Las2Old)){
          OldLas2=format_Telemetre(relevel2.data,relevel2.last_lenght,0.03,0,M_PI);
          Las2Old=Las2Time;
        }
        if (isafter(Las1Time,Las1Old)){
          OldLas1=format_Telemetre(relevel1.data,relevel1.last_lenght,0.03,0,0.7031*M_PI/180);
          Las1Old=Las1Time;
        }

        Telemetries.clear();
        if(isafter(Las2Old,LauncheTime) && !(enoughttimepassed(StartTime, Las2Old, 1000))){
          Telemetries.insert(Telemetries.end(),OldLas2.begin(),OldLas2.end());
        }
        if(isafter(Las1Old,LauncheTime) &&  !(enoughttimepassed(StartTime, Las1Old, 1000))){
          Telemetries.insert(Telemetries.end(),OldLas1.begin(),OldLas1.end());
        }
        clock_gettime(CLOCK_REALTIME, &TimeCompas);
        EstimeCompas=Compute_ThetaCompas(Telemetries,odoposP3D);
        //if(EstimeCompas!=999){
          LastCompas=EstimeCompas;
        //}
        logneed=true;
        if(presentModeCompas==COMPAS_RUNNINGMODE)
        {
          Compas_res.Set(EstimeCompas,TimeCompas);
        }
        break;
      case COMPAS_LOGLAS:
        Todo=odom.Get(odoposP3D);
        Las2Time=las2.Get(relevel2);
        Las1Time=las1.Get(relevel1);
        if (isafter(Las2Time,Las2Old)){
          OldLas2=format_Telemetre(relevel2.data,relevel2.last_lenght,0.03,0,M_PI);
          Las2Old=Las2Time;
        }
        if (isafter(Las1Time,Las1Old)){
          //OldLas1=format_Telemetre(relevel1.data,relevel1.last_lenght,0.03,0,0.7031*M_PI/180);
          OldLas1=format_Telemetre(relevel1.data,relevel1.last_lenght,0.03,0,0);
          Las1Old=Las1Time;
        }

        Telemetries.clear();
        if(isafter(Las2Old,LauncheTime) && !(enoughttimepassed(StartTime, Las2Old, 1000))){
          Telemetries.insert(Telemetries.end(),OldLas2.begin(),OldLas2.end());
        }
        if(isafter(Las1Old,LauncheTime) &&  !(enoughttimepassed(StartTime, Las1Old, 1000))){
          Telemetries.insert(Telemetries.end(),OldLas1.begin(),OldLas1.end());
        }
        clock_gettime(CLOCK_REALTIME, &Time);

        savefileRl<<(StartTime.tv_nsec+(StartTime.tv_sec*1000000000))<<","<<presentModeCompas;
        for(struct polar_coordinate E:Telemetries){
          savefileRl<<','<<E.dist<<','<<E.dir.theta;
        }
        savefileRl<<"\n";
        break;
      case COMPAS_RTMODE:
        Todo=odom.Get(odoposP3D);
        Las2Time=las2.Get(relevel2);
        Las1Time=las1.Get(relevel1);
        if (isafter(Las2Time,Las2Old)){
          OldLas2=format_Telemetre(relevel2.data,relevel2.last_lenght,0.03,0,M_PI);
          Las2Old=Las2Time;
        }
        if (isafter(Las1Time,Las1Old)){
          OldLas1=format_Telemetre(relevel1.data,relevel1.last_lenght,0.03,0,0.7031*M_PI/180);
          Las1Old=Las1Time;
        }

        Telemetries.clear();
        if(isafter(Las2Old,LauncheTime) && !(enoughttimepassed(StartTime, Las2Old, 1000))){
          Telemetries.insert(Telemetries.end(),OldLas2.begin(),OldLas2.end());
        }
        if(isafter(Las1Old,LauncheTime) &&  !(enoughttimepassed(StartTime, Las1Old, 1000))){
          Telemetries.insert(Telemetries.end(),OldLas1.begin(),OldLas1.end());
        }
        Mesures_Droite.clear();
        for(int i=0; i<Telemetries.size();i++){
          if(Telemetries[i].dir.theta<(-M_PI/4) && Telemetries[i].dir.theta>(-3*M_PI/4)){
            Mesures_Droite.push_back(Telemetries[i]);
          }
        }
        clock_gettime(CLOCK_REALTIME, &TimeCompas);
        EstimeCompas=Compute_ThetaCompas(Telemetries,odoposP3D);
        //if(EstimeCompas!=999){
          LastCompas=EstimeCompas;
        //}
        YRT=GetRTY(Mesures_Droite);
        logneed=true;
        break;
      default:
        break;
    }
    Reloc_Flag=0;
    SleepReloc_Flag=0;
    followSimStateOld=followSimState;
    switch (presentMode) {
      case LZA_IDLEMODE:
        Todo=odom.Get(OdoPosOld);
        P << pow(0.8,2),0,0,0,pow(0.5,2),0,0,0,pow(0.2062,2);//Non actif on ne sait donc pas bien ou on se trouve.
        Val_Recalage.pos.x=999;
        Val_Recalage.pos.y=999;
        Val_Recalage.pos.z=0;
        Val_Recalage.dir.phi=0;
        Val_Recalage.dir.psy=0;
        Val_Recalage.dir.theta=999;
        LastLoc=Val_Recalage;
        Nloc=0;
        LZArun=0;
        Fail_Count=0;
        followSimState=false;
        Localised_once=false;
        break;
      case LZA_HALTINGMODE:
        running = false;
        break;
      case LZA_FUS_INIT_COMPASMODE:
      case LZA_INIT_COMPASMODE:
        LZACompas=EstimeCompas;
      case LZA_FUS_INITMODE:
      case LZA_INITMODE: //On préchauffe LZA => On execute LZA mais on n'ecrase pas l'odometrie.
        //Recupération LAS-Odometrie.
        if(!Heated){
          HeatedPos.pos.x=0;
          HeatedPos.pos.y=0;
          HeatedPos.dir.theta=0;
        }
        las2.Get(relevel2);
        las1.Get(relevel1);
        Todo=odom.Get(odoposP3D);
        OtherRelocTime=RecalageLoc.Get(OtherRelocPos);
        clock_gettime(CLOCK_REALTIME, &TimeEstime);

        if (!isLZAusable(odoposP3D)){
            LZArun=0;
            break;
        }
        LZArun=1;
        //Lecture des recalage.
        //P3DPos=odoposP3D;
        //Si un nouveau recalage a lieux ET que la position odométrique lue est a moins de 10cm
        if(isafter(OtherRelocTime,OldOtherRelocTime)&&((sqrt((odoposP3D.pos.x-OtherRelocPos.pos.x)*(odoposP3D.pos.x-OtherRelocPos.pos.x)+(odoposP3D.pos.y-OtherRelocPos.pos.y)*(odoposP3D.pos.y-OtherRelocPos.pos.y)))<0.1)){
          //Alors on supose que la relocalisation a été prise en compte par l'odométrie
          // - P3D=Recalage
          // - MAJ de la date du dernier recalage
          OldOtherRelocTime=OtherRelocTime;
          OdoPosOld=OtherRelocPos;
        }
        //SI L'odometrie est postérieur a la dernière estimation on continu.
        if(isafter(Todo,Toldodo)){
          logneed=true;
          Format_LAS(relevel1, relevel2,datas_loc);
          clock_gettime(CLOCK_REALTIME, &TimeFormat);
          if (!isLZAusable(odoposP3D)){
            break;
          }

          //Estimation OdoRob.
          Recomput_Weeldist(OdoPosOld,odoposP3D,distLeft,distRight);

          //Calcule fenêtre avec Erreur Odo
          if(!Heated){
            compute_Fenetre_Error_Odo(OdoPosOld,distLeft,distRight,P,x1,x2,y1,y2,t1,t2);
            odopos=odoposP3D;
          }else{
            SimuOdo(HeatedPos,NewHeatedPos,distLeft,distRight);
            compute_Fenetre_Error_Odo(HeatedPos,distLeft,distRight,P,x1,x2,y1,y2,t1,t2);
            odopos=NewHeatedPos;
          }
          Dsleeped=sqrt(pow((Val_Recalage.pos.x-odopos.pos.x),2)+pow((Val_Recalage.pos.y-odopos.pos.y),2));
          if (Dsleeped<dist_reobserve_m){//On c'est relocalisé il y a peut on ne fais rien.
            SleepReloc_Flag=1;
            OdoPosOld=odoposP3D;
            Toldodo=Todo;
            HeatedPos = NewHeatedPos;
            break;
          }/**/
          SleepReloc_Flag=0;

          if(Localised_once){
            drecalage=sqrt(pow((LastLoc.pos.x-odopos.pos.x),2)+pow((LastLoc.pos.y-odopos.pos.y),2));
            if(drecalage!=0){
              thetad=atan2(odopos.pos.y-LastLoc.pos.y,odopos.pos.x-LastLoc.pos.x);
              Ix=Iadd(Isub(initInterval(LastLoc.pos.x,LastLoc.pos.x),
                          Ixinit),
                      Imul(Icos(Isub(initInterval(thetad,thetad),
                                Itinit)),
                          initInterval(drecalage,drecalage)));
              Iy=Iadd(Isub(initInterval(LastLoc.pos.y,LastLoc.pos.y),
                          Iyinit),
                      Imul(Icos(Isub(initInterval(thetad,thetad),
                                      Itinit)),
                          initInterval(drecalage,drecalage)));
            }else{
              Ix=Isub(initInterval(LastLoc.pos.x,LastLoc.pos.x),Ixinit);
              Iy=Isub(initInterval(LastLoc.pos.y,LastLoc.pos.y),Iyinit);
            }
            Ix=Iadd(Icentering(initInterval(x1,x2)),Ix);
            Iy=Iadd(Icentering(initInterval(y1,y2)),Iy);
            IVals(Ix,x2,x1);
            IVals(Iy,y2,y1);
          }else{
            x1=x1+1+consfail*0;//1;//2;
            x2=x2-1-consfail*0;//1;//2;
            y1=y1+1+consfail*0;//1;//2;
            y2=y2-1-consfail*0;//1;//2;
          }
          if(Monoparticular){
          //Si on suis une supposition
          if(followSimState==false){//On ne simule pas d'estimation.
            //EstimeLZA centré sur l'estimation du robot et la fenetre assoiee.
            lzamod.set_Fenetre(x1,y1,x2,y2);//forcage de la fenetre autour de la position odométrique.
            etat_robot.x_pos=odopos.pos.x;
            etat_robot.y_pos=odopos.pos.y;
            etat_robot.theta_pos=odopos.dir.theta;
            loclza=lzamod.Localise(datas_loc,4.0,etat_robot);
            if(presentMode==LZA_INIT_COMPASMODE){
                loclza.theta=LZACompas;
            }
            if(isLZAResult(loclza,odopos,Ttol,Localised_once,Val_Recalage)){
              LZA_estime.pos.x=loclza.x;
              LZA_estime.pos.y=loclza.y;
              LZA_estime.dir.theta=loclza.theta;
              new_estime_flag=true;
            }

            if (isLZAResult(loclza,odopos,Ttol,Localised_once,Val_Recalage)){//Si LZA nous donne un resultat valide.
                //initialisation de la position a partir de la quelle on simule a partir de la nouvelle position.
                Nloc=1;
                followSimState=true;
                //if(fabs(odopos.dir.theta-loclza.theta)>(Ttol)){
                //if((fabs(odopos.dir.theta-loclza.theta)>(Ttol)&&fabs(odopos.dir.theta-loclza.theta)<M_PI)||
              //(((2*M_PI)-fabs(odopos.dir.theta-loclza.theta))>(Ttol)&&fabs(odopos.dir.theta-loclza.theta)>M_PI)){
                  //oldOdosim.dir.theta=odopos.dir.theta;
                //}else{
                oldOdosim.dir.theta=loclza.theta;
                //}
                oldOdosim.pos.x=loclza.x;
                oldOdosim.pos.y=loclza.y;
                LastLoc=oldOdosim;
                PSim=P;
              }else{
                Nloc=0;
                Fail_Count=0;
                followSimState=false;
                Loc_requis=true;
              }
          }else{//On a une position a simuler.
            //Calcule fenêtre avec Erreur Odo simulee
            compute_Fenetre_Error_Odo(oldOdosim,distLeft,distRight,PSim,x1sim,x2sim,y1sim,y2sim,t1,t2);
            SimuOdo(oldOdosim,OdoposSim, distLeft, distRight);
            oldOdosim=OdoposSim;
            //EstimeLZA centré sur l'estimation de position.
            lzamod.set_Fenetre(x1sim,y1sim,x2sim,y2sim);//forcage de la fenetre autour de la position odométrique.
            etat_robot.x_pos=OdoposSim.pos.x;
            etat_robot.y_pos=OdoposSim.pos.y;
            etat_robot.theta_pos=OdoposSim.dir.theta;
            //EstimeLZA centré sur l'estimation de position.
            loclza=lzamod.Localise(datas_loc,4.0,etat_robot);
            if(presentMode==LZA_INIT_COMPASMODE){
                loclza.theta=LZACompas;
            }
            //clock_gettime(CLOCK_REALTIME, &TimeEstime);
            if(isLZAResult(loclza,OdoposSim,Ttol,Localised_once,Val_Recalage)){//L'erreur odometrique simulee permet tout de même d'avoire un resultat.
              LZA_estime.pos.x=loclza.x;
              LZA_estime.pos.y=loclza.y;
              LZA_estime.dir.theta=loclza.theta;
              new_estime_flag=true;

              //increment validation simulation.
              Nloc++;
            }else{//L'erreur odometrique ne permet pas de valider un resultat, reset de la simulation.
              lzamod.set_Fenetre(x1,y1,x2,y2);//forcage de la fenetre autour de la position odométrique.
              etat_robot.x_pos=odopos.pos.x;
              etat_robot.y_pos=odopos.pos.y;
              etat_robot.theta_pos=odopos.dir.theta;
              loclza=lzamod.Localise(datas_loc,4.0,etat_robot);
              if(presentMode==LZA_INIT_COMPASMODE){
                  loclza.theta=LZACompas;
              }
              //clock_gettime(CLOCK_REALTIME, &TimeEstime);
              if(isLZAResult(loclza,odopos,Ttol,Localised_once,Val_Recalage)){
                LZA_estime.pos.x=loclza.x;
                LZA_estime.pos.y=loclza.y;
                LZA_estime.dir.theta=loclza.theta;
                new_estime_flag=true;
                //Reset Simu
                Nloc=1;
                followSimState=true;
                //if(fabs(odopos.dir.theta-loclza.theta)>(Ttol)){

                //if((fabs(odopos.dir.theta-loclza.theta)>(Ttol)&&fabs(odopos.dir.theta-loclza.theta)<M_PI)||
                  //(((2*M_PI)-fabs(odopos.dir.theta-loclza.theta))>(Ttol)&&fabs(odopos.dir.theta-loclza.theta)>M_PI)){
                  //oldOdosim.dir.theta=odopos.dir.theta;
                //}else{
                oldOdosim.dir.theta=loclza.theta;
                //}
                oldOdosim.pos.x=loclza.x;
                oldOdosim.pos.y=loclza.y;
                EstimeInit=LZA_estime;
                PSim=P;
              }else{//Simu en cours : Simu Echec : localisation Echec
                if(Fail_Count<Failtol){
                  Fail_Count++;
                }else{
                  //Echec de localisation : perdu
                  Fail_Count=0;
                  consfail++;
                  Nloc=0;
                  followSimState=false;
                  Loc_requis=true;
                }
              }
            }
          }
        }else{///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          lzamod.set_Fenetre(x1,y1,x2,y2);//forcage de la fenetre autour de la position odométrique.
          etat_robot.x_pos=odopos.pos.x;
          etat_robot.y_pos=odopos.pos.y;
          etat_robot.theta_pos=odopos.dir.theta;
          loclza=lzamod.Localise(datas_loc,4.0,etat_robot);

          if(isLZAResult(loclza,odopos,Ttol,Localised_once,Val_Recalage)&&((presentMode!=LZA_INIT_COMPASMODE && presentMode!=LZA_FUS_INIT_COMPASMODE)||fabs(LZACompas)<=2*M_PI)){
            if(!followSimState){
                Nloc=Nloc+1;
            }else{
              Nloc=1;
              followSimState=true;
              Fail_Count=0;
            }
          }else{
            if(Fail_Count<Failtol){
              Fail_Count++;
            }else{
              Nloc=0;
              Fail_Count=0;
              followSimState=false;
              Loc_requis=true;
              Fail_Count=0;
              consfail++;
            }
          }
        }
          //OdoPosOld=odoposP3D;
          //Toldodo=Todo;
          //HeatedPos = NewHeatedPos;

          //Si les contition de relocalisation sont remplies:
          if((Loc_requis && Nloc>=Repet_Lost)||(!Loc_requis && Nloc>=Repet_maintain)){
            Localised_once=true;
            if(Monoparticular){
            //Set estimations
            Val_Recalage=OdoposSim;
            TimeLocOld=TimeEstime;
            LastLoc=EstimeInit;
            Reloc_Flag=1;
            //Reset P
            P=PSim;
            //Set offset.
            HeatedPos=Val_Recalage;
            Toldodo=TimeEstime;
            Loc_requis=false;
            consfail=0;
            Nloc=0;
            Fail_Count=0;
            Heated=true;
          }else{
            if((presentMode==LZA_INIT_COMPASMODE || presentMode==LZA_FUS_INIT_COMPASMODE) && fabs(LZACompas)<2*M_PI){
              loclza.theta=LZACompas;
            }
            HeatedPos.pos.x=loclza.x;
            HeatedPos.pos.y=loclza.y;
            HeatedPos.dir.theta=loclza.theta;

            HeatedPos.pos.x=HeatedPos.pos.x+0.2*cos(HeatedPos.dir.theta);
            HeatedPos.pos.y=HeatedPos.pos.y+0.2*sin(HeatedPos.dir.theta);

            Val_Recalage=HeatedPos;
            LastLoc=HeatedPos;
            Toldodo=TimeEstime;
            P=PLZA;
            Reloc_Flag=1;
            Loc_requis=false;
            Nloc=0;
            consfail=0;
            Fail_Count=0;
            Heated=true;
          }
          }
          if(presentMode==LZA_FUS_INIT_COMPASMODE||presentMode==LZA_FUS_INITMODE){
          //Intersection des intervals entre la position odométrique reconstituée de l'incertitude de LZA
          }
        }
        clock_gettime(CLOCK_REALTIME, &TimeLZA);
        break;
      case LZA_BACKGROUNDMODE:
        //en Background : datas_loc on enregistre simplement les lasers observés
        las2.Get(relevel2);
        las1.Get(relevel1);
        Format_LAS(relevel1, relevel2,datas_loc);
        Todo=odom.Get(OdoPosOld);
        P << pow(0.8,2),0,0,0,pow(0.5,2),0,0,0,pow(0.2062,2);//Non actif on ne sait donc pas bien ou on se trouve.
        Val_Recalage.pos.x=999;
        Val_Recalage.pos.y=999;
        Val_Recalage.pos.z=0;
        Val_Recalage.dir.phi=0;
        Val_Recalage.dir.psy=0;
        Val_Recalage.dir.theta=999;
        LastLoc=Val_Recalage;
        Nloc=0;
        consfail=0;
        Fail_Count=0;
        followSimState=false;
        Localised_once=false;
        Fail_Count=0;
        logneed=true;
        break;
      case LZA_FUS_RUNING_COMMPASMODE:
      case LZA_RUNING_COMMPASMODE:
        LZACompas=EstimeCompas;
      case LZA_FUS_RUNINGMODE:
      case LZA_RUNINGMODE:////Confirmation par Coherence avec une simulation odométrique. Working
        if (verbose){
          std::cout<<"LZA_T==============================================================="<<std::endl;
        }
        if(Heated){//premier cycle de sortie LZA sauté : relocalisation robot avec
        /*  if(presentMode==LZA_RUNING_COMMPASMODE && LastCompas!=999){
            HeatedPos.dir.theta=LastCompas;
          }/**/
          OdoPosOld=HeatedPos;
          RecalageLoc.Set(HeatedPos, Toldodo);
          Total_Reloc++;
          Toldodo=StartTime;
          Heated=false;
          break;
        }
        //Recupération LAS-Odometrie.
        las2.Get(relevel2);
        las1.Get(relevel1);
        Todo=odom.Get(odopos);
        clock_gettime(CLOCK_REALTIME, &TimeEstime);
        //SI L'odometrie est postérieur a la dernière estimation on continu.
        if(isafter(Todo,Toldodo)){
          logneed=true;

          Format_LAS(relevel1, relevel2,datas_loc);
          clock_gettime(CLOCK_REALTIME, &TimeFormat);

          if (!isLZAusable(odoposP3D)){
            LZArun=0;
            break;
          }
          LZArun=1;
          //Estimation OdoRob.
          Recomput_Weeldist(OdoPosOld,odopos,distLeft,distRight);
          //Calcule fenêtre avec Erreur Odo
          compute_Fenetre_Error_Odo(OdoPosOld,distLeft,distRight,P,x1,x2,y1,y2,t1,t2);

          Dsleeped=sqrt(pow((Val_Recalage.pos.x-odopos.pos.x),2)+pow((Val_Recalage.pos.y-odopos.pos.y),2));
          if (Dsleeped<dist_reobserve_m){//On c'est relocalisé il y a peut on ne fais rien.
            SleepReloc_Flag=1;
            OdoPosOld=odopos;
            Toldodo=Todo;
            break;
          }/**/
          SleepReloc_Flag=0;
          std::cout << "LZA--Sleeped" << '\n';

          if(Localised_once){
            drecalage=sqrt(pow((LastLoc.pos.x-odopos.pos.x),2)+pow((LastLoc.pos.y-odopos.pos.y),2));
            if(drecalage!=0){
              thetad=atan2(odopos.pos.y-LastLoc.pos.y,odopos.pos.x-LastLoc.pos.x);
              Ix=Iadd(Isub(initInterval(LastLoc.pos.x,LastLoc.pos.x),
                          Ixinit),
                      Imul(Icos(Isub(initInterval(thetad,thetad),
                                Itinit)),
                          initInterval(drecalage,drecalage)));
              Iy=Iadd(Isub(initInterval(LastLoc.pos.y,LastLoc.pos.y),
                          Iyinit),
                      Imul(Icos(Isub(initInterval(thetad,thetad),
                                      Itinit)),
                          initInterval(drecalage,drecalage)));
            }else{
              Ix=Isub(initInterval(LastLoc.pos.x,LastLoc.pos.x),Ixinit);
              Iy=Isub(initInterval(LastLoc.pos.y,LastLoc.pos.y),Iyinit);
            }
            Ix=Iadd(Icentering(initInterval(x1,x2)),Ix);
            Iy=Iadd(Icentering(initInterval(y1,y2)),Iy);
            IVals(Ix,x2,x1);
            IVals(Iy,y2,y1);
          }else{
            x1=x1+1+consfail*0;//1;//2;
            x2=x2-1-consfail*0;//1;//2;
            y1=y1+1+consfail*0;//1;//2;
            y2=y2-1-consfail*0;//1;//2;
          }

          if(x1>500){
            std::cout<<"C'est quoi ce bronx la fenêtre!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<'\n';
            std::cout<<"Distance depuis le recalage = "<<drecalage<<" : Dtheta = "<<thetad<<'\n';
            std::cout<<"X = ["<<x2<<","<<x1<<"] : Y = ["<<y2<<","<<y1<<"]"<<'\n';
            std::cout<<"LastLoc X = "<<LastLoc.pos.x<<": Y = "<<LastLoc.pos.y<<'\n';
            std::cout<<"odopos X = "<<odopos.pos.x<<": Y = "<<odopos.pos.y<<'\n';
          }
          if(Monoparticular){
          //Si on suis une supposition
          if(followSimState==false){//On ne simule pas d'estimation.
            //EstimeLZA centré sur l'estimation du robot et la fenetre assoiee.
            lzamod.set_Fenetre(x1,y1,x2,y2);//forcage de la fenetre autour de la position odométrique.
            etat_robot.x_pos=odopos.pos.x;
            etat_robot.y_pos=odopos.pos.y;
            etat_robot.theta_pos=odopos.dir.theta;
            loclza=lzamod.Localise(datas_loc,4.0,etat_robot);
            std::cout << "LZA--LOCALISE" << '\n';
            if(presentMode==LZA_RUNING_COMMPASMODE || presentMode==LZA_FUS_RUNING_COMMPASMODE){
                loclza.theta=LZACompas;
            }
            //clock_gettime(CLOCK_REALTIME, &TimeEstime);
            if(isLZAResult(loclza,odopos,Ttol,Localised_once,Val_Recalage)){
              LZA_estime.pos.x=loclza.x;
              LZA_estime.pos.y=loclza.y;
              LZA_estime.dir.theta=loclza.theta;
              new_estime_flag=true;
              std::cout << "LZA--RES" << '\n';
            }
              if (isLZAResult(loclza,odopos,Ttol,Localised_once,Val_Recalage)){//Si LZA nous donne un resultat valide.
              //initialisation de la position a partir de la quelle on simule a partir de la nouvelle position.
              std::cout << "LZA--FIRSTLOC" << '\n';
              Nloc=1;
              followSimState=true;
              //if(fabs(odopos.dir.theta-loclza.theta)>(Ttol)){
              //if((fabs(odopos.dir.theta-loclza.theta)>(Ttol)&&fabs(odopos.dir.theta-loclza.theta)<M_PI)||
                //(((2*M_PI)-fabs(odopos.dir.theta-loclza.theta))>(Ttol)&&fabs(odopos.dir.theta-loclza.theta)>M_PI)){
                //oldOdosim.dir.theta=odopos.dir.theta;
              //}else{/**/
                oldOdosim.dir.theta=loclza.theta;
              //}/**/
              oldOdosim.pos.x=loclza.x;
              oldOdosim.pos.y=loclza.y;
              LastLoc=oldOdosim;
              PSim=P;
              }else{
                Nloc=0;
                consfail++;
                followSimState=false;
                Fail_Count=0;
                Loc_requis=true;
                std::cout << "LZA--Not a result :" << '\n';
                std::cout << "LZA--Estim X :" << loclza.x <<" - Y : "<< loclza.y <<" - Thet : "<< loclza.theta << '\n';
                std::cout << "LZA--Odo X :" << odopos.pos.x <<" - Y : "<< odopos.pos.y <<" - Thet : "<< odopos.dir.theta << '\n';
                std::cout << "LZA--ttol = "<< Ttol << '\n';
              }
          }else{//On a une position a simuler.
            std::cout << "LZA--SIMU" << '\n';
            //Calcule fenêtre avec Erreur Odo simulee
            compute_Fenetre_Error_Odo(oldOdosim,distLeft,distRight,PSim,x1sim,x2sim,y1sim,y2sim,t1,t2);
            SimuOdo(oldOdosim,OdoposSim, distLeft, distRight);
            oldOdosim=OdoposSim;
            //EstimeLZA centré sur l'estimation de position.
            lzamod.set_Fenetre(x1sim,y1sim,x2sim,y2sim);//forcage de la fenetre autour de la position odométrique.
            etat_robot.x_pos=OdoposSim.pos.x;
            etat_robot.y_pos=OdoposSim.pos.y;
            etat_robot.theta_pos=OdoposSim.dir.theta;
            //EstimeLZA centré sur l'estimation de position.
            loclza=lzamod.Localise(datas_loc,4.0,etat_robot);
            if(presentMode==LZA_RUNING_COMMPASMODE){
                loclza.theta=LZACompas;
            }
            //clock_gettime(CLOCK_REALTIME, &TimeEstime);
            if(isLZAResult(loclza,OdoposSim,Ttol,Localised_once,Val_Recalage)){//L'erreur odometrique simulee permet tout de même d'avoire un resultat.
              LZA_estime.pos.x=loclza.x;
              LZA_estime.pos.y=loclza.y;
              LZA_estime.dir.theta=loclza.theta;
              new_estime_flag=true;

              //increment validation simulation.
              Nloc++;
            }else{//L'erreur odometrique ne permet pas de valider un resultat, reset de la simulation.
              lzamod.set_Fenetre(x1,y1,x2,y2);//forcage de la fenetre autour de la position odométrique.
              etat_robot.x_pos=odopos.pos.x;
              etat_robot.y_pos=odopos.pos.y;
              etat_robot.theta_pos=odopos.dir.theta;
              loclza=lzamod.Localise(datas_loc,4.0,etat_robot);
              if(presentMode==LZA_RUNING_COMMPASMODE){
                  loclza.theta=LZACompas;
              }
              //clock_gettime(CLOCK_REALTIME, &TimeEstime);
              if(isLZAResult(loclza,odopos,Ttol,Localised_once,Val_Recalage)){
                LZA_estime.pos.x=loclza.x;
                LZA_estime.pos.y=loclza.y;
                LZA_estime.dir.theta=loclza.theta;
                new_estime_flag=true;
                //Reset Simu
                Nloc=1;
                followSimState=true;
                //if(fabs(odopos.dir.theta-loclza.theta)>(Ttol)){
                //if((fabs(odopos.dir.theta-loclza.theta)>(Ttol)&&fabs(odopos.dir.theta-loclza.theta)<M_PI)||
                  //(((2*M_PI)-fabs(odopos.dir.theta-loclza.theta))>(Ttol)&&fabs(odopos.dir.theta-loclza.theta)>M_PI)){
                  //oldOdosim.dir.theta=odopos.dir.theta;
                //}else{
                  oldOdosim.dir.theta=loclza.theta;
                //}
                oldOdosim.pos.x=loclza.x;
                oldOdosim.pos.y=loclza.y;
                LastLoc=oldOdosim;
                PSim=P;
              }else{//Simu en cours : Simu Echec : localisation Echec
                if(Fail_Count<Failtol){
                  Fail_Count++;
                }else{
                  //Echec de localisation : perdu
                  Nloc=0;
                  consfail++;
                  Fail_Count=0;
                  followSimState=false;
                  Loc_requis=true;
                }
              }
            }
          }
        }else{///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
          lzamod.set_Fenetre(x1,y1,x2,y2);//forcage de la fenetre autour de la position odométrique.
          etat_robot.x_pos=odopos.pos.x;
          etat_robot.y_pos=odopos.pos.y;
          etat_robot.theta_pos=odopos.dir.theta;
          loclza=lzamod.Localise(datas_loc,4.0,etat_robot);

          if(isLZAResult(loclza,odopos,Ttol,Localised_once,Val_Recalage)&&((presentMode!=LZA_RUNING_COMMPASMODE && presentMode!=LZA_FUS_RUNING_COMMPASMODE)||fabs(LZACompas)<=4*M_PI)){
            if(!followSimState){
                Nloc=Nloc+1;
            }else{
              Nloc=1;
              followSimState=true;
              Fail_Count=0;
            }
          }else{
            if(Fail_Count<Failtol){
              Fail_Count++;
            }else{
              consfail++;
              Nloc=0;
              Fail_Count=0;
              followSimState=false;
              Loc_requis=true;
            }
          }
        }

          OdoPosOld=odopos;
          Toldodo=Todo;

          //Si les contition de relocalisation sont remplies:
          if((Loc_requis && Nloc>=Repet_Lost)||(!Loc_requis && Nloc>=Repet_maintain)){
            Localised_once=true;
            if(Monoparticular){
            //Set estimations
            Val_Recalage=OdoposSim;
            /*if(presentMode==LZA_RUNING_COMMPASMODE && LastCompas!=999){
              Val_Recalage.dir.theta=LastCompas;
            }/**/
            //RecalageLoc.Set(Val_Recalage, TimeEstime);
            TimeLocOld=TimeEstime;
            Reloc_Flag=1;
            LastLoc=EstimeInit;
            //Reset P
            P=PSim;
            //Set offset.
            OdoPosOld=Val_Recalage;
            Toldodo=TimeEstime;
            Loc_requis=false;
            Nloc=0;
            consfail=0;
            Fail_Count=0;
            RecalageLoc.Set(Val_Recalage, TimeEstime);
          }else{
            if((presentMode==LZA_RUNING_COMMPASMODE || presentMode==LZA_FUS_RUNING_COMMPASMODE) && fabs(LZACompas)<2*M_PI){
              loclza.theta=LZACompas;
            }
            Val_Recalage.pos.x=loclza.x;
            Val_Recalage.pos.y=loclza.y;
            Val_Recalage.dir.theta=loclza.theta;
            //Intersection des intervals entre la position odométrique reconstituée de l'incertitude de LZA
            //(double Iodo[6], int Mode, euclid_position LZARes,euclid_position OdoEstime,euclid_position &RelocVal)
            IOdo[0]=x2;
            IOdo[1]=x1;
            IOdo[2]=y2;
            IOdo[3]=y1;
            IOdo[4]=0;
            IOdo[5]=0;
            if(FusionLZA(IOdo,presentMode,Val_Recalage,odopos,Val_Recalage)){
              //RecalageLoc.Set(Val_Recalage, TimeEstime);
              //Ajou du décalage de 20cm en XS constaté dans le modèle d'erreur.
              Val_Recalage.pos.x=Val_Recalage.pos.x+0.2*cos(Val_Recalage.dir.theta);
              Val_Recalage.pos.y=Val_Recalage.pos.y+0.2*sin(Val_Recalage.dir.theta);
              Toldodo=TimeEstime;
              LastLoc=Val_Recalage;
              P=PLZA;
              Reloc_Flag=1;
              Loc_requis=false;
              Nloc=0;
              consfail=0;
              Fail_Count=0;
              OdoPosOld=Val_Recalage;
              Toldodo=TimeEstime;
              RecalageLoc.Set(Val_Recalage, TimeEstime);
              Total_Reloc++;
            }else{//on enlève le point de localisation qui avais permit le recalage et on ajoute simplement un echec.
              Nloc--;
              Fail_Count++;
            }
          }
          std::cout << "Recalage : X=" << Val_Recalage.pos.x << " : Y=" << Val_Recalage.pos.y << " : Theta=" << Val_Recalage.dir.theta << '\n';
          }
        }
        clock_gettime(CLOCK_REALTIME, &TimeLZA);
        break;
      default:
        break;
    }
    //Logging
    if(!(presentMode==LZA_IDLEMODE && presentModeCompas==COMPAS_IDLEMODE) && logneed && Logging){
      clock_gettime(CLOCK_REALTIME, &TimeLog);
      /*
      //Odopose
      Logs.push_back(OdoPosOld.pos.x);//--1
      Logs.push_back(OdoPosOld.pos.y);
      Logs.push_back(OdoPosOld.dir.theta);
      //Estimation LZA
      Logs.push_back(loclza.x);
      Logs.push_back(loclza.y);
      Logs.push_back(loclza.theta);
      //Fenetre
      Logs.push_back(x1);
      Logs.push_back(x2);
      Logs.push_back(y1);
      Logs.push_back(y2);
      //longueur fenetre --10
      Logs.push_back(abs(x1-x2));
      Logs.push_back(abs(y1-y2));
      //Nombre de localisation sucessive.
      Logs.push_back(Nloc);
      //Recalage
      Logs.push_back(Reloc_Flag);
      Logs.push_back(distLeft);
      Logs.push_back(distRight);

      //Odopose
      Logs.push_back(OdoposSim.pos.x);
      Logs.push_back(OdoposSim.pos.y);
      Logs.push_back(OdoposSim.dir.theta);
      //Fenetre -- 19
      Logs.push_back(x1sim);
      Logs.push_back(x2sim);
      Logs.push_back(y1sim);
      Logs.push_back(y2sim);
      //longueur fenetre
      Logs.push_back(abs(x1sim-x2sim));
      Logs.push_back(abs(y1sim-y2sim));
      //Recalage --25
      Logs.push_back(SleepReloc_Flag);

      Logs.push_back(Val_Recalage.pos.x);
      Logs.push_back(Val_Recalage.pos.y);
      Logs.push_back(Val_Recalage.dir.theta);
      Logs.push_back(Repet_Lost);
      Logs.push_back(Repet_maintain);
      if(Loc_requis){
        Logs.push_back(1);
      }else{
        Logs.push_back(0);
      }
      Logs.push_back(Nlocoffset); //--33
      if(followSimStateOld){
        Logs.push_back(1);
      }else{
        Logs.push_back(0);
      }
      if(followSimState){
        Logs.push_back(1);
      }else{
        Logs.push_back(0);
      }*/
      //////////////////////////////////////////////////////////////////////////////
      savefile<<(StartTime.tv_nsec+(StartTime.tv_sec*1000000000))<<","<<presentMode;
      savefile<<","<<(TimeFormat.tv_nsec+(TimeFormat.tv_sec*1000000000));
      savefile<<","<<(TimeLog.tv_nsec+(TimeLog.tv_sec*1000000000));
      savefile<<","<<OdoPosOld.pos.x;
      savefile<<","<<OdoPosOld.pos.y;
      savefile<<","<<OdoPosOld.dir.theta;
      savefile<<","<<(TimeLZA.tv_nsec+(TimeLZA.tv_sec*1000000000));
      savefile<<","<<LZArun;/////////////////////////////////////////////////////Ajout
      savefile<<","<<loclza.x;
      savefile<<","<<loclza.y;
      savefile<<","<<loclza.theta;
      savefile<<","<<x1;
      savefile<<","<<x2;//10
      savefile<<","<<y1;
      savefile<<","<<y2;
      savefile<<","<<fabs(x1-x2);
      savefile<<","<<fabs(y1-y2);
      savefile<<","<<Nloc;//15
      savefile<<","<<Fail_Count;
      savefile<<","<<Reloc_Flag;
      savefile<<","<<distLeft;
      savefile<<","<<distRight;
      savefile<<","<<OdoposSim.pos.x;//20
      savefile<<","<<OdoposSim.pos.y;
      savefile<<","<<OdoposSim.dir.theta;
      savefile<<","<<x1sim;
      savefile<<","<<x2sim;
      savefile<<","<<y1sim;//25
      savefile<<","<<y2sim;
      savefile<<","<<fabs(x1sim-x2sim);
      savefile<<","<<fabs(y1sim-y2sim);
      savefile<<","<<SleepReloc_Flag;
      savefile<<","<<Val_Recalage.pos.x;//30
      savefile<<","<<Val_Recalage.pos.y;
      savefile<<","<<Val_Recalage.dir.theta;
      if(Loc_requis){
        savefile<<","<<1;
      }else{
        savefile<<","<<0;
      }
      savefile<<","<<Nlocoffset; //--34
      if(followSimStateOld){//35
        savefile<<","<<1;
      }else{
        savefile<<","<<0;
      }
      if(followSimState){//36
        savefile<<","<<1;
      }else{
        savefile<<","<<0;
      }
      savefile<<","<<(TimeCompas.tv_nsec+(TimeCompas.tv_sec*1000000000));
      savefile<<","<<presentModeCompas;
      savefile<<","<<EstimeCompas;
      savefile<<","<<LastCompas;
      savefile<<","<<(TimeEstime.tv_nsec+(TimeEstime.tv_sec*1000000000));
      if(Heated){//41
        savefile<<","<<1;
      }else{
        savefile<<","<<0;
      }
      savefile<<","<<YRT;
      savefile<<"\n";

      logline=35;
      for (int j = 0; j < 57; j++) {
        LZA_LAS.push_back(datas_loc[j]);
      }
      LogTime.push_back(StartTime);
      LogMode.push_back(presentMode);
      if(new_estime_flag){//New LZA estimation for Kalman
        LZA_res.Set(LZA_estime,Time);
        new_estime_flag=false;
      }
    }

      //usleep(delayusleep(StartTime, 10));
      lsleep=delayusleep(StartTime, 100);
      if(lsleep<0){
        lsleep=0;
      }
      usleep(lsleep);
  }
  std::cout << "LZA_T : Sortie de la boucle" << '\n';
  if(Logging){
    /*std::string filename = PID_PATH("+PANORAMA_Log/LZA_LOG_t_"+std::to_string(LauncheTime.tv_sec)+".txt");
    std::ofstream savefile;
    savefile.open(filename,std::ios::out);
    std::cout << "LZA_T : logging LZA in "<<filename << '\n';
    for(int i=0; i<LogTime.size(); i++){
      savefile<<(LogTime[i].tv_nsec+(LogTime[i].tv_sec*1000000000))<<","<<LogMode[i];
      for(int j=0; j<logline; j++){
        savefile<<","<<Logs[i*logline+j];
      }
      savefile<<"\n";
    }*/
    savefile.close();

    std::string filenamelas = PID_PATH("+PANORAMA_Log/"+std::to_string(LauncheTime.tv_sec)+"/LZA_LAS_LOG.txt");
    FILE * savefile2=fopen(filenamelas.c_str(),"w");
    if (savefile2==NULL)
    {
      printf("Echec de l'ouverture du fichier de sauvegarde : %s\n",filenamelas.c_str());
      return;
    }

    std::cout << "LZA_T : logging LASin "<<filenamelas  << '\n';

    for(int i=0; i<LogTime.size(); i++){
      fprintf(savefile2,"%ld",(LogTime[i].tv_nsec+(LogTime[i].tv_sec*1000000000)));
      for(int j=0; j<57; j++){
        fprintf(savefile2,",%f",LZA_LAS[i*57+j]);
      }
      fprintf(savefile2,"\n");
    }
    fclose (savefile2);
  }

  std::cout << "LZA_T : Nombre de relocalisationtotal :"<< Total_Reloc<< '\n';
  std::cout << "LZA_T : END" << '\n';
}

void Format_LAS(laser_4m_record Las1, laser_4m_record Las2, double DIST[]){
  double datas_loc[57];
  double dist_las_robot_all[1024];
  double ang_las_robot_all[1024];

//  Telemetries=format_Telemetre(Las1.data,Las1.last_lenght,0.03,0,0);
//  Tmp_telemetries=format_Telemetre(Las2.data,Las2.last_lenght,0.03,0,M_PI);
//  Telemetries.insert(Telemetries.end(),Tmp_telemetries.begin(),Tmp_telemetries.end());

  for(int i=0;i<1024;i++)
  {
    if(i>=0 && i<256)//de -PI à -PI/2
    {
      dist_las_robot_all[i] = compute_value(Las2,340+i, 4.095);
      ang_las_robot_all[i] = Las2.data[340+i].dir.theta-M_PI;//i*(2*M_PI/1024); //
    }

    if(i>=256 && i<768)//de -PI/2 a +PI/2
    {
      dist_las_robot_all[i] = compute_value(Las1,84+i-256, 4.095);
      ang_las_robot_all[i] = Las1.data[84+i-256].dir.theta;//i*(2*M_PI/1024);  // ? voir comment c'est consid?r? dans la localisation
    }
    if(i>=768 && i<1024)
    {
      dist_las_robot_all[i] = compute_value(Las2, 84+i-768, 4.095);
      ang_las_robot_all[i] = Las2.data[84+i-768].dir.theta+M_PI;//-2*M_PI + i*(2*M_PI/1024);  // ? voir comment c'est consid?r? dans la localisation
    }
  }

//  std::cout << "Angle laser selectionnee = \n[";
  for(int j=0;j<57;j++){
      DIST[j]= dist_las_robot_all[j*18];
//      std::cout << ang_las_robot_all[j*18] << ' ';
      if (DIST[j] <= 0.06 || DIST[j] > 4)//4.095)
      {
        DIST[j] = 4;//.095;//portee max des lasers
      }
    }
//    std::cout << "]\n";
}

bool isLZAResult(localization::ROBLOC LZAres,euclid_position Odosource, double tetTol, bool Trusted,euclid_position LZArecal){
//  return !((LZAres.x==999 || LZAres.y==999 || LZAres.theta > 50)&&
//  (fabs(LZAres.theta-Odosource.dir.theta)<tetTol||fabs(LZAres.theta-Odosource.dir.theta)>(2*M_PI-tetTol)));
  if(LZAres.x==999){
    return false;
  }
  /*
  //Tentative de détection de recule trop important.
  double Ares=atan2(LZAres.y-Odosource.pos.y,LZAres.x-Odosource.pos.x);
  while(Ares>M_PI){
    Ares=Ares-2*M_PI;
  }
  while(Ares<=-1*M_PI){
    Ares=Ares+2*M_PI;
  }
  Ares=abs(Ares-Odosource.dir.theta);
  if(Ares>M_PI){
    Ares=(2*M_PI)-Ares;
  }
  if(sqrt(pow(LZAres.x-Odosource.pos.x,2)+pow(LZAres.y-Odosource.pos.y,2))>0.5 && Ares>7*M_PI/12){
    return false;
  }*/
  if(fabs(LZAres.theta-Odosource.dir.theta)<tetTol){
    return true;
  }
  if(fabs(2*M_PI-fabs(LZAres.theta-Odosource.dir.theta))<tetTol){
    return true;
  }
  return false;//!Trusted;//Si perdu on valide quand même si non on se méfie.
}

localization::ROBLOC MultiresLZA(localization::LZALocalization lzamod, double Las[57], double dist, P3D_Robot_State Rob_State){
  localization::ROBLOC locLZA;
  locLZA.x=999;
  locLZA.y==999;
  locLZA.theta=999;
/*  std::vector<LZACorel> Perf;
  LZACorel Perftmp;
  std::vector<localization::ROBLOC> vResLZA;
  std::vector<double> vResLZACorels;
  lzamod.ListLocalise(Las,dist,Rob_State,vResLZA,vResLZACorels);
  for(int j=0;j<vResLZACorels.size();++j){
    Perftmp.pos=vResLZA[j];
    Perftmp.corel=vResLZACorels[j];
    Perf.push_back(Perftmp);
  }
  sort(Perf.begin(),Perf.end(),CompLZA);
  for(int i=Perf.size()-1;i>=0;i--){
    if(fabs(Perf[i].pos.theta-Rob_State.theta_pos)<0.9*M_PI/2){
      locLZA.x=Perf[i].pos.x;
      locLZA.y=Perf[i].pos.y;
      locLZA.theta=Perf[i].pos.theta;
      break;
    }
  }*/
  return locLZA;
}

bool FusionLZA(double Iodo[6], int Mode, euclid_position LZARes,euclid_position OdoEstime,euclid_position &RelocVal){
  std::vector<double> ixLZA,iyLZA,itLZA,ixOrient,iyOrient,itOrient,ixInter,iyInter,itInter;
//  if(Mode==LZA_INITMODE || Mode==LZA_RUNINGMODE || Mode==LZA_INIT_COMPASMODE || Mode==LZA_RUNING_COMMPASMODE){
    RelocVal=LZARes;
    return true;
//  }
  if(Mode==LZA_FUS_INITMODE || Mode==LZA_FUS_RUNINGMODE ){
    ixLZA.push_back(-1.7474);
    ixLZA.push_back(0.90218);
    iyLZA.push_back(-0.28856);
    iyLZA.push_back(0.58067);
    itLZA.push_back(-0.13833);
    itLZA.push_back(0.19347);
  }else{
    if(Mode==LZA_FUS_INIT_COMPASMODE || Mode==LZA_FUS_RUNING_COMMPASMODE){
      ixLZA.push_back(-1.2913);
      ixLZA.push_back(0.10229);
      iyLZA.push_back(-0.26734);
      iyLZA.push_back(0.33004);
      itLZA.push_back(-0.12905);
      itLZA.push_back(0.10479);
    }
  }
  double ZonesMAP[][5]={{25,35,4,60,M_PI/2},{-5,24,-1,3,0},{24,35,-1,4,0}};
  //Si on ignore comment orienté l'erreur : On considéreur une erreur carré la plus grande possible
  ixOrient=Imul(initInterval(-1,1),ixLZA);
  iyOrient=Imul(initInterval(-1,1),iyLZA);
  ixOrient=Iunion(ixOrient,iyOrient);
  iyOrient=ixOrient;
  itOrient=Imul(initInterval(-1,1),itLZA);
  int Izone=-1;
  for(int i=0;i<3;i++){
    if(OdoEstime.pos.x>ZonesMAP[i][0]&&OdoEstime.pos.x<ZonesMAP[i][1]&&OdoEstime.pos.y>ZonesMAP[i][2]&&OdoEstime.pos.y<ZonesMAP[i][3]){
      Izone=i;
      break;
    }
  }
  switch (Izone) {
    case 0://Couloir 1
      if(fabs(ZonesMAP[Izone][4]-OdoEstime.dir.theta)<4*M_PI/12){//Retour
//        RoboDir=1;
        iyOrient=ixLZA;//L'arrière du robot est en Y negatif
        ixOrient=Imul(initInterval(-1,-1),iyLZA);
        itOrient=itLZA;
      }
      if(fabs(ZonesMAP[Izone][4]-OdoEstime.dir.theta)>8*M_PI/12){//Aller
//        RoboDir=2;
          iyOrient=Imul(initInterval(-1,-1),ixLZA); //L'arière du robot et en Y Positif
          ixOrient=iyLZA;
          itOrient=itLZA;
      }
    break;
    case 1://Couloir 2
      if(fabs(ZonesMAP[Izone][4]-OdoEstime.dir.theta)<4*M_PI/12){//Retour
//        RoboDir=1;
        iyOrient=iyLZA;//Condition d'établissement du model
        ixOrient=ixLZA;
        itOrient=itLZA;
      }
      if(fabs(ZonesMAP[Izone][4]-OdoEstime.dir.theta)>8*M_PI/12){//Aller
//        RoboDir=2;
        iyOrient=Imul(initInterval(-1,-1),iyLZA);//Le robot se déplace en sens invers de la conditon de création du modèle.
        ixOrient=Imul(initInterval(-1,-1),ixLZA);
        itOrient=itLZA;
      }
    break;
    default:
    break;
  }
  ixOrient=Isub(initInterval(LZARes.pos.x,LZARes.pos.x),ixOrient);
  iyOrient=Isub(initInterval(LZARes.pos.y,LZARes.pos.y),iyOrient);
  itOrient=Isub(initInterval(LZARes.dir.theta,LZARes.dir.theta),itOrient);
  ixInter=Iintersec(ixOrient,initInterval(Iodo[0],Iodo[1]));
  iyInter=Iintersec(iyOrient,initInterval(Iodo[2],Iodo[3]));
  itInter=Iintersec(itOrient,initInterval(Iodo[4],Iodo[5]));
  if(!IisEmpty(ixInter)&&!IisEmpty(iyInter)&&!IisEmpty(itInter)){
    RelocVal.pos.x=Imid(ixInter);
    RelocVal.pos.y=Imid(iyInter);
    RelocVal.dir.theta=Imid(itInter);
    return true;
  }
  return false;
}

bool isLZAusable(euclid_position odoposP3D){/*
  if(odoposP3D.pos.y>53.88)
    return false;
  if(odoposP3D.pos.y>47.88 && odoposP3D.pos.y<50.88)
    return false;
  if(odoposP3D.pos.x>22.38 && odoposP3D.pos.x<26.38)
    return false;
  if(odoposP3D.pos.x>13.38 && odoposP3D.pos.x<17.58)
    return false;*/
  return true;
}

double GetRTY(std::vector<struct polar_coordinate> Mesures){
  //std::vector<int> ransaclignefrompole(std::vector<struct polar_coordinate> source, double &a, double &b, double dtol , struct polar_coordinate &pref1, struct polar_coordinate &pref2);
  //double GetDistToLine(eucclid_coordinate p, double a, double b, eucclid_coordinate p1, eucclid_coordinate p2);
  std::vector<int> Iransac;
  double a,b,dtol;
  struct polar_coordinate pref1, pref2;
  eucclid_coordinate p0,p1,p2;
  std::vector<struct polar_coordinate> Tmp_telemetries;
  int ifind;

  p0.x=0;
  p0.y=0;
  p0.z=0;

  a=9000;
  Tmp_telemetries=Mesures;
//  std::cout << "Will get RTY" << '\n';
  while(a>15 && Tmp_telemetries.size()>(Mesures.size()*0.1)){
    for(int i=0; i<Iransac.size(); i++){
      ifind=Iransac.size()-1-i;
      Tmp_telemetries.erase(Tmp_telemetries.begin()+Iransac[ifind]);
    }
    Iransac=ransaclignefrompole(Tmp_telemetries, a, b, 0.01, pref1, pref2);
//    std::cout << "afind : "<< a << '\n';
  }
  if(a>15){
//    std::cout << "nofind return 999" << '\n';
    return 999;
  }
//  std::cout << "return estime" << '\n';
  return GetDistToLine(p0, a, b, ConvPolToEuclide(pref1), ConvPolToEuclide(pref2));

}


#ifndef BOOST_GEOMETRY_PROJECTIONS_OCEA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_OCEA_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace ocea
    {

            struct par_ocea
            {
                double    rok;
                double    rtk;
                double    sinphi;
                double    cosphi;
                double    singam;
                double    cosgam;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_ocea_spheroid : public base_t_fi<base_ocea_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_ocea m_proj_parm;

                inline base_ocea_spheroid(const Parameters& par)
                    : base_t_fi<base_ocea_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double t;

                    xy_y = sin(lp_lon);
                /*
                    xy_x = atan2((tan(lp_lat) * this->m_proj_parm.cosphi + this->m_proj_parm.sinphi * xy_y) , cos(lp_lon));
                */
                    t = cos(lp_lon);
                    xy_x = atan((tan(lp_lat) * this->m_proj_parm.cosphi + this->m_proj_parm.sinphi * xy_y) / t);
                    if (t < 0.)
                        xy_x += geometry::math::pi<double>();
                    xy_x *= this->m_proj_parm.rtk;
                    xy_y = this->m_proj_parm.rok * (this->m_proj_parm.sinphi * sin(lp_lat) - this->m_proj_parm.cosphi * cos(lp_lat) * xy_y);
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double t, s;

                    xy_y /= this->m_proj_parm.rok;
                    xy_x /= this->m_proj_parm.rtk;
                    t = sqrt(1. - xy_y * xy_y);
                    lp_lat = asin(xy_y * this->m_proj_parm.sinphi + t * this->m_proj_parm.cosphi * (s = sin(xy_x)));
                    lp_lon = atan2(t * this->m_proj_parm.sinphi * s - xy_y * this->m_proj_parm.cosphi,
                        t * cos(xy_x));
                }

                static inline std::string get_name()
                {
                    return "ocea_spheroid";
                }

            };

            // Oblique Cylindrical Equal Area
            template <typename Parameters>
            void setup_ocea(Parameters& par, par_ocea& proj_parm)
            {
                double phi_0=0.0, phi_1, phi_2, lam_1, lam_2, lonz, alpha;

                proj_parm.rok = par.a / par.k0;
                proj_parm.rtk = par.a * par.k0;
                if ( pj_param(par.params, "talpha").i) {
                    alpha    = pj_param(par.params, "ralpha").f;
                    lonz = pj_param(par.params, "rlonc").f;
                    proj_parm.singam = atan(-cos(alpha)/(-sin(phi_0) * sin(alpha))) + lonz;
                    proj_parm.sinphi = asin(cos(phi_0) * sin(alpha));
                } else {
                    phi_1 = pj_param(par.params, "rlat_1").f;
                    phi_2 = pj_param(par.params, "rlat_2").f;
                    lam_1 = pj_param(par.params, "rlon_1").f;
                    lam_2 = pj_param(par.params, "rlon_2").f;
                    proj_parm.singam = atan2(cos(phi_1) * sin(phi_2) * cos(lam_1) -
                        sin(phi_1) * cos(phi_2) * cos(lam_2),
                        sin(phi_1) * cos(phi_2) * sin(lam_2) -
                        cos(phi_1) * sin(phi_2) * sin(lam_1) );
                    proj_parm.sinphi = atan(-cos(proj_parm.singam - lam_1) / tan(phi_1));
                }
                par.lam0 = proj_parm.singam + geometry::math::half_pi<double>();
                proj_parm.cosphi = cos(proj_parm.sinphi);
                proj_parm.sinphi = sin(proj_parm.sinphi);
                proj_parm.cosgam = cos(proj_parm.singam);
                proj_parm.singam = sin(proj_parm.singam);
                par.es = 0.;
            }

        }} // namespace detail::ocea
    #endif // doxygen

    /*!
        \brief Oblique Cylindrical Equal Area projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
        \par Projection parameters
         - lonc: Longitude (only used if alpha (or gamma) is specified) (degrees)
         - alpha: Alpha (degrees)
         - lat_1: Latitude of first standard parallel (degrees)
         - lat_2: Latitude of second standard parallel (degrees)
         - lon_1 (degrees)
         - lon_2 (degrees)
        \par Example
        \image html ex_ocea.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct ocea_spheroid : public detail::ocea::base_ocea_spheroid<Geographic, Cartesian, Parameters>
    {
        inline ocea_spheroid(const Parameters& par) : detail::ocea::base_ocea_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::ocea::setup_ocea(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class ocea_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<ocea_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void ocea_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("ocea", new ocea_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_OCEA_HPP


#include <boost/pending/disjoint_sets.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <algorithm>
#include <cfloat>
#include <cmath>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iterator>
#include <map>
#include <numeric>
#include <queue>
#include <utility>
#include <vector>

#include "caffe/layer.hpp"
#include "caffe/layer_factory.hpp"
#include "caffe/layers/malis_loss_layer.hpp"
#include "caffe/util/math_functions.hpp"

namespace caffe {

template<class Dtype>
class MalisAffinityGraphCompare {
 private:
  const Dtype * mEdgeWeightArray;
 public:
  explicit MalisAffinityGraphCompare(const Dtype * EdgeWeightArray) {
    mEdgeWeightArray = EdgeWeightArray;
  }
  bool operator()(const uint64_t& ind1, const uint64_t& ind2) const {
    return (mEdgeWeightArray[ind1] > mEdgeWeightArray[ind2]);
  }
};

// Derived from https://github.com/srinituraga/malis/blob/master/matlab/malis_loss_mex.cpp
template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::Malis(const Dtype* conn_data,
                                  const int_tp conn_num_dims,
                                  const int_tp* conn_dims,
                                  const int_tp* nhood_data,
                                  const int_tp* nhood_dims,
                                  const Dtype* seg_data, const bool pos,
                                  Dtype* dloss_data, Dtype* loss_out,
                                  Dtype *classerr_out, Dtype *rand_index_out) {
  if ((nhood_dims[1] != (conn_num_dims - 1))
      || (nhood_dims[0] != conn_dims[0])) {
    LOG(FATAL) << "nhood and conn dimensions don't match"
        << " (" << nhood_dims[1] << " vs. " << (conn_num_dims - 1)
        << " and " << nhood_dims[0] << " vs. "
        << conn_dims[conn_num_dims - 1] <<")";
  }

  /* Cache for speed to access neighbors */
  // nVert stores (X * Y * z)
  uint64_t nVert = 1;
  for (uint64_t i = 1; i < conn_num_dims; ++i) {
    nVert *= conn_dims[i];
    // std::cout << i << " nVert: " << nVert << std::endl;
  }

  // prodDims stores X, X*Y, X*Y*z offsets
  vector<uint64_t> prodDims(conn_num_dims - 1);
  prodDims[conn_num_dims - 2] = 1;
  for (uint64_t i = 1; i < conn_num_dims - 1; ++i) {
    prodDims[conn_num_dims - 2 - i] = prodDims[conn_num_dims - 1 - i]
                                      * conn_dims[conn_num_dims - i];
    // std::cout << conn_num_dims - 2 - i << " dims: "
    //   << prodDims[conn_num_dims - 2 - i] << std::endl;
  }

  /* convert n-d offset vectors into linear array offset scalars */
  // nHood is a vector of size #edges

  vector<uint32_t> nHood(nhood_dims[0]);
  for (uint64_t i = 0; i < nhood_dims[0]; ++i) {
    nHood[i] = 0;
    for (uint64_t j = 0; j < nhood_dims[1]; ++j) {
      nHood[i] += (uint32_t) nhood_data[j + i * nhood_dims[1]] * prodDims[j];
    }
    // std::cout << i << " nHood: " << nHood[i] << std::endl;
  }

  /* Disjoint sets and sparse overlap vectors */
  vector<std::map<uint64_t, uint64_t> > overlap(nVert);
  vector<uint64_t> rank(nVert);
  vector<uint64_t> parent(nVert);
  std::map<uint64_t, uint64_t> segSizes;
  uint64_t nLabeledVert = 0;
  uint64_t nPairPos = 0;
  boost::disjoint_sets<uint64_t*, uint64_t*> dsets(&rank[0], &parent[0]);
  // Loop over all seg data items
  for (uint64_t i = 0; i < nVert; ++i) {
    dsets.make_set(i);
    if (0 != seg_data[i]) {
      overlap[i].insert(std::pair<uint64_t, uint64_t>(seg_data[i], 1));
      ++nLabeledVert;
      ++segSizes[seg_data[i]];
      nPairPos += (segSizes[seg_data[i]] - 1);
    }
  }

  uint64_t nPairTot = (nLabeledVert * (nLabeledVert - 1)) / 2;
  uint64_t nPairNeg = nPairTot - nPairPos;
  uint64_t nPairNorm;

  if (pos) {
    nPairNorm = nPairPos;
  } else {
    nPairNorm = nPairNeg;
  }

  uint64_t edgeCount = 0;
  // Loop over #edges
  for (uint64_t d = 0, i = 0; d < conn_dims[0]; ++d) {
    // Loop over Z
    for (uint64_t z = 0; z < conn_dims[1]; ++z) {
      // Loop over Y
      for (uint64_t Y = 0; Y < conn_dims[2]; ++Y) {
        // Loop over X
        for (uint64_t X = 0; X < conn_dims[3]; ++X, ++i) {
          // Out-of-bounds check:
          if (!((z + nhood_data[d * nhood_dims[1] + 0] < 0)
              ||(z + nhood_data[d * nhood_dims[1] + 0] >= conn_dims[1])
              ||(Y + nhood_data[d * nhood_dims[1] + 1] < 0)
              ||(Y + nhood_data[d * nhood_dims[1] + 1] >= conn_dims[2])
              ||(X + nhood_data[d * nhood_dims[1] + 2] < 0)
              ||(X + nhood_data[d * nhood_dims[1] + 2] >= conn_dims[3]))) {
            ++edgeCount;
          }
        }
      }
    }
  }

  /* Sort all the edges in increasing order of weight */
  vector<uint64_t> pqueue(edgeCount);
  uint64_t j = 0;
  // Loop over #edges
  for (uint64_t d = 0, i = 0; d < conn_dims[0]; ++d) {
    // Loop over Z
    for (uint64_t z = 0; z < conn_dims[1]; ++z) {
      // Loop over Y
      for (uint64_t Y = 0; Y < conn_dims[2]; ++Y) {
        // Loop over X
        for (uint64_t X = 0; X < conn_dims[3]; ++X, ++i) {
          // Out-of-bounds check:
          if (!((z + nhood_data[d * nhood_dims[1] + 0] < 0)
              ||(z + nhood_data[d * nhood_dims[1] + 0] >= conn_dims[1])
              ||(Y + nhood_data[d * nhood_dims[1] + 1] < 0)
              ||(Y + nhood_data[d * nhood_dims[1] + 1] >= conn_dims[2])
              ||(X + nhood_data[d * nhood_dims[1] + 2] < 0)
              ||(X + nhood_data[d * nhood_dims[1] + 2] >= conn_dims[3]))) {
            pqueue[j++] = i;
          }
        }
      }
    }
  }

  pqueue.resize(j);

  std::sort(pqueue.begin(), pqueue.end(),
       MalisAffinityGraphCompare<Dtype>(conn_data));

  /* Start MST */
  uint64_t minEdge;
  uint64_t e, v1, v2;
  uint64_t set1, set2;
  uint64_t nPair = 0;
  double loss = 0, dl = 0;
  uint64_t nPairIncorrect = 0;
  std::map<uint64_t, uint64_t>::iterator it1, it2;

  /* Start Kruskal's */
  for (uint64_t i = 0; i < pqueue.size(); ++i) {
    minEdge = pqueue[i];
    // nVert = X * Y * z, minEdge in [0, X * Y * z * #edges]

    // e: edge dimension
    e = minEdge / nVert;

    // v1: node at edge beginning
    v1 = minEdge % nVert;

    // v2: neighborhood node at edge e
    v2 = v1 + nHood[e];

    // std::cout << "V1: " << v1 << ", V2: " << v2 << std::endl;

    set1 = dsets.find_set(v1);
    set2 = dsets.find_set(v2);

    if (set1 != set2) {
      dsets.link(set1, set2);

      /* compute the dloss for this MST edge */
      for (it1 = overlap[set1].begin(); it1 != overlap[set1].end(); ++it1) {
        for (it2 = overlap[set2].begin(); it2 != overlap[set2].end(); ++it2) {
          nPair = it1->second * it2->second;

          if (pos && (it1->first == it2->first)) {
            // +ve example pairs
            dl = (Dtype(1.0) - conn_data[minEdge]);
            loss += dl * dl * nPair;
            // Use hinge loss
            dloss_data[minEdge] += dl * nPair;
            if (conn_data[minEdge] <= Dtype(0.5)) {  // an error
              nPairIncorrect += nPair;
            }

          } else if ((!pos) && (it1->first != it2->first)) {
            // -ve example pairs
            dl = (-conn_data[minEdge]);
            loss += dl * dl * nPair;
            // Use hinge loss
            dloss_data[minEdge] += dl * nPair;
            if (conn_data[minEdge] > Dtype(0.5)) {  // an error
              nPairIncorrect += nPair;
            }
          }
        }
      }

      if (nPairNorm > 0) {
        dloss_data[minEdge] /= nPairNorm;
      } else {
        dloss_data[minEdge] = 0;
      }

      if (dsets.find_set(set1) == set2) {
        std::swap(set1, set2);
      }

      for (it2 = overlap[set2].begin();
          it2 != overlap[set2].end(); ++it2) {
        it1 = overlap[set1].find(it2->first);
        if (it1 == overlap[set1].end()) {
          overlap[set1].insert(pair<uint64_t, uint64_t>
            (it2->first, it2->second));
        } else {
          it1->second += it2->second;
        }
      }
      overlap[set2].clear();
    }  // end link
  }  // end while

  /* Return items */
  double classerr, randIndex;
  if (nPairNorm > 0) {
    loss /= nPairNorm;
  } else {
    loss = 0;
  }

  // std::cout << "nPairIncorrect: " << nPairIncorrect << std::endl;
  // std::cout << "nPairNorm: " << nPairNorm << std::endl;

  *loss_out = loss;
  classerr = static_cast<double>(nPairIncorrect)
      / static_cast<double>(nPairNorm);
  *classerr_out = classerr;
  randIndex = 1.0 - static_cast<double>(nPairIncorrect)
      / static_cast<double>(nPairNorm);
  *rand_index_out = randIndex;
}

template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::LayerSetUp(
    const vector<Blob<MItype>*>& bottom, const vector<Blob<MOtype>*>& top) {
  LossLayer<Dtype, MItype, MOtype>::LayerSetUp(bottom, top);

  // Expected inputs:
  // Required (bottom 0 to 2):
  // Bottom 0: Predicted affinity, shaped     (batch size, #edges, (Z), (Y), X)
  // Bottom 1: Ground truth affinity, shaped  (batch size, #edges, (Z), (Y), X)
  // Bottom 2: Segmented ground truth, shaped (batch size, 1,      (Z), (Y), X)

  // Optional (bottom 3):
  // Bottom 3: Edge connectivity, size #edges * 3, shaped (Z,Y,X);(Z,Y,X);...
  // (this means pairs of 3 per edge)
}

template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::Reshape(
    const vector<Blob<MItype>*>& bottom, const vector<Blob<MOtype>*>& top) {
  LossLayer<Dtype, MItype, MOtype>::Reshape(bottom, top);

  if (top.size() >= 2) {
    top[1]->ReshapeLike(*bottom[0]);
  }

  // Up to 5 dimensional; supported modes:
  // batch, channels (edges), Z, Y, X    => 3D affinity
  // batch, channels (edges), Y, X       => 2D affinity
  // batch, channels (edges), X          => 1D affinity
  vector<int_tp> shape = bottom[0]->shape();

  conn_dims_.clear();
  nhood_dims_.clear();

  // #edges, Z, Y, X specification (4 dimensions)
  conn_num_dims_ = 4;

  // Channel axis equals number of edges
  nedges_ = shape[1];

  // #edges
  conn_dims_.push_back(nedges_);
  // Z-axis
  conn_dims_.push_back(shape.size() >= 5 ? shape[shape.size() - 3] : 1);
  // Y-axis
  conn_dims_.push_back(shape.size() >= 4 ? shape[shape.size() - 2] : 1);
  // X-axis
  conn_dims_.push_back(shape.size() >= 3 ? shape[shape.size() - 1] : 1);

  // #edges
  nhood_dims_.push_back(nedges_);
  // 3 dimensional (always, to simplify things;
  // can just set unused spatials to 0)
  nhood_dims_.push_back(3);

  affinity_pos_.Reshape(shape);
  affinity_neg_.Reshape(shape);
  dloss_pos_.Reshape(shape);
  dloss_neg_.Reshape(shape);
}

template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::Forward_cpu(const vector<Blob<MItype>*>& bottom,
                                        const vector<Blob<MOtype>*>& top) {
  // Set up the neighborhood
  nhood_data_.clear();
  if (bottom.size() == 4) {
    // Custom edges
    for (int_tp i = 0; i < nedges_; ++i) {
      // Z edge direction
      nhood_data_.push_back(bottom[3]->cpu_data()[i * 3 + 0]);
      // Y edge direction
      nhood_data_.push_back(bottom[3]->cpu_data()[i * 3 + 1]);
      // X edge direction
      nhood_data_.push_back(bottom[3]->cpu_data()[i * 3 + 2]);
    }
  } else {
    // Dimension primary edges (+Z, +Y, +X) only:
    // 1 edge:    +X          (0,0,1)
    // 2 edges:   +Y, +X      (0,1,0); (0,0,1)
    // 3 edges:   +Z, +Y, +X  (1,0,0); (0,1,0); (0,0,1)
    for (int_tp i = 3 - nedges_; i < 3; ++i) {
      nhood_data_.push_back((i + 3) % 3 == 0 ? 1 : 0);
      nhood_data_.push_back((i + 2) % 3 == 0 ? 1 : 0);
      nhood_data_.push_back((i + 1) % 3 == 0 ? 1 : 0);
    }
  }

  // Predicted affinity
  const Dtype* affinity_prob = bottom[0]->cpu_data();

  // Effective affinity
  const Dtype* affinity = bottom[1]->cpu_data();

  Dtype* affinity_data_pos = affinity_pos_.mutable_cpu_data();
  Dtype* affinity_data_neg = affinity_neg_.mutable_cpu_data();

// Affinity graph must be in the range (0,1)
// square loss (euclidean) is used by MALIS
  for (int_tp i = 0; i < bottom[0]->count(); ++i) {
    affinity_data_pos[i] = std::min(affinity_prob[i], affinity[i]);
    affinity_data_neg[i] = std::max(affinity_prob[i], affinity[i]);
  }

  uint_tp batch_offset = 1;
  for (int_tp i = 1; i < bottom[0]->shape().size(); ++i) {
    batch_offset *= bottom[0]->shape()[i];
  }

  uint_tp components_batch_offset = 1;
  uint_tp components_channel_offset = bottom[2]->shape()[1] == 2 ? 1 : 0;
  for (int_tp i = 1; i < bottom[2]->shape().size(); ++i) {
    components_batch_offset *= bottom[2]->shape()[i];
    if (i > 1) {
      components_channel_offset *= bottom[2]->shape()[i];
    }
  }

  float loss = 0;

#pragma omp parallel for reduction(+:loss)
  for (int_tp batch = 0; batch < bottom[0]->shape()[0]; ++batch) {
    Dtype loss_out = 0;
    Dtype classerr_out = 0;
    Dtype rand_index_out = 0;

    caffe_set(dloss_neg_.count(), Dtype(0.0), dloss_neg_.mutable_cpu_data());
    caffe_set(dloss_pos_.count(), Dtype(0.0), dloss_pos_.mutable_cpu_data());

    Malis(&affinity_data_neg[batch_offset * batch], conn_num_dims_,
          &conn_dims_[0], &nhood_data_[0], &nhood_dims_[0],
          bottom[2]->cpu_data() + components_batch_offset * batch, false,
          dloss_neg_.mutable_cpu_data() + batch_offset * batch, &loss_out,
          &classerr_out, &rand_index_out);

    loss += 0.5 * loss_out;
    // std::cout << "NEG: " << loss_out << std::endl;

    Malis(&affinity_data_pos[batch_offset * batch], conn_num_dims_,
          &conn_dims_[0], &nhood_data_[0], &nhood_dims_[0],
          bottom[2]->cpu_data() + components_batch_offset * batch
          + components_channel_offset, true,
          dloss_pos_.mutable_cpu_data() + batch_offset * batch, &loss_out,
          &classerr_out, &rand_index_out);

    loss += 0.5 * loss_out;
    // std::cout << "POS: " << loss_out << std::endl;
  }

  // Normalized loss over batch size
  top[0]->mutable_cpu_data()[0] = loss
      / (static_cast<Dtype>(bottom[0]->shape()[0]));

  if (top.size() == 2) {
    top[1]->ShareData(*(bottom[0]));
  }
}

template<typename Dtype, typename MItype, typename MOtype>
void MalisLossLayer<Dtype, MItype, MOtype>::Backward_cpu(const vector<Blob<MOtype>*>& top,
                                         const vector<bool>& propagate_down,
                                         const vector<Blob<MItype>*>& bottom) {
  if (propagate_down[0]) {
    Dtype* bottom_diff = bottom[0]->mutable_cpu_diff();
    const Dtype* dloss_pos_data = dloss_pos_.cpu_data();
    const Dtype* dloss_neg_data = dloss_neg_.cpu_data();

    // Clear the diff
    caffe_set(bottom[0]->count(), Dtype(0.0), bottom_diff);

    for (int_tp i = 0; i < bottom[0]->count(); ++i) {
      bottom_diff[i] = -(dloss_neg_data[i] + dloss_pos_data[i]) / 2.0;
    }
  }
}

INSTANTIATE_CLASS_3T_GUARDED(MalisLossLayer, (half_fp), (half_fp), (half_fp));
INSTANTIATE_CLASS_3T_GUARDED(MalisLossLayer, (float), (float), (float));
INSTANTIATE_CLASS_3T_GUARDED(MalisLossLayer, (double), (double), (double));

REGISTER_LAYER_CLASS(MalisLoss);
REGISTER_LAYER_CLASS_INST(MalisLoss, (half_fp), (half_fp), (half_fp));
REGISTER_LAYER_CLASS_INST(MalisLoss, (float), (float), (float));
REGISTER_LAYER_CLASS_INST(MalisLoss, (double), (double), (double));

}  // namespace caffe


#pragma once

#include <Geometry2d/Point.hpp>
#include <Geometry2d/ShapeSet.hpp>
#include <planning/InterpolatedPath.hpp>
#include <planning/MotionCommand.hpp>
#include <planning/MotionConstraints.hpp>
#include <planning/MotionInstant.hpp>
#include "SingleRobotPathPlanner.hpp"

#include <rrt/BiRRT.hpp>

#include "SystemState.hpp"

#include <Eigen/Dense>
#include <boost/optional.hpp>
#include <list>

namespace Planning {

struct CubicBezierControlPoints {
    Geometry2d::Point p0, p1, p2, p3;

    CubicBezierControlPoints(Geometry2d::Point p0, Geometry2d::Point p1,
                             Geometry2d::Point p2, Geometry2d::Point p3)
        : p0(p0), p1(p1), p2(p2), p3(p3) {}
};

/**
 * @brief Given a start point and an end point and some conditions, plans a path
 * for a robot to get there.
 *
 * @details There are many ways to plan paths.  This planner uses bidirectional
 * [RRTs](http://en.wikipedia.org/wiki/Rapidly-exploring_random_tree).
 * You can check out our interactive RRT applet on GitHub here:
 * https://github.com/RoboJackets/rrt.
 */
class RRTPlanner : public SingleRobotPathPlanner {
public:
    /**
     * Constructor taking in the max iterations the RRT planner should run
     */
    RRTPlanner(int minIterations, int maxIterations);

    /**
     * gets the maximum number of iterations for the RRT algorithm
     */
    int maxIterations() const { return _maxIterations; }

    /**
     * sets the maximum number of iterations for th RRT algorithm
     */
    void maxIterations(int value) { _maxIterations = value; }

    int minIterations() const { return _minIterations; }
    void minIterations(int value) { _minIterations = value; }

    /**
     * Takes in a waypoints and returns a full InterpolatedPath with a generated
     * Velocity Profile.
     */
    static std::unique_ptr<Planning::InterpolatedPath> generatePath(
        const std::vector<Geometry2d::Point>& points,
        const Geometry2d::ShapeSet& obstacles,
        const MotionConstraints& motionConstraints, Geometry2d::Point vi,
        Geometry2d::Point vf);

    // Overridden methods

    MotionCommand::CommandType commandType() const override {
        return MotionCommand::PathTarget;
    }

    virtual std::unique_ptr<Path> run(PlanRequest& planRequest) override;

    int reusePathTries = 0;

protected:
    /// minimum and maximum number of rrt iterations to run
    /// this does not include connect attempts
    int _minIterations, _maxIterations;

    /// Check to see if the previous path (if any) should be discarded and
    /// replaced with a newly-planned one
    bool shouldReplan(const PlanRequest& planRequest,
                      const std::vector<DynamicObstacle> dynamicObs,
                      std::string* debugOut = nullptr) const;

    /// Runs a bi-directional RRT to attempt to join the start and end states.
    std::vector<Geometry2d::Point> runRRT(
        MotionInstant start, MotionInstant goal,
        const MotionConstraints& motionConstraints,
        const Geometry2d::ShapeSet& obstacles, SystemState* state,
        unsigned shellID);

    std::unique_ptr<InterpolatedPath> generateRRTPath(
        const MotionInstant& start, const MotionInstant& goal,
        const MotionConstraints& motionConstraints,
        Geometry2d::ShapeSet& obstacles,
        const std::vector<DynamicObstacle> paths, SystemState* state,
        unsigned shellID);

    /**
     * Takes in waypoints and returns a InterpolatedPath with a generated
     * Velocity Profile.
     *
     * 1. It generates a bezier path from the waypoints given
     * 2. It generates a velocity profile to fit the constraints given
     * 3. It returns a interpolated path combining the bezier path and the
     *velocity profile
     */
    static std::unique_ptr<Planning::InterpolatedPath> generateCubicBezier(
        const std::vector<Geometry2d::Point>& points,
        const Geometry2d::ShapeSet& obstacles,
        const MotionConstraints& motionConstraints, Geometry2d::Point vi,
        Geometry2d::Point vf);

    /**
     *  Removes unnecesary waypoints in the path
     */
    static void optimize(std::vector<Geometry2d::Point>& path,
                         const Geometry2d::ShapeSet& obstacles,
                         Geometry2d::Point vi, Geometry2d::Point vf);

    /**
     * Generates a Cubic Bezier Path based on Albert's Bezier Velocity
     * Path Algorithm using waypoints. It creates a path with continuous
     * velocity and acceleration
     * that estimates speed using the trapezoidal motion heuristic
     */
    static std::vector<CubicBezierControlPoints> generateCubicBezierPath(
        const std::vector<Geometry2d::Point>& points,
        const MotionConstraints& motionConstraints, Geometry2d::Point vi,
        Geometry2d::Point vf,
        const boost::optional<std::vector<float>>& times = boost::none);

    /**
     * Generates a velocity profile from a Cubic Bezier Path under the given
     * motion constratins
     */
    static std::vector<InterpolatedPath::Entry> generateVelocityPath(
        const std::vector<CubicBezierControlPoints>& controlPoints,
        const MotionConstraints& motionConstraints, Geometry2d::Point vi,
        Geometry2d::Point vf, int interpolations = 40);

    /**
     * Generates a Cubic Bezier Path based on some attempted heuristical Control
     * Point Placement
     */
    static std::vector<CubicBezierControlPoints> generateNormalCubicBezierPath(
        const std::vector<Geometry2d::Point>& points,
        const MotionConstraints& motionConstraints, Geometry2d::Point vi,
        Geometry2d::Point vf);

    /**
     * Helper function for cubicBezier() which uses Eigen matrices to solve for
     * the
     * cubic bezier equations.
     */
    static Eigen::VectorXd cubicBezierCalc(double vi, double vf,
                                           std::vector<double>& points,
                                           std::vector<double>& ks,
                                           std::vector<double>& ks2);

    /**
     * Helper method for runRRT(), which creates a vector of points representing
     * the RRT path.
     */
    std::vector<Geometry2d::Point> runRRTHelper(
        MotionInstant start, MotionInstant goal,
        const MotionConstraints& motionConstraints,
        const Geometry2d::ShapeSet& obstacles, SystemState* state,
        unsigned shellID, bool straightLine);
};
}  // namespace Planning


/*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014-2015, Timm Linder, Fabian Girrbach, Social Robotics Lab, University of Freiburg
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions are met:
*
*  * Redistributions of source code must retain the above copyright notice, this
*    list of conditions and the following disclaimer.
*  * Redistributions in binary form must reproduce the above copyright notice,
*    this list of conditions and the following disclaimer in the documentation
*    and/or other materials provided with the distribution.
*  * Neither the name of the copyright holder nor the names of its contributors
*    may be used to endorse or promote products derived from this software
*    without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <srl_nearest_neighbor_tracker/data_association/basic_nearest_neighbor_data_association.h>

#include <srl_nearest_neighbor_tracker/base/defs.h>
#include <srl_nearest_neighbor_tracker/base/stl_helpers.h>
#include <srl_nearest_neighbor_tracker/ros/params.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include <Eigen/LU>
#include <limits>
#include <map>

namespace srl_nnt {

void BasicNearestNeighborDataAssociation::initializeDataAssociation(const ros::NodeHandle& nodeHandle, const ros::NodeHandle& privateNodeHandle) {
    m_nodeHandle = nodeHandle;
    m_privateNodeHandle = privateNodeHandle;
}

Pairings BasicNearestNeighborDataAssociation::performDataAssociation(Tracks& tracks, const Observations& observations){

    ROS_DEBUG("Performing data association");

    //
    // Step 0: Make sure observations are in a good state
    //

    foreach(Observation::Ptr observation, observations) {
        observation->matched = false;
    }

    Pairings compatiblePairings;

    //
    // Step 1: go through all possible associations and store compatible ones
    //

    const double MATRIX_LN_EPS = -1e8;
    const double MAX_GATING_DISTANCE = Params::get<double>("max_gating_distance", 1.0);

    typedef multimap<track_id, Pairing::Ptr> TrackSpecificPairings;
    TrackSpecificPairings trackSpecificPairings;

    ObsVector v;
    foreach(Track::Ptr track, tracks)
    {
        foreach(Observation::Ptr observation, observations)
                                                                                {
            v = observation->z - track->state->zp();

            if(v.norm() < MAX_GATING_DISTANCE)
            {
                // Create a new pairing
                Pairing::Ptr pairing( new Pairing );

                pairing->track = track;
                pairing->observation = observation;
                pairing->validated = false;

                // Calculate innovation v and inverse of innovation covariance S
                pairing->v = v;

                ObsMatrix S = track->state->H() * track->state->Cp() * track->state->H().transpose() + observation->R;
                Eigen::FullPivLU<ObsMatrix> lu(S);
                double ln_det_S = log(lu.determinant());

                // Calculate inverse of innovation covariance if possible
                if (ln_det_S > MATRIX_LN_EPS) {
                    pairing->Sinv = lu.inverse();
                    pairing->d = (pairing->v.transpose() * pairing->Sinv * pairing->v)(0,0);
                    pairing->singular = pairing->d < 0.0;
                }
                else {
                    pairing->Sinv = ObsMatrix::Constant(OBS_DIM, OBS_DIM, numeric_limits<double>::quiet_NaN());
                    pairing->d = numeric_limits<double>::quiet_NaN();
                    pairing->singular = true;

                    ROS_WARN_STREAM("Singular pairing encountered!\nTrack " << track->id << " measurement prediction:\n" << track->state->zp() << "\nTrack covariance prediction:\n" << track->state->Cp()
                                    << "\nObservation " << observation->id << " mean:\n" << observation->z << "\nObservation covariance:\n" << observation->R );
                }

                // Perform gating
                if(!pairing->singular && pairing->d < CHI2INV_99[OBS_DIM]) {
                    // Store in list of compatible pairings
                    trackSpecificPairings.insert( make_pair(track->id, pairing) );
                }
            }
                                                                                }
    }

    ROS_DEBUG("%zu compatible pairings have been found for %zu existing tracks and %zu new observations!", compatiblePairings.size(), tracks.size(), observations.size() );

    //
    // Step 2: go through all tracks, choose and mark nearest neighbour
    //

    foreach(Track::Ptr track, tracks)
    {
        // Store best pairing for this track
        double best_d = numeric_limits<double>::max();
        Pairing::Ptr bestPairing;

        // Iterate over all pairings for this track
        pair<TrackSpecificPairings::iterator, TrackSpecificPairings::iterator> pairingsForThisTrack = trackSpecificPairings.equal_range(track->id);
        for(TrackSpecificPairings::iterator it = pairingsForThisTrack.first; it != pairingsForThisTrack.second; it++) {
            Pairing::Ptr pairing = it->second;

            // Allow that a observation can be matched to several tracks
            // is better than what we have seen so far.
            if(pairing->d < best_d) {
                best_d = pairing->d;
                bestPairing = pairing;
            }
        }

        // See if we found a compatible pairing for this track.
        if(bestPairing) {
            compatiblePairings.push_back(bestPairing);
            markAsMatched(track, bestPairing);
        }
        else if (track->trackStatus == Track::OCCLUDED){
            // Track is occluded
            track->observation.reset();
            track->trackStatus = Track::OCCLUDED;
            track->numberOfConsecutiveOcclusions++;
        }
        else{
            // Track is missed
            track->observation.reset();
            track->trackStatus = Track::MISSED;
            track->numberOfConsecutiveMisses++;
        }
    }

    return compatiblePairings;
}

}


//==================================================================================================
/*
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================

#include <boost/simd/function/plus.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

#undef STF_EQUAL
#define STF_EQUAL(a, b) $.stream() << a << " == " << b << std::endl

struct auto_nothing {
    template <typename T, typename U>
    BOOST_FORCEINLINE T operator()(T const& a, U const&) const { return a; }

    template <typename T, typename U>
    BOOST_FORCEINLINE void equal(T&, U const&) const {}
};

#define AUTO_ARITHMETIC_OP(name, sym)                                                              \
  struct name {                                                                                \
    template <typename T, typename U>                                                              \
    BOOST_FORCEINLINE auto operator()(T const& a, U const& b) const                                \
    BOOST_NOEXCEPT_DECLTYPE_BODY(a sym b)                                                          \
                                                                                \
    template <typename T, typename U>                                                              \
    BOOST_FORCEINLINE void equal(T& a, U const& b) const                                           \
    { a BOOST_PP_CAT(sym, =) b; }                                                                  \
  }                                                                                \
  /**/

#define AUTO_LOGICAL_OP(name, sym)                                                                 \
  struct name {                                                                                \
    template <typename T, typename U>                                                              \
    BOOST_FORCEINLINE auto operator()(T const& a, U const& b) const                                \
    BOOST_NOEXCEPT_DECLTYPE_BODY(a sym b)                                                          \
  }                                                                                \
  /**/

AUTO_ARITHMETIC_OP(auto_add, +);
AUTO_ARITHMETIC_OP(auto_sub, -);
AUTO_ARITHMETIC_OP(auto_mul, *);
AUTO_ARITHMETIC_OP(auto_div, /);
AUTO_ARITHMETIC_OP(auto_mod, %);

AUTO_LOGICAL_OP(auto_lt, <);
AUTO_LOGICAL_OP(auto_gt, >);
AUTO_LOGICAL_OP(auto_le, <=);
AUTO_LOGICAL_OP(auto_ge, >=);

template <typename P, typename T, typename U, typename Op, typename Env>
void test_op(Env& $, T const& s0, U const& s1, Op const& op)
{
  P p0(s0);
  P p1(s1);
  P ref = P(op(s0, s1));

  STF_EQUAL(op(p0, p1), ref);
  STF_EQUAL(op(s0, p1), ref);
  STF_EQUAL(op(p0, s1), ref);

  op.equal(p0, p1);
  STF_EQUAL(p0, ref);
}

template <typename P, typename T, typename U, typename Op, typename Env>
void test_logical_op(Env& $, T const& s0, U const& s1, Op const& op)
{
  P p0(s0);
  P p1(s1);
  P ref0 = P(boost::simd::logical<typename P::value_type>(op(s0, s1)));
  P ref1 = P(boost::simd::logical<typename P::value_type>(op(s1, s0)));

  STF_EQUAL(op(p0, p1), ref0);
  STF_EQUAL(op(s0, p1), ref0);
  STF_EQUAL(op(p0, s1), ref0);

  STF_EQUAL(op(p1, p0), ref1);
  STF_EQUAL(op(s1, p0), ref1);
  STF_EQUAL(op(p1, s0), ref1);
}

template <typename P, typename T, typename Env>
void test_unary_op(Env& $, T const& s0)
{
  //STF_EQUAL(!P(s0), P(!s0));
  //STF_EQUAL(~P(s0), P(~s0));

  //STF_EQUAL(-P(s0), P(-s0));
  STF_EQUAL(+P(s0), P(+s0));

  {
    P p(s0);
    STF_EQUAL(++p, P(s0 + T(1)));
  }
  {
    P p(s0);
    STF_EQUAL(p++, P(s0));
    STF_EQUAL(p,   P(s0 + T(1)));
  }

  {
    P p(s0);
    STF_EQUAL(--p, P(s0 - T(1)));
  }
  {
    P p(s0);
    STF_EQUAL(p--, P(s0));
    STF_EQUAL(p,   P(s0 - T(1)));
  }
}

template <typename T, std::size_t N, typename U, typename V, typename Env>
void test(Env& $, U const& s0, V const& s1)
{
  STF_INFO("With N = " << N);
  using p_t = boost::simd::pack<T, N>;
  using auto_mod_or_nothing =
    typename std::conditional<std::is_integral<T>::value, auto_mod, auto_nothing>::type;

  test_op<p_t>($, s0, s1, auto_add());
  test_op<p_t>($, s0, s1, auto_sub());
  test_op<p_t>($, s0, s1, auto_mul());
  test_op<p_t>($, s0, s1, auto_div());
  test_op<p_t>($, s0, s1, auto_mod_or_nothing());

  test_unary_op<p_t>($, s0);

#if 0
  test_logical_op<p_t>($, s0, s1, auto_lt());
  test_logical_op<p_t>($, s0, s1, auto_le());
  test_logical_op<p_t>($, s0, s1, auto_gt());
  test_logical_op<p_t>($, s0, s1, auto_ge());
#endif
}

STF_CASE_TPL("Check pack's operators", STF_NUMERIC_TYPES)
{
  test<T,  2>($, 2, 4);
  test<T,  4>($, 2, 4);
  test<T,  8>($, 2, 4);
  test<T, 16>($, 2, 4);
//  test<T, 32>($, 2, 4);

  STF_PASS("You should have the same pack at lhs and rhs of ==");
}