GrabBag/Device/GlLineLaserDevice/Src/GlLineLaserDevice.cpp

#include "GlLineLaserDevice.h"
#include "VrError.h"
#include "VrLog.h"

#include <cstring>
#include <chrono>
#include <algorithm>

// 静态成员初始化
CGlLineLaserDevice* CGlLineLaserDevice::s_pInstance = nullptr;

CGlLineLaserDevice::CGlLineLaserDevice()
    : m_nDeviceId(0)
    , m_bDeviceOpen(false)
    , m_nProfileWidth(4096)
    , m_dXPitch(0.01)
    , m_dYPitch(1.0)
    , m_nBatchLines(200)
{
    memset(&m_modelInfo, 0, sizeof(GLX8_2_ModelInfo));
}

CGlLineLaserDevice::~CGlLineLaserDevice()
{
    if (m_bDeviceOpen) {
        CloseDevice();
    }
}

int CGlLineLaserDevice::InitDevice()
{
    // 初始化 gl_linelaser_sdk
    int ret = GLX8_2_Initialize();
    if (ret != 0) {
        LOG_ERROR("GLX8_2_Initialize failed: %d\n", ret);
        return ERR_CODE(DEV_OPEN_ERR);
    }

    LOG_DEBUG("GLX8_2_Initialize success, SDK version: %s\n", GLX8_2_GetVersion());
    return SUCCESS;
}

int CGlLineLaserDevice::SetStatusCallback(VzNL_OnNotifyStatusCBEx fNotify, void *param)
{
    m_pStatusCallback = fNotify;
    m_pStatusCallbackParam = param;
    return SUCCESS;
}

int CGlLineLaserDevice::OpenDevice(const char* sIP, bool bRGBD, bool bSwing, bool bFillLaser)
{
    // gl_linelaser_sdk 不支持 RGBD 和摆动模式，忽略这些参数
    (void)bRGBD;
    (void)bSwing;
    (void)bFillLaser;

    if (m_bDeviceOpen) {
        LOG_WARNING("Device already open\n");
        return SUCCESS;
    }

    
    // 解析IP地址
    GLX8_2_ETHERNET_CONFIG ethConfig;
    memset(&ethConfig, 0, sizeof(ethConfig));
    
    if (sIP && strlen(sIP) > 0) {
        LOG_ERROR("open IP address format: %s\n", sIP);
        // 解析IP字符串 "x.x.x.x"
        int ip[4];
        if (sscanf(sIP, "%d.%d.%d.%d", &ip[0], &ip[1], &ip[2], &ip[3]) == 4) {
            ethConfig.abyIpAddress[0] = (unsigned char)ip[0];
            ethConfig.abyIpAddress[1] = (unsigned char)ip[1];
            ethConfig.abyIpAddress[2] = (unsigned char)ip[2];
            ethConfig.abyIpAddress[3] = (unsigned char)ip[3];
            m_strDeviceIP = sIP;
        } else {
            LOG_ERROR("Invalid IP address format: %s\n", sIP);
            return ERR_CODE(DEV_ARG_INVAILD);
        }
    } else {
        // 搜索在线设备
        int count = 0;
        GLX8_2_ETHERNET_CONFIG* pDevices = GLX8_2_SearchOnline(&count, 3000);
        if (count == 0 || pDevices == nullptr) {
            LOG_ERROR("No device found\n");
            return ERR_CODE(DEV_NOT_FIND);
        }

        // 使用第一个找到的设备
        memcpy(&ethConfig, &pDevices[0], sizeof(GLX8_2_ETHERNET_CONFIG));
        char ipStr[32];
        sprintf(ipStr, "%d.%d.%d.%d", ethConfig.abyIpAddress[0], ethConfig.abyIpAddress[1], ethConfig.abyIpAddress[2], ethConfig.abyIpAddress[3]);
        m_strDeviceIP = ipStr;
        LOG_DEBUG("Found device: %s\n", m_strDeviceIP.c_str());
    }

    // 打开设备
    int ret = GLX8_2_EthernetOpen(m_nDeviceId, &ethConfig);
    if (ret != 0) {
        LOG_ERROR("GLX8_2_Ethernet Open failed: %d\n", ret);
        return ERR_CODE(DEV_OPEN_ERR);
    }

    m_bDeviceOpen = true;
    LOG_DEBUG("Device opened: %s\n", m_strDeviceIP.c_str());

    // 获取设备信息
    ret = GLX8_2_GetModelInfos(m_nDeviceId, &m_modelInfo);
    if (ret == 0) {
        m_nProfileWidth = m_modelInfo.ProfileDataWidth;
        m_dXPitch = m_modelInfo.xPixth;
        if (m_modelInfo.yPixth > 0) {
            m_dYPitch = m_modelInfo.yPixth;
        }
        LOG_DEBUG("Device model: %s, width: %d, xPitch: %.4f, yPitch: %.4f\n",
                  m_modelInfo.Model, m_nProfileWidth, m_dXPitch, m_dYPitch);
    }

    // 分配数据缓存
    m_profileBuffer.resize(static_cast<size_t>(m_nProfileWidth) * m_nBatchLines);
    m_intensityBuffer.resize(static_cast<size_t>(m_nProfileWidth) * m_nBatchLines);
    m_positionBuffer.resize(m_nProfileWidth);

    return SUCCESS;
}

