#include "StdAfx.h"
#include "CHMotorControls/MotorControl_Simulation.h"


CMotorControl_Simulation::CMotorControl_Simulation( int nIndex, DWORD dwPeriod/*=100*/,int nThreadCount /*= 1*/ ) : CMotorControl(nIndex, dwPeriod, nThreadCount)
{
	m_pMotionInfo_Axis	= NULL;
	m_pCurAxisPosition	= NULL;
	m_pCurAxisVelocity	= NULL;
	m_pCurAxisStatus	= NULL;

	m_MotionInfo_ThetaAxis.SetIndex(THETAMOTOR_AXIS_INDEX);
	m_lCurThetaAxisStatus		= 1;
	m_fCurThetaAxisPosition		= 0.;
	m_fCurThetaAxisVelocity		= 0.;
}

CMotorControl_Simulation::~CMotorControl_Simulation(void)
{
	if (m_pCurAxisStatus)
	{
		delete [] m_pCurAxisStatus;		
		m_pCurAxisStatus = NULL;
	}

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

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

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

int CMotorControl_Simulation::Connect( const CMotorControlInfo* pControlInfo )
{
	if (pControlInfo==NULL) return 0;

	// set motor control info
	m_ControlInfo = *pControlInfo;

	// alloc 
	m_nAxisCount = 0;
	for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
	{
		CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
		if (pGNode==NULL) continue;
		m_nAxisCount += pGNode->GetMotorAxisAddrCount();
	}

	if (m_nAxisCount<1) 
	{
		return -2;
	}

	if (m_pAxisStatus) delete  [] m_pAxisStatus;
	m_pAxisStatus = new long[m_nAxisCount];
	memset(m_pAxisStatus, 0, sizeof(long)*m_nAxisCount);

	if (m_pAxisPosition) delete  [] m_pAxisPosition;
	m_pAxisPosition = new float[m_nAxisCount];
	memset(m_pAxisPosition, -1, sizeof(float)*m_nAxisCount);

	if (m_pAxisStatusPrev) delete  [] m_pAxisStatusPrev;
	m_pAxisStatusPrev = new long[m_nAxisCount];
	memset(m_pAxisStatusPrev, 0, sizeof(long)*m_nAxisCount);

	if (m_pAxisPositionPrev) delete  [] m_pAxisPositionPrev;
	m_pAxisPositionPrev = new float[m_nAxisCount];
	memset(m_pAxisPositionPrev, -1, sizeof(float)*m_nAxisCount);

	if (m_pAxisStatusAddr) delete  [] m_pAxisStatusAddr;
	m_pAxisStatusAddr = new long[m_nAxisCount];

	if (m_pAxisPositionAddr) delete  [] m_pAxisPositionAddr;
	m_pAxisPositionAddr = new long[m_nAxisCount];

	int nAxisIndex = 0;
	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;

			m_pAxisPositionAddr[nAxisIndex] = pANode->m_nPositionAddr;
			m_pAxisStatusAddr[nAxisIndex] = pANode->m_nStatusAddr;
			nAxisIndex++;
		}
	}

	m_axisThetaStatus			= 0;
	m_axisThetaPosition			= 0.f;
	m_axisThetaStatusPrev		= 0;
	m_axisThetaPositionPrev		= -999.f;

	// new
	if (m_pMotionInfo_Axis) delete [] m_pMotionInfo_Axis;
	m_pMotionInfo_Axis = new CMotionInfo_Axis[m_nAxisCount];

	if (m_pCurAxisPosition) delete [] m_pCurAxisPosition;
	m_pCurAxisPosition = new float[m_nAxisCount];
	memset(m_pCurAxisPosition, 0, sizeof(float)*m_nAxisCount);

	if (m_pCurAxisVelocity) delete [] m_pCurAxisVelocity;
	m_pCurAxisVelocity = new float[m_nAxisCount];
	memset(m_pCurAxisVelocity, 0, sizeof(float)*m_nAxisCount);

	if (m_pCurAxisStatus) delete [] m_pCurAxisStatus;
	m_pCurAxisStatus = new long[m_nAxisCount];
	memset(m_pCurAxisStatus, 0, sizeof(long)*m_nAxisCount);

	for (int nAxisIndex=0; nAxisIndex<m_nAxisCount; nAxisIndex++)
	{
		m_pMotionInfo_Axis[nAxisIndex].SetIndex(nAxisIndex);
		m_pMotionInfo_Axis[nAxisIndex].SetMoveData(500, 0.1, 0.1, 0, 100);
		m_pMotionInfo_Axis[nAxisIndex].SetJogData(10, 0.1, 0.1, 0);

		m_pCurAxisStatus[nAxisIndex] = 1; // not moving
	}


	if(m_ControlInfo.GetUseThetaMotor() == TRUE)
	{
		m_MotionInfo_ThetaAxis.SetMoveData(1,0.1,0.1,0,360);
		m_MotionInfo_ThetaAxis.SetJogData(0.1, 0.1, 0.1, 0);
		m_lCurThetaAxisStatus		= 1;
		m_fCurThetaAxisPosition		= 0;
		m_fCurThetaAxisVelocity		= 0;
	}

	m_bConnected = TRUE;

	return 1;
}

