#include "VehicleController.h"

#include <dji_platform.h>
#include <dji_logger.h>
#include <dji_core.h>
#include <dji_aircraft_info.h>
#include <dji_fc_subscription.h>
#include <csignal>
#include "osal.h"
#include "osal_fs.h"
#include "osal_socket.h"
#include "hal_usb_bulk.h"
#include "hal_uart.h"
#include "hal_network.h"

#include "time.h"
#include "ZZ_Types_DJI.h"

#define ZZ_UNUSED(x)                                 ((x) = (x))
#define ZZ_MIN(a, b)                                 (((a) < (b)) ? (a) : (b))
#define ZZ_MAX(a, b)                                 (((a) > (b)) ? (a) : (b))
#include "dji_low_speed_data_channel.h"
#include "dji_high_speed_data_channel.h"
int VehicleController::m_siFlagIsPumpWorking = 0;
int VehicleController::m_siFlagIsStartCaptureSignalEmitted = 0;
VehicleController* VehicleController::spCaller = NULL;
char VehicleController::m_scCaptureMode = 0;

VehicleController::VehicleController(QObject* parent /*= nullptr*/)
{
    m_iFlagIsVehicleTakeoff = 0;
    m_iFlagIsVehicleCapturing = 0;
    m_iFlagIsCaptureModeInited = 0;

    m_fHeightOfHomePoint_BM = -1;
    //m_cCaptureMode = 0;

}
VehicleController::~VehicleController()
{

}



T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveFlightStatusCallback(const uint8_t* ccData, uint16_t sDataSize, const T_DjiDataTimestamp* tDjiTimestamp)
{
    return 0;
}

T_DjiReturnCode VehicleController::DjiTest_FcSubscriptionReceiveHeightCallback(const uint8_t* data, uint16_t dataSize, const T_DjiDataTimestamp* timestamp)
{
    float fHeight = *((T_DjiFcSubscriptionAltitudeFused*)data);
    //qDebug() <<"[Fused Height]:"<< fHeight;

    ///Pump need to be added
    if (m_siFlagIsPumpWorking==0&& fHeight > 10)
	{
		//system("sudo gpio mode 7 out");
        m_siFlagIsPumpWorking = 1;
		system("sudo gpio write 7 1");
        qDebug() << "[Fused Height]:" << fHeight;
    }
    if (m_siFlagIsPumpWorking == 1 && fHeight < 10)
    {
		//system("sudo gpio mode 7 out");
        m_siFlagIsPumpWorking = 0;
		system("sudo gpio write 7 0");
        qDebug() << "[Fused Height]:" << fHeight;
    }

    //T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler();
    //osalHandler->TaskSleepMs(2000);
    return 0;
}

T_DjiReturnCode VehicleController::ZZ_DjiWaypointV2EventCallback(T_DjiWaypointV2MissionEventPush eventData)
{
    if (spCaller==NULL)
    {
        qDebug() << "ZZ_DjiWaypointV2EventCallback Fatal Error.spCaller Undefined";
        return -1;
    }
	if (eventData.event == 0x10)
    {
        if (eventData.data.waypointIndex==0 && m_siFlagIsStartCaptureSignalEmitted==0)
        {
            if (m_scCaptureMode==0)
            {
                m_siFlagIsStartCaptureSignalEmitted++;
				spCaller->emit Signal_StartCapture();
            }
           // emit Signal_StartCapture();

        }
	}
	if (eventData.event == 0x11)
	{
        qDebug() << "eventData.event" << m_siFlagIsStartCaptureSignalEmitted;
		if (m_siFlagIsStartCaptureSignalEmitted > 0)
		{
			m_siFlagIsStartCaptureSignalEmitted = 0;
			spCaller->emit Signal_StopCapture();
		}
	}
    return 0;
}

