#include "AbstractFSController.h"
#include "ZZ_Math_HDRONLY.h"
#include <math.h>
void Delay_MSec(ZZ_U16 usMS)
{
	QEventLoop qeLoop;
	QTimer::singleShot(usMS, &qeLoop, SLOT(quit()));
	qeLoop.exec();
}
CAbstractFSController::CAbstractFSController(QObject* parent /*= nullptr*/)
{
	iFlagInit = 0;
	m_pFSCtrl = NULL;
	m_iThreadID = -1;

	m_vecDataFrameDark.clear();
	m_vecDataFrameSignal.clear();

	m_qstrCalFilePath = "/home/data/Cal";

	m_vecNonLinearCalP.clear();
	for (int i=0;i<10;i++) {
		m_taginfolast.ITtimeChange[i] =true;
		m_taginfolast.LastMAXValue[i] = 0;
		m_taginfolast.lastITTIME[i] = 0;
	}
}

CAbstractFSController::~CAbstractFSController()
{
	if (m_pFSCtrl!= 0 )
	{
		delete m_pFSCtrl;
	}

}

int CAbstractFSController::SetRunParas(int iThreadID, FSInfo fsInfo)
{
	connect(this, &CAbstractFSController::SignalInit_Self, this, &CAbstractFSController::InitializeFSControl);
	m_iThreadID = iThreadID;
	m_fsInfo = fsInfo;
	return 0;
}



int CAbstractFSController::InitializeFSControl()
{
	using namespace ZZ_MISCDEF::IRIS;

	int iRes = 0;

	if (m_iThreadID == -1/*|| m_iDeviceType == -1*/)
	{
		qDebug() << "Params Err. Call SetRunParas first";
		return 1;
	}
	switch (m_fsInfo.ucDeviceModel)
	{
	case DeviceModel::OSIFAlpha:  
		m_pFSCtrl = new OceanOptics_lib;
		if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) != 0)
		{
			qDebug() << "OSIFAlpha Not Opened";
			return 2;
		}
		iRes = LoadQEProLinearCalibrationFile();
		if (iRes != 0)
		{
			qDebug() << "LoadQEProLinearCalibrationFile Failed" << iRes;
			//return 5;
		}
		break;
	case DeviceModel::OSIFBeta:
		m_pFSCtrl = new OceanOptics_lib;
 		if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) !=0)
 		{
 			qDebug() << "OSIFBeta Not Opened";
 			return 2;
 		}
		iRes = LoadQEProLinearCalibrationFile();
		if (iRes != 0)
		{
			qDebug() << "LoadQEProLinearCalibrationFile Failed" << iRes;
			//return 5;
		}
		break;
	case DeviceModel::ISIF: 
		m_pFSCtrl = new ZZ_ATPControl_Serial_Qt;
		//m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, NULL);
		if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) != 0)
		{
			qDebug() << "ISIF Not Opened";
			return 3;
		}
		break;
	case DeviceModel::IS1:
		m_pFSCtrl = new ZZ_ATPControl_Serial_Qt;
		//m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, NULL);
		if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) != 0)
		{
			qDebug() << "IS1 Not Opened";
			return 3;
		}
		break;
	case DeviceModel::IS2:
		m_pFSCtrl = new ZZ_ATPControl_Serial_Qt;
		//m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, NULL);
		if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) != 0)
		{
			qDebug() << "IS2 Not Opened";
			return 3;
		}
		break;
	default: 
		break;
	}

	iRes = m_pFSCtrl->GetDeviceAttribute(m_daDeviceAttr);
	if (iRes != 0)
	{
		qDebug() << "GetDeviceAttribute Failed" << iRes;
		return 4;
	}
	
	iRes = m_pFSCtrl->SetDeviceTemperature(0);
	if (iRes != 0)
	{
		qDebug() << "SetDeviceTemperature Failed" << iRes;
		//return 5;
	}
	iFlagInit = 1;
	return 0;
}

int CAbstractFSController::InitializeFSControl_Self()
{
	//InitializeFSControl();
	emit SignalInit_Self();
	return 0;
}

