#include "MListeCouples.h"

//--------------------------------------------------------------------------------------
//  Calcul ZNCC a partir de signed short
//--------------------------------------------------------------------------------------
#ifdef ARCH_X86
static inline void ZNCC11x11_precalcShort0(signed short* In1,signed short* In2)
{
  //2 paquets de 8 octets
	movdqa_m2r(*In1,xmm0); 		//xmm0= z0 z1 z2 z3 z4 z5 z6 z7
	movdqa_m2r(*In2,xmm1);    //xmm1= y0 y1 y2 y3 y4 y5 y6 y7
	pmaddwd_r2r(xmm1,xmm0);   //xmm0= y0*z0+y1*z1 y2*z2+y3*z3 y4*z4+y5*z5 y6*z6+y7*z7	
}
static inline void ZNCC11x11_precalcShort1(signed short* In1,signed short* In2)
{
  //Resultat partiel dans xmm0

  //2 paquets de 8 octets
	movdqa_m2r(*In1,xmm1);
	movdqa_m2r(*In2,xmm2);
	pmaddwd_r2r(xmm2,xmm1);   //xmm1

	paddd_r2r(xmm1,xmm0);		//xmm0
}
static inline void ZNCC11x11_precalcShort2(int* result)
{
	//On met le resultat partiel dans result
	movdqu_r2m(xmm0,*result);
}
static inline void MsseZNCC11x11_precalcShort(signed short* ZNCCNum1,signed short* ZNCCNum2,float ZNCCDenom1,float ZNCCDenom2,float *res)
{
	int result[4];

  signed short *PteIn1=ZNCCNum1;
  signed short *PteIn2=ZNCCNum2;
	ZNCC11x11_precalcShort0(PteIn1,PteIn2);
	PteIn1+=8;
	PteIn2+=8;
	ZNCC11x11_precalcShort1(PteIn1,PteIn2);
	ZNCC11x11_precalcShort1(PteIn1+8,PteIn2+8);
	ZNCC11x11_precalcShort1(PteIn1+16,PteIn2+16);
	ZNCC11x11_precalcShort1(PteIn1+24,PteIn2+24);
	ZNCC11x11_precalcShort1(PteIn1+32,PteIn2+32);
	ZNCC11x11_precalcShort1(PteIn1+40,PteIn2+40);
	ZNCC11x11_precalcShort1(PteIn1+48,PteIn2+48);
	ZNCC11x11_precalcShort1(PteIn1+64,PteIn2+64);
	ZNCC11x11_precalcShort1(PteIn1+72,PteIn2+72);
	ZNCC11x11_precalcShort1(PteIn1+80,PteIn2+80);
	ZNCC11x11_precalcShort1(PteIn1+88,PteIn2+88);
	ZNCC11x11_precalcShort1(PteIn1+96,PteIn2+96);
	ZNCC11x11_precalcShort1(PteIn1+104,PteIn2+104);
	ZNCC11x11_precalcShort1(PteIn1+112,PteIn2+112);
  ZNCC11x11_precalcShort2(result);
  int num0=result[0]+result[1]+result[2]+result[3]+int(ZNCCNum1[120])*int(ZNCCNum2[120]);
	*res=float(num0)/(ZNCCDenom1*ZNCCDenom2);
}
#else
static inline void MsseZNCC11x11_precalcShort(signed short* ZNCCNum1,signed short* ZNCCNum2,float ZNCCDenom1,float ZNCCDenom2,float *res)
{}
#endif

