#include "VinceMotorControllerUsb.h"

VinceMotorControllerUsb::VinceMotorControllerUsb(QString serialPortNumber, QString paramJson, QObject* parent)
:QObject(parent)
{
	m_pSerialPort = new QSerialPort(this);

	m_pSerialPort->setPortName(serialPortNumber);
	m_pSerialPort->setReadBufferSize(512);

	bool bRes = m_pSerialPort->setBaudRate(9600);
	if (!bRes)
	{
		//qDebug() << "Err:setBaudRate Failed.Exit Code:1";
		//std::cout << "Err.setBaudRate Failed" << std::endl;
		printf("Err:setBaudRate Failed.Exit Code:1");
	}

	bRes = m_pSerialPort->open(QIODevice::ReadWrite);
	if (!bRes)
	{
		//qDebug() << "Err:open Failed.Exit Code:2";
		//std::cout << "Err.open Failed" << std::endl;
		printf("Err:open Failed.Exit Code:2");
	}

	m_iSpeed = 3000;

	init(paramJson);
}

VinceMotorControllerUsb::~VinceMotorControllerUsb()
{
	delete m_pSerialPort;
}

int VinceMotorControllerUsb::sendCommand2Motor(QString cmd)
{
	QByteArray commandData = cmd.toUtf8(); // 将 QString 转换为 QByteArray
	qint64 bytesWritten = m_pSerialPort->write(commandData);

	/*if (bytesWritten == -1) {
		qDebug() << "写入失败:" << m_pSerialPort->errorString();
	}
	else if (bytesWritten != commandData.size()) {
		qDebug() << "写入不完整";
	}
	else {
		qDebug() << "命令发送成功";
	}*/

	return bytesWritten;
}

int VinceMotorControllerUsb::recvData(QByteArray& dataRecv)
{
	dataRecv.clear();

	QByteArray temp;

	temp = m_pSerialPort->readAll();
	dataRecv.append(temp);

	while (dataRecv.size() < 21)
	{
		m_pSerialPort->waitForReadyRead(100);
		temp = m_pSerialPort->readAll();
		dataRecv.append(temp);
	}

	return 0;
}

IrisMotorErrorCode VinceMotorControllerUsb::extractOneValidFrame(QByteArray& buffer)
{
	//std::cout << "before------" << buffer.size() << std::endl;
	//std::cout << buffer.toHex().toStdString() << std::endl;

	IrisMotorErrorCode errorCode = MOTOR_NO_ERROR;

	//方式1：
	/*int startPos = buffer.lastIndexOf('\xFF');
	int endPos = buffer.indexOf('\xFE');

	if (endPos <= startPos)
	{
		errorCode = MOTOR_INVALID_FRAME;
		return errorCode;
	}

	buffer = buffer.mid(startPos, endPos - startPos + 1);*/

	//方式2：
	//ff00ffffff000200000000000000391f6900000d42030001feff00ff000200000000000000391f6900000d42030001feff0002043458000000003c745e00000d402b001afeff00ff0002000000000000003d3b7200000d4207003efeff00020c34580000000039076e00000d4023005cfeff00ff00020000000000000038411900000d420f0022feff000204345800000000477e7700000d402b0042feff00ff00020000000000000048456a00000d420b0021feff00020434245400000049006d00000d400b0020feff00ff000200000000000000493c3100000d4203000afeff0002043458000000004d444b00000d402f004afe
	//ff00ffffffff000200000000000000112d6f00000c42070018fe
	QList<QByteArray> validFrames;
	int start = 0;

	while (start < buffer.size())
	{
		// 查找帧头 'ff'
		int frameStart = buffer.indexOf(MOTOR_SYNC, start);
		if (buffer.at(frameStart + 1) == MOTOR_SYNC)
			continue;
		if (frameStart == -1)
		{
			break; // 没有找到帧头，退出循环
		}

		// 查找帧尾 'fe'
		int frameEnd = buffer.indexOf(MOTOR_ETX, frameStart + 1);
		if (frameEnd == -1)
		{
			break; // 没有找到帧尾，退出循环
		}

		// 提取有效帧（包括帧头和帧尾）
		QByteArray frame = buffer.mid(frameStart, frameEnd - frameStart + 1);

		// 检查帧长度是否等于21
		if (frame.size() == 21)
		{
			validFrames.append(frame);
		}

		// 更新起始位置，继续查找下一个帧
		start = frameEnd + 1;
	}
	if (validFrames.size() == 0)
	{
		std::cout << "error--------------: " << buffer.toHex().toStdString() << std::endl;


		errorCode = MOTOR_INVALID_FRAME;
		return errorCode;
	}

	buffer = validFrames.at(validFrames.size() - 1);

	//方式3：从结尾遍历，找到有效帧（1）找到所有fe（2）从最后一个fe开始向前找到有效帧（长度为21）
	
	//std::cout << "after" << buffer.size() << std::endl;
	//std::cout << buffer.toHex().toStdString() << std::endl;

	return errorCode;
}

