/**
 * \file rectification.c
 * \brief This file do the rectification.
 *
 *
 *  References:
 *  http://www.cs.unc.edu/~blloyd/comp290-089/fmatrix/
 *
 *
 *  \ingroup corefiles
 *  \date Jun 9, 2009
 *  \author Alexander Pimenov
 */

#include "global.h"

#include <string.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include "rectification.h"
#include "matrix.h"
#include "svd.h"

#undef TRACE


/**
 * \brief This function approximates the input matrix with rank 2 matrix to remove the noise
 * form Fundamental Matrix
 *
 * The algorithm is - decompose the F matrix with SVD. Take the smallest singular value and replace it with 0
 *
 * \param[in,out] F The matrix that should be approximated with rank 2
 *
 */
void assertRank2(Matrix *F)
{
#ifdef ASSERTS
	if (F->m != 3 || F->n != 3)
		DOASSERT("Wrong matrix size. Expected to be 3 by 3");
#endif
	Matrix *W = matrixCreate(1,3);
	Matrix *V = matrixCreate(3,3);

	Matrix *Wbig = matrixCreate(3,3);
	Matrix *VT = matrixCreate(3,3);

	Matrix *result = matrixCreate(3,3);

	singularVectorDecompose(F, W, V);

#ifdef TRACE
	printf("The singular values...\n");
	matrixPrint(W);
#endif

	// Leave out the last singular number. To assert that rank is 2
	int i;
	int min = -1;
	double minval = 1e10;

	int max = -1;
	double maxval = -1e10;

	for (i = 0; i < 3; i ++)
	{
		if (VECELEM(W, i) <= minval)
		{
			min = i;
			minval = VECELEM(W, i);
		}

		if (VECELEM(W, i) >= maxval)
		{
			max = i;
			maxval = VECELEM(W, i);
		}
	}

	for (i = 0; i < 3; i ++)
	{
		if (i != min)
			ELEM(Wbig, i, i) = ELEM(W, 0, i) / maxval;
	}

	matrixTranspose(V, VT);

	// Reconstruct the matrix
	matrixMultiply(F, Wbig, result);
	matrixMultiply(result, VT, F);

	matrixDelete(W);
	matrixDelete(Wbig);
	matrixDelete(V);
	matrixDelete(VT);
}


/**
 * This function finds the both epipoles for a given fundamental matrix.
 *
 * In fact it searches for a left and right null-space.
 *
 * \f[
 * 		\pmatrix{ E1_x & E1_y & E1_z}
 *      \pmatrix{
 *        F_{1,1} & F_{1,2} & F_{1,3}\cr
 *        F_{2,1} & F_{2,2} & F_{2,3}\cr
 *        F_{3,1} & F_{3,2} & F_{3,3}} = \pmatrix{ 0 \cr 0 \cr 0}
 * \f]
 * \f[
 *      \pmatrix{
 *        F_{1,1} & F_{1,2} & F_{1,3}\cr
 *        F_{2,1} & F_{2,2} & F_{2,3}\cr
 *        F_{3,1} & F_{3,2} & F_{3,3}}
 * 		\pmatrix{ E2_x \cr E2_y \cr E2_z} = \pmatrix{ 0 \cr 0 \cr 0}
 * \f]
 *
 *
 */
void findEpipole(projectiveTransform2D *F, vector3Dd *e1, vector3Dd* e2)
{
	Matrix *M = matrixCreateFromRaw(3,3, F->data);
	findEpipoleFromMatrix(M, e1, e2);
	matrixDelete(M);
}


/**
 * This function finds the both epipoles by the fundamental matrix
 *
 * \deprecated
 *
 */
