#include "StdAfx.h"
#include "CHReviewSetting/AlignCalibrator.h"
#include <math.h>

CAlignCalibrator::CAlignCalibrator()
{
	m_dDirectionX = 1.0;
	m_dDirectionY = 1.0;

	m_nResultCode = 0;

	memset(m_pMotorPos, 0, sizeof(SMarkPosition)*2);
	memset(m_pGlassPos, 0, sizeof(SMarkPosition)*2);

	m_sAlignResult.Reset();
}

CAlignCalibrator::~CAlignCalibrator(void)
{

}

void CAlignCalibrator::SetFirstMarkPos(double dMotorX, double dMotorY, double dGlassX, double dGlassY)
{
	m_pMotorPos[0].dPositionX = dMotorX;
	m_pMotorPos[0].dPositionY = dMotorY;

	m_pGlassPos[0].dPositionX = dGlassX;
	m_pGlassPos[0].dPositionY = dGlassY;
}

void CAlignCalibrator::SetSecondMarkPos(double dMotorX, double dMotorY, double dGlassX, double dGlassY)
{
	m_pMotorPos[1].dPositionX = dMotorX;
	m_pMotorPos[1].dPositionY = dMotorY;

	m_pGlassPos[1].dPositionX = dGlassX;
	m_pGlassPos[1].dPositionY = dGlassY;
}

/* A3 EA °Ë»ç±â

À§Ä¡	xÁÂÇ¥		yÁÂÇ¥
RT		3360.222	21.411
RB		3360.397	880.465
LT		1889.912	21.356
LB		1890.079	880.406

*/

BOOL CAlignCalibrator::CalculateAlignResult()
{
	m_nResultCode = 0;

	m_sAlignResult.Reset();

	double dx = fabs(m_pMotorPos[0].dPositionX - m_pMotorPos[1].dPositionX);
	double dy = fabs(m_pMotorPos[0].dPositionY - m_pMotorPos[1].dPositionY);

	if (dx > dy)
	{
		m_nResultCode = CalculateAxisX();
	}
	else
	{
		m_nResultCode = CalculateAxisY();
	}

	return (BOOL)m_nResultCode;
}

double EuclideanDistance(const SMarkPosition& pt1, const SMarkPosition& pt2)
{
	double dDistance = (pt1.dPositionX-pt2.dPositionX) * (pt1.dPositionX-pt2.dPositionX);

	dDistance += (pt1.dPositionY-pt2.dPositionY) * (pt1.dPositionY-pt2.dPositionY);
	
	return sqrt(dDistance);
}

