#include "VizumCamera.h"
#include "utils/Logger.h"
#include <iostream>
#include <cstring>
#include <chrono>
#include <thread>
#include <algorithm>

#ifdef ENABLE_VIZUM_CAMERA
#include "VZNL_Common.h"
#include "VZNL_Types.h"
#include "VZNL_DetectLaser.h"
#include "VZNL_DetectConfig.h"
#include "VZNL_EyeConfig.h"
#include "VZNL_SwingMotor.h"
#endif

/**
 * @file VizumCamera.cpp
 * @brief 伟景智能 Vizum 结构光相机封装实现
 */

namespace StructuredLight {

#ifdef ENABLE_VIZUM_CAMERA
struct VizumCameraCallbackHooks {
    static void AutoOutputLaserLine(int dataType, void* laserLinePoint, VizumCamera* camera)
    {
        if (!camera) {
            return;
        }
        camera->HandleAutoOutputLaserLine(dataType, laserLinePoint);
    }

    static void DeviceStatusChange(int status, VizumCamera* camera)
    {
        if (!camera) {
            return;
        }
        camera->HandleDeviceStatusChange(status);
    }
};
#endif

// 静态成员初始化
std::atomic<bool> VizumCamera::s_sdkInitialized(false);
std::mutex VizumCamera::s_sdkMutex;

struct VizumCamera::Impl {
#ifdef ENABLE_VIZUM_CAMERA
    VZNLHANDLE deviceHandle = nullptr;
    SVzNLEyeCBInfo vzDeviceInfo{};
#else
    void* deviceHandle = nullptr;
#endif
    std::string ipAddress;
    SLDeviceInfo deviceInfo;
    std::atomic<bool> isConnected{false};
    std::atomic<bool> isAsyncCapturing{false};
    FrameCallback frameCallback = nullptr;
    CaptureType captureType = CaptureType::All;
    mutable std::mutex mutex;
    mutable std::mutex deviceAccessMutex;
    std::atomic<uint64_t> frameCounter{0};

    struct CachedFrameData {
        std::string frameId;
        SLImage leftImage;
        SLImage rightImage;
        PointCloud pointCloud;
        DepthMap depthMap;
        uint64_t timestamp = 0;
    } cachedFrame;

    std::vector<std::vector<Point3D>> accumulatedScanLines;
    std::vector<std::vector<PixelLR>> accumulatedScanLines2D;   // 与扫描线一一对应的双目2D坐标
    std::vector<ScanLineInfo> accumulatedLineMeta;              // 每线 encodeNo/timestamp
    std::mutex scanLineMutex;
    std::atomic<bool> autoDetectRunning{false};
    std::atomic<bool> isPaused{true};
    std::atomic<int> scanCompletionStatus{0};
    PointCloud accumulatedCloud;
};

#define m_deviceHandle m_impl->deviceHandle
#define m_vzDeviceInfo m_impl->vzDeviceInfo
#define m_ipAddress m_impl->ipAddress
#define m_deviceInfo m_impl->deviceInfo
#define m_isConnected m_impl->isConnected
#define m_isAsyncCapturing m_impl->isAsyncCapturing
#define m_frameCallback m_impl->frameCallback
#define m_captureType m_impl->captureType
#define m_mutex m_impl->mutex
#define m_deviceAccessMutex m_impl->deviceAccessMutex
#define m_frameCounter m_impl->frameCounter
#define m_cachedFrame m_impl->cachedFrame
#define m_accumulatedScanLines m_impl->accumulatedScanLines
#define m_accumulatedScanLines2D m_impl->accumulatedScanLines2D
#define m_accumulatedLineMeta m_impl->accumulatedLineMeta
#define m_scanLineMutex m_impl->scanLineMutex
#define m_autoDetectRunning m_impl->autoDetectRunning
#define m_isPaused m_impl->isPaused
#define m_scanCompletionStatus m_impl->scanCompletionStatus
#define m_accumulatedCloud m_impl->accumulatedCloud

#ifdef ENABLE_VIZUM_CAMERA
namespace {

class DetectRoiRestoreGuard
{
public:
    DetectRoiRestoreGuard(VZNLHANDLE device,
                          const SVzNLROIRect& leftROI,
                          const SVzNLROIRect& rightROI)
        : m_device(device), m_leftROI(leftROI), m_rightROI(rightROI)
    {
    }

    bool RestoreNow()
    {
        if (m_restored)
            return true;

        const int ret = VzNL_ConfigDetectROI(m_device, &m_leftROI, &m_rightROI);
        if (ret != 0) {
            char errorMsg[256] = {0};
            VzNL_GetErrorInfo(ret, errorMsg);
            LOG_ERROR("[VizumCamera] Failed to restore detection ROI: {} (error code: {})",
                      errorMsg, ret);
            return false;
        }

        m_restored = true;
        LOG_INFO("[VizumCamera] Detection ROI restored: left=[{}, {}, {}, {}], "
                 "right=[{}, {}, {}, {}]",
                 m_leftROI.left, m_leftROI.right, m_leftROI.top, m_leftROI.bottom,
                 m_rightROI.left, m_rightROI.right, m_rightROI.top, m_rightROI.bottom);
        return true;
    }

    ~DetectRoiRestoreGuard()
    {
        if (!m_restored && !RestoreNow())
            LOG_ERROR("[VizumCamera] Emergency detection ROI restoration failed");
    }

