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

#define STR_SIZE_ADDRESS	10
#define STR_ERR_MESSAGE		100

#define MEMORY_MAX_SIZE		512

CMotorControl_Acs::CMotorControl_Acs(int nIndex, DWORD dwPeriod, int nThreadCount) : CMotorControl(nIndex, dwPeriod, nThreadCount)
{
	m_hAcsComm = ACSC_INVALID;
}

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

	Disconnect();
}

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

	// set motor control info
	m_ControlInfo = *pControlInfo;

	// initial handle
	m_hAcsComm = ACSC_INVALID;

	int nPort;
	char strIPAddress[50] = {0,};

	nPort = ACSC_SOCKET_STREAM_PORT;
	USES_CONVERSION;
	sprintf_s(strIPAddress, "%s", m_ControlInfo.GetConnectionPort());

	m_bConnected = FALSE;

	m_hAcsComm = acsc_OpenCommEthernetTCP(strIPAddress, nPort);
	if(m_hAcsComm == ACSC_INVALID) 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_Acs::Disconnect()
{
	if(m_hAcsComm != ACSC_INVALID)
	{
		if (acsc_CloseComm(m_hAcsComm))
		{
			m_hAcsComm = ACSC_INVALID; 
			m_bConnected = FALSE;
		}
	}
}

// gantry
BOOL CMotorControl_Acs::GantryManualGo( int nGantryIdx, const VectorDouble& vectorPos )
{
	UINT nManualAddr = GetGantryManualGoAddr(nGantryIdx);
	if (nManualAddr==MOTOR_ADDRESS_NONE) return FALSE;

	int nAxisCount = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisCount += GetGantryAxisCount(i);
	}

	long nValue = 0;
	for (int i=0; i<(int)vectorPos.size(); i++)
	{
		UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisCount+i);
		if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

		float dataPosition[2];
		ZeroMemory(dataPosition, sizeof(float)*2);

		dataPosition[0] = float(vectorPos[i]);
		if (WriteAddressValue(nGoPosAddr, dataPosition, 1)==FALSE)
		{
			return FALSE;
		}

		nValue = nValue | (1 << i);
	}

	return WriteAddressValue(nManualAddr, nValue);
}

BOOL CMotorControl_Acs::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) 
			{
				float dataPosition[2];
				ZeroMemory(dataPosition, sizeof(float)*2);

				dataPosition[0] = float(vectorPos[i]);
				if (WriteAddressValue(nGoPosAddr, dataPosition, 1)==FALSE)
				{
					continue;
				}
			}
		}
		nBitData = nBitData << 1;
	}
	Sleep(700);
	BOOL bResult = WriteAddressValue(nManualAddr, (long) nMoveType);

	//return WriteAddressValue(nManualAddr, (long) nMoveType);
	return bResult;
}

BOOL CMotorControl_Acs::GantryManualGo(int nGantryIdx, double dPosX, double dPosY)
{
	UINT nManualAddr = GetGantryManualGoAddr(nGantryIdx);
	if (nManualAddr==MOTOR_ADDRESS_NONE) return FALSE;

	int nAxisCount = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisCount += GetGantryAxisCount(i);
	}

	UINT nXGoPosAddr = GetAxisGoPositionAddr(nAxisCount);
	if (nXGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

	UINT nYGoPosAddr = GetAxisGoPositionAddr(nAxisCount+1);

	if (nYGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

	float dataPosition[2];
	ZeroMemory(dataPosition, sizeof(float)*2);

	dataPosition[0] = float(dPosX);
	if (WriteAddressValue(nXGoPosAddr, dataPosition, 1)==FALSE)
	{
		return FALSE;
	}

	dataPosition[0] = float(dPosY);
	if (WriteAddressValue(nYGoPosAddr, dataPosition, 1)==FALSE)
	{
		return FALSE;
	}

	return WriteAddressValue(nManualAddr, (long)3);
}

BOOL CMotorControl_Acs::AxisManualGo(int nAxisIdx, double dPosition)
{
	int nGantryIdx = GetAxisGantryIndex(nAxisIdx);

	if (nGantryIdx<0) return FALSE;

	UINT nManualGoAddr = GetGantryManualGoAddr(nGantryIdx);
	if (nManualGoAddr==MOTOR_ADDRESS_NONE) return FALSE;

	UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisIdx);
	if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;

	float dataPosition[2];
	ZeroMemory(dataPosition, sizeof(float)*2);

	dataPosition[0] = float(dPosition);
	if (WriteAddressValue(nGoPosAddr, dataPosition, 1)==FALSE)
	{
		return FALSE;
	}

	int nAxisCount = 0;
	for (int i=0; i<nGantryIdx; i++)
	{
		nAxisCount += GetGantryAxisCount(i);
	}

	// ÇØ´ç ºñÆ®¸¸ set
	long nValue = 1 << (nAxisIdx-nAxisCount);

	return WriteAddressValue(nManualGoAddr, nValue);
}

