#include "IS11_Ctrl_Qt.h"

IS11_Ctrl_Qt::IS11_Ctrl_Qt(QObject* parent /*= nullptr*/)
{
	m_pSerialPort = new QSerialPort;
	m_iBaudRate = 921600;
}

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

int IS11_Ctrl_Qt::Initialize(bool bIsUSBMode, std::string ucPortNumber, std::string strDeviceName)
{
	QString qstrPortName = QString::fromStdString(ucPortNumber);
	m_pSerialPort->setPortName(qstrPortName);
	m_pSerialPort->setReadBufferSize(512);
	 
	bool bRes = m_pSerialPort->setBaudRate(m_iBaudRate);
	if (!bRes)
	{
		//qDebug() << "Err:setBaudRate Failed.Exit Code:1";
		//std::cout << "Err.setBaudRate Failed" << std::endl;
		printf("Err:setBaudRate Failed.Exit Code:1");
		return 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");
		return 2;
	}
	

	// 	int testi;
	// 	GetDeviceAttribute(m_daDeviceAttr);
	// 	GetExposureTime(testi);
	// 	SetExposureTime(10000);
	// 	DataFrame test;
	// 	SingleShot(test);

	GetDeviceInfo(m_diDeviceInfo);
	//GetExposureTime_Init();

	std::string::size_type szPostion = m_diDeviceInfo.strSN.find(strDeviceName);
	if (szPostion == std::string::npos)
	{
		printf("Err:FS serial number not match.Exit Code:3");
		//qDebug() << "Err:FS serial number not match.Exit Code:3";
		//return 3;
	}
	else
	{
		return 0;
	}
	return 0;
}

void IS11_Ctrl_Qt::Close()
{

}

int IS11_Ctrl_Qt::SingleShot(DataFrame& dfData)
{
	SendData_CMD03((char*)GET_ALL_DN, sizeof(GET_ALL_DN));
	RecvData_CMD03(dfData);

	GetExposureTime(m_iExposureTime);
	dfData.usExposureTimeInMS = (unsigned short)m_iExposureTime;
	return 0;
}

int IS11_Ctrl_Qt::SetExposureTime(int iExposureTimeInMS)
{
	QByteArray qbaRecv;
	qbaRecv.clear();

	unsigned char pucExposureTime[2];
	pucExposureTime[0] = iExposureTimeInMS / 256;
	pucExposureTime[1] = iExposureTimeInMS % 256;
	SendData_CMD06((char *)SET_INTEGRAL_TIME, sizeof(SET_INTEGRAL_TIME), (char*)pucExposureTime, 2);
	//SendData_CMD03((char *)GET_INTEGRAL_TIME, sizeof(GET_INTEGRAL_TIME));
	int iRes = RecvData_CMD06(qbaRecv);
	return iRes;
}

int IS11_Ctrl_Qt::GetExposureTime(int & iExposureTimeInMS)
{
	QByteArray qbaRecv;
	qbaRecv.clear();
	SendData_CMD03((char *)GET_INTEGRAL_TIME, sizeof(GET_INTEGRAL_TIME));
	int iRes = RecvData_CMD03(qbaRecv);

	iExposureTimeInMS = ((unsigned char)(qbaRecv[0]))*256+ (unsigned char)(qbaRecv[1]);
	return iRes;
}

int IS11_Ctrl_Qt::GetDeviceInfo(DeviceInfo & Info)
{
	Info.strPN = "IS11";
	Info.strSN = "NULL";
	return 0;
}

