#include "MainAcqThread.h"
#include "unistd.h"
#include <cmath>
#include "hal_uart.h"
#define LINECOMMEND 0
#define  LINEDATASHOW 1
std::atomic<int> WorkingState;
std::atomic<int> SavingDate;
CMainAcqThread::CMainAcqThread(QObject* parent /*= nullptr*/)
{
	m_iFlagCaptureStatus = 0;
	iFlagIsPathGenerated = false;

	m_clsCapTimer.setTimerType(Qt::PreciseTimer);
	WorkingState = 0;
	SavingDate = 0;

#ifdef ZZ_FLAG_TEST
	//connect(&m_clsCapTimer, &QTimer::timeout, this, &CMainAcqThread::OnTestTimer);
#endif
	
	
	//m_clsCapTimer.start()
	
}

CMainAcqThread::~CMainAcqThread()
{
}

int CMainAcqThread::SetupContext()
{
	m_ctrlConfigParser.SetFilePath("/home/data/Settings/MainSettings.ini");
	m_ctrlConfigParser.GetParams(m_struMiscCtrls,m_struM300RTKSs,m_struSensorPort);

	m_ctrlVehicle.Initialize();
	m_ctrlVehicle.SetupEnvironment_M300RTK();
#ifdef ZZ_FLAG_TEST
	qDebug() << m_struSensorPort.qstrGasSensorPort;
	qDebug() << m_struSensorPort.qstrWindSensorPort;
	qDebug() << m_struMiscCtrls.iPumpGPIOPort;
	qDebug() << m_struM300RTKSs.qstrM300RTKSettingsFilePath;
	qDebug() << m_struM300RTKSs.qstrM300RTKWidgetFilePath;
#endif
	// m_ctrlVehicle.SetupEnvironment_M300RTK();
	//m_ctrlVehicle.SetupEnvironment();
	SetupMessagePipe();
	return 0;
}

int CMainAcqThread::StartUp()
{
	///pump control
	system("sudo gpio mode 7 out");
	system("sudo gpio write 7 0");
	// m_ctrlVehicle.StartupPSDK_M300RTK();

	m_ctrlVehicle.StartupPSDK_M300RTK();
	QString qstrMessage = "Initializing sensors,Please wait...";
	emit Signal_UpdateVehicleMessage(qstrMessage,LINECOMMEND);


	int iRes = m_ctrlGasSensor.Initialize(m_struSensorPort.qstrGasSensorPort.toStdString());
	if (iRes!=0)
	{
		qstrMessage.clear();
		qstrMessage.append("Fatal Error!!! Gas sensor not connected.Please check.");
		qstrMessage = "Fatal Error!!! Gas sensor not connected.Please check.";
		emit Signal_UpdateVehicleMessage(qstrMessage,LINECOMMEND);
		qDebug() << qstrMessage;
		//return 1;
	}

	iRes = m_ctrlWindSensor.Initialize(m_struSensorPort.qstrWindSensorPort.toStdString());
	if (iRes != 0)
	{
		qstrMessage.append("Fatal Error!!! Wind sensor not connected.Please check.");
		qstrMessage = "Fatal Error!!! Wind sensor not connected.Please check.";
		emit Signal_UpdateVehicleMessage(qstrMessage,LINECOMMEND);
		qDebug() << qstrMessage;
		//return 2;
	}

	qstrMessage.append("System ready.Waiting for signals");
	// qstrMessage = "System ready.Waiting for signals";
	// qstrMessage="123456789123456789123456789123456789123456789123456789123456789";
	emit Signal_UpdateVehicleMessage(qstrMessage,LINECOMMEND);
	return 0;
}

int CMainAcqThread::SetupMessagePipe()
{
	connect(&m_ctrlVehicle, &VehicleController::Signal_StartCapture, this, &CMainAcqThread::Slot_StartCapture);
	connect(&m_ctrlVehicle.m_clsWidget,&ZZ_Widget_M300RTK::SendCommand,this,&CMainAcqThread::GetCommand,Qt::QueuedConnection);
	connect(this, &CMainAcqThread::SendCommand, &m_ctrlVehicle.m_clsWidget, &ZZ_Widget_M300RTK::BackCommand);
	connect(&m_ctrlVehicle, &VehicleController::Signal_StopCapture,  this, &CMainAcqThread::Slot_StopCapture);

	connect(this, &CMainAcqThread::Signal_UpdateVehicleMessage, &m_ctrlVehicle, &VehicleController::Signal_UpdateVehicleMessage);

	connect(&m_clsCapTimer, &QTimer::timeout, this, &CMainAcqThread::OnTimerCapture);
	m_clsCapTimer.start(1000);
	return 0;
}