int VehicleController::Initialize()
{
    return 0;
}
static T_DjiReturnCode DjiUser_LocalWrite(const uint8_t *data, uint16_t dataLen)
{

		return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;

}
#include<dji_power_management.h>
#include<test_power_management.h>
static T_DjiReturnCode DjiTest_HighPowerApplyPinInit()
{
	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

static T_DjiReturnCode DjiTest_WriteHighPowerApplyPin(E_DjiPowerManagementPinState pinState)
{
	//attention: please pull up the HWPR pin state by hardware.
	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
int VehicleController::SetupEnvironment_M300RTK()
{
    using namespace ZZ::Device::DJI;

    spCaller = this;

    //for test
	//ZZ_ConfigParser_M300RTK clsConfigParser;
    //clsConfigParser.GetParams(m_struAppRegInfo, m_struHardwareInfo, m_struUIConfig);
    //
    //for use
    m_clsConfigParser.GetParams(m_struAppRegInfo, m_struHardwareInfo, m_struUIConfig);

 T_DjiReturnCode returnCode;

    /*********************** OSAL HANDLER *************************/
    T_DjiOsalHandler osalHandler;
    memset(&osalHandler, 0, sizeof(T_DjiOsalHandler));
	T_DjiHalUsbBulkHandler usbBulkHandler = { 0 };
    osalHandler.TaskCreate           = Osal_TaskCreate;
    osalHandler.TaskDestroy          = Osal_TaskDestroy;
    osalHandler.TaskSleepMs          = Osal_TaskSleepMs;
    osalHandler.MutexCreate          = Osal_MutexCreate;
    osalHandler.MutexDestroy         = Osal_MutexDestroy;
    osalHandler.MutexLock            = Osal_MutexLock;
    osalHandler.MutexUnlock          = Osal_MutexUnlock;
    osalHandler.SemaphoreCreate      = Osal_SemaphoreCreate;
    osalHandler.SemaphoreDestroy     = Osal_SemaphoreDestroy;
    osalHandler.SemaphoreWait        = Osal_SemaphoreWait;
    osalHandler.SemaphoreTimedWait   = Osal_SemaphoreTimedWait;
    osalHandler.SemaphorePost        = Osal_SemaphorePost;
    osalHandler.Malloc               = Osal_Malloc;
    osalHandler.Free                 = Osal_Free;
    osalHandler.GetRandomNum         = Osal_GetRandomNum;
    osalHandler.GetTimeMs            = Osal_GetTimeMs;
    osalHandler.GetTimeUs            = Osal_GetTimeUs;

    /*********************** LOGGER *************************/
    T_DjiLoggerConsole loggerConsole;
    memset(&loggerConsole, 0, sizeof(T_DjiLoggerConsole));

    loggerConsole.func           = DjiUser_PrintMessage;
    loggerConsole.consoleLevel   = DJI_LOGGER_CONSOLE_LOG_LEVEL_INFO;
    loggerConsole.isSupportColor = true;

    /*********************** UART *************************/
    T_DjiHalUartHandler uartHandler;
    memset(&uartHandler, 0, sizeof(T_DjiHalUartHandler));

    uartHandler.UartInit          = HalUart_Init;
    uartHandler.UartDeInit        = HalUart_DeInit;
    uartHandler.UartWriteData     = HalUart_WriteData;
    uartHandler.UartReadData      = HalUart_ReadData;
    uartHandler.UartGetStatus     = HalUart_GetStatus;
    uartHandler.UartGetDeviceInfo = HalUart_GetDeviceInfo;

    /*********************** NETWORK *************************/
    T_DjiHalNetworkHandler networkHandler;
    memset(&networkHandler, 0, sizeof(T_DjiHalNetworkHandler));

    networkHandler.NetworkInit          = HalNetWork_Init;
    networkHandler.NetworkDeInit        = HalNetWork_DeInit;
    networkHandler.NetworkGetDeviceInfo = HalNetWork_GetDeviceInfo;

    /*********************** FILE SYSTEM *************************/
    T_DjiFileSystemHandler fileSystemHandler;
    memset(&fileSystemHandler, 0, sizeof(T_DjiFileSystemHandler));

    fileSystemHandler.FileOpen   = Osal_FileOpen;
    fileSystemHandler.FileClose  = Osal_FileClose;
    fileSystemHandler.FileWrite  = Osal_FileWrite;
    fileSystemHandler.FileRead   = Osal_FileRead;
    fileSystemHandler.FileSync   = Osal_FileSync;
    fileSystemHandler.FileSeek   = Osal_FileSeek;
    fileSystemHandler.DirOpen    = Osal_DirOpen;
    fileSystemHandler.DirClose   = Osal_DirClose;
    fileSystemHandler.DirRead    = Osal_DirRead;
    fileSystemHandler.Mkdir      = Osal_Mkdir;
    fileSystemHandler.Unlink     = Osal_Unlink;
    fileSystemHandler.Rename     = Osal_Rename;
    fileSystemHandler.Stat       = Osal_Stat;

    /*********************** SOCKET *************************/
    T_DjiSocketHandler socketHandler;
    memset(&socketHandler, 0, sizeof(T_DjiSocketHandler));

    socketHandler.Socket        = Osal_Socket;
    socketHandler.Bind          = Osal_Bind;
    socketHandler.Close         = Osal_Close;
    socketHandler.UdpSendData   = Osal_UdpSendData;
    socketHandler.UdpRecvData   = Osal_UdpRecvData;
    socketHandler.TcpListen     = Osal_TcpListen;
    socketHandler.TcpAccept     = Osal_TcpAccept;
    socketHandler.TcpConnect    = Osal_TcpConnect;
    socketHandler.TcpSendData   = Osal_TcpSendData;
    socketHandler.TcpRecvData   = Osal_TcpRecvData;
	usbBulkHandler.UsbBulkInit = HalUsbBulk_Init;
	usbBulkHandler.UsbBulkDeInit = HalUsbBulk_DeInit;
	usbBulkHandler.UsbBulkWriteData = HalUsbBulk_WriteData;
	usbBulkHandler.UsbBulkReadData = HalUsbBulk_ReadData;
	usbBulkHandler.UsbBulkGetDeviceInfo = HalUsbBulk_GetDeviceInfo;

	returnCode = DjiPlatform_RegOsalHandler(&osalHandler);
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "VehicleController: Func DjiPlatform_RegOsalHandler. Register osal handler error";
		throw std::runtime_error("Register osal handler error.");
	}



	// if (m_struHardwareInfo.enumHCM == HardwareConnectionMode::UartAndNetwork)
	// {
	// 	returnCode = DjiPlatform_RegHalNetworkHandler(&networkHandler);
	// 	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	// 	{
	// 		qDebug() << "VehicleController: Func DjiPlatform_RegHalNetworkHandler. Register hal network handler error";
	// 		throw std::runtime_error("Register hal network handler error");
	// 	}
	// }
	if (m_struHardwareInfo.AirplaneType=="M350"||m_struHardwareInfo.AirplaneType == "M300")
    {
		returnCode = DjiPlatform_RegHalUartHandler(&uartHandler);
		if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
			qDebug() << "VehicleController: Func DjiPlatform_RegHalUartHandler. Register hal uart handler error";
			throw std::runtime_error("Register hal uart handler error.");
		}
    	//����
    	 returnCode = DjiPlatform_RegHalNetworkHandler(&networkHandler);
    	 if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    	 	printf("register hal network handler error");
    	 	return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
    	 }
	    //
    	// //Attention: if you want to use camera stream view function, please uncomment it.
    	returnCode = DjiPlatform_RegSocketHandler(&socketHandler);
    	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    		printf("register osal socket handler error");
    		return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
    	}
    }else {
    	// returnCode = DjiPlatform_RegHalNetworkHandler(&networkHandler);
    	// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    	// 	printf("register hal network handler error");
    	// 	return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
    	// }
    	// // //Attention: if you want to use camera stream view function, please uncomment it.
    	// returnCode = DjiPlatform_RegSocketHandler(&socketHandler);
    	// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    	// 	printf("register osal socket handler error");
    	// 	return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
    	// }
    		returnCode = DjiPlatform_RegHalUsbBulkHandler(&usbBulkHandler);
    		if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
    			printf("register hal usb bulk handler error");
    			return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
    		}
    }


	returnCode = DjiPlatform_RegFileSystemHandler(&fileSystemHandler);
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "VehicleController: Func DjiPlatform_RegFileSystemHandler. Register osal filesystem handler error";
		throw std::runtime_error("Register osal filesystem handler error.");
	}

	returnCode = DjiLogger_AddConsole(&loggerConsole);
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "VehicleController: Func DjiPlatform_RegFileSystemHandler. Register osal filesystem handler error";
		throw std::runtime_error("Add printf console error.");
	}



	//
	// returnCode = DjiLogger_AddConsole(&localRecordConsole);
	// if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
	// 	printf("add printf console error");
	// 	return DJI_ERROR_SYSTEM_MODULE_CODE_SYSTEM_ERROR;
	// }
	return 0;
}


