#include "StdAfx.h"
#include "CoordCalibrator.h"
#include <math.h>

CCoordCalibrator::CCoordCalibrator() : CAlignCalibrator()
{
	m_dOriginMotorX = 0.0;
	m_dOriginMotorY = 0.0;

	m_bUseAOICam	= FALSE;
	m_nARDirX		= 1;
	m_nARDirY		= 1;
	m_dARDistX		= 0.0;
	m_dARDistY		= 0.0;
}

CCoordCalibrator::~CCoordCalibrator(void)
{
}

void CCoordCalibrator::ResetAlignResult()
{
	m_nResultCode = 0;
	m_sAlignResult.Reset();

	m_sAlignResult.dOriginX = m_dOriginMotorX;
	m_sAlignResult.dOriginY = m_dOriginMotorY;
}

void CCoordCalibrator::SetOriginMotorPosition(double dPosX, double dPosY)
{
	m_nResultCode = 0;
	m_sAlignResult.Reset();

	m_dOriginMotorX	= m_sAlignResult.dOriginX = dPosX;
	m_dOriginMotorY	= m_sAlignResult.dOriginY = dPosY;
}

void CCoordCalibrator::SetRotationInfo(double dMotorPosX, double dMotorPosY, double dGlassPosX, double dGlassPosY, double dDegree)
{
	m_sAlignResult.Reset();

	m_dRotateCenterX		= dMotorPosX;
	m_dRotateCenterY		= dMotorPosY;
	m_dRotateDegree			= dDegree;

	m_sAlignResult.dDegree	= dDegree;
	m_sAlignResult.dRadian	= ToRadian(m_sAlignResult.dDegree);
	m_sAlignResult.dSinValue = sin(m_sAlignResult.dRadian);
	m_sAlignResult.dCosValue = cos(m_sAlignResult.dRadian);

	// 기준 얼라인 마크의 모터 위치를 회전중심으로 정/역 회전변환 행렬 계산
	m_sAlignResult.SetRotateMatrix(dMotorPosX, dMotorPosY);

	// 기준 얼라인 마크의 모터 위치와 글라스 위치로 글라스 원점의 모터 위치 설정
	m_sAlignResult.dOriginX = dMotorPosX - (dGlassPosX * m_dDirectionX);
	m_sAlignResult.dOriginY = dMotorPosY - (dGlassPosY * m_dDirectionY);

	m_nResultCode = 1;
}

void CCoordCalibrator::SetTransDirection(int nDirectionX, int nDirectionY)
{
	CAlignCalibrator::SetDirection(nDirectionX, nDirectionY);
}

void CCoordCalibrator::GetTransDirection(int& nDirectionX, int& nDirectionY) const
{
	CAlignCalibrator::GetDirection(nDirectionX, nDirectionY);
}

void CCoordCalibrator::TransGlass2Motor(const SCoordInfo* pCoordInfo, double& dPositionX, double& dPositionY) const
{
	double dTransX = 0.0, dTransY = 0.0;

	//  글라스 좌표를 기준카메라의 모터좌표로 변환
	CAlignCalibrator::TransformGlass2Motor(dPositionX, dPositionY, dTransX, dTransY);

	// 해당 카메라 모터좌표 변환
	dPositionX = (dTransX - pCoordInfo->dDistanceX) * pCoordInfo->nDirectionX;
	dPositionY = (dTransY - pCoordInfo->dDistanceY) * pCoordInfo->nDirectionY;
}

void CCoordCalibrator::TransMotor2Glass(const SCoordInfo* pCoordInfo, SMotorPosition* pMotorPosition) const
{
	double dTransX = 0.0, dTransY = 0.0;

	// 기준 카메라 모터 좌표로 변환
	dTransX = pCoordInfo->dDistanceX + pCoordInfo->nDirectionX * pMotorPosition->dMotorPosX;
	dTransY = pCoordInfo->dDistanceY + pCoordInfo->nDirectionY * pMotorPosition->dMotorPosY;

	// 원점기준 글라스 좌표 변환
	CAlignCalibrator::TransformMotor2Glass(dTransX, dTransY, pMotorPosition->dGlassPosX, pMotorPosition->dGlassPosY);

	if (pMotorPosition->bUseWSI)
	{
		// 기준 카메라 모터 좌표로 변환
		dTransX = pCoordInfo->dDistanceX + pCoordInfo->nDirectionX * (pMotorPosition->dMotorPosX + pMotorPosition->dWSIOffsetX);
		dTransY = pCoordInfo->dDistanceY + pCoordInfo->nDirectionY * (pMotorPosition->dMotorPosY + pMotorPosition->dWSIOffsetY);

		// 원점기준 글라스 좌표 변환
		CAlignCalibrator::TransformMotor2Glass(dTransX, dTransY, pMotorPosition->dWSIPosX, pMotorPosition->dWSIPosY);
	}
	
}

