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

#include "PowerPMAC/PowerPmac.h"
#include "PowerPMAC/PowerPmacDef.h"

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

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

	Disconnect();
}


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

	// set motor control info
	m_ControlInfo = *pControlInfo;

	DWORD dwIPAddr = 0;

	TransformIP_CStringToDWORD(m_ControlInfo.GetConnectionPort(), dwIPAddr);

	int nPort = 0;

	// Power PMAC Open
	m_uDeviceID	= DTKPowerPmacOpen(dwIPAddr, nPort);

	m_bConnected = FALSE;

	// Device Connect
	if (DTKConnect(m_uDeviceID) != DS_Ok)
	{
		return 0;
	}

	// 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_PowerPmacCmd::Disconnect()
{
	if (m_bConnected==FALSE) return;

	// Power PMAC Close
	DTKDisconnect(m_uDeviceID);

	DTKPowerPmacClose(m_uDeviceID);

	m_bConnected = FALSE;
}

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

	TCHAR pchResponse[256];
	CString str;
	str.Format(_T("sys.idata[%d]"), nAddress);
	UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);

	CString strResponse;
	strResponse.Format(_T("%s"), pchResponse);

	if (nResult == DS_Ok)
	{
		if (strResponse.GetLength()>0)
		{
			strResponse=strResponse.Mid(strResponse.Find(_T("="))+1);
			nValue = _ttof(strResponse);
		}		
		return TRUE;
	}

	return FALSE;
}

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

	TCHAR pchResponse[256];

	CString	str;
	str.Format(_T("sys.idata[%d]=%d"), nAddress, nValue);
	UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);

	if (nResult == DS_Ok)
		return TRUE;

	return FALSE;
}

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

	TCHAR pchResponse[256];
	CString str;
	str.Format(_T("sys.fdata[%d]"), nAddress);
	UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);

	CString strResponse;
	strResponse.Format(_T("%s"), pchResponse);

	if (nResult == DS_Ok)
	{
		if (strResponse.GetLength()>0)
		{
			strResponse=strResponse.Mid(strResponse.Find(_T("="))+1);
			fValue = (float)_ttof(strResponse);
		}		
		return TRUE;
	}

	return FALSE;
}

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

	TCHAR pchResponse[256];

	CString	str;
	str.Format(_T("sys.fdata[%d]=%f"), nAddress, fValue);
	UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);

	if (nResult == DS_Ok)
		return TRUE;

	return FALSE;
}

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

	BOOL bSuccess = TRUE;
	TCHAR pchResponse[256];

	CString str;
	str.Format(_T("sys.fdata[%d]"), nAddress);
	UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);

	CString strResponse;
	strResponse.Format(_T("%s"), pchResponse);

	if (nResult == DS_Ok)
	{
		if (strResponse.GetLength()>0)
		{
			strResponse=strResponse.Mid(strResponse.Find(_T("="))+1);
			dValue = _ttof(strResponse);
		}		
		return TRUE;
	}

	return FALSE;
}

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

	TCHAR pchResponse[256];

	CString	str;
	str.Format(_T("sys.fdata[%d]=%lf"), nAddress, dValue);
	UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);

	if (nResult == DS_Ok)
		return TRUE;

	return FALSE;
}

BOOL CMotorControl_PowerPmacCmd::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_PowerPmacCmd::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_PowerPmacCmd::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_PowerPmacCmd::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_PowerPmacCmd::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;
}