int VehicleController::StartupPSDK_M300RTK()
{
	T_DjiUserInfo userInfo;
	T_DjiReturnCode returnCode;
	T_DjiAircraftInfoBaseInfo aircraftInfoBaseInfo;
    T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler();

	//signal(SIGTERM, DjiUser_NormalExitHandler);

	returnCode = LoadUserAppInfo(&userInfo);
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
		throw std::runtime_error("Fill user info error, please check user info config.");
	}
	returnCode = DjiCore_SetSerialNumber("IRIS00001");
	returnCode = DjiCore_Init(&userInfo);
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
		throw std::runtime_error("Core init error.");
	}

	returnCode = DjiAircraftInfo_GetBaseInfo(&aircraftInfoBaseInfo);
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
		throw std::runtime_error("Get aircraft base info error.");
	}

// 	if (aircraftInfoBaseInfo.mountPosition != DJI_MOUNT_POSITION_EXTENSION_PORT)
//     {
// 		throw std::runtime_error("Please run this sample on extension port.");
// 	}

	returnCode = DjiCore_SetAlias("PSDK_APPALIAS");
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
		throw std::runtime_error("Set alias error.");
	}

	returnCode = DjiCore_ApplicationStart();
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
		throw std::runtime_error("Start sdk application error.");
	}

	T_DjiTestApplyHighPowerHandler applyHighPowerHandler = {
		.pinInit = DjiTest_HighPowerApplyPinInit,
		.pinWrite = DjiTest_WriteHighPowerApplyPin,
	};

	returnCode = DjiTest_RegApplyHighPowerHandler(&applyHighPowerHandler);
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
		USER_LOG_ERROR("regsiter apply high power handler error");
	}

	returnCode = DjiTest_PowerManagementStartService();
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
		USER_LOG_ERROR("power management init error");
	}
   osalHandler->TaskSleepMs(5000);
	SetupWidget();//<2F>?<EFBFBD><C9BC>?<EFBFBD><DFBC>��<EFBFBD>
   osalHandler->TaskSleepMs(5000);
    SetupMessagePipe();//widget<65><74>vehicle
    // SetupWaypointStatusCallback();
     //InitSystemParams();//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>?<EFBFBD>?<EFBFBD><E4A3AC><EFBFBD><EFBFBD>???<CDB3>??<E4A3AC><C8BB>?<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>?<EFBFBD><C4A3><EFBFBD>?<EFBFBD>?<EFBFBD>??<CDAC><CAB1><EFBFBD><EFBFBD>5<EFBFBD><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    //?<><CAB5><EFBFBD><EFBFBD>?<EFBFBD>?<EFBFBD><C4B6>?<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD>?<EFBFBD><CCAB><EFBFBD><EFBFBD>


	qDebug()<<"M300RTK PSDK Channel Started.";
	m_clsWidget.stratmessageflash();
	SetupSubscriptions();
	return 0;
}