int CAbstractFSController::GetDeviceAttr(DeviceAttribute &daAttr)
{
	daAttr = m_daDeviceAttr;
	return 0;
}

int CAbstractFSController::PerformAutoExposure(int position)
{
	if (position!=0) {
		// m_taginfolast.ITtimeChange[position] = true;
		m_taginfolast.lastITTIME[position] = m_taginfolast.lastITTIME[0]*0.5 ;
		 m_pFSCtrl->SetExposureTime(m_taginfolast.lastITTIME[position] );
		return 0;
	}





	qDebug() << "--------------------------Starting PerformAutoExposure" << " Thread ID:" << m_iThreadID;
	using namespace ZZ_MATH;
	float fPredictedExposureTime;
	int iDeviceDepth = (int)m_fsInfo.lDepth;

	qDebug() << "MAX---Min" << m_fsInfo.fMaxFactor << "---" << m_fsInfo.fMinFactor << " Thread ID:" << m_iThreadID;

	bool bFlagIsOverTrying = false;
	bool bFlagIsLowerMinExposureTime = false;
	bool bFlagIsOverMaxExposureTime = false;
	bool bFlagIsAutoExposureOK = false;
	bool bFlagIsAutoExposureFailed = false;

	bool bIsValueOverflow = false;
	bool bIsLastValueOverflow = false;

	int  iExposureTime = 1000;
	float  fTempExposureTime = 0;
	double fLastExposureTime = 0.1;
	int    iRepeatCount = 0;

	//int iRes = m_pFSCtrl->SetExposureTime(1000);//need change to load from files
	int iRes = 0;
	if (iRes != 0)
	{
		qDebug() << "Err:PerformAutoExposure Failed.Exit Code:1" << " Thread ID:" << m_iThreadID;
		return 1;
	}

	while (!bFlagIsAutoExposureOK && !bFlagIsAutoExposureFailed)
	{
		DataFrame dfTemp;

		if (iRepeatCount++ > 30)
		{
			bFlagIsAutoExposureFailed = true;
			bFlagIsOverTrying = true;
			break;
		}
		//m_pFSCtrl->SetExposureTime(5000);
		// m_pFSCtrl->GetExposureTime(iExposureTime);
		// qDebug() << "Current ExpTime:" << iExposureTime << " Thread ID:" << m_iThreadID;
		m_pFSCtrl->SetExposureTime(iExposureTime);
		//fExposureTime = (float)m_daDeviceAttr.iMinIntegrationTimeInMS;
		fTempExposureTime = iExposureTime;

		iRes = m_pFSCtrl->SingleShot(dfTemp);
		//iRes = m_pFSCtrl->SingleShot(dfTemp);
		if (iRes != 0)
		{
			qDebug() << "Err:PerformAutoExposure Failed.Exit Code:2" << " Thread ID:" << m_iThreadID;
			return 2;
		}

		HeapSort(dfTemp.lData, m_daDeviceAttr.iPixels);

		double dSum = 0;
		int iCount = m_daDeviceAttr.iPixels / 200;
		for (int i = 0; i < iCount; i++)
		{
			dSum += dfTemp.lData[i];
		}
		double dTemp = dSum / iCount;

		qDebug() << "Avg " << dTemp << " Thread ID:" << m_iThreadID;

		if (dTemp >= iDeviceDepth * 0.99)
		{
			bIsValueOverflow = true;
			if (!bIsLastValueOverflow)
			{
				iExposureTime = (float)(fLastExposureTime + iExposureTime) / 2;
			}
			else
			{
				iExposureTime = iExposureTime / 2;
			}
		}

		else if (iDeviceDepth * m_fsInfo.fMaxFactor >= dTemp && dTemp >= iDeviceDepth * m_fsInfo.fMinFactor)
		{
			qDebug() << "trace bFlagIsAutoExposureOK =1 " << iExposureTime << " Thread ID:" << m_iThreadID;
			bFlagIsAutoExposureOK = 1;
		}
		else if (dTemp > iDeviceDepth * m_fsInfo.fMaxFactor)
		{
			bIsValueOverflow = true;
			if (!bIsLastValueOverflow)
			{
				iExposureTime = (float)(fLastExposureTime + iExposureTime) / 2;
			}
			else
			{
				iExposureTime = iExposureTime * 3 / 4;
			}
		}
		else if (dTemp < iDeviceDepth * m_fsInfo.fMinFactor)
		{
			bIsValueOverflow = false;
			if (bIsLastValueOverflow)
			{
				iExposureTime = (float)(fLastExposureTime + iExposureTime) / 2;
			}
			else
			{
				double dFactor;
				dFactor = dTemp / (iDeviceDepth * m_fsInfo.fMaxFactor);
				iExposureTime = (float)(iExposureTime / dFactor);
			}
			
		}
		bIsLastValueOverflow = bIsValueOverflow;
		fLastExposureTime = fTempExposureTime;

		

		if (/*fExposureTime > 100 || */iExposureTime <= m_daDeviceAttr.iMinIntegrationTimeInMS)
		{
			bFlagIsAutoExposureOK = false;
			bFlagIsAutoExposureFailed = true;
			bFlagIsLowerMinExposureTime = true;

// 			qDebug() << "Warning:PerformAutoExposure lower than min integration time.Will be limited to " << m_daDeviceAttr.iMinIntegrationTimeInMS - 1 << "MS" << " Thread ID:" << m_iThreadID;
// 			iRes = m_pFSCtrl->SetExposureTime((int)iExposureTime);
// 			if (iRes != 0)
// 			{
// 				qDebug() << "Err:PerformAutoExposure Failed.Exit Code:4" << " Thread ID:" << m_iThreadID;
// 				return 3;
// 			}
// 			else
// 			{
// 				qDebug() << "Success:PerformAutoExposure. Value" << iExposureTime << " Thread ID:" << m_iThreadID;
// 			}
			m_taginfolast.ITtimeChange[position] = true;
			m_taginfolast.lastITTIME[position] = m_daDeviceAttr.iMinIntegrationTimeInMS;
			iRes = m_pFSCtrl->SetExposureTime(m_daDeviceAttr.iMinIntegrationTimeInMS);
			if (iRes != 0)
			{
				qDebug() << "Err:PerformAutoExposure Failed.Exit Code:3" << " Thread ID:" << m_iThreadID;
				return 3;
			}
			else
			{
				qDebug() << "Warning:PerformAutoExposure lower than min integration time.Will be limited to " << m_daDeviceAttr.iMinIntegrationTimeInMS << "MS" << " Thread ID:" << m_iThreadID;
			}

			break;
		}

		if (iExposureTime > m_daDeviceAttr.iMaxIntegrationTimeInMS-1)
		{
			bFlagIsAutoExposureOK = false;
			bFlagIsAutoExposureFailed = true;
			bFlagIsOverMaxExposureTime = true;
			//float fPredictedExposureTime = m_daDeviceAttr.iMaxIntegrationTimeInMS-1;
			//iRes = m_pFSCtrl->SetExposureTime(m_daDeviceAttr.iMaxIntegrationTimeInMS-1);
			//if (iRes != 0)
			//{
				//qDebug() << "Err:PerformAutoExposure Failed.Exit Code:3" << " Thread ID:" << m_iThreadID;
				//return 3;
			//}
			//else
			//{
				//qDebug() << "Warning:PerformAutoExposure exceed max integration time.Will be limited to 30sec";
			//}
			m_taginfolast.ITtimeChange[position] = true;
			m_taginfolast.lastITTIME[position] =m_daDeviceAttr.iMaxIntegrationTimeInMS - 1;
			iRes = m_pFSCtrl->SetExposureTime(m_daDeviceAttr.iMaxIntegrationTimeInMS - 1);
			if (iRes != 0)
			{
				qDebug() << "Err:PerformAutoExposure Failed.Exit Code:3" << " Thread ID:" << m_iThreadID;
				return 3;
			}
			else
			{
				qDebug() << "Warning:PerformAutoExposure exceed max integration time.Will be limited to " << m_daDeviceAttr.iMaxIntegrationTimeInMS - 1 << "MS" << " Thread ID:" << m_iThreadID;
			}

			break;
		}
		m_taginfolast.ITtimeChange[position] = true;
		m_taginfolast.lastITTIME[position] =(int)iExposureTime;
		iRes = m_pFSCtrl->SetExposureTime((int)iExposureTime);
		if (iRes != 0)
		{
			qDebug() << "Err:PerformAutoExposure Failed.Exit Code:4" << " Thread ID:" << m_iThreadID;
			return 3;
		}
		else
		{
			qDebug() << "Success:PerformAutoExposure. Value" << iExposureTime << " Thread ID:" << m_iThreadID;
		}
	}
	
	fPredictedExposureTime = iExposureTime;
	qDebug() << "--------------------------Stop PerformAutoExposure" << " Thread ID:" << m_iThreadID;
	//emit SignalAcqFinished(m_iThreadID, 1);

	return 0;
}

