#include "StdAfx.h"
#include "MotorCalibrator.h"
#include "OpenCV/cxcore.h"
#include "CHCommonClasses/MacroFile.h"

CMotorCalibrator::CMotorCalibrator(int nIndex)
{
	m_nIndex = nIndex;
	Reset();
}

CMotorCalibrator::~CMotorCalibrator(void)
{
	Reset();
}

void CMotorCalibrator::Reset(void)
{
	m_nRows = 0;
	m_nCols = 0;
	m_vectorDesignPos.clear();
	m_vectorMotorPos.clear();
	m_vectorCaliResultF.clear();
	m_vectorCaliResultB.clear();
}

void CMotorCalibrator::SetDesignPosition(const VectorDouble& vectorRows, const VectorDouble& vectorCols)
{
	Reset();

	if (vectorRows.size()<2 || vectorCols.size()<2) return;

	m_nRows = (int)vectorRows.size();
	m_nCols = (int)vectorCols.size();

	int nCount = 0;
	for (constVectorDoubleIt itR=vectorRows.begin(); itR!=vectorRows.end(); itR++)
	{
		for (constVectorDoubleIt itC=vectorCols.begin(); itC!=vectorCols.end(); itC++)
		{
			m_vectorDesignPos.push_back(SPosition2D(*itC, *itR));
			m_vectorMotorPos.push_back(SPosition2D(0, 0));
			nCount++;
		}
	}
}

void CMotorCalibrator::SetMotorPosition(int nIndex, const SPosition2D& sMotorPos)
{
	if (nIndex<0 || nIndex>=(int)m_vectorMotorPos.size()) return;

	m_vectorMotorPos[nIndex] = sMotorPos;
}


void CMotorCalibrator::SetMotorPosition(int nRow, int nCol, const SPosition2D& sMotorPos)
{
	if (nRow<0 || nCol<0) return;

	int nIndex = (nRow*m_nCols) + nCol;

	if (nIndex >=(int) m_vectorMotorPos.size()) return;

	m_vectorMotorPos[nIndex] = sMotorPos;
}

BOOL CMotorCalibrator::FCalibratePosition(double& dPosX, double& dPosY)
{
	for (VectorCalibrateResultIt it=m_vectorCaliResultF.begin(); it!=m_vectorCaliResultF.end(); it++)
	{
		if (it->InRect(dPosX, dPosY))
		{
			double x = dPosX;
			double y = dPosY;

			double div = (it->dCoefficient[6]*x)+(it->dCoefficient[7]*y)+it->dCoefficient[8];

			dPosX = ((it->dCoefficient[0]*x)+(it->dCoefficient[1]*y)+it->dCoefficient[2]) / div;
			dPosY = ((it->dCoefficient[3]*x)+(it->dCoefficient[4]*y)+it->dCoefficient[5]) / div;

			return TRUE;
		}
	}

	return FALSE;
}

BOOL CMotorCalibrator::BCalibratePosition(double& dPosX, double& dPosY)
{
	for (VectorCalibrateResultIt it=m_vectorCaliResultB.begin(); it!=m_vectorCaliResultB.end(); it++)
	{
		if (it->InRect(dPosX, dPosY))
		{
			double x = dPosX;
			double y = dPosY;

			double div = (it->dCoefficient[6]*x)+(it->dCoefficient[7]*y)+it->dCoefficient[8];

			dPosX = ((it->dCoefficient[0]*x)+(it->dCoefficient[1]*y)+it->dCoefficient[2]) / div;
			dPosY = ((it->dCoefficient[3]*x)+(it->dCoefficient[4]*y)+it->dCoefficient[5]) / div;

			return TRUE;
		}
	}

	return FALSE;
}


SPosition2D GetStartPosition(const VectorPosition2D& vectorPos)
{
	SPosition2D sPos(DBL_MAX, DBL_MAX);
	for (constVectorPosition2DIt it=vectorPos.begin(); it!=vectorPos.end(); it++)
	{
		if (sPos.dPositionX > it->dPositionX)
		{
			sPos.dPositionX = it->dPositionX;
		}

		if (sPos.dPositionY > it->dPositionY)
		{
			sPos.dPositionY = it->dPositionY;
		}
	}

	return sPos;
}


SPosition2D GetEndPosition(const VectorPosition2D& vectorPos)
{
	SPosition2D sPos(DBL_MIN, DBL_MIN);
	for (constVectorPosition2DIt it=vectorPos.begin(); it!=vectorPos.end(); it++)
	{
		if (sPos.dPositionX < it->dPositionX)
		{
			sPos.dPositionX = it->dPositionX;
		}

		if (sPos.dPositionY < it->dPositionY)
		{
			sPos.dPositionY = it->dPositionY;
		}
	}

	return sPos;
}