int VehicleController::UpdateUIConfig()
{
    m_clsWidget.GetSettings(m_struUIConfig);
    m_clsConfigParser.UpdateUIConfig(m_struUIConfig);
    return 0;
}

int VehicleController::GetOneDataFrame(M300RTKDataFrame& M300RTKDataFrame)
{

    T_DjiReturnCode tDjiReturnCode;
    T_DjiFcSubscriptionVelocity tDjiVelocity = { 0 };
    T_DjiFcSubscriptionQuaternion tDjiQuaternion = { 0 };
    T_DjiFcSubscriptionGpsPosition tDjiGpsPosition = { 0 };
    T_DjiDataTimestamp tDjiTimestamp = { 0 };
    T_DjiFcSubscriptionAltitudeFused tDjiAltFused = { 0 };

    tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY,(uint8_t*)&tDjiVelocity,sizeof(T_DjiFcSubscriptionVelocity),&tDjiTimestamp);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
        qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY";
        //return 1;
	}

	tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, (uint8_t*)&tDjiGpsPosition, sizeof(T_DjiFcSubscriptionGpsPosition), &tDjiTimestamp);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY";
		//return 2;
	}

	tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION, (uint8_t*)&tDjiQuaternion, sizeof(T_DjiFcSubscriptionQuaternion), &tDjiTimestamp);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION";
		//return 3;
	}

	tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION, (uint8_t*)&tDjiQuaternion, sizeof(T_DjiFcSubscriptionQuaternion), &tDjiTimestamp);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION";
		//return 3;
	}

	tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_BAROMETER, (uint8_t*)&tDjiAltFused, sizeof(T_DjiFcSubscriptionAltitudeFused), &tDjiTimestamp);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_GetLatestValueOfTopic error.DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_BAROMETER";
		//return 4;
	}

    M300RTKDataFrame.stVelocity.x = tDjiVelocity.data.x;
    M300RTKDataFrame.stVelocity.y = tDjiVelocity.data.y;
    M300RTKDataFrame.stVelocity.z = tDjiVelocity.data.z;

    M300RTKDataFrame.stQuaternion.w_q0 = tDjiQuaternion.q0;
    M300RTKDataFrame.stQuaternion.x_q1 = tDjiQuaternion.q1;
    M300RTKDataFrame.stQuaternion.y_q2 = tDjiQuaternion.q2;
    M300RTKDataFrame.stQuaternion.z_q3 = tDjiQuaternion.q3;

    M300RTKDataFrame.stGPSPosition.x = tDjiGpsPosition.x;
    M300RTKDataFrame.stGPSPosition.y = tDjiGpsPosition.y;
    M300RTKDataFrame.stGPSPosition.z = tDjiGpsPosition.z;

    M300RTKDataFrame.fAltitudeFused  = tDjiAltFused;


    //////////////////////////////////////////////////////////////////////////Calc Euler From Quaternion 20230428
    M300RTKDataFrame.stEulerAngle.pitch = (dji_f64_t)asinf(-2 * tDjiQuaternion.q1 * tDjiQuaternion.q3 + 2 * tDjiQuaternion.q0 * tDjiQuaternion.q2) * 57.3;

    M300RTKDataFrame.stEulerAngle.roll  = (dji_f64_t)atan2f(2 * tDjiQuaternion.q2 * tDjiQuaternion.q3 + 2 * tDjiQuaternion.q0 * tDjiQuaternion.q1,
                                                           -2 * tDjiQuaternion.q1 * tDjiQuaternion.q1 - 2 * tDjiQuaternion.q2 * tDjiQuaternion.q2 + 1) * 57.3;

    M300RTKDataFrame.stEulerAngle.yaw   = (dji_f64_t)atan2f(2 * tDjiQuaternion.q1 * tDjiQuaternion.q2 + 2 * tDjiQuaternion.q0 * tDjiQuaternion.q3,
		                                                   -2 * tDjiQuaternion.q2 * tDjiQuaternion.q2 - 2 * tDjiQuaternion.q3 * tDjiQuaternion.q3 + 1) * 57.3;
    //////////////////////////////////////////////////////////////////////////


