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

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

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_DEBUG("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;

    // 启动数据采集线程（主动轮询模式）
    m_bDetecting = true;
    m_detectThread = std::thread(&CGlLineLaserDevice::DetectThreadFunc, this);

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

    // 等待线程结束
    if (m_detectThread.joinable()) {
        m_detectThread.join();
    }

    m_bDetecting = false;

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

// 数据采集线程函数（主动轮询模式，参考 test_deal_atch_datas）
void CGlLineLaserDevice::DetectThreadFunc()
{
    LOG_DEBUG("Detect thread started\n");

    while (!m_bStopDetect) {
        // 启动批处理（参考 test_deal_atch_datas）
        int ret = GLX8_2_StartMeasure(m_nDeviceId, 5000);  // 5秒超时
        if (ret != 0) {
            LOG_ERROR("GLX8_2_StartMeasure failed: %d\n", ret);
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            continue;
        }

        // 等待一小段时间让设备准备好
        std::this_thread::sleep_for(std::chrono::milliseconds(50));

        // 获取批处理数据（参考 test_batch_datas）
        GetBatchData();

        // 批处理完成后等待一段时间再开始下一次
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    LOG_DEBUG("Detect thread stopped\n");
}

// 获取批处理数据（参考 test_batch_datas 的流程）
void CGlLineLaserDevice::GetBatchData()
{
    GLX8_2_STR_CALLBACK_INFO info;
    memset(&info, 0, sizeof(info));

    const int maxBatchLines = m_nBatchLines;

    // 确保缓存足够大
    size_t totalPoints = static_cast<size_t>(m_nProfileWidth) * maxBatchLines;
    if (m_profileBuffer.size() < totalPoints) {
        m_profileBuffer.resize(totalPoints);
    }
    if (m_intensityBuffer.size() < totalPoints) {
        m_intensityBuffer.resize(totalPoints);
    }

    std::vector<uint32_t> encoderBuffer(maxBatchLines);

    // 使用 GLX8_2_ReceiveDataAuto 顺序获取批处理数据
    int ret = GLX8_2_ReceiveDataAuto(m_nDeviceId, &info,
                                      m_profileBuffer.data(),
                                      m_intensityBuffer.data(),
                                      encoderBuffer.data());

    if (ret != 0) {
        LOG_WARNING("GLX8_2_ReceiveDataAuto failed: %d\n", ret);
        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\n",
              batchCount, width, info.startEncoder);

    // 确保位置缓存足够大
    if (m_positionBuffer.size() < static_cast<size_t>(width)) {
        m_positionBuffer.resize(width);
    }

    // 逐行处理数据并回调（转换为xyz点云数据）
    for (int lineIdx = 0; lineIdx < batchCount; lineIdx++) {
        if (m_bStopDetect) {
            break;
        }

        // 计算当前行在缓存中的偏移
        const int32_t* lineProfile = m_profileBuffer.data() + static_cast<size_t>(lineIdx) * width;

        // 转换为xyz坐标
        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 = encoderBuffer[lineIdx];  // 使用实际的编码器值
        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);
}

// 将轮廓数据转换为位置数据
void CGlLineLaserDevice::ConvertProfileToPosition(const int32_t* profileData, int count, int lineIndex)
{
    // lineIndex 参数保留用于未来扩展，当前使用 m_ullFrameIndex 计算全局偏移
    (void)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;
}