BOOL CMotorControl_Acs::ReadAxisPosition( float *pAxisPos, int nAxisCount )
{
	BOOL bRet = FALSE;
	bRet = ReadAddressValue(m_pAxisPositionAddr[0], pAxisPos[0]);
	bRet = bRet | ReadAddressValue(m_pAxisPositionAddr[1], pAxisPos[1]);

	return bRet;
}

BOOL CMotorControl_Acs::ReadAxisStatus( long *pAxisStatus, int nAxisCount )
{
	BOOL bRet = TRUE;

	for (int nIdx=0; nIdx<nAxisCount; nIdx++)
	{
		bRet = bRet & ReadAddressValue(m_pAxisStatusAddr[nIdx], pAxisStatus[nIdx]);
	}

	return bRet;
}

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

	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
	sprintf_s(strAddress, "D%d", nAddress);

	if (acsc_ReadReal(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dValue, NULL)==0) return FALSE;

	return TRUE;
}

BOOL CMotorControl_Acs::ReadAddressValue(int nAddress, long &nValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;
	if (m_hAcsComm==ACSC_INVALID) return FALSE;

	int dataValue = 0;
	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
	sprintf_s(strAddress, "D%d", nAddress);

	if (acsc_ReadInteger(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dataValue, NULL)==0) return FALSE;

	nValue = long(dataValue);

	return TRUE;	
}

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

	double dataValue = 0;
	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);

	sprintf_s(strAddress, "D%d", nAddress);

	if (acsc_ReadReal(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dataValue, NULL)==0) return FALSE;

	fValue = float(dataValue);

	return TRUE;
}

BOOL CMotorControl_Acs::WriteAddressValue(int nAddress, float fValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;
	if (m_hAcsComm==ACSC_INVALID) return FALSE;

	double dataValue = double(fValue);
	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
	sprintf_s(strAddress, "D%d", nAddress);

	if (acsc_WriteReal(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dataValue, NULL)==0) return FALSE;

	return TRUE;
}

BOOL CMotorControl_Acs::WriteAddressValue(int nAddress, double dValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;
	if (m_hAcsComm==ACSC_INVALID) return FALSE;

	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
	sprintf_s(strAddress, "D%d", nAddress);

	if (acsc_WriteReal(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dValue, NULL)==0) return FALSE;

	return TRUE;
}

