/*
 * \file main_test_svd.c
 *
 * \date Jun 16, 2009
 * \author: Alexander Pimenov
 */

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

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

int main1 (int argC, char **argV)
{
	double Adata[16] =
	{ 7,      9,   10, 7.6,
	  -5, 0.0001,    6, 2345,
	   6,     45,  234, -34,
	  657, 0.0001, 0, -1};

	Matrix *A = matrixCreateFromRaw(4,4, Adata);
	Matrix *Acopy = matrixCreate(4,4);
	matrixCopy(A, Acopy);
	Matrix *W = matrixCreate(1,4);
	Matrix *Wbig = matrixCreate(4,4);
	Matrix *V = matrixCreate(4,4);
	Matrix *VT = matrixCreate(4,4);

	Matrix *result = matrixCreate(4,4);
	Matrix *result1 = matrixCreate(4,4);


	singularVectorDecompose(A, W, V);
	int i;
	for (i = 0; i < 4; i ++)
		ELEM(Wbig, i, i) = ELEM(W, 0, i);

	matrixTranspose(V, VT);
	matrixPrint(A);
	matrixPrint(Wbig);
	matrixPrint(V);

	printf("TESTING...");
	matrixMultiply(A, Wbig, result);
	matrixMultiply(result, VT, result1);

	matrixPrint(result1);
	printf("Should be...");
	matrixPrint(Acopy);

	return 0;
}
/*
int main2 (int argC, char **argV)
{


	vector2Dd pairs[28] =
	{
	   {0.003 , 0.0025}, {0.003 , 0.0125 },
	   {0.100 , 0.100 }, {0.100 , 0.110},
	   {0.200 , 0.200 }, {0.200 , 0.210},
	   {0     , 0.100 }, {0     , 0.110},
	   {0.100 , 0     }, {0.100 , 0.01},
	   {0     , 0.200 }, {0     , 0.210},
	   {0.200 , 0     }, {0.200 , 0.01},
	   {0.100 , 0.200 }, {0.100 , 0.210},
	   {0.200 , 0.100 }, {0.200 , 0.110},
	   {0.300 , 0     }, {0.300 , 0.01},
	   {0     , 0.300 }, {0     , 0.310},
	   {0.400 , 0     }, {0.400 , 0.01},
	   {0.300 , 0.400 }, {0.300 , 0.410},
	   {0.300 , 0.100 }, {0.300 , 0.110}

	};

	rectify(pairs, 14);

	return 0;
}*/


#define GRID_STEP (10)

#define GRID_SIZE (GRID_STEP * GRID_STEP)


int main (int argC, char **argV)
{

	Correspondance *points = (Correspondance *) malloc(GRID_SIZE * sizeof(Correspondance));
	int i,j;

	for (i = 0; i < GRID_STEP; i++)
	{
		for (j = 0; j < GRID_STEP; j++)
		{
			Correspondance * corr = &(points[i * GRID_STEP + j]);
			double step = 1.0 / (GRID_STEP - 1);
			double x = (step * j) - 0.5;
			double y = (step * i) - 0.5;
			double alpha = 0.1;
			double sa = sin (alpha);
			double ca = cos (alpha);

			corr->start.x = x;
			corr->start.y = y;

			corr->end.x =  x * ca + y * sa + 0.01;
			corr->end.y = -x * sa + y * ca - 0.1 ;

			corr->flags = 0;
			corr->value = 1;
		}
	}


	/*vector2Dd pairs[18] =
	{
	   {0, 0}, {0,0 },
	   {100, 100}, {100, 100},
	   {200, 200}, {200, 200},
	   {0, 100}, {0, 100},
	   {100, 0}, {100, 0},
	   {0, 200}, {0,200},
	   {200, 0}, {200,0},
	   {100,200}, {100,200},
	   {200,100}, {200,100}
	};*/

	srand(1);
	Matrix *F = rectifyRANSAC(points, GRID_SIZE, 9, 100, 0.01);
	matrixPrint(F);
	return 0;
}