void CMotorControl_Simulation::Disconnect()
{
	if (m_bConnected)
	{
		m_bConnected = FALSE;
	}
}

BOOL CMotorControl_Simulation::ReadAddressValue( int nAddress, long &nValue )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::ReadAddressValue( int nAddress, float &fValue )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::ReadAddressValue( int nAddress, double &dValue )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::ReadAddressValue( long nAddress, long *pArrayData, int nArrayCount )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::ReadAddressValue( long nAddress, float *pArrayData, int nArrayCount )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::WriteAddressValue( int nAddress, long nValue )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::WriteAddressValue( int nAddress, float fValue )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::WriteAddressValue( int nAddress, double dValue )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::WriteAddressValue( long nAddress, long *pArrayData, int nArrayCount )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::WriteAddressValue( long nAddress, float *pArrayData, int nArrayCount )
{
	return TRUE;
}

BOOL CMotorControl_Simulation::CommonSetAutoStop()
{
	return TRUE;
}

BOOL CMotorControl_Simulation::AxisManualGo( int nAxisIdx, double dPosition )
{
	if (nAxisIdx<0 || nAxisIdx>=m_nAxisCount) return FALSE;

	if (m_pMotionThread==NULL) return FALSE;

	CMotionData data;
	data.nIndex				= nAxisIdx;
	data.nType				= ThreadDataType_AxisManualGo;
	data.uData[0].dValue	= (double)m_pCurAxisPosition[nAxisIdx];	// current pos
	data.uData[1].dValue	= dPosition;							// target pos

	return m_pMotionThread->AddThreadData(data);
}

BOOL CMotorControl_Simulation::AxisJogPlus( int nAxisIdx )
{
	if (nAxisIdx<0 || nAxisIdx>=m_nAxisCount) return FALSE;

	if (m_pMotionThread==NULL) return FALSE;

	CMotionData data;
	data.nIndex				= nAxisIdx;
	data.nType				= ThreadDataType_JogGo;
	data.uData[0].dValue	= (double)m_pCurAxisPosition[nAxisIdx];	// current pos
	data.uData[1].nValue	= 1; // jog plus

	return m_pMotionThread->AddThreadData(data);
}

BOOL CMotorControl_Simulation::AxisJogMinus( int nAxisIdx )
{
	if (nAxisIdx<0 || nAxisIdx>=m_nAxisCount) return FALSE;

	if (m_pMotionThread==NULL) return FALSE;

	CMotionData data;
	data.nIndex				= nAxisIdx;
	data.nType				= ThreadDataType_JogGo;
	data.uData[0].dValue	= (double)m_pCurAxisPosition[nAxisIdx];	// current pos
	data.uData[1].nValue	= -1; // jog minus

	return m_pMotionThread->AddThreadData(data);
}

BOOL CMotorControl_Simulation::AxisJogStop( int nAxisIdx )
{
	CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(nAxisIdx);
	if (pMotionAxis==NULL) return FALSE;

	if (pMotionAxis->GetCurStatus()==0) // moving?
	{
		pMotionAxis->SetEmergencyStop(1); // stop command
		TRACE(_T("%d Axis Stop!\n"), nAxisIdx);
	}
	return TRUE;
}

BOOL CMotorControl_Simulation::AxisManualGoSpeed( int nAxisIdx, double dSpeed )
{
	CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(nAxisIdx);
	if (pMotionAxis==NULL) return FALSE;
	pMotionAxis->SetMoveVelocity(dSpeed);
	return TRUE;
}

