#include "MEstimeTranslation.h"

static inline void Tirage1Couple(MListeCouples& ListeCouples,double& x1,double& y1,double& x2,double& y2)
{
  int nbCouples=ListeCouples.getNbCouples();
  int n1;
  float x1f,y1f,x2f,y2f;
  //1 point
  n1=(int) (float(nbCouples)*rand()/(RAND_MAX+1.0));
  ListeCouples.GetCoords(n1,x1f,y1f,x2f,y2f);
  x1=double(x1f);y1=double(y1f);x2=double(x2f);y2=double(y2f);
}

static inline void CalculeTranslation1Couple(double x1,double y1,double x2,double y2,double& tx,double& ty)
{
  tx=x2-x1;
  ty=y2-y1;
}

static inline double CalculeErreur(MCouple& Couple,double tx,double ty)
{
  double x1=double(Couple.getX1());
  double y1=double(Couple.getY1());
  double x2=double(Couple.getX2());
  double y2=double(Couple.getY2());
  double x1p=x1+tx;
  double y1p=y1+ty;
  double e=(x2-x1p)*(x2-x1p)+(y2-y1p)*(y2-y1p);
  return sqrt(e);
}
static void CalculeTranslationCouplesSelected(MListeCouples& ListeCouples,double& tx,double& ty)
{
  int nb=0;
  tx=0.0;
  ty=0.0;
  TlistMCouple* liste=ListeCouples.getListe();
  for (TlistMCouple::iterator it=liste->begin(); it!=liste->end(); ++it)
  {
    if (it->Selected())
    {
      double x1=double(it->getX1());
      double y1=double(it->getY1());
      double x2=double(it->getX2());
      double y2=double(it->getY2());
      nb++;
      tx+=x2-x1;
      ty+=y2-y1;
    }
  }
  tx=tx/double(nb);
  ty=ty/double(nb);
}

int MEstimeTranslation::Estime(MListeCouples& ListeCouples,double& tx,double& ty)
{
  int nbCouples=ListeCouples.getNbCouples();
  int NbTirages=50;
  int NbInliers;
  int NbInliersMax=0;
  double SeuilErreur=3.0;
  double tx0,ty0;
  
  double x1,y1,x2,y2;
  for (int i=0;i<NbTirages;i++)
  {
    //tirage de 1 couple de points
    Tirage1Couple(ListeCouples,x1,y1,x2,y2);
    //On calcule la translation correspondante
    CalculeTranslation1Couple(x1,y1,x2,y2,tx0,ty0);
    //On compte les inliers pour cette translation
    NbInliers=0;
    TlistMCouple* liste=ListeCouples.getListe();
    for (TlistMCouple::iterator it=liste->begin(); it!=liste->end(); ++it)
    {
      //Calcul de l'erreur pour le couple par rapport a tx0,ty0;
      double e=CalculeErreur(*it,tx0,ty0);
      if (e<SeuilErreur) NbInliers++;
    }
    
    //On retient le tirage avec le plus grand nombre d'inliers
    if (NbInliers>=NbInliersMax)
    {
      NbInliersMax=NbInliers;
      //selection des inliers
      ListeCouples.SetSelectedAll(-1);
      for (TlistMCouple::iterator it=liste->begin(); it!=liste->end(); ++it)
      {
	//Calcul de l'erreur pour le couple par rapport a tx0,ty0;
	double e=CalculeErreur(*it,tx0,ty0);
	if (e<SeuilErreur) it->setSelected(1);
	else it->setSelected(0);
      }
    }
  }
  
  //Calcul final de la translation avec les inliers
  CalculeTranslationCouplesSelected(ListeCouples,tx,ty);
  return NbInliersMax;
}