void findEpipoleFromMatrix(Matrix *F, vector3Dd *e1, vector3Dd* e2)
{
	Matrix *U = matrixCreate(3,3);
	Matrix *W = matrixCreate(1,3);
	Matrix *V = matrixCreate(3,3);

	matrixCopy(F, U);
    singularVectorDecompose(U, W, V);
#ifdef TRACE
    printf("U matrix\n");
	matrixPrint(U);
    printf("W matrix\n");
	matrixPrint(W);
    printf("V matrix\n");
	matrixPrint(V);
#endif

	int i;
	int min = -1;
	double minval = 1e10;

	for (i = 0; i < 3; i ++)
	{
		if (ELEM(W, 0, i) <= minval)
		{
			min = i;
			minval = ELEM(W, 0, i);
		}
	}

	printf("Minimum singular value is:%lg", minval);
	e1->x = ELEM(V, 0, min);
	e1->y = ELEM(V, 1, min);
	e1->z = ELEM(V, 2, min);

	e2->x = ELEM(U, 0, min);
	e2->y = ELEM(U, 1, min);
	e2->z = ELEM(U, 2, min);

	matrixDelete(U);
	matrixDelete(W);
	matrixDelete(V);

}

//#define TRACE
//#define ASSERTS


vector3Dd getBestZ(projectiveTransform2D *F, vector3Dd *E1 )
{
	double h = 400;
	double w = 500;

	projectiveTransform2D PP = getProjective2DRaw(
			(w * w - 1) / (w*h / 12),                          0,   0,
				                   0,   (h * h - 1) / (w*h / 12),   0,
				                   0,                          0,   0);

	projectiveTransform2D PCPC = getProjective2DRaw(
			(w - 1) * (w - 1) , (w - 1) * (h - 1), 2 * (w - 1),
			(w - 1) * (h - 1) , (h - 1) * (h - 1), 2 * (h - 1),
			2 * (w - 1)       ,       2 * (h - 1),   4);


	vector3Dd toReturn = {1,1,1};
	return toReturn;
}

vector3Dd getBestZLMA(projectiveTransform2D *F, vector3Dd *E1 )
{
        vector3Dd toReturn = {1,0,0};
        return toReturn;
}


double getNormalisingTransformation(Correspondance *points, int pointNumber, projectiveTransform2D *left, projectiveTransform2D *right )
{
	vector2Dd lmean = {0,0};
	vector2Dd lmeansq = {0,0};
	vector2Dd rmean = {0,0};
	vector2Dd rmeansq = {0,0};

	int i = 0;
	for ( i = 0; i < pointNumber; i++)
	{
		lmean.x += points[i].start.x;
		lmean.y += points[i].start.y;

		lmeansq.x += points[i].start.x * points[i].start.x;
		lmeansq.y += points[i].start.y * points[i].start.y;

		rmean.x += points[i].end.x;
		rmean.y += points[i].end.y;

		rmeansq.x += points[i].end.x * points[i].end.x;
		rmeansq.y += points[i].end.y * points[i].end.y;
	}
	lmean.x /= pointNumber;
	lmean.y /= pointNumber;
	lmeansq.x /= pointNumber;
	lmeansq.y /= pointNumber;
	rmean.x /= pointNumber;
	rmean.y /= pointNumber;
	rmeansq.x /= pointNumber;
	rmeansq.y /= pointNumber;


	double lscaler = sqrt (lmeansq.x - lmean.x * lmean.x + lmeansq.y - lmean.y * lmean.y);
	double rscaler = sqrt (rmeansq.x - rmean.x * rmean.x + rmeansq.y - rmean.y * rmean.y);


	*left = multiplyBy2D(getScale2D(sqrt(2.0f) / lscaler, sqrt(2.0f) / lscaler), getShift2D(-lmean.x, -lmean.y));
	*right = multiplyBy2D(getScale2D(sqrt(2.0f) / rscaler, sqrt(2.0f) / rscaler), getShift2D(-rmean.x, -rmean.y));

	return sqrt(2.0f) / ( (lscaler + rscaler) * 2) ;
}

void transformCorrespondence(Correspondance *points, int pointNumber, projectiveTransform2D *left, projectiveTransform2D *right)
{
	int i = 0;
	for ( i = 0; i < pointNumber; i++)
	{
		points[i].start = projective2DapplyToCartesian(left, points[i].start);
		points[i].end = projective2DapplyToCartesian(right, points[i].end);
	}
}



