#include "CGugaoDriver.h"
#include <windows.h>
#include "gts.h"
#include "gtgl500.h"
#include <thread>

CGugaoDriver::CGugaoDriver()
{
}


CGugaoDriver::~CGugaoDriver()
{
}

bool CGugaoDriver::OpenCard(std::vector<int>& axisIdVec)
{
	short status = -1;
	short sRtn = -1;
	sRtn = GTN_Open(CHANNEL_PCIE, 1); // 打开运动控制器，第一个参数：打开运动控制器方式，第二个参数默认1，暂无意义
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_Open is error");
		return false;
	}

	sRtn = GTN_Reset(CORE_NUM); //复位核1,参数：内核编号
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_Reset is error");
		return false;
	}

	//初始化总线
	sRtn = GTN_TerminateEcatComm(CORE_NUM); //结束EtherCAT通讯。
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_TerminateEcatComm is error");
		return false;
	}

	sRtn = GTN_InitEcatComm(CORE_NUM); //EtherCAT初始化。
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_InitEcatComm is error");
		return false;
	}

	short sCnt = 0;
	do
	{
		sRtn = GTN_IsEcatReady(1, &status); //查询EtherCAT通讯状态。 0：通讯未完全建立；1：通讯完全建立。
		Sleep(500);
		sCnt++;
	} while ((status != 1 || sRtn != 0) && (sCnt < 20));      //通讯在0.5s*20仍未准备好

	sRtn = GTN_StartEcatComm(CORE_NUM); //启动EtherCAT通讯。成功调用这条指令后，才可以调用其他运动指令。
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_StartEcatComm is error");
		return false;
	}

	// 配置运动控制器
	char szFilePath[MAX_PATH + 1] = { 0 };
	GetModuleFileNameA(NULL, szFilePath, MAX_PATH);
	(strrchr(szFilePath, '\\'))[1] = 0; //删除文件名，只获得路径
	sprintf(szFilePath, "%sgtn_core1.cfg", szFilePath);
	//下载配置信息到运动控制器,调用该指令后需再调用GTN_ClrSts才能使该指令生
	sRtn = GTN_LoadConfig(CORE_NUM, szFilePath); //第一个参数：内核，第二个参数：文件路径
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_LoadConfig error");
		return false;
	}

	for (auto& iter : axisIdVec)
	{
		//第一个参数：内核编号，第二个参数：轴编号，第三个参数：默认为1，暂无实际意义
		sRtn = GTN_ClrSts(CORE_NUM, iter, 10); //清除驱动器报警标志、跟随误差越限标志、限位触发标志。
		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "GTN_ClrSts is error");
			return false;
		}
	}

	for (auto& iter : axisIdVec)
	{
		//第一个参数：内核编号，第二个参数：轴编号，第三个参数：默认为1
		sRtn = GTN_ZeroPos(CORE_NUM, iter, 1);      //清零规划位置和实际位置，并进行零漂补偿。
	}

	//驱动器使能
	for (auto& iter : axisIdVec) 
	{
		sRtn = GTN_AxisOn(CORE_NUM, iter); //打开驱动器使能, 第一个参数：内核编号，第二个参数：轴编号
		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "GTN_AxisOn is error");
			return false;
		}
		Sleep(50);
	}

	sRtn = GT_GLinkInit(0); //扩展模块初始化,第一个参数，目前默认为0
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "GT_GLinkInit is error");
		return false;
	}

	m_open = true;
	return true;
}

bool CGugaoDriver::CloseCard(std::vector<int>& axisIdVec)
{
	//驱动器使能
	short sRet;
	for (auto& iter : axisIdVec)
	{
		sRet = GTN_AxisOff(CORE_NUM, iter); //关闭驱动器使能。第一个参数：内核，第二个参数：轴号
		if (sRet)
		{
			Logger::GetInstance()->WriteLog(0, "GTN_AxisOff is error");
			return false;
		}
		Sleep(50);
	}

	sRet = GTN_Close(); //关闭运动控制器
	if (sRet)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_Close is error");
		return false;
	}
	m_open = false;
	return true;
}