//--------------------------------------------------------------------------------------
//  Calcul ZNCC a partir de char
//--------------------------------------------------------------------------------------
#ifdef ARCH_X86
static inline void ZNCC11x11_precalcChar0()
{
  static mmx_t mmx_0000=  {0x0000000000000000ull};
  static mmx_t mmx_127w={0x007F007F007F007Full};

  movq_m2r(mmx_0000,mm6);
  movq_m2r(mmx_127w,mm7);
	movq_m2r(mmx_0000,mm4);		
}
//lecture de 2 paquets de 8 unsigned char, conversion en signed short, soustraction de 127, produit membre a membre, addition
//retourne 1 signed short
static inline void ZNCC11x11_precalcChar1(unsigned char* In1,unsigned char* In2)
{

  //Les registres mm5,mm6,mm7 sont reserves
   
  //premier paquet de 8 octets
  movq_m2r(*In1,mm0);        // mm0= i7 i6 i5 i4 i3 i2 i1 i0
  movq_m2r(*In2,mm2);        // mm2= j7 j6 j5 j4 j3 j2 j1 j0
  movq_r2r(mm0,mm1);
  movq_r2r(mm2,mm3);
  //Conversion des char en signed short
  //et soustraction de 127
  punpckhbw_r2r(mm6,mm0);    // mm0= 00 i7 00 i6 00 i5 00 i4
  punpckhbw_r2r(mm6,mm2);    // mm2= 00 j7 00 j6 00 j5 00 j4
  psubsw_r2r(mm7,mm0);   // mm0= 00i7 00i6 00i5 00i4 (valeurs en signed short centrées en 0)
  psubsw_r2r(mm7,mm2);  // mm2= 00j7 00j6 00j5 00j4 (valeurs en signed short centrées en 0)
  punpcklbw_r2r(mm6,mm1);    // mm1= 00 i3 00 i2 00 i1 00 i0
  punpcklbw_r2r(mm6,mm3);    // mm3= 00 j3 00 j2 00 j1 00 j0
  psubsw_r2r(mm7,mm1);   // mm1= 00i3 00i2 00i1 00i0 (valeurs en signed short centrées en 0)

  //deuxieme paquet de 8 octets
  psubsw_r2r(mm7,mm3);  // mm3= 00j3 00j2 00j1 00j0 (valeurs en signed short centrées en 0)

  pmaddwd_r2r(mm0,mm2);			 // mm2= i7*j7+i6*j6 i5*j5+i4*j4 (2 doubleword)
  pmaddwd_r2r(mm1,mm3);			 // mm3= i3*j3+i2*j2 i1*j1+i0*j0 (2 doubleword)
  paddd_r2r(mm2,mm3);				 // mm3= (i7*j7+i6*j6+i3*j3+i2*j2) (i5*j5+i4*j4+i1*j1+i0*j0) (2 doubleword)

  paddd_r2r(mm3,mm4);		//mm4 resultat intermediaire  
}
static inline void ZNCC11x11_precalcChar2(int* result)
{
	movq_r2m(mm4,*result);
}


static inline void MsseZNCC11x11_precalcChar(unsigned char* ZNCCNum1,unsigned char* ZNCCNum2,float ZNCCDenom1,float ZNCCDenom2,float *res)
{
	int result[2];
  float r;
  
	ZNCC11x11_precalcChar0();
  unsigned char *PteIn1=ZNCCNum1;
  unsigned char *PteIn2=ZNCCNum2;  
	for (int i=0;i<15;i++)
	{
		ZNCC11x11_precalcChar1(PteIn1,PteIn2);
		PteIn1+=8;
		PteIn2+=8;		
	}
  ZNCC11x11_precalcChar2(result);
  Memms();
  
  int num0=result[0]+result[1]+(int(ZNCCNum1[120])-127)*(int(ZNCCNum2[120])-127);
	*res=float(num0)/(ZNCCDenom1*ZNCCDenom2);
}
#else
static inline void MsseZNCC11x11_precalcChar(unsigned char* ZNCCNum1,unsigned char* ZNCCNum2,float ZNCCDenom1,float ZNCCDenom2,float *res){}
#endif



