#include "DetectPresenter.h"
#include "WD_particleSizeMeasure_Export.h"
#include <fstream>

DetectPresenter::DetectPresenter(/* args */)
{
    LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_particleSegVersion());
}

DetectPresenter::~DetectPresenter()
{
}

int DetectPresenter::DetectWorkpiece(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    DetectionResult& detectionResult)
{
    if (laserLines.empty()) {
        LOG_WARNING("No laser lines data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 获取当前相机的校准参数
    VrCameraPlaneCalibParam cameraCalibParamValue;
    const VrCameraPlaneCalibParam* cameraCalibParam = nullptr;
    if (algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraCalibParamValue)) {
        cameraCalibParam = &cameraCalibParamValue;
    }

    // 保存debug数据
    std::string timeStamp = CVrDateUtils::GetNowTime();
    if(debugParam.enableDebug && debugParam.savePointCloud){
        LOG_INFO("[Algo Thread] Debug mode is enabled, saving point cloud data\n");
        std::string fileName = debugParam.debugOutputPath + "/Laserline_" + std::to_string(cameraIndex) + "_" + timeStamp + ".txt";
        dataLoader.SaveLaserScanData(fileName, laserLines, laserLines.size(), 0.0, 0, 0);
    }

    int nRet = SUCCESS;

    // 转换为算法需要的XYZ格式
    std::vector<std::vector<SVzNL3DPosition>> xyzData;
    int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, xyzData);
    if (convertResult != SUCCESS || xyzData.empty()) {
        LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 准备粒度检测参数
    SWD_paricleSizeParam particleSizeParam;
    // 直接赋值，因为现在结构匹配
    particleSizeParam.minSize.length = algorithmParams.particleSizeParam.minSize.length;
    particleSizeParam.minSize.width = algorithmParams.particleSizeParam.minSize.width;
    particleSizeParam.minSize.height = algorithmParams.particleSizeParam.minSize.height;
    particleSizeParam.alarmSize.length = algorithmParams.particleSizeParam.alarmSize.length;
    particleSizeParam.alarmSize.width = algorithmParams.particleSizeParam.alarmSize.width;
    particleSizeParam.alarmSize.height = algorithmParams.particleSizeParam.alarmSize.height;

    // 准备算法参数
    SWD_PSM_algoParam algoParam;
    algoParam.cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
    algoParam.cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
    algoParam.cornerParam.scale = algorithmParams.cornerParam.scale;
    algoParam.cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
    algoParam.cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
    algoParam.cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;

    algoParam.growParam.yDeviation_max = algorithmParams.growParam.yDeviation_max;
    algoParam.growParam.zDeviation_max = algorithmParams.growParam.zDeviation_max;
    algoParam.growParam.maxLineSkipNum = algorithmParams.growParam.maxLineSkipNum;
    algoParam.growParam.maxSkipDistance = algorithmParams.growParam.maxSkipDistance;
    algoParam.growParam.minLTypeTreeLen = algorithmParams.growParam.minLTypeTreeLen;
    algoParam.growParam.minVTypeTreeLen = algorithmParams.growParam.minVTypeTreeLen;

    algoParam.filterParam.continuityTh = algorithmParams.filterParam.continuityTh;
    algoParam.filterParam.outlierTh = algorithmParams.filterParam.outlierTh;

    // 准备平面校准参数
    SSG_planeCalibPara groundCalibPara;
    if(cameraCalibParam){
        memcpy(groundCalibPara.planeCalib, cameraCalibParam->planeCalib, sizeof(double) * 9);
        memcpy(groundCalibPara.invRMatrix, cameraCalibParam->invRMatrix, sizeof(double) * 9);
        groundCalibPara.planeHeight = cameraCalibParam->planeHeight;
    } else {
        // 使用默认单位矩阵
        double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        memcpy(groundCalibPara.planeCalib, identity, sizeof(double) * 9);
        memcpy(groundCalibPara.invRMatrix, identity, sizeof(double) * 9);
        groundCalibPara.planeHeight = -1.0;
    }

    if(debugParam.enableDebug && debugParam.printDetailLog)
    {
        LOG_INFO("[Algo Thread] minSize l:%.1f, w:%.1f, h:%.1f\n", particleSizeParam.minSize.length, particleSizeParam.minSize.width, particleSizeParam.minSize.height);
        LOG_INFO("              alarmSize l:%.1f, w:%.1f, h:%.1f\n",particleSizeParam.alarmSize.length, particleSizeParam.alarmSize.width, particleSizeParam.alarmSize.height);
        LOG_INFO("[Algo Thread] Corner: cornerTh=%.1f, scale=%.1f\n", algoParam.cornerParam.cornerTh, algoParam.cornerParam.scale);
        LOG_INFO("[Algo Thread] Filter: continuityTh=%.1f, outlierTh=%.1f\n", algoParam.filterParam.continuityTh, algoParam.filterParam.outlierTh);
        LOG_INFO("Plane height: %.3f\n", groundCalibPara.planeHeight);
    }

    // 数据预处理：调平和去除地面
    if(cameraCalibParam){
        LOG_DEBUG("Processing data with plane calibration\n");
        for(size_t i = 0; i < xyzData.size(); i++){
            wd_lineDataR(xyzData[i], cameraCalibParam->planeCalib, groundCalibPara.planeHeight);
        }
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;
    std::vector<SWD_ParticlePosInfo> particles;

    LOG_DEBUG("before wd_particleSizeMeasure \n");

    // 调用粒度检测算法
    wd_particleSizeMeasure(
        xyzData,
        particleSizeParam,
        groundCalibPara,
        algoParam,
        particles,
        &errCode
    );

    LOG_DEBUG("after wd_particleSizeMeasure \n");

    LOG_INFO("wd_particleSizeMeasure: detected %d particles, err=%d runtime=%.3fms\n", (int)particles.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());

    ERR_CODE_RETURN(errCode);

    // 保存检测到的颗粒信息到结果
    detectionResult.positions.clear();
    for(size_t i = 0; i < particles.size(); i++) {
        ParticleSizePosition pos;

        pos.index = i + 1;
        pos.length = particles[i].size.length;
        pos.width = particles[i].size.width;
        pos.height = particles[i].size.height;

        // 保存颗粒尺寸
        pos.width = particles[i].size.width;
        pos.height = particles[i].size.height;

        // 检查是否超过告警阈值
        pos.isAlarm = (pos.length > particleSizeParam.alarmSize.length || pos.width > particleSizeParam.alarmSize.width || pos.height > particleSizeParam.alarmSize.height);

        detectionResult.positions.push_back(pos);

        if(debugParam.enableDebug && debugParam.printDetailLog) {
            LOG_INFO("Particle %d: size=(%.2f, %.2f, %.2f), alarm=%d\n", pos.index, pos.length, pos.width, pos.height, pos.isAlarm);
        }
    }

    // 生成可视化图像
    if (!xyzData.empty()) {
        // 转换颗粒信息格式
        std::vector<SimpleParticleInfo> simpleParticles;
        for (const auto& p : particles) {
            SimpleParticleInfo sp;
            for (int i = 0; i < 8; i++) {
                sp.vertix[i] = p.vertix[i];
            }
            sp.size.width = p.size.width;
            sp.size.height = p.size.height;
            simpleParticles.push_back(sp);
        }

        detectionResult.image = PointCloudImageUtils::GeneratePointCloudImageWithParticles(xyzData, simpleParticles, cameraIndex);
    }

    return SUCCESS;
}