int compareDouble (const void *a, const void *b)
{
	double *a1 = (double *)a;
	double *b1 = (double *)b;

	double res = *a1 - *b1;
	if (res > 0)
		return 1;
	if (res < 0)
		return -1;
	return 0;
}

/**
 *
 * This function implements the simple rectification.
 * It is not optimal in any scene.
 *
 *
 *
 */

void getOptimalProjectiveTransform(projectiveTransform2D *F, projectiveTransform2D *Pleft, projectiveTransform2D *Pright, vector3Dd z)
{
        vector3Dd E1;
        vector3Dd E2;
        findEpipole(F, &E1, &E2);

        //vector3Dd z = getBestZLMA(F, &E1 );

        vector3Dd w1 = crossProduct(&z,&E1);
        vector3Dd w2 = projective2Dapply(F, &z);
        vectorHomogeniousNormalize(&w1);
        vectorHomogeniousNormalize(&w2);

        *Pleft = getProjective2D(w1.x, w1.y);
        *Pright = getProjective2D(w2.x, w2.y);
}

void getOptimalAffineTransform(projectiveTransform2D *F, projectiveTransform2D *Aleft, projectiveTransform2D *Aright, vector3Dd z)
{
        vector3Dd E1;
        vector3Dd E2;
        findEpipole(F, &E1, &E2);

        //vector3Dd z = getBestZLMA(F, &E1 );

        vector3Dd w1 = crossProduct(&z,&E1);
        vector3Dd w2 = projective2Dapply(F, &z);
        vectorHomogeniousNormalize(&w1);
        vectorHomogeniousNormalize(&w2);

        double wc = 0;
        double a,b;

        a = F->data[7] - w1.y * F->data[8];
        b = w2.x * F->data[8] - F->data[6];
        *Aleft = getProjective2DRaw(
                 a,  b,  0,
                -b,  a,  F->data[8] + wc,
                 0,  0,  1 );

        a = w2.y * F->data[8] - F->data[5];
        b = F->data[2] - w2.x * F->data[8];
        *Aright= getProjective2DRaw(
                 a,  b,  0,
                -b,  a,  wc,
                 0,  0,  1 );


}

void getScaleTransform(projectiveTransform2D *F, projectiveTransform2D *Aleft, projectiveTransform2D *Aright, projectiveTransform2D *S, vector3Dd z)
{
        double d1 = sqrt(projective2Ddeterminant(Aleft));
        double d2 = sqrt(projective2Ddeterminant(Aright));
        double scaleFactor = d1 > d2 ? d1 : d2;

        printf("Scale factor %lg\n", scaleFactor);
        *S =  getScale2D(1 / scaleFactor,1 / scaleFactor);

}