int CMainAcqThread::WindSensorCorrection()
{
	QuaternionToRotationMatrix();
	ConvertWindData();
	RotateWindVec();
	FormFixedWindData();
	return 0;
}

int CMainAcqThread::QuaternionToRotationMatrix()
{
	m_dRotationMatrix[0] = 1.0 - 2.0 * (m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.y_q2 + m_struM300RTKDataFrame.stQuaternion.z_q3 * m_struM300RTKDataFrame.stQuaternion.z_q3);
	m_dRotationMatrix[1] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.y_q2 - m_struM300RTKDataFrame.stQuaternion.z_q3 * m_struM300RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[2] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.z_q3 + m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[3] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.y_q2 + m_struM300RTKDataFrame.stQuaternion.z_q3 * m_struM300RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[4] = 1.0 - 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.x_q1 + m_struM300RTKDataFrame.stQuaternion.z_q3 * m_struM300RTKDataFrame.stQuaternion.z_q3);
	m_dRotationMatrix[5] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.z_q3 - m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[6] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.z_q3 - m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[7] = 2.0 * (m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.z_q3 + m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[8] = 1.0 - 2.0 * (m_struM300RTKDataFrame.stQuaternion.x_q1 * m_struM300RTKDataFrame.stQuaternion.x_q1 + m_struM300RTKDataFrame.stQuaternion.y_q2 * m_struM300RTKDataFrame.stQuaternion.y_q2);
// 	R[0] = r11; R[1] = r12; R[2] = r13;
// 	R[3] = r21; R[4] = r22; R[5] = r23;
// 	R[6] = r31; R[7] = r32; R[8] = r33;
	return 0;
}

int CMainAcqThread::ConvertWindData()
{
	m_fTempWindVecX = m_struUASDataFrame.fWindSpeed * cos(m_struUASDataFrame.fWindDirection / 180 * 3.14159);
	m_fTempWindVecY = m_struUASDataFrame.fWindSpeed * sin(m_struUASDataFrame.fWindDirection / 180 * 3.14159);
	m_fTempWindVecZ = 0;
	return 0;
}

int CMainAcqThread::RotateWindVec()
{
	m_fTempFixedWindVecX = m_dRotationMatrix[0] * m_fTempWindVecX + m_dRotationMatrix[1] * m_fTempWindVecY /*+ m_dRotationMatrix[2] * m_fTempWindVecZ*/;
	m_fTempFixedWindVecY = m_dRotationMatrix[3] * m_fTempWindVecX + m_dRotationMatrix[4] * m_fTempWindVecY /*+ m_dRotationMatrix[5] * m_fTempWindVecZ*/;
	m_fTempFixedWindVecZ = m_dRotationMatrix[6] * m_fTempWindVecX + m_dRotationMatrix[7] * m_fTempWindVecY /*+ m_dRotationMatrix[8] * m_fTempWindVecZ*/;
	return 0;
}



int CMainAcqThread::FormFixedWindData()
{
	m_fTempFixedWindVecX = m_fTempFixedWindVecX-m_struM300RTKDataFrame.stVelocity.x;
	m_fTempFixedWindVecY = m_fTempFixedWindVecY - m_struM300RTKDataFrame.stVelocity.y;
	m_fTempFixedWindVecZ = m_fTempFixedWindVecZ - m_struM300RTKDataFrame.stVelocity.z;

	double dVectorLength = sqrt(m_fTempFixedWindVecX * m_fTempFixedWindVecX + m_fTempFixedWindVecY * m_fTempFixedWindVecY + m_fTempFixedWindVecZ * m_fTempFixedWindVecZ);

	if (dVectorLength < 0.0001)
	{
		m_struUASDataFrame.fFixedWindDirection = 0;
		m_struUASDataFrame.fFixedWindSpeed = 0;
		return 0;
	}

	double dDotProduct = m_fTempFixedWindVecX;
	double dCosAngle = dDotProduct / dVectorLength;
	if (dCosAngle > 1.0) dCosAngle = 1.0;
	if (dCosAngle < -1.0) dCosAngle = -1.0;
	double dAngleInRadians = acos(dCosAngle);
	double angleInDegrees = dAngleInRadians * (180.0 / M_PI);

	m_struUASDataFrame.fFixedWindDirection = angleInDegrees;
	m_struUASDataFrame.fFixedWindSpeed = dVectorLength;
	return 0;
}

