#include "MListePI.h"

MListePI::MListePI(int inNbPointsMax)
{
	nbPoints=0;
  nbPointsMax=inNbPointsMax;
  tabX=new int[nbPointsMax];
  tabY=new int[nbPointsMax];

	image=0;
	imageCritere=0;

	ZNCCNum=new (float*)[nbPointsMax];
	for (int i=0;i<nbPointsMax;i++) ZNCCNum[i]=(float *)(MAlign16ByteNew(11*11));

	/*ZNCCDenom=new float[nbPointsMax];
	ZNCCNumShort=new (signed short*)[nbPointsMax];
	for (int i=0;i<nbPointsMax;i++) ZNCCNumShort[i]=(signed short *)(MAlign16ByteNew(11*11));*/
}

MListePI::MListePI(int inNbPointsMax,MCharImage& inImage,MFloatImage& inImageCritere)
{
  nbPointsMax=inNbPointsMax;
  nbPoints=0;
  tabX=new int[nbPointsMax];
  tabY=new int[nbPointsMax];

  image=&inImage;
  imageCritere=&inImageCritere;

	ZNCCNum=new (float*)[nbPointsMax];
	for (int i=0;i<nbPointsMax;i++) ZNCCNum[i]=(float *)(MAlign16ByteNew(11*11));

	/*ZNCCDenom=new float[nbPointsMax];
	ZNCCNumShort=new (signed short*)[nbPointsMax];
	for (int i=0;i<nbPointsMax;i++) ZNCCNumShort[i]=(signed short *)(MAlign16ByteNew(11*11));*/
}

MListePI::~MListePI()
{
	if (tabX!=0) delete [] tabX;
	if (tabY!=0) delete [] tabY;
	deleteZNCCPrecalculs();
}

void MListePI::deleteZNCCPrecalculs()
{
	if (ZNCCNum!=0)
	{
		for (int i=0;i<nbPointsMax;i++)
		{
			MAlign16ByteDelete((int*)(ZNCCNum[i]));
		}
		delete ZNCCNum;
	}
	ZNCCNum=0;

	/*if (ZNCCNumShort!=0)
	{
		for (int i=0;i<nbPointsMax;i++)
		{
			MAlign16ByteDelete((int*)(ZNCCNumShort[i]));
		}
		delete ZNCCNumShort;
	}
	ZNCCNumShort=0;
	if (ZNCCDenom!=0) delete [] ZNCCDenom;*/
}

void MListePI::Init(MCharImage& inImage,MFloatImage& inImageCritere)
{
  nbPoints=0;

  image=&inImage;
  imageCritere=&inImageCritere;
}

//-----------------------------------------------------------------------------------------------
// Methodes de tri
//-----------------------------------------------------------------------------------------------
void MListePI::HeapSort()
{
  int i;
  int nb=nbPoints;
  for (i = (nb / 2); i >= 0; i--)
  {
    HeapSortMaxsiftDown(i, nbPoints-1);
  }
  for (i = nb-1; i >= 1; i--)
  {
    Swap(tabX[0],tabX[i]);
    Swap(tabY[0],tabY[i]);
    HeapSortMaxsiftDown(0, i-1);
  }
}
void MListePI::HeapSortMaxsiftDown(int root, int bottom)
{
  int done, maxChild;

  done = 0;
  while ((root*2 <= bottom) && (!done))
  {
    if (root*2 == bottom)
      maxChild = root * 2;
    else if (imageCritere->M(tabX[root*2],tabY[root*2]) < imageCritere->M(tabX[root*2+1],tabY[root*2+1]))
      maxChild = root * 2;
    else
      maxChild = root * 2 + 1;
    if ( imageCritere->M(tabX[root],tabY[root]) > imageCritere->M(tabX[maxChild],tabY[maxChild]))
    {
      Swap(tabX[root],tabX[maxChild]);
      Swap(tabY[root],tabY[maxChild]);
      root = maxChild;
    }
    else
      done = 1;
  }
}