#ifdef ZZ_FLAG_TEST
    qDebug() << tDjiVelocity.data.x << tDjiVelocity.data.y << tDjiVelocity.data.z << tDjiGpsPosition.x << tDjiGpsPosition.y << tDjiGpsPosition.z;
#endif

    return 0;
}

int VehicleController::SetupSubscriptions()
{
	QString qstrInfo = QString("Waiting for airborn gps Time Valide");
	emit Signal_UpdateVehicleMessage(qstrInfo, 0);
    T_DjiReturnCode tDjiReturnCode;
    QString qstrWBackPath("/home/data/Settings/MainSettings.ini");

	tDjiReturnCode = DjiFcSubscription_Init();
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "init data subscription module error.";
		return 0;
	}
    else
    {
        qDebug() << "init data subscription module finished.";
    }

    // Subscribe GPS related topics for time sync and homepoint altitude
	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic GPS_SIGNAL_LEVEL error.";
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic GPS_DATE error.";
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic GPS_TIME error.";
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic ALTITUDE_OF_HOMEPOINT error.";
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

    // Subscribe other flight data topics
	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, DjiTest_FcSubscriptionReceiveHeightCallback);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION error.";
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY, DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ, NULL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY error.";
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ, NULL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION error.";
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION, DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ, NULL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION error.";
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_BAROMETER, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_BAROMETER error.";
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

    // Wait for GPS signal and sync system time
    T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler();
    bool bStop = false;
