#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_M300.h"

using namespace ZZ_DATA_DEF::DJI;
#define ZZ_UNUSED(x)                                 ((x) = (x))
#define ZZ_MIN(a, b)                                 (((a) < (b)) ? (a) : (b))
#define ZZ_MAX(a, b)                                 (((a) > (b)) ? (a) : (b))

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()
{
	//m_clsWidget.GetSettings(m_struUIConfig);
	m_clsConfigParser.GetParams(m_struAppRegInfo, m_struHardwareInfo, m_struSensorPort);

	//m_clsConfigParser.UpdateUIConfig(m_struUIConfig);
    return 0;
}

int VehicleController::SetupEnvironment()
{
  //  using namespace ZZ::Device::DJI::M300RTK;

    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;
    T_DjiOsalHandler osalHandler = { 0 };
    T_DjiHalUartHandler uartHandler = { 0 };
    T_DjiHalUsbBulkHandler usbBulkHandler = { 0 };
    T_DjiLoggerConsole printConsole;
    T_DjiLoggerConsole localRecordConsole;
    T_DjiFileSystemHandler fileSystemHandler = { 0 };
    T_DjiSocketHandler socketHandler{ 0 };
    T_DjiHalNetworkHandler networkHandler = { 0 };

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

    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;

    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.GetTimeMs = Osal_GetTimeMs;
    osalHandler.GetTimeUs = Osal_GetTimeUs;
    osalHandler.GetRandomNum = Osal_GetRandomNum;

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

    usbBulkHandler.UsbBulkInit = HalUsbBulk_Init;
    usbBulkHandler.UsbBulkDeInit = HalUsbBulk_DeInit;
    usbBulkHandler.UsbBulkWriteData = HalUsbBulk_WriteData;
    usbBulkHandler.UsbBulkReadData = HalUsbBulk_ReadData;
    usbBulkHandler.UsbBulkGetDeviceInfo = HalUsbBulk_GetDeviceInfo;

    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,

	printConsole.func = DjiUser_PrintMessage;
	printConsole.consoleLevel = DJI_LOGGER_CONSOLE_LOG_LEVEL_INFO;
	printConsole.isSupportColor = false;

	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.");
	}

	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.");
	}

	if (/*m_struHardwareInfo.enumHCM == HardwareConnectionMode::UartAndUSBBulk*/1)
	{
		returnCode = DjiPlatform_RegHalUsbBulkHandler(&usbBulkHandler);
		if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
			qDebug() << "VehicleController: Func DjiPlatform_RegHalUsbBulkHandler. Register hal usb bulk handler error";
			throw std::runtime_error("Register hal usb bulk 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");
	//	}
	//}
	//else if (m_struHardwareInfo.enumHCM == HardwareConnectionMode::UartAndUSBBulk)
	//{
	//	returnCode = DjiPlatform_RegHalUsbBulkHandler(&usbBulkHandler);
	//	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	//	{
	//		qDebug() << "VehicleController: Func DjiPlatform_RegHalUsbBulkHandler. Register hal usb bulk handler error";
	//		throw std::runtime_error("Register hal usb bulk handler error.");
	//	}
	//	}
	//else if (m_struHardwareInfo.enumHCM == HardwareConnectionMode::UartOnly)
	//{
	//	returnCode = DjiPlatform_RegSocketHandler(&socketHandler);
	//	if (returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	//	{
	//		qDebug() << "VehicleController: Func DjiPlatform_RegSocketHandler. Register osal socket handler error";
	//		throw std::runtime_error("register osal socket handler error");
	//	}
	//}
	//else
	//{
	//	qDebug() << "VehicleController: Func DjiPlatform_RegSocketHandler. Register osal socket handler error";
	//	throw std::runtime_error("hardware connection configuration 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(&printConsole);
	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.");
	}


	return 0;
}

int VehicleController::StartupPSDK_M350RTK()
{
	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_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.");
	}

   osalHandler->TaskSleepMs(5000);
	SetupWidget();
   osalHandler->TaskSleepMs(5000);
    SetupMessagePipe();
   //SetupWaypointStatusCallback();
   //InitSystemParams();
   SetupSubscriptions();


	qDebug()<<"M350RTK PSDK Channel Started.";
	return 0;
}

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