    DetectRoiRestoreGuard(const DetectRoiRestoreGuard&) = delete;
    DetectRoiRestoreGuard& operator=(const DetectRoiRestoreGuard&) = delete;

private:
    VZNLHANDLE m_device = nullptr;
    SVzNLROIRect m_leftROI{};
    SVzNLROIRect m_rightROI{};
    bool m_restored = false;
};

bool NormalizeToDeviceFrame(SLImage& image, uint32_t frameWidth, uint32_t frameHeight)
{
    if (image.empty() || frameWidth == 0 || frameHeight == 0
        || image.originX < 0 || image.originY < 0 || image.channels <= 0) {
        return false;
    }

    const uint64_t right = static_cast<uint64_t>(image.originX) + image.width;
    const uint64_t bottom = static_cast<uint64_t>(image.originY) + image.height;
    if (right > frameWidth || bottom > frameHeight)
        return false;

    if (image.originX == 0 && image.originY == 0
        && image.width == frameWidth && image.height == frameHeight) {
        return true;
    }

    const size_t bytesPerChannel = image.bitDepth > 8 ? 2u : 1u;
    const size_t pixelBytes = static_cast<size_t>(image.channels) * bytesPerChannel;
    const size_t sourceRowBytes = static_cast<size_t>(image.width) * pixelBytes;
    if (image.data.size() < sourceRowBytes * image.height)
        return false;

    SLImage fullImage;
    fullImage.width = frameWidth;
    fullImage.height = frameHeight;
    fullImage.originX = 0;
    fullImage.originY = 0;
    fullImage.channels = image.channels;
    fullImage.bitDepth = image.bitDepth;
    fullImage.data.assign(static_cast<size_t>(frameWidth) * frameHeight * pixelBytes, 0);

    const size_t targetRowBytes = static_cast<size_t>(frameWidth) * pixelBytes;
    const size_t targetXBytes = static_cast<size_t>(image.originX) * pixelBytes;
    for (uint32_t row = 0; row < image.height; ++row) {
        const size_t sourceOffset = static_cast<size_t>(row) * sourceRowBytes;
        const size_t targetOffset = (static_cast<size_t>(image.originY) + row) * targetRowBytes
                                  + targetXBytes;
        std::memcpy(fullImage.data.data() + targetOffset,
                    image.data.data() + sourceOffset, sourceRowBytes);
    }

    image = std::move(fullImage);
    return true;
}

const char* DeviceStatusName(int status)
{
    switch (static_cast<EVzDeviceWorkStatus>(status)) {
        case keDeviceWorkStatusUnknown: return "Unknown";
        case keDeviceWorkStatus_Eye_Comming: return "Eye_Comming";
        case keDeviceWorkStatus_Eye_Reconnect: return "Eye_Reconnect";
        case keDeviceWorkStatus_Working: return "Working";
        case keDeviceWorkStatus_Offline: return "Offline";
        case keDeviceWorkStatus_Device_Swing_Finish: return "Device_Swing_Finish";
        case keDeviceWorkStatus_Device_Auto_Stop: return "Device_Auto_Stop";
        case keDeviceWorkStatus_Device_Swing_Running: return "Device_Swing_Running";
        default: return "Other";
    }
}

void AutoOutputLaserLineExCB(EVzResultDataType eDataType,
                             SVzLaserLineData* pLaserLinePoint,
                             void* pParam)
{
    VizumCameraCallbackHooks::AutoOutputLaserLine(
        static_cast<int>(eDataType), pLaserLinePoint, static_cast<VizumCamera*>(pParam));
}

void DeviceStatusCB(EVzDeviceWorkStatus eStatus, void* pParam)
{
    VizumCameraCallbackHooks::DeviceStatusChange(
        static_cast<int>(eStatus), static_cast<VizumCamera*>(pParam));
}

} // namespace
#endif

void VizumCamera::HandleAutoOutputLaserLine(int dataType, void* laserLinePoint)
{
#ifdef ENABLE_VIZUM_CAMERA
    static bool firstCallback = true;
    if (firstCallback) {
        firstCallback = false;
        LOG_INFO("[VizumCamera] Received first callback, eDataType={}", dataType);
    }

    if (dataType == static_cast<int>(keResultDataType_PointXYZRGBA)) {
        ConvertLaserLineDataToPointCloud(laserLinePoint);
    } else {
        LOG_WARN("[VizumCamera] Callback received with unexpected data type: {}", dataType);
    }
#else
    (void)dataType;
    (void)laserLinePoint;
#endif
}

void VizumCamera::HandleDeviceStatusChange(int status)
{
#ifdef ENABLE_VIZUM_CAMERA
    const char* statusName = DeviceStatusName(status);
    LOG_INFO("[VizumCamera] Device status changed: {} ({})", statusName, status);

    const bool completesScan =
        status == static_cast<int>(keDeviceWorkStatus_Device_Swing_Finish)
        || status == static_cast<int>(keDeviceWorkStatus_Device_Auto_Stop);
    if (completesScan) {
        if (!m_autoDetectRunning.load()) {
            LOG_INFO("[VizumCamera] Completion status {} ({}) received outside an active "
                     "scan wait; ignored",
                     statusName, status);
            return;
        }
        size_t scanLines = 0;
        size_t validPoints = 0;
        {
            std::lock_guard<std::mutex> lock(m_scanLineMutex);
            scanLines = m_accumulatedScanLines.size();
            validPoints = m_accumulatedCloud.points.size();
        }
        m_scanCompletionStatus = status;
        LOG_INFO("[VizumCamera] Scan completion status received: {} ({}); "
                 "releasing scan wait, scan lines={}, valid points={}",
                 statusName, status, scanLines, validPoints);
        m_isPaused = true;
        return;
    }

    if (status == static_cast<int>(keDeviceWorkStatus_Offline)) {
        LOG_WARN("[VizumCamera] Camera went offline");
    } else if (status == static_cast<int>(keDeviceWorkStatus_Device_Swing_Running)) {
        LOG_INFO("[VizumCamera] Camera swing motor started running");
    } else if (status == static_cast<int>(keDeviceWorkStatus_Eye_Reconnect)) {
        LOG_INFO("[VizumCamera] Camera reconnected");
    }
#else
    (void)status;
#endif
}

void VizumCamera::ConvertLaserLineDataToPointCloud(void* laserLinePoint)
{
#ifdef ENABLE_VIZUM_CAMERA
    SVzLaserLineData* pLaserLinePoint = static_cast<SVzLaserLineData*>(laserLinePoint);
    if (pLaserLinePoint == nullptr) {
        return;
    }

    int nPoints = pLaserLinePoint->nPointCount;
    
    // 检查点数量和指针有效性
    if (nPoints <= 0 || pLaserLinePoint->p3DPoint == nullptr) {
        return;
    }
    
    // 将 p3DPoint 转换为 SVzNLPointXYZRGBA* 类型
    SVzNLPointXYZRGBA* pPoints = static_cast<SVzNLPointXYZRGBA*>(pLaserLinePoint->p3DPoint);

    // keResultDataType_PointXYZRGBA 对应的 2D 数据类型为 SVzNL2DLRPoint(左右目像素坐标)
    SVzNL2DLRPoint* pPoints2D = static_cast<SVzNL2DLRPoint*>(pLaserLinePoint->p2DPoint);

    // 使用锁保护对累积点云的访问
    std::lock_guard<std::mutex> lock(m_scanLineMutex);

    // 预先分配内存
    m_accumulatedCloud.points.reserve(m_accumulatedCloud.points.size() + nPoints);

    // 同时保存扫描线数据
    std::vector<Point3D> scanLine;
    scanLine.reserve(nPoints);
    std::vector<PixelLR> scanLine2D;
    scanLine2D.reserve(nPoints);

    int validCount = 0;
    for (int i = 0; i < nPoints; ++i) {
        SVzNLPointXYZRGBA& point = pPoints[i];
        
        // 只将有效点添加到显示用的点云
        if (point.x != 0.0f || point.y != 0.0f || point.z != 0.0f) {
            Point3D pt;
            pt.x = point.x;
            pt.y = point.y;
            pt.z = point.z;
            m_accumulatedCloud.points.push_back(pt);
            validCount++;
        }
        
        // 保存所有点到扫描线数据（包括无效点），保持网格格式
        Point3D scanPt;
        scanPt.x = point.x;
        scanPt.y = point.y;
        scanPt.z = point.z;
        scanLine.push_back(scanPt);

        // 同步保存双目 2D 像素坐标(无 2D 数据时补零占位,保持与 3D 点对齐)
        PixelLR px;
        if (pPoints2D != nullptr) {
            px.leftX = pPoints2D[i].sLeft.x;
            px.leftY = pPoints2D[i].sLeft.y;
            px.rightX = pPoints2D[i].sRight.x;
            px.rightY = pPoints2D[i].sRight.y;
        }
        scanLine2D.push_back(px);
    }

    // 逐线统计只留 TRACE(SPDLOG_ACTIVE_LEVEL=DEBUG 下编译期剔除,不再刷爆日志);
    // 每次扫描的汇总信息由采集结束处的 "Organized grid built" 一条日志给出
    LOG_TRACE("[VizumCamera] 收到扫描线: {} 个点, 有效点: {}", nPoints, validCount);

    // 将扫描线添加到累积的扫描线数据中
    if (!scanLine.empty()) {
        m_accumulatedScanLines.push_back(std::move(scanLine));
        m_accumulatedScanLines2D.push_back(std::move(scanLine2D));
        ScanLineInfo meta;
        meta.pointCount = (size_t)nPoints;
        meta.timestamp = pLaserLinePoint->llTimeStamp;
        meta.encodeNo = pLaserLinePoint->nEncodeNo;
        m_accumulatedLineMeta.push_back(meta);
        LOG_TRACE("[VizumCamera] 累积扫描线数量: {}", m_accumulatedScanLines.size());
    }
#else
    (void)laserLinePoint;
#endif
}

// 获取累积的扫描线数据
std::vector<std::vector<Point3D>> VizumCamera::GetAccumulatedScanLines()
{
    std::lock_guard<std::mutex> lock(m_scanLineMutex);
    return m_accumulatedScanLines;
}

// 清除累积的扫描线数据
void VizumCamera::ClearAccumulatedScanLines()
{
    std::lock_guard<std::mutex> lock(m_scanLineMutex);
    m_accumulatedScanLines.clear();
    m_accumulatedScanLines2D.clear();
    m_accumulatedLineMeta.clear();
    m_accumulatedCloud.clear();
}

// ============================================================================
// 构造与析构
// ============================================================================

VizumCamera::VizumCamera()
    : m_impl(std::make_unique<Impl>())
{
#ifdef ENABLE_VIZUM_CAMERA
    memset(&m_vzDeviceInfo, 0, sizeof(m_vzDeviceInfo));
#endif
}

VizumCamera::~VizumCamera()
{
    Disconnect();
}

// ============================================================================
// SDK初始化/销毁
// ============================================================================

bool VizumCamera::InitSDK()
{
#ifdef ENABLE_VIZUM_CAMERA
    std::lock_guard<std::mutex> lock(s_sdkMutex);
    
    if (s_sdkInitialized) {
        return true;
    }
    
    // 与VznlSDKManager::InitializeSDK保持一致
    SVzNLConfigParam configParam;
    configParam.nDeviceTimeOut = 0;
    
    int result = VzNL_Init(&configParam);
    if (result != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(result, errorMsg);
        LOG_ERROR("[VizumCamera] SDK initialization failed: {}", errorMsg);
        return false;
    }
    
    // SDK 日志:只记警告及以上、只写文件(VizumLog/)
    // Information 级会逐帧刷屏,现场排查需要更详细时临时调回
    VzNL_SetLogLevel(keNLLogLevel_Warning, keNLLogType_File);
    
    s_sdkInitialized = true;
    LOG_INFO("[VizumCamera] SDK initialized successfully");
    return true;
#else
    LOG_ERROR("[VizumCamera] Vizum camera support not enabled");
    return false;
#endif
}

void VizumCamera::DestroySDK()
{
#ifdef ENABLE_VIZUM_CAMERA
    std::lock_guard<std::mutex> lock(s_sdkMutex);
    
    if (s_sdkInitialized) {
        VzNL_Destroy();
        s_sdkInitialized = false;
        LOG_INFO("[VizumCamera] SDK destroyed");
    }
#endif
}

// ============================================================================
// 基本操作实现
// ============================================================================

std::string VizumCamera::GetSDKVersion() const
{
#ifdef ENABLE_VIZUM_CAMERA
    if (m_deviceHandle) {
        SVzNLVersionInfo versionInfo;
        memset(&versionInfo, 0, sizeof(versionInfo));
        if (VzNL_GetVersion(m_deviceHandle, &versionInfo) == 0) {
            return std::string(versionInfo.szSDKVersion);
        }
    }
    return "Unknown";
#else
    return "Vizum SDK not enabled";
#endif
}

bool VizumCamera::SearchCameras(std::vector<SLDeviceInfo>& deviceList)
{
#ifdef ENABLE_VIZUM_CAMERA
    deviceList.clear();
    
    // 确保SDK已初始化
    if (!s_sdkInitialized) {
        if (!InitSDK()) {
            return false;
        }
    }
    
    // 搜索设备 - 使用与成功实现相同的标志
    int result = VzNL_ResearchDevice(keSearchDeviceFlag_EthLaserRobotEye);
    if (result != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(result, errorMsg);
        LOG_ERROR("[VizumCamera] Device search failed: {}", errorMsg);
        return false;
    }
    
    // 获取设备数量
    int deviceCount = 0;
    result = VzNL_GetEyeCBDeviceInfo(nullptr, &deviceCount);
    
    if (deviceCount <= 0) {
        LOG_INFO("[VizumCamera] No devices found");
        return false;
    }
    
    // 获取设备信息
    std::vector<SVzNLEyeCBInfo> vzDevices(deviceCount);
    result = VzNL_GetEyeCBDeviceInfo(vzDevices.data(), &deviceCount);
    if (result != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(result, errorMsg);
        LOG_ERROR("[VizumCamera] Failed to get device info: {}", errorMsg);
        return false;
    }
    
    // 转换为通用设备信息，并绑定未绑定的设备
    for (int i = 0; i < deviceCount; ++i) {
        // 检查设备是否有效，如果无效则尝试绑定（与成功实现一致）
        if (vzDevices[i].bValidDevice == VzFalse) {
            LOG_INFO("[VizumCamera] Device {} is not bound, attempting to bind...", 
                     std::string(vzDevices[i].byServerIP));
            
            // 获取设备配置信息
            SVzNLEthernetEyeConfigInfo ethConfigInfo;
            int configResult = VzNL_GetEthernetEyeConfigInfo(&vzDevices[i], &ethConfigInfo);
            if (configResult != 0) {
                char errorMsg[256] = {0};
                VzNL_GetErrorInfo(configResult, errorMsg);
                LOG_WARN("[VizumCamera] Failed to get ethernet eye config: {}", errorMsg);
            }
            
            // 绑定设备
            int bindResult = VzNL_BindEthernetEye(&vzDevices[i]);
            if (bindResult != 0) {
                char errorMsg[256] = {0};
                VzNL_GetErrorInfo(bindResult, errorMsg);
                LOG_WARN("[VizumCamera] Failed to bind ethernet eye: {}", errorMsg);
                continue;  // 绑定失败，跳过此设备
            }
            LOG_INFO("[VizumCamera] Device bound successfully");
        }
        
        SLDeviceInfo info;
        info.ipAddress = std::string(vzDevices[i].byServerIP);
        info.serialNumber = std::string(vzDevices[i].szGUID);
        info.model = std::string(vzDevices[i].szDeviceName);
        info.alias = std::string(vzDevices[i].szDesc);
        // 宽高暂时设置为默认值，连接后会更新
        info.width = 1280;
        info.height = 960;
        deviceList.push_back(info);
    }
    
    LOG_INFO("[VizumCamera] Found {} device(s)", deviceCount);
    return !deviceList.empty();
#else
    LOG_ERROR("[VizumCamera] Vizum camera support not enabled");
    return false;
#endif
}

bool VizumCamera::Connect(const std::string& ipAddress)
{
#ifdef ENABLE_VIZUM_CAMERA
    std::unique_lock<std::mutex> lock(m_mutex);
    
    if (m_isConnected) {
        if (m_ipAddress == ipAddress) {
            return true;  // 已连接到相同地址
        }
        lock.unlock();
        Disconnect();  // 断开之前的连接
        lock.lock();
    }
    
    // 确保SDK已初始化
    if (!s_sdkInitialized) {
        if (!InitSDK()) {
            return false;
        }
    }
    
    m_ipAddress = ipAddress;
    
    // 搜索设备 - 使用与成功实现相同的标志
    int result = VzNL_ResearchDevice(keSearchDeviceFlag_EthLaserRobotEye);
    if (result != 0) {
        LOG_ERROR("[VizumCamera] Device search failed during connect");
        m_ipAddress.clear();
        return false;
    }
    
    // 获取设备列表
    int deviceCount = 0;
    VzNL_GetEyeCBDeviceInfo(nullptr, &deviceCount);
    
    if (deviceCount <= 0) {
        LOG_ERROR("[VizumCamera] No devices found");
        m_ipAddress.clear();
        return false;
    }
    
    std::vector<SVzNLEyeCBInfo> vzDevices(deviceCount);
    VzNL_GetEyeCBDeviceInfo(vzDevices.data(), &deviceCount);
    
    // 查找匹配的设备
    bool found = false;
    for (int i = 0; i < deviceCount; ++i) {
        if (std::string(vzDevices[i].byServerIP) == ipAddress) {
            m_vzDeviceInfo = vzDevices[i];
            found = true;
            
            // 检查设备是否有效，如果无效则尝试绑定（与成功实现一致）
            if (m_vzDeviceInfo.bValidDevice == VzFalse) {
                LOG_INFO("[VizumCamera] Device is not bound, attempting to bind...");
                
                // 获取设备配置信息
                SVzNLEthernetEyeConfigInfo ethConfigInfo;
                int configResult = VzNL_GetEthernetEyeConfigInfo(&m_vzDeviceInfo, &ethConfigInfo);
                if (configResult != 0) {
                    char errorMsg[256] = {0};
                    VzNL_GetErrorInfo(configResult, errorMsg);
                    LOG_WARN("[VizumCamera] Failed to get ethernet eye config: {}", errorMsg);
                }
                
                // 绑定设备
                int bindResult = VzNL_BindEthernetEye(&m_vzDeviceInfo);
                if (bindResult != 0) {
                    char errorMsg[256] = {0};
                    VzNL_GetErrorInfo(bindResult, errorMsg);
                    LOG_ERROR("[VizumCamera] Failed to bind ethernet eye: {}", errorMsg);
                    m_ipAddress.clear();
                    return false;
                }
                LOG_INFO("[VizumCamera] Device bound successfully");
            }
            break;
        }
    }
    
    if (!found) {
        LOG_ERROR("[VizumCamera] Device with IP {} not found", ipAddress);
        m_ipAddress.clear();
        return false;
    }
    
    // 打开设备（与VznlCamera::OpenCamera保持一致，不设置额外参数）
    SVzNLOpenDeviceParam openParam{};
    // 注意：正确示例中不设置任何参数，使用默认值
    
    int errorCode = 0;
    m_deviceHandle = VzNL_OpenDevice(&m_vzDeviceInfo, &openParam, &errorCode);
    
    if (!m_deviceHandle || errorCode != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(errorCode, errorMsg);
        LOG_ERROR("[VizumCamera] Failed to open device: {}", errorMsg);
        m_ipAddress.clear();
        return false;
    }
    
    // 设置设备状态回调。静态扫描以 Device_Auto_Stop 作为官方完成信号，注册失败不可静默继续。
    const int statusNotifyResult = VzNL_SetDeviceStatusNotify(m_deviceHandle, &DeviceStatusCB, this);
    if (statusNotifyResult != 0) {
        char statusErrorMsg[256] = {0};
        VzNL_GetErrorInfo(statusNotifyResult, statusErrorMsg);
        LOG_ERROR("[VizumCamera] Failed to register device status callback: {} (error code: {})",
                  statusErrorMsg, statusNotifyResult);
        VzNL_CloseDevice(m_deviceHandle);
        m_deviceHandle = nullptr;
        m_ipAddress.clear();
        return false;
    }
    LOG_INFO("[VizumCamera] Device status callback registered; scan completion events: "
             "Device_Auto_Stop or Device_Swing_Finish");

    // OpenDevice 之后立即设置图像输出格式（在此之前设置对本次连接无效）
    VzNL_SetOutputImageFormat(keVzNLImageType_BGR888);
    LOG_INFO("[VizumCamera] Output image format set to BGR888");

    // 更新设备信息
    if (!UpdateDeviceInfo()) {
        VzNL_CloseDevice(m_deviceHandle);
        m_deviceHandle = nullptr;
        m_ipAddress.clear();
        return false;
    }

    // 初始化激光检测（非致命，与 HandEyeKit 保持一致）
    int laserResult = VzNL_BeginDetectLaser(m_deviceHandle);
    if (laserResult != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(laserResult, errorMsg);
        LOG_WARN("[VizumCamera] BeginDetectLaser failed (non-fatal): {}", errorMsg);
    } else {
        LOG_INFO("[VizumCamera] Laser detection initialized");
    }

    // 切到 Master 主动模式 — 避免设备停留在外触发模式导致无法取图/设参数（非致命）
    {
        const int trigResult = VzNL_SetTriggerMode(m_deviceHandle, keEyeTriggerMode_Master);
        if (trigResult != 0) {
            char errorMsg[256] = {0};
            VzNL_GetErrorInfo(trigResult, errorMsg);
            LOG_WARN("[VizumCamera] SetTriggerMode(Master) failed (non-fatal): {}", errorMsg);
        } else {
            LOG_INFO("[VizumCamera] Trigger mode set to Master");
        }
    }

    // 忽略外部触发使能信号（非致命）
    {
        const int ignResult = VzNL_IgnoreTriggerExtSignal(m_deviceHandle, VzTrue);
        if (ignResult != 0) {
            char errorMsg[256] = {0};
            VzNL_GetErrorInfo(ignResult, errorMsg);
            LOG_WARN("[VizumCamera] IgnoreTriggerExtSignal failed (non-fatal): {}", errorMsg);
        } else {
            LOG_INFO("[VizumCamera] External trigger signal ignored");
        }
    }
    
    m_isConnected = true;
    LOG_INFO("[VizumCamera] Connected to camera at {}", ipAddress);
    return true;
#else
    LOG_ERROR("[VizumCamera] Vizum camera support not enabled");
    return false;
#endif
}

void VizumCamera::Disconnect()
{
#ifdef ENABLE_VIZUM_CAMERA
    std::lock_guard<std::mutex> lock(m_mutex);
    const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
    
    if (m_isAsyncCapturing) {
        StopAsyncCapture();
    }
    
    if (m_deviceHandle) {
        // 结束激光检测（与VZNLCameraAdapter保持一致）
        VzNL_EndDetectLaser(m_deviceHandle);
        VzNL_StopCapture(m_deviceHandle);
        VzNL_CloseDevice(m_deviceHandle);
        m_deviceHandle = nullptr;
    }
    if (s_sdkInitialized) {
        VzNL_Destroy();
        s_sdkInitialized = false;
    }
    
    m_isConnected = false;
    m_ipAddress.clear();
    m_deviceInfo = SLDeviceInfo();
    memset(&m_vzDeviceInfo, 0, sizeof(m_vzDeviceInfo));
    
    LOG_INFO("[VizumCamera] Disconnected");
#endif
}

bool VizumCamera::IsConnected() const
{
    return m_isConnected;
}

bool VizumCamera::GetDeviceInfo(SLDeviceInfo& info)
{
    if (!m_isConnected) {
        return false;
    }
    
    info = m_deviceInfo;
    return true;
}

bool VizumCamera::UpdateDeviceInfo()
{
#ifdef ENABLE_VIZUM_CAMERA
    if (!m_deviceHandle) {
        return false;
    }
    
    // 获取分辨率
    SVzVideoResolution resolution;
    if (VzNL_GetResolution(m_deviceHandle, &resolution) == 0) {
        m_deviceInfo.width = resolution.nFrameWidth;
        m_deviceInfo.height = resolution.nFrameHeight;
    } else {
        m_deviceInfo.width = 1280;
        m_deviceInfo.height = 960;
    }
    
    m_deviceInfo.ipAddress = m_ipAddress;
    m_deviceInfo.serialNumber = std::string(m_vzDeviceInfo.szGUID);
    m_deviceInfo.model = std::string(m_vzDeviceInfo.szDeviceName);
    m_deviceInfo.alias = std::string(m_vzDeviceInfo.szDesc);
    
    return true;
#else
    return false;
#endif
}

// ============================================================================
// 同步采集实现
// ============================================================================

bool VizumCamera::TriggerCapture(std::string& frameId, CaptureType captureType)
{
    LOG_INFO("[VizumCamera] TriggerCapture called, captureType={}", static_cast<int>(captureType));
#ifdef ENABLE_VIZUM_CAMERA
    if (!m_isConnected || !m_deviceHandle) {
        LOG_ERROR("[VizumCamera] TriggerCapture failed: Camera not connected or handle invalid");
        return false;
    }
    
    // 生成帧ID
    m_frameCounter++;
    frameId = "VZ_" + std::to_string(m_frameCounter);
    
    // 执行采集并缓存数据
    if (!CaptureAndCacheData(captureType)) {
        LOG_ERROR("[VizumCamera] CaptureAndCacheData failed");
        return false;
    }
    
    m_cachedFrame.frameId = frameId;
    LOG_INFO("[VizumCamera] TriggerCapture success, frameId={}", frameId);
    return true;
#else
    return false;
#endif
}

bool VizumCamera::CaptureAndCacheData(CaptureType captureType)
{
    LOG_INFO("[VizumCamera] CaptureAndCacheData called, captureType={}", static_cast<int>(captureType));
#ifdef ENABLE_VIZUM_CAMERA
    const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
    if (!m_deviceHandle) {
        LOG_ERROR("[VizumCamera] CaptureAndCacheData failed: Device handle is null");
        return false;
    }
    
    // 清空缓存
    m_cachedFrame.leftImage.clear();
    m_cachedFrame.rightImage.clear();
    m_cachedFrame.pointCloud.clear();
    m_cachedFrame.depthMap.clear();
    
    // 获取当前时间戳
    auto now = std::chrono::system_clock::now();
    m_cachedFrame.timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
        now.time_since_epoch()).count();
    