//--------------------------------------------------------------------------------------
//  Calcul ZNCC en float
//--------------------------------------------------------------------------------------
#ifdef ARCH_X86
static inline void MsseZNCC11x11_precalc0(float *ZNCCNum1,float* ZNCCNum2,float* result)
{
	movaps_m2r(*ZNCCNum1,xmm0);
	movaps_m2r(*ZNCCNum2,xmm1);
	mulps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+4),xmm1);
	movaps_m2r(*(ZNCCNum2+4),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+8),xmm1);
	movaps_m2r(*(ZNCCNum2+8),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+12),xmm1);
	movaps_m2r(*(ZNCCNum2+12),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+16),xmm1);
	movaps_m2r(*(ZNCCNum2+16),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+20),xmm1);
	movaps_m2r(*(ZNCCNum2+20),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+24),xmm1);
	movaps_m2r(*(ZNCCNum2+24),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+28),xmm1);
	movaps_m2r(*(ZNCCNum2+28),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+32),xmm1);
	movaps_m2r(*(ZNCCNum2+32),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+36),xmm1);
	movaps_m2r(*(ZNCCNum2+36),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+40),xmm1);
	movaps_m2r(*(ZNCCNum2+40),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+44),xmm1);
	movaps_m2r(*(ZNCCNum2+44),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+48),xmm1);
	movaps_m2r(*(ZNCCNum2+48),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+52),xmm1);
	movaps_m2r(*(ZNCCNum2+52),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+56),xmm1);
	movaps_m2r(*(ZNCCNum2+56),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+60),xmm1);
	movaps_m2r(*(ZNCCNum2+60),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+64),xmm1);
	movaps_m2r(*(ZNCCNum2+64),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+68),xmm1);
	movaps_m2r(*(ZNCCNum2+68),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+72),xmm1);
	movaps_m2r(*(ZNCCNum2+72),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+76),xmm1);
	movaps_m2r(*(ZNCCNum2+76),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+80),xmm1);
	movaps_m2r(*(ZNCCNum2+80),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+84),xmm1);
	movaps_m2r(*(ZNCCNum2+84),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+88),xmm1);
	movaps_m2r(*(ZNCCNum2+88),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+92),xmm1);
	movaps_m2r(*(ZNCCNum2+92),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+96),xmm1);
	movaps_m2r(*(ZNCCNum2+96),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+100),xmm1);
	movaps_m2r(*(ZNCCNum2+100),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+104),xmm1);
	movaps_m2r(*(ZNCCNum2+104),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+108),xmm1);
	movaps_m2r(*(ZNCCNum2+108),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+112),xmm1);
	movaps_m2r(*(ZNCCNum2+112),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movaps_m2r(*(ZNCCNum1+116),xmm1);
	movaps_m2r(*(ZNCCNum2+116),xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);

	movups_r2m(xmm0,*result);
}
static inline void MsseZNCC11x11_precalc(float *ZNCCNum1,float* ZNCCNum2,float* result)
{
  float res[4];
  //Ajout des 120 premiers produits
	MsseZNCC11x11_precalc0(ZNCCNum1,ZNCCNum2,res);
	//plus le dernier pour faire 121=11*11
	*result=(res[3]+res[2]+res[1]+res[0]+ZNCCNum1[120]*ZNCCNum2[120]);	
}
//
static inline void MsseZNCC11x11_precalcBoucle0(float *ZNCCNum1,float* ZNCCNum2)
{
	movaps_m2r(*ZNCCNum1,xmm0);
	movaps_m2r(*ZNCCNum2,xmm1);
	mulps_r2r(xmm1,xmm0);
}
static inline void MsseZNCC11x11_precalcBoucle1(float *ZNCCNum1,float* ZNCCNum2)
{
	movaps_m2r(*ZNCCNum1,xmm1);
	movaps_m2r(*ZNCCNum2,xmm2);
	mulps_r2r(xmm2,xmm1);
	addps_r2r(xmm1,xmm0);
}
static inline void MsseZNCC11x11_precalcBoucle2(float *result)
{
	movups_r2m(xmm0,*result);
}
static inline void MsseZNCC11x11_precalcBoucle(float *ZNCCNum1,float* ZNCCNum2,float* result)
{
  float res[4];
  MsseZNCC11x11_precalcBoucle0(ZNCCNum1,ZNCCNum2);
  //Ajout des 120 premiers produits
	for (int i=0;i<28;i++)
	{
		MsseZNCC11x11_precalcBoucle1(ZNCCNum1,ZNCCNum2);
		ZNCCNum1+=4;
		ZNCCNum2+=4;
	}
	MsseZNCC11x11_precalcBoucle2(res);
	//plus le dernier pour faire 121=11*11
	*result=(res[3]+res[2]+res[1]+res[0]+ZNCCNum1[120]*ZNCCNum2[120]);
}
#else
static inline void MsseZNCC11x11_precalc(float *ZNCCNum1,float* ZNCCNum2,float* result) {}
static inline void MsseZNCC11x11_precalcBoucle(float *ZNCCNum1,float* ZNCCNum2,float* result) {}
#endif
static inline float ZNCC11x11_precalc(float* ZNCCNum1,float* ZNCCNum2)
{
	float r=0;  
  if (MWithMmxSseUse())
  {
		MsseZNCC11x11_precalc(ZNCCNum1,ZNCCNum2,&r);
  }
  else
  {
		for (int i=0;i<121;i++)
		{
			r+=ZNCCNum1[i]*ZNCCNum2[i];
		}
	}
  return r;
}
static inline float ZNCC11x11_precalcChar(unsigned char* ZNCCNumChar1,unsigned char* ZNCCNumChar2,float ZNCCDenom1,float ZNCCDenom2)
{
	float r=0;
  if (MWithMmxSseUse())
  {
		MsseZNCC11x11_precalcChar(ZNCCNumChar1,ZNCCNumChar2,ZNCCDenom1,ZNCCDenom2,&r);
  }
  else
  {
		int num=0;
		for (int i=0;i<121;i++)
		{
			num+=((int)(ZNCCNumChar1[i])-127)*((int)(ZNCCNumChar2[i])-127);
		}
		r=float(num)/(ZNCCDenom1*ZNCCDenom2);
	}
  return (4.0*r);
}
static inline float ZNCC11x11_precalcShort(signed short* ZNCCNum1,signed short* ZNCCNum2,float ZNCCDenom1,float ZNCCDenom2)
{
	float r=0;
  if (MWithMmxSseUse())
  {
		MsseZNCC11x11_precalcShort(ZNCCNum1,ZNCCNum2,ZNCCDenom1,ZNCCDenom2,&r);
  }
  else
  {
		int num=0;
		for (int i=0;i<121;i++)
		{
			num+=(int)(ZNCCNum1[i])*(int)(ZNCCNum2[i]);
		}
		r=float(num)/(ZNCCDenom1*ZNCCDenom2);
	}
  return r;
}

