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

DetectPresenter::DetectPresenter(/* args */)
{
}

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

        // 获取当前时间戳，格式为YYYYMMDDHHMMSS
        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);
    }

    // 工件角点提取参数
    SSX_BQworkpiecePara bqWorkpieceParam;
    bqWorkpieceParam.lineLen = algorithmParams.workpieceParam.lineLen;

    SSG_cornerParam cornerParam;
    cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
    cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
    cornerParam.scale = algorithmParams.cornerParam.scale;
    cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
    cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
    cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;

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

    SSG_outlierFilterParam filterParam;
    filterParam.continuityTh = algorithmParams.filterParam.continuityTh;
    filterParam.outlierTh = algorithmParams.filterParam.outlierTh;

    if(debugParam.enableDebug && debugParam.printDetailLog)
    {
        LOG_INFO("[Algo Thread] clibMatrix: \n\t[%.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f]\n",
            clibMatrix[0], clibMatrix[1], clibMatrix[2], clibMatrix[3],
            clibMatrix[4], clibMatrix[5], clibMatrix[6], clibMatrix[7],
            clibMatrix[8], clibMatrix[9], clibMatrix[10], clibMatrix[11],
            clibMatrix[12], clibMatrix[13], clibMatrix[14], clibMatrix[15]);

        // 打印拐角参数
        LOG_INFO("[Algo Thread] Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
            cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap,
            cornerParam.minEndingGap_z, cornerParam.jumpCornerTh_1, cornerParam.jumpCornerTh_2);

        // 打印树生长参数
        LOG_INFO("[Algo Thread] Tree Grow: yDeviation_max=%.1f, zDeviation_max=%.1f, maxLineSkipNum=%d, maxSkipDistance=%.1f, minLTypeTreeLen=%.1f, minVTypeTreeLen=%.1f\n",
            growParam.yDeviation_max, growParam.zDeviation_max, growParam.maxLineSkipNum,
            growParam.maxSkipDistance, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);

        // 打印滤波参数
        LOG_INFO("[Algo Thread] Filter: continuityTh=%.1f, outlierTh=%.1f\n", filterParam.continuityTh, filterParam.outlierTh);

        // 打印工件角点提取参数
        LOG_INFO("[Algo Thread] BQ Workpiece: lineLen=%.1f\n", bqWorkpieceParam.lineLen);
    }

    // 准备平面校准参数
    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("Plane height: %.3f\n", groundCalibPara.planeHeight);
        LOG_INFO("  Plane calibration matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
                 groundCalibPara.planeCalib[0], groundCalibPara.planeCalib[1], groundCalibPara.planeCalib[2],
                 groundCalibPara.planeCalib[3], groundCalibPara.planeCalib[4], groundCalibPara.planeCalib[5],
                 groundCalibPara.planeCalib[6], groundCalibPara.planeCalib[7], groundCalibPara.planeCalib[8]);

        LOG_INFO("  Plane invRMatrix matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
                 groundCalibPara.invRMatrix[0], groundCalibPara.invRMatrix[1], groundCalibPara.invRMatrix[2],
                 groundCalibPara.invRMatrix[3], groundCalibPara.invRMatrix[4], groundCalibPara.invRMatrix[5],
                 groundCalibPara.invRMatrix[6], groundCalibPara.invRMatrix[7], groundCalibPara.invRMatrix[8]);
    }

    // 数据预处理：调平和去除地面（使用当前相机的调平参数）
    if(cameraCalibParam){
        LOG_DEBUG("Processing data with plane calibration\n");
        for(size_t i = 0; i < xyzData.size(); i++){
            sx_BQ_lineDataR(xyzData[i], cameraCalibParam->planeCalib, -1);
        }
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before sx_BQ_getWorkpieceCorners \n");
    // 调用工件角点提取算法
    SSX_debugInfo debugContours[4];
    SSX_BQworkpieceResult bqResult = sx_BQ_getWorkpieceCorners(
        xyzData,
        cornerParam,
        filterParam,
        growParam,
        groundCalibPara,
        bqWorkpieceParam,
#if _OUTPUT_DEBUG_DATA
        debugContours,
#endif
        &errCode
    );
    LOG_DEBUG("after sx_BQ_getWorkpieceCorners \n");

    LOG_INFO("sx_BQ_getWorkpieceCorners: workpieceType=%d err=%d runtime=%.3fms\n", bqResult.workpieceType, errCode, oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    // 保存工件类型到检测结果
    detectionResult.workpieceType = bqResult.workpieceType;

    // 构建用于可视化的角点数组
    // 按照逆时针顺序排列：左侧(从下到上) -> 顶部(从左到右) -> 右侧(从上到下) -> 底部(从右到左)
    std::vector<std::vector<SVzNL3DPoint>> objOps;
    std::vector<SVzNL3DPoint> allCorners;

    // 1. 添加左侧角点（从下到上，逆时针）
    for (int i = 0; i < 3; i++) {
        SVzNL3DPoint pt;
        pt.x = bqResult.corner_L[i].x;
        pt.y = bqResult.corner_L[i].y;
        pt.z = bqResult.corner_L[i].z;
        allCorners.push_back(pt);
    }

    // 2. 添加顶部角点（从左到右，逆时针）
    for (int i = 0; i < 3; i++) {
        SVzNL3DPoint pt;
        pt.x = bqResult.corner_T[i].x;
        pt.y = bqResult.corner_T[i].y;
        pt.z = bqResult.corner_T[i].z;
        allCorners.push_back(pt);
    }

    // 3. 添加右侧角点（从上到下，逆时针）
    for (int i = 0; i < 3; i++) {
        SVzNL3DPoint pt;
        pt.x = bqResult.corner_R[i].x;
        pt.y = bqResult.corner_R[i].y;
        pt.z = bqResult.corner_R[i].z;
        allCorners.push_back(pt);
    }

    // 4. 添加底部角点（从右到左，逆时针）
    for (int i = 0; i < 3; i++) {
        SVzNL3DPoint pt;
        pt.x = bqResult.corner_B[i].x;
        pt.y = bqResult.corner_B[i].y;
        pt.z = bqResult.corner_B[i].z;
        allCorners.push_back(pt);
    }

    objOps.push_back(allCorners);

    // 从点云数据生成投影图像
    detectionResult.image = PointCloudImageUtils::GeneratePointCloudRetPointImage(xyzData, objOps);

    // 转换检测结果为UI显示格式（使用机械臂坐标系数据）
    for (size_t i = 0; i < objOps.size(); i++) {
        
        for(size_t j = 0; j < objOps[i].size(); j++){
            const SVzNL3DPoint& obj = objOps[i][j];
    
            // 进行坐标转换：从算法坐标系转换到机械臂坐标系
            SVzNL3DPoint targetObj;
            targetObj.x = obj.x;
            targetObj.y = obj.y;
            targetObj.z = obj.z;
    
            SVzNL3DPoint robotObj;
            CVrConvert::EyeToRobot(targetObj, robotObj, clibMatrix);
    
            // 创建位置数据（使用转换后的机械臂坐标）
            WorkpiecePosition pos;
            pos.x       = robotObj.x;           // 机械臂坐标X
            pos.y       = robotObj.y;           // 机械臂坐标Y
            pos.z       = robotObj.z;           // 机械臂坐标Z
            pos.roll    = 0.0;                  // 搭接焊缝检测不提供姿态角
            pos.pitch   = 0.0;                  // 搭接焊缝检测不提供姿态角
            pos.yaw     = 0.0;                  // 搭接焊缝检测不提供姿态角
    
            detectionResult.positions.push_back(pos);
    
            if(debugParam.enableDebug && debugParam.printDetailLog){
                LOG_INFO("[Algo Thread] Object %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", i, obj.x, obj.y, obj.z);
                LOG_INFO("[Algo Thread] Object %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
                            i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
            }
        }
    }

    if(debugParam.enableDebug && debugParam.saveDebugImage){
        // 获取当前时间戳，格式为YYYYMMDDHHMMSS
        std::string fileName = debugParam.debugOutputPath + "/Image_" + std::to_string(cameraIndex) + "_" + timeStamp + ".png";
        LOG_INFO("[Algo Thread] Debug image saved image : %s\n", fileName.c_str());

        // 保存检测结果图片
        if (!detectionResult.image.isNull()) {
            QString qFileName = QString::fromStdString(fileName);
            detectionResult.image.save(qFileName);
        } else {
            LOG_WARNING("[Algo Thread] No valid image to save for debug\n");
        }
    }

    return nRet;
}