    // 采集图像
    if (captureType & CaptureType::Image) {
        SVzNLImageData* pLeftImage = nullptr;
        SVzNLImageData* pRightImage = nullptr;
        
        int result = VzNL_GetEyeImage(m_deviceHandle, &pLeftImage, &pRightImage, 3000);
        if (result == 0) {
            if (pLeftImage) {
                ConvertVzImageToSLImage(pLeftImage, m_cachedFrame.leftImage);
                VzNL_ReleaseImage(&pLeftImage);
            }
            if (pRightImage) {
                ConvertVzImageToSLImage(pRightImage, m_cachedFrame.rightImage);
                VzNL_ReleaseImage(&pRightImage);
            }
        } else {
            char errorMsg[256] = {0};
            VzNL_GetErrorInfo(result, errorMsg);
            LOG_ERROR("[VizumCamera] Failed to get image: {}", errorMsg);
        }
    }
    
    // 采集点云（通过激光扫描）
    // 按照 VZNLCameraAdapter::scan 的逻辑，使用自动检测模式采集
    if (captureType & CaptureType::PointCloud) {
        LOG_INFO("[VizumCamera] CaptureAndCacheData: Requesting point cloud via CapturePointCloudWithAutoDetect...");
        
        PointCloud pc;
        // 使用默认超时或根据需要调整
        if (CapturePointCloudWithAutoDetect(pc, 3000)) {
            m_cachedFrame.pointCloud = std::move(pc);
            LOG_INFO("[VizumCamera] CaptureAndCacheData: Successfully captured {} points", m_cachedFrame.pointCloud.points.size());
        } else {
            LOG_WARN("[VizumCamera] CaptureAndCacheData: Failed to capture point cloud");
        }
    }
    