BOOL CMotorControl_Simulation::AxisJogSpeed( int nAxisIdx, double dSpeed )
{
	CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(nAxisIdx);
	if (pMotionAxis==NULL) return FALSE;
	pMotionAxis->SetJogVelocity(dSpeed);
	return TRUE;
}

BOOL CMotorControl_Simulation::AxisManualGoAccel( int nAxisIdx, int nAccel )
{
	CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(nAxisIdx);
	if (pMotionAxis==NULL) return FALSE;
	pMotionAxis->SetMoveAccelTime(double(nAccel) / 1000.);
	pMotionAxis->SetMoveDecelTime(double(nAccel) / 1000.);
	return TRUE;
}

BOOL CMotorControl_Simulation::GantryManualGo( int nGantryIdx, const VectorDouble& vectorPos )
{
	if (m_pMotionThread==NULL) return FALSE;

	// axis count check
	int nAxisCount = GetGantryAxisCount(nGantryIdx);
	if (nAxisCount < (int)vectorPos.size()) return FALSE;

	// axis index check
	int nAxisIdx = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisIdx += GetGantryAxisCount(i);
	}

	CMotionData data;
	data.nIndex				= nGantryIdx;
	data.nType				= ThreadDataType_GantryGo;

	int nTotalIdx = 0;
	data.uData[nTotalIdx++].nValue	= (int)vectorPos.size(); // axis count

	for (int nIdx=0; nIdx<(int)vectorPos.size(); nIdx++)
	{
		// axis index
		data.uData[nTotalIdx++].nValue = nAxisIdx;

		// axis cur pos
		data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];

		// axis target pos
		data.uData[nTotalIdx++].dValue = vectorPos[nIdx];

		nAxisIdx++;
	}

	return m_pMotionThread->AddThreadData(data);
}

BOOL CMotorControl_Simulation::GantryManualGo( int nGantryIdx, const VectorDouble& vectorPos, int nMoveType )
{
	if (m_pMotionThread==NULL) return FALSE;

	// axis count check
	int nAxisCount = GetGantryAxisCount(nGantryIdx);
	if (nAxisCount < (int)vectorPos.size()) return FALSE;

	// axis index check
	int nAxisIdx = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisIdx += GetGantryAxisCount(i);
	}

	CMotionData data;
	data.nIndex				= nGantryIdx;
	data.nType				= ThreadDataType_GantryGo;

	int nTotalIdx = 0;
	data.uData[nTotalIdx++].nValue	= (int)vectorPos.size(); // axis count

	for (int nIdx=0; nIdx<(int)vectorPos.size(); nIdx++)
	{
		// axis index
		data.uData[nTotalIdx++].nValue = nAxisIdx;

		// axis cur pos
		data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];

		// axis target pos
		data.uData[nTotalIdx++].dValue = vectorPos[nIdx];

		nAxisIdx++;
	}

	return m_pMotionThread->AddThreadData(data);
}

BOOL CMotorControl_Simulation::GantryManualGo( int nGantryIdx, double dPosX, double dPosY )
{
	if (m_pMotionThread==NULL) return FALSE;

	// axis count check
	int nAxisCount = GetGantryAxisCount(nGantryIdx);
	if (nAxisCount < 2) return FALSE;

	// axis index check
	int nAxisIdx = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisIdx += GetGantryAxisCount(i);
	}

	CMotionData data;
	data.nIndex				= nGantryIdx;
	data.nType				= ThreadDataType_GantryGo;

	int nTotalIdx = 0;
	data.uData[nTotalIdx++].nValue	= 2; // axis count

	// axis index
	data.uData[nTotalIdx++].nValue = nAxisIdx;

	// axis cur pos
	data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];

	// axis target pos
	data.uData[nTotalIdx++].dValue = dPosX;

	nAxisIdx++;

	// axis index
	data.uData[nTotalIdx++].nValue = nAxisIdx;

	// axis cur pos
	data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];

	// axis target pos
	data.uData[nTotalIdx++].dValue = dPosY;

	return m_pMotionThread->AddThreadData(data);
}

