#include "MainAcqThread.h"
#include "unistd.h"
#include <cmath>
#include "hal_uart.h"

CMainAcqThread::CMainAcqThread(QObject* parent /*= nullptr*/)
{
	m_iFlagCaptureStatus = 0;
	iFlagIsPathGenerated = false;

	m_clsCapTimer.setTimerType(Qt::PreciseTimer);

#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);

#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.Initialize();
	m_ctrlVehicle.SetupEnvironment();
	SetupMessagePipe();

	
	//m_ctrlWindSensor.Initialize(m_ctrlVehicle.m_struSensorPort.qstrWindSensorPort.toStdString());
	//m_ctrlGasSensor.Initialize(m_ctrlVehicle.m_struSensorPort.qstrGasSensorPort.toStdString());


	return 0;
}

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

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

	int iRes = m_ctrlGasSensor.Initialize(m_ctrlVehicle.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);
		qDebug() << qstrMessage;
		//return 1;
	}

	iRes = m_ctrlWindSensor.Initialize(m_ctrlVehicle.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);
		qDebug() << qstrMessage;
		//return 2;
	}

	qstrMessage.clear();
	qstrMessage.append("Initializing finished.");
	emit Signal_UpdateVehicleMessage(qstrMessage);

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

int CMainAcqThread::SetupMessagePipe()
{
	connect(&m_ctrlVehicle, &VehicleController::Signal_StartCapture, this, &CMainAcqThread::Slot_StartCapture);
	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);
	return 0;
}



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

int CMainAcqThread::QuaternionToRotationMatrix()
{
	m_dRotationMatrix[0] = 1.0 - 2.0 * (m_struM350RTKDataFrame.stQuaternion.y_q2 * m_struM350RTKDataFrame.stQuaternion.y_q2 + m_struM350RTKDataFrame.stQuaternion.z_q3 * m_struM350RTKDataFrame.stQuaternion.z_q3);
	m_dRotationMatrix[1] = 2.0 * (m_struM350RTKDataFrame.stQuaternion.x_q1 * m_struM350RTKDataFrame.stQuaternion.y_q2 - m_struM350RTKDataFrame.stQuaternion.z_q3 * m_struM350RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[2] = 2.0 * (m_struM350RTKDataFrame.stQuaternion.x_q1 * m_struM350RTKDataFrame.stQuaternion.z_q3 + m_struM350RTKDataFrame.stQuaternion.y_q2 * m_struM350RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[3] = 2.0 * (m_struM350RTKDataFrame.stQuaternion.x_q1 * m_struM350RTKDataFrame.stQuaternion.y_q2 + m_struM350RTKDataFrame.stQuaternion.z_q3 * m_struM350RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[4] = 1.0 - 2.0 * (m_struM350RTKDataFrame.stQuaternion.x_q1 * m_struM350RTKDataFrame.stQuaternion.x_q1 + m_struM350RTKDataFrame.stQuaternion.z_q3 * m_struM350RTKDataFrame.stQuaternion.z_q3);
	m_dRotationMatrix[5] = 2.0 * (m_struM350RTKDataFrame.stQuaternion.y_q2 * m_struM350RTKDataFrame.stQuaternion.z_q3 - m_struM350RTKDataFrame.stQuaternion.x_q1 * m_struM350RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[6] = 2.0 * (m_struM350RTKDataFrame.stQuaternion.x_q1 * m_struM350RTKDataFrame.stQuaternion.z_q3 - m_struM350RTKDataFrame.stQuaternion.y_q2 * m_struM350RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[7] = 2.0 * (m_struM350RTKDataFrame.stQuaternion.y_q2 * m_struM350RTKDataFrame.stQuaternion.z_q3 + m_struM350RTKDataFrame.stQuaternion.x_q1 * m_struM350RTKDataFrame.stQuaternion.w_q0);
	m_dRotationMatrix[8] = 1.0 - 2.0 * (m_struM350RTKDataFrame.stQuaternion.x_q1 * m_struM350RTKDataFrame.stQuaternion.x_q1 + m_struM350RTKDataFrame.stQuaternion.y_q2 * m_struM350RTKDataFrame.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_struM350RTKDataFrame.stVelocity.x;
	m_fTempFixedWindVecY = m_fTempFixedWindVecY - m_struM350RTKDataFrame.stVelocity.y;
	m_fTempFixedWindVecZ = m_fTempFixedWindVecZ - m_struM350RTKDataFrame.stVelocity.z;

	double dDotProduct = m_fTempFixedWindVecX; // 计算点积
	double dVectorLength = sqrt(m_fTempFixedWindVecX * m_fTempFixedWindVecX + m_fTempFixedWindVecY * m_fTempFixedWindVecY + m_fTempFixedWindVecZ * m_fTempFixedWindVecZ); // 计算向量模长
	double dAngleInRadians = acos(dDotProduct / dVectorLength); // 计算夹角的弧度值
	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()
{
	qDebug() << "enter GetData gas";
	double dCH4 =0, dH2O =0;
	m_ctrlGasSensor.GetLastValidDataFrame(dCH4,dH2O);
	m_struGasDataFrame.dCH4 = dCH4;
	m_struGasDataFrame.dH2O = dH2O;
	//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;

	qDebug() << "enter GetData wind";
	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;

	qDebug() << "enter GetData vehicle";
	m_ctrlVehicle.GetOneDataFrame(m_struM350RTKDataFrame);//获取飞行器数据
}

int CMainAcqThread::OnTimerCapture()
{
//#ifdef ZZ_FLAG_TEST
//	qDebug() << "OnTimerCapture entering time:" << QTime::currentTime();
//#endif
	//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(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

	//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");
	qDebug() << "enter CMainAcqThread::Slot_StartCapture";
	iFlagIsPathGenerated = true;
	m_iFlagCaptureStatus = true;

	m_ctrlData.GenerateFilePath();
	m_ctrlData.GenerateFile();

	while(m_iFlagCaptureStatus)
	{
		//OnTestTimer();
		GetData();
		WindSensorCorrection();
		m_ctrlData.WriteData(m_struM350RTKDataFrame, m_struGasDataFrame, m_struUASDataFrame);

		QString qstrMessage;
		qstrMessage = QString("CH4: %1ppm,	H20: %2ppm\r\nFixed Wind Speed: %3m/s\r\nFixed Wind Direction: %4deg").arg(m_struGasDataFrame.dCH4).arg(m_struGasDataFrame.dH2O)
																						 .arg(m_struUASDataFrame.fFixedWindSpeed).arg(m_struUASDataFrame.fFixedWindDirection);
	
		emit Signal_UpdateVehicleMessage(qstrMessage);

		QCoreApplication::processEvents();
	}

	//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
	QString qstrMessage = QString("Capture Stopped---Slot_StartCapture.");
	emit Signal_UpdateVehicleMessage(qstrMessage);
	
	
	return 0;
}

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

	iFlagIsPathGenerated = false;
	m_iFlagCaptureStatus = false;
	//m_clsCapTimer.stop();
	m_ctrlData.CloseData();
	//m_ctrlData.CloseData();
#ifdef ZZ_FLAG_TEST
	qDebug() << "CMainAcqThread Test Cap Stopped";
#endif // ZZ_FLAG_TEST
	
	QString qstrMessage = QString("Capture Stop Cmd Send---Slot_StopCapture.");
	emit Signal_UpdateVehicleMessage(qstrMessage);
	
	return 0;
}