//--------------------------------------------------------------------------------------
//Creation de la liste de couples definitive avec localisation subpixellique des points retenus
//--------------------------------------------------------------------------------------
void MListeCouples::Apparie(MListePI& ListePI1,MDetecteur& Detecteur1,MListePI& ListePI2,MDetecteur& Detecteur2,int xROI,int yROI,float ScoreMin)
{
	Create(ListePI1,ListePI2,xROI,yROI,ScoreMin);
	KeepBest();
	RaffineCoords(Detecteur1,Detecteur2);
}

//--------------------------------------------------------------------------------------
//  Formation des couples et calcul de leur ZNCC
//--------------------------------------------------------------------------------------	
void MListeCouples::CreateSansZones(MListePI& ListePI1,MListePI& ListePI2,int xROI,int yROI,float ScoreMin)
{
	//Pour chaque point de la liste 1 on cherche les points de la liste 2 situés dans la ROI
  int nbPoints1=ListePI1.getNbPoints();
  int nbPoints2=ListePI2.getNbPoints();

  float ZNCCMax,score;
   
	for (int i=0;i<nbPoints1;i++)
	{
		int x1=ListePI1.getX(i);
		int y1=ListePI1.getY(i);
    int xmin=x1-xROI;
    int xmax=x1+xROI;
    int ymin=y1-yROI;
    int ymax=y1+yROI;
    ZNCCMax=-2.0;		//Meilleur ZNCC trouve avec P1 pour premier point
    for (int j=0;j<nbPoints2;j++)
    {
			int x2=ListePI2.getX(j);
			if ((x2>xmin) && (x2<xmax))
			{
        int y2=ListePI2.getY(j);
        if ((y2>ymin) && (y2<ymax))
        {
					//Le point est dans la ROI
					//calcul du ZNCC
					score=ZNCC11x11_precalc(ListePI1.getZNCCNum(i),ListePI2.getZNCCNum(j));
					//Ajout du couple si le ZNCC calcule est suffisant
					if ((score>=ScoreMin) && (score>ZNCCMax))
					{
						Add(x1,y1,x2,y2,score);
						ZNCCMax=score;
					}	
				}
			}
		}
	}
	//cout << "Nb couples (sans zones): " << listeCouples.size() << endl;
}
int MListeCouples::Create(MListePI& ListePI1,MListePI& ListePI2,int xROI,int yROI,float ScoreMin)
{
	int nbCouplesTestes=0;
	float score;
	//Pour chaque point de la liste 1 on cherche les points de la liste 2 situés dans la ROI
  int nbPoints1=ListePI1.getNbPoints();
  int nbPoints2=ListePI2.getNbPoints();

  int LargeurImage=ListePI2.GetImage().x();
  int HauteurImage=ListePI2.GetImage().y();

  //Creation d'une table contenant la liste des points de l'image 2 classées par zones d'image
  int nbZonesX;
  int nbZonesY;
  if (LargeurImage%xROI==0) nbZonesX=LargeurImage/xROI;
  else nbZonesX=LargeurImage/xROI+1;
  if (HauteurImage%yROI==0) nbZonesY=HauteurImage/yROI;
  else nbZonesY=HauteurImage/yROI+1;
  
  int nbZones=nbZonesX*nbZonesY;
  
  int tabZone[nbZones][nbPoints2];	//liste des points dans chaque zone
  int nbPointsZone[nbZones];				//nombre de points dans chaque zone (=nombre de points dans la liste correspondante)
  for (int i=0;i<nbZones;i++) nbPointsZone[i]=0;

  //classement par zone de tous les points dans la table  
  for	(int i=0;i<nbPoints2;i++)
  {
		//le point i est dans la zone (xz,yz)
		int xz=ListePI2.getX(i)/xROI;
		int yz=ListePI2.getY(i)/yROI;
		int numZone=xz+yz*nbZonesX;
    tabZone[numZone][nbPointsZone[numZone]]=i;
		nbPointsZone[numZone]+=1;
	}			

  /*//controle du nombre de points total
  int nbPointsTotal=0;
  for (int xz=0;xz<nbZonesX;xz++) for (int yz=0;yz<nbZonesY;yz++) nbPointsTotal+=nbPointsZone[xz+yz*nbZonesX];
  cout << nbPointsTotal << " / " << nbPoints2 << endl;*/
  
	for (int i=0;i<nbPoints1;i++)
	{
		int x1=ListePI1.getX(i);
		int y1=ListePI1.getY(i);
    int xmin=x1-xROI;
    int xmax=x1+xROI;
    int ymin=y1-yROI;
    int ymax=y1+yROI;

    //recherche de la zone a laquelle appartient le point P1
		int xz1=x1/xROI;
		int yz1=y1/yROI;
    for (int xz=Max(0,xz1-1);xz<Min(nbZonesX,xz1+2);xz++)
    {
			for (int yz=Max(0,yz1-1);yz<Min(nbZonesY,yz1+2);yz++)
			{
			  //parcours des points de la liste 2 dans la zone (xz,yz)
			  int numZone=xz+yz*nbZonesX;
			  for (int k=0;k<nbPointsZone[numZone];k++)
			  {
					int j=tabZone[numZone][k];
					int x2=ListePI2.getX(j);
					if ((x2>xmin) && (x2<xmax))
					{
						int y2=ListePI2.getY(j);
						if ((y2>ymin) && (y2<ymax))
						{
							//Le point est dans la ROI
							//calcul du ZNCC
							score=ZNCC11x11_precalc(ListePI1.getZNCCNum(i),ListePI2.getZNCCNum(j));
							//Ajout du couple si le ZNCC calcule est suffisant
							if (score>=ScoreMin)
							{
								Add(x1,y1,x2,y2,score);
							}
							nbCouplesTestes++;
						}
					}
			  } //fin for k
			}
		} //fin for xz		
	}
	return nbCouplesTestes;
}

