/*! \file tracking.cpp
 *  \brief Track multiple object along an image sequence
 *  \brief 
 *  \author Nicolas Papadakis 
 *  \date 2010-02-22
 */


#include <stdio.h>
#include "functions_tracking.cpp"
using namespace cimg_library;
int main(int argc,char **argv)
{
printf("Input/Output:\n");
  const char *seq = cimg_option("-seq","Datos/Image_0","Image name");
  const char *type = cimg_option("-type","ppm","Image type");
  const int format = cimg_option("-ba",3,"Number of digit of image name");
  const int init = cimg_option("-i",1,"Number of the first image");
  const int taille = cimg_option("-t",244,"Number of images");
  const char *resu = cimg_option("-res","Resultats","Directory of results");
  const bool ini = cimg_option("-init",1,"Load initialization masks (1), or select manually  curves (0)");
  const char *seq_init = cimg_option("-seqin","Datos/Init_000","Path of initialization masks");
  const char *type_init = cimg_option("-initype","ppm","Initialization image type");
  const bool occ = cimg_option("-occ",1,"Display the occulted part of the objects for output");

printf("\n\nTracking information:\n");
  const int rep = cimg_option("-rep",0,"Continue computation (to use saved initialization)");
  const int nb_objet = cimg_option("-nb",1,"Number of tracked objects");
  const int maj = cimg_option("-upd",20,"Frequency of histogram update");




printf("\n\nModel parameters:\n");
  const double bet = cimg_option("-b",1.5,"Disparition coefficient");
  const double alph = cimg_option("-a",1.2,"Occlusion coefficient");
  double alph2 = cimg_option("-a2",1.2,"Staying in occlusion coefficient");

  alph2=Min(alph,Max(0.0,alph2));

  const double gamma = cimg_option("-g",1.5,"Disocclusion/apparition coefficient");
  const double r = cimg_option("-r",10.0,"Regularization coefficient for visible area");
  const double sigma_grad = cimg_option("-sg",80.0,"Image gradient variance for visible regularization");
  const double r2 = cimg_option("-r2",1.0,"Regularization coefficient for good and bad predictions");


printf("\n\nVelocity parameters:\n");
  const int pp = cimg_option("-pp",4,"Velocity exponent influence");
  const bool dynamique = cimg_option("-dyn",1,"Use a consistent dynamic?");
  const double confiance = cimg_option("-conf",0.5,"Confidance in dynamic");
  const  bool filtrage = cimg_option("-fil",1,"Filtering of the motion estimation");
  const double sigma = cimg_option("-si",3.0,"Velocity variance for objects");

printf("\n\nHarris and KLT parameters:\n");
  const int niv = cimg_option("-n",3,"Number of scales");
  const int fenetre = cimg_option("-w",5,"Size of windows");
  const double seuil_max = cimg_option("-s",200.0,"Maximum gradient threshold");
  const int bord = cimg_option("-bord",10,"Bordure in pixel  for Harris");
  const double bord_opt = cimg_option("-bord_opt",0,"Bordure in pixel for KLT");

  char nom[200];
sprintf(nom,"install -d %s",resu);
system(nom);
sprintf(nom,"install -d %s/Occ",resu);
system(nom);

bool normalize=1;
  switch(format) 

    {
    case 2:{
      sprintf(nom,"%s%.2d.%s",seq,init,type);break;
    }
    case 3:{
      sprintf(nom,"%s%.3d.%s",seq,init,type);break;
    }
    case 4:{
      sprintf(nom,"%s%.4d.%s",seq,init,type);break;
    }
    default:{
      sprintf(nom,"%s%d.%s",seq,init,type);break;
    }
    }


  CImg<int> colore(Max(nb_objet,1),3,1,1,50);

  colore(0,0) = 255;
  colore(0,1) = 0;
  colore(0,2) = 0;

if(nb_objet>1){
  colore(1,0) = 0;
  colore(1,1) = 255;
  colore(1,2) = 0;
}
if(nb_objet>2){
  colore(2,0) = 0;
  colore(2,1) = 0;
  colore(2,2) = 255;
}

if(nb_objet>3){
  colore(3,0) = 128;
  colore(3,1) = 128;
  colore(3,2) = 0;
}


if(nb_objet>4){
  colore(4,0) = 0;
  colore(4,1) = 128;
  colore(4,2) = 128;
}


if(nb_objet>5){
  colore(5,0) = 128;
  colore(5,1) = 0;
  colore(5,2) = 128;
}


if(nb_objet>6){
  colore(6,0) = 128;
  colore(6,1) = 0;
  colore(6,2) = 0;
}


if(nb_objet>7){
  colore(7,0) = 0;
  colore(7,1) = 128;
  colore(7,2) = 0;
}


if(nb_objet>8){
  colore(8,0) = 0;
  colore(8,1) = 0;
  colore(8,2) = 128;
}


if(nb_objet>9){
  colore(9,0) = 64;
  colore(9,1) = 0;
  colore(9,2) = 196;
}


if(nb_objet>10){
  colore(10,0) = 0;
  colore(10,1) = 64;
  colore(10,2) = 196;
}


if(nb_objet>11){
  colore(11,0) = 64;
  colore(11,1) = 196;
  colore(11,2) = 100;
}


if(nb_objet>12){
  colore(12,0) = 196;
  colore(12,1) = 64;
  colore(12,2) = 100;
}


if(nb_objet>13){
  colore(13,0) = 100;
  colore(13,1) = 196;
  colore(13,2) = 64;
}


if(nb_objet>14){
  colore(14,0) = 196;
  colore(14,1) = 0;
  colore(14,2) = 64;
}


  int color_temp[3];


  CImg<int> Im_precedante(nom);
 


Im_precedante.save_bmp("First_frame.bmp");
  nbc=Im_precedante.width;
  nbl=Im_precedante.height;
  CImg<int> nb_pixels(nb_objet+1,1,1,1,1);
  CImg<int> nb_pixels_visible(nb_objet+1,1,1,1,1);
  int nb_pixels_tot=1;
  CImg<double> Im_cou(nbc,nbl,1,1,0);
  CImg<int> Im_courante(nbc,nbl,1,3,0);
  CImg<int> Im_res(nbc,nbl,1,3,0);
  CImg<double> Im_courante_gris(nbc,nbl,1,1,0.);
  CImg<double> Im_precedante_gris(nbc,nbl,1,1,0.);


  Im_precedante_gris=RGB2gris(Im_precedante);
  Im_precedante_gris  = normalize?Im_precedante_gris.get_blur(0.5).equalize_histogram():Im_precedante_gris.get_blur(0.5);


  CImg<int> Im_label(nbc,nbl,1,1);

  CImg<double> edge_poids_courant(nbc,nbl,4,1);

  CImg<double> sigma_objet(nb_objet,2,1,1,sigma);
  CImg<int> pos_image(nbc,nbl,1,1);
  CImgList<int> pos_objet(nb_objet,nbc,nbl,1,1,0);

  CImgList<bool> Objet_precedant(nb_objet+1,nbc,nbl,1,1);
  CImgList<bool> Objet_precedant_visible(nb_objet+1,nbc,nbl,1,1);
  CImg<bool> Objet_prediction(nbc,nbl,1,1);
  CImg<bool> Objet_prediction_visible(nbc,nbl,1,1);
  CImgList<double> Objet_distance_prediction(nb_objet+1,nbc,nbl,1,1);
  CImgList<double> Objet_distance_prediction_visible(nb_objet+1,nbc,nbl,1,1);
  CImgList<double> beta(nb_objet,nb_objet+1,nbc,nbl,1);
  CImgList<double> alpha(nb_objet,nb_objet+1,nbc,nbl,1);

 /*CImgList<int> Image(taille,nbc,nbl,1,3);
for(int t=0;t<taille;t++)
{
      switch(format)
	{
	case 2:{
	  sprintf(nom,"%s%02d.%s",seq,init+t,type);break;
	}
	case 3:{
	  sprintf(nom,"%s%03d.%s",seq,init+t,type);break;
	}
	case 4:{
	  sprintf(nom,"%s%04d.%s",seq,init+t,type);break;
	}
	default:{
	  sprintf(nom,"%s%d0%s",seq,init+t,type);break;
	}
	}
Image[t].assign(nom);
}*/





  CImgList<bool> Objet_label(nb_objet,nbc,nbl,1,1,0);
  CImgList<bool> Objet_courant(nb_objet+1,nbc,nbl,1,1);
  CImgList<bool> Objet_courant_visible(nb_objet+1,nbc,nbl,1,1);

//   CImgList<double> Objet_vitesse_precedant(nb_objet,2,1,1,1);
  CImgList<double> Objet_vitesse(nb_objet,2,1,1,1,0.0);
  CImgList<int> Objet_vitesse_approx(nb_objet,2,1,1,1,0);

  CImgList<double> Objet_histo(nb_objet+1,16,16,16,1);
 for(int o=0;o<nb_objet+1;o++)
Objet_histo[o].fill(1.0);

  CImg<double> Nb_histo(nb_objet+1,1,1,1,0.0);
 for(int o=0;o<nb_objet+1;o++)
Nb_histo(o)=16.0*16.0*16.0;


  CImgList<double> Objet_pos_moyenne(nb_objet,2,1,1,1);
  CImgList<double> Objet_pos_moyenne_prec(nb_objet,2,1,1,1);
 for(int o=0;o<nb_objet;o++)
Objet_vitesse[o].fill(0.0);

  CImgList<double> Carte_vitesse(nb_objet,nbc,nbl,2,1,0.0);
  CImg<double> Carte_vitesse_temp(nbc,nbl,2,1);
  //Initialization


Im_res=Im_precedante;

  for(int o=0;o<nb_objet;o++)
    {
    Objet_precedant[o].fill(0);
    if(!rep)
    {
    if(ini)
	{
	sprintf(nom,"%s%d.%s",seq_init,o+1,type_init);

CImg<int> tp_new(nom);  
       
      for(int j=0;j<nbl;j++)    
	for(int i=0;i<nbc;i++)
    if(tp_new(i,j,0,0)!=0)
Objet_precedant[o](i,j)=1; 



	}
    else
   	initialisation(Im_precedante,Objet_precedant[o]);

    sprintf(nom,"Init_%02d.png",o);
   ( Objet_precedant[o]*255).save(nom);

    }
   else
   {
   sprintf(nom,"Init_%02d.png",o);

Objet_precedant[o].assign(nom);




   }




Objet_pos_moyenne_prec[o](0)=0.0;
Objet_pos_moyenne_prec[o](1)=0.0;
int comp=0;

      for(int j=0;j<nbl;j++)    //Definition du fond
	for(int i=0;i<nbc;i++)
             if(Objet_precedant[o](i,j)==1)
		{
		comp++;
		Objet_pos_moyenne_prec[o](0)+=i;
		Objet_pos_moyenne_prec[o](1)+=j;
		}

Objet_pos_moyenne_prec[o](0)=Objet_pos_moyenne_prec[o](0)/comp;
Objet_pos_moyenne_prec[o](1)=Objet_pos_moyenne_prec[o](1)/comp;


	  for(int k=0;k<3;k++)
	    color_temp[k]=colore(o,k);

	  for(int j=0;j<nbl;j++)
	    for(int i=0;i<nbc;i++)
	      {

		if(Objet_precedant[o](i,j)==1)
		  Im_res.draw_point(i,j,color_temp,0.75);

	      }
	

     Objet_precedant_visible[o]=Objet_precedant[o];




      for(int j=0;j<nbl;j++)    //Definition du fond
	for(int i=0;i<nbc;i++)
	  if(Objet_precedant_visible[o](i,j)==1)
	    Objet_precedant_visible[nb_objet](i,j)=0;
    }




	  sprintf(nom,"%s/Init.png",resu);

      Im_res.save(nom);

  //Graph

  GraphType *g = new GraphType(/*estimated # of nodes*/ (nb_objet+1)*nbc*nbl, /*estimated # of edges*/ 2*nbc*nbl-nbc-nbl+nb_objet*nbc*nbl); 

  Objet_distance_prediction_visible[nb_objet].fill(0.0);
  Objet_distance_prediction[nb_objet].fill(0.0);


time_t seconds;
time_t seconds2;
  seconds = time (NULL);

int t=1;
while (t<taille && nb_pixels_tot>0)
  //For each image of the sequence
    {
    printf("\rImage %3d/%3d",t+1,taille);fflush(0);
      //Load the new image
   CImg<bool> Carte_courante(nbc,nbl,1,1,0);

	//Im_courante=Image[t];
		switch(format)
		{
			case 2:{
				sprintf(nom,"%s%02d.%s",seq,init+t,type);break;
			}
			case 3:{
				sprintf(nom,"%s%03d.%s",seq,init+t,type);break;
			}
			case 4:{
				sprintf(nom,"%s%04d.%s",seq,init+t,type);break;
			}
			default:{
				sprintf(nom,"%s%d0%s",seq,init+t,type);break;
			}
		}
		Im_courante.assign(nom);



      for(int j=0;j<nbl;j++)
	for(int i=0;i<nbc;i++)
	  {
	    if(i+1<nbc)
	      {edge_poids_courant(i,j,0)=r*exp(0.0-(absolue(Im_courante(i+1,j,0,0)-Im_courante(i,j,0,0))+absolue(Im_courante(i+1,j,0,1)-Im_courante(i,j,0,1))+absolue(Im_courante(i+1,j,0,2)-Im_courante(i,j,0,2)))/sigma_grad);
              if(j+1<nbl)
		edge_poids_courant(i,j,2)=r*exp(0.0-(absolue(Im_courante(i+1,j+1,0,0)-Im_courante(i,j,0,0))+absolue(Im_courante(i+1,j+1,0,1)-Im_courante(i,j,0,1))+absolue(Im_courante(i+1,j+1,0,2)-Im_courante(i,j,0,2)))/sigma_grad);
              if(j>0)
		edge_poids_courant(i,j,3)=r*exp(0.0-(absolue(Im_courante(i+1,j-1,0,0)-Im_courante(i,j,0,0))+absolue(Im_courante(i+1,j-1,0,1)-Im_courante(i,j,0,1))+absolue(Im_courante(i+1,j-1,0,2)-Im_courante(i,j,0,2)))/sigma_grad);
	      	      
              }
		

	    if(j+1<nbl)
	      edge_poids_courant(i,j,1)=r*exp(0.0-(absolue(Im_courante(i,j+1,0,0)-Im_courante(i,j,0,0))+absolue(Im_courante(i,j+1,0,1)-Im_courante(i,j,0,1))+absolue(Im_courante(i,j+1,0,2)-Im_courante(i,j,0,2)))/sigma_grad);

	  }



      Objet_precedant[nb_objet].fill(1);
      Objet_precedant_visible[nb_objet].fill(1);

      for(int o=0;o<nb_objet;o++)if(nb_pixels(o)>0)
	for(int j=0;j<nbl;j++)
	  for(int i=0;i<nbc;i++)
	    if(Objet_precedant[o](i,j)==1) 
	      Objet_precedant_visible[nb_objet](i,j)=0;  

      //Mise a jour de l'histogramme du fond
      if(t==1 ||t%maj==0)
        estim_histo(Objet_histo[nb_objet],Nb_histo(nb_objet),Objet_precedant_visible[nb_objet],Im_precedante);

        CImg<bool>  corner(nbc,nbl,1,1,0);
        harris_detect(init+t-1,corner,seq,type,format,bord);

      for(int o=0;o<nb_objet;o++) if(nb_pixels(o)>0)//For each object
	{
  
  
	  Im_courante_gris=RGB2gris(Im_courante);
	  //Mise a jour de l'histogramme de l'objet
           if(t==1 ||t%maj==0) if(nb_pixels_visible(o)>0){
	    estim_histo(Objet_histo[o],Nb_histo(o),Objet_precedant_visible[o],Im_precedante);}
 

	  Im_courante_gris  = normalize?Im_courante_gris.get_blur(0.5).equalize_histogram():Im_courante_gris.get_blur(0.5);

          CImg<bool> Objet_temp(nbc,nbl,1,1,0);




	  Objet_prediction.fill(0);
	  Objet_prediction_visible.fill(0);

	  beta[o].fill(bet);






		opflow_atpoints(Im_courante_gris,Im_precedante_gris,Objet_precedant_visible[o],corner,Carte_vitesse[o],bord_opt,seuil_max,niv,fenetre);


 		for(int j=0;j<nbl;j++)
 		  for(int i=0;i<nbc;i++)
 		    if( corner(i,j) == 1   && Objet_precedant_visible[o](i,j)==1)  
			{
			Objet_temp(i,j)=1;

			}

		Carte_vitesse_temp=Carte_vitesse[o];


                int cpt=0;
 		for(int j=0;j<nbl;j++)
 		  for(int i=0;i<nbc;i++)
 		    if(Objet_temp(i,j) == 1)
 			cpt++;
        
         if(cpt>1)
	  if(dynamique) //Si consistance dynamique
	    	{
		if(t>1)
 		for(int j=0;j<nbl;j++)
 		  for(int i=0;i<nbc;i++)
 		    if(Objet_temp(i,j) == 1)
 		        {
			double tempuu=/*sqrt*/(Carte_vitesse[o](i,j,0)-Objet_vitesse[o](0))*(Carte_vitesse[o](i,j,0)-Objet_vitesse[o](0));
			double tempvv=/*sqrt*/(Carte_vitesse[o](i,j,1)-Objet_vitesse[o](1))*(Carte_vitesse[o](i,j,1)-Objet_vitesse[o](1));
			double temp=(tempuu+tempvv)/(sigma_objet(o,0)+sigma_objet(o,1));

              		double proba=(double)Objet_histo[o]((int)Im_precedante(i,j,0,0)/16,(int)Im_precedante(i,j,0,1)/16,(int)Im_precedante(i,j,0,2)/16);
			if(filtrage){
                        double probatot=proba;
                        for(int o2=0;o2<nb_objet+1;o2++)if (o2!=o)
                           probatot+=(double)Objet_histo[o2]((int)Im_precedante(i,j,0,0)/16,(int)Im_precedante(i,j,0,1)/16,(int)Im_precedante(i,j,0,2)/16);
                        proba=Max(confiance,proba/*/probatot*/*exp(0.0-temp/*/sigma2*/));}

              		Carte_vitesse[o](i,j,0)=(1.0-proba)*Objet_vitesse[o](0)+(proba)*Carte_vitesse[o](i,j,0);
              		Carte_vitesse[o](i,j,1)=(1.0-proba)*Objet_vitesse[o](1)+(proba)*Carte_vitesse[o](i,j,1);

 		        }
		}

 		double tempu=0.0;
 		double tempv=0.0;
 		double tempu2=0.0;
 		double tempv2=0.0;
		if(cpt>1)
		  {
 		for(int j=0;j<nbl;j++)
 		  for(int i=0;i<nbc;i++)
 		    if(Objet_temp(i,j) == 1)
 		      {
 			
 			tempu+=Carte_vitesse[o](i,j,0);
			tempv+=Carte_vitesse[o](i,j,1);

 		      }

 		    Objet_vitesse[o](0)=tempu/cpt;
 		    Objet_vitesse[o](1)=tempv/cpt;
 		}

		if(cpt>1)
		for(int j=0;j<nbl;j++)
		for(int i=0;i<nbc;i++)
		   {
                   if(Objet_temp(i,j) == 1)
                            {

			double tempuu=/*sqrt*/(Carte_vitesse[o](i,j,0)-Objet_vitesse[o](0))*(Carte_vitesse[o](i,j,0)-Objet_vitesse[o](0));
			double tempvv=/*sqrt*/(Carte_vitesse[o](i,j,1)-Objet_vitesse[o](1))*(Carte_vitesse[o](i,j,1)-Objet_vitesse[o](1));
			double temp=(tempuu+tempvv)/(sigma_objet(o,0)+sigma_objet(o,1));



 			    beta[o](0,i,j)+=1.0/(temp/*/sigma*/+0.001);

			    temp=pow(temp,pp)*exp(0.0-temp/*/sigma2*/);

			    beta[o](0,i,j)-=temp/*-(Carte_vitesse_temp(i,j,0)*Objet_vitesse[o](0)+Carte_vitesse_temp(i,j,1)-Objet_vitesse[o](1))*temp*//*-temp*temp*/;



                            for(int o2=1;o2<nb_objet+1;o2++)
			    beta[o](o2,i,j)=beta[o](0,i,j);


                            }
		    
		    }
		



		if(cpt>1)
		  {
 


 		
		for(int j=0;j<nbl;j++)
 		  for(int i=0;i<nbc;i++)
 		    if(Objet_temp(i,j) == 1)
 		      { 

 			tempu2+=pow(Carte_vitesse[o](i,j,0)-Objet_vitesse[o](0),2);
			tempv2+=pow(Carte_vitesse[o](i,j,1)-Objet_vitesse[o](1),2);

 		  }

                    sigma_objet(o,0)=/*Min(sigma,tempu2/cpt)*/sigma/2.0;
                    sigma_objet(o,1)=/*Min(sigma,tempv2/cpt)*/sigma/2.0;

                 }


  		for(int i=0;i<2;i++)
                     Objet_vitesse_approx[o](i)=(int)round(Objet_vitesse[o](i));



             if(nb_pixels(o)>0)
	      for(int j=0;j<nbl;j++)
		for(int i=0;i<nbc;i++)
                  {


		  if(Objet_precedant[o](i,j)==1) 
		    {
		      if((i+Objet_vitesse_approx[o](0)<nbc)&& (i+Objet_vitesse_approx[o](0)>=0)&&(j+Objet_vitesse_approx[o](1)<nbl)&&(j+Objet_vitesse_approx[o](1)>=0))
			Objet_prediction(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))=1;

			else
		          {Objet_precedant[o](i,j)=0;Objet_precedant_visible[o](i,j)=0;}	      

		    }
		  }

             if(nb_pixels_visible(o)>0)Objet_distance_prediction_visible[o].fill(1.0);
	      for(int j=0;j<nbl;j++)
		for(int i=0;i<nbc;i++)
		  if(Objet_precedant_visible[o](i,j)==1) 
		    {
		      if((i+Objet_vitesse_approx[o](0)<nbc)&& (i+Objet_vitesse_approx[o](0)>=0)&&(j+Objet_vitesse_approx[o](1)<nbl)&&(j+Objet_vitesse_approx[o](1)>=0))
			Objet_prediction_visible(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))=1;

		    }
                  





 if(nb_pixels(o)>0)
	  distance(Objet_prediction,Objet_distance_prediction[o],1.0);
	else         
	  Objet_distance_prediction[o].fill(1.0);


        if(nb_pixels_visible(o)>0)
 	  distance3(Objet_prediction_visible,Objet_distance_prediction_visible[o],1.0);
	else         
	  Objet_distance_prediction[o].fill(1.0);




 if(nb_pixels(o)>0)
  for(int j=0;j<nbl;j++)
	for(int i=0;i<nbc;i++)
		if(Objet_distance_prediction[o](i,j)<=20) Carte_courante(i,j)=1;

      for(int j=0;j<nbl;j++)
	for(int i=0;i<nbc;i++)
	{
      for(int o2=0;o2<nb_objet+1;o2++)
       alpha[o](o2,i,j)=/*trunk*/(Objet_distance_prediction_visible[o](i,j)*alph);


	}



	  Objet_courant[o].fill(0);
	  Objet_courant_visible[o].fill(0);

	}

	  Objet_courant[nb_objet].fill(1);
	  Objet_courant_visible[nb_objet].fill(1);
      
	
      Objet_precedant[nb_objet]=Objet_precedant_visible[nb_objet];

      Im_label.fill(nb_objet);  //Assign the background label to each pixel

      int iter=0;
      double energy=10.0*nbc*nbl;
      double energy_current=energy-1.0;
      while(energy_current<energy && iter<nb_objet)//cycles of a-expenasion
	{

	
	  if(iter>0)
	    energy=energy_current;

	  iter++;
	  int maxi=nb_objet;
	  for(int l=0;l<maxi;l++) if(nb_pixels(l)>0)    //Iteration of labels: nb_ojet + fond
	    {
	      g -> reset();

	      double tmp=0.0;
// 	      pos_image.fill(0);
	      for(int o=0;o<nb_objet;o++)
		pos_objet[o].fill(0);

	      int compteur=0;

	      for(int j=0;j<nbl;j++)
		for(int i=0;i<nbc;i++)if(Carte_courante(i,j)==1)
		  {
		    g -> add_node();    //Node of the current image
		    pos_image(i,j)=compteur;

		    g -> add_tweights( pos_image(i,j), gamma*Objet_distance_prediction_visible[l](i,j)-(double)log(Objet_histo[l]((int)Im_courante(i,j,0,0)/16,(int)Im_courante(i,j,0,1)/16,(int)Im_courante(i,j,0,2)/16)) , gamma*Objet_distance_prediction_visible[Im_label(i,j)](i,j)-(double)log(Objet_histo[Im_label(i,j)]((int)Im_courante(i,j,0,0)/16,(int)Im_courante(i,j,0,1)/16,(int)Im_courante(i,j,0,2)/16)));


		    compteur++;
		  }

	      for(int j=0;j<nbl;j++)
		for(int i=0;i<nbc;i++)
		  {
		    for(int o=0;o<nb_objet;o++) if(nb_pixels(o)>0)//For each object
		      if(Objet_precedant[o](i,j)==1)
			if((i+Objet_vitesse_approx[o](0)<nbc)&& (i+Objet_vitesse_approx[o](0)>=0)&&(j+Objet_vitesse_approx[o](1)<nbl)&&(j+Objet_vitesse_approx[o](1)>=0) && Carte_courante(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))==1)
			  {
			    g -> add_node();
			    pos_objet[o](i,j)=compteur;
                            
			    if(Objet_label[o](i,j)==1)
			      {
				if(l==o)
				  { 
				    if(Objet_courant_visible[o](i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))==1/*o*/)
				      {
					if(Objet_precedant_visible[o](i,j)==1)
					    g -> add_tweights(compteur,0.0-alph2,0.0-alph2);
// 					  
				      }
				    else
				      {
					if(Objet_precedant_visible[o](i,j)==1)
					    g -> add_tweights(pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),0.0-alph2,alpha[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)));
					else
				    g -> add_tweights(pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),0.0,alpha[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))-alph2);

				      }
				  }
				else
				  { 
				  if(Objet_courant_visible[o](i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))==1/*o*/)
				      {
					if(Objet_precedant_visible[o](i,j)==1)
					  {

					    g -> add_tweights(compteur,beta[o](l,i,j),0.0);
					    g -> add_tweights(pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),0.0,0.0-alph2);/*printf("3a\n");*/
					    g -> add_edge(compteur,pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),alpha[o](l,i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),50000000.0);
					  } 
					else
					  {

					    g -> add_tweights(compteur,beta[o](l,i,j),0.0);
					    g -> add_edge(compteur,pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),alpha[o](l,i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))-alph2,50000000.0);
					  } 
				      }
				    else
				      {

					double c1=beta[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i,j)-alpha[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1));
					if(Objet_precedant_visible[o](i,j)==0) c1+=alph2;
					double c2=beta[o](l,i,j)-beta[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i,j);
					    


  					    
					if(c1>=0.)
					    {
					    g -> add_tweights(compteur,c1,0.0); 
					    if(c2>=0.)
						{g -> add_tweights(pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),c2,0.0); 
			
						tmp+=alpha[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1));
						if(Objet_precedant_visible[o](i,j)==0) tmp-=alph2;
						}
					    else
						{g -> add_tweights(pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),0.0,0.0-c2); 