void CMainAcqThread::OnTestTimer()
{
	qDebug() << "entering time:"<< QTime::currentTime();
	QTime currentTime; 
	int elapsed = 0; 
	if (lastTime == QTime())
	{
		lastTime = QTime::currentTime();
	}
	else
	{
		currentTime = QTime::currentTime();
		elapsed = lastTime.msecsTo(currentTime);
		lastTime = QTime:: currentTime();
	}
			
	qDebug() << "Run.elapsed =" << elapsed << "ms";

	for (int i=0;i< 99999999;i++)
	{
		int b = i + 1;
		if (b%2==0)
		{
			b = b + 2;
		}
	}

	qDebug() << "leave time:" << QTime::currentTime();
}

int CMainAcqThread::GetData()
{
	double ulCO2=0, ulH2O=0;
	float fGasTemp=-1,fPP=-1,fPB=-1;
	m_ctrlGasSensor.GetMeasuredData(ulCO2, ulH2O, fGasTemp, fPP, fPB);
	m_struGSDataFrame.ulCO2 = ulCO2;
	m_struGSDataFrame.ulH2O = ulH2O;
	m_struGSDataFrame.fTemp = fGasTemp;
	m_struGSDataFrame.fPP = fPP;
	m_struGSDataFrame.fPB = fPB;

	float fSpeed=-1, fAngle=-1,fWindTemp=-1;
	m_ctrlWindSensor.GetSA_NChk(fSpeed,fAngle, fWindTemp);

	m_struUASDataFrame.fWindDirection = fAngle;
	m_struUASDataFrame.fWindSpeed = fSpeed;
	m_struUASDataFrame.fWindTemperature = fWindTemp;

	m_ctrlVehicle.GetOneDataFrame(m_struM300RTKDataFrame);//��ȡ����������
}

// #define ZZ_FLAG_TEST

int CMainAcqThread::OnTimerCapture()
{
	if (m_iFlagCaptureStatus >1)
	{
		return 0;
	}
#ifdef ZZ_FLAG_TEST
	qDebug() << "OnTimerCapture entering time:" << QTime::currentTime();
#endif

	//m_ctrlGasSensor.GetMeasuredData();
	//m_ctrlWindSensor.GetSA_NChk();
	//m_ctrlVehicle.GetOneDataFrame(m_struM300RTKDataFrame);
	GetData();
	WindSensorCorrection();
	
	//////////////////////////////////////////////////////////////////////////update message to controller
	//m_struGSDataFrame.ulH2O = 223;
	//m_struGSDataFrame.ulCO2 = 530;
	QString qstrMessage;
	qstrMessage = QString("CO2: %1ppm,	H20: %2ppm\r\nFixed Wind Speed: %3m/s\r\nFixed Wind Direction: %4deg").arg(m_struGSDataFrame.ulCO2).arg(m_struGSDataFrame.ulH2O)
		                                                                                  .arg(m_struUASDataFrame.fFixedWindSpeed).arg(m_struUASDataFrame.fFixedWindDirection);
	emit Signal_UpdateVehicleMessage("CO2:"+QString::number(m_struGSDataFrame.ulCO2,'f',1)+"\t H2O:"+QString::number(m_struGSDataFrame.ulH2O,'f',1),LINEDATASHOW);
	emit Signal_UpdateVehicleMessage("WS:"+QString::number(m_struUASDataFrame.fFixedWindSpeed,'f',2)+"\t D:"+QString::number(m_struUASDataFrame.fFixedWindDirection,'f',2)+"",LINEDATASHOW+1);
	// emit Signal_UpdateVehicleMessage("PP:"+QString::number(m_struGSDataFrame.fPP,'f',2)+"\t PB:"+QString::number(m_struGSDataFrame.fPB,'f',2)+"",LINEDATASHOW+2);
	emit Signal_UpdateVehicleMessage("T:"+QString::number(m_struGSDataFrame.fTemp,'f',2)+"\t PB:"+QString::number(m_struGSDataFrame.fPB,'f',2),LINEDATASHOW+2);

	// emit Signal_UpdateVehicleMessage(qstrMessage);
	//////////////////////////////////////////////////////////////////////////
#ifdef ZZ_FLAG_TEST
	qDebug() << m_struM300RTKDataFrame.stGPSPosition.x << m_struM300RTKDataFrame.stGPSPosition.y << m_struM300RTKDataFrame.stGPSPosition.z
		     << m_struM300RTKDataFrame.stVelocity.x    << m_struM300RTKDataFrame.stVelocity.y    << m_struM300RTKDataFrame.stVelocity.z;
#endif
	if (SavingDate==1)
	{
		m_ctrlData.WriteData(m_struM300RTKDataFrame, m_struGSDataFrame, m_struUASDataFrame);
	}




#ifdef ZZ_FLAG_TEST
	qDebug() << "OnTimerCapture leave time:" << QTime::currentTime();
#endif
	

	return 0;
}