int CGugaoDriver::AbsMove(int iAxisId,
	int pulseTo1mm,
	double dPos,
	double dAcc,
	double dDec,
	double dMoveSpeed,
	bool bWaitFinished)
{
	if (!m_open)
		return 1;

	//执行安全回调
	if (m_onAxisMoveSafeFunc != nullptr)
	{
		if (!m_onAxisMoveSafeFunc(iAxisId, dPos))
		{
			return 1;
		}
	}
	
	//换算成脉冲
	double targetPulse = pulseTo1mm * dPos;

	unsigned short uirvMode;
	//读取 EtheCAT 轴的操作模式,第一个参数：内核，第二个参数轴号，第三个参数私服操作模式
	short  sRet = GTN_GetEcatAxisMode(CORE_NUM, iAxisId, &uirvMode);
	if (!sRet && uirvMode != CYCLIC_SYNC_POSITION)
	{
		//设置轴的操作模式
		sRet = GTN_SetEcatAxisMode(CORE_NUM, iAxisId, CYCLIC_SYNC_POSITION);
	}

	long lAxisStatus;
	sRet = GTN_GetSts(CORE_NUM, iAxisId, &lAxisStatus); //获取轴的状态
	if (sRet)
	{
		Logger::GetInstance()->WriteLog(0, "AbsMotion中的GTN_GetSts失败");
		return false;
	}
	if (!(lAxisStatus & (1 << 9)))//第9位为1时，电机使能
	{
		Logger::GetInstance()->WriteLog(0, "轴未使能");
		return false;
	}
	if (lAxisStatus & (1 << 10))//检查轴是否正在运动
	{
		Logger::GetInstance()->WriteLog(0, "轴正在运动");
		return false;
	}

	sRet = GTN_PrfTrap(CORE_NUM, iAxisId); //切换为点位运动
	if (sRet)
		Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");

	// 读取点位运动参数
	TTrapPrm trap;
	sRet = GTN_GetTrapPrm(CORE_NUM, iAxisId, &trap);
	if (sRet)
	{
		Logger::GetInstance()->WriteLog(0, "SetPointMotionParam中的GTN_GetTrapPrm失败");
		return false;
	}
	trap.acc = dAcc;
	trap.dec = dDec;
	trap.velStart = dMoveSpeed;
	trap.smoothTime = 50;

	sRet = GTN_SetTrapPrm(CORE_NUM, iAxisId, &trap); // 设置点位运动参数
	if (sRet)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_SetTrapPrm is error");
		return 1;
	}

	sRet = GTN_SetVel(CORE_NUM, iAxisId, dMoveSpeed); //设置目标速度
	if (sRet)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_SetVel is error");
		return 1;
	}

	// 设置AXIS轴的目标位置
	sRet = GTN_SetPos(CORE_NUM, (short)iAxisId, (long)targetPulse);
	Sleep(100);
	if (sRet)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_SetPos is error");
		return 1;
	}

	// 启动AXIS轴的运动当 bit 位为 1 时表示启动对应的轴。
	short sRtn = GTN_Update(CORE_NUM, 1 << (iAxisId - 1));
	Sleep(100);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "GTN_Update is error");
		return 1;
	}

	if (bWaitFinished)
	{
		long lAxisSts = 0;
		while (true)
		{
			// 读取AXIS轴的状态
			sRtn = GTN_GetSts(CORE_NUM, iAxisId, &lAxisSts);
			if (sRtn)
			{
				Logger::GetInstance()->WriteLog(0, "GTN_GetSts is error");
				return 1;
			}

			if (lAxisSts & (1 << 1))
			{
				Logger::GetInstance()->WriteLog(0, "Driver alarm flag");
			}
			else if (lAxisSts & (1 << 4))
			{
				Logger::GetInstance()->WriteLog(0, "跟随误差越限标志");
			}
			else if (lAxisSts & (1 << 5))
			{
				Logger::GetInstance()->WriteLog(0, "正限位触发标志");
			}
			else if (lAxisSts & (1 << 6))
			{
				Logger::GetInstance()->WriteLog(0, "负限位触发标志");
			}
			else if (lAxisSts & (1 << 7))
			{
				Logger::GetInstance()->WriteLog(0, "IO平滑停止触发标志");
			}
			else if (lAxisSts & (1 << 8))
			{
				Logger::GetInstance()->WriteLog(0, "IO急停触发标志");
			}
			else if (lAxisSts & (1 << 9))
			{
				Logger::GetInstance()->WriteLog(0, "电机使能标志");
			}

			if (lAxisSts & (1 << 10))
			{
				continue;
			}
			else
			{
				double prefPos;
				// 读取AXIS轴的规划位置
				short sRtn = GTN_GetPrfPos(CORE_NUM, iAxisId, &prefPos);
				if (sRtn)
				{
					Logger::GetInstance()->WriteLog(0, "function:GetCurPos, GTN_GetPrfPos is error");
					return 1;
				}
				long pulsePos = (long)prefPos;
				if (pulsePos == (long)targetPulse) {
					return 0;
				}
				else
				{
					Logger::GetInstance()->WriteLog(0, "GetPos != setPos");
					continue;
				}
			}
		}
		Sleep(25);
	}
	return 0;
}

