#include "pch.h"
#include "VSMD12XControl.h"
#include "ZZ_SeiralPort.h"
#include <iostream>
#define sprintf_s snprintf
CVSMD12XControl::CVSMD12XControl()
{
	m_iProtocolType = 0;
	m_iCID = 1;
	m_bIsSyncMove = 0;
	m_piPositionInPulses = NULL;
}

CVSMD12XControl::~CVSMD12XControl()
{
	if (m_piPositionInPulses!=NULL)
	{
		delete[] m_piPositionInPulses;
	}
}

bool CVSMD12XControl::ILMES_InitializeComm(PortInfo portinfo, int iPortType /*= 1*/, int iCID /*= 1*/, bool bSyncMove /*= true*/)
{
	SetProtocolType(iPortType);
	SetCID(iCID);
	SetSyncMode(bSyncMove);

	return InitAndOpenPort(portinfo);
}

bool CVSMD12XControl::ILMES_InitializeParams(ControllerParams cpParams)
{
	bool bRes = false;

 	bRes = CVSMD12XControl::SetBaudRate(cpParams.bdr);
 	if (!bRes)
 	{
 		qDebug() << "SetBaudRate Err";
 		return false;
 	}

	bRes = CVSMD12XControl::SetSpeed(cpParams.spd,cpParams.acc,cpParams.dec);
	if (!bRes)
	{
		qDebug() << "SetSpeed Err";
		return false;
	}

	bRes = CVSMD12XControl::SetMicroSteps(cpParams.mcs);
	if (!bRes)
	{
		qDebug() << "SetMicroSteps Err";
		return false;
	}

	bRes = CVSMD12XControl::SetCurrent(cpParams.cra,cpParams.crn,cpParams.crh);
	if (!bRes)
	{
		qDebug() << "SetCurrent Err";
		return false;
	}

	bRes = CVSMD12XControl::SetZeroParams(cpParams.zmd,cpParams.osv, cpParams.snr,cpParams.zsd,cpParams.zsp);
	if (!bRes)
	{
		qDebug() << "SetZeroParams Err";
		return false;
	}
	return bRes;
}

void CVSMD12XControl::ILMES_SetPosition(int *piPositionInPulse, size_t szSize)
{
	m_piPositionInPulses = new int[szSize];
	for (size_t i=0;i< szSize;i++)
	{
		m_piPositionInPulses[i] = piPositionInPulse[i];
	}
}

bool CVSMD12XControl::ILMES_MoveToPos(int iPositionIndex)
{
	bool bFlagFailed = true;
	bool bRes = false;
	int iCount = 0;

	for (int i=0;i<3;i++)
	{
		if (CVSMD12XControl::StartBackZero())
		{
			bFlagFailed = false;
			break;
		}
		while (!bRes)
		{
			iCount++;
			bRes = StopBackZero();
			if (iCount>10)
			{
				qDebug() << "StopBackZero Err.Give up...";
				bRes = true;
			}
		}
	}
	if (bFlagFailed)
	{
		qDebug() << "StartBackZero Err";
		return false;
	}

	bFlagFailed = true;
	for (int i = 0; i < 3; i++)
	{
		if (MoveTo(m_piPositionInPulses[iPositionIndex]))
		{
			bFlagFailed = false;
			break;
		}
	}
	if (bFlagFailed)
	{
		qDebug() << "MoveTo Err";
		return false;
	}

	return true;

// 	bool bRes = false;
// 	bRes = CVSMD12XControl::StartBackZero();
// 	if (!bRes)
// 	{
// 		qDebug() << "StartBackZero Err";
// 		return false;
// 	}
// 	bRes = CVSMD12XControl::MoveTo(m_piPositionInPulses[iPositionIndex]);
// 	if (!bRes)
// 	{
// 		qDebug() << "MoveTo Err";
// 		return false;
// 	}
// 	return bRes;
}

void CVSMD12XControl::SetProtocolType(int iProtocolType /*= 0*/)
{
	m_iProtocolType = iProtocolType;
}

void CVSMD12XControl::SetCID(int iID)
{
	m_iCID = iID;

	char cmd[64];
	sprintf_s(cmd, 64, "%d ", iID);
	std::string strCID(cmd);
	m_strCID = strCID;
	return;
}

void CVSMD12XControl::SetSyncMode(bool bSyncMove /*= false*/)
{
	m_bIsSyncMove = bSyncMove;
}

