#include "functions_tracking.h"
using namespace cimg_library;
int absolue(int a)
{
  if(a<0) return 0-a;
  else
    return a;
}
double Min(double a, double b)
{
  double res=a;
  if(b<res)
    res=b;


  return res;
}
double Max(double a, double b)
{
  double res=a;
  if(b>res)
    res=b;


  return res;
}
double Max(double a, double b, double c)
{
  double res=a;
  if(b>res)
    res=b;
  if(c>res)
    res=c;

  return res;
}

double Min(double a, double b, double c)
{
  double res=a;
  if(b<res)
    res=b;
  if(c<res)
    res=c;

  return res;
}

int Min(int a, int b)
{
  int res=a;
  if(b<res)
    res=b;

  return res;
}


int Max(int a, int b)
{
  int res=a;
  if(b>res)
    res=b;

  return res;
}


double absolue(double a)
{
  if(a<0) return 0-a;
  else
    return a;
}

double absolued(double a)
{
  if(a<0) return 0-a;
  else
    return a;

  // return 1.0;

}

double absolued(int a)
{
  if(a<0) return -(double)a;
   else return (double)a;

  // return 1.0;

}





void estim_histo(CImg<double> &histo2,double &nb_histo,  const CImg<bool> &Objet_precedant_visible,const CImg<int> &Im_precedante)
{
  CImg<double>  histo(16,16,16,1,1.0);
//   histo=histo/*+histo2*nb_histo*/;
  int nbc=Objet_precedant_visible.width;
  int nbl=Objet_precedant_visible.height;


  nb_histo=16*16*16/*+nb_histo*/;

  for(int j=0;j<nbl;j++)
    for(int i=0;i<nbc;i++)
      for(int k=0;k<3;k++)
	if(Objet_precedant_visible(i,j)==1 )
	  {
	    histo((int)Im_precedante(i,j,0,0)/16,(int)Im_precedante(i,j,0,1)/16,(int)Im_precedante(i,j,0,2)/16)+=1.0;
	    nb_histo+=1.0;
	  }

//   if(cpt!=0.0)



    histo=histo/nb_histo;
  for(int i=0;i<16;i++)    for(int i2=0;i2<16;i2++)    for(int i3=0;i3<16;i3++)
 histo2(i,i2,i3)=((histo(i,i2,i3)+histo2(i,i2,i3))/2.0);

histo2=histo;
//   for(int i=0;i<16;i++)    for(int i2=0;i2<16;i2++)    for(int i3=0;i3<16;i3++)
//  histo(i,i2,i3)=1.0/((1.0/histo(i,i2,i3)+1.0/histo2(i,i2,i3))/2.0);








}

CImg<double>  RGB2gris(CImg<double>  A)
{

  int nbc=A.width;
  int nbl=A.height;
  CImg<double>  B(nbc,nbl,1,1,0.0);

  for(int j=0;j<nbl;j++)
    for(int i=0;i<nbc;i++)
      for(int k=0;k<3;k++)
	B(i,j)+=A(i,j,0,k);

  return B/3.0;


}