int CGugaoDriver::WriteIO(short sSlaveNo, short sIoIndex, unsigned char ucValue)
{
	if (m_onSetIoSafeFunc != nullptr)
	{
		if (!m_onSetIoSafeFunc(sIoIndex, ucValue)) {
			return 1;
		}
	}

	//设置数字量输出，sSlaveNo：模块号，sIoIndex：IO号，ucValue：数值
	short rtn = GT_SetGLinkDoBit(sSlaveNo, sIoIndex, ucValue); 
	if (rtn != 0) {
		Logger::GetInstance()->WriteLog(0, "function:writeIO, GT_SetGLinkDoBit is error");
		return 1;
	}
	return 0;
}

int CGugaoDriver::WriteAO(short sSlaveNo, short sIoIndex, short sValue)
{
	//设置模拟量，sSlaveNo：模块号，sIoIndex：IO号，ucValue：数值
	short rtn = GT_SetGLinkAo(sSlaveNo, sIoIndex, &sValue, 1);
	if (rtn != 0) {
		Logger::GetInstance()->WriteLog(0, "function:writeAO, GT_SetGLinkAo is error");
		return 1;
	}
	return 0;
}

short CGugaoDriver::ReadDo(short sSlaveNo, short sIoIndex)
{
	//读取模拟量输出数值, sSlaveNo：模块号，sIoIndex：IO号
	short outval = 0;
	short rtn = GT_GetGLinkAo(sSlaveNo, sIoIndex, &outval, 1);
	if (rtn != 0) {
		Logger::GetInstance()->WriteLog(0, "function:ReadDo, GT_GetGLinkAo is error");
		return outval;
	}
	return outval;
}

unsigned char CGugaoDriver::ReadDi(short sSlaveNo, short sIoIndex)
{
	unsigned char bit = 0;
	short sRet = GT_GetGLinkDiBit(sSlaveNo, sIoIndex, &bit);
	if (sRet != 0) {
		Logger::GetInstance()->WriteLog(0, "function:ReadIO, GT_GetGLinkDiBit is error");
		return bit;
	}
	return bit;
}

double CGugaoDriver::GetCurPos(int iAxisId, int pulseTo1mm)
{
	if (!m_open)
	{
		Logger::GetInstance()->WriteLog(0, "function:GetCurPos, device is not open");
		return 0;
	}

	double pos;

	// 读取AXIS轴的规划位置
	short sRtn = GTN_GetPrfPos(CORE_NUM, iAxisId, &pos);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "function:GetCurPos, GTN_GetPrfPos is error");
		return 0;
	}

	if (pulseTo1mm > 0)
	{
		pos /= (double)pulseTo1mm;
	}

	return pos;
}

void CGugaoDriver::stopMove(int iAxisId)
{

}

int CGugaoDriver::home(int iAxisId, double dSearchSpeed, double iIndexSpeed, double sSearchAcc, short sModule)
{
	if (!m_open)
		return 1;

	if (m_onAxisPrevHomeFunc != nullptr)
	{
		if (!m_onAxisPrevHomeFunc(iAxisId)) {
			return 1;
		}
	}

	// 必须处于伺服使能状态， 切换到回零模式，数字6代表回零模式
	short sRtn = GTN_SetHomingMode(CORE_NUM, iAxisId, 6);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");
		sRtn = GTN_SetHomingMode(CORE_NUM, iAxisId, 8);// 切换到位置控制模式
		if (sRtn)
			Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");
		return 1;
	}

	//设置回零参数
	sRtn = GTN_SetEcatHomingPrm(CORE_NUM, iAxisId,
		sModule,//回零模式
		dSearchSpeed, //搜索开关速度，单位： 驱动器设置的用户速度单位
		iIndexSpeed, //搜索 index 标识速度，单位： 驱动器设置的用户速度单位
		sSearchAcc, //搜索加速度
		0, //原点偏移量
		0 //探针功能
	);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_GetEcatHomingStatus is error");
		return 1;
	}

	// 启动回零
	sRtn = GTN_StartEcatHoming(CORE_NUM, iAxisId);
	if (sRtn)
	{
		Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_GetEcatHomingStatus is error");
		return 1;
	}

	unsigned short sHomeSts;	// 回零状态
	while (true)
	{

		sRtn = GTN_GetEcatHomingStatus(CORE_NUM, iAxisId, &sHomeSts); //读回零状态
		if (sRtn)
		{
			Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_GetEcatHomingStatus is error");
			return 1;
		}

		if (sHomeSts == 3) //3代表回零完成和回零成功完成
		{
			double encPos;
			if (GTN_GetEncPos(CORE_NUM, iAxisId, &encPos) == 0) {
				//回零后编码器位置
				std::string str = "Encoder position after returning to zero: " + std::to_string(encPos);
				Logger::GetInstance()->WriteLog(0, str);
			}

			double prfPos;
			if (GTN_GetPrfPos(CORE_NUM, iAxisId, &prfPos) == 0) {
				//回零后规划位置
				std::string str = "Plan location after returning to zero: " + std::to_string(encPos);
				Logger::GetInstance()->WriteLog(0, str);
			}

			sRtn = GTN_ClrSts(CORE_NUM, iAxisId);
			if (sRtn)
				Logger::GetInstance()->WriteLog(0, "function:Home, GTN_ClrSts is error");

			sRtn = GTN_ZeroPos(CORE_NUM, iAxisId);//清零规划位置和实际位置，并进行零漂补偿。
			if (sRtn)
				Logger::GetInstance()->WriteLog(0, "function:Home, GTN_ZeroPos is error");


			if (GTN_GetEncPos(CORE_NUM, iAxisId, &encPos) == 0) {
				//回零后编码器位置
				std::string str = "Encoder position after clear(GTN_ZeroPos): " + std::to_string(encPos);
				Logger::GetInstance()->WriteLog(0, str);
			}

			if (GTN_GetPrfPos(CORE_NUM, iAxisId, &prfPos) == 0) {
				//回零后规划位置
				std::string str = "Plan location after clear(GTN_ZeroPos): " + std::to_string(encPos);
				Logger::GetInstance()->WriteLog(0, str);
			}

			if (GTN_SetHomingMode(CORE_NUM, iAxisId, 8))// 切换到位置控制模式,8代表位置控制模式
				Logger::GetInstance()->WriteLog(0, "function: Home -> GTN_SetHomingMode is error");

			break;
		}
		Sleep(200);
	}

	if (m_AxisAfterHomeFunc != nullptr)
	{
		if (!m_AxisAfterHomeFunc(iAxisId)) 
		{
			return 1;
		}
	}
	return 0;
}