/*void getOptimalScaleTransform(projectiveTransform2D *F, projectiveTransform2D *APleft, projectiveTransform2D *APright, projectiveTransform2D *Sleft, projectiveTransform2D *Sright, vector3Dd z)
{
        double h = 400;
        double w = 500;

        vector2Dd xl = projective2DapplyToCartesian(&APleft, points[0].start);
        vector2Dd yl = projective2DapplyToCartesian(&APleft, points[0].end);
        vector2Dd xr = projective2DapplyToCartesian(&APright, points[0].start);
        vector2Dd yr = projective2DapplyToCartesian(&APright, points[0].end);

        double al = (h*h*xl.y*xl.y + w*w*yl.y*yl.y)/(h*w*(xl.y*yl.x - xl.x*yl.y));
        double bl = (h*h*xl.y*xl.x + w*w*yl.y*yl.y)/(h*w*(xl.x*yl.y - xl.y*yl.x));
        if (al<0)
        {
            al = -al;
            bl = -bl;
        }
        double ar = (h*h*xr.y*xr.y + w*w*yr.y*yr.y)/(h*w*(xr.y*yr.x - xr.x*yr.y));
        double br = (h*h*xr.y*xr.x + w*w*yr.y*yr.y)/(h*w*(xr.x*yr.y - xr.y*yr.x));
        if (ar<0)
        {
            ar = -ar;
            br = -br;
        }

        projectiveTransform2D Sleft =  getProjective2DRaw(
                 al,  bl,  0,
                 0,  1,    0,
                 0,  0,  1 );
        projectiveTransform2D Sright =	getProjective2DRaw(
                 ar,  br,  0,
                 0,  1,    0,
                 0,  0,  1 );

        *leftTranstorm = multiplyBy2D(Sleft, Aleft);
        *rightTranstorm = multiplyBy2D(Sright, Aright);
}
*/
void getRectificationTransformationSimple1(projectiveTransform2D *F, Correspondance *points, int pointNumber, projectiveTransform2D *leftTranstorm, projectiveTransform2D *rightTranstorm, vector3Dd z)
{


//	assertRank2(F);
	vector3Dd E1;
	vector3Dd E2;
	findEpipole(F, &E1, &E2);


#ifdef TRACE
	printf("Fundamental Matrix:\n");
	projective2Dprint(F);
	printf("Epipoles:\n");
	vectorPrint(&E1);
	vectorPrint(&E2);
#endif

	/// \todo TODO: In fact this vector should be guessed
	//vector3Dd z = {-0.75,0.75,0};
	//vector3Dd z = {0.75,-0.75,0};
        //vector3Dd z = {1,0,0};


	vector3Dd w1 = crossProduct(&z,&E1);
	vector3Dd w2 = projective2Dapply(F, &z);

#ifdef TRACE
	printf("Projective correctives:\n");
	printf("[%lg : %lg : %lg ]\n", w1.x, w1.y, w1.z);
	printf("[%lg : %lg : %lg ]\n", w2.x, w2.y, w2.z);
#endif

	vectorHomogeniousNormalize(&w1);
	vectorHomogeniousNormalize(&w2);

#ifdef TRACE
	printf("Checks:\n");
	printf("Dot product for 1st corrector: %lg\n", dotProduct(&E1,&w1));
	printf("Dot product for 2nd corrector: %lg\n", dotProduct(&E2,&w2));
#endif

	projectiveTransform2D Pl = getProjective2D(w1.x, w1.y);
	projectiveTransform2D Pr = getProjective2D(w2.x, w2.y);

	double wc = 0;
	double a,b;


	a = F->data[7] - w1.y * F->data[8];
	b = w2.x * F->data[8] - F->data[6];
	projectiveTransform2D Rl = getProjective2DRaw(
		 a,  b,  0,
		-b,  a,  F->data[8] + wc,
		 0,  0,  1 );

	a = w2.y * F->data[8] - F->data[5];
	b = F->data[2] - w2.x * F->data[8];
	projectiveTransform2D Rr = getProjective2DRaw(
		 a,  b,  0,
		-b,  a,  wc,
		 0,  0,  1 );

	projectiveTransform2D left =  multiplyBy2D(Rl, Pl);
	projectiveTransform2D right =  multiplyBy2D(Rr, Pr);

#ifdef ASSERTS
	projectiveTransform2D Fprim;

	projectiveTransform2D Ix = getProjective2DRaw(
			 0,  0,  0,
			 0,  0, -1,
			 0,  1,  0 );

	Fprim = projective2Dtranspose(right);
	Fprim = multiplyBy2D(Fprim, Ix);
	Fprim = multiplyBy2D(Fprim, left);

	printf("Recollect\n");
	projective2Dprint(&Fprim);
	printf("Old Matrix was\n");
	projective2Dprint(F);
#endif // ASSERTS
#ifdef TRACE
	printf("Rotator1 %lg:\n", projective2Ddeterminant(&Rl));
	projective2Dprint(&Rl);
	printf("Rotator2 %lg:\n", projective2Ddeterminant(&Rr));
	projective2Dprint(&Rr);
#endif

	double d1 = sqrt(projective2Ddeterminant(&Rl));
	double d2 = sqrt(projective2Ddeterminant(&Rr));
	double scaleFactor = d1 > d2 ? d1 : d2;

	printf("Scale factor %lg\n", scaleFactor);
	projectiveTransform2D scale =  getScale2D(1 / scaleFactor,1 / scaleFactor);
	//projectiveTransform2D scale =  getScale2D(15,15);

	projectiveTransform2D Sleft =  multiplyBy2D(scale, left);
	projectiveTransform2D Sright =	multiplyBy2D(scale, right);

	/* Shift the border for disparity being > 0 */


	//Sright = multiplyBy2D(getShift2D(52, 0), Sright);

	//projectiveTransform2D SleftInv =  projective2D+inverse(&Sleft);
	//projectiveTransform2D SrightInv =  projective2Dinverse(&Sright);

	*leftTranstorm = Sleft;
	*rightTranstorm = Sright;
}

