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

#include "Runtime.h"

CMotorControl_Umac::CMotorControl_Umac(int nIndex, DWORD dwPeriod, int nThreadCount) : CMotorControl(nIndex, dwPeriod, nThreadCount)
{
	m_uDeviceID = 0;
}

CMotorControl_Umac::~CMotorControl_Umac(void)
{
	//AfxMessageBox(_T("CMotorControl_Umac"));

	Disconnect();
}


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

	// set motor control info
	m_ControlInfo = *pControlInfo;

	OpenRuntimeLink();

	m_uDeviceID = _ttoi(m_ControlInfo.GetConnectionPort());

	// Power Umac Open
	m_bConnected = DeviceOpen(m_uDeviceID);

	// 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 0;
	}

	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 *ANode = pGNode->GetMotorAxisAddr(nA_Idx);
			if (ANode==NULL) continue;

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

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

	m_bConnected = TRUE;

	return 1;
}

void CMotorControl_Umac::Disconnect()
{
	if (m_bConnected==FALSE) return;

	DeviceClose(m_uDeviceID);

	m_bConnected = FALSE;
}

BOOL CMotorControl_Umac::GantryManualGo(int nGantryIdx, const VectorDouble& vectorPos, 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, vectorPos[i])==FALSE)
				{
					continue;
				}
			}
		}
		nBitData = nBitData << 1;
	}
	Sleep(700);
	BOOL bResult = WriteAddressValue(nManualAddr, (long)5);			// 3d review x=axis0, y=axis2 (1+4=5)

	return bResult;
}

BOOL CMotorControl_Umac::ReadAddressValue(int nAddress, long &nValue)
{
	if (!m_bConnected) return FALSE;

	INT		nRet;
	TCHAR	szResponse[255];
	CString	strBuffer=_T(""), strResponse=_T("");

	strBuffer.Format(_T("P%d"), nAddress);
	nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));

	strResponse = CA2T(szResponse);
	nValue = _ttoi(strResponse);

	return TRUE;
}

BOOL CMotorControl_Umac::WriteAddressValue(int nAddress, long nValue)
{
	if (!m_bConnected) return FALSE;	

	INT		nRet;
	CHAR	szResponse[255];
	CString	strBuffer=_T(""), strResponse=_T("");

	strBuffer.Format(_T("P%d=%d"), nAddress, nValue);
	nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));

	strResponse = CA2T(szResponse);

	return TRUE;
}

BOOL CMotorControl_Umac::ReadAddressValue(int nAddress, float &fValue)
{
	if (!m_bConnected) return FALSE;

	INT		nRet;
	CHAR	szResponse[255];
	CString	strBuffer=_T(""), strResponse=_T("");

	strBuffer.Format(_T("P%d"), nAddress);
	nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));

	strResponse = CA2T(szResponse);
	fValue = float(_ttof(strResponse));

	return TRUE;
}

BOOL CMotorControl_Umac::WriteAddressValue(int nAddress, float fValue)
{
	if (!m_bConnected) return FALSE;

	INT		nRet;
	CHAR	szResponse[255];
	CString	strBuffer=_T(""), strResponse=_T("");

	strBuffer.Format(_T("P%d=%f"), nAddress, fValue);
	nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));

	strResponse = CA2T(szResponse);

	return TRUE;
}

BOOL CMotorControl_Umac::ReadAddressValue(int nAddress, double &dValue)
{
	if (!m_bConnected) return FALSE;	

	INT		nRet;
	CHAR	szResponse[255];
	CString	strBuffer=_T(""), strResponse=_T("");

	strBuffer.Format(_T("P%d"), nAddress);
	nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));

	strResponse = CA2T(szResponse);
	dValue = _ttof(strResponse);
		

	return TRUE;
}

BOOL CMotorControl_Umac::WriteAddressValue(int nAddress, double dValue)
{
	if (!m_bConnected) return FALSE;

	INT		nRet;
	CHAR	szResponse[255];
	CString	strBuffer=_T(""), strResponse=_T("");

	strBuffer.Format(_T("P%d=%f"), nAddress, dValue);
	nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));

	strResponse = CA2T(szResponse);

	return TRUE;
}

BOOL CMotorControl_Umac::ReadAxisPosition(float *pAxisPos, int nAxisCount)
{
	BOOL bRet = FALSE;

	// read x position
	bRet = ReadAddressValue(m_pAxisPositionAddr[0], pAxisPos[0]);

	bRet = bRet | ReadAddressValue(m_pAxisPositionAddr[1], pAxisPos[1]);

	return bRet;
}

BOOL CMotorControl_Umac::ReadAddressValue(long nAddress, long *pArrayData, int nArrayCount)
{
	BOOL bResult = TRUE;
	for (int i=0; i<nArrayCount; i++)
	{
		bResult = bResult && ReadAddressValue(nAddress+i, pArrayData[i]);
	}
	return bResult;
}

BOOL CMotorControl_Umac::WriteAddressValue(long nAddress, long *pArrayData, int nArrayCount)
{
	BOOL bResult = TRUE;
	for (int i=0; i<nArrayCount; i++)
	{
		bResult = bResult && WriteAddressValue(nAddress+i, pArrayData[i]);
	}
	return bResult;
}

BOOL CMotorControl_Umac::ReadAddressValue(long nAddress, float *pArrayData, int nArrayCount)
{
	BOOL bResult = TRUE;
	for (int i=0; i<nArrayCount; i++)
	{
		bResult = bResult && ReadAddressValue(nAddress+i, pArrayData[i]);
	}
	return bResult;
}

BOOL CMotorControl_Umac::WriteAddressValue(long nAddress, float *pArrayData, int nArrayCount)
{
	BOOL bResult = TRUE;
	for (int i=0; i<nArrayCount; i++)
	{
		bResult = bResult && WriteAddressValue(nAddress+i, pArrayData[i]);
	}
	return bResult;
}

int CMotorControl_Umac::GetStatus( int& nStatusCode, CString& strStatusMessage )
{
	if (DeviceReadReady(m_uDeviceID)!=FALSE)		// È®ÀÎ ÇÊ¿ä
	{
		nStatusCode = MotorStatus_NotConnected;
		strStatusMessage = _T("Not_Connected");
		return 0;
	}

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

	return 1;
}