// 						if(Objet_precedant_visible[o](i,j)==1)
						tmp+=alpha[o](l,i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1));
						if(Objet_precedant_visible[o](i,j)==0) tmp-=alph2;
						}
				            }
					else
					    {
					    g -> add_tweights(compteur,0.0,0.0-c1); 
					    if(c2>=0.)
						{g -> add_tweights(pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),c2,0.0); 
						tmp+=beta[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i,j);
						}
					    else
						{g -> add_tweights(pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),0.0,0.0-c2); 
						tmp+=beta[o](l,i,j);
						}
					    }
					  
				      } 
				  } 
			      }
			    else
			      {
			      if(l==o)
				  {
					if(Objet_precedant_visible[o](i,j)==1)
					  {
					    g -> add_tweights(compteur,0.0,beta[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i,j));
					    g -> add_tweights(pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),0.0-alph2,0.0);
					    g -> add_edge(compteur,pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),50000000.0,alpha[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)));
					  } 
					else
					  {
					   
					    g -> add_tweights(compteur,0.0,beta[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i,j));
					    g -> add_edge(compteur,pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),50000000.0,alpha[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))-alph2);
					  } 
				  }
				else
				  {
				    g -> add_tweights( pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),beta[o](l,i,j),beta[o](Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)),i,j));
				  }

			      }




			    compteur++;
			  }
			else 
                          {
			  g -> add_node();
			  pos_objet[o](i,j)=compteur;
                          compteur++;
			  }
		  }  




              











