GrabBag/Device/GlLineLaserDevice/Src/GlLineLaserDevice.cpp

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

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

// 静态实例指针，供SDK回调访问
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));
    s_pInstance = this;
}

CGlLineLaserDevice::~CGlLineLaserDevice()
{
    if (m_bDeviceOpen) {
        CloseDevice();
    }
    if (s_pInstance == this) {
        s_pInstance = nullptr;
    }
}

int CGlLineLaserDevice::InitDevice()
{
    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)
{
    (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_DEBUG("open IP address format: %s\n", sIP);
        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_EthernetOpen 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);
    }

    // 注册SDK批处理回调（批处理参数在设备端软件预先配置）
    ret = RegisterBatchCallback();
    if (ret != SUCCESS) {
        LOG_ERROR("RegisterBatchCallback failed: %d\n", ret);
        return ret;
    }

    LOG_DEBUG("Device initialized successfully (callback mode)\n");
    return SUCCESS;
}

// 配置批处理模式
int CGlLineLaserDevice::ConfigureBatchMode()
{
    LOG_DEBUG("Configuring batch mode...\n");

    char inval[4];
    uint32_t ival;

    // 1. 开启批处理测量
    ival = 1;
    memcpy(inval, &ival, 4);
    int ret = GLX8_2_SetSetting(m_nDeviceId, 1, 0, 0, 3, nullptr, inval, 4);
    if (ret != 0) {
        LOG_ERROR("Failed to enable batch mode: %d\n", ret);
        return ERR_CODE(DEV_CTRL_ERR);
    }
    LOG_DEBUG("Batch mode enabled\n");

    // 2. 设置批处理数量
    ival = m_nBatchLines;
    memcpy(inval, &ival, 4);
    ret = GLX8_2_SetSetting(m_nDeviceId, 1, 0, 0, 0x0a, nullptr, inval, 4);
    if (ret != 0) {
        LOG_ERROR("Failed to set batch lines: %d\n", ret);
        return ERR_CODE(DEV_CTRL_ERR);
    }
    LOG_DEBUG("Batch lines set to %d\n", m_nBatchLines);

    // 3. 设置带亮度输出
    ival = 1;
    memcpy(inval, &ival, 4);
    ret = GLX8_2_SetSetting(m_nDeviceId, 1, 2, 0, 0x0b, nullptr, inval, 4);
    if (ret != 0) {
        LOG_ERROR("Failed to enable intensity output: %d\n", ret);
        return ERR_CODE(DEV_CTRL_ERR);
    }
    LOG_DEBUG("Intensity output enabled\n");

    return SUCCESS;
}

// 注册SDK批处理回调
int CGlLineLaserDevice::RegisterBatchCallback()
{
    s_pInstance = this;

    int ret = GLX8_2_SetBatchOneTimeDataHandler(m_nDeviceId, BatchOneTimeCallback);
    if (ret != 0) {
        LOG_ERROR("GLX8_2_SetBatchOneTimeDataHandler failed: %d\n", ret);
        return ERR_CODE(DEV_CTRL_ERR);
    }

    LOG_DEBUG("Batch callback registered\n");
    return SUCCESS;
}

// SDK批处理回调（静态函数，由SDK线程调用）
void CGlLineLaserDevice::BatchOneTimeCallback(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj)
{
    if (s_pInstance == nullptr) {
        return;
    }

    if (!s_pInstance->m_bDetecting) {
        return;
    }

    s_pInstance->ProcessBatchData(info, DataObj);
}