BOOL CMotorControl_Simulation::GantryAutoGo( int nGantryIdx, const VectorDouble& vectorPosX, const VectorDouble& vectorPosY, BOOL bCommand/*=FALSE*/ )
{
	if (m_pMotionThread==NULL) return FALSE;

	// axis count check
	int nAxisCount = GetGantryAxisCount(nGantryIdx);
	if (nAxisCount < 2) return FALSE;

	// axis index check
	int nAxisIdx = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisIdx += GetGantryAxisCount(i);
	}

	CMotionData data;
	data.nIndex				= nGantryIdx;
	data.nType				= ThreadDataType_AutoGo;

	int nTotalIdx = 0;
	data.uData[nTotalIdx++].nValue	= 2; // axis count

	// axis index
	data.uData[nTotalIdx++].nValue = nAxisIdx;

	// axis cur pos
	data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];

	// axis target pos
	data.uData[nTotalIdx++].dValue = vectorPosX[0];
	data.vecDoubleX = vectorPosX;

	nAxisIdx++;

	// axis index
	data.uData[nTotalIdx++].nValue = nAxisIdx;

	// axis cur pos
	data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];

	// axis target pos
	data.uData[nTotalIdx++].dValue = vectorPosY[0];
	data.vecDoubleY = vectorPosY;

	return m_pMotionThread->AddThreadData(data);

	//////////////////////////////////////////////////////////////////////////////////////////////

	return TRUE;
}

void CMotorControl_Simulation::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;

	case ThreadDataType_AxisManualGo:
		Process_AxisManualGo(motionData);
		break;

	case ThreadDataType_JogGo :
		Process_JogGo(motionData);
		break;

	case ThreadDataType_GantryGo :
		Process_GantryGo(motionData);
		break;

	case ThreadDataType_AutoGo :
		Process_AutoGo(motionData);
		break;
	}
}

void CMotorControl_Simulation::Process_AxisManualGo( const CMotionData& motionData )
{
	CMotionInfo_Axis *pMotionAxis	= GetMotionInfo_Axis(motionData.nIndex);
	float *pCurPos					= GetCurAxisPosition(motionData.nIndex);
	float *pCurVel					= GetCurAxisVelocity(motionData.nIndex);
	long *pCurStatus				= GetCurAxisStatus(motionData.nIndex);

	if (pMotionAxis==NULL || pCurPos==NULL || pCurVel==NULL || pCurStatus==NULL) return;

	pMotionAxis->SetMoveStartPos(motionData.uData[0].dValue); // start pos
	pMotionAxis->SetMoveEndPos(motionData.uData[1].dValue); // end pos
	DWORD dwPeriod = m_dwPeriod / 2;
	double dTimeSec = 0.0;
	double dPos, dVel;
	UINT nStatus;

	DWORD dwStartTick = GetTickCount();
	do 
	{
		::Sleep(dwPeriod);

		// cal time
		dTimeSec = double(GetTickCount() - dwStartTick) / 1000.0;

		// cal motion
		if (pMotionAxis->CalculateMoveMotion(dTimeSec, dPos, dVel, nStatus))
		{
			*pCurPos	= (float)dPos;
			*pCurVel	= (float)dVel;
			*pCurStatus	= (long)nStatus;
		}

		// check emo
		if (pMotionAxis->GetEmergencyStop())
		{
			pMotionAxis->SetEmergencyStop(0);
			pMotionAxis->SetCurStatus(1);
			//AfxMessageBox(_T("stop"));
			break;
		}

	} while (nStatus!=1);


}

void CMotorControl_Simulation::Process_JogGo( const CMotionData& motionData )
{
	CMotionInfo_Axis *pMotionAxis	= GetMotionInfo_Axis(motionData.nIndex);
	float *pCurPos					= GetCurAxisPosition(motionData.nIndex);
	float *pCurVel					= GetCurAxisVelocity(motionData.nIndex);
	long *pCurStatus				= GetCurAxisStatus(motionData.nIndex);

	if (pMotionAxis==NULL || pCurPos==NULL || pCurVel==NULL || pCurStatus==NULL) return;

	pMotionAxis->SetJogStartPos(motionData.uData[0].dValue); // start pos
	int nJogCmd = motionData.uData[1].nValue;
	DWORD dwPeriod = m_dwPeriod / 2;
	double dTimeSec = 0.0;
	double dPos, dVel;
	UINT nStatus;

	DWORD dwStartTick = GetTickCount();
	do 
	{
		::Sleep(dwPeriod);

		// cal time
		dTimeSec = double(GetTickCount() - dwStartTick) / 1000.0;

		// cal motion
		if (pMotionAxis->CalculateJogMotion(dTimeSec, nJogCmd, dPos, dVel, nStatus))
		{
			*pCurPos	= (float)dPos;
			*pCurVel	= (float)dVel;
			*pCurStatus	= (long)nStatus;
		}

		// check emo
		if (pMotionAxis->GetEmergencyStop())
		{
			pMotionAxis->SetEmergencyStop(0);
			pMotionAxis->SetCurStatus(1);
			//AfxMessageBox(_T("stop"));
			break;
		}

	} while (nStatus!=1);

}