void CCoordCalibrator::SetARMotorInfo(int nDirX, int nDirY, double dDistX, double dDistY)
{
	m_nARDirX	= (nDirX==0) ? 1: nDirX;
	m_nARDirY	= (nDirY==0) ? 1: nDirY;
	m_dARDistX	= dDistX;
	m_dARDistY	= dDistY;
}

void CCoordCalibrator::TransAOI2Review(const SCoordInfo* pCoordInfo, double dAOIPosX, double dAOIPosY, double& dReviewPosX, double& dReviewPosY) const
{
	double dReviewX=0, dReviewY=0;

	// AOI 모터 좌표를 Review 모터 좌표로 변환
	dReviewX = (dAOIPosX - m_dARDistX) * m_nARDirX;
	dReviewY = (dAOIPosY - m_dARDistY) * m_nARDirY;

	// 해당 Review 카메라 모터 좌표로 변환
	dReviewPosX = (dReviewX - pCoordInfo->dDistanceX) * pCoordInfo->nDirectionX;
	dReviewPosY = (dReviewY - pCoordInfo->dDistanceY) * pCoordInfo->nDirectionY;
}

void CCoordCalibrator::TransReview2AOI(const SCoordInfo* pCoordInfo, double dReviewPosX, double dReviewPosY, double& dAOIPosX, double& dAOIPosY) const
{
	double dReviewX=0, dReviewY=0;
	
	// 기준 Review 카메라 모터 좌표로 변환
	dReviewX = pCoordInfo->dDistanceX + pCoordInfo->nDirectionX * dReviewPosX;
	dReviewY = pCoordInfo->dDistanceY + pCoordInfo->nDirectionY * dReviewPosY;

	// Review 모터 좌표를 AOI 모터 좌표로 변환
	dAOIPosX = m_dARDistX + m_nARDirX * dReviewX;
	dAOIPosY = m_dARDistY + m_nARDirY * dReviewY;
}

void CCoordCalibrator::TransMain2Sub(const SCoordInfo* pCoordInfo, double dMainPosX, double dMainPosY, double& dSubPosX, double& dSubPosY) const
{
	// main 카메라 좌표를 sub 카메라 좌표로 변환
	dSubPosX = (dMainPosX - pCoordInfo->dDistanceX) * pCoordInfo->nDirectionX;
	dSubPosY = (dMainPosY - pCoordInfo->dDistanceY) * pCoordInfo->nDirectionY;
}

void CCoordCalibrator::TransSub2Main(const SCoordInfo* pCoordInfo, double dSubPosX, double dSubPosY, double& dMainPosX, double& dMainPosY) const
{
	// sub 카메라 좌표를 main 카메라 좌표로 변환
	dMainPosX = pCoordInfo->dDistanceX + pCoordInfo->nDirectionX * dSubPosX;
	dMainPosY = pCoordInfo->dDistanceY + pCoordInfo->nDirectionY * dSubPosY;
}


/*
void CCoordCalibrator::TransGlass2Motor(SCoordInfo* pCoordInfo, double& dPositionX, double& dPositionY)
{
	double dTempX = 0.0, dTempY = 0.0, dTransX = 0.0, dTransY = 0.0;

	//1. 기준 카메라 모터좌표로 변환
	dTempX = (dPositionX - m_dOriginMotorX) * (m_nTransDirectionX * -1);
	dTempY = (dPositionY - m_dOriginMotorY) * (m_nTransDirectionY * -1);

	//2. 얼라인 역보정
	dTransX = (cos(m_dAlignTheta * -1) * dTempX) - (sin(m_dAlignTheta * -1) * dTempY);
	dTransY = (sin(m_dAlignTheta * -1) * dTempX) + (cos(m_dAlignTheta * -1) * dTempY);

	//3. 해당 카메라 모터좌표 변환
	dPositionX = pCoordInfo->dDistanceX + (pCoordInfo->nDirectionX * -1) * dTransX;
	dPositionY = pCoordInfo->dDistanceY + (pCoordInfo->nDirectionY * -1) * dTransY;
}

void CCoordCalibrator::TransMotor2Glass(SCoordInfo* pCoordInfo, SMotorPosition* pMotorPosition)
{
	double dTempX = 0.0, dTempY = 0.0, dTransX = 0.0, dTransY = 0.0;

	//1. 기준 카메라 모터 좌표로 변환
	dTempX = pCoordInfo->dDistanceX + pCoordInfo->nDirectionX * pMotorPosition->dMotorPosX;
	dTempY = pCoordInfo->dDistanceY + pCoordInfo->nDirectionY * pMotorPosition->dMotorPosY;

	//2. 얼라인 보정
	dTransX = (cos(m_dAlignTheta) * dTempX) - (sin(m_dAlignTheta) * dTempY);
	dTransY = (sin(m_dAlignTheta) * dTempX) + (cos(m_dAlignTheta) * dTempY);

	//3. 원점기준 글라스 좌표로 변환
	pMotorPosition->dGlassPosX = (dTransX - m_dOriginMotorX)  * m_nTransDirectionX;
	pMotorPosition->dGlassPosY = (dTransY - m_dOriginMotorY)  * m_nTransDirectionY;
}
*/