#pragma once

#include <vector>
#include <math.h>

enum CoordinateDirection { COORDINATE_DIRECTION_FORWARD = 1, COORDINATE_DIRECTION_BACKWARD = -1 };

#define M_PI       3.14159265358979323846
#define ToRadian(degree)	( (degree) * (M_PI/180.0) )
#define ToDegree(radian)	( (radian) * (180.0/M_PI) )

struct SMarkPosition
{
	SMarkPosition() 
	{
		Reset();
	}

	void Reset() 
	{
		dPositionX = 0.;
		dPositionY = 0.;
	}

	double	dPositionX;
	double	dPositionY;
};
typedef std::vector<SMarkPosition>				VectorMarkPosition;
typedef std::vector<SMarkPosition>::iterator	VectorMarkPositionIt;

struct SAlignResult
{
	SAlignResult() 
	{
		Reset();
	}

	void Reset()
	{
		dDegree = 0.;
		dRadian = 0.;
		dOriginX = 0.;
		dOriginY = 0.;
		dSinValue = 0.;
		dCosValue = 0.;

		memset(dRotateMatrix, 0, sizeof(dRotateMatrix));
		memset(dRotateMatrixInv, 0, sizeof(dRotateMatrixInv));
	}

	void RotatePos(double& dX, double& dY) const
	{
		double tx = dX;
		double ty = dY;

		dX = dRotateMatrix[0]*tx + dRotateMatrix[1]*ty + dRotateMatrix[2];
		dY = dRotateMatrix[3]*tx + dRotateMatrix[4]*ty + dRotateMatrix[5];
	}

	void InvRotatePos(double& dX, double& dY) const
	{
		double tx = dX;
		double ty = dY;

		dX = dRotateMatrixInv[0]*tx + dRotateMatrixInv[1]*ty + dRotateMatrixInv[2];
		dY = dRotateMatrixInv[3]*tx + dRotateMatrixInv[4]*ty + dRotateMatrixInv[5];
	}

	void SetRotateMatrix(double dCenterX, double dCenterY)
	{
		double tx = dCenterX;
		double ty = dCenterY;
		double tc = cos(dRadian);
		double ts = sin(dRadian);

		dRotateMatrix[0] = tc;
		dRotateMatrix[1] = -ts;
		dRotateMatrix[2] = tx-(tc*tx)+(ts*ty);
		dRotateMatrix[3] = ts;
		dRotateMatrix[4] = tc;
		dRotateMatrix[5] = ty-(ts*tx)-(tc*ty);
		dRotateMatrix[6] = 0.0;
		dRotateMatrix[7] = 0.0;
		dRotateMatrix[8] = 1.0;

		tc = cos(-dRadian);
		ts = sin(-dRadian);
		dRotateMatrixInv[0] = tc;
		dRotateMatrixInv[1] = -ts;
		dRotateMatrixInv[2] = tx-(tc*tx)+(ts*ty);
		dRotateMatrixInv[3] = ts;
		dRotateMatrixInv[4] = tc;
		dRotateMatrixInv[5] = ty-(ts*tx)-(tc*ty);
		dRotateMatrixInv[6] = 0.0;
		dRotateMatrixInv[7] = 0.0;
		dRotateMatrixInv[8] = 1.0;
	}

	double	dRadian;
	double	dDegree;
	double	dOriginX;
	double	dOriginY;
	double	dSinValue;
	double	dCosValue;

	double	dRotateMatrix[9];
	double	dRotateMatrixInv[9];
};


class AFX_EXT_CLASS CAlignCalibrator
{
public:
	CAlignCalibrator();
	virtual ~CAlignCalibrator(void);

	void SetDirection(int nXDir, int nYDir)		{ m_dDirectionX=nXDir; m_dDirectionY=nYDir; }
	void SetFirstMarkPos(double dMotorX, double dMotorY, double dGlassX, double dGlassY);
	void SetSecondMarkPos(double dMotorX, double dMotorY, double dGlassX, double dGlassY);

	void GetDirection(int& nXDir, int& nYDir) const 	{ nXDir=int(m_dDirectionX); nYDir=int(m_dDirectionY); }
	SAlignResult	GetAlignResult() const				{ return m_sAlignResult; }
	void			SetAlignResult(SAlignResult sAlignResult) 			{  m_sAlignResult = sAlignResult; }

	BOOL CalculateAlignResult();

protected:
	BOOL TransformGlass2Motor(double dGlassX, double dGlassY, double& dMotorX, double& dMotorY) const;
	BOOL TransformMotor2Glass(double dMotorX, double dMotorY, double& dGlassX, double& dGlassY) const;

	BOOL TransformGlass2MotorTest(double dGlassX, double dGlassY, double& dMotorX, double& dMotorY);
	BOOL TransformMotor2GlassTest(double dMotorX, double dMotorY, double& dGlassX, double& dGlassY);

	BOOL CalculateAxisX();
	BOOL CalculateAxisY();

	int					m_nResultCode;
	SAlignResult		m_sAlignResult;

	double				m_dDirectionX;
	double				m_dDirectionY;

	SMarkPosition		m_pMotorPos[2];
	SMarkPosition		m_pGlassPos[2];

};