#include "stdafx.h"
#include "CHMotorControls/MotorControl.h"

CMotorControl::CMotorControl(int nIndex, DWORD dwPeriod,int nThreadCount) : 
	m_nIndex(nIndex), 
	m_dwPeriod(dwPeriod), 
	CTimerThreadPools(dwPeriod, 1)
{
	m_pMC2P				= NULL;
	m_pMotionThread		= NULL;
	m_ControlInfo.Reset();
	m_bConnected		= FALSE;

	// var
	m_nAxisCount				= 0;
	m_pAxisStatus				= NULL;
	m_pAxisPosition				= NULL;
	m_pAxisStatusPrev			= NULL;
	m_pAxisPositionPrev			= NULL;
	m_pAxisStatusAddr			= NULL;
	m_pAxisPositionAddr			= NULL;

	// review enable
	m_nAutoEnable				= 0;
	m_nAutoEnablePrev			= 0;

	// wsi signal
	m_nWsiMotionEndPrev			= 0;

	m_axisThetaStatus			= 0;
	m_axisThetaStatusPrev		= 0;
	m_axisThetaPosition			= 0.f;
	m_axisThetaPositionPrev		= -999.0;
		
	m_bUseWsi					= FALSE;

	m_pMotionThread				= new CMotionThread(static_cast<IMotionThread2Parent*>(this), nThreadCount);
}

CMotorControl::~CMotorControl(void)
{
	if (m_pMotionThread)
	{
		delete m_pMotionThread;
	}
	m_pMotionThread = NULL;


	m_nAxisCount = 0;

	if (m_pAxisStatus)
	{
		delete [] m_pAxisStatus;		
		m_pAxisStatus = NULL;
	}

	if (m_pAxisPosition)
	{
		delete [] m_pAxisPosition;		
		m_pAxisPosition = NULL;
	}

	if (m_pAxisStatusPrev)
	{
		delete [] m_pAxisStatusPrev;	
		m_pAxisStatusPrev = NULL;
	}

	if (m_pAxisPositionPrev)
	{
		delete [] m_pAxisPositionPrev;	
		m_pAxisPositionPrev = NULL;
	}

	if (m_pAxisStatusAddr)
	{
		delete [] m_pAxisStatusAddr;	
		m_pAxisStatusAddr = NULL;
	}

	if (m_pAxisPositionAddr)
	{
		delete [] m_pAxisPositionAddr;	
		m_pAxisPositionAddr = NULL;
	}

	m_ControlInfo.Reset();

}

void CMotorControl::TimerThreadProcess(PVOID pParameter)
{
	if (!m_bConnected) return;

	if (m_pMC2P==NULL) return;

	if (m_nAxisCount<1) return;

	// auto enable
	const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
	if (pAddr && pAddr->m_nAllAutoEnableAddr!=MOTOR_ADDRESS_NONE)
	{
		if (ReadAddressValue(pAddr->m_nAllAutoEnableAddr, m_nAutoEnable))
		{
			if (m_nAutoEnablePrev != m_nAutoEnable)
			{
				m_pMC2P->IMC2P_UpdateAutoEnable(m_nIndex, m_nAutoEnable);
			}
			m_nAutoEnablePrev = m_nAutoEnable;
		}
	}
	
	// axis position+-3
	static int nAxisMultiValue = 1000;

	if (ReadAxisPosition(m_pAxisPosition, m_nAxisCount))
	{
		for (int i=0; i<m_nAxisCount; i++)
		{
			if (int(m_pAxisPositionPrev[i]*nAxisMultiValue)!=int(m_pAxisPosition[i]*nAxisMultiValue))
			{
				m_pMC2P->IMC2P_UpdatePosition(m_nIndex, i, m_pAxisPosition[i]);
			}
			m_pAxisPositionPrev[i] = m_pAxisPosition[i];
		}
	}

	// axis status
	if (ReadAxisStatus(m_pAxisStatus, m_nAxisCount))
	{
		for (int i=0; i<m_nAxisCount; i++)
		{
			if (m_pAxisStatusPrev[i]!=m_pAxisStatus[i])
			{
				m_pMC2P->IMC2P_UpdateStatus(m_nIndex, i, m_pAxisStatus[i]);
			}
			m_pAxisStatusPrev[i] = m_pAxisStatus[i];
		}
	}

	// wsi motion end signal
	if(GetUseWsi())
	{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		int nAddr = GetGantryWsiMotionEndAddr(nG_Idx);
		if (nAddr==MOTOR_ADDRESS_NONE) continue;

		long nMotionEnd = 0;

		if (ReadAddressValue(nAddr, nMotionEnd))
		{
			if (nG_Idx == 0)
			{
				if ((m_nWsiMotionEndPrev != nMotionEnd) && nMotionEnd == 1)
				{
					m_pMC2P->IMC2P_UpdateWsiMotionEnd(m_nIndex, nG_Idx);

				}
				m_nWsiMotionEndPrev = nMotionEnd;
			}
			else
			{
				if ((m_nWsiMotionEndPrev2 != nMotionEnd) && nMotionEnd == 1)
				{
					m_pMC2P->IMC2P_UpdateWsiMotionEnd(m_nIndex, nG_Idx);

				}
				m_nWsiMotionEndPrev2 = nMotionEnd;
			}
		
		}
	}
	}

	// theta axis
	if (m_ControlInfo.GetUseThetaMotor())
	{
		static int nThetaAxisMultiValue = 1000000;

		// theta position
		if (ReadThetaAxisPosition(m_axisThetaPosition))
		{
			if (int(m_axisThetaPositionPrev*nThetaAxisMultiValue)!=int(m_axisThetaPosition*nThetaAxisMultiValue))
			{
				m_pMC2P->IMC2P_UpdateThetaPosition(m_nIndex, m_axisThetaPosition);
			}
			m_axisThetaPositionPrev = m_axisThetaPosition;
		}

		// theta status
		if (ReadThetaAxisStatus(m_axisThetaStatus))
		{
			if (m_axisThetaStatusPrev!=m_axisThetaStatus)
			{
				m_pMC2P->IMC2P_UpdateThetaStatus(m_nIndex, m_axisThetaStatus);
			}
			m_axisThetaStatusPrev = m_axisThetaStatus;
		}
	}
}

BOOL CMotorControl::ReadAxisPosition( float *pAxisPos, int nAxisCount )
{
	return ReadAddressValue(m_pAxisPositionAddr[0], pAxisPos, nAxisCount);
}

BOOL CMotorControl::ReadAxisStatus( long *pAxisStatus, int nAxisCount )
{
	return ReadAddressValue(m_pAxisStatusAddr[0], pAxisStatus, nAxisCount);
}

BOOL CMotorControl::ReadThetaAxisPosition( float &fAxisPos)
{
	const CMotorThetaAxisAddr* pAddr = m_ControlInfo.GetThetaMotorAxisAddr();
	if (pAddr==NULL) return FALSE;
	return ReadAddressValue(pAddr->m_nPositionAddr, fAxisPos);
}

