#include "StdAfx.h"
#include "CHMotorControls/MotorControl_eXcom.h"
#include "eXcomDevice.h"

CMotorControl_eXcom::CMotorControl_eXcom(int nIndex, DWORD dwPeriod, int nThreadCount) : CMotorControl(nIndex, dwPeriod, nThreadCount)
{
	m_peXcomDevice	= new CeXcomDevice(nIndex);
}

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

	Disconnect();

	if (m_peXcomDevice) 
	{
		delete m_peXcomDevice;
		m_peXcomDevice = NULL;
	}

	m_bCheckConnect = FALSE;
}

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

	// set motor control info
	m_ControlInfo = *pControlInfo;

	if(m_peXcomDevice == NULL) return 0;

	int nPort;
	CString strIPAddress;
	DWORD dwIPAddress;

	nPort = 1500;
	strIPAddress = m_ControlInfo.GetConnectionPort(); 

	TransformIP_CStringToDWORD(strIPAddress, dwIPAddress);

	m_bConnected = FALSE;

	m_bConnected = m_peXcomDevice->Connect(dwIPAddress, nPort);

	if (m_bConnected==FALSE) 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 -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_eXcom::Disconnect()
{
	if(m_peXcomDevice == NULL) return;

	if (m_bConnected)
	{
		m_peXcomDevice->Close();
		m_bConnected = FALSE;
	}
}

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

	float fValue = 0.0f;
	if (!m_peXcomDevice->ReadOneData(nAddress, fValue))
	{
		return FALSE;
	}

	dValue = fValue;

	return TRUE;
}

BOOL CMotorControl_eXcom::ReadAddressValue(int nAddress, long &nValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;
	if (m_peXcomDevice==NULL) return FALSE;

	return m_peXcomDevice->ReadOneData(nAddress, nValue);	
}

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

	return m_peXcomDevice->ReadOneData(nAddress, fValue);
}

BOOL CMotorControl_eXcom::WriteAddressValue(int nAddress, float fValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;
	if (m_peXcomDevice==NULL) return FALSE;

	return m_peXcomDevice->WriteOneData(nAddress, fValue);
}

BOOL CMotorControl_eXcom::WriteAddressValue(int nAddress, double dValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;
	if (m_peXcomDevice==NULL) return FALSE;;

	float fValue = (float)dValue;
	return m_peXcomDevice->WriteOneData(nAddress, fValue);
}

BOOL CMotorControl_eXcom::WriteAddressValue(int nAddress, long nValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;
	if (m_peXcomDevice==NULL) return FALSE;

	return m_peXcomDevice->WriteOneData(nAddress, nValue);
}

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

	return m_peXcomDevice->ReadMultyData(nAddress, pArrayData, nArrayCount);
}

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

	return m_peXcomDevice->WriteMultyData(nAddress, pArrayData, nArrayCount);
}

// use read position func
BOOL CMotorControl_eXcom::ReadAddressValue(long nAddress, float *pArrayData, int nArrayCount)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;
	if (m_peXcomDevice==NULL) return FALSE;

	BOOL bRet = m_peXcomDevice->ReadMultyData(nAddress, pArrayData, nArrayCount);

	m_bCheckConnect = bRet;

	return bRet;
}

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

	return m_peXcomDevice->WriteMultyData(nAddress, pArrayData, nArrayCount);
}

int CMotorControl_eXcom::GetStatus( int& nStatusCode, CString& strStatusMessage )
{
	m_bCheckConnect = FALSE;

	Sleep(500);
	if (m_bCheckConnect==FALSE)		// Å×½ºÆ® ÇÊ¿ä
	{
		nStatusCode = MotorStatus_NotConnected;
		strStatusMessage = _T("Not_Connected");
		return 0;
	}

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

	return 1;
}