//--------------------------------------------------------------------------------------
// SelectBest : marque le meilleur couple comme selectionne et supprime les couples devenus impossibles
//--------------------------------------------------------------------------------------
int MListeCouples::SelectBest()
{
	float scoreMax;
  if (nbSelected<nbCouples)
  {
		//S'il reste des couples non selectionnes

		TlistMCouple::iterator itMax=0;

		//Recherche du meilleur couple non encore selectionne
		for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
		{
      if (it->NotSelected())
      {
				if (itMax==0)
				{
					//Le premier couple non selectionne sert a l'initialisation du max
					itMax=it;
					scoreMax=itMax->getScore();
				}
				
				if ((itMax!=0) && (it->getScore() > scoreMax))
				{
					scoreMax=it->getScore();
					itMax=it;
				}
			}
		}
		//Le meilleur couple est marque selectionne
		itMax->select();
		nbSelected++;
		//Suppression de tous les couples contenant un des 2 points du meilleur couple sauf le meilleur couple lui-meme
		RemoveNotSelected(itMax->getX1(),itMax->getY1(),itMax->getX2(),itMax->getY2());
		return 1;
	}
	else return 0;
}
int MListeCouples::KeepBest()
{
  while (SelectBest()) {}
	return nbSelected;
}

//--------------------------------------------------------------------------------------
// Remove
// Supprime tous les couples contenant un des 2 points (x1,y1) ou (x2,y2)
//--------------------------------------------------------------------------------------
void MListeCouples::Remove(float x1,float y1,float x2,float y2)
{
	for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
	{
		if ( ((it->getX1()==x1) && (it->getY1()==y1)) || ((it->getX2()==x2) && (it->getY2()==y2)) )
		{
			TlistMCouple::iterator itErase=it;
			it++;
			if (itErase->Selected()) nbSelected--;
			listeCouples.erase(itErase);
			nbCouples--;
		}
	}	
}
void MListeCouples::RemoveNotSelected(float x1,float y1,float x2,float y2)
{
	for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
	{
		if (it->NotSelected())
		{
			if ( ((it->getX1()==x1) && (it->getY1()==y1)) || ((it->getX2()==x2) && (it->getY2()==y2)) )
			{
				TlistMCouple::iterator itErase=it;
				it++;
				listeCouples.erase(itErase);
				nbCouples--;
			}
		}
	}
}
//--------------------------------------------------------------------------------------
// Empty
//--------------------------------------------------------------------------------------
void MListeCouples::Empty()
{
	for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
	{
		TlistMCouple::iterator itErase=it;
		it++;
		listeCouples.erase(itErase);
		nbCouples--;
	}
	nbCouples=0;
}
//--------------------------------------------------------------------------------------
// FindPoint1
//--------------------------------------------------------------------------------------
MCouple* MListeCouples::FindPoint1(float x1,float y1)
{
	MCouple* c=0;
	for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
	{
		if ( ((it->getX1()==x1) && (it->getY1()==y1)) )
		{
		  c=&(*it);
		}
	}
	return c;
}