int CMotorCalibrator::CalculateCoefficient()
{
	if (m_nRows<2 || m_nCols<2) return 1;

	int nResultRows = m_nRows-1;
	int nResultCols = m_nCols-1;

	m_vectorCaliResultF.clear();
	m_vectorCaliResultF.resize(nResultRows*nResultCols);

	m_vectorCaliResultB.clear();
	m_vectorCaliResultB.resize(nResultRows*nResultCols);

	int temp_idx = 0, nFSuccessCount = 0, nFFailCount = 0, nBSuccessCount = 0, nBFailCount = 0;
	VectorPosition2D vectorSrc, vectorDst;

	for (int nIndex = 0; nIndex<(int)m_vectorCaliResultF.size(); nIndex++)
	{
		int nRow = nIndex / nResultCols;
		int nCol = nIndex % nResultCols;

		vectorSrc.clear();
		vectorDst.clear();
		
		// get position
		temp_idx = (nRow*m_nCols)+nCol;
		vectorSrc.push_back(m_vectorMotorPos[temp_idx]);
		vectorDst.push_back(m_vectorDesignPos[temp_idx]);

		temp_idx++;
		vectorSrc.push_back(m_vectorMotorPos[temp_idx]);
		vectorDst.push_back(m_vectorDesignPos[temp_idx]);
		
		temp_idx = ((nRow+1)*m_nCols)+nCol;
		vectorSrc.push_back(m_vectorMotorPos[temp_idx]);
		vectorDst.push_back(m_vectorDesignPos[temp_idx]);

		temp_idx++;
		vectorSrc.push_back(m_vectorMotorPos[temp_idx]);
		vectorDst.push_back(m_vectorDesignPos[temp_idx]);

		// foreward calculate coeff
		if (CalculatePerspectiveTransform(vectorSrc, vectorDst, m_vectorCaliResultF[nIndex])!=0)
		{
			m_vectorCaliResultF[nIndex].vectorRegion	= vectorSrc;
			m_vectorCaliResultF[nIndex].sStartPosition	= GetStartPosition(vectorSrc);
			m_vectorCaliResultF[nIndex].sEndPosition	= GetEndPosition(vectorSrc);
			nFSuccessCount++;
		}
		else
		{
			nFFailCount++;
		}

		// backward calculate coeff
		if (CalculatePerspectiveTransform(vectorDst, vectorSrc, m_vectorCaliResultB[nIndex])!=0)
		{
			m_vectorCaliResultB[nIndex].vectorRegion	= vectorDst;
			m_vectorCaliResultB[nIndex].sStartPosition	= GetStartPosition(vectorDst);
			m_vectorCaliResultB[nIndex].sEndPosition	= GetEndPosition(vectorDst);
			nBSuccessCount++;
		}
		else
		{
			nBFailCount++;
		}
	}

	return nFFailCount;
}

int CMotorCalibrator::CalculatePerspectiveTransform(const VectorPosition2D& vectorSrc, const VectorPosition2D& vectorDst, SCalibrateResult& result)
{
	if (vectorSrc.size()!=4 || vectorDst.size()!=4) return 0;

	double a[8][8], b[8];
	for( int i = 0; i < 4; ++i )
	{
		a[i][0] = a[i+4][3] = vectorSrc[i].dPositionX;
		a[i][1] = a[i+4][4] = vectorSrc[i].dPositionY;
		a[i][2] = a[i+4][5] = 1;
		a[i][3] = a[i][4] = a[i][5] = a[i+4][0] = a[i+4][1] = a[i+4][2] = 0.0;
		a[i][6] = -vectorSrc[i].dPositionX * vectorDst[i].dPositionX;
		a[i][7] = -vectorSrc[i].dPositionY * vectorDst[i].dPositionX;
		a[i+4][6] = -vectorSrc[i].dPositionX * vectorDst[i].dPositionY;
		a[i+4][7] = -vectorSrc[i].dPositionY * vectorDst[i].dPositionY;
		b[i] = vectorDst[i].dPositionX;
		b[i+4] = vectorDst[i].dPositionY;
	}

	CvMat A = cvMat(8, 8, CV_64FC1, a);
	CvMat B = cvMat(8, 1, CV_64FC1, b);
	CvMat X = cvMat(8, 1, CV_64FC1, result.dCoefficient);

	int nValue = cvSolve(&A, &B, &X, CV_SVD);

	result.dCoefficient[8] = 1.;

	return nValue;
}

