#include "ARMFCaptureD3D.h"
#include "Calibration.h"
#include <math.h>
#include <assert.h>
#include "debug.h"
#include "ConfigFile\ConfigFile.h"

#define DUMP_CALIBRATION

Calibration::Calibration(double width, double height, double calibInset)
{
	//dot is the original calibration image
	dots[TopLeft][0] = calibInset; //top left X
	dots[TopLeft][1] = calibInset; //top left Y
	dots[TopRight][0] = width - calibInset; //top right
	dots[TopRight][1] = calibInset; //top right
	dots[BottomLeft][0] = calibInset; //bot left
	dots[BottomLeft][1] = height - calibInset; //bot left
	dots[BottomRight][0] = width - calibInset; //bot right
	dots[BottomRight][1] = height - calibInset; //bot right
	RazCalibration();
}
void Calibration::SetCalibrationPoint(Corners i, double x, double y)
{
	cal[i][0] = x;
	cal[i][1] = y;
	cal_set[i] = true;
	calib_points_ok = true;
	for ( int j = 0; j <4; j++ )
		if ( !cal_set[j] )
		{
			calib_points_ok = false;
			break;
		}
}
void Calibration::RazCalibration()
{
	a1 = b1 = c1 = a3 = b3 = a2 = b2 = c2 = 0.0;
	for ( int i = 0; i <4; i++ )
		cal_set[i] = false;
	calib_points_ok = false;
}
void Calibration::translateCoords(double X, double Y, double *cx, double *cy)
{
	assert( calibrated == true );
	
	*cx = (a1 * X + b1 * Y + c1) / (a3 * X + b3 * Y + 1.0);
	*cy = (a2 * X + b2 * Y + c2) / (a3 * X + b3 * Y + 1.0);
}
bool  Calibration::Calibrate()
{
#ifdef DUMP_CALIBRATION
	 DBGMSG(TEXT("running calibration\n"));
#endif
	 if ( !calib_points_ok ) return false;
	
	 RazCalibration();

	double matrix[8][8] = 
	{ 
		{ -1, -1, -1, -1, 0, 0, 0, 0 },

		{   -cal[0][0], -cal[1][0], -cal[2][0], -cal[3][0], 0, 0, 0, 0 },
		{ -cal[0][1], -cal[1][1], -cal[2][1], -cal[3][1], 0,0,0,0 },
		{ 0,0,0,0,-1,-1,-1,-1 },
		{ 0,0,0,0, -cal[0][0], -cal[1][0], -cal[2][0], -cal[3][0] },
		{ 0,0,0,0, -cal[0][1], -cal[1][1], -cal[2][1], -cal[3][1] },
		{ cal[0][0] * dots[0][0], cal[1][0] * dots[1][0], cal[2][0] * dots[2][0], cal[3][0] * dots[3][0], cal[0][0] * dots[0][1], cal[1][0] * dots[1][1], cal[2][0] * dots[2][1], cal[3][0] * dots[3][1] },
		{ cal[0][1] * dots[0][0], cal[1][1] * dots[1][0], cal[2][1] * dots[2][0], cal[3][1] * dots[3][0], cal[0][1] * dots[0][1], cal[1][1] * dots[1][1], cal[2][1] * dots[2][1], cal[3][1] * dots[3][1] },
	};


	double bb[]  = { -dots[0][0], -dots[1][0], -dots[2][0], -dots[3][0], -dots[0][1], -dots[1][1], -dots[2][1], -dots[3][1] };

	// gauß-elimination

	for (int j = 1; j < 4; j++)
	{

		for (int i = 1; i < 8; i++)
		{
			matrix[i][j] = -matrix[i][j] + matrix[i][0];
		}
		bb[j] = -bb[j] + bb[0];
		matrix[0][j] = 0;

	}


	for (int i = 2; i < 8; i++)
	{
		matrix[i][2] = -matrix[i][2] / matrix[1][2] * matrix[1][1] + matrix[i][1];
	}
	bb[2] = -bb[2] / matrix[1][2] * matrix[1][1] + bb[1];
	matrix[1][2] = 0;


	for (int i = 2; i < 8; i++)
	{
		matrix[i][3] = -matrix[i][3] / matrix[1][3] * matrix[1][1] + matrix[i][1];
	}
	bb[3] = -bb[3] / matrix[1][3] * matrix[1][1] + bb[1];
	matrix[1][3] = 0;



	for (int i = 3; i < 8; i++)
	{
		matrix[i][3] = -matrix[i][3] / matrix[2][3] * matrix[2][2] + matrix[i][2];
	}
	bb[3] = -bb[3] / matrix[2][3] * matrix[2][2] + bb[2];
	matrix[2][3] = 0;
#ifdef DUMP_CALIBRATION
	 DBGMSG(TEXT("var57, var56, var55\n"));
	 DBGMSG(TEXT("%0.3f %0.3f %03.f\n"),matrix[4][6], matrix[4][5] , + matrix[4][4]);
#endif
	for (int j = 5; j < 8; j++)
	{
		for (int i = 4; i < 8; i++)
		{
			matrix[i][j] = -matrix[i][j] + matrix[i][4];
		}
		bb[j] = -bb[j] + bb[4];
		matrix[3][j] = 0;
	}


	for (int i = 5; i < 8; i++)
	{
		matrix[i][6] = -matrix[i][6] / matrix[4][6] * matrix[4][5] + matrix[i][5];
	}

	bb[6] = -bb[6] / matrix[4][6] * matrix[4][5] + bb[5];
	matrix[4][6] = 0;


	for (int i = 5; i < 8; i++)
	{
		matrix[i][7] = -matrix[i][7] / matrix[4][7] * matrix[4][5] + matrix[i][5];
	}
	bb[7] = -bb[7] / matrix[4][7] * matrix[4][5] + bb[5];
	matrix[4][7] = 0;


	for (int i = 6; i < 8; i++)
	{
		matrix[i][7] = -matrix[i][7] / matrix[5][7] * matrix[5][6] + matrix[i][6];
	}
	bb[7] = -bb[7] / matrix[5][7] * matrix[5][6] + bb[6];
	matrix[5][7] = 0;



	matrix[7][7] = -matrix[7][7] / matrix[6][7] * matrix[6][3] + matrix[7][3];
	bb[7] = -bb[7] / matrix[6][7] * matrix[6][3] + bb[3];
	matrix[6][7] = 0;
#ifdef DUMP_CALIBRATION

	DBGMSG(TEXT("data dump\n"));
	for (int i = 0; i < 8; i++)
	{
		for (int j = 0; j < 8; j++)
		{
			 DBGMSG(TEXT("%0.3f,"),matrix[i][j]);
		}
		 DBGMSG(TEXT("\n"));
	}

	DBGMSG(TEXT("bb\n"));
	for (int j = 0; j < 8; j++)
	{
		 DBGMSG(TEXT("%0.3f,"),bb[j]);
	}

	 DBGMSG(TEXT("\n"));
#endif
	b3 = bb[7] / matrix[7][7];
	b2 = (bb[6] - (matrix[7][6] * b3 + matrix[6][6] * a3)) / matrix[5][6];
	a2 = (bb[5] - (matrix[7][5] * b3 + matrix[6][5] * a3 + matrix[5][5] * b2)) / matrix[4][5];
	c2 = (bb[4] - (matrix[7][4] * b3 + matrix[6][5] * a3 + matrix[5][4] * b2 + matrix[4][4] * a2)) / matrix[3][4];
	a3 = (bb[3] - (matrix[7][3] * b3)) / matrix[6][3];
	b1 = (bb[2] - (matrix[7][2] * b3 + matrix[6][2] * a3 + matrix[5][2] * b2 + matrix[4][2] * a2 + matrix[3][2] * c2)) / matrix[2][2];
	a1 = (bb[1] - (matrix[7][1] * b3 + matrix[6][1] * a3 + matrix[5][1] * b2 + matrix[4][1] * a2 + matrix[3][1] * c2 + matrix[2][1] * b1)) / matrix[1][1];
	c1 = (bb[0] - (matrix[7][0] * b3 + matrix[6][0] * a3 + matrix[5][0] * b2 + matrix[4][0] * a2 + matrix[3][0] * c2 + matrix[2][0] * b1 + matrix[1][0] * a1)) / matrix[0][0];

	if ( _isnan(b3)|| _isnan(b2)|| _isnan(a2)|| _isnan(c2)|| _isnan(a3)|| _isnan(b1)|| _isnan(a1)|| _isnan(c1) )
	{

#ifdef DUMP_CALIBRATION
	 DBGMSG(TEXT("calibrated BAd Result NAN!!\n"));
#endif
	 calibrated = false;
	}
	else
	{
#ifdef DUMP_CALIBRATION
	 DBGMSG(TEXT("calibrated OK\n"));
#endif
	calibrated = true;
	}
	return calibrated ;
}