//Je sais plus
//  if(2==3)
//   for(int j=0;j<nbl;j++)
// 		for(int i=0;i<nbc;i++)
// 		  if(corner(i,j)==1)
// 		    for(int o=0;o<nb_objet;o++) 
// 		      if(Objet_precedant_visible[o](i,j)==1)
// 			if((i+(int)round(Carte_vitesse[o](i,j,0))<nbc)&& (i+(int)round(Carte_vitesse[o](i,j,0))>=0)&&(j+(int)round(Carte_vitesse[o](i,j,1))<nbl)&&(j+(int)round(Carte_vitesse[o](i,j,1))>=0))
// 			  {
// 			    if(Objet_label[o](i,j)==1)
// 			      {
// 				if(l==o)
// 				  { 
// 				    if(Objet_courant_visible[o](i+(int)round(Carte_vitesse[o](i,j,0)),j+(int)round(Carte_vitesse[o](i,j,1)))==0/*o*/)
// 				       g -> add_tweights(pos_image(i+(int)round(Carte_vitesse[o](i,j,0)),j+(int)round(Carte_vitesse[o](i,j,1))),0.0,beta[o](l,i,j));
// 				  }
// 				else
// 				  { 
// 				  if(Objet_courant_visible[o](i+(int)round(Carte_vitesse[o](i,j,0)),j+(int)round(Carte_vitesse[o](i,j,1)))==1/*o*/)
// 				      {
//                                       g -> add_tweights(pos_image(i+(int)round(Carte_vitesse[o](i,j,0)),j+(int)round(Carte_vitesse[o](i,j,1))),beta[o](l,i,j),0.0);
// 				      g -> add_edge(pos_objet[o](i,j),pos_image(i+(int)round(Carte_vitesse[o](i,j,0)),j+(int)round(Carte_vitesse[o](i,j,1))),0.0,50000000.0);
// 				      } 
// 				  else
// 					{
// 					g -> add_tweights(pos_objet[o](i,j),beta[o](l,i,j),beta[o](l,i,j));
// 					} 
// 				  }
// 				
// 			      }
// 			    else
// 			      {
// 			      if(l==o)
// 				  {
//                                   g -> add_tweights(pos_image(i+(int)round(Carte_vitesse[o](i,j,0)),j+(int)round(Carte_vitesse[o](i,j,1))),0.0,beta[o](l,i,j));
// 				  g -> add_edge(pos_objet[o](i,j),pos_image(i+(int)round(Carte_vitesse[o](i,j,0)),j+(int)round(Carte_vitesse[o](i,j,1))),50000000.0,0.0);
// 				  }
// 				else
// 				  {
// 			           g -> add_tweights(pos_objet[o](i,j),beta[o](l,i,j),beta[o](l,i,j));
// 
// 				  }
// 
// 			      }
// 
// 
// 			  }
// 
// 		    






	      //Regularisation
	      if(r>0.0)
