#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.SetupEnvironment_M300RTK();
	SetupMessagePipe();
	return 0;
}

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

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

	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);
		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);
		qDebug() << qstrMessage;
		//return 2;
	}

	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_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 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()
{
	unsigned long ulCO2=0, ulH2O=0;
	float fTemp=-1,fPP=-1,fPB=-1;
	m_ctrlGasSensor.GetMeasuredData(ulCO2, ulH2O, fTemp, fPP, fPB);
	m_struGSDataFrame.ulCO2 = ulCO2;
	m_struGSDataFrame.ulH2O = ulH2O;
	m_struGSDataFrame.fTemp = fTemp;
	m_struGSDataFrame.fPP = fPP;
	m_struGSDataFrame.fPB = fPB;

	float fSpeed=-1, fAngle=-1;
	m_ctrlWindSensor.GetSA_NChk(fSpeed,fAngle);
	m_struUASDataFrame.fWindDirection = fAngle;
	m_struUASDataFrame.fWindSpeed = fSpeed;

	m_ctrlVehicle.GetOneDataFrame(m_struM300RTKDataFrame);
}

int CMainAcqThread::OnTimerCapture()
{
#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(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");

	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

	
	
	return 0;
}

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

	iFlagIsPathGenerated = false;

	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);
	
	return 0;
}