BOOL CMotorControl::ReadThetaAxisStatus( long &nAxisStatus )
{
	const CMotorThetaAxisAddr* pAddr = m_ControlInfo.GetThetaMotorAxisAddr();
	if (pAddr==NULL) return FALSE;
	return ReadAddressValue(pAddr->m_nStatusAddr, nAxisStatus);
}

void CMotorControl::IMT2P_RunThreadProcess(const CMotionData& motionData)
{
	switch(motionData.nType)
	{
	case ThreadDataType_AxisMotionEnd:
		Process_AxisMotionEnd(motionData);
		break;

	case ThreadDataType_GantryMotionEnd:
		Process_GantryMotionEnd(motionData);
		break;
	case ThreadDataType_ThetaMotionEnd:
		Process_ThetaMotionEnd(motionData);
		break;
	}
}

BOOL CMotorControl::TransformIP_CStringToDWORD(const CString& strServerIP, DWORD& dwServerIP)
{
	BOOL bReturn = TRUE;
	int nStartIdx = 0, nFieldIdx = 0;
	int nIPField0, nIPField1, nIPField2, nIPField3;
	CString strToken, strIPField0, strIPField1, strIPField2, strIPField3;

	if(FALSE == strServerIP.IsEmpty())
	{
		do 
		{
			strToken = strServerIP.Tokenize(_T("."), nStartIdx);

			switch(nFieldIdx)
			{
			case 0:
				strIPField0 = strToken;
				break;
			case 1:
				strIPField1 = strToken;
				break;
			case 2:
				strIPField2 = strToken;
				break;
			case 3:
				strIPField3 = strToken;
				break;
			default:
				break;
			}  

			nFieldIdx++;
		} while (strToken != _T(""));

		nIPField0 = _ttoi(strIPField0);
		nIPField1 = _ttoi(strIPField1);
		nIPField2 = _ttoi(strIPField2);
		nIPField3 = _ttoi(strIPField3);
		dwServerIP = (nIPField0 << 24) | (nIPField1 << 16) | (nIPField2 << 8) | (nIPField3);
	}
	else
	{
		bReturn =  FALSE;
	}

	return bReturn;
}




void CMotorControl::Process_AxisMotionEnd(const CMotionData& motionData)
{
	if (m_pMC2P==NULL) return;

	// 0: delay time, 1: wait time

	// delay time
	::Sleep(motionData.uData[0].dwValue);

	BOOL bResult = FALSE;
	DWORD nStartTick = GetTickCount();

	do 
	{
		bResult = IsAxisMotionEnd(motionData.nIndex);
		::Sleep(10);

		// wait time
		if ( (GetTickCount()-nStartTick) > motionData.uData[1].dwValue)
		{
			break;
		}
	} while (!bResult);

	m_pMC2P->IMC2P_AxisMotionEnd(m_nIndex, motionData.nIndex, bResult);

	return;
}

void CMotorControl::Process_GantryMotionEnd(const CMotionData& motionData)
{
	if (m_pMC2P==NULL) return;

	// 0: delay time, 1: wait time

	// delay time
	::Sleep(motionData.uData[0].dwValue);

	BOOL bResult = FALSE;
	DWORD nStartTick = GetTickCount();

	do 
	{
		bResult = IsGantryMotionEnd(motionData.nIndex);
		::Sleep(10);

		if ( (GetTickCount()-nStartTick) >motionData.uData[1].dwValue)
		{
			break;
		}
	} while (!bResult);

	m_pMC2P->IMC2P_GantryMotionEnd(m_nIndex, motionData.nIndex, bResult);

	return;
}

void CMotorControl::Process_ThetaMotionEnd(const CMotionData& motionData)
{
	if (m_pMC2P==NULL) return;

	// 0: delay time, 1: wait time

	// delay time
	::Sleep(motionData.uData[0].dwValue);

	BOOL bResult = FALSE;
	DWORD nStartTick = GetTickCount();

	do 
	{
		bResult = IsAxisThetaMotionEnd();
		::Sleep(10);

		// wait time
		if ( (GetTickCount()-nStartTick) > motionData.uData[1].dwValue)
		{
			break;
		}
	} while (!bResult);

	m_pMC2P->IMC2P_AxisMotionEnd(m_nIndex, motionData.nIndex, bResult);

	return;
}

int	 CMotorControl::GetAxisGantryIndex(int nAxisIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				return pANode->m_nGantryIdx;
			}
		}
	}
	return -1;
}

int CMotorControl::GetAxisPositionAddr(int nAxisIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				return pANode->m_nPositionAddr;
			}
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetAxisStatusAddr(int nAxisIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				return pANode->m_nStatusAddr;
			}
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetAxisGoPositionAddr(int nAxisIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				return pANode->m_nMovePositionAddr;
			}
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetAxisGoPositionCommonAddr( int nAxisIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				return pANode->m_nMovePositionCommonAddr;
			}
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetAxisJogCommandAddr(int nAxisIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				return pANode->m_nJogCommandAddr;
			}
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int	CMotorControl::GetAxisManualGoSpeedAddr(int nAxisIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				return pANode->m_nMoveSpeedAddr;
			}
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int	CMotorControl::GetAxisManualGoAccelAddr(int nAxisIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				return pANode->m_nAccelTimeAddr;
			}
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int	CMotorControl::GetAxisJogSpeedAddr(int nAxisIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				return pANode->m_nJogSpeedAddr;
			}
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int	CMotorControl::GetAxisThetaPositionAddr()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
	return m_ControlInfo.GetThetaMotorAxisAddr()->m_nPositionAddr;
}

int	CMotorControl::GetAxisThetaStatusAddr()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
	return m_ControlInfo.GetThetaMotorAxisAddr()->m_nStatusAddr;
}
int	CMotorControl::GetAxisThetaGoPositionAddr()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
	return m_ControlInfo.GetThetaMotorAxisAddr()->m_nMovePositionAddr;
}

int	CMotorControl::GetAxisThetaJogCommandAddr()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
	return m_ControlInfo.GetThetaMotorAxisAddr()->m_nJogCommandAddr;
}

int	CMotorControl::GetAxisThetaManualGoAddr()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
	return m_ControlInfo.GetThetaMotorAxisAddr()->m_nManualGoAddr;
}

int CMotorControl::GetAxisThetaManualGoAckAddr()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
	return m_ControlInfo.GetThetaMotorAxisAddr()->m_nManualGoAckAddr;
}

int	CMotorControl::GetAxisThetaManualGoSpeedAddr()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
	return m_ControlInfo.GetThetaMotorAxisAddr()->m_nMoveSpeedAddr;
}

int	CMotorControl::GetAxisThetaManualGoAccelAddr()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
	return m_ControlInfo.GetThetaMotorAxisAddr()->m_nAccelTimeAddr;
}

int	CMotorControl::GetAxisThetaJogSpeedAddr()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
	return m_ControlInfo.GetThetaMotorAxisAddr()->m_nJogSpeedAddr;
}

int CMotorControl::GetGantryAxisCount(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->GetMotorAxisAddrCount();
		}
	}
	return 0;
}