int CAbstractFSController::ComputExposure(int position) {
	if (position==0) {


	int lastittime=m_taginfolast.lastITTIME[position];
	int lastmaxvalue=m_taginfolast.LastMAXValue[position];

	int maxallowvalue=m_fsInfo.lDepth * m_fsInfo.fMaxFactor;
	int minallowvalue=m_fsInfo.lDepth * m_fsInfo.fMinFactor;
	if (lastmaxvalue>maxallowvalue) {
		if (lastmaxvalue>=m_fsInfo.lDepth) {
			lastittime=lastittime/2;

		}else {
			lastittime=lastittime*(maxallowvalue*1.0/lastmaxvalue)*0.95;
		}

		m_taginfolast.ITtimeChange[position]=true;
		m_taginfolast.lastITTIME[position]=lastittime;
		m_pFSCtrl->SetExposureTime(lastittime);
		qDebug()<< "Set exposure time for position"<<position<<" to "<<lastittime;
		return lastittime;

	}
	if (lastmaxvalue<minallowvalue) {
		lastittime=lastittime*(minallowvalue*1.0/lastmaxvalue)*1.05;
		if (lastittime>=60000) {
			lastittime=60000;
		}
		m_taginfolast.ITtimeChange[position]=true;
		m_taginfolast.lastITTIME[position]=lastittime;
		m_pFSCtrl->SetExposureTime(lastittime);
		qDebug()<< "Set exposure time for position"<<position<<" to "<<lastittime;
		return lastittime;
	}
		m_taginfolast.ITtimeChange[position]=false;
		m_pFSCtrl->SetExposureTime(lastittime);
		qDebug()<< "Set exposure time for position"<<position<<" to "<<lastittime;
		return lastittime;
	}
	int lasttimeindev=0;
	m_pFSCtrl->GetExposureTime(lasttimeindev);

	m_pFSCtrl->SetExposureTime(m_taginfolast.lastITTIME[0]*0.5);
	if (lasttimeindev!=m_taginfolast.lastITTIME[0]*0.5) {
		Delay_MSec(lasttimeindev);
	}
	qDebug()<< "Set exposure time for position"<<position<<" to "<<m_taginfolast.lastITTIME[0]*0.5;
	return m_taginfolast.lastITTIME[0]*0.5;

}