#ifdef ZZ_FLAG_TEST
    bStop = 1;
#endif

    T_DjiFcSubscriptionGpsSignalLevel tDjiGpsSignalLevel = { 0 };
    T_DjiDataTimestamp tTimestamp = { 0 };

    osalHandler->TaskSleepMs(5000);
    QTime qtStart = QTime::currentTime();
    int iRetryTime = 0;

    while (!bStop)
    {
        if (iRetryTime > 60)
        {
            bStop = 1;
        }
        osalHandler->TaskSleepMs(2000);
        DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL, (uint8_t*)&tDjiGpsSignalLevel, sizeof(T_DjiFcSubscriptionGpsSignalLevel), &tTimestamp);
        qDebug() << "GPS_SIGNAL_LEVEL:" << tDjiGpsSignalLevel;
        QTime qtElapsed = QTime::currentTime();
        if (tDjiGpsSignalLevel > 1)
        {
            bStop = 1;
        }
        else
        {
            QString qstrInfo = QString("Warning:Weak GPS signal, Waiting....GPS_SIGNAL_LEVEL:%1, Elapsed time:%2").arg(tDjiGpsSignalLevel).arg(qtStart.msecsTo(qtElapsed));
            emit Signal_UpdateVehicleMessage(qstrInfo, 0);
        }
        iRetryTime++;
    }


    // Get GPS datetime and sync system time
    T_DjiFcSubscriptionGpsDate tGpsDate = { 0 };
    T_DjiFcSubscriptionGpsTime tGpsTime = { 0 };
    T_DjiFcSubscriptionAltitudeOfHomePoint tAltOfHP = { 0 };
    T_DjiFcSubscriptionGpsPosition tDjiGpsPosition = { 0 };

    DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE, (uint8_t*)&tGpsDate, sizeof(T_DjiFcSubscriptionGpsDate), &tTimestamp);
    DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME, (uint8_t*)&tGpsTime, sizeof(T_DjiFcSubscriptionGpsTime), &tTimestamp);
    DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT, (uint8_t*)&tAltOfHP, sizeof(T_DjiFcSubscriptionAltitudeOfHomePoint), &tTimestamp);
    DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, (uint8_t*)&tDjiGpsPosition, sizeof(T_DjiFcSubscriptionGpsPosition), &tTimestamp);

    // Save homepoint altitude to config
    m_fHeightOfHomePoint_BM = tAltOfHP;
    float fHeightOfHomePoint_GPS = tDjiGpsPosition.z;
    QSettings qsMainSettings(qstrWBackPath, QSettings::Format::IniFormat);
    qsMainSettings.setValue(QString("WBACK/HeightOfHomePoint_BM"), QString("%1").arg(m_fHeightOfHomePoint_BM));
    qsMainSettings.setValue(QString("WBACK/HeightOfHomePoint_GPS"), QString("%1").arg(fHeightOfHomePoint_GPS));
    qDebug() << "HeightOfHomePoint_BM:" << m_fHeightOfHomePoint_BM;
    qDebug() << "HeightOfHomePoint_GPS:" << fHeightOfHomePoint_GPS;

    // Sync system time from GPS
    QDateTime qdtDateTime;
    QString qstrDate = QString::number(tGpsDate);
    QString qstrTime = QString::number(tGpsTime);
    if (qstrTime.length() == 5)
    {
        qstrTime = "0" + qstrTime;
    }
    QString qstrDateTime = qstrDate + qstrTime;
    qdtDateTime = QDateTime::fromString(qstrDateTime, "yyyyMMddhhmmss");

    if (qdtDateTime.isValid())
    {
        QString command = "date -s \"" + qdtDateTime.toString("yyyy-MM-dd hh:mm:ss") + "\"";
        system(command.toLatin1().data());

        time_t stdTime = qdtDateTime.toTime_t();
        stime(&stdTime);
        qDebug() << "System time synced from GPS:" << qdtDateTime.toString("yyyy-MM-dd hh:mm:ss");
    }
	qstrInfo = QString("begin system");
	emit Signal_UpdateVehicleMessage(qstrInfo, 0);
    return 0;
}