    return true;
#else
    return false;
#endif
}

#ifdef ENABLE_VIZUM_CAMERA
bool VizumCamera::ConvertVzImageToSLImage(void* vzImagePtr, SLImage& slImage)
{
    SVzNLImageData* vzImage = static_cast<SVzNLImageData*>(vzImagePtr);
    if (!vzImage || !vzImage->pBuffer) {
        return false;
    }
    
    slImage.width = vzImage->nWidth;
    slImage.height = vzImage->nHeight;
    slImage.originX = vzImage->ptOriPos.x;
    slImage.originY = vzImage->ptOriPos.y;
    slImage.channels = vzImage->nChannels;
    slImage.bitDepth = vzImage->byBitDepth;
    
    size_t dataSize = vzImage->nWidth * vzImage->nHeight * vzImage->nChannels;
    if (vzImage->byBitDepth == 16) {
        dataSize *= 2;
    }
    
    slImage.data.resize(dataSize);
    memcpy(slImage.data.data(), vzImage->pBuffer, dataSize);
    
    return true;
}
#endif

bool VizumCamera::GetImage(const std::string& frameId, SLImage& image)
{
    if (m_cachedFrame.frameId != frameId) {
        return false;
    }
    
    // 返回左目图像作为主图像
    image = m_cachedFrame.leftImage;
    return !image.empty();
}

bool VizumCamera::GetLeftImage(SLImage& image)
{
    image = m_cachedFrame.leftImage;
    return !image.empty();
}

bool VizumCamera::GetRightImage(SLImage& image)
{
    image = m_cachedFrame.rightImage;
    return !image.empty();
}

