/**
 * \file main_test_matrix.c
 * \brief This file unit tests the linear equation solving, Levenberg-Marquardt algorithm and matrix manipulation
 *
 *
 * \date May 22, 2009
 * \author Alexander Pimenov
 */

#include "matrix.h"
#include "levmar.h"
#include "math.h"

double realInput[4] = {0.001, 0.00002, 0.01, 0.045};

#define GRID_SIZE 3

/* Some testing for LM */
double LMfunctionAl(int innum, double input[])
{

	int num = innum / 2;
	int dy = (num / GRID_SIZE) - (GRID_SIZE / 2);
	int dx = (num % GRID_SIZE) - (GRID_SIZE / 2);
	double dxsq = dx * dx;
	double dysq = dy * dy;
	double dxdy = dx * dy;

	double rsq = dxsq + dysq;
	double radialCorrection = input[0] * rsq +  input[1] * rsq * rsq;

	double radialX = (double)dx * radialCorrection;
	double radialY = (double)dy * radialCorrection;

	double tangentX = 2 * input[2] * dxdy + input[3] * ( rsq + 2 * dxsq );
	double tangentY = input[2] * (rsq + 2 * dysq) + 2 * input[3] * dxdy;

	double x = radialX + tangentX;
	double y = radialY + tangentY;
	if (innum % 2 == 0)
		return x;
	else
		return y;
}

double LMYAl(int num)
{
	//double input[4] = {0.001, 0.00002, 0.01, 0.045};
	return LMfunctionAl(num, realInput);
}

double LMJAl1(int innum, int component, double input[])
{
	double step = 0.04;
	double value = LMfunctionAl(innum, input);
	double locals[4];
	memcpy(locals, input, sizeof(double) * 4);
	locals[component] = input[component] + step;

	double fnew = LMfunctionAl(innum, locals);
	double derold = (fnew - value) / step;
	double dernew;
	do {
		locals[component] = input[component] + step;
		double fnew = LMfunctionAl(innum, locals);
		dernew = (fnew - value) / step;
		step /= 1.5;
	} while (fabs(dernew - derold) > 0.0000000000001);
	return dernew;
}


double LMJAl(int innum, int component, double input[])
{

	int num = innum / 2;
	int dy = (num / GRID_SIZE) - (GRID_SIZE / 2);
	int dx = (num % GRID_SIZE) - (GRID_SIZE / 2);
	double dxsq = dx * dx;
	double dysq = dy * dy;
	double dxdy = dx * dy;

	double rsq = dxsq + dysq;

	if (innum % 2 == 0)
	{
		switch(component)
		{
			case 0:
				return (double)dx * rsq;
				break;
			case 1:
				return (double)dx * rsq * rsq;
				break;
			case 2:
				return 2 * dxdy;
				break;
			case 3:
				return rsq + 2 * dxsq;
				break;
		}
	}
	else
	{
		switch(component)
		{
			case 0:
				return (double)dy * rsq;
				break;
			case 1:
				return (double)dy * rsq * rsq;
				break;
			case 2:
				return rsq + 2 * dysq;
				break;
			case 3:
				return 2 * dxdy;
				break;
		}
	}
}



int main (int argC, char** argV)
{
	Matrix *A = matrixCreate(3,3);
	Matrix *Aclone = matrixCreate(3,3);

	Matrix *B = matrixCreate(3,1);
	Matrix *result = matrixCreate(3,1);

	matrixFillIdentity(A);
	A->data[1] = 1;

	matrixClone(A, Aclone);


	B->data[0] = 5;
	B->data[1] = 6;
	B->data[2] = 7;

	matrixSolveGaussian(A, B);
	matrixPrint(B);
	matrixMultiply(Aclone, B, result);
	matrixPrint(result);

	double guess[4] = {0.0015, 0.000025, 0.015, 0.040};
	double guess1[4];
	memcpy(guess1, guess, 4 * sizeof (double));

	doLMFit(GRID_SIZE * GRID_SIZE * 2, 4, LMfunctionAl, LMYAl, LMJAl1, guess);

	int i;
	printf("\n");
	for (i = 0; i < 4; i++)
	{
		printf("%lf ", guess1[i]);
	}
	printf("\n");
	for (i = 0; i < 4; i++)
	{
		printf("%lf ", guess[i]);
	}
	printf("\n");
	for (i = 0; i < 4; i++)
	{
		printf("%lf ", realInput[i]);
	}



	return 0;
}