int VehicleController::SetupWaypointStatusCallback()
{
	T_DjiReturnCode returnCode;

	returnCode = DjiWaypointV2_Init();
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
		qDebug()<<"Init waypoint V2 module error, error code:"<< returnCode;
		return returnCode;
	}

	returnCode = DjiWaypointV2_RegisterMissionEventCallback(ZZ_DjiWaypointV2EventCallback);
	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
        qDebug() <<"Register waypoint V2 event failed, error code:" << returnCode;
        return returnCode;
	}
    return 0;
}

// int VehicleController::TransSignal_StartCapture(VehicleController* pCaller)
// {
//     pCaller->emit Signal_StartCapture();
//     return 0;
// }

int VehicleController::LoadUserAppInfo(T_DjiUserInfo* struDjiUserInfo)
{
	using namespace ZZ::Device::DJI::M300RTK;

	memset(struDjiUserInfo->appName,          0, sizeof(struDjiUserInfo->appName));
	memset(struDjiUserInfo->appId,            0, sizeof(struDjiUserInfo->appId));
	memset(struDjiUserInfo->appKey,           0, sizeof(struDjiUserInfo->appKey));
	memset(struDjiUserInfo->appLicense,       0, sizeof(struDjiUserInfo->appLicense));
	memset(struDjiUserInfo->developerAccount, 0, sizeof(struDjiUserInfo->developerAccount));
	memset(struDjiUserInfo->baudRate,         0, sizeof(struDjiUserInfo->baudRate));

	if (m_struAppRegInfo.qstrUserAppName.length() >= sizeof(struDjiUserInfo->appName) ||
        m_struAppRegInfo.qstrUserAppID.length()   > sizeof(struDjiUserInfo->appId) ||
        m_struAppRegInfo.qstrUserAppKey.length()  > sizeof(struDjiUserInfo->appKey) ||
        m_struAppRegInfo.qstrUserAppLic.length()  > sizeof(struDjiUserInfo->appLicense) ||
        m_struAppRegInfo.qstrUserAppAcc.length() >= sizeof(struDjiUserInfo->developerAccount) ||
        m_struHardwareInfo.qstrBaudRate.length()  > sizeof(struDjiUserInfo->baudRate))
    {
		qDebug()<<"VehicleController:LoadUserAppInfo.Length of user information string is beyond limit,Please check";
		return DJI_ERROR_SYSTEM_MODULE_CODE_INVALID_PARAMETER;
	}

	strncpy(struDjiUserInfo->appName, m_struAppRegInfo.qstrUserAppName.toLatin1().data(), m_struAppRegInfo.qstrUserAppName.length());
	memcpy(struDjiUserInfo->appId, m_struAppRegInfo.qstrUserAppID.toLatin1().data(), ZZ_MIN(sizeof(struDjiUserInfo->appId), m_struAppRegInfo.qstrUserAppID.length()));
	memcpy(struDjiUserInfo->appKey, m_struAppRegInfo.qstrUserAppKey.toLatin1().data(), ZZ_MIN(sizeof(struDjiUserInfo->appKey), m_struAppRegInfo.qstrUserAppKey.length()));
	memcpy(struDjiUserInfo->appLicense, m_struAppRegInfo.qstrUserAppLic.toLatin1().data(),
		ZZ_MIN(sizeof(struDjiUserInfo->appLicense), m_struAppRegInfo.qstrUserAppLic.length()));
	memcpy(struDjiUserInfo->baudRate, m_struHardwareInfo.qstrBaudRate.toLatin1().data(), ZZ_MIN(sizeof(struDjiUserInfo->baudRate), m_struHardwareInfo.qstrBaudRate.length()));
	strncpy(struDjiUserInfo->developerAccount, m_struAppRegInfo.qstrUserAppAcc.toLatin1().data(), sizeof(struDjiUserInfo->developerAccount) - 1);

    return 0;
}

