#include "StdAfx.h"
#include "CHMotorControls/MotorControl_PowerPmac.h"
#include "PPmacDprRuntime.h"

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

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

	Disconnect();
}

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

	// set motor control info
	m_ControlInfo = *pControlInfo;

	ClosePPmacDprRuntimeLink();

	if (OpenPPmacDprRuntimeLink()==NULL)
	{
		return 0;
	}

	PPmacDprDisconnect(m_uDeviceID);
	PPmacDprClose(m_uDeviceID);

	m_bConnected = FALSE;


	DWORD dwipAddress = 0;

	TransformIP_CStringToDWORD(m_ControlInfo.GetConnectionPort(), dwipAddress);

	int nPortNo = 1025;

	m_uDeviceID = PPmacDprOpen(dwipAddress, nPortNo);
	if ( PPmacDprConnect(m_uDeviceID)!=PS_Ok )
	{
		m_bConnected = FALSE;
		return -1;
	}

	m_bConnected = TRUE;

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

	m_bConnected = TRUE;

	return 1;
}

void CMotorControl_PowerPmac::Disconnect()
{
	if (m_bConnected)
	{
		PPmacDprDisconnect(m_uDeviceID);

		PPmacDprClose(m_uDeviceID);

		m_bConnected = FALSE;
	}

	ClosePPmacDprRuntimeLink();
}

inline void CMotorControl_PowerPmac::SwapBytes(int nSizeType, void *pSource, int nSourceSize)
{
	if (nSizeType==0 || (nSizeType%2)!=0) return;

	unsigned char temp;
	unsigned char* pSrcData = (unsigned char*)pSource;
	unsigned char* pSrc = NULL;
	unsigned char* pDst = NULL;

	int nLoopCount = nSizeType / 2;
	int nJumpSize = nSizeType - 1;

	for (int i=0; i<nSourceSize; i++)
	{		
		pSrc = pSrcData;
		pDst = pSrc + nJumpSize;
		for (int j=0; j<nLoopCount; j++)
		{
			// swap data
			temp = *pSrc;
			*pSrc = *pDst;
			*pDst = temp;

			pSrc++;
			pDst--;
		}
		pSrcData += nSizeType;
	}
}

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

	float fValue = 0.0f;
	if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float), PVOID(&fValue))!=PS_Ok)
	{
		return FALSE;
	}

	// swap
	SwapBytes(sizeof(float), &fValue, 1);

	dValue = fValue;

	return TRUE;
}

BOOL CMotorControl_PowerPmac::ReadAddressValue(int nAddress, long &lValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;

	lValue = 0;
	if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(long), PVOID(&lValue))!=PS_Ok)
	{
		return FALSE;
	}

	// swap
	SwapBytes(sizeof(long), &lValue, 1);

	return TRUE;
}

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

	float fValue = float(dValue);

	// swap
	SwapBytes(sizeof(float), &fValue, 1);

	if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float), PVOID(&fValue))!=PS_Ok)
	{
		return FALSE;
	}

	return TRUE;
}

BOOL CMotorControl_PowerPmac::WriteAddressValue(int nAddress, long lValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;

	// swap
	SwapBytes(sizeof(long), &lValue, 1);

	if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(long), PVOID(&lValue))!=PS_Ok)
	{
		return FALSE;
	}

	return TRUE;
}


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

	if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float), PVOID(&fValue))!=PS_Ok)
	{
		return FALSE;
	}

	// swap
	SwapBytes(sizeof(float), &fValue, 1);

	return TRUE;
}

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

	// swap
	SwapBytes(sizeof(float), &fValue, 1);

	if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float), PVOID(&fValue))!=PS_Ok)
	{
		return FALSE;
	}

	return TRUE;
}


BOOL CMotorControl_PowerPmac::ReadAddressValue(long nAddress, long *pArrayData, int nArrayCount)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;

	if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(long)*nArrayCount, PVOID(pArrayData))!=PS_Ok)
	{
		return FALSE;
	}

	// swap
	SwapBytes(sizeof(long), pArrayData, nArrayCount);

	return TRUE;
}

BOOL CMotorControl_PowerPmac::WriteAddressValue(long nAddress, long *pArrayData, int nArrayCount)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;

	// swap
	SwapBytes(sizeof(long), pArrayData, nArrayCount);

	if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(long)*nArrayCount, PVOID(pArrayData))!=PS_Ok)
	{
		return FALSE;
	}

	return TRUE;
}

BOOL CMotorControl_PowerPmac::ReadAddressValue(long nAddress, float *pArrayData, int nArrayCount)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;

	if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float)*nArrayCount, PVOID(pArrayData))!=PS_Ok)
	{
		return FALSE;
	}

	// swap
	SwapBytes(sizeof(float), pArrayData, nArrayCount);

	return TRUE;
}

BOOL CMotorControl_PowerPmac::WriteAddressValue(long nAddress, float *pArrayData, int nArrayCount)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;

	// swap
	SwapBytes(sizeof(float), pArrayData, nArrayCount);

	if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float)*nArrayCount, PVOID(pArrayData))!=PS_Ok)
	{
		return FALSE;
	}

	return TRUE;
}

int CMotorControl_PowerPmac::GetStatus( int& nStatusCode, CString& strStatusMessage )
{
	if (PPmacDprIsConnected(m_uDeviceID)!=PS_Ok)	// Å×½ºÆ® ÇÊ¿ä
	{
		nStatusCode = MotorStatus_NotConnected;
		strStatusMessage = _T("Not_Connected");
		return 0;
	}

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

	return 1;
}