// 处理一次批处理回调数据
void CGlLineLaserDevice::ProcessBatchData(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj)
{
    if (info == nullptr || DataObj == nullptr) {
        LOG_ERROR("ProcessBatchData: null parameter\n");
        return;
    }

    // 检查批处理状态
    if (info->returnStatus != 0) {
        LOG_WARNING("Batch returnStatus: %d\n", info->returnStatus);
        return;
    }

    int batchCount = info->BatchPoints;
    int width = info->xPoints;

    if (batchCount <= 0 || width <= 0) {
        LOG_WARNING("Invalid batch data: batchCount=%d, width=%d\n", batchCount, width);
        return;
    }

    LOG_DEBUG("Received batch: %d lines, width: %d, startEncoder: %d, batchTimes: %d\n", batchCount, width, info->startEncoder, info->BatchTimes);

    // 使用回调info中的实时参数
    double xPitch = info->xPixth;
    if (xPitch <= 0) {
        xPitch = m_dXPitch;  // 回退到设备初始化时获取的值
    }

    // 从SDK获取数据指针（SDK内部管理内存，无需自己分配）
    int32_t* profileData = GLX8_2_GetBatchProfilePoint(DataObj, 0);
    uint32_t* encoderData = GLX8_2_GetBatchEncoderPoint(DataObj, 0);

    if (profileData == nullptr) {
        LOG_ERROR("GLX8_2_GetBatchProfilePoint returned null\n");
        return;
    }

    // 局部位置缓存
    std::vector<SVzNL3DPosition> positionBuffer(width);

    // 逐行处理数据并回调给上层
    for (int lineIdx = 0; lineIdx < batchCount; lineIdx++) {
        if (!m_bDetecting) {
            break;
        }

        // 计算当前行在批处理数据中的偏移
        const int32_t* lineProfile = profileData + static_cast<size_t>(lineIdx) * width;

        // 转换为xyz坐标
        double xOffset = (encoderData[lineIdx] -  info->startEncoder)* m_dYPitch;
        for (int i = 0; i < width; i++) {
            SVzNL3DPosition& pos = positionBuffer[i];
            pos.nPointIdx = i;
            pos.pt3D.x = xOffset;
            pos.pt3D.y = (static_cast<double>(i) - static_cast<double>(width) / 2.0) * xPitch;

            int32_t rawZ = lineProfile[i];
            if (rawZ < -9990.0) {
                pos.pt3D.x = 0;
                pos.pt3D.y = 0;
                pos.pt3D.z = 0;
            } else {
                double z_mm = static_cast<double>(rawZ) * 0.00001;  // 0.01um -> mm
                pos.pt3D.z = m_dBaseDistance - z_mm;  // 基准距离偏移
            }
        }

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

        laserLineData.p3DPoint = positionBuffer.data();
        laserLineData.p2DPoint = nullptr;
        laserLineData.nPointCount = width;
        laserLineData.dTotleOffset = xOffset;
        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 = encoderData ? encoderData[lineIdx] : 0;
        laserLineData.fSwingAngle = 0.0f;
        laserLineData.bEndOnceScan = (lineIdx == batchCount - 1) ? VzTrue : VzFalse;

        // 回调给上层应用
        if (m_pDetectCallback) {
            m_pDetectCallback(m_eDataType, &laserLineData, m_pDetectCallbackParam);
        }

        m_ullFrameIndex++;
    }

    LOG_DEBUG("Processed %d lines, total frames: %llu\n", batchCount, m_ullFrameIndex);

    // 通知上层本次批处理数据处理完成
    if (m_pStatusCallback) {
        m_pStatusCallback(keDeviceWorkStatus_Device_Swing_Finish, nullptr, 0, m_pStatusCallbackParam);
    }

    m_bDetecting = false;
    
}

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_ullFrameIndex = 0;

    m_bDetecting = true;

    // 启动回调模式批处理（0=立即开始）
    int ret = GLX8_2_StartMeasureWithCallback(m_nDeviceId, 0);
    if (ret != 0) {
        LOG_ERROR("GLX8_2_StartMeasureWithCallback failed: %d\n", ret);
        m_bDetecting = false;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    LOG_DEBUG("Detection started (callback mode)\n");

    return SUCCESS;
}

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

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

    m_bDetecting = false;

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

    // 通知状态变化
    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;
}

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

int CGlLineLaserDevice::SetDetectROI(SVzNLRect& leftROI, SVzNLRect& rightROI)
{
    (void)leftROI;
    (void)rightROI;
    return SUCCESS;
}

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

// ============ 曝光/增益相关 ============

int CGlLineLaserDevice::SetEyeExpose(unsigned int& exposeTime)
{
    (void)exposeTime;
    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;
}

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

int CGlLineLaserDevice::SetBaseDistance(double distance)
{
    if (!m_bDeviceOpen) {
        LOG_ERROR("Device not open, cannot set base distance\n");
        return ERR_CODE(DEV_NO_OPEN);
    }

    int ret = GLX8_2_SET_EXTEND_563TIF_Z_OFFSET(m_nDeviceId, distance);
    if (ret != 0) {
        LOG_ERROR("GLX8_2_SET_EXTEND_563TIF_Z_OFFSET failed: %d\n", ret);
        return ERR_CODE(DEV_CTRL_ERR);
    }

    m_dBaseDistance = distance;
    LOG_INFO("Base distance set to %.1f mm\n", distance);
    return SUCCESS;
}

// ============ 工厂方法实现 ============

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