//-----------------------------------------------------------------------------------------------
// Affichage des points pour visualisation
//-----------------------------------------------------------------------------------------------
void MListePI::draw(MCharImage &img)
{
	for (int i=0;i<nbPoints;i++)
	{
    img.y(tabX[i],tabY[i])=255;
	}
}


//-----------------------------------------------------------------------------------------------
// Precalcul ZNCC
//-----------------------------------------------------------------------------------------------

//calcul du denominateur
#ifdef ARCH_X86
static inline void MsseZNCC11x11Denom0(float *ZNCCNum)
{
	movaps_m2r(*ZNCCNum,xmm0);
	mulps_r2r(xmm0,xmm0);
}
static inline void MsseZNCC11x11Denom1(float *ZNCCNum)
{
	movaps_m2r(*ZNCCNum,xmm1);
	mulps_r2r(xmm1,xmm1);
	addps_r2r(xmm1,xmm0);
}
static inline void MsseZNCC11x11Denom2(float *result)
{
	movaps_r2m(xmm0,*result);
}
static inline void MsseZNCC11x11Denom(float *ZNCCNum,float *result)
{
	float res[4];
	MsseZNCC11x11Denom0(ZNCCNum);
	for (int i=0;i<29;i++)
	{
		ZNCCNum+=4;
		MsseZNCC11x11Denom1(ZNCCNum);
	}
	ZNCCNum+=4;
	MsseZNCC11x11Denom2(res);
	//ajout de la 121eme valeur
	*result=sqrt(res[3]+res[2]+res[1]+res[0]+ZNCCNum[0]*ZNCCNum[0]);
}
#else
static inline void MsseZNCC11x11Denom(float *ZNCCNum,float *result){}
#endif
inline float ZNCC11x11Denom(float* ZNCCNum)
{
  float r;
  if (MWithMmxSseUse())
  {
		MsseZNCC11x11Denom(ZNCCNum,&r);
	}
	else
  {
		r=0;
		for (int i=0;i<121;i++)
		{
			r+=ZNCCNum[i]*ZNCCNum[i];
		}
		r=sqrt(r);		
	}
	return r;
}
//Division
#ifdef ARCH_X86
static inline void MsseZNCC11x11Division0(float *Denominateur)
{
	movups_m2r(*Denominateur,xmm0);
}
static inline void MsseZNCC11x11Division1(float *ZNCCNum)
{
	movaps_m2r(*ZNCCNum,xmm1);
	divps_r2r(xmm0,xmm1);
	movaps_r2m(xmm1,*ZNCCNum);
}
static inline void MsseZNCC11x11Division(float *ZNCCNum,float *Denom)
{
	float Denominateur[4];
	Denominateur[0]=*Denom;
	Denominateur[1]=*Denom;
	Denominateur[2]=*Denom;
	Denominateur[3]=*Denom;
	
	MsseZNCC11x11Division0(Denominateur);
	for (int i=0;i<30;i++)
	{
		MsseZNCC11x11Division1(ZNCCNum);
		ZNCCNum+=4;
	}
	//la 121eme valeur
	*ZNCCNum=(*ZNCCNum)/(*Denom);
}
#else
static inline void MsseZNCC11x11Division(float *ZNCCNum,float *Denom,float *result){}
#endif

void MListePI::PrecalculZNCC11x11()
{											
  float ZNCCMoyenne,ZNCCDenom0;
  for (int i=0;i<nbPoints;i++)
  {
		//Calcul de Moyenne
		ZNCCMoyenne=image->ZNCC11x11Moyenne(tabX[i],tabY[i]);
		//Calcul du numerateur
    image->ZNCC11x11Num(ZNCCNum[i],tabX[i],tabY[i],ZNCCMoyenne);
		//calcul du denominateur
		ZNCCDenom0=ZNCC11x11Denom(ZNCCNum[i]);
		if (MWithMmxSseUse())
		{
			MsseZNCC11x11Division(ZNCCNum[i],&ZNCCDenom0);
		}
		else for (int j=0;j<121;j++) ZNCCNum[i][j]=ZNCCNum[i][j]/ZNCCDenom0;
	}	
}