//               
                {
		cree_reg(Carte_courante,Im_label,l, edge_poids_courant,pos_image, g,1,0,0);
		cree_reg(Carte_courante,Im_label,l, edge_poids_courant,pos_image, g,0,1,1);
		cree_reg(Carte_courante,Im_label,l, edge_poids_courant,pos_image, g,1,1,2);
		cree_reg(Carte_courante,Im_label,l, edge_poids_courant,pos_image, g,1,-1,3);
		}


                if(r2>0.0)
		  for(int o=0;o<nb_objet;o++)if(nb_pixels(o)>0)
			 {
                         cree_reg_obj(Objet_precedant[o],l,o, Objet_label[o],pos_objet[o],g,1,0,r2);
                         cree_reg_obj(Objet_precedant[o],l,o, Objet_label[o],pos_objet[o],g,0,1,r2);
                         cree_reg_obj(Objet_precedant[o],l,o, Objet_label[o],pos_objet[o],g,1,1,r2);
                         cree_reg_obj(Objet_precedant[o],l,o, Objet_label[o],pos_objet[o],g,1,-1,r2);
			 }
		






	      double energy_current2 = g -> maxflow()+tmp;



	      if(energy_current2<energy_current)
		{
		  energy_current=energy_current2;


		  for(int j=0;j<nbl;j++)
		    for(int i=0;i<nbc;i++)if(Carte_courante(i,j)==1)
		      {
			if(g->what_segment(pos_image(i,j)) == GraphType::SINK)
			  {
			    Im_label(i,j)=l;
			    Objet_courant_visible[l](i,j)=1;
			    Objet_courant[l](i,j)=1;
			    for(int o=0;o<nb_objet+1;o++)
			      if(l!=o)
				Objet_courant_visible[o](i,j)=0;

			  }

		      }




		  for(int o=0;o<nb_objet;o++)if(nb_pixels(o)>0)
		    for(int j=0;j<nbl;j++)
		      for(int i=0;i<nbc;i++)
			{
			  if(Objet_precedant[o](i,j)==1)
			    if((i+Objet_vitesse_approx[o](0)<nbc)&& (i+Objet_vitesse_approx[o](0)>=0)&&(j+Objet_vitesse_approx[o](1)<nbl)&&(j+Objet_vitesse_approx[o](1)>=0) && Carte_courante(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))==1)
			      if(l==o)
				{
				  if(g->what_segment(pos_objet[o](i,j)) == GraphType::SINK)
				    {
				      Objet_courant[o](i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))=1;

				      Objet_label[o](i,j)=1;
				    }
				  else
				    if(Objet_label[o](i,j)==1)
				      {

				      }
				}
			      else
				{
				  if(g->what_segment(pos_objet[o](i,j)) == GraphType::SINK)
				    {
				      Objet_label[o](i,j)=0;

				      Objet_courant[o](i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))=0;

				    }
				  else
                                     if(g->what_segment(pos_image(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1))) == GraphType::SINK && Objet_label[o](i,j)==1)
				            {

				            }
				}

			}


		  for(int o=0;o<nb_objet;o++)if(nb_pixels(o)>0)
		    for(int j=0;j<nbl;j++)
		      for(int i=0;i<nbc;i++)if(Objet_precedant[o](i,j)==1)
			  if(Objet_label[o](i,j)==0)
				    if((i+Objet_vitesse_approx[o](0)<nbc)&& (i+Objet_vitesse_approx[o](0)>=0)&&(j+Objet_vitesse_approx[o](1)<nbl)&&(j+Objet_vitesse_approx[o](1)>=0))