BOOL CAlignCalibrator::CalculateAxisX()
{
	double dx1 = m_pMotorPos[0].dPositionX - m_pMotorPos[1].dPositionX;
	double dy1 = m_pMotorPos[0].dPositionY - m_pMotorPos[1].dPositionY;
	double dx2 = m_pMotorPos[1].dPositionX - m_pMotorPos[0].dPositionX;
	double dy2 = m_pMotorPos[1].dPositionY - m_pMotorPos[0].dPositionY;

	// ȸÀü°¢ °è»ê
//	if(m_pGlassPos[0].dPositionX < m_pMotorPos[1].dPositionX)
		m_sAlignResult.dRadian = atan( dy1 / dx1 );
//	else
//		m_sAlignResult.dRadian = -atan( dy1 / dx1 );
	m_sAlignResult.dDegree = ToDegree(m_sAlignResult.dRadian);
	m_sAlignResult.dSinValue = sin(m_sAlignResult.dRadian);
	m_sAlignResult.dCosValue = cos(m_sAlignResult.dRadian);

	// ±âÁØ ¾ó¶óÀÎ ¸¶Å©ÀÇ ¸ðÅÍ À§Ä¡¸¦ ȸÀüÁß½ÉÀ¸·Î Á¤/¿ª ȸÀüº¯È¯ Çà·Ä °è»ê
	m_sAlignResult.SetRotateMatrix(m_pMotorPos[0].dPositionX, m_pMotorPos[0].dPositionY);

	// ±âÁØ ¾ó¶óÀÎ ¸¶Å©ÀÇ ¸ðÅÍ À§Ä¡¿Í ±Û¶ó½º À§Ä¡·Î ±Û¶ó½º ¿øÁ¡ÀÇ ¸ðÅÍ À§Ä¡ ¼³Á¤
	m_sAlignResult.dOriginX = m_pMotorPos[0].dPositionX - (m_pGlassPos[0].dPositionX * m_dDirectionX);
	m_sAlignResult.dOriginY = m_pMotorPos[0].dPositionY - (m_pGlassPos[0].dPositionY * m_dDirectionY);

	// ¸¶Å©°£ °Å¸® °è»ê
	double dMotorDist = EuclideanDistance(m_pMotorPos[0], m_pMotorPos[1]);
	double dGalssDist = EuclideanDistance(m_pGlassPos[0], m_pGlassPos[1]);
	double dDistThres = 2.0 * fabs(dMotorDist-dGalssDist);

	double tx, ty;
	double sx, sy;

	// ¾ó¶óÀÎ ¸¶Å© ¼³°èÀ§Ä¡¸¦ Á¤¹æÇâ ȸÀüÇßÀ»¶§ ¸ðÅÍÀ§Ä¡¿ÍÀÇ Â÷ÀÌ°ª °è»ê
	tx = ty = 0.0;
	for (int i=0; i<2; i++)
	{
		TransformGlass2MotorTest(m_pGlassPos[i].dPositionX, m_pGlassPos[i].dPositionY, sx, sy);
		tx += fabs(m_pMotorPos[i].dPositionX - sx);
		ty += fabs(m_pMotorPos[i].dPositionY - sy);
	}

	if (tx>dDistThres || ty>dDistThres)
	{
		return FALSE;
	}

	// ¾ó¶óÀÎ ¸¶Å© ¸ðÅÍÀ§Ä¡¸¦ ¿ª¹æÇâ ȸÀüÇßÀ»¶§ ¼³°èÀ§Ä¡¿ÍÀÇ Â÷ÀÌ°ª °è»ê
	tx = ty = 0.0;
	for (int i=0; i<2; i++)
	{
		TransformMotor2GlassTest(m_pMotorPos[i].dPositionX, m_pMotorPos[i].dPositionY, sx, sy);
		tx += fabs(m_pGlassPos[i].dPositionX - sx);
		ty += fabs(m_pGlassPos[i].dPositionY - sy);
	}

	if (tx>dDistThres || ty>dDistThres)
	{
		return FALSE;
	}

	return TRUE;
}

BOOL CAlignCalibrator::CalculateAxisY()
{
	double dx1 = m_pMotorPos[0].dPositionX - m_pMotorPos[1].dPositionX;
	double dy1 = m_pMotorPos[0].dPositionY - m_pMotorPos[1].dPositionY;
	double dx2 = m_pMotorPos[1].dPositionX - m_pMotorPos[0].dPositionX;
	double dy2 = m_pMotorPos[1].dPositionY - m_pMotorPos[0].dPositionY;

	// ȸÀü°¢ °è»ê
	m_sAlignResult.dRadian = -1.0 * atan( dx1 / dy1 );
	m_sAlignResult.dDegree = ToDegree(m_sAlignResult.dRadian);
	m_sAlignResult.dSinValue = sin(m_sAlignResult.dRadian);
	m_sAlignResult.dCosValue = cos(m_sAlignResult.dRadian);

	// ±âÁØ ¾ó¶óÀÎ ¸¶Å©ÀÇ ¸ðÅÍ À§Ä¡¸¦ ȸÀüÁß½ÉÀ¸·Î Á¤/¿ª ȸÀüº¯È¯ Çà·Ä °è»ê
	m_sAlignResult.SetRotateMatrix(m_pMotorPos[0].dPositionX, m_pMotorPos[0].dPositionY);

	// ±âÁØ ¾ó¶óÀÎ ¸¶Å©ÀÇ ¸ðÅÍ À§Ä¡¿Í ±Û¶ó½º À§Ä¡·Î ±Û¶ó½º ¿øÁ¡ÀÇ ¸ðÅÍ À§Ä¡ ¼³Á¤
	m_sAlignResult.dOriginX = m_pMotorPos[0].dPositionX - (m_pGlassPos[0].dPositionX * m_dDirectionX);
	m_sAlignResult.dOriginY = m_pMotorPos[0].dPositionY - (m_pGlassPos[0].dPositionY * m_dDirectionY);

	// ¸¶Å©°£ °Å¸® °è»ê
	double dMotorDist = EuclideanDistance(m_pMotorPos[0], m_pMotorPos[1]);
	double dGalssDist = EuclideanDistance(m_pGlassPos[0], m_pGlassPos[1]);
	double dDistThres = 2.0 * fabs(dMotorDist-dGalssDist);

	double tx, ty;
	double sx, sy;

	tx = ty = 0.0;

	sx = sy = 0.0;
	for (int i=0; i<2; i++)
	{
		TransformGlass2MotorTest(m_pGlassPos[i].dPositionX, m_pGlassPos[i].dPositionY, sx, sy);
		tx += fabs(m_pMotorPos[i].dPositionX - sx);
		ty += fabs(m_pMotorPos[i].dPositionY - sy);
	}

	if (tx>dDistThres || ty>dDistThres)
	{
		return FALSE;
	}

	tx = ty = 0.0;
	sx = sy = 0.0;

	for (int i=0; i<2; i++)
	{
		TransformMotor2GlassTest(m_pMotorPos[i].dPositionX, m_pMotorPos[i].dPositionY, sx, sy);
		tx += fabs(m_pGlassPos[i].dPositionX - sx);
		ty += fabs(m_pGlassPos[i].dPositionY - sy);
	}

	if (tx>dDistThres || ty>dDistThres)
	{
		return FALSE;
	}

	return TRUE;
}