void distance(const CImg<double> &in,CImg<double> &D, double d)
{


  int nbc=in.width;
  int nbl=in.height;

  D.fill(2550.0*2550.0);
  for(int j=0;j<nbl;j++)
    for(int i=0;i<nbc;i++)
      if(in(i,j)==1)
	D(i,j)=0;





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


	  if(i>0)
	    if(D(i-1,j)+1.0<D(i,j))
	      D(i,j)=D(i-1,j)+1.0;


	  if(j>0)
	    if(D(i,j-1)+1.0<D(i,j))
	      D(i,j)=D(i,j-1)+1.0;


	  if(i>0 && j>0)
	    if(D(i-1,j-1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i-1,j-1)+sqrt(2.0);


	  if(i<nbc-1 && j>0)
	    if(D(i+1,j-1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i+1,j-1)+sqrt(2.0);


	}



      for(int i=nbc-2;i>=0;i--) //Masque 2
	if(D(i+1,j)+1.0<D(i,j))
	  D(i,j)=D(i+1,j)+1.0;



    }


  for(int j=nbl-1;j>=0;j--)
    {
      for(int i=nbc-1;i>=0;i--)   //Masque 3
	{

	  if(i<nbc-1)
	    if(D(i+1,j)+1.0<D(i,j))
	      D(i,j)=D(i+1,j)+1.0;


	  if(j<nbl-1)
	    if(D(i,j+1)+1.0<D(i,j))
	      D(i,j)=D(i,j+1)+1.0;



	  if(i<nbc-1 && j<nbl-1)
	    if(D(i+1,j+1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i+1,j+1)+sqrt(2.0);


	  if(i>0 && j<nbl-1)
	    if(D(i-1,j+1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i-1,j+1)+sqrt(2.0);


	}


      for(int i=1;i<nbc;i++) //Masque 4
	if(D(i-1,j)+1.0<D(i,j))
	  D(i,j)=D(i-1,j)+1.0;


    }


  for(int j=0;j<nbl;j++)
    for(int i=0;i<nbc;i++)
      if(D(i,j)<=0.0)
	D(i,j)=0.0;
      else
	D(i,j)=1.0+D(i,j)/d;


}



void distance3(const CImg<double> &in,CImg<double> &D, double d)
{


  int nbc=in.width;
  int nbl=in.height;

  D.fill(2550.0*2550.0);
  for(int j=0;j<nbl;j++)
    for(int i=0;i<nbc;i++)
      if(in(i,j)==1)
	D(i,j)=0;





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


	  if(i>0)
	    if(D(i-1,j)+1.0<D(i,j))
	      D(i,j)=D(i-1,j)+1.0;


	  if(j>0)
	    if(D(i,j-1)+1.0<D(i,j))
	      D(i,j)=D(i,j-1)+1.0;


	  if(i>0 && j>0)
	    if(D(i-1,j-1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i-1,j-1)+sqrt(2.0);


	  if(i<nbc-1 && j>0)
	    if(D(i+1,j-1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i+1,j-1)+sqrt(2.0);


	}



      for(int i=nbc-2;i>=0;i--) //Masque 2
	if(D(i+1,j)+1.0<D(i,j))
	  D(i,j)=D(i+1,j)+1.0;



    }


  for(int j=nbl-1;j>=0;j--)
    {
      for(int i=nbc-1;i>=0;i--)   //Masque 3
	{

	  if(i<nbc-1)
	    if(D(i+1,j)+1.0<D(i,j))
	      D(i,j)=D(i+1,j)+1.0;


	  if(j<nbl-1)
	    if(D(i,j+1)+1.0<D(i,j))
	      D(i,j)=D(i,j+1)+1.0;



	  if(i<nbc-1 && j<nbl-1)
	    if(D(i+1,j+1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i+1,j+1)+sqrt(2.0);


	  if(i>0 && j<nbl-1)
	    if(D(i-1,j+1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i-1,j+1)+sqrt(2.0);


	}


      for(int i=1;i<nbc;i++) //Masque 4
	if(D(i-1,j)+1.0<D(i,j))
	  D(i,j)=D(i-1,j)+1.0;


    }


  for(int j=0;j<nbl;j++)
    for(int i=0;i<nbc;i++)
//       if(D(i,j)<=0.0)
	D(i,j)=1.0+absolue(D(i,j))/d;
//       else
// 	D(i,j)=1.0+D(i,j)/d;


}





void distance2(CImg<double> &D,const  CImg<bool> &in)
{


  int nbc=in.width;
  int nbl=in.height;

  D.fill(2550.0*2550.0);
  for(int j=0;j<nbl;j++)
    for(int i=0;i<nbc;i++)
      if(in(i,j)==1)
	D(i,j)=0;





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


	  if(i>0)
	    if(D(i-1,j)+1.0<D(i,j))
	      D(i,j)=D(i-1,j)+1.0;


	  if(j>0)
	    if(D(i,j-1)+1.0<D(i,j))
	      D(i,j)=D(i,j-1)+1.0;


	  if(i>0 && j>0)
	    if(D(i-1,j-1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i-1,j-1)+sqrt(2.0);


	  if(i<nbc-1 && j>0)
	    if(D(i+1,j-1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i+1,j-1)+sqrt(2.0);


	}



      for(int i=nbc-2;i>=0;i--) //Masque 2
	if(D(i+1,j)+1.0<D(i,j))
	  D(i,j)=D(i+1,j)+1.0;



    }


  for(int j=nbl-1;j>=0;j--)
    {
      for(int i=nbc-1;i>=0;i--)   //Masque 3
	{

	  if(i<nbc-1)
	    if(D(i+1,j)+1.0<D(i,j))
	      D(i,j)=D(i+1,j)+1.0;


	  if(j<nbl-1)
	    if(D(i,j+1)+1.0<D(i,j))
	      D(i,j)=D(i,j+1)+1.0;



	  if(i<nbc-1 && j<nbl-1)
	    if(D(i+1,j+1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i+1,j+1)+sqrt(2.0);


	  if(i>0 && j<nbl-1)
	    if(D(i-1,j+1)+sqrt(2.0)<D(i,j))
	      D(i,j)=D(i-1,j+1)+sqrt(2.0);


	}


      for(int i=1;i<nbc;i++) //Masque 4
	if(D(i-1,j)+1.0<D(i,j))
	  D(i,j)=D(i-1,j)+1.0;


    }


 


}




/*-----------------------------------------------------------

Compute optical flow for one scale
( semi-implicite scheme )

----------------------------------------------------------*/
CImg<double> optmonoflow(const CImg<>& I1,const CImg<>& I2,const CImg<>& u0, const double smooth, const double precision,const CImg<double>& m) {
  double tmpf,X,Y,deltaI,dt=2,E=1e100,Eold;
  //   CImg_3x3(I,double);
  //   int white = 255;
  CImg<double> u = u0.get_resize(I1.width,I1.height,1,2,3),dI1(u);
  int nbc=I1.width;
  int nbl=I1.height;
  // first derivatives of I1
  for(int y=1;y<nbl-1;y++)for(int x=1;x<nbc-1;x++)
    if(m(x,y)!=0)if(m(x,y)!=0.0) if(m(x+1,y)!=0.0 && m(x-1,y)!=0.0 && m(x,y+1)!=0.0 && m(x,y-1)!=0.0)
      {
	dI1(x,y,0) = 0.5*(I1(x+1,y)-I1(x-1,y));
	dI1(x,y,1) = 0.5*(I1(x,y+1)-I1(x,y-1));
      }
  
  // Main PDE iterations
  for (int iter=0; iter<100; iter++) {
    fprintf(stderr,"\r- Iteration %d - E = %g",iter,E); fflush(stderr);
    Eold = E; E = 0;
    for(int y=1;y<nbl-1;y++)for(int x=1;x<nbc-1;x++) if(m(x,y)!=0.0) if(m(x+1,y)!=0.0 && m(x-1,y)!=0.0 && m(x,y+1)!=0.0 && m(x,y-1)!=0.0){
      X = x + u(x,y,0,0);
      Y = y + u(x,y,0,1);      
      deltaI = I1.at2(X,Y) - I2(x,y);
      tmpf = 0;
      for(int k=0;k<2;k++) {
	const double
	  ux  = 0.5*(u(x+1,y,0,k)-u(x-1,y,0,k)),
	  uy  = 0.5*(u(x,y+1,0,k)-u(x,y-1,0,k));
	
	u(x,y,0,k) = ( u(x,y,0,k) +
		       dt*(
			   -deltaI*dI1.at2(X,Y,k) +
			   smooth* ( u(x+1,y,0,k) + u(x-1,y,0,k) + u(x,y+1,0,k) + u(x,y-1,0,k) )
			   )	      
		       )/(1+4*smooth*dt);
	tmpf += ux*ux + uy*uy;
      }
      E += deltaI*deltaI + smooth * tmpf;
    }
    if (fabs(Eold-E)<precision) break;
    if (Eold<E) dt*=0.5;

  }
  return u;
}

/*------------------------------------------
	
Compute multi-scale optical flow
	
---------------------------------------*/
CImg<double> optflow(const CImg<>& xsrc,const CImg<>& xdest,const double smooth,const double precision,const int pnb_scale,const CImg<bool>& masque) {
  CImg<> src  = xsrc.get_norm_pointwise(1).resize(xdest.width,xdest.height,1,1,3).normalize(0,1),
    dest = xdest.get_norm_pointwise(1).resize(xdest.width,xdest.height,1,1,3).normalize(0,1),
    u    = CImg<double>(src.width,src.height,1,2).fill(0.0);
  



  const int nb_scale = pnb_scale>0?pnb_scale:(int)(log(Max((int)src.width,(int)src.height))/log(2))-1;
  
  for (int scale=nb_scale-1; scale>=0; scale--) {
    const CImg<> I1 = src.get_resize((int)(src.width/pow(2.0,scale)), (int)(src.height/pow(2.0,scale)) ,1,1,3);
    const CImg<> I2 = dest.get_resize((int)(src.width/pow(2.0,scale)), (int)(src.height/pow(2.0,scale)) ,1,1,3);
    CImg<double> m = masque.get_resize((int)(src.width/pow(2.0,scale)), (int)(src.height/pow(2.0,scale)) ,1,1,3);
    m.fill(1.0);
    fprintf(stderr," * Scale %d\n",scale);
    u*=2.0;
    u = optmonoflow(I2,I1,u,smooth,precision/(scale+1),m);
    //     u.display();
    putchar('\n');
  }
	
  return u;
}





void harris_detect(int t,CImg<bool>& corner,const char *seq,const char *type,const int format, int bord){
  char nom[200];
  switch(format)
    {
    case 2:{
      sprintf(nom,"%s%.2d",seq,t);break;
    }
    case 3:{
      sprintf(nom,"%s%.3d",seq,t);break;
    }
    case 4:{
      sprintf(nom,"%s%.4d",seq,t);break;
    }
    default:{
      sprintf(nom,"%s%d",seq,t);break;
    }
    }

  char* command = new char[500];

  sprintf(command,"convert %s.%s point.ppm",nom,type);
  system(command);
	
  sprintf(command,"./fast-Darwin-ppc -s -l point.ppm point.txt");
  
	//sleep(0.2);
  FILE* file=fopen("point.txt","rb");
  char ligne[10000];
  int num;
  float x,y;
  //Checks if the input file is empty
  if (file==NULL)
    {
      printf("le fichier d'entrée est vide\n");
    }
  else {
    while (fgets(ligne, sizeof(ligne),file))
      {
        sscanf(ligne,"%d",&num);
        sscanf(ligne,"%f %f",&x,&y);
        if((int)round(x)>bord && (int)round(x)<(int)corner.width-bord && (int)round(y)>bord && (int)round(y)<(int)corner.height-bord )
        corner((int)round(x),(int)round(y),0,0) = 1;
      }
  }
 
	fclose(file);
}



void opflow_atpoints(CImg<double> &Im_courante_gris,CImg<double>& Im_precedante_gris, CImg<bool> &Objet_precedant_visible, CImg<bool> &corner,CImg<double>& vitesse  ,double bord_opt, double seuil_max, int nbscale,int sliding) 

{
int nbc=Objet_precedant_visible.width;
int nbl=Objet_precedant_visible.height;
CImg<double> dist(nbc,nbl,1,1);

distance2(dist,Objet_precedant_visible);

  /**** GRADIENT IMAGES  ******/
  CImg<double> Gxim(Im_precedante_gris.get_gradientXY(2)[0].abs());  
  CImg<double> Gyim(Im_precedante_gris.get_gradientXY(2)[1].abs());

  /**** CONSTRUCTION DES PYRAMIDES  ******/
  CImgList<double> pyr1(nbscale,nbc,nbl,1,1,0);
  CImgList<double> pyr2(nbscale,nbc,nbl,1,1,0);
  construct_pyramid(Im_precedante_gris, pyr1, nbscale); 
  construct_pyramid(Im_courante_gris, pyr2, nbscale);
  
  /**** INITIALISATION DES VARIABLES ******/ 
 
  CImg<double> curIm1(2*sliding+1,2*sliding+1,1,1,0);
  CImg<double> curIm2(2*sliding+1,2*sliding+1,1,1,0);

  int nbpt = 0;
  for(int j=0;j<nbl;j++)
    for(int i=0;i<nbc;i++)
      if(dist(i,j)<=0.1-bord_opt && corner(i,j) == 1 ) {
	//Is the gradient not too big and not too small ?
	if ( (Gxim(i,j,0,0)>=1.0 ||  Gyim(i,j,0,0)>=1.0 )/*&& absolue(Im_precedante_gris(i,j)-Im_courante_gris(i,j))>=seuil_min*/&& absolue(Im_precedante_gris(i,j)-Im_courante_gris(i,j))<=seuil_max){// &&  (Gxim(i,j,0,0)<100 ||  Gyim(i,j,0,0)<100)){
	  nbpt++;
	  int coord_x = 0;
	  int coord_y = 0; 
	  int hc = 0;
	  int wc = 0;
	  CImg<double> vec(2);
	    
	  /**** CALCUL DU MOUVEMENT POUR CHAQUE ECHELLE DES PYRAMIDES ******/
	  for (int c=0; c<nbscale; c++) {
	    coord_x = (int)round((i)/powf(2.0,nbscale-c-1));
	    coord_y = (int)round((j)/powf(2.0,nbscale-c-1));
	    hc = (int)round((nbl)/powf(2.0,nbscale-c-1));
	    wc = (int)round((nbc)/powf(2.0,nbscale-c-1));
	    // Mise a jour des coordonnees et des mouvements pour les echelles superieures.
	    if (c >= 1) {
	      vec(0) *= 2;
	      vec(1) *= 2;
	    }
	    else {
	      vec(0) = 0;
	      vec(1) = 0;
	    }
	    for (int n=0; n<=2*sliding; n++)
	      for (int m=0; m<=2*sliding; m++) 
		curIm1(m,n,0,0) = pyr1[c](coord_x-sliding+m, coord_y-sliding+n,0,0) ;
	    
// 	    printf("     %d %d %d  %f %f\n",i,j,c,vec(0),vec(1));
	    
	    //spatial gradient matrix
	    CImg<double> dI(2,(2*sliding-1)*(2*sliding-1),1,1,0);
	    // compute first derivatives of curIm1
	    ComputeXDerivative(curIm1,sliding,dI);
	    ComputeYDerivative(curIm1,sliding,dI);

	    CImg<double> dItransp(dI.get_transpose());   
	    CImg<double> DI(dItransp*dI); 

	    double dete = DI.det();
	    
	    CImg<double> dT(1,(2*sliding-1)*(2*sliding-1),1,1,0);
	    CImg<double> U(1,2,1,1,0);
	    

	    /******* ARRET APRES 6 iterations  ******/ 
	    int r = 0;
	    while ( r<6){
	      int lowCindex, lowRindex;
	      border(lowRindex,lowCindex ,hc, wc, coord_x+vec(0),coord_y+vec(1),sliding);
	      for (int n=0; n<=2*sliding; n++)	 
		for (int m=0; m<=2*sliding; m++) 
		  curIm2(m,n,0,0) = pyr2[c](lowRindex+m, lowCindex+n ,0,0);
	 
	      double old_u = vec(0);
	      double old_v = vec(1);
	      if (dete == 0){
		vec.fill(0);
	      }
	      else{

		ComputeTimeDerivative(curIm1, curIm2,sliding, dT);
		U.assign(DI.get_invert()*(dItransp*dT));
		vec(0) = U[0];
		vec(1) = U[1];
	      }
	      
	      vec(0) += old_u;
	      vec(1) += old_v;
	      
// 	      printf("%d %d %d %d %f %f\n",i,j,c,r,vec(0),vec(1));
	      if (! ((int)round(vec(1)+coord_y) >= 0 && 
		     (int)round(vec(1)+coord_y) < hc && 
		     (int)round(coord_x+vec(0)) >= 0 && 
		     (int)round(coord_x+vec(0)) < wc) )
		r = 60;
	      
	      r++;
	    }
	  }
// 	printf("%f %f\n",vec(0),vec(1));

//          if(Objet_precedant(i,j)==0 || t==1)
//               {
              vitesse(i,j,0)=vec(0);
              vitesse(i,j,1)=vec(1);
//               }
//          else
//               {
//               double proba=(double)objet_histo((int)Im_courante(i,j,0,0)/16,(int)Im_courante(i,j,0,1)/16,(int)Im_courante(i,j,0,2)/16);
//               vitesse(i,j,0)=proba*vec(0)+(1.-proba)*vitesse(i,j,0);
//               vitesse(i,j,1)=proba*vec(1)+(1.-proba)*vitesse(i,j,1);
//               }


	}
	else
	  corner(i,j) = 0;
// 	flot.display();
	
      }
//   printf("points %d\n",nbpt);
}




/* BORDER OF THE IMAGE */
void border(int &lowx, int &lowy, int hc, int wc, double x, double y, int s){
  lowy  =  (int)round(y-s);
  lowx  =  (int)round(x-s);

  if (lowy < 0) 
    lowy = 0;  
  if (lowy+2*s >= (int)hc) 
    lowy = hc-2*s-1;
  if (lowx+2*s >= (int)wc)
    lowx = wc-2*s-1;  
  if (lowx < 0) 
    lowx = 0;
}

/* CONSTRUCT PYRAMID
   %Gaussian mask = [0.05 0.25 0.4 0.25 0.05] 
   % Apply 1d mask to alternate pixels along each row of image
   % apply 1d mask to each pixel along alternate columns of resulting image */
void construct_pyramid(CImg<double> &Im,  CImgList<double> & pyr, int nbscale) {

  CImg<double> mask(5,1,1,1,0);
  mask(0,0,0,0) = 0.05;
  mask(1,0,0,0) = 0.25;
  mask(2,0,0,0) = 0.4;
  mask(3,0,0,0) = 0.25;
  mask(4,0,0,0) = 0.05;

  CImg<double> Inter(Im);
  
   
  for (int j=0; j<nbl; j++){
    for (int i=0; i<nbc; i++) {
      pyr[nbscale-1](i,j,0,0) = Im(i,j,0,0);
    }  
  }
  for (int c=2; c<=nbscale; c++) {
    Inter.convolve(mask);
    Inter.convolve(mask.transpose());
    Inter.resize(Inter.width/2, Inter.height/2, 1, 1, 5);
    Inter.normalize(0,255);
    for (int j=0; j<(int)Inter.height; j++){
      for (int i=0; i<(int)Inter.width; i++) {
   	pyr[nbscale-c](i,j,0,0) = Inter(i,j,0,0); 
      }
    }
  }
}


/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %ComputeDerivatives	
  Compute horizontal, vertical and time derivative
  between two gray-level images.
*/

void ComputeXDerivative(CImg<double> & im1,int s, CImg<double> & dI) {

  for (int m=0; m<=2*s; m++) {
    for (int n=0; n<=2*s; n++) {  
      if (m>0 && n>0 && m<2*s && n< 2*s)  
	dI(0,(m-1)+(n-1)*(2*s-1),0,0) = 0.5*(im1(m+1,n)-im1(m-1,n));//Tempx2(m,n,0,0); 
    }
  }  
}

void ComputeYDerivative(CImg<double>  &im1, int s, CImg<double> & dI) {

  
  for (int m=0; m<=2*s; m++) {
    for (int n=0; n<=2*s; n++) {
      if (m>0 && n>0 && m<2*s && n< 2*s) 
	dI(1,(m-1)+(n-1)*(2*s-1),0,0) =0.5*(im1(m,n+1)-im1(m,n-1));// Tempy2(m,n,0,0); 
    }
  }  
}

void ComputeTimeDerivative(CImg<double> & im1,CImg<double>  &im2,  int s, CImg<double> & Ft) {

    
  for (int m=0; m<=2*s; m++) {
    for (int n=0; n<=2*s; n++) {  
      if (m>0 && n>0 && m<2*s && n< 2*s) 
	Ft(0,(m-1)+(n-1)*(2*s-1),0,0) = im1(m,n,0,0)-im2(m,n,0,0);
    }
  } 
  
}








void Interpolation_vitesse(CImg<double> &Carte_vitesse,CImg<bool> & Objet_precedant,CImg<bool> &corner,CImg<double> &vitesse_moyenne)
{

for(int i=0;i<(int)Carte_vitesse.width;i++)
for(int j=0;j<(int)Carte_vitesse.height;j++)
if(Objet_precedant(i,j)==1)
if(corner(i,j)==0)
{

double nombre=0.0;
double valeuru=0.0;
double valeurv=0.0;
int i2=0;

while(nombre<10.0 && i2<(int)Carte_vitesse.width )
{
int j2=0;
while(nombre<10.0 && j2<(int)Carte_vitesse.height)
{
// for(int i2=0;i2<(int)Carte_vitesse.width;i2++)
// for(int j2=0;j2<(int)Carte_vitesse.height;j2++)
if(Objet_precedant(i2,j2)==1)
if(corner(i2,j2)==1)
{
double coef=exp(0.0-absolue(i-i2)-absolue(j-j2));
nombre=nombre+coef;
valeuru=valeuru+Carte_vitesse(i2,j2,0)*coef;
valeurv=valeurv+Carte_vitesse(i2,j2,1)*coef;
}

j2++;
}
i2++;
}

if(nombre>0.0)
{
Carte_vitesse(i,j,0)=valeuru/nombre;
Carte_vitesse(i,j,1)=valeurv/nombre;
}
else
{
Carte_vitesse(i,j,0)=vitesse_moyenne(0);
Carte_vitesse(i,j,1)=vitesse_moyenne(1);
}


}



}







void initialisation(CImg<int> &a, CImg<bool> &b)
{
  FILE *fichier;
  int i,j,area;
  int nbl,nbc;
  
  writeIMG(a);

  system("ascii2fimage in");
  system("ll_smallest -s in out");

  fichier = fopen("pixels.txt", "r");
  fscanf(fichier, "%d %d %d\n", &nbl, &nbc, &area);
  
 

  
  
  
  
    for (int k=0 ; k<area ; k++) 
       {
       fscanf(fichier, "%d\t%d\n", &i, &j);
       b(j,i) = 1;
 
       }
  fclose(fichier);


}





void writeIMG(CImg<int> &Y)
{
  int nbl, nbc;
  FILE *fichier;

  
  nbc=Y.width;
  nbl=Y.height;
  
  fichier=fopen("ascii.txt", "w");
  

  fprintf(fichier, "%d %d\n", nbl, nbc);
  
  for (int i=1 ; i<=nbl ; i++) {
    for (int j=1 ; j<=nbc ; j++) {
      fprintf(fichier, "%d ", Y(j-1,i-1));
    }
    fprintf(fichier, "\n");
  }

  fclose(fichier);
}







double trunk(double a)
{
return round(a*100.0)/100.0;
}





void cree_reg(CImg<bool> &carte, CImg<int> &Im_label, int l, CImg<double> &edge_poids_courant,CImg<int> &pos_image, GraphType *g, int a, int b, int c)  
   {
int nbc=Im_label.width;
int nbl=Im_label.height;
// printf("%d %d %d %d %d\n",a,b,c,nbc,nbl);
int deb=0;
if(b==-1) deb=1;


		for(int j=deb;j<nbl-1;j++)
		  for(int i=0;i<nbc-1;i++)if(carte(i,j)==1 && carte(i+a,j+b)==1)
		    {
		      if(Im_label(i,j)!=l && Im_label(i+a,j+b)!=l && Im_label(i+a,j+b)!=Im_label(i,j))
			{/*if(pos_image(i,j)==pos_image(i+1,j)) printf("\n error 0a\n");*/
			  g -> add_edge(pos_image(i,j),pos_image(i+a,j+b),edge_poids_courant(i,j,c),0.0);
			  g -> add_tweights( pos_image(i+a,j+b),   0.0,  edge_poids_courant(i,j,c));
			}
		      else
			if(Im_label(i,j)!=l && Im_label(i+a,j+b)!=l)
			  {/*if(pos_image(i,j)==pos_image(i+1,j)) printf("\n error 0b\n");*/
                          g -> add_edge(pos_image(i,j),pos_image(i+a,j+b),edge_poids_courant(i,j,c),edge_poids_courant(i,j,c));}
			else
			  if(Im_label(i,j)==l && Im_label(i+a,j+b)!=l)
			    g -> add_tweights( pos_image(i+a,j+b),    0.0,  edge_poids_courant(i,j,c));
			  else
			    if(Im_label(i,j)!=l&& Im_label(i+a,j+b)==l)
			      g -> add_tweights( pos_image(i,j),    0.0,   edge_poids_courant(i,j,c));

		}


}

void cree_reg_obj(CImg<bool> &Objet_precedant, int l, int o, CImg<bool> &Objet_label,CImg<int> &pos_objet, GraphType *g, int a, int b, double r2)  
   {
int nbc=Objet_label.width;
int nbl=Objet_label.height;

int deb=0;
if(b==-1) deb=1;


		for(int j=1;j<nbl-1;j++)
		  for(int i=0;i<nbc-1;i++)
		    {
		    if(Objet_precedant(i,j)==1)
			    {
			       if(Objet_precedant(i+a,j+b)==1)
				{/*if(pos_objet(i,j)==pos_objet(i+a,j+b)) printf("\nerreur 5\n");*/
				if(l==o)
				  {
				    if(Objet_label(i,j)!=1 && Objet_label(i+a,j+b)!=1)
				      {g -> add_edge(pos_objet(i,j),pos_objet(i+a,j+b),r2,r2);}
				    else
				      if(Objet_label(i,j)==1 && Objet_label(i+a,j+b)!=1)
					g -> add_tweights( pos_objet(i+a,j+b),    0.0,  r2);
				      else
					if(Objet_label(i,j)!=1&& Objet_label(i+a,j+b)==1)
					  g -> add_tweights( pos_objet(i,j),    0.0,  r2);
				  }
				else
				  {
				    if(Objet_label(i,j)!=0 && Objet_label(i+a,j+b)!=0)
				      {g -> add_edge(pos_objet(i,j),pos_objet(i+a,j+b),r2,r2);}
				    else
				      if(Objet_label(i,j)==0 && Objet_label(i+a,j+b)!=0)
					g -> add_tweights( pos_objet(i+a,j+b),    0.0, r2);
				      else
					if(Objet_label(i,j)!=0&& Objet_label(i+a,j+b)==0)
					  g -> add_tweights( pos_objet(i,j),    0.0,  r2);
				  }
				}
			}
		}

}