int CAbstractFSController::TakeDarkFrame(int position)
{
	int shuttertime = 0;
	m_pFSCtrl->GetExposureTime(shuttertime);
	qDebug() << "Starting TakeDarkFrame" << " position ID:" << position<<" ITtime:"<<shuttertime<<"\n";
	if (m_taginfolast.ITtimeChange[0]) {
		m_taginfolast.LastDarkframe[position]=TakeOneFrame();
	}


	m_vecDataFrameDark.push_back(m_taginfolast.LastDarkframe[position]);
	

	qDebug() << "Stop TakeDarkFrame" << " Thread ID:" << m_iThreadID;
	//emit SignalAcqFinished(m_iThreadID, 1);
	return 0;
}


int CAbstractFSController::ComputeMaxValue(DataFrame Data) {
	ZZ_S32 *pData = Data.lData;
	int number = 4096;
	int maxvalue=0;
	for (int i=0;i<number-1;i++) {
		if (pData[i]>maxvalue) {
			maxvalue=pData[i];
		}
	}
	return maxvalue;

	// if (number <= 10) {
	// 	// 如果数据量小，直接排序计算
	// 	int sum = 0;
	// 	for (int i = 0; i < number; i++) {
	// 		sum += pData[i];
	// 	}
	// 	return number > 0 ? sum / number : 0;
	// }
	//
	// // 复制数据（或者直接操作原数组如果允许修改）
	// std::vector<int> temp(pData, pData + number);
	//
	// // 使用nth_element找到第10大的元素
	// std::nth_element(temp.begin(), temp.begin() + 9, temp.end(), std::greater<int>());
	//
	// // 计算前10个最大值的平均
	// int sum = 0;
	// for (int i = 0; i < 10; i++) {
	// 	sum += temp[i];
	// }
	//
	// return sum / 10;




	// int maxvalue[10]={0};
	// ZZ_S32 *pData=Data.lData;
	// int number=4096;
	// //获取pdata中最大的10个值
	// for (int i=0;i<number;i++) {
	// 	int val=pData[i];
	// 	for (int j=0;j<10;j++) {
	// 		if (val>maxvalue[j]) {
	// 			//插入到maxvalue中
	// 			for (int k=9;k>j;k--) {
	// 				maxvalue[k]=maxvalue[k-1];
	// 			}
	// 			maxvalue[j]=val;
	// 			break;
	// 		}
	// 	}
	// }

}
int CAbstractFSController::TakeSignalFrame(int position)
{
	int shuttertime = 0;
	m_pFSCtrl->GetExposureTime(shuttertime);
	qDebug() << "Starting TakeSignal" << " position" << position<<" ITtime:"<<shuttertime<<"\n";

	m_vecDataFrameSignal.push_back(TakeOneFrame());
	// todo: compute max ten value 计算前10个值平均值
	m_taginfolast.LastMAXValue[position]=ComputeMaxValue(m_vecDataFrameSignal.at(m_vecDataFrameSignal.size()-1));
	if (m_taginfolast.LastMAXValue[position]>=65535) {
		m_vecDataFrameSignal.at(m_vecDataFrameSignal.size()-1).bIsValid=false;
	}
	qDebug() << "Stop TakeSignal" << " Thread ID:" << m_iThreadID;
	qDebug()<< "Max 10 Average Value:" << m_taginfolast.LastMAXValue[position] << " Thread ID:" << m_iThreadID;
	//emit SignalAcqFinished(m_iThreadID, 1);
	return 0;
}