void getRectificationTransformationOptimal(projectiveTransform2D *F, Correspondance *points, int pointNumber, projectiveTransform2D *leftTranstorm, projectiveTransform2D *rightTranstorm, vector3Dd z)
{

        projectiveTransform2D Pl,Pr;
        getOptimalProjectiveTransform(F, &Pl, &Pr, z);
        
        projectiveTransform2D Al,Ar;
        getOptimalAffineTransform(F, &Al, &Ar, z);
        projectiveTransform2D APl =  multiplyBy2D(Al, Pl);
        projectiveTransform2D APr =  multiplyBy2D(Ar, Pr);

        projectiveTransform2D S;
        getScaleTransform(F, &Al, &Ar, &S, z);
        *leftTranstorm =  multiplyBy2D(S, APl);
        *rightTranstorm =  multiplyBy2D(S, APr);

}


/**
 *
 *
 *
 *
 */
double doLateralCorrection(Correspondance *points, int pointNumber, projectiveTransform2D *leftTranstorm, projectiveTransform2D *rightTranstorm)
{
	double min = 1e10;
	double max = -1e10;
	int i;
	int refs = 0;

	int count = 0;
	for (i = 0; i < pointNumber; i++)
	{
		if (!points[i].flags)
			continue;
		count++;
	}

	double *diffs = (double *) dv_malloc(count * sizeof(double));

	int j = 0;
	for (i = 0; i < pointNumber; i++)
	{
		if (!points[i].flags)
			continue;
		refs++;
		vector2Dd source = projective2DapplyToCartesian(leftTranstorm, points[i].start);
		vector2Dd dest =  projective2DapplyToCartesian(rightTranstorm, points[i].end);
		//printf("[%lf, %lf] -> [%lf, %lf]\n", source.x, source.y, dest.x, dest.y);
		if (dest.x - source.x < min)
			min = dest.x - source.x;
		if (dest.x - source.x > max)
			max = dest.x - source.x;
		diffs[j] = dest.x - source.x;
		j++;
	}

	qsort(diffs, count, sizeof(double), compareDouble);
	FILE *Out = fopen("graph.csv", "wt");
	for (i = 0; i < count; i++)
	{
		fprintf(Out,"%d,%lf\n", i, diffs[i]);
		//printf("%d,%lf\n", i, diffs[i]);
	}
	fclose(Out);

	printf("Shift is based on %d points\n", refs);
	printf("min disparity over the reference points is %lf, max %lf\n", min, max);
	//max = 100;
	*leftTranstorm = multiplyBy2D(getShift2D(max / 2, 0), *leftTranstorm);
	*rightTranstorm = multiplyBy2D(getShift2D(-max / 2, 0), *rightTranstorm);

	// Second Result
	min = 1e10;
	max = -1e10;
	j = 0;
	for (i = 0; i < pointNumber; i++)
	{
		if (!points[i].flags)
			continue;
		refs++;
		vector2Dd source = projective2DapplyToCartesian(leftTranstorm, points[i].start);
		vector2Dd dest =  projective2DapplyToCartesian(rightTranstorm, points[i].end);
		//printf("[%lf, %lf] -> [%lf, %lf]\n", source.x, source.y, dest.x, dest.y);
		if (dest.x - source.x < min)
			min = dest.x - source.x;
		if (dest.x - source.x > max)
			max = dest.x - source.x;
		diffs[j] = dest.x - source.x;
		j++;
	}

	qsort(diffs, count, sizeof(double), compareDouble);
	/*Out = fopen("graph1.csv", "wt");
	for (i = 0; i < count; i++)
	{
		fprintf(Out,"%d,%lf\n", i, diffs[i]);
		//printf("%d,%lf\n", i, diffs[i]);
	}
	fclose(Out);*/

	printf("min disparity over the reference points is %lf, max %lf\n", min, max);
	return min;
}