int IS11_Ctrl_Qt::GetDeviceAttribute(DeviceAttribute & Attr)
{
	QByteArray qbaRecv;
	qbaRecv.clear();
	float fCoef[4];
	unsigned short usTempCoef[8];

	Attr.iPixels = 2048;
	Attr.iMinIntegrationTimeInMS = 1;
	Attr.iMaxIntegrationTimeInMS = 60000;

	SendData_CMD03((char *)GET_WAVELENTH_COEFF, sizeof(GET_WAVELENTH_COEFF));
	RecvData_CMD03(qbaRecv);

	//memcpy(fTempCoef,qbaRecv,16);
	memcpy(usTempCoef, qbaRecv, 16);

	for (int i=0;i<8;i++)
	{
		usTempCoef[i] = qToBigEndian(usTempCoef[i]);
	}

	//float *pfTemp = (float*)usTempCoef;
	memcpy(fCoef, usTempCoef,16);

	//Conv_LTB((char*)fTempCoef,16);

	for (int i = 1; i <= 2048; i++)
	{
		Attr.fWaveLengthInNM[i - 1] = fCoef[0] * i*i*i + fCoef[1] * i*i + fCoef[2] * i + fCoef[3];
		//setem.WavelenthStr = setem.WavelenthStr + String(setem.wavelenthlist[i - 1]).c_str() + ",";
	}
	return 0;
}

int IS11_Ctrl_Qt::SetDeviceTemperature(float fTemperature)
{
	return 0;
}

int IS11_Ctrl_Qt::GetDeviceTemperature(float & fTemperature)
{
	fTemperature = -1000;
	return 0;
}

int IS11_Ctrl_Qt::PerformAutoExposure(float fMinScaleFactor, float fMaxScaleFactor, float & fPredictedExposureTime)
{
	return 0;
}

// unsigned short IS11_Ctrl_Qt::crc16(const uint8_t *pbdata, size_t sz)
// {
// 	uint16_t i, j, tmp, CRC16;
// 
// 	CRC16 = 0xFFFF; // CRC寄存器初始值
// 	for (i = 0; i < sz; i++)
// 	{
// 		CRC16 ^= pbdata[i];
// 		for (j = 0; j < 8; j++)
// 		{
// 			tmp = (uint16_t)(CRC16 & 0x0001);
// 			CRC16 >>= 1;
// 			if (tmp == 1)
// 			{
// 				CRC16 ^= 0xa001; // 异或多项式
// 			}
// 		}
// 	}
// 	return CRC16;
// }

unsigned short IS11_Ctrl_Qt::CalCRC16(const uint8_t *pbData, size_t szLength)
{
	uint16_t usCRC16 = CRC16_INIT; 
	for (size_t i = 0; i < szLength; i++)
	{
		usCRC16 ^= pbData[i];
		for (size_t j = 0; j < 8; j++) 
		{
			if ((usCRC16 & 0x0001) != 0) 
			{
				usCRC16 = (usCRC16 >> 1) ^ CRC16_POLYNOMIAL; 
			}
			else 
			{
				usCRC16 = usCRC16 >> 1;
			}
		}
	}
	return usCRC16;
}

void IS11_Ctrl_Qt::Conv_LTB(char * pcData, int iLength)
{
	char *TempData = new char[iLength];
	memcpy(TempData, pcData, iLength);

	for (int i = 0; i < iLength / 2; i++)
	{
		pcData[2 * i] = TempData[2 * i + 1];
		pcData[2 * i + 1] = TempData[2 * i];
		/* code */
	}
	delete[] TempData;
}

int IS11_Ctrl_Qt::SendData_CMD03(char * pcCMD, size_t szCMDLength)
{
	QString    qstrSend,qstrCRC16;
	QByteArray qbaSend;
	qbaSend.clear();

	//char *pctest=new char[szCMDLength+2];
	//memcpy(pctest, pcCMD, szCMDLength);
	//ushort ustest = crc16((unsigned char*)pcCMD,szCMDLength);
	//qstrCRC16= QString::fromLatin1((const char*)usCRC16, (int)sizeof(uint16_t));
	//memcpy(pctest+ szCMDLength,&usCRC16, 2);
	//int iReturn = m_pSerialPort->write(pctest,szCMDLength+2);
	//delete[] pctest;
	uint16_t usCRC16 = CalCRC16((unsigned char*)pcCMD, szCMDLength);
	qstrSend = QString::fromLatin1((const char*)pcCMD, (int)szCMDLength);
	const char * pucSend = (char *)&usCRC16;

	qbaSend.append(qstrSend);
	qbaSend.append(pucSend, (int)sizeof(uint16_t));

	int iReturn = m_pSerialPort->write(qbaSend);

	return iReturn;
}