int CGlLineLaserDevice::GetVersion(SVzNLVersionInfo& sVersionInfo)
{
    memset(&sVersionInfo, 0, sizeof(SVzNLVersionInfo));

    const char* sdkVersion = GLX8_2_GetVersion();
    if (sdkVersion) {
        strncpy(sVersionInfo.szSDKVersion, sdkVersion, VZNL_VERSION_LENGTH - 1);
    }

    // 填充其他版本信息
    strncpy(sVersionInfo.szAppVersion, "GlLineLaser", VZNL_VERSION_LENGTH - 1);

    return SUCCESS;
}

int CGlLineLaserDevice::GetDevInfo(SVzNLEyeDeviceInfoEx& sDeviceInfo)
{
    memset(&sDeviceInfo, 0, sizeof(SVzNLEyeDeviceInfoEx));

    // 填充设备信息
    strncpy(sDeviceInfo.sEyeCBInfo.byServerIP, m_strDeviceIP.c_str(), VZNL_SDK_NETWORK_IPv4_LENGTH - 1);
    strncpy(sDeviceInfo.sEyeCBInfo.szDeviceName, m_modelInfo.Model, VZNL_DEVICE_NAME_LENGTH - 1);
    strncpy(sDeviceInfo.sEyeCBInfo.szDeviceID, m_modelInfo.HeaderSerial, VZNL_GUID_LENGTH - 1);

    // 设置分辨率
    sDeviceInfo.sVideoRes.nFrameWidth = m_nProfileWidth;
    sDeviceInfo.sVideoRes.nFrameHeight = m_nBatchLines;

    return SUCCESS;
}

int CGlLineLaserDevice::CloseDevice()
{
    if (!m_bDeviceOpen) {
        return SUCCESS;
    }

    // 先停止检测
    if (m_bDetecting) {
        StopDetect();
    }

    // 关闭设备
    int ret = GLX8_2_CommClose(m_nDeviceId);
    if (ret != 0) {
        LOG_ERROR("GLX8_2_CommClose failed: %d\n", ret);
    }

    m_bDeviceOpen = false;
    LOG_DEBUG("Device closed\n");

    return SUCCESS;
}