void CMotorControl_Simulation::Process_GantryGo( const CMotionData& motionData )
{
	int nTotalIdx = 0;
	int nGantryIdx = motionData.nIndex;
	int nAxisCount = motionData.uData[nTotalIdx++].nValue;


	CMotionInfo_Axis **pMotionAxis	= new CMotionInfo_Axis*[nAxisCount];
	float **pCurPos					= new float*[nAxisCount];
	float **pCurVel					= new float*[nAxisCount];
	long **pCurStatus				= new long*[nAxisCount];

	for (int nIdx=0; nIdx<nAxisCount; nIdx++)
	{
		pMotionAxis[nIdx]	= GetMotionInfo_Axis(motionData.uData[nTotalIdx].nValue);
		pCurPos[nIdx]		= GetCurAxisPosition(motionData.uData[nTotalIdx].nValue);
		pCurVel[nIdx]		= GetCurAxisVelocity(motionData.uData[nTotalIdx].nValue);
		pCurStatus[nIdx]	= GetCurAxisStatus(motionData.uData[nTotalIdx].nValue);
		nTotalIdx++;

		if (pMotionAxis[nIdx]==NULL || pCurPos[nIdx]==NULL || pCurVel[nIdx]==NULL || pCurStatus[nIdx]==NULL) continue;

		pMotionAxis[nIdx]->SetMoveStartPos(motionData.uData[nTotalIdx++].dValue); // start pos
		pMotionAxis[nIdx]->SetMoveEndPos(motionData.uData[nTotalIdx++].dValue); // end pos
	}

	DWORD dwPeriod = m_dwPeriod / 2;
	double dTimeSec = 0.0;
	double dPos, dVel;
	UINT nStatus;
	UINT nTotalStatus;

	DWORD dwStartTick = GetTickCount();
	do 
	{
		::Sleep(dwPeriod);

		// cal time
		dTimeSec = double(GetTickCount() - dwStartTick) / 1000.0;

		nTotalStatus = 1;
		for (int nIdx=0; nIdx<nAxisCount; nIdx++)
		{
			// cal motion
			if (pMotionAxis[nIdx]->CalculateMoveMotion(dTimeSec, dPos, dVel, nStatus))
			{
				*pCurPos[nIdx]		= (float)dPos;
				*pCurVel[nIdx]		= (float)dVel;
				*pCurStatus[nIdx]	= (long)nStatus;

				nTotalStatus = nTotalStatus * nStatus;
			}

			// check emo
			if (pMotionAxis[nIdx]->GetEmergencyStop())
			{
				pMotionAxis[nIdx]->SetEmergencyStop(0);
				pMotionAxis[nIdx]->SetCurStatus(1);
				nTotalStatus = 1;
				break;
			}
		}

	} while (nTotalStatus!=1);

	delete [] pMotionAxis;
	delete [] pCurPos;
	delete [] pCurVel;
	delete [] pCurStatus;

}

