#include "MDetecteur.h"

//-------------------------------------------------------------------------------------------------------------
// Creation de la liste de points a partir de l'image en float (commun aux 2 detecteurs)
//-------------------------------------------------------------------------------------------------------------
int MDetecteur::CreateListPI(MCharImage& Image,MListePI& ListePI,int NbMax)
{
	float MaxCritere=ImgCritere->findMax();
	ListePI.Init(Image,*ImgCritere);
	float SeuilMaxCritere=0.0;
	ImgCritere->LocalMaximaList(ListePI,MaxCritere*SeuilMaxCritere);

	int NbPointCritere;
	if (NbMax!=-1)
	{
		ListePI.HeapSort();
		NbPointCritere=ListePI.Cut(NbMax);
	}
	else NbPointCritere=ListePI.getNbPoints();

	//precalculs du ZNCC pour appariement
	ListePI.PrecalculZNCC11x11();

	return NbPointCritere;
}


//-------------------------------------------------------------------------------------------------------------
//Calcule plus precisement la position d'un max local
//-------------------------------------------------------------------------------------------------------------
void MDetecteur::Raffine(int x0,int y0,float& x,float& y)
{
  //Approx quadratique en x puis en y
	float* Pt=&(ImgCritere->M(x0,y0));
  float a=(Pt[-1]+Pt[1])/2-Pt[0];
  float b=(Pt[1]-Pt[-1])/2;
	float deltaX=-b/(2*a);

	int xStride=ImgCritere->x();
  a=(Pt[-xStride]+Pt[xStride])/2-Pt[0];
  b=(Pt[xStride]-Pt[-xStride])/2;
	float deltaY=-b/(2*a);

	x=x0+deltaX;
	y=y0+deltaY;
}

//-------------------------------------------------------------------------------------------------------------
// Critere de Harris
//-------------------------------------------------------------------------------------------------------------
MDetecteurHarris::MDetecteurHarris()
{
	ImgA=0;
	ImgB=0;
	ImgC=0;
	ImgTempSignedShort=0;
	ImgTempChar=0;
	GradX=0;
	GradY=0;	
}
MDetecteurHarris::MDetecteurHarris(int x,int y)
{
  MDetecteur::SetSize(x,y);
  SetSize(x,y);
}
MDetecteurHarris::MDetecteurHarris(MCharImage& Image)
{
  MDetecteur::SetSize(Image.x(),Image.y());
  SetSize(Image.x(),Image.y());
}
void MDetecteurHarris::SetSize(int x,int y)
{
	ImgA=new MSignedShortImage(x,y);
	ImgB=new MSignedShortImage(x,y);
	ImgC=new MSignedShortImage(x,y);
	ImgTempSignedShort=new MSignedShortImage(x,y);
	ImgTempChar=new MCharImage(x,y);
	GradX=new MCharImage(x,y);
	GradY=new MCharImage(x,y);	
}
MDetecteurHarris::~MDetecteurHarris()
{
	if (ImgA!=0) delete ImgA;
  if (ImgB!=0) delete ImgB;
  if (ImgC!=0) delete ImgC;
  if (ImgTempSignedShort!=0) delete ImgTempSignedShort;
  if (ImgTempChar!=0) delete ImgTempChar;
  if (GradX!=0) delete GradX;
  if (GradY!=0) delete GradY;
}


int MDetecteurHarris::Detecte(MCharImage& Image,MListePI& ListePI,int NbMax) {

  //filtrage 121 en x et clacul du gradient en y
  ImgTempChar->Smooth121(0,Image);
  GradY->Gradient_101(1,*ImgTempChar);

  //filtrage 121 en y et clacul du gradient en x
  ImgTempChar->Smooth121(1,Image);
  GradX->Gradient_101(0,*ImgTempChar);
 
  //Matrice de Harris MH=
  //  A C
  //  C B
  //Calcul de A=carre(dI/dx), B=carre(dI/dy), C=( (dI/dx) * (dI/dy) )
  //puis lissage de chaque image obtenue
  //lissage 5x5
  ImgA->CharProduct(*GradX,*GradX);
  ImgTempSignedShort->Smooth121(1,*ImgA);
  ImgA->Smooth121(0,*ImgTempSignedShort);
  ImgTempSignedShort->Smooth121(1,*ImgA);
  ImgA->Smooth121(0,*ImgTempSignedShort);

  ImgB->CharProduct(*GradY,*GradY);
  ImgTempSignedShort->Smooth121(1,*ImgB);
  ImgB->Smooth121(0,*ImgTempSignedShort);
  ImgTempSignedShort->Smooth121(1,*ImgB);
  ImgB->Smooth121(0,*ImgTempSignedShort);

  ImgC->CharProduct(*GradX,*GradY);
  ImgTempSignedShort->Smooth121(1,*ImgC);
  ImgC->Smooth121(0,*ImgTempSignedShort);
  ImgTempSignedShort->Smooth121(1,*ImgC);
  ImgC->Smooth121(0,*ImgTempSignedShort);
  //Calcul de det(MH)-k*trace(MH)=A*B-C^2-k*(A+B)^2
  ImgCritere->Harris(*ImgA,*ImgB,*ImgC);
  int n=CreateListPI(Image,ListePI,NbMax);
  return n;
}