DataFrame CAbstractFSController::TakeOneFrame()
{
	using namespace ZZ_MISCDEF::IRIS;
	//int iExpTime = 0;
	DataFrame dfTemp;
// 	m_pFSCtrl->GetExposureTime(iExpTime);
// 	dfTemp.usExposureTimeInMS = iExpTime;
// 	m_pFSCtrl->GetDeviceTemperature(dfTemp.fTemperature);

	if (m_fsInfo.ucDeviceModel== DeviceModel::ISIF)
	{
		float fTemp;
		m_pFSCtrl->GetDeviceTemperature(fTemp);
		dfTemp.fTemperature = fTemp;
	}
	else if(m_fsInfo.ucDeviceModel == DeviceModel::IS1)
	{
		dfTemp.fTemperature = 0;
	}

	int iRes = m_pFSCtrl->SingleShot(dfTemp);
	if (iRes != 0)
	{
		qDebug() << "Err. SingleShot" << " Thread ID:" << m_iThreadID;
	}

	if (m_fsInfo.ucDeviceModel == DeviceModel::OSIFAlpha|| m_fsInfo.ucDeviceModel == DeviceModel::OSIFBeta)
	{
		if (m_vecNonLinearCalP.size() != 8)
		{
			qDebug() << "Err.Non Linear calibration parameters not fit.Skip..." << " Thread ID:" << m_iThreadID;
			return dfTemp;
		}
		for (int i=0;i<m_daDeviceAttr.iPixels;i++)
		{
			dfTemp.lData[i] = dfTemp.lData[i] / ( m_vecNonLinearCalP[0]                           +
				                                  m_vecNonLinearCalP[1] * dfTemp.lData[i]         +
				                                  m_vecNonLinearCalP[2] * pow(dfTemp.lData[i], 2) +
				                                  m_vecNonLinearCalP[3] * pow(dfTemp.lData[i], 3) +
				                                  m_vecNonLinearCalP[4] * pow(dfTemp.lData[i], 4) + 
				                                  m_vecNonLinearCalP[5] * pow(dfTemp.lData[i], 5) + 
				                                  m_vecNonLinearCalP[6] * pow(dfTemp.lData[i], 6) + 
				                                  m_vecNonLinearCalP[7] * pow(dfTemp.lData[i], 7)
				                                 );
		}
	}


	return dfTemp;
// 	DataFrame dfTemp;
// 	int iRes = m_pFSCtrl->SingleShot(dfTemp);
// 	if (iRes != 0)
// 	{
// 		qDebug() << "Err. SingleShot" << " Thread ID:" << m_iThreadID;
// 	}
// 
// 	return dfTemp;
}