//--------------------------------------------------------------------------------------
//Calcul plus precis des coordonnees des 2 points du couple
//On doit fournir les detecteurs ayant servi a calculer chacun des 2 points
//--------------------------------------------------------------------------------------
void MListeCouples::RaffineCoords(MDetecteur& Detecteur1,MDetecteur& Detecteur2)
{
  float x1,y1,x2,y2;
	for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
	{
    Detecteur1.Raffine(int(it->getX1()),int(it->getY1()),x1,y1);
    Detecteur2.Raffine(int(it->getX2()),int(it->getY2()),x2,y2);
    it->setX1(x1);
    it->setY1(y1);
    it->setX2(x2);
    it->setY2(y2);
	}
}
  


//--------------------------------------------------------------------------------------
//Methode de relaxation (Zhengyou ZHANG)
//--------------------------------------------------------------------------------------

//--------------------------------------------------------------------------------------
//Calcul du strength of match pour un couple donne
// En entrant dans cette methode listeCouples doit etre organisee comme suit:
// (P1,Q1)
// (P1,Q2)
// (P1,..)
// (P1,Qn)
// (P2,Q1)
// (P2,..)
//
// Elle est sous cette forme a la sortie de MListeCouples::Create() et de MListeCouples::KeepBest()
//--------------------------------------------------------------------------------------
float MListeCouples::CalculeSM(MCouple& Couple)
{
	MCouple *N1;
	MCouple *N2;
	float R=20;
	float EpsilonR=0.3;
  float sm=0;
  float d1,d2;
  
  //Parcours des voisins de Couple.P1
  TlistMCouple::iterator itVoisin1=listeCouples.begin();
  while (itVoisin1!=listeCouples.end())
  {
		N1=&(*itVoisin1);
		d1=Distance1(*N1,Couple);
    if (d1<R)
    {
			//N1 est un voisin de Couple.P1

			float valMax=0.0;
			//Parcours des couples du type (N1.P1,XX) avec XX un voisin de Couple.P2
      TlistMCouple::iterator itVoisin2=itVoisin1;
			while ( (itVoisin2->getX1()==itVoisin1->getX1()) && (itVoisin2->getY1()==itVoisin1->getY1()) && (itVoisin2!=listeCouples.end()) )
			{
				N2=&(*itVoisin2);
        d2=Distance2(*N2,Couple);
				if (d2<R)
				{
					//N2 est un voisin de Couple.P2
					float dmoy=(d1+d2)/2;
					float r=abs(d1-d2)/dmoy;
          float val;
          if (r>=EpsilonR)
          {
						val=0.0;
					}
					else
					{
						float denom=1+dmoy;
						//Recherche du score du couple (N1.P1,N2.P2)
						float score=N2->getScore();
						//valeur retenue pour la recherche du max
						val=score*exp(-r/EpsilonR)/denom;
					}
					if (val>=valMax) valMax=val;
        }
        ++itVoisin2;
      }
      sm+=valMax;
      itVoisin1=itVoisin2;
			//fin N1 est un voisin			 
		}
    else ++itVoisin1;
	}
  
  sm=sm*Couple.getScore();
  return sm;
}