int CMotorControl::GetGantryManualGoAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nManualGoAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryManualGoAckAddr( int nGantryIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nManualGoAckAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryCollisionPositionXAddr(int nGantryIdx)
{
	for (int nG_Idx = 0; nG_Idx < m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode == NULL) continue;

		if (pGNode->m_nGantryIdx == nGantryIdx)
		{
			return pGNode->m_nCollisionXAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryCollisionPositionYAddr(int nGantryIdx)
{
	for (int nG_Idx = 0; nG_Idx < m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode == NULL) continue;

		if (pGNode->m_nGantryIdx == nGantryIdx)
		{
			return pGNode->m_nCollisionYAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryAutoGoAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nAutoReviewGoAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryAutoGoAckAddr( int nGantryIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nAutoReviewGoAckAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int	 CMotorControl::GetGantryMeasureAutoGoAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nAutoMeasureGoAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryMeasureAutoGoAckAddr( int nGantryIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nAutoMeasureGoAckAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryWsiAutoGoAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nAutoWsiGoAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryWsiAutoGoAckAddr( int nGantryIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nAutoWsiGoAckAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryMoveCountAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nReviewMoveCountAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int	CMotorControl::GetGantryMeasureMoveCountAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nMeasureMoveCountAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryWsiMoveCountAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nWsiMoveCountAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryTriggerCountAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nReviewTriggerCountAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int	CMotorControl::GetGantryMeasureTriggerCountAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nMeasureTriggerCountAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryWsiTriggerCountAddr(int nGantryIdx)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nWsiTriggerCountAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryWsiMotionEndAddr( int nGantryIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			//¸ðÅÍ ¸ð¼Ç END ÁÖ¼Ò °¡Á®¿À´Â ºÎºÐ
			//return 332;//pGNode->m_nWsiMotionEndAddr;
			if (nGantryIdx == 0)
			{
				return 452;//pGNode->m_nWsiMotionEndAddr;
			}
			else
			{ 
				return 453;
			}
			
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetGantryWsiNextPosMoveAddr( int nGantryIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			if (nGantryIdx == 0)
			{
				//C-PJT
				return 472;
			}
			else
			{
				return 473;
			}
			//return 352;//pGNode->m_nWsiNextPosMoveAddr;
		}
		
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetSoftwareTriggerAddr( int nGantryIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nSoftwareTriggerAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetSoftwareTriggerCmdAddr( int nGantryIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nSoftwareTriggerCmdAdr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

int CMotorControl::GetImageSnapCompleteAddr( int nGantryIdx )
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		if (pGNode->m_nGantryIdx==nGantryIdx)
		{
			return pGNode->m_nImageSnapCompleteAddr;
		}
	}
	return MOTOR_ADDRESS_NONE;
}

BOOL CMotorControl::GetAxisLimitPosition(int nAxisIdx, double& dPlusLimit, double& dMinusLimit)
{
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;

		for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
		{
			CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (pANode==NULL) continue;

			if (pANode->m_nAxisIdx==nAxisIdx)
			{
				dPlusLimit =  pANode->m_dPlusLimitPos;
				dMinusLimit =  pANode->m_dMinusLimitPos;
				return TRUE;
			}
		}
	}
	return FALSE;
}


// axis
BOOL CMotorControl::AxisManualGo(int nAxisIndex, double dPosition)
{
	int nGantryIdx = GetAxisGantryIndex(nAxisIndex);

	if (nGantryIdx<0) return FALSE;

	UINT nManualGoAddr = GetGantryManualGoAddr(nGantryIdx);
	if (nManualGoAddr==MOTOR_ADDRESS_NONE) return FALSE;

	UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisIndex);
	if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

	if (WriteAddressValue(nGoPosAddr, dPosition)==FALSE)
	{
		return FALSE;
	}

	int nAxisCount = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisCount += GetGantryAxisCount(i);
	}

	// ÇØ´ç ºñÆ®¸¸ set
	long nValue = 1 << (nAxisIndex-nAxisCount);

	return WriteAddressValue(nManualGoAddr, nValue);
}


BOOL CMotorControl::AxisManualGoEnd(int nAxisIdx, double dPosition)
{
	if (AxisManualGo(nAxisIdx, dPosition)==FALSE) return FALSE;

	if (m_pMotionThread==NULL) return FALSE;

	CMotionData motionData;
	motionData.nType				= ThreadDataType_AxisMotionEnd;
	motionData.nIndex				= nAxisIdx;
	motionData.uData[0].dwValue		= 500;	// delay time
	motionData.uData[1].dwValue		= 5000; // wait time

	return m_pMotionThread->AddThreadData(motionData);
}

BOOL CMotorControl::AxisManualGo(const VectorInteger& vectorAxis, const VectorDouble& vectorPosition)
{
	if (vectorAxis.size()<1 || vectorPosition.size()<1) return FALSE;

	if (vectorAxis.size() != vectorPosition.size()) return FALSE;

	// ¸ðµç ÃàÀÇ °ÕÆ®¸® À妽º°¡ °°Àº°¡ È®ÀÎ
	int nGantryIdx = GetAxisGantryIndex(vectorAxis[0]);
	if (nGantryIdx<0) return FALSE;
	for (int i=1; i<(int)vectorAxis.size(); i++)
	{
		if (nGantryIdx!=GetAxisGantryIndex(vectorAxis[i]))
		{
			return FALSE;
		}
	}

	// °ÕÆ®¸® ¼öµ¿À̵¿ ÁÖ¼Ò È®ÀÎ
	UINT nManualGoAddr = GetGantryManualGoAddr(nGantryIdx);
	if (nManualGoAddr==MOTOR_ADDRESS_NONE) return FALSE;

	// Ãະ ¼öµ¿À̵¿ ÁÂÇ¥ ¾²±â
	for (int i=0; i<(int)vectorAxis.size(); i++)
	{
		UINT nGoPosAddr = GetAxisGoPositionAddr(vectorAxis[i]);
		if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

		if (WriteAddressValue(nGoPosAddr, vectorPosition[i])==FALSE)
		{
			return FALSE;
		}
	}

	int nAxisCount = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisCount += GetGantryAxisCount(i);
	}

	// ÇØ´ç ÃàÀÇ ºñÆ®¸¸ set
	long nValue = 0;
	for (int i=0; i<(int)vectorAxis.size(); i++)
	{
		long nTemp = 1 << (vectorAxis[i]-nAxisCount);
		nValue = nValue | nTemp;
	}

	return WriteAddressValue(nManualGoAddr, nValue);
}

BOOL CMotorControl::AxisJogPlus(int nAxisIdx)
{
	int nJogAddr = (int)GetAxisJogCommandAddr(nAxisIdx);
	if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nJogAddr, (long)1);
}

BOOL CMotorControl::AxisJogMinus(int nAxisIdx)
{
	int nJogAddr = (int)GetAxisJogCommandAddr(nAxisIdx);
	if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nJogAddr, (long)2);
}

BOOL CMotorControl::AxisJogStop(int nAxisIdx)
{
	int nJogAddr = (int)GetAxisJogCommandAddr(nAxisIdx);
	if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nJogAddr, (long)0);
}

BOOL CMotorControl::AxisManualGoSpeed(int nAxisIdx, double dSpeed)
{
	int nAddr = GetAxisManualGoSpeedAddr(nAxisIdx);

	if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nAddr, dSpeed);
}

BOOL CMotorControl::AxisJogSpeed(int nAxisIdx, double dSpeed)
{
	int nAddr = GetAxisJogSpeedAddr(nAxisIdx);

	if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nAddr, dSpeed);
}