void Calibration::SaveCalibration( string filename )
{
	ConfigFile config;
	config.add<double>("A1",a1);
	config.add<double>("B1",b1);
	config.add<double>("C1",c1);
	config.add<double>("A2",a2);
	config.add<double>("B2",b2);
	config.add<double>("C2",c2);
	config.add<double>("A3",a3);
	config.add<double>("B3",b3);

	// Construct a ConfigFile, write keys and values from given file
	std::ofstream out( filename.c_str() );
	
	//if( !out ) throw file_not_found( filename ); 
	
	out << config;
	out.close();
}
bool Calibration::RestoreCalibration(string filename)
{
	try
	{
	ConfigFile config(filename);
	a1 = config.read<double>("A1");
	b1 = config.read<double>("B1");
	c1 = config.read<double>("C1");
	a2 = config.read<double>("A2");
	b2 = config.read<double>("B2");
	c2 = config.read<double>("C2");
	a3 = config.read<double>("A3");
	b3 = config.read<double>("B3");
	calibrated = true;
	return true;
	}
	catch(ConfigFile::file_not_found ex)
	{
		WCHAR msg[MAX_PATH];
		HRESULT hr = S_OK;
		hr = StringCbPrintfW(msg, sizeof(msg), L"Fichier de Calibration not trouve: %S", ex.filename.c_str());

		if (SUCCEEDED(hr))
		{
			MessageBox(NULL, msg, L"Error", MB_ICONERROR);
		}
		return false;
	}
}