BOOL CMotorControl_Acs::WriteAddressValue(int nAddress, long nValue)
{
	if (!m_bConnected) return FALSE;	
	if (nAddress<0) return FALSE;
	if (m_hAcsComm==ACSC_INVALID) return FALSE;

	int dataValue = nValue;
	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
	sprintf_s(strAddress, "D%d", nAddress);

	int nRet = acsc_WriteInteger(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dataValue, NULL);
	if (nRet==0) return FALSE;

	return TRUE;
}

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

	int pValue[MEMORY_MAX_SIZE];
	ZeroMemory(pValue, sizeof(int)*MEMORY_MAX_SIZE);
	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
	sprintf_s(strAddress, "D%d", nAddress);

	if (acsc_ReadInteger(m_hAcsComm, -1, strAddress, 0, nArrayCount, -1, -1, pValue, NULL)==0)
	{
		return FALSE;
	}

	for (int nIdx=0; nIdx<nArrayCount; nIdx++)
	{
		pArrayData[nIdx] = pValue[nIdx];
	}

	return TRUE;
}

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

	int pValue[MEMORY_MAX_SIZE];
	ZeroMemory(pValue, sizeof(int)*MEMORY_MAX_SIZE);
	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
	sprintf_s(strAddress, "D%d", nAddress);

	for (int nIdx=0; nIdx<nArrayCount; nIdx++)
	{
		pValue[nIdx] = pArrayData[nIdx];
	}

	if (acsc_WriteInteger(m_hAcsComm, -1, strAddress, 0, nArrayCount, -1, -1, pValue, NULL)==0)
	{
		return FALSE;
	}

	return TRUE;
}

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

	double pValue[MEMORY_MAX_SIZE];
	ZeroMemory(pValue, sizeof(double)*MEMORY_MAX_SIZE);
	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);

	// find common address
	long nCommonAddress = -1;
	for (int nGIdx=0; nGIdx<m_ControlInfo.GetMotorGantryAddrCount(); nGIdx++)
	{
		const CMotorGantryAddr* pGantryAddrInfo = m_ControlInfo.GetMotorGantryAddr(nGIdx);
		if  (pGantryAddrInfo==NULL) continue;

		for (int nAxisIdx=0; nAxisIdx<pGantryAddrInfo->GetMotorAxisAddrCount(); nAxisIdx++)
		{
			const CMotorAxisAddr* pAxisAddrInfo = pGantryAddrInfo->GetMotorAxisAddr(nAxisIdx);
			if (pAxisAddrInfo==NULL) continue;

			if (nAddress == pAxisAddrInfo->m_nMovePositionAddr)
				nCommonAddress = pAxisAddrInfo->m_nMovePositionCommonAddr<=0 ? 300 : pAxisAddrInfo->m_nMovePositionCommonAddr;
		}
	}

	sprintf_s(strAddress, "D%d", nCommonAddress);

	if (acsc_ReadReal(m_hAcsComm, -1, strAddress, nAddress, nAddress+nArrayCount, -1, -1, pValue, NULL)==0)
	{
		return FALSE;
	}

	for (int nIdx=0; nIdx<nArrayCount; nIdx++)
	{
		pArrayData[nIdx] = float(pValue[nIdx]);
	}

	return TRUE;
}

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

	double pValue[MEMORY_MAX_SIZE];
	ZeroMemory(pValue, sizeof(double)*MEMORY_MAX_SIZE);
	char strAddress[STR_SIZE_ADDRESS];
	ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);

	// find common address
	long nCommonAddress = -1;
	for (int nGIdx=0; nGIdx<m_ControlInfo.GetMotorGantryAddrCount(); nGIdx++)
	{
		const CMotorGantryAddr* pGantryAddrInfo = m_ControlInfo.GetMotorGantryAddr(nGIdx);
		if  (pGantryAddrInfo==NULL) continue;

		for (int nAxisIdx=0; nAxisIdx<pGantryAddrInfo->GetMotorAxisAddrCount(); nAxisIdx++)
		{
			const CMotorAxisAddr* pAxisAddrInfo = pGantryAddrInfo->GetMotorAxisAddr(nAxisIdx);
			if (pAxisAddrInfo==NULL) continue;

			if (nAddress == pAxisAddrInfo->m_nMovePositionAddr)
				nCommonAddress = pAxisAddrInfo->m_nMovePositionCommonAddr<=0 ? 300 : pAxisAddrInfo->m_nMovePositionCommonAddr;
		}
	}

	sprintf_s(strAddress, "D%d", nCommonAddress);

	for (int nIdx=0; nIdx<nArrayCount; nIdx++)
	{
		pValue[nIdx] = double(pArrayData[nIdx]);
	}

	if (acsc_WriteReal(m_hAcsComm, -1, strAddress, nAddress, nAddress+nArrayCount-1, -1, -1, pValue, NULL)==0)
	{
		return FALSE;
	}

	return TRUE;
}

int CMotorControl_Acs::GetStatus( int& nStatusCode, CString& strStatusMessage )
{
	if (GetConnected()==FALSE) 
	{
		nStatusCode = MotorStatus_NotConnected;
		strStatusMessage = _T("Not_Connected");
		return 0;
	}

	if (m_hAcsComm==ACSC_INVALID) return 0;

	int nStatus = 0;
	if (acsc_GetMotorState(m_hAcsComm, ACSC_AXIS_0, &nStatus, NULL)==0)
	{
		int nRecvCnt = 0;
		char strErr[STR_ERR_MESSAGE+1];
		ZeroMemory(strErr, sizeof(char)*(STR_ERR_MESSAGE+1));
		int nErrCode = acsc_GetLastError();
		acsc_GetErrorString(m_hAcsComm, nErrCode, strErr, STR_ERR_MESSAGE, &nRecvCnt);

		strStatusMessage = strErr;
		
		return 0;
	}

	if ((nStatus&ACSC_MST_ENABLE) || (nStatus&ACSC_MST_INPOS) || (nStatus&ACSC_MST_MOVE) || (nStatus&ACSC_MST_ACC))
	{
		nStatusCode = MotorStatus_Connected;
		strStatusMessage = _T("Connected");
	}
	else
	{
		return 0;
	}

	return 1;
}