BOOL CMotorCalibrator::LoadDesignPosition(const CString& strFilename)
{
	int nRows=0, nCols=0;
	if (!LoadPosition(strFilename, m_vectorDesignPos, nRows, nCols))
	{	
		return FALSE;
	}

	m_nRows = nRows;
	m_nCols = nCols;

	return TRUE;
}

BOOL CMotorCalibrator::SaveDesignPosition(const CString& strFilename)
{
	return LoadPosition(strFilename, m_vectorDesignPos, m_nRows, m_nCols);
}

BOOL CMotorCalibrator::LoadMotorPosition(const CString& strFilename)
{
	int nRows=0, nCols=0;
	if (!LoadPosition(strFilename, m_vectorMotorPos, nRows, nCols))
	{	
		return FALSE;
	}

	m_nRows = nRows;
	m_nCols = nCols;

	return TRUE;
}

BOOL CMotorCalibrator::SaveMotorPosition(const CString& strFilename)
{
	return SavePosition(strFilename, m_vectorMotorPos, m_nRows, m_nCols);
}

BOOL CMotorCalibrator::LoadCalibrateResultF(const CString& strFilename)
{
	return LoadCalibrateResult(strFilename, m_vectorCaliResultF);
}

BOOL CMotorCalibrator::SaveCalibrateResultF(const CString& strFilename)
{
	return SaveCalibrateResult(strFilename, m_vectorCaliResultF);
}

BOOL CMotorCalibrator::LoadCalibrateResultB(const CString& strFilename)
{
	return LoadCalibrateResult(strFilename, m_vectorCaliResultB);
}

BOOL CMotorCalibrator::SaveCalibrateResultB(const CString& strFilename)
{
	return SaveCalibrateResult(strFilename, m_vectorCaliResultB);
}

BOOL CMotorCalibrator::LoadPosition(const CString& strFilename, VectorPosition2D& vectorPos, int& nRows, int& nCols)
{
	FILE *fp = NULL;

	_tfopen_s(&fp, strFilename, _T("r"));

	if (fp==NULL) return FALSE;

	vectorPos.clear();

	TCHAR strValue[200];

	_ftscanf_s(fp, _T("%s,"), strValue, 5);
	_ftscanf_s(fp, _T("%d\n"), &nRows);

	_ftscanf_s(fp, _T("%s,"), strValue, 5);
	_ftscanf_s(fp, _T("%d\n"), &nCols);

	SPosition2D pos;
	for (int i=0; i<nRows*nCols; i++)
	{
		pos.Reset();
		_ftscanf_s(fp, _T("%lf,%lf\n"), &pos.dPositionX, &pos.dPositionY);
		vectorPos.push_back(pos);
	}

	fclose(fp);

	return TRUE;

	/*
	CMacroFile macroFile;
	if (!macroFile.Read(strFilename))
	{
	return FALSE;
	}

	vectorPos.clear();
	
	macroFile.GetItem(_T("POSITION_COUNT_ROW"), nRows, 0);
	macroFile.GetItem(_T("POSITION_COUNT_COL"), nCols, 0);

	CString strItem = _T("");

	SPosition2D pos;
	for (int nIndex=0; nIndex<nRows*nCols; nIndex++)
	{
	pos.Reset();

	strItem.Format(_T("POSITION_X_%02d"), nIndex);
	macroFile.GetItem(strItem, pos.dPositionX, 0.0);

	strItem.Format(_T("POSITION_Y_%02d"), nIndex);
	macroFile.GetItem(strItem, pos.dPositionY, 0.0);

	vectorPos.push_back(pos);
	}

	return TRUE;
	*/
}

BOOL CMotorCalibrator::SavePosition(const CString& strFilename, const VectorPosition2D& vectorPos, int nRows, int nCols)
{
	if (int(vectorPos.size())!=(nRows*nCols)) return FALSE;

	FILE *fp = NULL;

	_tfopen_s(&fp, strFilename, _T("w"));

	if (fp==NULL) return FALSE;

	_ftprintf_s(fp, _T("ROW_COUNT,%d\n"), nRows);
	_ftprintf_s(fp, _T("COL_COUNT,%d\n"), nCols);

	for (constVectorPosition2DIt it=vectorPos.begin(); it!=vectorPos.end(); it++)
	{
		_ftprintf_s(fp, _T("%lf,%lf\n"), it->dPositionX, it->dPositionY);
	}

	fclose(fp);

	return TRUE;

	/*
	CMacroFile macroFile;

	int nValue = 0;
	CString strItem = _T("");

	macroFile.SetItem(_T("POSITION_COUNT_ROW"), nRows);
	macroFile.SetItem(_T("POSITION_COUNT_COL"), nCols);

	int nIndex = 0;
	for (constVectorPosition2DIt it=vectorPos.begin(); it!=vectorPos.end(); it++)
	{
	strItem.Format(_T("POSITION_X_%02d"), nIndex);
	macroFile.SetItem(strItem, it->dPositionX);

	strItem.Format(_T("POSITION_Y_%02d"), nIndex);
	macroFile.SetItem(strItem, it->dPositionY);

	nIndex++;
	}

	return macroFile.Write(strFilename);
	*/
}