int CAbstractFSController::SaveDataFile()
{
	return 0;
}

int CAbstractFSController::LoadQEProLinearCalibrationFile()
{
	m_vecNonLinearCalP.clear();

	QDir qdirPath(m_qstrCalFilePath);
	if (!qdirPath.exists())
	{
		qDebug() << "Non-Linear Calibration Folder not exist" << " Thread ID:" << m_iThreadID;
		return 1;
	}
	QString qstrFilePath;
	
	qstrFilePath = m_qstrCalFilePath + QString("/")+QString::fromStdString(m_fsInfo.strSN)+ QString(".NLC");

	QFile qfCalFile(qstrFilePath);
	bool bRes = qfCalFile.open(QFile::ReadOnly);
	if (!bRes)
	{
		qDebug() << "Non-Linear Calibration File open Failed" << " Thread ID:" << m_iThreadID;
		return 2;
	}

	while (!qfCalFile.atEnd())
	{
		QByteArray qbData = qfCalFile.readLine();
		qbData.remove(qbData.size()-1, 1);
		m_vecNonLinearCalP.push_back(qbData.toDouble());
		//qDebug() << qbData;
	}
	qfCalFile.close();

	qDebug() <<"Non-Linear Calibration Params:"<< m_vecNonLinearCalP.size() << " Thread ID:" << m_iThreadID;



	return 0;
}

int CAbstractFSController::StartAcquisitionSignal(int position)
{
	//
	qDebug() << "Starting acq Signal" << " Thread ID:" << m_iThreadID;

// 	DataFrame struDF;
// 	int iii;
// 	m_pFSCtrl->SetExposureTime(10000000);
// 	m_pFSCtrl->GetExposureTime(iii);
// 	m_pFSCtrl->SingleShot(struDF);

	if (m_taginfolast.lastITTIME[position]==0) {
		PerformAutoExposure(position);
	}else {
		ComputExposure(position);
	}


	TakeSignalFrame(position);

	qDebug() << "Stop acq Signal" << " Thread ID:" << m_iThreadID;
	emit SignalAcqFinished_Signal(m_iThreadID, 1);
	return 0;
}

int CAbstractFSController::StartAcquisitionDark(int position)
{
	qDebug() << "Starting acq Dark" << " Thread ID:" << m_iThreadID;
	TakeDarkFrame(position);
	qDebug() << "Stop acq Dark"<< " Thread ID:" << m_iThreadID;
	emit SignalAcqFinished_Dark(m_iThreadID, 1);

	return 0;
}

int CAbstractFSController::StopAcquisition()
{
	return 0;
}

int CAbstractFSController::ClearBuffer()
{
	m_vecDataFrameDark.clear();
	m_vecDataFrameSignal.clear();
	return 0;
}

int CAbstractFSController::GetBuffer(std::vector<DataFrame> &pvecDataFrameDark, std::vector<DataFrame> &pvecDataFrameSignal)
{
	for (size_t i=0; i < m_vecDataFrameSignal.size(); i++)
	{
		pvecDataFrameSignal.push_back(m_vecDataFrameSignal[i]);
		pvecDataFrameDark.push_back(m_vecDataFrameDark[i]);
	}
	return 0;
}