bool CVSMD12XControl::InitAndOpenPort(PortInfo portinfo)
{
	bool bRes = false;
	if (portinfo.iPortType==0)
	{
		m_pAbsPort = new CZZ_SeiralPort_QT;
	}
	
	m_pAbsPort->InitPort(portinfo);
	bRes = m_pAbsPort->SetPort();
	if (!bRes)
	{
		return bRes;
	}
	bRes = m_pAbsPort->Open();
	return bRes;
}

std::string CVSMD12XControl::ReadReceive()
{
	return m_pAbsPort->ReadAll();
}

int CVSMD12XControl::SendCommand(const std::string strSend)
{
	 return m_pAbsPort->Write(strSend);
}

bool CVSMD12XControl::InitController()
{
// 	std::string strTest = "this is a test message.";
// 
// 	char *pcTest = new char[strTest.length()];
// 	strTest.copy(pcTest, strTest.length(),0);
// 
// 	memcpy(pcTest, strTest.c_str(), 23);
// 
// 	char *pcTest1 = (char*)strTest.c_str();
// 	std::string test=m_pAbsPort->ReadAll();
	std::string strCMD = "dev\n";
	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}
	int iRet=m_pAbsPort->Write(strCMD);

	QThread::msleep(500);
	std::string strRecv = m_pAbsPort->ReadAll();
	ParseReturnedString(strRecv, 0);
	std::string::size_type szPosition;
	szPosition = strRecv.find("VSMD", 0);
	if (szPosition != strRecv.npos)
	{
		return true;
	}
	return false;
}

bool CVSMD12XControl::GetStatus(MSInfo &stuMotorParam)
{
	std::string strCMD = "cts\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	//QThread::msleep(200);

	std::string strRecv = m_pAbsPort->ReadAll();
	ParseReturnedParam(strRecv);

	stuMotorParam = m_stuMSInfo;
	return true;
}

bool CVSMD12XControl::Move(int iRelPulse)
{
	using namespace ZZ_MISCDEF;
	char cmd[64];
	sprintf_s(cmd, 64, "rmv %d\n", iRelPulse);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);
	QThread::msleep(200);
	m_pAbsPort->ReadAll();
	if (m_bIsSyncMove)
	{
		if (iRet != 0)
		{
			return false;
		}

		bool bFlagIsStopped=false;
		while (!bFlagIsStopped)
		{
			QThread::msleep(200);
			GetStatus(m_stuMSInfo);
			ZZ_U8 ucFlag = m_stuMSInfo.uiFlags & 0x000000FF;
			ucFlag = ucFlag & 0x10;
			if (ucFlag==0x10 && m_stuMSInfo.fVelocity==0)
			{
				bFlagIsStopped = true;
			}
		}
		return true;
	}
	else
	{
		if (iRet == 0)
		{
			return true;
		}
		return false;
	}
	
}

bool CVSMD12XControl::Enable()
{
	std::string strCMD = "ena\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(200);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

bool CVSMD12XControl::Disable()
{
	std::string strCMD = "off\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(200);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);

}

bool CVSMD12XControl::MoveTo(int iAbsPulse)
{
	
	char cmd[64];
	sprintf_s(cmd, 64, "pos %d\n", iAbsPulse);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}


	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(200);
	//Delay_MSec(200);

	m_pAbsPort->ReadAll();

	if (m_bIsSyncMove)
	{
		if (iRet != 0)
		{
			return false;
		}

// 		bool bFlagIsStopped = false;
// 		while (!bFlagIsStopped)
// 		{
// 			QThread::msleep(200);
// 			//Delay_MSec(200);
// 			GetStatus(m_stuMSInfo);
// 			ZZ_U8 ucFlag = m_stuMSInfo.uiFlags & 0x000000FF;
// 			ucFlag = ucFlag & 0x10;
// 			if (ucFlag == 0x10 && m_stuMSInfo.fVelocity == 0)
// 			{
// 				bFlagIsStopped = true;
// 			}
// 		}
		return IsMotionFinished();
	}
	else
	{
		if (iRet == 0)
		{
			return true;
		}
		return false;
	}
// 	if (iRet == 0)
// 	{
// 		return true;
// 	}
// 	return false;
// 	std::string strTest;
// 	strTest.resize(7);
// 	char test[7];
// 	//memcpy(test, strCMD.c_str(), 7);
// 	for (size_t i = 0; i < 7; i++)
// 	{
// 		strTest[i] = cmd[i];
// 	}
}