IrisMotorErrorCode VinceMotorControllerUsb::parseOneValidFrame(QByteArray& buf, State& backdatt)
{
	IrisMotorErrorCode errorCode = MOTOR_NO_ERROR;

	backdatt.Speed = MOTOR_disconnection;

	buf.remove(0, 3);
	buf.remove(buf.length() - 3, 3);
	int location;
	unsigned char a[15];
	memcpy(a, buf.data(), 15);
	location = ((a[0] & 0x0f) << 28) | ((a[1] & 0x7f) << 21) | ((a[2] & 0x7f) << 14) | ((a[3] & 0x7f) << 7) | ((a[4] & 0x7f));
	float speed;

	memcpy(&speed, &location, 4);
	backdatt.Speed = speed;
	int loc;
	loc = ((a[5] & 0x0f) << 28) | ((a[6] & 0x7f) << 21) | ((a[7] & 0x7f) << 14) | ((a[8] & 0x7f) << 7) | ((a[9] & 0x7f));
	backdatt.Location = loc;
	backdatt.Speed = speed;
	location = ((a[10] & 0x0f) << 28) | ((a[11] & 0x7f) << 21) | ((a[12] & 0x7f) << 14) | ((a[13] & 0x7f) << 7) | ((a[14] & 0x7f));
	backdatt.Stata = unsigned int(location);

	/*QString str1 = "Speed " + QString::number(backdatt.Speed, 'f', 2) + " location " + QString::number(backdatt.Location);
	qDebug() << str1;*/

	return errorCode;
}

IrisMotorErrorCode VinceMotorControllerUsb::parseBytedata(QByteArray buf, State& backdatt)
{
	IrisMotorErrorCode errorCode = extractOneValidFrame(buf);

	backdatt.Speed = MOTOR_disconnection;
	backdatt.Location = 0;
	if (errorCode == MOTOR_NO_ERROR)
	{
		parseOneValidFrame(buf, backdatt);
		return errorCode;
	}

	errorCode = MOTOR_INVALID_FRAME;
	return errorCode;
}

void VinceMotorControllerUsb::init(QString jsonPath)
{
	enable();

	JsonOperate xxx(jsonPath);
	//xxx.createJsonFile();
	QStringList re = xxx.parseJsonFile();

	for (const QString& pair : re)
	{
		//qDebug().noquote() << pair;
		sendCommand2Motor(pair);

		QByteArray buf;
		recvData(buf);
	}

	//QString cmd;

	////cmd = "cfg s1f=2\n";//用不上
	////sendCommand2Motor(cmd);

	////归零设置
	//cmd = "cfg zmd=2\n";
	//sendCommand2Motor(cmd);
	//cmd = "cfg snr=0\n";
	//sendCommand2Motor(cmd);
	//cmd = "cfg osv=0\n";//归零用传感器常开常闭设置
	//sendCommand2Motor(cmd);
	//cmd = "cfg zsd=3000\n";//正数向原点走
	//sendCommand2Motor(cmd);
	//cmd = "cfg zsp=2400\n";
	//sendCommand2Motor(cmd);

	////设置正负极限
	//cmd = "cfg msr=1\n";//负极限
	//sendCommand2Motor(cmd);
	//cmd = "cfg msv=0\n";
	//sendCommand2Motor(cmd);
	//cmd = "cfg psr=2\n";
	//sendCommand2Motor(cmd);
	//cmd = "cfg psv=0\n";
	//sendCommand2Motor(cmd);

	////加减速度和电流设置
	//cmd = "cfg mcs=7\n";//设置细分
	//sendCommand2Motor(cmd);
	//cmd = "cfg acc=20000\n";
	//sendCommand2Motor(cmd);
	//cmd = "cfg dec=20000\n";
	//sendCommand2Motor(cmd);
	//cmd = "cfg crn=4\n";
	//sendCommand2Motor(cmd);
	//cmd = "cfg cra=4\n";
	//sendCommand2Motor(cmd);
	//cmd = "cfg crh=1\n";
	//sendCommand2Motor(cmd);
}

bool VinceMotorControllerUsb::isConnected()
{
	State re = getState();
	if (re.Speed == MOTOR_disconnection)
	{
		return false;
	}
	else
	{
		return true;
	}
}

void VinceMotorControllerUsb::enable()
{
	QString cmd = "ena\n";
	sendCommand2Motor(cmd);
	QByteArray buf;
	recvData(buf);
}

void VinceMotorControllerUsb::move2Pos(double pos)
{
	QString cmd = "pos " + QString::number(pos, 'f', 10) + "\n";
	sendCommand2Motor(cmd);
	QByteArray buf;
	recvData(buf);
}

void VinceMotorControllerUsb::move(bool towardOrigin)
{
	if (towardOrigin)
	{
		setSpeed(abs(m_iSpeed) * -1);
	}
	else
	{
		setSpeed(abs(m_iSpeed));
	}

	sendCommand2Motor("mov\n");
	QByteArray buf;
	recvData(buf);
}

void VinceMotorControllerUsb::stopMove()
{
	QString cmd = "stp\n";
	sendCommand2Motor(cmd);
	QByteArray buf;
	recvData(buf);
}

void VinceMotorControllerUsb::setSpeed(double speed)
{
	m_iSpeed = speed;
	QString cmd = "cfg spd=" + QString::number(speed) + "\n";
	sendCommand2Motor(cmd);
	QByteArray buf;
	recvData(buf);
}

double VinceMotorControllerUsb::getSpeed()
{
	State re = getState();
	return re.Speed;
}

double VinceMotorControllerUsb::getCurrentLoc()
{
	State re = getState();
	return re.Location;
}

void VinceMotorControllerUsb::zeroStart()
{
	QString cmd = "zero start\n";
	sendCommand2Motor(cmd);
	QByteArray buf;
	recvData(buf);
}

int VinceMotorControllerUsb::getSpeedLocation(double& speed, double& locatioin)
{
	State re = getState();
	speed = re.Speed;
	locatioin = re.Location;
	
	return 0;
}

State VinceMotorControllerUsb::getState()
{
	QString cmd = "sts\n";
	sendCommand2Motor(cmd);

	QByteArray buf;
	recvData(buf);
	//std::cout << buf.size() << std::endl;
	//std::cout << buf.toHex().toStdString() << std::endl;

	State back;
	IrisMotorErrorCode errorCode = parseBytedata(buf, back);

	return back;
}