BOOL CMotorCalibrator::LoadCalibrateResult(const CString& strFilename, VectorCalibrateResult& vectorResult)
{
	CMacroFile macroFile;
	if (!macroFile.Read(strFilename))
	{
		return FALSE;
	}

	vectorResult.clear();

	int nValue = 0;
	CString strItem = _T("");
	SCalibrateResult calResult;
	macroFile.GetItem(_T("CALIBRATE_COUNT"), nValue, 0);

	for (int nIndex=0; nIndex<nValue; nIndex++)
	{
		calResult.Reset();

		// start pos
		strItem.Format(_T("CALI_START_POS_X_%02d"), nIndex);
		macroFile.GetItem(strItem, calResult.sStartPosition.dPositionX, 0.0);
		strItem.Format(_T("CALI_START_POS_Y_%02d"), nIndex);
		macroFile.GetItem(strItem, calResult.sStartPosition.dPositionY, 0.0);

		// end pos
		strItem.Format(_T("CALI_END_POS_X_%02d"), nIndex);
		macroFile.GetItem(strItem, calResult.sEndPosition.dPositionX, 0.0);
		strItem.Format(_T("CALI_END_POS_Y_%02d"), nIndex);
		macroFile.GetItem(strItem, calResult.sEndPosition.dPositionY, 0.0);

		// cali coefficient
		for (int j=0; j<9; j++)
		{
			strItem.Format(_T("CALI_COEFFICIENT_%02d_%02d"), nIndex, j);
			macroFile.GetItem(strItem, calResult.dCoefficient[j], 0.0);
		}

		// vector region
		for (int j=0; j<4; j++)
		{
			SPosition2D pos;
			strItem.Format(_T("CALI_REGION_X_%02d_%02d"), nIndex, j);
			macroFile.GetItem(strItem, pos.dPositionX, 0.0);

			strItem.Format(_T("CALI_REGION_Y_%02d_%02d"), nIndex, j);
			macroFile.GetItem(strItem, pos.dPositionY, 0.0);
			calResult.vectorRegion.push_back(pos);
		}

		vectorResult.push_back(calResult);
	}

	return TRUE;
}

BOOL CMotorCalibrator::SaveCalibrateResult(const CString& strFilename, const VectorCalibrateResult& vectorResult)
{
	CMacroFile macroFile;

	int nValue = 0;
	CString strItem = _T("");

	nValue = (int)vectorResult.size();
	macroFile.SetItem(_T("CALIBRATE_COUNT"), nValue);

	int nIndex = 0;
	for (constVectorCalibrateResultIt it=vectorResult.begin(); it!=vectorResult.end(); it++)
	{
		// start pos
		strItem.Format(_T("CALI_FOR_START_POS_X_%02d"), nIndex);
		macroFile.SetItem(strItem, it->sStartPosition.dPositionX);
		strItem.Format(_T("CALI_FOR_START_POS_Y_%02d"), nIndex);
		macroFile.SetItem(strItem, it->sStartPosition.dPositionY);

		// end pos
		strItem.Format(_T("CALI_FOR_END_POS_X_%02d"), nIndex);
		macroFile.SetItem(strItem, it->sEndPosition.dPositionX);
		strItem.Format(_T("CALI_FOR_END_POS_Y_%02d"), nIndex);
		macroFile.SetItem(strItem, it->sEndPosition.dPositionY);

		// cali coefficient
		for (int j=0; j<9; j++)
		{
			strItem.Format(_T("CALI_FOR_COEFFICIENT_%02d_%02d"), nIndex, j);
			macroFile.SetItem(strItem, it->dCoefficient[j]);
		}

		// vector region
		for (int j=0; j<4; j++)
		{
			strItem.Format(_T("CALI_FOR_REGION_X_%02d_%02d"), nIndex, j);
			macroFile.SetItem(strItem, it->vectorRegion[j].dPositionX);

			strItem.Format(_T("CALI_FOR_REGION_Y_%02d_%02d"), nIndex, j);
			macroFile.SetItem(strItem, it->vectorRegion[j].dPositionY);
		}

		nIndex++;
	}
	
	return macroFile.Write(strFilename);
}