bool CVSMD12XControl::SetLogicZero()
{
	std::string strCMD = "org\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(200);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

bool CVSMD12XControl::StartBackZero()
{
	std::string strCMD = "zero start\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	//QThread::msleep(200);
	//Delay_MSec(200);

	std::string strRecv = m_pAbsPort->ReadAll();

	if (!ParseReturnedString(strRecv, -1))
	{
		return false;
	}

	return IsMotionFinished();
// 	bool bFlagIsStopped = false;
// 	while (!bFlagIsStopped)
// 	{
// 		QThread::msleep(200);
// 		//Delay_MSec(200);
// 		GetStatus(m_stuMSInfo);
// 		ZZ_U8 ucFlag = m_stuMSInfo.uiFlags & 0x000000FF;
// 		ucFlag = ucFlag & 0x10;
// 		if (ucFlag == 0x10 && m_stuMSInfo.fVelocity == 0)
// 		{
// 			bFlagIsStopped = true;
// 		}
// 	}
// 	return true;
//	return ParseReturnedString(strRecv, -1);
	
}

bool CVSMD12XControl::StopBackZero()
{
	std::string strCMD = "zero stop\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(200);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

bool CVSMD12XControl::SetBaudRate(int iBaudRate)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg bdr=%d\n", iBaudRate);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

int CVSMD12XControl::GetBaudRate()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes= ParseReturnedParam(strRecv);
	if (bRes=false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.bdr;
	}
}

bool CVSMD12XControl::SetMicroSteps(int iMicroSteps)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg mcs=%d\n", iMicroSteps);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

int CVSMD12XControl::GetMicroSteps()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.mcs;
	}
}

bool CVSMD12XControl::SetAccSpeed(float fAccSpd)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg acc=%f\n", fAccSpd);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

float CVSMD12XControl::GetAccSpeed()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.acc;
	}
}

bool CVSMD12XControl::SetRunSpeed(float fRunSpeed)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg spd=%f\n", fRunSpeed);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

float CVSMD12XControl::GetRunSpeed()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.spd;
	}
}

bool CVSMD12XControl::SetDecSpeed(float fDecSpeed)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg dec=%f\n", fDecSpeed);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

float CVSMD12XControl::GetDecSpeed()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.dec;
	}
}

bool CVSMD12XControl::SetSpeed(float fRunSpeed, float fAccSpd, float fDecSpeed)
{
	char cmd[128];
	sprintf_s(cmd, 128, "cfg spd=%f acc=%f dec=%f\n", fRunSpeed, fAccSpd, fDecSpeed);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

bool CVSMD12XControl::SetHoldCurrent(float fHoldCurrent)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg crh=%f\n", fHoldCurrent);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

float CVSMD12XControl::GetHoldCurrent()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.crh;
	}
}

bool CVSMD12XControl::SetAccCurrent(float fAccCurrent)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg cra=%f\n", fAccCurrent);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

float CVSMD12XControl::GetAccCurrent()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.cra;
	}
}

bool CVSMD12XControl::SetRunCurrent(float fRunCurrent)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg crn=%f\n", fRunCurrent);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

float CVSMD12XControl::GetRunCurrent()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.crn;
	}
}

bool CVSMD12XControl::SetCurrent(float fAccCurrent, float fRunCurrent, float fHoldCurrent)
{
	char cmd[128];
	sprintf_s(cmd, 128, "cfg cra=%f crn=%f crh=%f\n", fAccCurrent, fRunCurrent, fHoldCurrent);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

bool CVSMD12XControl::SetS1FETE(ZZ_U8 bS1FETE)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg s1f=%d\n", bS1FETE);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

ZZ_U8 CVSMD12XControl::GetS1FETE()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.s1f;
	}
}

bool CVSMD12XControl::SetS1RETE(ZZ_U8 bS1RETE)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg s1r=%d\n", bS1RETE);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

ZZ_U8 CVSMD12XControl::GetS1RETE()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);
	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.s1r;
	}
}

bool CVSMD12XControl::SetS2FETE(ZZ_U8 bS2FETE)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg s2f=%d\n", bS2FETE);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

ZZ_U8 CVSMD12XControl::GetS2FETE()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.s2f;
	}
}

bool CVSMD12XControl::SetS2RETE(ZZ_U8 bS2RETE)
{
	char cmd[64];
	sprintf_s(cmd, 64, "cfg s2r=%d\n", bS2RETE);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	return ParseReturnedString(strRecv, -1);
}

ZZ_U8 CVSMD12XControl::GetS2RETE()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.s2r;
	}
}