bool VizumCamera::CaptureFullFrameLeftImage(SLImage& image, std::string& frameId)
{
    image.clear();
    frameId.clear();
#ifdef ENABLE_VIZUM_CAMERA
    const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
    if (!m_isConnected || !m_deviceHandle) {
        LOG_ERROR("[VizumCamera] Full-frame capture failed: device not connected");
        return false;
    }

    SVzNLROIRect oldLeftROI{};
    SVzNLROIRect oldRightROI{};
    int ret = VzNL_GetConfigDetectROI(m_deviceHandle, &oldLeftROI, &oldRightROI);
    if (ret != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(ret, errorMsg);
        LOG_ERROR("[VizumCamera] Full-frame capture cannot read detection ROI: {} "
                  "(error code: {})", errorMsg, ret);
        return false;
    }

    SVzVideoResolution resolution{};
    ret = VzNL_GetResolution(m_deviceHandle, &resolution);
    if (ret != 0 || resolution.nFrameWidth == 0 || resolution.nFrameHeight == 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(ret, errorMsg);
        LOG_ERROR("[VizumCamera] Full-frame capture cannot read device resolution: {} "
                  "(error code: {}, size={}x{})", errorMsg, ret,
                  resolution.nFrameWidth, resolution.nFrameHeight);
        return false;
    }

    DetectRoiRestoreGuard restoreGuard(m_deviceHandle, oldLeftROI, oldRightROI);
    const SVzNLROIRect fullLeftROI{0, static_cast<int>(resolution.nFrameWidth),
                                  0, static_cast<int>(resolution.nFrameHeight)};
    const SVzNLROIRect fullRightROI = fullLeftROI;
    ret = VzNL_ConfigDetectROI(m_deviceHandle, &fullLeftROI, &fullRightROI);
    if (ret != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(ret, errorMsg);
        LOG_ERROR("[VizumCamera] Failed to apply temporary full-frame ROI: {} "
                  "(error code: {}, size={}x{})", errorMsg, ret,
                  resolution.nFrameWidth, resolution.nFrameHeight);
        return false;
    }
    LOG_INFO("[VizumCamera] Temporary full-frame ROI applied: {}x{}; CalibROI unchanged",
             resolution.nFrameWidth, resolution.nFrameHeight);

    SVzNLImageData* leftImage = nullptr;
    SVzNLImageData* rightImage = nullptr;
    ret = VzNL_GetEyeImage(m_deviceHandle, &leftImage, &rightImage, 3000);
    bool converted = false;
    if (ret == 0 && leftImage) {
        converted = ConvertVzImageToSLImage(leftImage, image);
        LOG_INFO("[VizumCamera] Full-frame SDK image received: size={}x{}, origin=({}, {})",
                 leftImage->nWidth, leftImage->nHeight,
                 leftImage->ptOriPos.x, leftImage->ptOriPos.y);
    } else {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(ret, errorMsg);
        LOG_ERROR("[VizumCamera] Full-frame image acquisition failed: {} (error code: {})",
                  errorMsg, ret);
    }
    if (leftImage)
        VzNL_ReleaseImage(&leftImage);
    if (rightImage)
        VzNL_ReleaseImage(&rightImage);

    const bool normalized = converted
        && NormalizeToDeviceFrame(image, resolution.nFrameWidth, resolution.nFrameHeight);
    if (converted && !normalized) {
        LOG_ERROR("[VizumCamera] SDK full-frame image cannot be placed in device frame: "
                  "image={}x{} origin=({}, {}), device={}x{}",
                  image.width, image.height, image.originX, image.originY,
                  resolution.nFrameWidth, resolution.nFrameHeight);
    }

    const bool restored = restoreGuard.RestoreNow();
    if (!normalized || !restored) {
        image.clear();
        return false;
    }

    const uint64_t id = ++m_frameCounter;
    frameId = "VZ_FULL_" + std::to_string(id);
    LOG_INFO("[VizumCamera] Full-frame capture completed and ROI restored: frameId={}, "
             "output={}x{} origin=(0, 0)", frameId, image.width, image.height);
    return true;
#else
    return false;
#endif
}

bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int timeoutMs)
{
    LOG_INFO("[VizumCamera] CapturePointCloudWithAutoDetect called, timeoutMs={}", timeoutMs);
    LOG_INFO("[VizumCamera] Input pointCloud state: scanLines.size()={}, points.size()={}", 
             pointCloud.scanLines.size(), pointCloud.points.size());
    
#ifdef ENABLE_VIZUM_CAMERA
    LOG_INFO("[VizumCamera] ENABLE_VIZUM_CAMERA is defined");
    LOG_INFO("[VizumCamera] m_isConnected={}, m_deviceHandle={}", m_isConnected, (m_deviceHandle ? "valid" : "null"));
    
    if (!m_isConnected || !m_deviceHandle) {
        LOG_ERROR("[VizumCamera] Camera not connected, returning false");
        return false;
    }
    
    LOG_INFO("[VizumCamera] ========================================");
    LOG_INFO("[VizumCamera] Starting point cloud capture (AutoDetect mode)...");
    LOG_INFO("[VizumCamera] ========================================");
    
    int result = 0;
    char errorMsg[256] = {0};
    
    // 0. 检查并配置摆镜扫描（关键步骤！）
    LOG_INFO("[VizumCamera] Step 0: Checking and configuring swing motor...");
    
    // 检查是否支持摆镜
    int errorCode = 0;
    VzBool supportSwing = VzNL_IsSupportSwingMotor(m_deviceHandle, &errorCode);
    if (supportSwing == VzTrue) {
        LOG_INFO("[VizumCamera] Swing motor is supported");
        
        // 检查摆镜是否已启用
        VzBool swingEnabled = VzNL_IsEnableSwingMotor(m_deviceHandle, &errorCode);
        if (swingEnabled != VzTrue) {
            // 启用摆镜
            LOG_INFO("[VizumCamera] Enabling swing motor...");
            result = VzNL_EnableSwingMotor(m_deviceHandle, VzTrue);
            if (result != 0) {
                VzNL_GetErrorInfo(result, errorMsg);
                LOG_WARN("[VizumCamera] Failed to enable swing motor: {} (error code: {})", errorMsg, result);
                // 继续尝试，可能摆镜已经手动启用
            } else {
                LOG_INFO("[VizumCamera] Swing motor enabled successfully");
            }
        } else {
            LOG_INFO("[VizumCamera] Swing motor is already enabled");
        }
        
        // 获取并记录摆镜配置信息
        int startAngle = 0, endAngle = 0;
        if (VzNL_GetSwingMotorAngle(m_deviceHandle, &startAngle, &endAngle) == 0) {
            LOG_INFO("[VizumCamera] Swing motor angle range: {} to {}", startAngle, endAngle);
        }
        
        int angleSpeed = 0;
        if (VzNL_GetSwingAngleSpeed(m_deviceHandle, &angleSpeed) == 0) {
            LOG_INFO("[VizumCamera] Swing motor angle speed: {}", angleSpeed);
        }
        
        // 获取当前角度
        float curAngle = VzNL_GetSwingCurAngle(m_deviceHandle, &errorCode);
        if (errorCode == 0) {
            LOG_INFO("[VizumCamera] Current swing angle: {:.2f}", curAngle);
        }
    } else {
        LOG_WARN("[VizumCamera] Swing motor is NOT supported, point cloud may not be captured correctly");
    }
    
    // 设置点云处理模式为固定步长模式（摆镜扫描推荐）
    // 注意：参考实现中没有设置此模式，如果遇到问题可以尝试注释掉
    /*
    LOG_INFO("[VizumCamera] Setting point cloud processing mode to FixedStep...");
    result = VzNL_SetPointCloudProcMode(m_deviceHandle, kePointCloudProcMode_FixedStep);
    if (result != 0) {
        VzNL_GetErrorInfo(result, errorMsg);
        LOG_WARN("[VizumCamera] Failed to set point cloud proc mode: {} (trying Speed mode...)", errorMsg);
        // 尝试速度模式作为备选
        result = VzNL_SetPointCloudProcMode(m_deviceHandle, kePointCloudProcMode_Speed);
        if (result == 0) {
            LOG_INFO("[VizumCamera] Set point cloud proc mode to Speed successfully");
        }
    } else {
        LOG_INFO("[VizumCamera] Set point cloud proc mode to FixedStep successfully");
    }
    */
    
    // 获取当前点云处理模式
    EVzPointCloudProcMode procMode = kePointCloudProcMode_Invalid;
    if (VzNL_GetPointCloudProcMode(m_deviceHandle, &procMode) == 0) {
        const char* modeStr = "Unknown";
        switch(procMode) {
            case kePointCloudProcMode_Speed: modeStr = "Speed"; break;
            case kePointCloudProcMode_Encoder: modeStr = "Encoder"; break;
            case kePointCloudProcMode_FixedStep: modeStr = "FixedStep"; break;
            default: break;
        }
        LOG_INFO("[VizumCamera] Current point cloud proc mode: {}", modeStr);
    }
    
    // 1. 清空累积的点云和扫描线数据（与VZNLCameraAdapter::scan一致）
    {
        std::lock_guard<std::mutex> lock(m_scanLineMutex);
        m_accumulatedScanLines.clear();
        m_accumulatedScanLines2D.clear();
        m_accumulatedLineMeta.clear();
        m_accumulatedCloud.clear();
    }
    LOG_INFO("[VizumCamera] Step 1: Cleared accumulated data");
    
    // 2. 重新初始化激光检测（关键：参考示例代码，每次扫描前重新初始化）
    // 示例代码中 StopAutoDetect 会调用 EndDetectLaser，所以这里需要 BeginDetectLaser
    LOG_INFO("[VizumCamera] Step 1.5: Re-initializing laser detection...");
    VzNL_EndDetectLaser(m_deviceHandle);  // 确保之前的已结束
    
    result = VzNL_BeginDetectLaser(m_deviceHandle);
    if (result != 0) {
        VzNL_GetErrorInfo(result, errorMsg);
        LOG_WARN("[VizumCamera] Failed to begin detect laser: {}", errorMsg);
        // 如果失败，可能是因为已经处于检测状态，尝试继续
    } else {
        LOG_INFO("[VizumCamera] Laser detection initialized successfully");
    }

    // 2.5 启用激光点填充:SDK 对每条激光线输出定长点序列,无测量处填充占位点,
    //     使扫描线构成真正列对齐的 W×H 栅格(供组织化点云保存/栅格聚类使用)
    result = VzNL_EnableFillLaserPoint(m_deviceHandle, VzTrue);
    if (result != 0) {
        VzNL_GetErrorInfo(result, errorMsg);
        LOG_WARN("[VizumCamera] VzNL_EnableFillLaserPoint failed: {} (error code: {})", errorMsg, result);
    } else {
        LOG_INFO("[VizumCamera] Fill laser point mode enabled (rasterized scan lines)");
    }

    // 3. 启动自动检测模式（与VznlCamera::StartAutoDetect一致）
    m_scanCompletionStatus = 0;
    m_isPaused = false;
    m_autoDetectRunning = true;
    LOG_INFO("[VizumCamera] Step 2: Starting AutoDetectEx...");
    result = VzNL_StartAutoDetectEx(m_deviceHandle, keResultDataType_PointXYZRGBA,
                                         keFlipType_None, 
                                         &AutoOutputLaserLineExCB, this);
    if (result != 0) {
        m_autoDetectRunning = false;
        VzNL_GetErrorInfo(result, errorMsg);
        LOG_ERROR("[VizumCamera] Failed to start auto detect: {} (error code: {})", errorMsg, result);
        // 尝试清理
        VzNL_EndDetectLaser(m_deviceHandle);
        return false;
    }
    LOG_INFO("[VizumCamera] AutoDetectEx started successfully");
    
    // 稍微等待一下，确保状态更新
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    
    // 检查自动检测状态
    VzBool isDetecting = VzNL_IsAutoDetecting(m_deviceHandle, &errorCode);
    if (errorCode == 0) {
        LOG_INFO("[VizumCamera] IsAutoDetecting after start: {}",
                 isDetecting == VzTrue ? "Yes" : "No");
    } else {
        char detectingErrorMsg[256] = {0};
        VzNL_GetErrorInfo(errorCode, detectingErrorMsg);
        LOG_WARN("[VizumCamera] Failed to query IsAutoDetecting after start: {} "
                 "(error code: {})", detectingErrorMsg, errorCode);
    }
    
    // 3. 等待扫描完成
    // 策略：等待以下任一条件满足：
    //   1. 收到 SDK 扫描完成信号 Device_Auto_Stop 或 Device_Swing_Finish
    //   2. 等待超时
    // 5秒后仍无数据时持续告警，但保留等待，避免慢启动时误判失败。
    int maxWaitTime = (timeoutMs > 0) ? timeoutMs : 30000;
    LOG_INFO("[VizumCamera] Step 3: Waiting up to {}ms for scan to finish...", maxWaitTime);
    LOG_INFO("[VizumCamera] NOTE: The camera will collect point cloud data while the swing motor is running.");
    LOG_INFO("[VizumCamera] If no data is received, please check if the swing motor is enabled in the camera software.");
    
    auto startTime = std::chrono::steady_clock::now();
    int lastLoggedPoints = 0;
    int logInterval = 1000; // 每秒输出一次状态
    int lastLogTime = 0;
    int noDataTime = 0;     // 无数据的持续时间
    const int noDataWarningThreshold = 5000; // 5秒无数据发出警告
    bool dataReceived = false;
    
    while (true) {
        // 检查是否暂停（扫描完成）
        if (m_isPaused) {
            // 获取当前点云状态
            int currentPoints = 0;
            {
                std::lock_guard<std::mutex> lock(m_scanLineMutex);
                currentPoints = static_cast<int>(m_accumulatedCloud.points.size());
            }

            if (currentPoints > 0) {
                LOG_INFO("[VizumCamera] Scan finished with {} points.", currentPoints);
                break;
            } else {
                auto now = std::chrono::steady_clock::now();
                int elapsed = static_cast<int>(std::chrono::duration_cast<std::chrono::milliseconds>(now - startTime).count());
                
                // 如果时间太短且没有点云，可能是虚假的结束信号（例如刚开始时的残留信号），忽略并继续
                if (elapsed < 2000) { // 2秒内结束且无数据
                    const int completionStatus = m_scanCompletionStatus.load();
                    LOG_WARN("[VizumCamera] Received completion status {} ({}) too early "
                             "({}ms) without data. Ignoring...",
                             DeviceStatusName(completionStatus), completionStatus, elapsed);
                    m_scanCompletionStatus = 0;
                    m_isPaused = false;
                } else {
                    LOG_WARN("[VizumCamera] Scan finished but no points received after {}ms.", elapsed);
                    break;
                }
            }
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        
        auto now = std::chrono::steady_clock::now();
        int elapsed = static_cast<int>(std::chrono::duration_cast<std::chrono::milliseconds>(now - startTime).count());
        
        // 获取当前点云状态
        int currentPoints = 0;
        int scanLines = 0;
        {
            std::lock_guard<std::mutex> lock(m_scanLineMutex);
            currentPoints = static_cast<int>(m_accumulatedCloud.points.size());
            scanLines = static_cast<int>(m_accumulatedScanLines.size());
        }
        
        // 检测是否收到数据
        if (currentPoints > 0 && !dataReceived) {
            dataReceived = true;
            LOG_INFO("[VizumCamera] First data received! Points: {}", currentPoints);
        }
        
        // 每秒输出一次当前状态
        if (elapsed - lastLogTime >= logInterval) {
            lastLogTime = elapsed;
            
            if (currentPoints != lastLoggedPoints) {
                LOG_INFO("[VizumCamera] Scanning... elapsed: {}ms, scan lines: {}, points: {} (new: {})", 
                    elapsed, scanLines, currentPoints, currentPoints - lastLoggedPoints);
                lastLoggedPoints = currentPoints;
                noDataTime = 0; // 重置无数据计时
            } else {
                noDataTime += logInterval;
                LOG_INFO("[VizumCamera] Waiting... elapsed: {}ms, no new points for {}ms (total: {})", 
                    elapsed, noDataTime, currentPoints);
                
                // 如果超过阈值没有数据，输出警告
                if (noDataTime >= noDataWarningThreshold && !dataReceived) {
                    LOG_WARN("[VizumCamera] No data received for {}ms. Please check:", noDataTime);
                    LOG_WARN("[VizumCamera]   1. Is the swing motor enabled and running in the camera software?");
                    LOG_WARN("[VizumCamera]   2. Is the laser line visible on the calibration board?");
                    LOG_WARN("[VizumCamera]   3. Are the camera parameters (exposure, ROI) configured correctly?");
                }
            }
            
            // 检查摆镜状态
            float curAngle = VzNL_GetSwingCurAngle(m_deviceHandle, &errorCode);
            if (errorCode == 0) {
                LOG_INFO("[VizumCamera] Current swing angle: {:.2f}", curAngle);
            }
        }
        
        if (elapsed > maxWaitTime) {
            LOG_WARN("[VizumCamera] Scan timed out after {}ms", maxWaitTime);
            break;
        }
    }
    // 从此处起 StopAutoDetect 触发的 Auto_Stop 仅属于清理动作，不能改写本次完成原因。
    m_autoDetectRunning = false;
    
    int totalDuration = static_cast<int>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::steady_clock::now() - startTime).count());
    
    const int completionStatus = m_scanCompletionStatus.load();
    if (m_isPaused) {
        LOG_INFO("[VizumCamera] Scan finished by completion status {} ({}). Duration: {}ms",
                 DeviceStatusName(completionStatus), completionStatus, totalDuration);
    } else {
        int detectingError = 0;
        const VzBool stillDetecting = VzNL_IsAutoDetecting(m_deviceHandle, &detectingError);
        char detectingErrorMsg[256] = {0};
        if (detectingError != 0) {
            VzNL_GetErrorInfo(detectingError, detectingErrorMsg);
        }
        size_t timeoutScanLines = 0;
        size_t timeoutValidPoints = 0;
        {
            std::lock_guard<std::mutex> lock(m_scanLineMutex);
            timeoutScanLines = m_accumulatedScanLines.size();
            timeoutValidPoints = m_accumulatedCloud.points.size();
        }
        LOG_WARN("[VizumCamera] Timeout diagnostics: duration={}ms, autoDetecting={}, "
                 "detectError={} ({}), completion status={} ({}), scan lines={}, valid points={}",
                 totalDuration, stillDetecting == VzTrue ? "Yes" : "No", detectingError,
                 detectingError == 0 ? "none" : detectingErrorMsg,
                 DeviceStatusName(completionStatus), completionStatus,
                 timeoutScanLines, timeoutValidPoints);
    }
    
    // 输出扫描过程中的状态
    {
        std::lock_guard<std::mutex> lock(m_scanLineMutex);
        LOG_INFO("[VizumCamera] After waiting: 扫描线 {} 条, 点数: {}", m_accumulatedScanLines.size(), m_accumulatedCloud.points.size());
    }
    
    // 4. 停止自动检测（与VznlCamera::StopAutoDetect一致）
    LOG_INFO("[VizumCamera] Step 4: Stopping AutoDetect...");
    result = VzNL_StopAutoDetect(m_deviceHandle);
    if (result != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(result, errorMsg);
        LOG_WARN("[VizumCamera] Failed to stop auto detect: {}", errorMsg);
    }

    // 4.1 结束激光检测（关键：与示例代码一致，每次扫描后结束检测）
    LOG_INFO("[VizumCamera] Step 4.1: Ending laser detection...");
    VzNL_EndDetectLaser(m_deviceHandle);
    
    // 5. 获取累积的点云数据
    {
        std::lock_guard<std::mutex> lock(m_scanLineMutex);
        
        // 如果有扫描线数据，优先构建带结构的扫描线点云（用于VZNL格式保存）
        if (!m_accumulatedScanLines.empty()) {
            // 每条扫描线=一行(回调已保全宽、无效点为 (0,0,0) 占位)，补齐到最大列宽 → 规整组织化栅格。
            // 这样保存/识别可走"栅格化"链路(去壁面+网格聚类分离粘着圆柱)。
            const size_t H = m_accumulatedScanLines.size();
            size_t W = 0;
            for (const auto& line : m_accumulatedScanLines) {
                if (line.size() > W) W = line.size();
            }
            LOG_INFO("[VizumCamera] Step 5: Building organized grid {}x{} from {} scan lines", W, H, H);

            pointCloud.clear();
            pointCloud.width = static_cast<uint32_t>(W);
            pointCloud.height = static_cast<uint32_t>(H);
            pointCloud.isOrganized = true;
            pointCloud.points.assign(W * H, Point3D{});  // (0,0,0) = 无效占位
            pointCloud.pixels.assign(W * H, PixelLR{});  // 双目2D坐标,零占位
            pointCloud.scanLines.reserve(H);
            for (size_t r = 0; r < H; ++r) {
                const auto& line = m_accumulatedScanLines[r];
                ScanLineInfo info;
                info.startIndex = r * W;
                info.pointCount = line.size();
                if (r < m_accumulatedLineMeta.size()) {
                    info.timestamp = m_accumulatedLineMeta[r].timestamp;
                    info.encodeNo = m_accumulatedLineMeta[r].encodeNo;
                }
                pointCloud.scanLines.push_back(info);
                for (size_t c = 0; c < line.size() && c < W; ++c) {
                    pointCloud.points[r * W + c] = line[c];
                }
                if (r < m_accumulatedScanLines2D.size()) {
                    const auto& line2D = m_accumulatedScanLines2D[r];
                    for (size_t c = 0; c < line2D.size() && c < W; ++c) {
                        pointCloud.pixels[r * W + c] = line2D[c];
                    }
                }
            }

            // 记录摆镜角速度(供保存元信息用,查询失败保持 0)
            int angleSpeed = 0;
            if (VzNL_GetSwingAngleSpeed(m_deviceHandle, &angleSpeed) == 0) {
                pointCloud.scanSpeed = static_cast<float>(angleSpeed);
            }

            LOG_INFO("[VizumCamera] Organized grid built: {}x{} ({} cells), {} scan lines, total {} points",
                     W, H, W * H, pointCloud.scanLines.size(), pointCloud.points.size());
        }
        else {
            // 如果没有扫描线数据（异常情况），回退到仅使用有效点
            LOG_INFO("[VizumCamera] Step 5: Building point cloud from {} valid points (No scanlines)", m_accumulatedCloud.points.size());
            
            pointCloud = std::move(m_accumulatedCloud);
            pointCloud.width = static_cast<uint32_t>(pointCloud.points.size());
            pointCloud.height = 1;
            pointCloud.isOrganized = false;
        }
        
        // 清空累积数据
        m_accumulatedScanLines.clear();
        m_accumulatedScanLines2D.clear();
        m_accumulatedLineMeta.clear();
        m_accumulatedCloud.clear();
    }
    
    // 在返回前再次验证点云状态
    LOG_INFO("[VizumCamera] Before return: pointCloud.scanLines.size()={}, points.size()={}", 
             pointCloud.scanLines.size(), pointCloud.points.size());
    
    LOG_INFO("[VizumCamera] ========================================");
    LOG_INFO("[VizumCamera] Capture completed: {} points", pointCloud.points.size());
    LOG_INFO("[VizumCamera] ========================================");
    
    return !pointCloud.empty();