if(Im_label(i+Objet_vitesse_approx[o](0),j+Objet_vitesse_approx[o](1)) == o)
printf("Error\n");

		}





	    }
	}





      for(int o=0;o<nb_objet;o++)if(nb_pixels(o)>0)
	{
	  Objet_precedant[o]=Objet_courant[o];
	  Objet_precedant_visible[o]=Objet_courant_visible[o];
	  Objet_label[o].fill(0);
	}
      

      Im_precedante_gris=Im_courante_gris;
      Im_precedante=Im_courante;
      Im_res=Im_courante;


{
if(occ){
      for(int o=0;o<nb_objet;o++)if(nb_pixels(o)>0)
	{
	  for(int k=0;k<3;k++)
	    color_temp[k]=colore(o,k);

	  for(int j=0;j<nbl;j++)
	    for(int i=0;i<nbc;i++)
	      {

		if(Objet_courant_visible[o](i,j)==1)
		  Im_courante.draw_point(i,j,color_temp,0.75);
		else
      		  if(Objet_courant[o](i,j)==1)
		    Im_courante.draw_point(i,j,color_temp,0.2);

	      }
	}

      switch(format)
	{
	case 2:{
	  sprintf(nom,"%s/Occ/res_%.2d.png",resu,init+t);break;
	}
	case 3:{
	  sprintf(nom,"%s/Occ/res_%.3d.png",resu,init+t);break;
	}
	case 4:{
	  sprintf(nom,"%s/Occ/res_%.4d.png",resu,init+t);break;
	}
	default:{
	  sprintf(nom,"%s/Occ/res_%d.png",resu,init+t);break;
	}
	}

      Im_courante.save(nom);

}

 for(int o=0;o<nb_objet;o++)if(nb_pixels(o)>0)
	{
	  for(int k=0;k<3;k++)
	    color_temp[k]=colore(o,k);

	  for(int j=0;j<nbl;j++)
	    for(int i=0;i<nbc;i++)
	      {

		if(Objet_courant_visible[o](i,j)==1)
		  Im_res.draw_point(i,j,color_temp,0.75);

	      }
	}

      switch(format)
	{
	case 2:{
	  sprintf(nom,"%s/res_%.2d.png",resu,init+t);break;
	}
	case 3:{
	  sprintf(nom,"%s/res_%.3d.png",resu,init+t);break;
	}
	case 4:{
	  sprintf(nom,"%s/res_%.4d.png",resu,init+t);break;
	}
	default:{
	  sprintf(nom,"%s/res_%d.png",resu,init+t);break;
	}
	}

	
	
      Im_res.save(nom);
}