bool CVSMD12XControl::SetZeroParams(ZZ_U8 bZeroMode, ZZ_U8 bSwitchOpenVL, ZZ_U8 bZeroSensor, float fZeroVelocity, int iSafePosition)
{
	char cmd[128];
	sprintf_s(cmd, 128, "cfg zmd=%d osv=%d snr=%d zsd=%f zsp=%d\n",
		(int)bZeroMode, (int)bSwitchOpenVL, (int)bZeroSensor, fZeroVelocity, iSafePosition);
	std::string strCMD(cmd);

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(800);

	std::string strRecv = m_pAbsPort->ReadAll();

	return ParseReturnedString(strRecv, -1);
}

ZZ_U8 CVSMD12XControl::GetZeroMode()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.zmd;
	}
}

ZZ_U8 CVSMD12XControl::GetSwitchOpenVoltageLevel()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.osv;
	}
}

ZZ_U8 CVSMD12XControl::GetZeroSensor()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.snr;
	}
}

int CVSMD12XControl::GetSafePosition()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.zsp;
	}
}

float CVSMD12XControl::GetZeroVelocity()
{
	std::string strCMD = "cfg\n";

	if (m_iProtocolType)
	{
		strCMD = m_strCID + strCMD;
	}

	int iRet = m_pAbsPort->Write(strCMD);

	QThread::msleep(500);

	std::string strRecv = m_pAbsPort->ReadAll();
	bool bRes = ParseReturnedParam(strRecv);
	if (bRes = false)
	{
		return -1;
	}
	else
	{
		return m_stuControllerParams.zsp;
	}
}

bool CVSMD12XControl::ParseReturnedString(std::string &strRecv, int iType)
{
	char cHead =(char) 0xFF;
	if (iType == -1)
	{
		if (strRecv[0]== cHead)
		{
			return true;
		}
		return false;
	}
	else if (iType == 0)
	{
		if (strRecv.size()<3)
		{
			return false;
		}
		strRecv.erase(0, 3);
		strRecv.erase(strRecv.size() - 3, 3);
		return true;
	}
	else
	{
		return false;
	}
}

bool CVSMD12XControl::ParseReturnedParam(std::string &strRecv)
{
	while (uchar(strRecv[0]) != 0xff && strRecv.size() > 0)
	{
		//std::cout << strRecv.size() << endl;
		strRecv.erase(strRecv.begin());
		//std::cout << strRecv.size() << endl;

	}
	int iTry = 0;
	while (strRecv.size() < 2)
	{

		std::string strRecv1 = m_pAbsPort->ReadAll();
		if (strRecv1.size() == 0)
		{
			return false;

		}
		strRecv = strRecv + strRecv1;
		iTry++;
		if (iTry == 2)
		{
			return false;

		}
	}


	if (strRecv[2]==0x2)
	{
		//strRecv.erase(0, 3);
		//strRecv.erase(strRecv.size() - 3, 3);

		int iRetry = 0;
		while (strRecv.size() != 21)
		{
			std::string strRecvTemp = "";
			strRecvTemp = m_pAbsPort->ReadAll(20);
			if (strRecvTemp.size() == 0)
			{
				return false;

			}
			strRecv = strRecv + strRecvTemp;
			iRetry++;
			if (iRetry == 5)
			{
				return false;

			}
			if (strRecv.size() > 21)
			{
				return false;
			}
			//return false;
		}


		if (strRecv.size() != 21)
		{
			return false;
		}
		ZZ_U8 *pbRecv = (ZZ_U8*)strRecv.c_str();

		VSMD_BitShift(&pbRecv[3], &m_unValue);
		m_stuMSInfo.fVelocity = m_unValue.fValue;
		VSMD_BitShift(&pbRecv[8], &m_unValue);
		m_stuMSInfo.iPosition = m_unValue.i32Value;
		VSMD_BitShift(&pbRecv[13], &m_unValue);
		m_stuMSInfo.uiFlags = m_unValue.u32Value;

		//delete pbRecv;
		return true;
	}
	else if (strRecv[2] == 0x3)
	{
		strRecv[strRecv.size() - 3] = 0;
		char* pcParam = ((char*)strRecv.c_str())+3;

		while (pcParam)
		{
			char* pcNext =     VSMD_Split(pcParam, ' ');
			char* pcValueStr = VSMD_Split(pcParam, '=');

			//
			if (pcValueStr)
			{
				if (strcmp(pcParam, "bdr") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.bdr = iValue;
				}
				else if (strcmp(pcParam, "mcs") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.mcs = iValue;
				}
				else if (strcmp(pcParam, "spd") == 0)
				{
					float fValue = (float)atof(pcValueStr);
					m_stuControllerParams.spd = fValue;
				}
				else if (strcmp(pcParam, "acc") == 0)
				{
					float fValue = (float)atof(pcValueStr);
					m_stuControllerParams.acc = fValue;
				}
				else if (strcmp(pcParam, "dec") == 0)
				{
					float fValue = (float)atof(pcValueStr);
					m_stuControllerParams.dec = fValue;
				}
				else if (strcmp(pcParam, "cra") == 0)
				{
					float fValue = (float)atof(pcValueStr);
					m_stuControllerParams.cra = fValue;
				}
				else if (strcmp(pcParam, "crn") == 0)
				{
					float fValue = (float)atof(pcValueStr);
					m_stuControllerParams.crn = fValue;
				}
				else if (strcmp(pcParam, "crh") == 0)
				{
					float fValue = (float)atof(pcValueStr);
					m_stuControllerParams.crh = fValue;
				}
				else if (strcmp(pcParam, "s1f") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.s1f = iValue;
				}
				else if (strcmp(pcParam, "s1r") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.s1r = iValue;
				}
				else if (strcmp(pcParam, "s2f") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.s2f = iValue;
				}
				else if (strcmp(pcParam, "s2r") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.s2r = iValue;
				}
				else if (strcmp(pcParam, "zmd") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.zmd = iValue;
				}
				else if (strcmp(pcParam, "osv") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.osv = iValue;
				}
				else if (strcmp(pcParam, "snr") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.snr = iValue;
				}
				else if (strcmp(pcParam, "zsd") == 0)
				{
					float fValue = (float)atof(pcValueStr);
					m_stuControllerParams.zsd = fValue;
				}
				else if (strcmp(pcParam, "zsp") == 0)
				{
					int iValue = atoi(pcValueStr);
					m_stuControllerParams.zsp = iValue;
				}

			}
			pcParam = pcNext;
		}
		return true;
	}
	
	return false;
}