#else
    LOG_WARN("[VizumCamera] ENABLE_VIZUM_CAMERA is NOT defined, returning false");
    (void)pointCloud;
    (void)timeoutMs;
    return false;
#endif
}

bool VizumCamera::GetPointCloud(const std::string& frameId, PointCloud& cloud)
{
    if (m_cachedFrame.frameId != frameId) {
        return false;
    }
    
    cloud = m_cachedFrame.pointCloud;
    return !cloud.empty();
}

bool VizumCamera::GetPointCloudRaw(const std::string& frameId, 
                                    float* buffer, size_t bufferSize)
{
    if (m_cachedFrame.frameId != frameId || !buffer) {
        return false;
    }
    
    const auto& cloud = m_cachedFrame.pointCloud;
    size_t requiredSize = cloud.points.size() * 3 * sizeof(float);
    if (bufferSize < requiredSize) {
        return false;
    }
    
    for (size_t i = 0; i < cloud.points.size(); ++i) {
        buffer[i * 3 + 0] = cloud.points[i].x;
        buffer[i * 3 + 1] = cloud.points[i].y;
        buffer[i * 3 + 2] = cloud.points[i].z;
    }
    
    return true;
}

bool VizumCamera::GetPointCloudVector(const std::string& frameId, 
                                       std::vector<float>& data)
{
    if (m_cachedFrame.frameId != frameId) {
        return false;
    }
    
    const auto& cloud = m_cachedFrame.pointCloud;
    data.resize(cloud.points.size() * 3);
    
    for (size_t i = 0; i < cloud.points.size(); ++i) {
        data[i * 3 + 0] = cloud.points[i].x;
        data[i * 3 + 1] = cloud.points[i].y;
        data[i * 3 + 2] = cloud.points[i].z;
    }
    
    return true;
}

bool VizumCamera::GetDepthMap(const std::string& frameId, DepthMap& depthMap)
{
    if (m_cachedFrame.frameId != frameId) {
        return false;
    }
    
    depthMap = m_cachedFrame.depthMap;
    return !depthMap.empty();
}

bool VizumCamera::GetDepthMapRaw(const std::string& frameId,
                                  float* buffer, size_t bufferSize)
{
    if (m_cachedFrame.frameId != frameId || !buffer) {
        return false;
    }
    
    const auto& depth = m_cachedFrame.depthMap;
    size_t requiredSize = depth.data.size() * sizeof(float);
    if (bufferSize < requiredSize) {
        return false;
    }
    
    memcpy(buffer, depth.data.data(), requiredSize);
    return true;
}

bool VizumCamera::CaptureFrame(FrameData& frameData, CaptureType captureType)
{
    std::string frameId;
    if (!TriggerCapture(frameId, captureType)) {
        return false;
    }
    
    frameData.frameId = frameId;
    frameData.timestamp = m_cachedFrame.timestamp;
    
    if (captureType & CaptureType::Image) {
        frameData.image = m_cachedFrame.leftImage;
    }
    
    if (captureType & CaptureType::PointCloud) {
        frameData.pointCloud = m_cachedFrame.pointCloud;
    }
    
    if (captureType & CaptureType::DepthMap) {
        frameData.depthMap = m_cachedFrame.depthMap;
    }
    
    return true;
}

// ============================================================================
// 异步采集实现（预留）
// ============================================================================

bool VizumCamera::StartAsyncCapture(FrameCallback callback, CaptureType captureType)
{
    (void)callback;
    (void)captureType;
    return false;  // 暂不实现
}

void VizumCamera::StopAsyncCapture()
{
    // 暂不实现
}

bool VizumCamera::IsAsyncCapturing() const
{
    return false;
}

// ============================================================================
// 相机参数实现
// ============================================================================

bool VizumCamera::GetCameraMatrix(CameraMatrix& matrix)
{
#ifdef ENABLE_VIZUM_CAMERA
    if (!m_isConnected || !m_deviceHandle) {
        return false;
    }
    
    // 获取左目内参 (3x3 + 5x1 畸变参数)
    double calibData[14] = {0};
    int result = VzNL_QueryLeftEyeCalibData(m_deviceHandle, calibData);
    if (result != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(result, errorMsg);
        LOG_ERROR("[VizumCamera] Failed to get camera matrix: {}", errorMsg);
        return false;
    }
    
    // calibData格式: [fx, fy, cx, cy, k1, k2, p1, p2, k3, ...]
    // 内参矩阵: [fx, 0, cx; 0, fy, cy; 0, 0, 1]
    matrix.fx = static_cast<float>(calibData[0]);
    matrix.fy = static_cast<float>(calibData[1]);
    matrix.cx = static_cast<float>(calibData[2]);
    matrix.cy = static_cast<float>(calibData[3]);
    
    return true;
#else
    return false;
#endif
}

bool VizumCamera::GetCameraMatrixVector(std::vector<float>& matrixVec)
{
    CameraMatrix matrix;
    if (!GetCameraMatrix(matrix)) {
        return false;
    }
    
    matrixVec = matrix.toVector();
    return true;
}