int IS11_Ctrl_Qt::RecvData_CMD03(QByteArray &qbaRecv)
{
	int iRetryCount = 0;
	QByteArray qbaOriRecv,qbaTemp;
	qbaOriRecv.clear();
	qbaTemp.clear();

	///read all once
	Read_IS11(qbaTemp);
	qbaOriRecv.append(qbaTemp);
	while (qbaOriRecv.size() < 2 || ParseHdr(qbaOriRecv, 3) == 1)
	{
		m_pSerialPort->waitForReadyRead(1000);
		Read_IS11(qbaTemp);
		qbaOriRecv.append(qbaTemp);
		iRetryCount++;
		if (iRetryCount>100)
		{
			qDebug() << "Recv Hdr Err.out of retry time.RecvData_CMD03";
			return 1;
		}
	}

	iRetryCount = 0;
	while (qbaOriRecv.size()< qbaOriRecv[2]+5)
	{
		m_pSerialPort->waitForReadyRead(1000);
		Read_IS11(qbaTemp);
		qbaOriRecv.append(qbaTemp);
		iRetryCount++;
		if (iRetryCount > 66)
		{
			qDebug() << "Recv Data Err.out of retry time";
			return 2;
		}
	}

	if (qbaOriRecv.size()!= qbaOriRecv[2] + 5)
	{
		/// can be handled but dont want to
		qDebug() << "Wrong Recv data size.RecvData_CMD03_QBA";
		return 3;
	}

	unsigned short usCRCC=CalCRC16((unsigned char *)qbaOriRecv.data(), qbaOriRecv.size()-2);
	char *pc = (char *)&usCRCC;
	if ((pc[0]== qbaOriRecv[qbaOriRecv.size()-1]&& pc[1]== qbaOriRecv[qbaOriRecv.size()-2])|| 
		(pc[0] == qbaOriRecv[qbaOriRecv.size()-2] && pc[1] == qbaOriRecv[qbaOriRecv.size()-1]))
	{
		qbaRecv = qbaOriRecv.mid(3, qbaOriRecv.length() - 5);
	}
	else
	{
		///error crc
		qDebug() << "Recv data crc16 Err."<<pc[1]<< pc[0];
		return 4;
	}
	

	/*while (ParseHdr(qbaOriRecv, 3)==1)
	{
		m_pSerialPort->waitForReadyRead(100);
		Read_IS11(qbaTemp);
		qbaOriRecv.append(qbaTemp);
	}*/
	
	return 0;
}

int IS11_Ctrl_Qt::RecvData_CMD03(DataFrame& dfData)
{
	int iRecvDataLength = 0;
	int iRetryCount = 0;
	QByteArray qbaOriRecv, qbaTemp;
	qbaOriRecv.clear();
	qbaTemp.clear();

	///read all once
	Read_IS11(qbaTemp);
	qbaOriRecv.append(qbaTemp);
	while (qbaOriRecv.size() < 4 || ParseHdr(qbaOriRecv, 3) == 1)
	{
		m_pSerialPort->waitForReadyRead(1000);
		Read_IS11(qbaTemp);
		qbaOriRecv.append(qbaTemp);
		iRetryCount++;
		if (iRetryCount > 66)
		{
			qDebug() << "Recv Hdr Err.out of retry time";
			return 1;
		}
	}

	int length = qbaOriRecv[2] * 256 + qbaOriRecv[3];
	iRetryCount = 0;
	while (qbaOriRecv.size() < length + 4)
	{
		m_pSerialPort->waitForReadyRead(1000);
		Read_IS11(qbaTemp);
		qbaOriRecv.append(qbaTemp);
		iRetryCount++;
		if (iRetryCount > 66)
		{
			qDebug() << "Recv Data Err.out of retry time";
			return 2;
		}
	}

	if (qbaOriRecv.size() != length + 4)
	{
		/// can be handled but dont want to
		qDebug() << "Wrong Recv data size.RecvData_CMD03_DF";
		return 3;
	}

	QByteArray qbaData;
	qbaData = qbaOriRecv.right(length - 4);

	int iDataSizeInPixel = qbaData.size() / 2;
	unsigned short *pusData = new unsigned short[iDataSizeInPixel];

	memcpy(pusData, qbaData, iDataSizeInPixel * 2);
	for (size_t i = 0; i < iDataSizeInPixel; i++)
	{
		dfData.lData[i] = qToBigEndian(pusData[i]);
		//qDebug() << dfData.lData[i];

	}
	///it seems the manufacture not enable crc16 check for this function
// 	unsigned short usCRCC = CalCRC16((unsigned char *)qbaOriRecv.data(), qbaOriRecv.size() - 2);
// 	char *pc = (char *)&usCRCC;
// 	if ((pc[0] == qbaOriRecv[qbaOriRecv.size() - 1] && pc[1] == qbaOriRecv[qbaOriRecv.size() - 2]) ||
// 		(pc[0] == qbaOriRecv[qbaOriRecv.size() - 2] && pc[1] == qbaOriRecv[qbaOriRecv.size() - 1]))
// 	{
// 		//qbaRecv = qbaOriRecv.mid(3, qbaOriRecv.length() - 5);
// 		//dataframe process here
// 
// 	}
// 	else
// 	{
// 		///error crc
// 		qDebug() << "Recv data crc16 Err." << pc[1] << pc[0];
// 		return 4;
// 	}

	return 0;
}