int CGlLineLaserDevice::StartDetect(VzNL_AutoOutputLaserLineExCB fCallFunc, EVzResultDataType eDataType, void *param)
{
    if (!m_bDeviceOpen) {
        LOG_ERROR("Device not open\n");
        return ERR_CODE(DEV_NO_OPEN);
    }

    if (m_bDetecting) {
        LOG_WARNING("Already detecting\n");
        return SUCCESS;
    }

    m_pDetectCallback = fCallFunc;
    m_pDetectCallbackParam = param;
    m_eDataType = eDataType;
    m_bStopDetect = false;
    m_ullFrameIndex = 0;

    // 设置回调实例指针
    s_pInstance = this;

    // 设置批处理回调
    int ret = GLX8_2_SetBatchOneTimeDataHandler(m_nDeviceId, BatchDataCallback);
    if (ret != 0) {
        LOG_ERROR("GLX8_2_SetBatchOneTimeDataHandler failed: %d\n", ret);
        return ERR_CODE(DEV_CTRL_ERR);
    }

    // 开始批处理采集（立即开始）
    ret = GLX8_2_StartMeasureWithCallback(m_nDeviceId, 0);
    if (ret != 0) {
        LOG_ERROR("GLX8_2_StartMeasureWithCallback failed: %d\n", ret);
        return ERR_CODE(DEV_CTRL_ERR);
    }

    m_bDetecting = true;
    LOG_DEBUG("Detection started\n");

    return SUCCESS;
}

bool CGlLineLaserDevice::IsDetectIng()
{
    return m_bDetecting;
}

int CGlLineLaserDevice::StopDetect()
{
    if (!m_bDetecting) {
        return SUCCESS;
    }

    m_bStopDetect = true;

    // 停止批处理
    int ret = GLX8_2_StopMeasure(m_nDeviceId);
    if (ret != 0) {
        LOG_ERROR("GLX8_2_StopMeasure failed: %d\n", ret);
    }

    m_bDetecting = false;
    s_pInstance = nullptr;

    // 通知状态变化
    if (m_pStatusCallback) {
        m_pStatusCallback(keDeviceWorkStatus_Device_Swing_Finish, nullptr, 0, m_pStatusCallbackParam);
        m_pStatusCallback(keDeviceWorkStatus_Device_Auto_Stop, nullptr, 0, m_pStatusCallbackParam);
    }

    LOG_DEBUG("Detection stopped\n");

    return SUCCESS;
}

// 批处理数据回调（静态函数）
void CGlLineLaserDevice::BatchDataCallback(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj)
{
    if (s_pInstance) {
        s_pInstance->ProcessBatchData(info, DataObj);
    }
}

// 处理批处理数据
void CGlLineLaserDevice::ProcessBatchData(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj)
{
    if (!m_pDetectCallback || m_bStopDetect) {
        return;
    }

    // 获取批处理轮廓数据
    int32_t* profileData = GLX8_2_GetBatchProfilePoint(DataObj, 0);
    uint8_t* intensityData = GLX8_2_GetBatchIntensityPoint(DataObj, 0);

    if (!profileData) {
        LOG_WARNING("No profile data in batch\n");
        return;
    }

    int width = info->xPoints;
    int batchCount = info->BatchPoints;
    LOG_DEBUG("BatchPoints: %d, xPoints: %d\n", batchCount, width);

    // 逐行处理数据并回调
    for (int lineIdx = 0; lineIdx < batchCount; lineIdx++) {
        // 转换当前行的数据
        const int32_t* lineProfile = profileData + static_cast<size_t>(lineIdx) * width;
        ConvertProfileToPosition(lineProfile, width, lineIdx);

        // 填充 SVzLaserLineData 结构
        SVzLaserLineData laserLineData;
        memset(&laserLineData, 0, sizeof(SVzLaserLineData));

        laserLineData.p3DPoint = m_positionBuffer.data();
        laserLineData.p2DPoint = nullptr;  // 线激光没有2D数据
        laserLineData.nPointCount = width;
        laserLineData.dTotleOffset = static_cast<double>(m_ullFrameIndex) * m_dYPitch;
        laserLineData.dStep = m_dYPitch;
        laserLineData.llFrameIdx = m_ullFrameIndex;
        laserLineData.llTimeStamp = std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::steady_clock::now().time_since_epoch()).count();
        laserLineData.nEncodeNo = info->startEncoder + lineIdx;
        laserLineData.fSwingAngle = 0.0f;  // 线激光没有摆动角度
        laserLineData.bEndOnceScan = (lineIdx == batchCount - 1) ? VzTrue : VzFalse;

        // 回调
        m_pDetectCallback(m_eDataType, &laserLineData, m_pDetectCallbackParam);

        m_ullFrameIndex++;
    }

    // 如果是最后一个批处理，通知完成
    if (info->returnStatus != 0) {
        StopDetect();
    }
}