void getLateralCorrection(
		Correspondance *points,
		int pointNumber,
		projectiveTransform2D *leftTranstorm,
		projectiveTransform2D *rightTranstorm,
		double *minimum,
		double *maximum)
{
	double min = 1e10;
	double max = -1e10;
	int i;
	int refs = 0;

	int count = 0;
	for (i = 0; i < pointNumber; i++)
	{
		if (!points[i].flags)
			continue;
		count++;
	}

	double *diffs = (double *) dv_malloc(count * sizeof(double));

	int j = 0;
	for (i = 0; i < pointNumber; i++)
	{
		if (!points[i].flags)
			continue;
		refs++;
		vector2Dd source = projective2DapplyToCartesian(leftTranstorm, points[i].start);
		vector2Dd dest =  projective2DapplyToCartesian(rightTranstorm, points[i].end);
		//printf("[%lf, %lf] -> [%lf, %lf]\n", source.x, source.y, dest.x, dest.y);
		if (dest.x - source.x < min)
			min = dest.x - source.x;
		if (dest.x - source.x > max)
			max = dest.x - source.x;
		diffs[j] = dest.x - source.x;
		j++;
	}

	qsort(diffs, count, sizeof(double), compareDouble);

	printf("Shift is based on %d points\n", refs);
	printf("min disparity over the reference points is %lf, max %lf\n", min, max);
	*minimum = min;
	*maximum = max;
}

/**
 * \brief The calculation of the fundamental matrix of two stereo images.
 * This algorithm uses RANSAC.
 *
 * \code
 *             [ a_11   a_12  a_13 ]  [ x']    [ 0 ]
 * [x, y ,1]   [ a_21   a_22  a_23 ]  [ y'] =  [ 0 ]
 *             [ a_31   a_32  a_33 ]  [ 1 ]    [ 0 ]
 * \endcode
 *
 * \f$
 * 		\pmatrix{ x & y & 1}
 *      \pmatrix{
 *        F_{1,1} & F_{1,2} & F_{1,3}\cr
 *        F_{2,1} & F_{2,2} & F_{2,3}\cr
 *        F_{3,1} & F_{3,2} & F_{3,3}}
 *      \pmatrix{ x' \cr y' \cr 1} = \pmatrix{ 0 \cr 0 \cr 0}
 * \f$
 *
 *
 * The inliers calculation is as follows:
 *  1. We first get the epipolar line for a left input point
 *
 *  \f[
 *  	\pmatrix{ x & y & 1 }
 *      \pmatrix{
 *        F_{1,1} & F_{1,2} & F_{1,3}\cr
 *        F_{2,1} & F_{2,2} & F_{2,3}\cr
 *        F_{3,1} & F_{3,2} & F_{3,3}} = \pmatrix{ a \cr b \cr c }
 *  \f]
 *
 *  Here is the equation of the line - \f$ax' + by' + c = 0\f$. And now we want to know how far is the real right point from the predicted by the current model -
 *
 *   \f[ D = \frac{\left | a x' + b y' + c \right |}{\sqrt{a^2+b^2}}. \f]
 *
 *  When we get the distance we can say if the point is to far from the model and is not the inlier or close enought.
 *
 *
 *
 *
 *
 *
 * \param[in, out] points  the corresponding points pairs
 * \param[in] pointNumber the number of input pairs
 * \param[in] trySize  the size of the minimal set for RANSAC. Should not be less then 8. Typically should be around 9-11.
 * \param[in] maxIterations  maximum number of iterations for RANSAC
 * \param[in] treshold a threshold to consider the pair satisfies the model
 *
 * \return the fundamental matrix.
 *
 */