BOOL CAlignCalibrator::TransformGlass2MotorTest(double dGlassX, double dGlassY, double& dMotorX, double& dMotorY)
{
	// ±Û¶ó½º ÁÂÇ¥¸¦ ¸ðÅÍÁÂÇ¥·Î º¯È¯(¿øÁ¡±âÁØ)
	double motor_x = (dGlassX * m_dDirectionX) + m_sAlignResult.dOriginX;
	double motor_y = (dGlassY * m_dDirectionY) + m_sAlignResult.dOriginY;

	// ¸ðÅÍÁÂÇ¥¸¦ radian ¹æÇâÀ¸·Î ȸÀüÇÑ ¸ðÅÍÁÂÇ¥¸¦ °è»ê
	m_sAlignResult.RotatePos(motor_x, motor_y);
	dMotorX = motor_x;
	dMotorY = motor_y;

	return TRUE;
}

BOOL CAlignCalibrator::TransformGlass2Motor(double dGlassX, double dGlassY, double& dMotorX, double& dMotorY) const
{
	// ±Û¶ó½º ÁÂÇ¥¸¦ ¸ðÅÍÁÂÇ¥·Î º¯È¯(¿øÁ¡±âÁØ)
	double motor_x = (dGlassX * m_dDirectionX) + m_sAlignResult.dOriginX;
	double motor_y = (dGlassY * m_dDirectionY) + m_sAlignResult.dOriginY;

	// ¸ðÅÍÁÂÇ¥¸¦ radian ¹æÇâÀ¸·Î ȸÀüÇÑ ¸ðÅÍÁÂÇ¥¸¦ °è»ê
	if (m_nResultCode)
	{
		m_sAlignResult.RotatePos(motor_x, motor_y);
	}

	dMotorX = motor_x;
	dMotorY = motor_y;

	return TRUE;
}

BOOL CAlignCalibrator::TransformMotor2GlassTest(double dMotorX, double dMotorY, double& dGlassX, double& dGlassY)
{
	double motor_x = dMotorX;
	double motor_y = dMotorY;

	// ¸ðÅÍÁÂÇ¥¸¦ -radian ¹æÇâÀ¸·Î ȸÀüÇÑ ¸ðÅÍÁÂÇ¥ °è»ê
	m_sAlignResult.InvRotatePos(motor_x, motor_y);

	// ȸÀüÇÑ ¸ðÅÍÁÂÇ¥¸¦ ±Û¶ó½º ÁÂÇ¥·Î º¯È¯(¿øÁ¡±âÁØ)
	dGlassX = (motor_x - m_sAlignResult.dOriginX) * m_dDirectionX;
	dGlassY = (motor_y - m_sAlignResult.dOriginY) * m_dDirectionY;

	return TRUE;
}

BOOL CAlignCalibrator::TransformMotor2Glass(double dMotorX, double dMotorY, double& dGlassX, double& dGlassY) const
{
	double motor_x = dMotorX;
	double motor_y = dMotorY;

	// ¸ðÅÍÁÂÇ¥¸¦ -radian ¹æÇâÀ¸·Î ȸÀüÇÑ ¸ðÅÍÁÂÇ¥ °è»ê
	if (m_nResultCode)
	{
		m_sAlignResult.InvRotatePos(motor_x, motor_y);
	}

	// ȸÀüÇÑ ¸ðÅÍÁÂÇ¥¸¦ ±Û¶ó½º ÁÂÇ¥·Î º¯È¯
	dGlassX = (motor_x - m_sAlignResult.dOriginX) * m_dDirectionX;
	dGlassY = (motor_y - m_sAlignResult.dOriginY) * m_dDirectionY;

	return TRUE;
}