void CVSMD12XControl::VSMD_BitShift(ZZ_U8* src, unValue* dst)
{
	if (src)
	{
		dst->u32Value = src[0];

		dst->u32Value <<= 7;
		dst->u32Value |= src[1];

		dst->u32Value <<= 7;
		dst->u32Value |= src[2];

		dst->u32Value <<= 7;
		dst->u32Value |= src[3];

		dst->u32Value <<= 7;
		dst->u32Value |= src[4];
	}
}

char* CVSMD12XControl::VSMD_Split(char* cStr, char cSplit)
{
	if (cStr)
	{
		for (char* cPtr = cStr; *cPtr; cPtr++)
		{
			if (*cPtr == cSplit)
			{
				*cPtr = '\0';
				cPtr++;
				return cPtr;
			}
		}
	}

	return NULL;
}

bool CVSMD12XControl::IsMotionFinished()
{
	//true  means stopped
	//false means unknown status
	int iCountTotal = 0;
	int iCountStopped = 0;
	bool bFlagIsStopped = false;

	while (!bFlagIsStopped)
	{
		iCountTotal++;
		QThread::msleep(200);
		//Delay_MSec(200);
		GetStatus(m_stuMSInfo);
		ZZ_U8 ucFlag = m_stuMSInfo.uiFlags & 0x000000FF;
		ucFlag = ucFlag & 0x10;
		if (m_stuMSInfo.fVelocity == 0)
		{
			iCountStopped++;
		}
		if (ucFlag == 0x10 && m_stuMSInfo.fVelocity == 0)
		{
			bFlagIsStopped = true;
			return true;
		}
		if (iCountStopped>=50)
		{
			qDebug() <<"Warning.Motion Err,should be stopped already.checking register and retry...";
			qDebug() <<"motion status:"<<ucFlag<<"all status:"<<QString::number(m_stuMSInfo.uiFlags,2);
			bFlagIsStopped = true;
			return false;
		}
		if (iCountTotal>1000)
		{
			qDebug() << "Err.Motor Hardware Err,should be stopped already";
			qDebug() << "motion status:" << ucFlag << "all status:" << QString::number(m_stuMSInfo.uiFlags, 2);
			bFlagIsStopped = true;
			return false;
		}
	}
	return false;

}

void CVSMD12XControl::Delay_MSec(ZZ_U16 usMS)
{
	QEventLoop qeLoop;
	QTimer::singleShot(usMS, &qeLoop, SLOT(quit()));
	qeLoop.exec();
}

// int CVSMD12XControl::add(int a, int b)
// {
// 	return a + b;
// }