Matrix* rectifyRANSAC (Correspondance *points, int pointNumber, int trySize, int maxIterations, double treshold)
{
#ifdef ASSERTS
	if (trySize > pointNumber)
	{
		DOASSERT("The selection is bigger then the number of inputs. Algorithm will not work");
	}
	if (trySize < 8)
	{
		DOASSERT("The selection should be bigger then 8 for a stable solution. Algorithm will not work");
	}
	if (treshold < 0)
	{
		DOASSERT("The threshold should be positive. Algorithm will not work");
	}
#endif

	int count = 0;

	Correspondance **choice = (Correspondance **)dv_malloc(trySize * sizeof(Correspondance *));
	int *passers = (int *)dv_malloc(pointNumber * sizeof(int));
	int *bestPassers = (int *)dv_malloc(pointNumber * sizeof(int));

	int bestInliers = 0;


	Matrix *X = matrixCreate(trySize, 9);
	Matrix *W = matrixCreate(1,9);
	Matrix *V = matrixCreate(9,9);
	Matrix *F = matrixCreate(3,3);
	Matrix *bestF = matrixCreate(3,3);

#ifdef DEEP_TRACE
	Matrix *Xcopy = matrixCreate(trySize, 9);
	Matrix *Wbig = matrixCreate(9, 9);
	Matrix *VT = matrixCreate(9,9);
	Matrix *res1 = matrixCreate(trySize, 9);
	Matrix *res2 = matrixCreate(trySize, 9);
	Matrix *res3 = matrixCreate(trySize, 9);
#endif

	srand(0x5EED);
	while (count < maxIterations)
	{
		int i;
		/*Select randomly a group of trySize. */

		for (i = 0; i < pointNumber; i++)
		{
			points[i].flags = 0;
		}

		for (i = 0; i < trySize; i++)
		{
			int index;
			do {
				index = (rand() % pointNumber);
			} while (points[index].flags != 0);

			points[index].flags = 1;
			choice[i] = &(points[index]);
		}

		/* Calculate the fundamental matrix */

		for (i = 0; i < trySize; i++) {
		   vector2Dd *first =  &(choice[i]->start);
		   vector2Dd *second = &(choice[i]->end);

		   ELEM(X,i,0) = first->x * second->x;
		   ELEM(X,i,1) = first->x * second->y;
		   ELEM(X,i,2) = first->x;

		   ELEM(X,i,3) = first->y * second->x;
		   ELEM(X,i,4) = first->y * second->y;
		   ELEM(X,i,5) = first->y;

		   ELEM(X,i,6) = second->x;
		   ELEM(X,i,7) = second->y;
		   ELEM(X,i,8) = 1;
		}


		/* TODO: Here the transform should be done to increase the precision.
		 * The choice should be centered to 0 and scaled to average of sqrt(2) */

#ifdef DEEP_TRACE
		printf("X Matrix");
		matrixPrint(X);
		matrixClone(X, Xcopy);
#endif
		singularVectorDecompose(X, W, V);

#ifdef DEEP_TRACE
		printf("V Matrix");
		matrixPrint(V);
		printf("The singular values...\n");
		matrixPrint(W);

		for (i = 0; i < 9; i ++)
			ELEM(Wbig, i, i) = ELEM(W, 0, i);

		matrixTranspose(V, VT);
		matrixMultiply(X, Wbig, res1);
		matrixMultiply(res1, VT, res2);
		matrixSubstract(Xcopy, res2, res3);
		printf("Check matrix:\n");
		matrixPrint(res3);
#endif


		int min = -1;
		double minval = 1e10;
		for (i = 0; i < 9; i ++)
		{
			if (ELEM(W, 0, i) <= minval)
			{
				min = i;
				minval = ELEM(W, 0, i);
			}
		}

		ELEM(F,0,0) = ELEM(V, 0, min);
		ELEM(F,0,1) = ELEM(V, 1, min);
		ELEM(F,0,2) = ELEM(V, 2, min);

		ELEM(F,1,0) = ELEM(V, 3, min);
		ELEM(F,1,1) = ELEM(V, 4, min);
		ELEM(F,1,2) = ELEM(V, 5, min);

		ELEM(F,2,0) = ELEM(V, 6, min);
		ELEM(F,2,1) = ELEM(V, 7, min);
		ELEM(F,2,2) = ELEM(V, 8, min);

#ifdef DEEP_TRACE
		printf("Fundamental Matrix\n");
		matrixPrint(F);
#endif

		assertRank2(F);
#ifdef TRACE
		printf("Fundamental Matrix for iteration %d :\n", count);
		matrixPrint(F);
#endif

		int inliers = 0;

		// Count the number of inliers
		for (i = 0; i < pointNumber; i++) {
		   vector2Dd *first =  &(points[i].end);
		   vector2Dd *second = &(points[i].start);

		   double epx = ELEM(F, 0, 0) * first->x + ELEM(F, 0, 1) * first->y + ELEM(F, 0, 2);
		   double epy = ELEM(F, 1, 0) * first->x + ELEM(F, 1, 1) * first->y + ELEM(F, 1, 2);
		   double epz = ELEM(F, 2, 0) * first->x + ELEM(F, 2, 1) * first->y + ELEM(F, 2, 2);

		   double scale = sqrt(epx * epx + epy * epy);
		   double res = (epx * second->x + epy * second->y + epz) / scale;

		   if (fabs(res) < treshold)
		   {
		      inliers ++;
		      passers[i] = 1;
		   } else {
			  passers[i] = 0;
		   }
		}

#ifdef TRACE
		printf("Inliers number is %d from %d\n ", inliers, pointNumber);
#endif

		if (inliers > bestInliers)
		{
			bestInliers = inliers;
			// TODO: No copy needed in fact...
			memcpy(bestPassers, passers, pointNumber * sizeof(int));
			matrixCopy(F, bestF);
		}
		count++;
	}


	//free (choice);
	matrixDelete(X);
	matrixDelete(V);
	matrixDelete(W);
	//*in_choice = best_choice;

#ifdef DEEP_TRACE
	matrixDelete(Xcopy);
	matrixDelete(Wbig);
	matrixDelete(VT);
	matrixDelete(res1);
	matrixDelete(res2);
	matrixDelete(res3);
#endif

	int i;
	printf("After %d iteration best Inliers number is %d form %d which is %d%%\n", maxIterations, bestInliers, pointNumber, bestInliers * 100 / pointNumber);
	for (i = 0; i < pointNumber; i++)
	{
		points[i].flags = bestPassers[i];
	}

	dv_free(passers);
	dv_free(bestPassers);
	dv_free(choice);
	return bestF;

}

void transformPoint(double x, double y, Matrix* F, double *rx, double *ry)
{
	//double x1 =  ELEM(F, 0, 0) * x + ELEM(F, 0, 1) * y + ELEM(F, 0, 2);
	//double y1 =  ELEM(F, 1, 0) * x + ELEM(F, 1, 1) * y + ELEM(F, 1, 2);
	//double z1 =  ELEM(F, 2, 0) * x + ELEM(F, 2, 1) * y + ELEM(F, 2, 2);
	/* Now we have the equation x1 * rx + y1 * ry + z1 * rz = 0 */

}

Matrix* rectifyAffineRANSAC (Correspondance *points, int pointNumber, int trySize, int maxIterations, double treshold)
{

	return NULL;
}