T_DjiReturnCode VehicleController::DjiUser_PrintMessage(const uint8_t* data, uint16_t dataLen)
{
    QString qstrMessage((char*)data);
    qDebug() << qstrMessage;
    return 0;
}

int VehicleController::SetupMessagePipe()
{
    connect(&m_clsWidget, &ZZ_Widget_M300RTK::Signal_UpdateCaptureMode, this, &VehicleController::Slot_OnChangeCaptureMode);
    connect(&m_clsWidget, &ZZ_Widget_M300RTK::Signal_StartCapture, this, &VehicleController::Signal_StartCapture);
    connect(&m_clsWidget, &ZZ_Widget_M300RTK::Signal_StopCapture, this, &VehicleController::Signal_StopCapture);
    connect(this, &VehicleController::Signal_UpdateVehicleMessage, &m_clsWidget, &ZZ_Widget_M300RTK::Slot_UpdatePSDKFloatMessage);
    /// for test
#ifdef ZZ_FLAG_TEST
	connect(this, &VehicleController::Signal_StartCapture, this, &VehicleController::Slot_TestStartCapture);
	connect(this, &VehicleController::Signal_StopCapture, this, &VehicleController::Slot_TestStopCapture);
#endif
    return 0;
}

int VehicleController::SetupWidget()
{
     m_clsWidget.SetUIFilePath("/home/DJI/Widget",100);
	//m_clsWidget.SetUIFilePath("/home/pi/airborn/configfile/DJI/Widget",100);


    m_clsWidget.SetSettings(m_struUIConfig);
    m_clsWidget.PreparteEnvironment();
   
    return 0;
}

int VehicleController::Slot_OnChangeCaptureMode(char cMode)
{
    //ZZ_Widget_M300RTK* test = qobject_cast<ZZ_Widget_M300RTK*>(sender());
    //if (test== &m_clsWidget)
    //{
    //    qDebug() << "from ZZ_Widget_M300RTK";
    //}
  
    ///0:Auto 1:Manual
    switch (cMode)
    {
    case 0:
 		//m_clsWidget.SetUIFilePath("/home/DJI/Widget_Auto/", 100);
 		//m_clsWidget.PreparteEnvironment();
        //m_clsWidget.UpdateCaptureStatus(0);
        m_scCaptureMode = 0;
        break;
    case 1:
		//m_clsWidget.SetUIFilePath("/home/DJI/Widget_Manual/", 100);
		//m_clsWidget.PreparteEnvironment();
        //m_clsWidget.UpdateCaptureStatus(1);
        m_scCaptureMode = 1;
        break;
    default:
        break;
    }
    return 0;
}

int VehicleController::Slot_TestStartCapture()
{
    qDebug() << "VehicleController Test Cap Started";
    return 0;
}

int VehicleController::Slot_TestStopCapture()
{
    qDebug() << "VehicleController Test Cap Stopped";
    return 0;
}