#include <iostream>
#include <vector>
#include <thread>
#include <chrono>
#include <atomic>
#include <mutex>

// VzNLSDK includes
#include "../../SDK/VzNLSDK/Inc/VZNL_Common.h"
#include "../../SDK/VzNLSDK/Inc/VZNL_Types.h"
#include "../../SDK/VzNLSDK/Inc/VZNL_DetectLaser.h"
#include "../../SDK/VzNLSDK/Inc/VZNL_RGBConfig.h"
#include "../../SDK/VzNLSDK/Inc/VZNL_SwingMotor.h"

class CameraDemoTest
{
private:
    VZNLHANDLE m_hDevice;
    std::atomic<bool> m_bScanning;
    std::atomic<int> m_nScanDataCount;
    bool m_isRGBD;
    
public:
    CameraDemoTest() 
        : m_hDevice(nullptr)
        , m_bScanning(false)
        , m_nScanDataCount(0)
        , m_isRGBD(false)
    {
    }
    
    ~CameraDemoTest()
    {
        Cleanup();
    }
    
    // Initialize camera
    bool InitializeCamera(bool isRGBD)
    {
        m_isRGBD = isRGBD;
        int nErrCode = 0;

        // 初始化SDK
        SVzNLConfigParam sVzNlConfigParam;
        sVzNlConfigParam.nDeviceTimeOut = 5000; // 5 seconds timeout
        nErrCode = VzNL_Init(&sVzNlConfigParam);

        // 搜索设备
        VzNL_ResearchDevice(keSearchDeviceFlag_EthLaserRobotEye);

        // 获取搜索到的设备
        SVzNLEyeCBInfo* pCBInfo = nullptr;
        int nCount = 0;
        nErrCode = VzNL_GetEyeCBDeviceInfo(nullptr, &nCount);
        if (0 != nErrCode)
        {
            return false;
        }

        if(0 == nCount)
        {
            return false;
        }

        pCBInfo = new SVzNLEyeCBInfo[nCount];
        if (!pCBInfo) {
            return false;
        }
        
        nErrCode = VzNL_GetEyeCBDeviceInfo(pCBInfo, &nCount);
        if (0 != nErrCode) {
            delete[] pCBInfo;
            return false;
        }

        SVzNLOpenDeviceParam sVzNlOpenDeviceParam;
        sVzNlOpenDeviceParam.eEyeResolution = keVzNLEyeResolution_Unknown;
        sVzNlOpenDeviceParam.eSDKType = keSDKType_DetectLaser;
        sVzNlOpenDeviceParam.bStopStream = VzTrue;

        m_hDevice = VzNL_OpenDevice(&pCBInfo[0], &sVzNlOpenDeviceParam, &nErrCode);


        // 清理pCBInfo数组
        delete[] pCBInfo;
        pCBInfo = nullptr;


        if(nullptr == m_hDevice)
        {
            return false;
        }

        if(0 != nErrCode)
        {
            return false;
        }

        SVzNLEyeDeviceInfoEx m_sEeyCBDeviceInfo;
        m_sEeyCBDeviceInfo.sEyeCBInfo.nSize = sizeof(SVzNLEyeDeviceInfoEx);
        VzNL_GetDeviceInfo(m_hDevice, (SVzNLEyeCBInfo *)(&m_sEeyCBDeviceInfo));

        VzNL_BeginDetectLaser(m_hDevice);
        VzNL_EnableSwingMotor(m_hDevice, VzTrue);

        SVzNLVersionInfo sVersionInfo;
        VzNL_GetVersion(m_hDevice, &sVersionInfo);
        std::cout << "version : " << sVersionInfo.szSDKVersion << std::endl;

        // int nnnRet = VzNL_SetEthSendDataLength(m_pHandle, 1024);
        // LOG_DEBUG("SenddataLen ret : %d\n", nnnRet);

        //启用RGBD
        int nRGBRet = VzNL_EnableRGB(m_hDevice, isRGBD ? VzTrue : VzFalse);
        std::cout << "Enable RGB " << (isRGBD ? "true" : "false") << " ret : " << nRGBRet << std::endl;

        // 填充
        VzNL_EnableFillLaserPoint(m_hDevice, VzTrue);

        VzNL_SetDeviceStatusNotifyEx(m_hDevice, StatusCallback, this);
        
        return true;
    }
    