BOOL CMotorControl::AxisManualGoAccel(int nAxisIdx, int nAccel)
{
	int nAddr = GetAxisManualGoAccelAddr(nAxisIdx);

	if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nAddr, (long)nAccel);
}

BOOL CMotorControl::AxisJogStop()
{
	int nResultCode = 1;
	for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
	{
		nResultCode = nResultCode & AxisJogStop(nAxisIdx);
	}
	return nResultCode;
}

BOOL CMotorControl::AxisManualGoSpeed(double dSpeed)
{
	int nResultCode = 1;
	for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
	{
		nResultCode = nResultCode & AxisManualGoSpeed(nAxisIdx, dSpeed);
	}
	return nResultCode;
}

BOOL CMotorControl::AxisJogSpeed(double dSpeed)
{
	int nResultCode = 1;
	for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
	{
		nResultCode = nResultCode & AxisJogSpeed(nAxisIdx, dSpeed);
	}
	return nResultCode;
}

BOOL CMotorControl::AxisManualGoAccel(int nAccel)
{
	int nResultCode = 1;
	for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
	{
		nResultCode = nResultCode & AxisManualGoAccel(nAxisIdx, (long)nAccel);
	}
	return nResultCode;
}

BOOL CMotorControl::IsAxisMotionEnd()
{
	int nResultCode = 1;
	for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
	{
		nResultCode = nResultCode && IsAxisMotionEnd(nAxisIdx);
	}
	return nResultCode;
}

// axis theta
BOOL CMotorControl::AxisThetaManualGo(double dPosition)
{
	if (m_ControlInfo.GetUseThetaMotor()!=1) return FALSE;

	UINT nManualGoAddr = GetAxisThetaManualGoAddr();
	if (nManualGoAddr==MOTOR_ADDRESS_NONE) return FALSE;

	UINT nGoPosAddr = GetAxisThetaGoPositionAddr();
	if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

	if (WriteAddressValue(nGoPosAddr, dPosition)==FALSE)
	{
		return FALSE;
	}
	return WriteAddressValue(nManualGoAddr, (long)1);
}


BOOL CMotorControl::AxisThetaManualGoAck( long &lAck )
{
	if (m_ControlInfo.GetUseThetaMotor()!=1) return FALSE;

	UINT nManualGoAckAddr = GetAxisThetaManualGoAckAddr();
	if (nManualGoAckAddr == MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nManualGoAckAddr, lAck);
}

BOOL CMotorControl::AxisThetaManualGoAck( long &lAck, DWORD nWaitTime )
{
	// delay time
	int nDelayTime = 10;

	BOOL bResult = FALSE;
	DWORD nStartTick = GetTickCount();

	do 
	{
		::Sleep(nDelayTime);
		AxisThetaManualGoAck(lAck);

		// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼­... )
		if (lAck == TRUE)
			bResult = TRUE;

		// wait time
		if ( (GetTickCount()-nStartTick) > nWaitTime)
		{
			break;
		}
	} while (!bResult);

	return TRUE;
}

BOOL CMotorControl::AxisThetaManualGoEnd(double dPosition)
{
	if (AxisThetaManualGo(dPosition)==FALSE) return FALSE;

	if (m_pMotionThread==NULL) return FALSE;

	CMotionData motionData;
	motionData.nType				= ThreadDataType_ThetaMotionEnd;
	motionData.nIndex				= THETAMOTOR_AXIS_INDEX;
	motionData.uData[0].dwValue		= 500;	// delay time
	motionData.uData[1].dwValue		= 5000; // wait time

	return m_pMotionThread->AddThreadData(motionData);
}

BOOL CMotorControl::AxisThetaJogPlus()
{
	int nJogAddr = (int)GetAxisThetaJogCommandAddr();
	if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nJogAddr, (long)1);
}

BOOL CMotorControl::AxisThetaJogMinus()
{
	int nJogAddr = (int)GetAxisThetaJogCommandAddr();
	if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nJogAddr, (long)2);
}

BOOL CMotorControl::AxisThetaJogStop()
{
	int nJogAddr = (int)GetAxisThetaJogCommandAddr();
	if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nJogAddr, (long)0);
}

BOOL CMotorControl::AxisThetaManualGoSpeed(double dSpeed)
{
	int nAddr = GetAxisThetaManualGoSpeedAddr();

	if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nAddr, dSpeed);
}

BOOL CMotorControl::AxisThetaJogSpeed(double dSpeed)
{
	int nAddr = GetAxisThetaJogSpeedAddr();

	if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nAddr, dSpeed);
}

BOOL CMotorControl::AxisThetaManualGoAccel(int nAccel)
{
	int nAddr = GetAxisThetaManualGoAccelAddr();

	if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nAddr, (long)nAccel);
}

// gantry
BOOL CMotorControl::GantryManualGo(int nGantryIdx, const VectorDouble& vectorAxisPos)
{	
	UINT nManualAddr = GetGantryManualGoAddr(nGantryIdx);
	if (nManualAddr==MOTOR_ADDRESS_NONE) return FALSE;

	int nAxisCount = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisCount += GetGantryAxisCount(i);
	}

	long nValue = 0;
	for (int i=0; i<(int)vectorAxisPos.size(); i++)
	{
		UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisCount+i);
		if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

		if (WriteAddressValue(nGoPosAddr, vectorAxisPos[i])==FALSE)
		{
			return FALSE;
		}

		nValue = nValue | (1 << i);
	}

	return WriteAddressValue(nManualAddr, nValue);
}

BOOL CMotorControl::GantryManualGo(int nGantryIdx, const VectorDouble& vectorAxisPos, int nMoveType)
{
	UINT nManualAddr = GetGantryManualGoAddr(nGantryIdx);
	if (nManualAddr==MOTOR_ADDRESS_NONE) return FALSE;

	int nAxisCount = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisCount += GetGantryAxisCount(i);
	}

	int nBitData = 1;
	for (int i=0; i<GetGantryAxisCount(nGantryIdx); i++)
	{
		if ( nBitData & nMoveType ) 
		{
			UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisCount+i);
			if (nGoPosAddr!=MOTOR_ADDRESS_NONE) 
			{
				if (WriteAddressValue(nGoPosAddr, vectorAxisPos[i])==FALSE)
				{
					continue;
				}
			}
		}
		nBitData = nBitData << 1;
	}
	Sleep(700);
	BOOL bResult = WriteAddressValue(nManualAddr, (long) nMoveType);

	//return WriteAddressValue(nManualAddr, (long) nMoveType);
	return bResult;
}