//int IS11_Ctrl_Qt::RecvData_CMD03(float& fWavelength)
//{
//	int iRetryCount = 0;
//	QByteArray qbaOriRecv, qbaTemp,qbaRecv;
//	qbaOriRecv.clear();
//	qbaTemp.clear();
//	qbaRecv.clear();
//
//	///read all once
//	Read_IS11(qbaTemp);
//	qbaOriRecv.append(qbaTemp);
//	while (qbaOriRecv.size() < 2 || ParseHdr(qbaOriRecv, 3) == 1)
//	{
//		m_pSerialPort->waitForReadyRead(100);
//		Read_IS11(qbaTemp);
//		qbaOriRecv.append(qbaTemp);
//		iRetryCount++;
//		if (iRetryCount > 20)
//		{
//			qDebug() << "Recv Hdr Err.out of retry time";
//			return 1;
//		}
//	}
//
//	iRetryCount = 0;
//	while (qbaOriRecv.size() < qbaOriRecv[2] + 5)
//	{
//		m_pSerialPort->waitForReadyRead(100);
//		Read_IS11(qbaTemp);
//		qbaOriRecv.append(qbaTemp);
//		iRetryCount++;
//		if (iRetryCount > 20)
//		{
//			qDebug() << "Recv Data Err.out of retry time";
//			return 2;
//		}
//	}
//
//	if (qbaOriRecv.size() != qbaOriRecv[2] + 5)
//	{
//		/// can be handled but dont want to
//		qDebug() << "Wrong Recv data size.";
//		return 3;
//	}
//
//	unsigned short usCRCC = CalCRC16((unsigned char *)qbaOriRecv.data(), qbaOriRecv.size() - 2);
//	char *pc = (char *)&usCRCC;
//	if ((pc[0] == qbaOriRecv[qbaOriRecv.size() - 1] && pc[1] == qbaOriRecv[qbaOriRecv.size() - 2]) ||
//		(pc[0] == qbaOriRecv[qbaOriRecv.size() - 2] && pc[1] == qbaOriRecv[qbaOriRecv.size() - 1]))
//	{
//		qbaRecv = qbaOriRecv.mid(3, qbaOriRecv.length() - 5);
//	}
//	else
//	{
//		///error crc
//		qDebug() << "Recv data crc16 Err." << pc[1] << pc[0];
//		return 4;
//	}
//
//	fWavelength = qbaRecv.toFloat();
//	return 0;
//}