    // Start scanning
    bool StartScanning()
    {
        std::cout << "\n--- Starting Scan ---" << std::endl;
        
        if (!m_hDevice)
        {
            std::cout << "Error: Device not opened" << std::endl;
            return false;
        }
        
        // Clear previous data
        m_nScanDataCount = 0;
        
        // Select data type based on scan mode
        EVzResultDataType dataType;
        dataType = m_isRGBD ? keResultDataType_PointXYZRGBA : keResultDataType_Position;
        
        m_bScanning = true;
        
        // Start automatic detection
        int ret = VzNL_StartAutoDetectEx(m_hDevice, dataType, keFlipType_None, ScanDataCallback, this);
        if (ret != 0)
        {
            m_bScanning = false;
            std::cout << "Error: Failed to start scanning, error code: " << ret << std::endl;
            return false;
        }
        
        std::cout << "✓ Scanning started, data type: " << (m_isRGBD ? "RGBD" : "Position") << std::endl;
        
        return true;
    }
    
    // Stop scanning
    bool StopScanning()
    {
        std::cout << "\n--- Stopping Scan ---" << std::endl;
        
        if (!m_hDevice)
        {
            std::cout << "Error: Device not opened" << std::endl;
            return false;
        }
        
        m_bScanning = false;
        
        int ret = VzNL_StopAutoDetect(m_hDevice);
        if (ret != 0)
        {
            std::cout << "Error: Failed to stop scanning, error code: " << ret << std::endl;
            return false;
        }

        VzNL_EndDetectLaser(m_hDevice);
        
        std::cout << "✓ Scanning stopped" << std::endl;
        return true;
    }

    bool IsScanning()
    {
        return m_bScanning.load();
    }
    
    // Get scan data count
    int GetScanDataCount()
    {
        return m_nScanDataCount.load();
    }
    
    // Cleanup resources
    void Cleanup()
    {
        if (m_hDevice)
        {
            if (m_bScanning)
            {
                VzNL_StopAutoDetect(m_hDevice);
            }
            VzNL_CloseDevice(m_hDevice);
            m_hDevice = nullptr;
        }
        
        // Destroy SDK
        VzNL_Destroy();
    }
    
private:
    // Status callback function
    static void StatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pParam)
    {
        CameraDemoTest* pThis = static_cast<CameraDemoTest*>(pParam);
        if (!pThis) return;
        
        std::cout << "Device status change: ";
        switch (eStatus)
        {
        case keDeviceWorkStatus_Eye_Comming:
            std::cout << "Eye connected" << std::endl;
            break;
        case keDeviceWorkStatus_Eye_Reconnect:
            std::cout << "Eye reconnected" << std::endl;
            break;
        case keDeviceWorkStatus_Working:
            std::cout << "Working" << std::endl;
            break;
        case keDeviceWorkStatus_Offline:
            std::cout << "Offline" << std::endl;
            break;
        case keDeviceWorkStatus_Device_Swing_Finish:
            std::cout << "Swing module completed" << std::endl;
            break;
        case keDeviceWorkStatus_Device_Auto_Stop:
            std::cout << "Device auto stopped, total scan data: " << pThis->GetScanDataCount() << std::endl;
            pThis->m_bScanning = false;
            break;
        default:
            std::cout << "Unknown status: " << eStatus << std::endl;
            break;
        }
    }
    
    // Scan data callback function
    static void ScanDataCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pParam)
    {
        CameraDemoTest* pThis = static_cast<CameraDemoTest*>(pParam);
        if (!pThis || !pLaserLinePoint) return;
        
        // Increment data count
        pThis->m_nScanDataCount++;
    }
};

int main(int argc, char* argv[])
{
    std::cout << "Camera Demo Test Program (Direct VzNLSDK)" << std::endl;
    std::cout << "=========================================" << std::endl;
    
    CameraDemoTest cameraTest;
    
    // Check command line argument for RGBD mode
    bool isRGBD = (argc > 1 && std::string(argv[1]) == "rgbd");
    std::cout << "Scan mode: " << (isRGBD ? "RGBD" : "Position") << std::endl;
    
    // Initialize camera
    if (!cameraTest.InitializeCamera(isRGBD))
    {
        std::cout << "Failed to initialize camera" << std::endl;
        return -1;
    }
    
    // Start scanning
    if (!cameraTest.StartScanning())
    {
        std::cout << "Failed to start scanning" << std::endl;
        return -1;
    }
    
    // Wait for scanning to complete
    std::cout << "Scanning in progress..." << std::endl;
    while (cameraTest.IsScanning())
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    
    // Stop scanning
    cameraTest.StopScanning();
    
    // Display final results
    std::cout << "\n=== Final Results ===" << std::endl;
    std::cout << "Total scan data received: " << cameraTest.GetScanDataCount() << std::endl;
    
    std::cout << "\nAll tests completed!" << std::endl;
    return 0;
}