BOOL CMotorControl::GantryManualGo(int nGantryIdx, double dPosX, double dPosY)
{
	UINT nManualAddr = GetGantryManualGoAddr(nGantryIdx);
	if (nManualAddr==MOTOR_ADDRESS_NONE) return FALSE;

	int nAxisCount = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisCount += GetGantryAxisCount(i);
	}

	UINT nXGoPosAddr = GetAxisGoPositionAddr(nAxisCount);
	if (nXGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

	UINT nYGoPosAddr = GetAxisGoPositionAddr(nAxisCount+1);
	//UINT nYGoPosAddr = GetAxisGoPositionAddr(nAxisCount+2);	// lmk motor y2
	if (nYGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

	if (WriteAddressValue(nXGoPosAddr, dPosX)==FALSE)
	{
		return FALSE;
	}

	if (WriteAddressValue(nYGoPosAddr, dPosY)==FALSE)
	{
		return FALSE;
	}

	return WriteAddressValue(nManualAddr, (long)3);
	//return WriteAddressValue(nManualAddr, (long)5);	// lmk motor y2
}

BOOL CMotorControl::GantryManualGoAck( int nGantryIdx , long &lAck)
{
	UINT nGantryManualGoAckAddr = GetGantryManualGoAckAddr(nGantryIdx);
	if (nGantryManualGoAckAddr == MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nGantryManualGoAckAddr,lAck );
}

BOOL CMotorControl::GantryManualGoAck( int nGantryIdx, long &lAck, DWORD nWaitTime )
{
	// delay time
	int nDelayTime = 10;

	BOOL bResult = FALSE;
	DWORD nStartTick = GetTickCount();

	do 
	{
		::Sleep(nDelayTime);
		GantryManualGoAck( nGantryIdx , lAck);

		// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼­... )
		if (lAck == TRUE)
			bResult = TRUE;

		// wait time
		if ( (GetTickCount()-nStartTick) > nWaitTime)
		{
			break;
		}
	} while (!bResult);

	return TRUE;
}


BOOL CMotorControl::GantryManualGoEnd(int nGantryIdx, const VectorDouble& vectorPos)
{
	if (GantryManualGo(nGantryIdx, vectorPos)==FALSE) return FALSE;

	if (m_pMotionThread==NULL) return FALSE;

	CMotionData motionData;
	motionData.nType				= ThreadDataType_GantryMotionEnd;
	motionData.nIndex				= nGantryIdx;
	motionData.uData[0].dwValue		= 500;	// delay time
	motionData.uData[1].dwValue		= 5000; // wait time

	return m_pMotionThread->AddThreadData(motionData);
}

BOOL CMotorControl::GantryManualGoEnd(int nGantryIdx, const VectorDouble& vectorPos, int nMoveType)
{
	if (GantryManualGo(nGantryIdx, vectorPos, nMoveType)==FALSE) return FALSE;

	if (m_pMotionThread==NULL) return FALSE;

	CMotionData motionData;
	motionData.nType				= ThreadDataType_GantryMotionEnd;
	motionData.nIndex				= nGantryIdx;
	motionData.uData[0].dwValue		= 500;	// delay time
	motionData.uData[1].dwValue		= 5000; // wait time

	return m_pMotionThread->AddThreadData(motionData);
}

BOOL CMotorControl::GantryManualGoEnd(int nGantryIdx, double dPosX, double dPosY)
{
	if (GantryManualGo(nGantryIdx, dPosX, dPosY)==FALSE) return FALSE;

	if (m_pMotionThread==NULL) return FALSE;

	CMotionData motionData;
	motionData.nType				= ThreadDataType_GantryMotionEnd;
	motionData.nIndex				= nGantryIdx;
	motionData.uData[0].dwValue		= 500;	// delay time
	motionData.uData[1].dwValue		= 5000; // wait time

	return m_pMotionThread->AddThreadData(motionData);
}

BOOL CMotorControl::GantrySetCollisionPosition(int nGantryIdx, double dPosX, double dPosY,int nFirstIn)
{
	BOOL bRet = FALSE;

	int nAddr = (int)GetGantryCollisionPositionXAddr(nGantryIdx);
	if (nAddr != MOTOR_ADDRESS_NONE)
		bRet = WriteAddressValue(nAddr, dPosX);

	int nAddr2 = 400;
	if (nAddr2 != MOTOR_ADDRESS_NONE)
		bRet = WriteAddressValue(nAddr2,(long)nFirstIn);


	//nAddr = (int)GetGantryCollisionPositionYAddr(nGantryIdx);
	//if (nAddr != MOTOR_ADDRESS_NONE)
	//	bRet |= WriteAddressValue(nAddr, dPosY);

	return bRet;
}

const int CMotorControl::GetGantryCollisionXAddr(int nGantryIdx)
{
	for (int nG_Idx = 0; nG_Idx < m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode == NULL)
			continue;

		if (pGNode->m_nGantryIdx == nGantryIdx)
			return pGNode->m_nCollisionXAddr;
	}

	return MOTOR_ADDRESS_NONE;
}

BOOL CMotorControl::GantrySetTwoGantrySyncModeSend(bool bUse)
{
	BOOL bRet = FALSE;
	//WSI ÁøÇà½Ã Á¿ìÁøÇàÈÄ ´ÙÀ½ Æ÷ÀÎÆ® ÁøÇà  1-> ON 
	int nAddr3 = 410;
	if (nAddr3 != MOTOR_ADDRESS_NONE)
		bRet = WriteAddressValue(nAddr3, (long)bUse);
	return bRet;
}

BOOL CMotorControl::IsAxisMotionEnd(int nAxisIdx)
{
	// axis
	if (nAxisIdx<0 || nAxisIdx>=m_nAxisCount) return TRUE;

	if( (m_pAxisStatus[nAxisIdx] & 0x01) == 0)
	{
		return FALSE;
	}
	return TRUE;
}

BOOL CMotorControl::IsAxisThetaMotionEnd()
{
	if (m_ControlInfo.GetUseThetaMotor()==FALSE) return TRUE;

	// axis theta
	if( (m_axisThetaStatus & 0x01) == 0)
	{
		return FALSE;
	}

	return TRUE;	
}


BOOL CMotorControl::IsGantryMotionEnd(int nGantryIdx)
{
	int nAxisStart = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisStart += GetGantryAxisCount(i);
	}

	int nAxisEnd = nAxisStart + GetGantryAxisCount(nGantryIdx);

	for (int i=nAxisStart; i<nAxisEnd; i++)
	{
		if( (m_pAxisStatus[i] & 0x01) == 0)
		{
			return FALSE;
		}
	}

	return TRUE;
}