int VehicleController::GetOneDataFrame(M350RTKDataFrame& M350RTKDataFrame)
{
    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;
	}

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

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

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

	M350RTKDataFrame.fAltitudeFused  = tDjiAltFused;


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

	M350RTKDataFrame.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;

	M350RTKDataFrame.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()
{
    T_DjiReturnCode tDjiReturnCode;

 	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.";
     }

 //   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;
	}


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

	//struDjiUserInfo->appName = "Project_Fiora";
	//struDjiUserInfo->appId = "151266";
	//struDjiUserInfo->appKey = "d8bc685458ca5dca9280abcdc992d93";
	//struDjiUserInfo->appLicense = "lXcHSZDwjws7SQnm3TjUINbKIyCC7438vJqiRBBhGe/ckOg3WmhhbYJC5aS/uKSRRMnB2UrqmiE6z+Ta+fBbUOmQUObSQJFrn/VHAsgGKR9nOMdrRNDDSwBbfLcheDIbJwjVmBAgk8RPcTZPH3WNtCohlRcpxuLIrG7oUWaawbLDwwwJ11hx5RqKKHn31I/CXTrbYQbJOm2Da32l/U7/BPF22eBUIJKts7aNfBBbFESNJI3p/xosmxfALkuXYKXMGBiIHk9bu+u0dT53Ddhz++tHh/oFyNQuyhCH4EtecvZu0PR3aMsmxyXbvXhAIocB5+AKZN+kykxhF8ToQS61bQ==";
	//struDjiUserInfo->developerAccount = "1033584732@qq.com";
	//struDjiUserInfo->baudRate = "460800";

	//strncpy(struDjiUserInfo->appName, "Project_Fiora", sizeof(struDjiUserInfo->appName));
	//strncpy(struDjiUserInfo->appId, "151266", sizeof(struDjiUserInfo->appId));
	//strncpy(struDjiUserInfo->appKey, "d8bc685458ca5dca9280abcdc992d93", sizeof(struDjiUserInfo->appKey));
	//strncpy(struDjiUserInfo->appLicense, "lXcHSZDwjws7SQnm3TjUINbKIyCC7438vJqiRBBhGe/ckOg3WmhhbYJC5aS/uKSRRMnB2UrqmiE6z+Ta+fBbUOmQUObSQJFrn/VHAsgGKR9nOMdrRNDDSwBbfLcheDIbJwjVmBAgk8RPcTZPH3WNtCohlRcpxuLIrG7oUWaawbLDwwwJ11hx5RqKKHn31I/CXTrbYQbJOm2Da32l/U7/BPF22eBUIJKts7aNfBBbFESNJI3p/xosmxfALkuXYKXMGBiIHk9bu+u0dT53Ddhz++tHh/oFyNQuyhCH4EtecvZu0PR3aMsmxyXbvXhAIocB5+AKZN+kykxhF8ToQS61bQ==", sizeof(struDjiUserInfo->appLicense));
	//strncpy(struDjiUserInfo->developerAccount, "1033584732@qq.com", sizeof(struDjiUserInfo->developerAccount));
	//strncpy(struDjiUserInfo->baudRate, "921600", 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_Nikira::Signal_StartCapture, this, &VehicleController::Signal_StartCapture);
    connect(&m_clsWidget, &ZZ_Widget_Nikira::Signal_StopCapture,  this, &VehicleController::Signal_StopCapture);
    connect(this, &VehicleController::Signal_UpdateVehicleMessage, &m_clsWidget, &ZZ_Widget_Nikira::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(const_cast<char*>("/home/DJI/Widget"),100);
   // m_clsWidget.SetSettings(m_struUIConfig);
    m_clsWidget.PreparteEnvironment();
   
    return 0;
}

int VehicleController::InitSystemParams()
{
	T_DjiReturnCode tDjiReturnCode;
    QString qstrWBackPath("/home/data/Settings/MainSettings.ini");


    ///Init
	tDjiReturnCode = DjiFcSubscription_Init();
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "init data subscription module error.";
		return 1;
	}
	else
	{
		qDebug() << "InitSystemDateTime started.";
	}

    ///SubscribeTopic GPS_SIGNAL_LEVEL
	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 DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL error.";
		return -1;
	}

    ///SubscribeTopic GPS_DATE
	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 DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE error.";
		return -2;
	}

    ///SubscribeTopic GPS_TIME
	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 DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME error.";
		return -3;
	}

    ///SubscribeTopic ALTITUDE_OF_HOMEPOINT
	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 DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT error.";
		return -4;
	}

    ///SubscribeTopic DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION
	tDjiReturnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "Subscribe topic DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION error.";
		return -5;
	}

    ///Make sure Gps avalible
    T_DjiOsalHandler* osalHandler = DjiPlatform_GetOsalHandler();
    bool bStop = false;
    /////for test