bool VizumCamera::GetDistortionCoeffs(DistortionCoeffs& coeffs)
{
#ifdef ENABLE_VIZUM_CAMERA
    if (!m_isConnected || !m_deviceHandle) {
        return false;
    }
    
    // 获取左目标定数据
    double calibData[14] = {0};
    int result = VzNL_QueryLeftEyeCalibData(m_deviceHandle, calibData);
    if (result != 0) {
        return false;
    }
    
    // calibData格式: [fx, fy, cx, cy, k1, k2, p1, p2, k3, ...]
    coeffs.k1 = static_cast<float>(calibData[4]);
    coeffs.k2 = static_cast<float>(calibData[5]);
    coeffs.p1 = static_cast<float>(calibData[6]);
    coeffs.p2 = static_cast<float>(calibData[7]);
    coeffs.k3 = static_cast<float>(calibData[8]);
    
    return true;
#else
    return false;
#endif
}

bool VizumCamera::GetDistortionCoeffsVector(std::vector<float>& coeffsVec)
{
    DistortionCoeffs coeffs;
    if (!GetDistortionCoeffs(coeffs)) {
        return false;
    }
    
    coeffsVec = coeffs.toVector();
    return true;
}

// ============================================================================
// 配置接口实现
// ============================================================================

bool VizumCamera::GetIntParam(const std::string& name, int& value)
{
    (void)name;
    (void)value;
    // TODO: 根据需要实现
    return false;
}

bool VizumCamera::SetIntParam(const std::string& name, int value)
{
    (void)name;
    (void)value;
    // TODO: 根据需要实现
    return false;
}

bool VizumCamera::GetFloatParam(const std::string& name, float& value)
{
#ifdef ENABLE_VIZUM_CAMERA
    const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
    if (!m_isConnected || !m_deviceHandle) {
        return false;
    }

    if (name == "ExposureTime" || name == "Exposure" || name == "exposure") {
        EVzNLExposeMode exposeMode = keVzNLExposeMode_Fix;
        uint32_t exposeUs = 0;
        const int result = VzNL_GetConfigEyeExpose(m_deviceHandle, &exposeMode, &exposeUs);
        if (result != 0) {
            return false;
        }
        value = static_cast<float>(exposeUs);
        return true;
    }

    if (name == "Gain" || name == "gain") {
        unsigned short rawGain = 0;
        int result = VzNL_GetCameraGain(m_deviceHandle, keEyeSensorType_Left, &rawGain);
        if (result != 0) {
            result = VzNL_GetCameraGain(m_deviceHandle, keEyeSensorType_Right, &rawGain);
        }
        if (result != 0) {
            return false;
        }
        value = static_cast<float>(rawGain);
        return true;
    }

    return false;
#else
    (void)name;
    (void)value;
    return false;
#endif
}

bool VizumCamera::SetFloatParam(const std::string& name, float value)
{
#ifdef ENABLE_VIZUM_CAMERA
    const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
    if (!m_isConnected || !m_deviceHandle) {
        return false;
    }

    if (name == "ExposureTime" || name == "Exposure" || name == "exposure") {
        unsigned int minVal = 1;
        unsigned int maxVal = 65535;
        VzNL_QueryParamRange(m_deviceHandle, keDeviceParamType_Expose, &minVal, &maxVal);
        LOG_INFO("[VizumCamera] SetExposure: requested={:.1f}us, range=[{}, {}]", value, minVal, maxVal);
        const float bounded = std::clamp(value, static_cast<float>(minVal), static_cast<float>(maxVal));
        const uint32_t exposeUs = static_cast<uint32_t>(bounded + 0.5f);
        const int result = VzNL_ConfigEyeExpose(m_deviceHandle, keVzNLExposeMode_Fix, exposeUs);
        if (result != 0) {
            char errMsg[256] = {0};
            VzNL_GetErrorInfo(result, errMsg);
            LOG_ERROR("[VizumCamera] VzNL_ConfigEyeExpose failed: code={} msg={}", result, errMsg);
        } else {
            LOG_INFO("[VizumCamera] VzNL_ConfigEyeExpose OK: {}us", exposeUs);
        }
        return result == 0;
    }

    if (name == "Gain" || name == "gain") {
        unsigned int minVal = 0;
        unsigned int maxVal = 255;
        VzNL_QueryParamRange(m_deviceHandle, keDeviceParamType_Gain, &minVal, &maxVal);
        LOG_INFO("[VizumCamera] SetGain: requested={:.2f}, range=[{}, {}]", value, minVal, maxVal);
        const float bounded = std::clamp(value, static_cast<float>(minVal), static_cast<float>(maxVal));
        const unsigned short rawGain = static_cast<unsigned short>(bounded + 0.5f);
        const int leftResult  = VzNL_SetCameraGain(m_deviceHandle, keEyeSensorType_Left,  rawGain);
        const int rightResult = VzNL_SetCameraGain(m_deviceHandle, keEyeSensorType_Right, rawGain);
        if (leftResult != 0 && rightResult != 0) {
            char errMsgL[256] = {0}, errMsgR[256] = {0};
            VzNL_GetErrorInfo(leftResult,  errMsgL);
            VzNL_GetErrorInfo(rightResult, errMsgR);
            LOG_ERROR("[VizumCamera] VzNL_SetCameraGain both failed: L={}({}) R={}({})",
                      leftResult, errMsgL, rightResult, errMsgR);
        } else {
            LOG_INFO("[VizumCamera] VzNL_SetCameraGain OK: gain={} L={} R={}", rawGain, leftResult, rightResult);
        }
        return leftResult == 0 || rightResult == 0;
    }

    return false;
#else
    (void)name;
    (void)value;
    return false;
#endif
}

bool VizumCamera::GetStringParam(const std::string& name, std::string& value)
{
    (void)name;
    (void)value;
    // TODO: 根据需要实现
    return false;
}

bool VizumCamera::SetStringParam(const std::string& name, const std::string& value)
{
    (void)name;
    (void)value;
    // TODO: 根据需要实现
    return false;
}

bool VizumCamera::GetConfig(std::string& configJson)
{
    (void)configJson;
    // TODO: 根据需要实现
    return false;
}

bool VizumCamera::SetConfig(const std::string& configJson)
{
    (void)configJson;
    // TODO: 根据需要实现
    return false;
}

bool VizumCamera::GetDetectROI(ROIRect& leftROI, ROIRect& rightROI)
{
#ifdef ENABLE_VIZUM_CAMERA
    const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
    if (!m_isConnected || !m_deviceHandle) {
        LOG_WARN("[VizumCamera] GetDetectROI failed: device not connected");
        return false;
    }

    SVzNLROIRect sdkLeft{};
    SVzNLROIRect sdkRight{};
    const int ret = VzNL_GetConfigDetectROI(m_deviceHandle, &sdkLeft, &sdkRight);
    if (ret != 0) {
        char errorMsg[256] = {0};
        VzNL_GetErrorInfo(ret, errorMsg);
        LOG_ERROR("[VizumCamera] VzNL_GetConfigDetectROI failed: {} (error code: {})",
                  errorMsg, ret);
        return false;
    }

    leftROI = {sdkLeft.left, sdkLeft.right, sdkLeft.top, sdkLeft.bottom};
    rightROI = {sdkRight.left, sdkRight.right, sdkRight.top, sdkRight.bottom};
    LOG_INFO("[VizumCamera] Detect ROI read only: left=[{}, {}, {}, {}], "
             "right=[{}, {}, {}, {}]",
             leftROI.left, leftROI.right, leftROI.top, leftROI.bottom,
             rightROI.left, rightROI.right, rightROI.top, rightROI.bottom);
    return true;
#else
    (void)leftROI;
    (void)rightROI;
    return false;
#endif
}

bool VizumCamera::SetCalibROIEnabled(bool enable)
{
#ifdef ENABLE_VIZUM_CAMERA
    const std::lock_guard<std::mutex> deviceLock(m_deviceAccessMutex);
    if (!m_isConnected || !m_deviceHandle) {
        LOG_WARN("[VizumCamera] SetCalibROIEnabled({}) failed: device not connected", enable);
        return false;
    }
    const int ret = VzNL_EnableCalibROI(m_deviceHandle, enable ? VzTrue : VzFalse);
    if (ret != 0) {
        LOG_ERROR("[VizumCamera] VzNL_EnableCalibROI({}) failed, error={}", enable, ret);
        return false;
    }
    LOG_INFO("[VizumCamera] CalibROI (rectified image output) {}", enable ? "enabled" : "disabled");
    return true;
#else
    (void)enable;
    return false;
#endif
}

SLCameraPtr CreateVizumCamera()
{
    return std::make_unique<VizumCamera>();
}

#undef m_deviceHandle
#undef m_vzDeviceInfo
#undef m_ipAddress
#undef m_deviceInfo
#undef m_isConnected
#undef m_isAsyncCapturing
#undef m_frameCallback
#undef m_captureType
#undef m_mutex
#undef m_deviceAccessMutex
#undef m_frameCounter
#undef m_cachedFrame
#undef m_accumulatedScanLines
#undef m_scanLineMutex
#undef m_autoDetectRunning
#undef m_isPaused
#undef m_scanCompletionStatus
#undef m_accumulatedCloud

} // namespace StructuredLight