BOOL CMotorControl::GantryAutoGo(int nGantryIdx, const VectorDouble& vectorPosX, const VectorDouble& vectorPosY, BOOL bCommand)
{
	double dPlusLimitX, dMinusLimitX, dPlusLimitY, dMinusLimitY;
	int nPointCount = (int)vectorPosX.size();

	if (nPointCount > m_ControlInfo.GetMaxAxisMoveCount()) return FALSE;

	if (nPointCount<1 || vectorPosY.size()<1) return FALSE;

	if (nPointCount!=(int)vectorPosY.size()) return FALSE;

	int nStartAxisIdx = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nStartAxisIdx += GetGantryAxisCount(i);
	}

	int nGoPosAddrX = GetAxisGoPositionAddr(nStartAxisIdx);
	if (nGoPosAddrX==MOTOR_ADDRESS_NONE) return FALSE;

	int nGoPosAddrY = GetAxisGoPositionAddr(nStartAxisIdx+1);
	if (nGoPosAddrY==MOTOR_ADDRESS_NONE) return FALSE;

	GetAxisLimitPosition(nStartAxisIdx, dPlusLimitX, dMinusLimitX);
	GetAxisLimitPosition(nStartAxisIdx+1, dPlusLimitY, dMinusLimitY);

	BOOL bSuccess = TRUE;
	int nMaxMotorMoveCount = m_ControlInfo.GetMaxAxisMoveCount();
	float *fXPos=NULL, *fYPos=NULL;
	fXPos = new float[nMaxMotorMoveCount];
	fYPos = new float[nMaxMotorMoveCount];
	ZeroMemory(fXPos, sizeof(float)*nMaxMotorMoveCount);
	ZeroMemory(fYPos, sizeof(float)*nMaxMotorMoveCount);

	for (int i=0; i<nPointCount; i++)
	{
		if(vectorPosX[i] > dMinusLimitX && vectorPosX[i] < dPlusLimitX)
		{
			fXPos[i] = float(vectorPosX[i]);
		}
		else
		{
			fXPos[i] = 0.0f;
			if (m_pMC2P)
			{
				m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("X MotorPosition Idx[%d] Position[%.03lf] Limit"), i, vectorPosX[i]);
			}
		}

		if(vectorPosY[i] > dMinusLimitY && vectorPosY[i] < dPlusLimitY)
		{
			fYPos[i] = float(vectorPosY[i]);
		}
		else
		{
			fYPos[i] = 0.0f;
			if (m_pMC2P)
			{
				m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("Y MotorPosition Idx[%d] Position[%.03lf] Limit"), i, vectorPosY[i]);
			}
		}
	}

	// write x pos
	if (WriteAddressValue(nGoPosAddrX, fXPos, nPointCount) == FALSE)
	{
		delete fXPos;
		delete fYPos;

		return FALSE;
	}

	// write y pos
	if (WriteAddressValue(nGoPosAddrY, fYPos, nPointCount)==FALSE)
	{
		delete fXPos;
		delete fYPos;

		return FALSE;
	}

	delete fXPos;
	delete fYPos;

	// count
	int nMoveCountAddr = (int)GetGantryMoveCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
	if (WriteAddressValue(nMoveCountAddr, (long)nPointCount)==FALSE)
	{
		return FALSE;
	}

	if (bCommand==FALSE) 
	{
		return TRUE;
	}

	// command
	int nMoveCmdAddr = (int)GetGantryAutoGoAddr(nGantryIdx);

	return WriteAddressValue(nMoveCmdAddr, (long)1);
}

BOOL CMotorControl::GantryAutoGo(int nGantryIdx)
{
	int nGoCmdAddr = (int)GetGantryAutoGoAddr(nGantryIdx);

	if (nGoCmdAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nGoCmdAddr, (long)1);
}

BOOL CMotorControl::GantryAutoGoAck( int nGantryIdx, long& lAck )
{
	int nGantryReviewAutoGoAck = GetGantryManualGoAckAddr(nGantryIdx);
	if (nGantryReviewAutoGoAck == FALSE) return FALSE;

	return ReadAddressValue(nGantryReviewAutoGoAck,lAck);
}

BOOL CMotorControl::GantryAutoGoAck( int nGantryIdx, long& lAck, DWORD nWaitTime )
{
	// delay time
	int nDelayTime = 10;

	BOOL bResult = FALSE;
	DWORD nStartTick = GetTickCount();

	do 
	{
		::Sleep(nDelayTime);
		GantryAutoGoAck( nGantryIdx, lAck );

		// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼­... )
		if (lAck == TRUE)
			bResult = TRUE;

		// wait time
		if ( (GetTickCount()-nStartTick) > nWaitTime)
		{
			break;
		}
	} while (!bResult);

	return TRUE;
}

BOOL CMotorControl::GantrySetMoveCount(int nGantryIdx, long nCount)
{
	UINT nMoveCountAddr = GetGantryMoveCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nMoveCountAddr, nCount);
}

BOOL CMotorControl::GantryGetMoveCount(int nGantryIdx, long& nCount)
{
	UINT nCountAddr = GetGantryMoveCountAddr(nGantryIdx);
	if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nCountAddr, nCount);
}

BOOL CMotorControl::GantryGetTriggerCount(int nGantryIdx, long& nCount)
{
	UINT nCountAddr = GetGantryTriggerCountAddr(nGantryIdx);
	if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nCountAddr, nCount);
}

BOOL CMotorControl::GantryMeasureAutoGo(int nGantryIdx, const VectorDouble& vectorPosData, BOOL bCommand)
{
	int nGantryAxisCount = GetGantryAxisCount(nGantryIdx);

	int nTotalPointCount = (int)vectorPosData.size();

	if ( (nTotalPointCount%nGantryAxisCount)!=0 ) return FALSE;

	int nPointCount = nTotalPointCount / nGantryAxisCount;

	if (nPointCount > m_ControlInfo.GetMaxAxisMoveCount()) return FALSE;

	int nStartAxisIdx = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nStartAxisIdx += GetGantryAxisCount(i);
	}

	BOOL bSuccess = TRUE;
	double dPosData = 0.0;
	double dPlusLimit, dMinusLimit;
	int nMaxMotorMoveCount = m_ControlInfo.GetMaxAxisMoveCount();
	float *fPosData = new float[nMaxMotorMoveCount];
	memset(fPosData, 0, sizeof(float)*nMaxMotorMoveCount);

	for (int nAxisIdx=0; nAxisIdx<nGantryAxisCount; nAxisIdx++)
	{
		int nGoPosAddr = GetAxisGoPositionAddr(nStartAxisIdx+nAxisIdx);
		if (nGoPosAddr==MOTOR_ADDRESS_NONE) 
		{
			delete fPosData;

			return FALSE;
		}

		GetAxisLimitPosition(nStartAxisIdx+nAxisIdx, dPlusLimit, dMinusLimit);

		for (int nPosIdx=0; nPosIdx<nPointCount; nPosIdx++)
		{
			dPosData = vectorPosData[(nPosIdx*nGantryAxisCount)+nAxisIdx];

			if(dPosData > dMinusLimit && dPosData < dPlusLimit)
			{
				fPosData[nPosIdx] = float(dPosData);
			}
			else
			{
				fPosData[nPosIdx] = 0.0;
				if (m_pMC2P)
				{
					m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("MotorPosition Idx[%d] Position[%.03lf] Limit"), nAxisIdx, dPosData);
				}
			}
		}

		// write y pos
		if (WriteAddressValue(nGoPosAddr, fPosData, nPointCount)==FALSE)
		{
			delete fPosData;

			return FALSE;
		}
	}

	delete fPosData;

	// count
	int nMoveCountAddr = (int)GetGantryMeasureMoveCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
	if (WriteAddressValue(nMoveCountAddr, (long)nPointCount)==FALSE)
	{
		return FALSE;
	}

	if (bCommand==FALSE) 
	{
		return TRUE;
	}

	// command
	int nMoveCmdAddr = (int)GetGantryMeasureAutoGoAddr(nGantryIdx);

	return WriteAddressValue(nMoveCmdAddr, (long)1);
}