//--------------------------------------------------------------------------------------
// SetSelectedAll
//--------------------------------------------------------------------------------------
void MListeCouples::SetSelectedAll(int inS)
{
  for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
  {
    it->setSelected(inS);
	}
}

//--------------------------------------------------------------------------------------
// getNbCouplesSelected
//--------------------------------------------------------------------------------------
int MListeCouples::getNbCouplesSelected(void)
{
	int nb=0;
  for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
  {
    if (it->getSelected()==1) nb++;
	}
  return nb;
}

//--------------------------------------------------------------------------------------
// CalculeSM
//--------------------------------------------------------------------------------------
void MListeCouples::CalculeSM()
{
	MCouple *Couple;

  for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
  {
    if (it->NotSelected())
    {
			float s=CalculeSM(*it);
			it->setScoreSM(s);
		}
	}
}

//--------------------------------------------------------------------------------------
//Suppression de certains couples de la liste
//renvoie 0 quand il n'est plus possible d'optimiser (plus de couples a supprimer)
//--------------------------------------------------------------------------------------
int MListeCouples::IterationOptimiseSM()
{
	//methode "winner take all"
	float scoreMax;
  if (nbSelected<nbCouples)
  {
		//S'il reste des couples non selectionnes
		TlistMCouple::iterator itMax=0;

		//Recherche du meilleur couple non encore selectionne
		for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
		{
      if (it->NotSelected())
      {
				if (itMax==0)
				{
					//Le premier couple non selectionne sert a l'initialisation du max
					itMax=it;
					scoreMax=itMax->getScoreSM();
				}

				if ((itMax!=0) && (it->getScoreSM() > scoreMax))
				{
					scoreMax=it->getScoreSM();
					itMax=it;
				}
			}
		}
		//Le meilleur couple est marque selectionne
		itMax->select();
		nbSelected++;
		//Suppression de tous les couples contenant un des 2 points du meilleur couple sauf le meilleur couple lui-meme
		RemoveNotSelected(itMax->getX1(),itMax->getY1(),itMax->getX2(),itMax->getY2());
		return 1;
	}
	else return 0;	
}

	
//--------------------------------------------------------------------------------------
// OptimiseSM
//--------------------------------------------------------------------------------------
int MListeCouples::OptimiseSM()
{
	int n=0;
	//remise a zero du drapeau selected pour tous les couples
  for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it) it->setSelected(-1);
  nbSelected=0;

	//Calcul de scores SM
	CalculeSM();
	//Boucle d'optimisation
  while (IterationOptimiseSM())
  {
		CalculeSM();
	}
	return nbCouples;
}