int IS11_Ctrl_Qt::SendData_CMD06(char *pcCMD, size_t szCMDLength, char *pcValue, size_t szValueLenth)
{
// 	QString    qstrSend, qstrSend1, qstrCRC16;///QByteArray append出现多余字节
// 	QByteArray qbaSend;
// 	qbaSend.clear();
// 
// 	qstrSend = QString::fromLatin1(pcCMD,  (int)szCMDLength);
// 	qbaSend.append(qstrSend);
// 
// 	qstrSend1.clear();
// 	qstrSend1 = QString::fromLatin1(pcValue,(int)szValueLenth);
// 	qbaSend.append(qstrSend1);

	QByteArray qbaSend;
	qbaSend.append(pcCMD, szCMDLength);
	qbaSend.append(pcValue, szValueLenth);

	unsigned short usCRC16 = CalCRC16((unsigned char*)qbaSend.data(), qbaSend.size());
	const char * pucSend = (char *)&usCRC16;
	qbaSend.append(pucSend, (int)sizeof(unsigned short));

	int iReturn = m_pSerialPort->write(qbaSend);

	return 0;

}

int IS11_Ctrl_Qt::RecvData_CMD06(QByteArray &qbaRecv)
{
	int iRetryCount = 0;
	QByteArray qbaOriRecv, qbaTemp;
	qbaOriRecv.clear();
	qbaTemp.clear();

	Read_IS11(qbaTemp);
	qbaOriRecv.append(qbaTemp);
	while (qbaOriRecv.size() < 2 || ParseHdr(qbaOriRecv, 6) == 1)
	{
		m_pSerialPort->waitForReadyRead(1000);
		Read_IS11(qbaTemp);
		qbaOriRecv.append(qbaTemp);
		iRetryCount++;
		if (iRetryCount > 100)
		{
			qDebug() << "Recv Hdr Err.out of retry time.RecvData_CMD06";
			return 1;
		}
	}

	iRetryCount = 0;
	while (qbaOriRecv.size() < 8)
	{
		m_pSerialPort->waitForReadyRead(1000);
		Read_IS11(qbaTemp);
		qbaOriRecv.append(qbaTemp);
		iRetryCount++;
		if (iRetryCount > 66)
		{
			qDebug() << "Recv Data Err.out of retry time";
			return 2;
		}
	}

	if (qbaOriRecv.size() != 8)
	{
		/// can be handled but dont want to
		qDebug() << "Wrong Recv data size.RecvData_CMD06";
		return 3;
	}

	unsigned short usCRCC = CalCRC16((unsigned char *)qbaOriRecv.data(), qbaOriRecv.size() - 2);
	char *pc = (char *)&usCRCC;
	if ((pc[0] == qbaOriRecv[qbaOriRecv.size() - 1] && pc[1] == qbaOriRecv[qbaOriRecv.size() - 2]) ||
		(pc[0] == qbaOriRecv[qbaOriRecv.size() - 2] && pc[1] == qbaOriRecv[qbaOriRecv.size() - 1]))
	{
		qbaRecv = qbaOriRecv.mid(2, 4);
	}
	else
	{
		///error crc
		qDebug() << "Recv data crc16 Err." << pc[1] << pc[0];
		return 4;
	}

	return 0;
}

int IS11_Ctrl_Qt::ParseHdr(QByteArray &qbaData, char cHdrType)
{
	QByteArray qbaTemp(qbaData);
	while ((qbaTemp[0] != (char)0x01) &&(qbaTemp[1] != cHdrType))
	{
		qbaTemp.remove(0, 1);
		if (qbaTemp.size()<=2)
		{
			return 1;
		}
	}

	if (qbaTemp!= qbaData)
	{
		///warning some communication error may happened.
		qbaData = qbaTemp;
		return 1000;
	}

	return 0;
}

int IS11_Ctrl_Qt::Write_IS11(char * pcCMD, size_t szCMDLength)
{
	QString    qstrSend;
	QByteArray qbaSend;
	qbaSend.clear();

	qstrSend = QString::fromLatin1((const char*)pcCMD, (int)szCMDLength);
	return (int)m_pSerialPort->write(qbaSend);
}

int IS11_Ctrl_Qt::Read_IS11(QByteArray &qbaRecv)
{
	//m_pSerialPort->waitForReadyRead(100);
	qbaRecv = m_pSerialPort->readAll();
	return qbaRecv.size();
}