void CMotorControl_Simulation::Process_AutoGo( const CMotionData& motionData )
{

	// Auto Go Param
	const CMotorControlInfo *pMotorContorlInfo = GetControlInfo();
	if(pMotorContorlInfo == NULL) return;

	DWORD dwAFTime		= pMotorContorlInfo->GetAfDelayTime();		// AF Delay
	DWORD dwSnapTime	= pMotorContorlInfo->GetImageDelayTime(); 	// Snap Delay

	// Gantry Go Param
	int nTotalIdx = 0;
	int nGantryIdx = motionData.nIndex;
	int nAxisCount = motionData.uData[nTotalIdx++].nValue;

	DWORD dwPeriod = m_dwPeriod / 2;
	double dTimeSec = 0.0;
	double dPos, dVel;
	UINT nStatus;
	UINT nTotalStatus;

	CMotionInfo_Axis **pMotionAxis	= new CMotionInfo_Axis*[nAxisCount];
	float **pCurPos					= new float*[nAxisCount];
	float **pCurVel					= new float*[nAxisCount];
	long **pCurStatus				= new long*[nAxisCount];

	// Get Current Axis Info
	for (int nIdx=0; nIdx<nAxisCount; nIdx++)
	{
		pMotionAxis[nIdx]	= GetMotionInfo_Axis(motionData.uData[nTotalIdx].nValue);
		pCurPos[nIdx]		= GetCurAxisPosition(motionData.uData[nTotalIdx].nValue);
		pCurVel[nIdx]		= GetCurAxisVelocity(motionData.uData[nTotalIdx].nValue);
		pCurStatus[nIdx]	= GetCurAxisStatus(motionData.uData[nTotalIdx].nValue);
		nTotalIdx+= 3;
	}

	for(int nPosIdx = 0; nPosIdx < (int)motionData.vecDoubleX.size(); nPosIdx++)
	{
		nTotalIdx = 1;

		for (int nIdx=0; nIdx<nAxisCount; nIdx++)
		{
			nTotalIdx++;
			if (pMotionAxis[nIdx]==NULL || pCurPos[nIdx]==NULL || pCurVel[nIdx]==NULL || pCurStatus[nIdx]==NULL) continue;

			if(nPosIdx == 0) // 1st Position
			{
				pMotionAxis[nIdx]->SetMoveStartPos(motionData.uData[nTotalIdx++].dValue); // start pos
				pMotionAxis[nIdx]->SetMoveEndPos(motionData.uData[nTotalIdx++].dValue); // end pos
			}
			else			// N-th Position
			{
				if(nIdx == 0)	// Set X Axis
				{
					pMotionAxis[nIdx]->SetMoveStartPos(motionData.vecDoubleX[nPosIdx-1]);	// start pos
					pMotionAxis[nIdx]->SetMoveEndPos(motionData.vecDoubleX[nPosIdx]);		// end pos
				}
				else			// Set Y Axis
				{
					pMotionAxis[nIdx]->SetMoveStartPos(motionData.vecDoubleY[nPosIdx-1]);	// start pos
					pMotionAxis[nIdx]->SetMoveEndPos(motionData.vecDoubleY[nPosIdx]);		// end pos
				}
			}
		}

		dTimeSec = 0.0;

		DWORD dwStartTick = GetTickCount();
		do 
		{
			::Sleep(dwPeriod);

			// cal time
			dTimeSec = double(GetTickCount() - dwStartTick) / 1000.0;

			nTotalStatus = 1;
			for (int nIdx=0; nIdx<nAxisCount; nIdx++)
			{
				// cal motion
				if (pMotionAxis[nIdx]->CalculateMoveMotion(dTimeSec, dPos, dVel, nStatus))
				{
					*pCurPos[nIdx]		= (float)dPos;
					*pCurVel[nIdx]		= (float)dVel;
					*pCurStatus[nIdx]	= (long)nStatus;

					nTotalStatus = nTotalStatus * nStatus;
				}

				// check emo
				if (pMotionAxis[nIdx]->GetEmergencyStop())
				{
					pMotionAxis[nIdx]->SetEmergencyStop(0);
					pMotionAxis[nIdx]->SetCurStatus(1);
					nTotalStatus = 1;
					break;
				}
			}

		} while (nTotalStatus!=1);

		Sleep(dwAFTime);		// AF Delay

		m_pMC2P->IMC2P_GantrySoftWareTrigger(m_nIndex,nGantryIdx,nPosIdx); // Software Trigger

		Sleep(dwSnapTime);		// Snap Delay

	}

	delete [] pMotionAxis;
	delete [] pCurPos;
	delete [] pCurVel;
	delete [] pCurStatus;
}

CMotionInfo_Axis* CMotorControl_Simulation::GetMotionInfo_Axis( int nAxisIndex )
{
	if(nAxisIndex == THETAMOTOR_AXIS_INDEX)
	{
		return &m_MotionInfo_ThetaAxis;
	}

	if (nAxisIndex<0 || nAxisIndex>=m_nAxisCount) return NULL;
	return &m_pMotionInfo_Axis[nAxisIndex];
}