bool CGugaoDriver::IsHome(int iAxisId)
{
	if (!m_open) 
	{
		return false;
	}

	unsigned short sHomeSts;	// 回零状态
	short sRtn = GTN_GetEcatHomingStatus(CORE_NUM, iAxisId, &sHomeSts); //读回零状态
	if (sHomeSts == 3) //3代表回零完成和回零成功完成
	{
		return true;
	}
	return false;
}

bool CGugaoDriver::IsHoming(int iAxisId)
{
	if (!m_open)
	{
		return false;
	}

	unsigned short sHomeSts;	// 回零状态
	short sRtn = GTN_GetEcatHomingStatus(CORE_NUM, iAxisId, &sHomeSts); //读回零状态
	if (sHomeSts == 0) //0代表正在回零
	{
		return true;
	}
	return false;
}

bool CGugaoDriver::IsMoveing(int iAxisId)
{
	if (!m_open)
	{
		return false;
	}
	long lAxisStatus = 0;
	// 读取AXIS轴的状态
	short sRtn = GTN_GetSts(CORE_NUM, iAxisId, &lAxisStatus);
	if (lAxisStatus & 0x400) //0x400正好在运动标志位
	{
		return true;
	}
	return false;
}

bool CGugaoDriver::IsMoveEnd(int iAxisId)
{
	if (!m_open)
	{
		return false;
	}
	long lAxisStatus = 0;
	// 读取AXIS轴的状态
	short sRtn = GTN_GetSts(CORE_NUM, iAxisId, &lAxisStatus);
	if (!(lAxisStatus & 0x400))//0x400正好在运动标志位,反向
	{
		return true;
	}
	return false;
}

bool CGugaoDriver::IsAxisEnable(int iAxisId)
{
	if (!m_open)
	{
		return false;
	}
	long lAxisStatus = 0;
	// 读取AXIS轴的状态
	short sRtn = GTN_GetSts(CORE_NUM, iAxisId, &lAxisStatus);
	if (lAxisStatus & 0x200)//0x200私服使能
	{
		return true;
	}
	return false;
}

bool CGugaoDriver::IsAxisError(int iAxisId)
{
	if (!m_open)
	{
		return false;
	}
	long lAxisStatus = 0;
	// 读取AXIS轴的状态
	short sRtn = GTN_GetSts(CORE_NUM, iAxisId, &lAxisStatus);
	if (lAxisStatus & 0x2) // 0x2私服报警标志
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool CGugaoDriver::GetAxisErrorCode(int iAxisId)
{
	return false;
}

bool CGugaoDriver::IsAxisPaslimit(int iAxisId)
{
	if (!m_open)
	{
		return false;
	}
	long lAxisStatus = 0;
	// 读取AXIS轴的状态
	short sRtn = GTN_GetSts(CORE_NUM, iAxisId, &lAxisStatus);
	// 正向限位触发
	if (lAxisStatus & 0x20)
	{
		return true;
	}
	return false;
}

bool CGugaoDriver::isAxisNagLimit(int iAxisId)
{
	long lAxisStatus = 0;
	// 读取AXIS轴的状态
	short sRtn = GTN_GetSts(CORE_NUM, iAxisId, &lAxisStatus);
	if (lAxisStatus & 0x40)
	{
		return true;
	}
	return false;
}