for(int o=0;o<nb_objet;o++)if(nb_pixels(o)>0)
  {

Objet_pos_moyenne[o](0)=0.0;
Objet_pos_moyenne[o](1)=0.0;
   nb_pixels_visible(o)=0;
   nb_pixels(o)=0;
   for(int j=0;j<nbl;j++)
    for(int i=0;i<nbc;i++)
     {
     if(Objet_courant[o](i,j)==1)
		{
		nb_pixels(o)++;
		Objet_pos_moyenne[o](0)+=i;
		Objet_pos_moyenne[o](1)+=j;
		}

     if(Objet_courant_visible[o](i,j)==1)
		{
		nb_pixels_visible(o)++;
		}

     }


if(t==1)
if(nb_pixels(o)>0)
{
Objet_pos_moyenne[o](0)=Objet_pos_moyenne[o](0)/nb_pixels(o);
Objet_pos_moyenne[o](1)=Objet_pos_moyenne[o](1)/nb_pixels(o);
Objet_vitesse[o](0)=(Objet_vitesse[o](0)+Objet_pos_moyenne[o](0)-Objet_pos_moyenne_prec[o](0))/2.0;
Objet_vitesse[o](1)=(Objet_vitesse[o](1)+Objet_pos_moyenne[o](1)-Objet_pos_moyenne_prec[o](1))/2.0;
Objet_pos_moyenne_prec[o]=Objet_pos_moyenne[o];


}



 }


    nb_pixels_tot=0;
   for(int o=0;o<nb_objet;o++)
   nb_pixels_tot+=nb_pixels(o);



    t++;
    }


seconds2 = time (NULL);
printf("\tTemps: %ld secondes, frame/rate= %f\n",seconds2-seconds,(double)t/(double)(seconds2-seconds));fflush(0);


printf("\n");

  return 0;
}