float* CMotorControl_Simulation::GetCurAxisPosition( int nAxisIndex )
{
	if(nAxisIndex == THETAMOTOR_AXIS_INDEX)
	{
		return &m_fCurThetaAxisPosition;
	}
	if (nAxisIndex<0 || nAxisIndex>=m_nAxisCount) return NULL;
	return &m_pCurAxisPosition[nAxisIndex];
}

float* CMotorControl_Simulation::GetCurAxisVelocity( int nAxisIndex )
{
	if(nAxisIndex == THETAMOTOR_AXIS_INDEX)
	{
		return &m_fCurThetaAxisVelocity;
	}
	if (nAxisIndex<0 || nAxisIndex>=m_nAxisCount) return NULL;
	return &m_pCurAxisVelocity[nAxisIndex];
}

long* CMotorControl_Simulation::GetCurAxisStatus( int nAxisIndex )
{
	if(nAxisIndex == THETAMOTOR_AXIS_INDEX)
	{
		return &m_lCurThetaAxisStatus;
	}

	if (nAxisIndex<0 || nAxisIndex>=m_nAxisCount) return NULL;
	return &m_pCurAxisStatus[nAxisIndex];
}

BOOL CMotorControl_Simulation::ReadAxisPosition( float *pAxisPos, int nAxisCount )
{
	if (m_pCurAxisPosition==NULL || pAxisPos==NULL || (m_nAxisCount<nAxisCount)) return FALSE;
	memcpy(pAxisPos, m_pCurAxisPosition, sizeof(float)*nAxisCount);
	return TRUE;
}

BOOL CMotorControl_Simulation::ReadAxisStatus( long *pAxisStatus, int nAxisCount )
{
	if (m_pCurAxisStatus==NULL || pAxisStatus==NULL || (m_nAxisCount<nAxisCount)) return FALSE;
	memcpy(pAxisStatus, m_pCurAxisStatus, sizeof(long)*nAxisCount);
	return TRUE;
}

BOOL CMotorControl_Simulation::ReadThetaAxisPosition( float& fAxisPos )
{
	fAxisPos = m_fCurThetaAxisPosition;
	return TRUE;
}

BOOL CMotorControl_Simulation::ReadThetaAxisStatus( long& nAxisStatus )
{
	nAxisStatus = m_lCurThetaAxisStatus;
	return TRUE;
}

BOOL CMotorControl_Simulation::AxisThetaJogPlus()
{
	if (m_pMotionThread==NULL) return FALSE;

	CMotionData data;
	data.nIndex				= THETAMOTOR_AXIS_INDEX;
	data.nType				= ThreadDataType_JogGo;
	data.uData[0].dValue	= (double)m_fCurThetaAxisPosition;	// current pos
	data.uData[1].nValue	= 1; // jog plus

	return m_pMotionThread->AddThreadData(data);
}

BOOL CMotorControl_Simulation::AxisThetaJogMinus()
{
	if (m_pMotionThread==NULL) return FALSE;

	CMotionData data;
	data.nIndex				= THETAMOTOR_AXIS_INDEX;
	data.nType				= ThreadDataType_JogGo;
	data.uData[0].dValue	= (double)m_fCurThetaAxisPosition;	// current pos
	data.uData[1].nValue	= -1; // jog Minus

	return m_pMotionThread->AddThreadData(data);
}

BOOL CMotorControl_Simulation::AxisThetaManualGo( double dPosition )
{
	if (m_pMotionThread==NULL) return FALSE;

	CMotionData data;
	data.nIndex				= THETAMOTOR_AXIS_INDEX;
	data.nType				= ThreadDataType_AxisManualGo;
	data.uData[0].dValue	= (double)m_fCurThetaAxisPosition;	// current pos
	data.uData[1].dValue	= dPosition;							// target pos

	return m_pMotionThread->AddThreadData(data);
}

BOOL CMotorControl_Simulation::AxisThetaJogStop()
{
	CMotionInfo_Axis *pMotionAxis = &m_MotionInfo_ThetaAxis;
	if (pMotionAxis==NULL) return FALSE;

	if (pMotionAxis->GetCurStatus()==0) // moving?
	{
		pMotionAxis->SetEmergencyStop(1); // stop command
		TRACE(_T("Theta Axis Stop!\n"));

	}
	return TRUE;
}