//-------------------------------------------------------------------------------------------------------------
// Critere de Moravec
//-------------------------------------------------------------------------------------------------------------
MDetecteurMoravec::MDetecteurMoravec()
{
	ImgTempChar1=0;
	ImgTempChar2=0;
}
MDetecteurMoravec::MDetecteurMoravec(int x,int y)
{
  MDetecteur::SetSize(x,y);
  SetSize(x,y);
}
MDetecteurMoravec::MDetecteurMoravec(MCharImage& Image)
{
  MDetecteur::SetSize(Image.x(),Image.y());
  SetSize(Image.x(),Image.y());
}
void MDetecteurMoravec::SetSize(int x,int y)
{
	ImgTempChar1=new MCharImage(x,y);
	ImgTempChar2=new MCharImage(x,y);
}
MDetecteurMoravec::~MDetecteurMoravec()
{
  if (ImgTempChar1!=0) delete ImgTempChar1;
  if (ImgTempChar2!=0) delete ImgTempChar2;
}

int MDetecteurMoravec::Detecte(MCharImage& Image,MListePI& ListePI,int NbMax)
{
	ImgTempChar1->Smooth121(0,Image);
  ImgTempChar2->Smooth121(1,*ImgTempChar1);
  ImgCritere->Moravec5x5(*ImgTempChar2);

	return CreateListPI(Image,ListePI,NbMax);
}

//-------------------------------------------------------------------------------------------------------------
// Critere : plus petite valeur propre
//-------------------------------------------------------------------------------------------------------------
MDetecteurVPInf::MDetecteurVPInf()
{
	ImgA=0;
	ImgB=0;
	ImgC=0;
	ImgTempSignedShort=0;
	ImgTempChar=0;
	GradX=0;
	GradY=0;
}
MDetecteurVPInf::MDetecteurVPInf(int x,int y)
{
  MDetecteur::SetSize(x,y);
  SetSize(x,y);
}
MDetecteurVPInf::MDetecteurVPInf(MCharImage& Image)
{
  MDetecteur::SetSize(Image.x(),Image.y());
  SetSize(Image.x(),Image.y());
}
void MDetecteurVPInf::SetSize(int x,int y)
{
	ImgA=new MSignedShortImage(x,y);
	ImgB=new MSignedShortImage(x,y);
	ImgC=new MSignedShortImage(x,y);
	ImgTempSignedShort=new MSignedShortImage(x,y);
	ImgTempChar=new MCharImage(x,y);
	GradX=new MCharImage(x,y);
	GradY=new MCharImage(x,y);
}
MDetecteurVPInf::~MDetecteurVPInf()
{
	if (ImgA!=0) delete ImgA;
  if (ImgB!=0) delete ImgB;
  if (ImgC!=0) delete ImgC;
  if (ImgTempSignedShort!=0) delete ImgTempSignedShort;
  if (ImgTempChar!=0) delete ImgTempChar;
  if (GradX!=0) delete GradX;
  if (GradY!=0) delete GradY;
}


int MDetecteurVPInf::Detecte(MCharImage& Image,MListePI& ListePI,int NbMax)
{
	//filtrage 121 en x et clacul du gradient en y
  ImgTempChar->Smooth121(0,Image);
  GradY->Gradient_101(1,*ImgTempChar);

  //filtrage 121 en y et clacul du gradient en x
  ImgTempChar->Smooth121(1,Image);
  GradX->Gradient_101(0,*ImgTempChar);

  //Matrice de Harris MH=
  //  A C
  //  C B
  //Calcul de A=carre(dI/dx), B=carre(dI/dy), C=( (dI/dx) * (dI/dy) )
  //puis lissage de chaque image obtenue
  ImgA->CharProduct(*GradX,*GradX);
  ImgTempSignedShort->Smooth121(1,*ImgA);
  ImgA->Smooth121(0,*ImgTempSignedShort);
  ImgTempSignedShort->Smooth121(1,*ImgA);
  ImgA->Smooth121(0,*ImgTempSignedShort);

  ImgB->CharProduct(*GradY,*GradY);
  ImgTempSignedShort->Smooth121(1,*ImgB);
  ImgB->Smooth121(0,*ImgTempSignedShort);
  ImgTempSignedShort->Smooth121(1,*ImgB);
  ImgB->Smooth121(0,*ImgTempSignedShort);

  ImgC->CharProduct(*GradX,*GradY);
  ImgTempSignedShort->Smooth121(1,*ImgC);
  ImgC->Smooth121(0,*ImgTempSignedShort);
  ImgTempSignedShort->Smooth121(1,*ImgC);
  ImgC->Smooth121(0,*ImgTempSignedShort);

  //Calcul de la plus petite valeur propre de la matrice
  // A C
  // C B
  ImgCritere->VPInf(*ImgA,*ImgB,*ImgC);

  int n=CreateListPI(Image,ListePI,NbMax);

  return n;
}