BOOL CMotorControl::GantryMeasureAutoGo(int nGantryIdx)
{
	int nGoCmdAddr = (int)GetGantryMeasureAutoGoAddr(nGantryIdx);

	if (nGoCmdAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nGoCmdAddr, (long)1);
}

BOOL CMotorControl::GantryMeasureAutoGoAck( int nGantryIdx, long &lAck )
{
	int nGantryMeasureAutoGoAckAddr = GetGantryMeasureAutoGoAckAddr(nGantryIdx);
	if (nGantryMeasureAutoGoAckAddr == MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nGantryMeasureAutoGoAckAddr, lAck);
}

BOOL CMotorControl::GantryMeasureAutoGoAck( int nGantryIdx, long &lAck, DWORD nWaitTime )
{
	// delay time
	int nDelayTime = 10;

	BOOL bResult = FALSE;
	DWORD nStartTick = GetTickCount();

	do 
	{
		::Sleep(nDelayTime);
		GantryMeasureAutoGoAck( nGantryIdx, lAck );

		// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼­... )
		if (lAck == TRUE)
			bResult = TRUE;

		// wait time
		if ( (GetTickCount()-nStartTick) > nWaitTime)
		{
			break;
		}
	} while (!bResult);

	return TRUE;
}

BOOL CMotorControl::GantrySetMeasureMoveCount(int nGantryIdx, long nCount)
{
	UINT nMoveCountAddr = GetGantryMeasureMoveCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nMoveCountAddr, nCount);
}

BOOL CMotorControl::GantryGetMeasureMoveCount(int nGantryIdx, long& nCount)
{
	UINT nCountAddr = GetGantryMeasureMoveCountAddr(nGantryIdx);
	if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nCountAddr, nCount);
}

BOOL CMotorControl::GantryGetMeasureTriggerCount(int nGantryIdx, long& nCount)
{
	UINT nCountAddr = GetGantryMeasureTriggerCountAddr(nGantryIdx);
	if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nCountAddr, nCount);
}

BOOL CMotorControl::GantryWsiAutoGo(int nGantryIdx, const VectorDouble& vectorPosX, const VectorDouble& vectorPosY, BOOL bCommand)
{
	double dPlusLimitX, dMinusLimitX, dPlusLimitY, dMinusLimitY;
	int nPointCount = (int)vectorPosX.size();

	if (nPointCount > m_ControlInfo.GetMaxAxisMoveCount()) return FALSE;

	if (nPointCount<1 || vectorPosY.size()<1) return FALSE;

	if (nPointCount!=(int)vectorPosY.size()) return FALSE;

	int nStartAxisIdx = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nStartAxisIdx += GetGantryAxisCount(i);
	}

	int nGoPosAddrX = GetAxisGoPositionAddr(nStartAxisIdx);
	if (nGoPosAddrX==MOTOR_ADDRESS_NONE) return FALSE;

	int nGoPosAddrY = GetAxisGoPositionAddr(nStartAxisIdx+1);
	if (nGoPosAddrY==MOTOR_ADDRESS_NONE) return FALSE;

	GetAxisLimitPosition(nStartAxisIdx, dPlusLimitX, dMinusLimitX);
	GetAxisLimitPosition(nStartAxisIdx+1, dPlusLimitY, dMinusLimitY);

	BOOL bSuccess = TRUE;
	int nMaxMotorMoveCount = m_ControlInfo.GetMaxAxisMoveCount();
	float *fXPos=NULL, *fYPos=NULL;
	fXPos = new float[nMaxMotorMoveCount];
	fYPos = new float[nMaxMotorMoveCount];
	ZeroMemory(fXPos, sizeof(float)*nMaxMotorMoveCount);
	ZeroMemory(fYPos, sizeof(float)*nMaxMotorMoveCount);

	for (int i=0; i<nPointCount; i++)
	{
		if(vectorPosX[i] > dMinusLimitX && vectorPosX[i] < dPlusLimitX)
		{
			fXPos[i] = float(vectorPosX[i]);
		}
		else
		{
			fXPos[i] = 0.0f;
			if (m_pMC2P)
			{
				m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("X MotorPosition Idx[%d] Position[%.03lf] Limit"), i, vectorPosX[i]);
			}
		}

		if(vectorPosY[i] > dMinusLimitY && vectorPosY[i] < dPlusLimitY)
		{
			fYPos[i] = float(vectorPosY[i]);
		}
		else
		{
			fYPos[i] = 0.0f;
			if (m_pMC2P)
			{
				m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("Y MotorPosition Idx[%d] Position[%.03lf] Limit"), i, vectorPosY[i]);
			}
		}
	}

	// write x pos
	if (WriteAddressValue(nGoPosAddrX, fXPos, nPointCount)==FALSE)
	{
		delete fXPos;
		delete fYPos;

		return FALSE;
	}

	// write y pos
	if (WriteAddressValue(nGoPosAddrY, fYPos, nPointCount)==FALSE)
	{
		delete fXPos;
		delete fYPos;

		return FALSE;
	}

	delete fXPos;
	delete fYPos;

	if (bCommand==FALSE) return TRUE;


	int nMoveCountAddr = (int)GetGantryWsiMoveCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
	if (WriteAddressValue(nMoveCountAddr, (long)nPointCount)==FALSE)
	{
		return FALSE;
	}

	if (bCommand==FALSE) 
	{
		return TRUE;
	}


	int nGoCmdAddr = (int)GetGantryWsiAutoGoAddr(nGantryIdx);
	//int nGoCmdAddr = (int)GetGantryWsiAutoGoAddr((long)1);
	if (nGoCmdAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nGoCmdAddr, (long)1);
}

BOOL CMotorControl::GantryWsiAutoGo(int nGantryIdx)
{
	int nGoCmdAddr = (int)GetGantryWsiAutoGoAddr(nGantryIdx);

	if (nGoCmdAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nGoCmdAddr, (long)1);
}

BOOL CMotorControl::GantryWsiAutoGoAck( int nGantryIdx, long &lAck )
{
	int nGantryWsiAutoGoAckAddr = GetGantryWsiAutoGoAckAddr(nGantryIdx);
	if(nGantryWsiAutoGoAckAddr == MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nGantryWsiAutoGoAckAddr,lAck);
}