// 将轮廓数据转换为位置数据
void CGlLineLaserDevice::ConvertProfileToPosition(const int32_t* profileData, int count, int lineIndex)
{
    // 确保缓存足够大
    if (m_positionBuffer.size() < static_cast<size_t>(count)) {
        m_positionBuffer.resize(count);
    }

    // 计算Y偏移（基于行索引）
    double yOffset = static_cast<double>(m_ullFrameIndex) * m_dYPitch;

    // 转换每个点
    for (int i = 0; i < count; i++) {
        SVzNL3DPosition& pos = m_positionBuffer[i];

        pos.nPointIdx = i;

        // X 坐标：根据点索引和X间距计算
        // X范围居中：从 -width/2 * xPitch 到 width/2 * xPitch
        pos.pt3D.x = (static_cast<double>(i) - static_cast<double>(count) / 2.0) * m_dXPitch;

        // Y 坐标：扫描方向偏移
        pos.pt3D.y = yOffset;

        // Z 坐标：高度值，gl_linelaser_sdk 单位是 0.01um，转换为 mm
        // 无效值通常是最大值或特定值，这里假设 0x7FFFFFFF 或负值为无效
        int32_t rawZ = profileData[i];
        if (rawZ == 0x7FFFFFFF || rawZ < -100000000) {
            pos.pt3D.z = 0.0;  // 无效值设为0
        } else {
            pos.pt3D.z = static_cast<double>(rawZ) * 0.00001;  // 0.01um -> mm
        }
    }
}

// ============ ROI相关（线激光不支持）============

int CGlLineLaserDevice::SetDetectROI(SVzNLRect& leftROI, SVzNLRect& rightROI)
{
    (void)leftROI;
    (void)rightROI;
    // 线激光不支持ROI设置
    return SUCCESS;
}

int CGlLineLaserDevice::GetDetectROI(SVzNLRect& leftROI, SVzNLRect& rightROI)
{
    memset(&leftROI, 0, sizeof(SVzNLRect));
    memset(&rightROI, 0, sizeof(SVzNLRect));
    return SUCCESS;
}

// ============ 曝光/增益相关（线激光通过SDK设置）============

int CGlLineLaserDevice::SetEyeExpose(unsigned int& exposeTime)
{
    (void)exposeTime;
    // gl_linelaser_sdk 通过 GLX8_2_SetSetting 设置曝光
    // 暂不实现详细接口
    return SUCCESS;
}

int CGlLineLaserDevice::GetEyeExpose(unsigned int& exposeTime)
{
    exposeTime = 1000;  // 默认值
    return SUCCESS;
}

int CGlLineLaserDevice::SetEyeGain(unsigned int& gain)
{
    (void)gain;
    return SUCCESS;
}

int CGlLineLaserDevice::GetEyeGain(unsigned int& gain)
{
    gain = 100;  // 默认值
    return SUCCESS;
}

// ============ 帧率相关 ============

int CGlLineLaserDevice::SetFrame(int& frame)
{
    (void)frame;
    // 线激光帧率由硬件决定
    return SUCCESS;
}

int CGlLineLaserDevice::GetFrame(int& frame)
{
    frame = 100;  // 默认帧率
    return SUCCESS;
}

// ============ RGBD相关（线激光不支持）============

bool CGlLineLaserDevice::IsSupport()
{
    return false;  // 不支持RGBD
}

int CGlLineLaserDevice::SetRGBDExposeThres(float& value)
{
    (void)value;
    return ERR_CODE(DEV_UNSUPPORT);
}

int CGlLineLaserDevice::GetRGBDExposeThres(float& value)
{
    value = 0.0f;
    return ERR_CODE(DEV_UNSUPPORT);
}

// ============ 过滤高度 ============

int CGlLineLaserDevice::SetFilterHeight(double& dHeight)
{
    (void)dHeight;
    return SUCCESS;
}