//--------------------------------------------------------------------------------------
//Lectures de 4 couples donnés par leur numéro dans la liste
//--------------------------------------------------------------------------------------
void MListeCouples::GetCoords4Couples(int n0,int n1,int n2,int n3,float* x1,float* y1,float *x2,float* y2)
{
	int n=0;
	for (TlistMCouple::iterator it=listeCouples.begin(); it!=listeCouples.end(); ++it)
	{
		if (n==n0)
		{
			x1[0]=it->getX1();
			y1[0]=it->getY1();
			x2[0]=it->getX2();
			y2[0]=it->getY2();
		}
		if (n==n1)
		{
			x1[1]=it->getX1();
			y1[1]=it->getY1();
			x2[1]=it->getX2();
			y2[1]=it->getY2();
		}
		if (n==n2)
		{
			x1[2]=it->getX1();
			y1[2]=it->getY1();
			x2[2]=it->getX2();
			y2[2]=it->getY2();
		}
		if (n==n3)
		{
			x1[3]=it->getX1();
			y1[3]=it->getY1();
			x2[3]=it->getX2();
			y2[3]=it->getY2();
		}
		n++;
	}
}
//--------------------------------------------------------------------------------------
//Lectures des coordonnees d'un couple donne par son numero dans la lsite
//--------------------------------------------------------------------------------------
void MListeCouples::GetCoords(int n0,float& x1,float& y1,float& x2,float& y2)
{
	int n=0;
	bool trouve=false;
	TlistMCouple::iterator it=listeCouples.begin();
	while ( (it!=listeCouples.end()) && (!trouve) )
	{
		if (n==n0)
		{
			x1=it->getX1();
			y1=it->getY1();
			x2=it->getX2();
			y2=it->getY2();
			trouve=true;
		}
		n++;
		++it;
	}	
}