BOOL CMotorControl::GantryWsiAutoGoAck( int nGantryIdx, long &lAck, DWORD nWaitTime )
{
	// delay time
	int nDelayTime = 10;

	BOOL bResult = FALSE;
	DWORD nStartTick = GetTickCount();

	do 
	{
		::Sleep(nDelayTime);
		GantryWsiAutoGoAck( nGantryIdx, lAck );

		// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼­... )
		if (lAck == TRUE)
			bResult = TRUE;

		// wait time
		if ( (GetTickCount()-nStartTick) > nWaitTime)
		{
			break;
		}
	} while (!bResult);

	return TRUE;
}

BOOL CMotorControl::GantrySetWsiMoveCount(int nGantryIdx, long nCount)
{
	UINT nMoveCountAddr = GetGantryWsiMoveCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nMoveCountAddr, nCount);
}

BOOL CMotorControl::GantryGetWsiMoveCount(int nGantryIdx, long& nCount)
{
	UINT nCountAddr = GetGantryWsiMoveCountAddr(nGantryIdx);
	if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nCountAddr, nCount);
}

BOOL CMotorControl::GantryGetWsiTriggerCount(int nGantryIdx, long& nCount)
{
	UINT nCountAddr = GetGantryWsiTriggerCountAddr(nGantryIdx);
	if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return ReadAddressValue(nCountAddr, nCount);
}

BOOL CMotorControl::GantryWsiNextPosMove( int nGantryIdx, long nNextIdx )
{
	if (nNextIdx<0 || nNextIdx>m_ControlInfo.GetMaxAxisMoveCount()) return FALSE;

	UINT nAddr = GetGantryWsiNextPosMoveAddr(nGantryIdx);
	if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;

	//¸ðÅÍ¿¡°Ô ÇöÀç À妽º ¿Í ´ÙÀ½ ÃøÁ¤ Æ÷ÀÎÆ® À̵¿ ¸í·É

	// set next index
	if (!WriteAddressValue(nAddr+2, nNextIdx)) return FALSE;

	// set next position move signal
	return WriteAddressValue(nAddr, long(1));
}

// Software
BOOL CMotorControl::SoftWareTriggerCmd( int nGantryIdx, long lCmd )
{
	int nSoftwareTriggerCmdAddr = GetSoftwareTriggerCmdAddr(nGantryIdx);
	if (nSoftwareTriggerCmdAddr == MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(nSoftwareTriggerCmdAddr, lCmd);
}

// common
BOOL CMotorControl::CommonSetAfDelayTime(long nTime)
{
	if (!m_bConnected) return FALSE;

	const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
	if (pAddr==NULL) return FALSE;

	if (pAddr->m_nAfDelayTimeAddr==MOTOR_ADDRESS_NONE) return FALSE;

	if(nTime < 1)
	{
		return WriteAddressValue(pAddr->m_nAfDelayTimeAddr, (long)m_ControlInfo.GetAfDelayTime());
	}

	return WriteAddressValue(pAddr->m_nAfDelayTimeAddr, (float)nTime);
}

BOOL CMotorControl::CommonSetImageDelayTime(long nTime)
{
	if (!m_bConnected) return FALSE;

	const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
	if (pAddr==NULL) return FALSE;

	if (pAddr->m_nAfDelayTimeAddr==MOTOR_ADDRESS_NONE) return FALSE;

	if(nTime < 1)
	{
		return WriteAddressValue(pAddr->m_nImageDelayTimeAddr, (long)m_ControlInfo.GetImageDelayTime());
	}

	return WriteAddressValue(pAddr->m_nImageDelayTimeAddr, (float)nTime);
}


BOOL CMotorControl::CommonSetWsiDelayTime(long nTime)
{
	if (!m_bConnected) return FALSE;

	const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
	if (pAddr==NULL) return FALSE;

	if (pAddr->m_nWsiDelayTimeAddr==MOTOR_ADDRESS_NONE) return FALSE;

	if(nTime < 1)
	{
		return WriteAddressValue(pAddr->m_nWsiDelayTimeAddr, (long)m_ControlInfo.GetWsiDelayTime());
	}

	return WriteAddressValue(pAddr->m_nWsiDelayTimeAddr, (float)nTime);
}

BOOL CMotorControl::CommonSetAutoStop()
{
	if (!m_bConnected) return FALSE;

	const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
	if (pAddr==NULL) return FALSE;

	if (pAddr->m_nAllAutoStopAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(pAddr->m_nAllAutoStopAddr, (long)1);
}

BOOL CMotorControl::CommonSetAutoGoAll()
{
	if (!m_bConnected) return FALSE;

	const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
	if (pAddr==NULL) return FALSE;

	if (pAddr->m_nAllAutoReviewGoAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(pAddr->m_nAllAutoReviewGoAddr, (long)1);
}

BOOL CMotorControl::CommonSetAutoMeasureGoAll()
{
	if (!m_bConnected) return FALSE;

	const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
	if (pAddr==NULL) return FALSE;

	if (pAddr->m_nAllAutoMeasureGoAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(pAddr->m_nAllAutoMeasureGoAddr, (long)1);
}

BOOL CMotorControl::CommonSetAutoWsiGoAll()
{
	const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
	if (pAddr==NULL) return FALSE;

	if (pAddr->m_nAllAutoWsiGoAddr==MOTOR_ADDRESS_NONE) return FALSE;

	return WriteAddressValue(pAddr->m_nAllAutoWsiGoAddr, (long)1);
}


	
BOOL CMotorControl::RemoveAllAutoGoCount(int nGantryIdx)
{	
	if (m_bConnected==FALSE) return FALSE;

	// count
	int nMoveCountAddr = (int)GetGantryMeasureMoveCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
	if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
	{
		return FALSE;
	}	

	nMoveCountAddr = (int)GetGantryMoveCountAddr(nGantryIdx); 
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
	if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
	{
		return FALSE;
	}	

	nMoveCountAddr = (int)GetGantryWsiMoveCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
	if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
	{
		return FALSE;
	}	

	//Trigger
	nMoveCountAddr = (int)GetGantryMeasureTriggerCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
	if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
	{
		return FALSE;
	}	

	nMoveCountAddr = (int)GetGantryTriggerCountAddr(nGantryIdx); 
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
	if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
	{
		return FALSE;
	}	

	nMoveCountAddr = (int)GetGantryWsiTriggerCountAddr(nGantryIdx);
	if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
	if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
	{
		return FALSE;
	}	

	return TRUE;
}

BOOL CMotorControl::StartThread()
{
	return CTimerThreadPools::StartThread();
}

void CMotorControl::StopThread()
{
	CTimerThreadPools::StopThread();
}

int CMotorControl::GetStatus( int& nStatusCode, CString& strStatusMessage )
{
	if (GetConnected()==FALSE) 
	{
		nStatusCode = MotorStatus_NotConnected;
		strStatusMessage = _T("Not_Connected");
		return 0;
	}

	nStatusCode = MotorStatus_Connected;
	strStatusMessage = _T("Connected");

	return 1;

}