int CGlLineLaserDevice::GetFilterHeight(double& dHeight)
{
    dHeight = 0.0;
    return SUCCESS;
}

// ============ 摆动机构相关（线激光不支持）============

int CGlLineLaserDevice::GetSwingSpeed(float& fSpeed)
{
    fSpeed = 0.0f;
    return SUCCESS;
}

int CGlLineLaserDevice::SetSwingSpeed(float& fSpeed)
{
    (void)fSpeed;
    return SUCCESS;
}

int CGlLineLaserDevice::SetSwingAngle(float& fMin, float& fMax)
{
    (void)fMin;
    (void)fMax;
    return SUCCESS;
}

int CGlLineLaserDevice::GetSwingAngle(float& fMin, float& fMax)
{
    fMin = 0.0f;
    fMax = 0.0f;
    return SUCCESS;
}

int CGlLineLaserDevice::SetWorkRange(double& dMin, double& dMax)
{
    (void)dMin;
    (void)dMax;
    return SUCCESS;
}

int CGlLineLaserDevice::GetWorkRange(double& dMin, double& dMax)
{
    // 从设备信息获取工作范围
    dMin = m_modelInfo.zRangmin;
    dMax = m_modelInfo.zRangmax;
    return SUCCESS;
}

// ============ GL线激光专用接口实现 ============

int CGlLineLaserDevice::GetProfileDataWidth()
{
    return m_nProfileWidth;
}

double CGlLineLaserDevice::GetXPitch()
{
    return m_dXPitch;
}

double CGlLineLaserDevice::GetYPitch()
{
    return m_dYPitch;
}

int CGlLineLaserDevice::SetYPitch(double pitch)
{
    if (pitch <= 0) {
        return ERR_CODE(DEV_ARG_INVAILD);
    }
    m_dYPitch = pitch;
    return SUCCESS;
}

int CGlLineLaserDevice::SetBatchLines(unsigned int batchLines)
{
    if (batchLines == 0) {
        return ERR_CODE(DEV_ARG_INVAILD);
    }
    m_nBatchLines = batchLines;

    // 重新分配缓存
    m_profileBuffer.resize(static_cast<size_t>(m_nProfileWidth) * m_nBatchLines);
    m_intensityBuffer.resize(static_cast<size_t>(m_nProfileWidth) * m_nBatchLines);

    return SUCCESS;
}

unsigned int CGlLineLaserDevice::GetBatchLines()
{
    return m_nBatchLines;
}

int CGlLineLaserDevice::SwitchProgram(int programNo)
{
    int ret = GLX8_2_SwitchProgram(m_nDeviceId, programNo);
    if (ret != 0) {
        LOG_ERROR("GLX8_2_SwitchProgram failed: %d\n", ret);
        return ERR_CODE(DEV_CTRL_ERR);
    }
    return SUCCESS;
}

int CGlLineLaserDevice::GetModelInfo(char* model, char* serialNumber)
{
    if (model) {
        strcpy(model, m_modelInfo.Model);
    }
    if (serialNumber) {
        strcpy(serialNumber, m_modelInfo.HeaderSerial);
    }
    return SUCCESS;
}

int CGlLineLaserDevice::GetMeasureRange(double& xMin, double& xMax, double& zMin, double& zMax)
{
    xMin = m_modelInfo.xRangmin;
    xMax = m_modelInfo.xRangmax;
    zMin = m_modelInfo.zRangmin;
    zMax = m_modelInfo.zRangmax;
    return SUCCESS;
}

// ============ 工厂方法实现 ============
// 注意：IVrEyeDevice::CreateObject 在 VrEyeDevice 中实现，创建 VzNLSDK 设备
// GlLineLaserDevice 只提供 IGlLineLaserDevice::CreateGlLineLaserObject 工厂方法

int IGlLineLaserDevice::CreateGlLineLaserObject(IGlLineLaserDevice** ppDevice)
{
    CGlLineLaserDevice* p = new CGlLineLaserDevice();
    *ppDevice = p;
    return SUCCESS;
}