#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() << "DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL" << tDjiGpsSignalLevel;
        QTime qtElapsed = QTime::currentTime();
        if (tDjiGpsSignalLevel>1)
        {
            bStop = 1;
            //emit Signal_UpdatePSDKFloatMessage("");
        }
        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);
        }

        iRetryTime++;
    }
    //osalHandler->TaskSleepMs(2000);

    ///Get GPS Datetime
    T_DjiFcSubscriptionGpsDate tGpsDate = { 0 };
    T_DjiFcSubscriptionGpsTime tGpsTime = { 0 };
    ///Get Other
    T_DjiFcSubscriptionAltitudeOfHomePoint tAltOfHP = { 0 };
    T_DjiFcSubscriptionGpsPosition tDjiGpsPosition = { 0 };

    tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE, (uint8_t*)&tGpsDate, sizeof(T_DjiFcSubscriptionGpsDate), &tTimestamp);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_GetLatestValueOfTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE.";
		return -1;
	}


    tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME, (uint8_t*)&tGpsTime, sizeof(T_DjiFcSubscriptionGpsTime), &tTimestamp);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_GetLatestValueOfTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE.";
		return -2;
	}

    tDjiReturnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT, (uint8_t*)&tAltOfHP, sizeof(T_DjiFcSubscriptionAltitudeOfHomePoint), &tTimestamp);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_GetLatestValueOfTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT.";
		return -3;
	}

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

    m_fHeightOfHomePoint_BM = tAltOfHP;
    float fHeightOfHomePoint_GPS= tDjiGpsPosition.z;
    QString qstrHeightOfHomePoint_BM = QString("%1").arg(m_fHeightOfHomePoint_BM);
    QString qstrHeightOfHomePoint_GPS = QString("%1").arg(tDjiGpsPosition.z);
    //////////////////////////////////////////////////////////////////////////write back AOHP
    QSettings qsMainSettings(qstrWBackPath, QSettings::Format::IniFormat);
    qsMainSettings.setValue(QString("WBACK/HeightOfHomePoint_BM"), qstrHeightOfHomePoint_BM);
    qsMainSettings.setValue(QString("WBACK/HeightOfHomePoint_GPS"), qstrHeightOfHomePoint_GPS);
    qDebug() << qstrHeightOfHomePoint_BM  << m_fHeightOfHomePoint_BM;
    qDebug() << qstrHeightOfHomePoint_GPS << tDjiGpsPosition.z;
    //////////////////////////////////////////////////////////////////////////

    //qDebug() << "Date" << tGpsDate << "Time" << tGpsTime;

    QDateTime qdtDateTime;
    QString qstrDate, qstrTime,qstrDateTime;
    qstrDate = QString::number(tGpsDate);
    qstrTime = QString::number(tGpsTime);
    if (qstrTime.length()==5)
    {
        qstrTime = "0" + qstrTime/*+"000"*/;
    }
//     else
//     {
//         qstrTime= qstrTime + "000";
//     }
//     QDate qd;
//     qd = QDate::fromString("20121022", "yyyyMMdd");
//     QTime qt;
//     qt.fromString(qstrTime,"hhmmsszzz");
    qstrDateTime = qstrDate + qstrTime;
    qdtDateTime  = QDateTime::fromString(qstrDateTime, "yyyyMMddhhmmss");
    time_t stdTime = qdtDateTime.toTime_t();

    if (qdtDateTime.isValid())
    {
        stime(&stdTime);
    }
#ifndef ZZ_FLAG_TEST
    //stime(&stdTime);
#else
   
#endif
   
//     qDebug() << qstrDate;
//     qDebug() << qstrTime;
//     qDebug() << qd.isValid();
//     qDebug() << qd.toString("yyyyMMdd");
//     qDebug() << qt.isValid();
//     qDebug() << qt;
    qDebug() <<"[GPS Time Check]" << qdtDateTime.isValid();
    qDebug() <<"[GPS Time:]"      << qstrDateTime << "####" << qdtDateTime.toString("yyyyMMddhhmmsszzz");

    ///UnSubscribeTopic
	/*tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL.";
		return -11;
	}*/

	tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_SIGNAL_LEVEL.";
		return -11;
	}

	tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_DATE.";
		return -12;
	}

	tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_GPS_TIME.";
		return -13;
	}

	tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT.";
		return -14;
	}

	tDjiReturnCode = DjiFcSubscription_UnSubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION);
	if (tDjiReturnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		qDebug() << "DjiFcSubscription_UnSubscribeTopic module error,DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT.";
		return -15;
	}



	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;
}