int CMainAcqThread::Slot_StartCapture()
{
	// ground test
	system("sudo gpio write 7 1");

	iFlagIsPathGenerated = true;

	m_ctrlVehicle.UpdateUIConfig();

	m_ctrlData.GenerateFilePath();

	m_ctrlData.GenerateFile();

	// m_clsCapTimer.start(1000);
#ifdef ZZ_FLAG_TEST
	qDebug() << "CMainAcqThread Test Cap Started";
#endif // ZZ_FLAG_TEST

	m_iFlagCaptureStatus = 1;
	SavingDate = 1;
	
	return 0;
}

int CMainAcqThread::Slot_StopCapture()
{
	// ground test
	system("sudo gpio write 7 0");

	iFlagIsPathGenerated = false;

	SavingDate=0;
	//m_clsCapTimer.stop();

	m_ctrlData.CloseData();
#ifdef ZZ_FLAG_TEST
	qDebug() << "CMainAcqThread Test Cap Stopped";
#endif // ZZ_FLAG_TEST
	
	QString qstrMessage = QString("Capture Stopped.");
	emit Signal_UpdateVehicleMessage(qstrMessage,LINECOMMEND);
	
	return 0;
}

void CMainAcqThread::GetCommand(QString Worker, QString Command) {
	// 用qDebug打印接收到的命令
	qDebug()<<"Received Command - Worker:" << Worker << ", Command:" << Command;

	int waitebefore=120;
	int waitetime=10;
	qDebug()<<"GetCommand Worker:"<<Worker<<" Command:"<<Command;
	if (Worker=="WDA") {
		m_iFlagCaptureStatus=WADCALI;
		QThread::msleep(1100);

		//#分割字符comman
		QStringList arc=Command.split("#");
		if (arc[0]=="StartCalibrate") {
			if (arc.length()<2) {
				emit SendCommand("WDA","Finish");
				m_iFlagCaptureStatus=NORMAL;
				return;
			}
			m_ctrlWindSensor.CalibrateWDA(arc[1].toDouble());
			emit SendCommand("WDA","Finish");
			m_iFlagCaptureStatus=NORMAL;
		}


	}
	else if (Worker == "GAS") {
		m_iFlagCaptureStatus = GASGALI;
		QStringList arc = Command.split("#");
		qDebug()<<"arc is"<<arc;
		if (arc[0] == "ZeroCalibrate") {

			if (arc.length() < 2) {
					emit SendCommand("GAS", "Finish");
				// system("sudo gpio write 7 0");
				return;
			}
			if (arc[1]=="Air") {
				 system("sudo gpio write 7 1");
				qDebug()<<"Start Gas Motor now";
				QThread::sleep(waitetime);
				m_ctrlGasSensor.ZeroCalibration_Air();
				QThread::sleep(waitetime);
				emit SendCommand("GAS", "Finish");
				 system("sudo gpio write 7 0");
				qDebug()<<"Stop Gas Motor now";
			}
			else if (arc[1] == "N2") {
				 system("sudo gpio write 7 1");
				qDebug()<<"Start Gas Motor now";
				QThread::sleep(waitetime);
				m_ctrlGasSensor.ZeroCalibration_N2();
				QThread::sleep(waitetime);
				emit SendCommand("GAS", "Finish");
				 system("sudo gpio write 7 0");
				qDebug()<<"Stop Gas Motor now";
			}
			else {
				emit SendCommand("GAS", "Finish");
			}


		}
		else if (arc[0] == "SpanCalibrate") {

			if (arc.size()<3) {
				emit SendCommand("GAS", "Finish");
				m_iFlagCaptureStatus=NORMAL;
				return;
			}
			char cChannel=0;
			if (arc[1]=="CO2") {
				cChannel=0x03;
			} else if(arc[1]=="H2O") {
				cChannel=0x02;
			}
			else {
				emit SendCommand("GAS", "Finish");
				m_iFlagCaptureStatus=NORMAL;
				return;
			}
			unsigned int uiPPM=arc[2].toUInt();
			 system("sudo gpio write 7 1");
			qDebug()<<"Start Gas Motor now";
			QThread::sleep(waitebefore);
			m_ctrlGasSensor.SpanCalibration(cChannel,uiPPM);
			QThread::sleep(waitetime);
			emit SendCommand("GAS", "Finish");
			 system("sudo gpio write 7 0");
			qDebug()<<"Stop Gas Motor now";


		}

		m_iFlagCaptureStatus=NORMAL;
	}
}