#include "DetectPresenter.h"
#include "rodAndBarDetection_Export.h"
#include <fstream>
#include <QPainter>
#include <QPen>
#include <QColor>

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

DetectPresenter::~DetectPresenter()
{
}


int DetectPresenter::DetectScrew(
    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);
    }

    // 螺杆定位算法参数
    double rodDiameter = algorithmParams.screwParam.rodDiameter;
    bool isHorizonScan = algorithmParams.screwParam.isHorizonScan;

    SSG_cornerParam cornerParam;
    cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
    cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
    cornerParam.scale = rodDiameter / 4;  // 参考测试文件
    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] Screw: rodDiameter=%.1f, isHorizonScan=%s\n",
            rodDiameter, isHorizonScan ? "true" : "false");

        // 打印拐角参数
        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);
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before sx_hexHeadScrewMeasure \n");

    // 调用螺杆定位算法
    std::vector<SSX_hexHeadScrewInfo> screwInfo;
    sx_hexHeadScrewMeasure(
        xyzData,
        isHorizonScan,  // true:激光线平行槽道；false:激光线垂直槽道
        cornerParam,
        filterParam,
        growParam,
        rodDiameter,
        screwInfo,
        &errCode);

    LOG_DEBUG("after sx_hexHeadScrewMeasure \n");

    LOG_INFO("sx_hexHeadScrewMeasure: detected %zu screws, err=%d runtime=%.3fms\n",
             screwInfo.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    // 构建检测结果：生成点云图像
    // 1. 获取所有螺杆的中心点用于可视化
    std::vector<std::vector<SVzNL3DPoint>> objOps;
    std::vector<SVzNL3DPoint> screwCenters;

    for (const auto& screw : screwInfo) {
        SVzNL3DPoint pt;
        pt.x = screw.center.x;
        pt.y = screw.center.y;
        pt.z = screw.center.z;
        screwCenters.push_back(pt);
    }
    if (!screwCenters.empty()) {
        objOps.push_back(screwCenters);
    }

    // 从点云数据生成投影图像（10cm边界）
    detectionResult.image = PointCloudImageUtils::GeneratePointCloudRetPointImage(xyzData, objOps, 100.0);

    // 在图像上绘制螺杆的轴向方向线
    if (!detectionResult.image.isNull() && !screwInfo.empty()) {
        QPainter painter(&detectionResult.image);
        painter.setRenderHint(QPainter::Antialiasing);

        // 计算点云范围（与PointCloudImageUtils相同的方式）
        double xMin = 1e10, xMax = -1e10, yMin = 1e10, yMax = -1e10;
        for (const auto& line : xyzData) {
            for (const auto& pt : line) {
                if (pt.pt3D.z < 1e-4) continue;
                xMin = std::min(xMin, (double)pt.pt3D.x);
                xMax = std::max(xMax, (double)pt.pt3D.x);
                yMin = std::min(yMin, (double)pt.pt3D.y);
                yMax = std::max(yMax, (double)pt.pt3D.y);
            }
        }

        // 扩展边界（与GeneratePointCloudRetPointImage相同）
        double margin = 100.0;  // 10cm = 100mm
        xMin -= margin;
        xMax += margin;
        yMin -= margin;
        yMax += margin;

        // 使用与GeneratePointCloudRetPointImage相同的参数
        int imgRows = detectionResult.image.height();
        int imgCols = detectionResult.image.width();
        int x_skip = 50;
        int y_skip = 50;

        // 计算投影比例（与PointCloudImageUtils相同的方式）
        double y_rows = (double)(imgRows - y_skip * 2);
        double x_cols = (double)(imgCols - x_skip * 2);
        double x_scale = (xMax - xMin) / x_cols;
        double y_scale = (yMax - yMin) / y_rows;

        // 使用统一的比例尺
        if (x_scale < y_scale)
            x_scale = y_scale;
        else
            y_scale = x_scale;

        // 转换3D坐标到图像坐标的lambda函数
        auto toImageCoord = [&](const SVzNL3DPoint& pt) -> QPointF {
            int px = (int)((pt.x - xMin) / x_scale + x_skip);
            int py = (int)((pt.y - yMin) / y_scale + y_skip);
            return QPointF(px, py);
        };

        // 绘制法向量线
        QPen pen(QColor(255, 0, 0), 2);
        painter.setPen(pen);

        double len = 60;  // 法向量线长度
        for (const auto& screw : screwInfo) {
            SVzNL3DPoint pt0 = { screw.center.x - len * screw.axialDir.x,
                                 screw.center.y - len * screw.axialDir.y,
                                 screw.center.z - len * screw.axialDir.z };
            SVzNL3DPoint pt1 = { screw.center.x + len * screw.axialDir.x,
                                 screw.center.y + len * screw.axialDir.y,
                                 screw.center.z + len * screw.axialDir.z };

            QPointF imgPt0 = toImageCoord(pt0);
            QPointF imgPt1 = toImageCoord(pt1);
            painter.drawLine(imgPt0, imgPt1);
        }
    }

    // 转换检测结果为UI显示格式（使用机械臂坐标系数据）
    for (size_t i = 0; i < screwInfo.size(); i++) {
        const auto& screw = screwInfo[i];

        // 进行坐标转换：从算法坐标系转换到机械臂坐标系
        SVzNL3DPoint targetObj;
        targetObj.x = screw.center.x;
        targetObj.y = screw.center.y;
        targetObj.z = screw.center.z;

        SVzNL3DPoint robotObj;
        CVrConvert::EyeToRobot(targetObj, robotObj, clibMatrix);

        // 创建位置数据（使用转换后的机械臂坐标）
        ScrewPosition pos;
        pos.x       = robotObj.x;           // 机械臂坐标X
        pos.y       = robotObj.y;           // 机械臂坐标Y
        pos.z       = robotObj.z;           // 机械臂坐标Z
        pos.roll    = screw.rotateAngle;    // 使用螺杆旋转角度作为roll
        pos.pitch   = 0.0;
        pos.yaw     = 0.0;

        detectionResult.positions.push_back(pos);

        // 保存螺杆信息
        ScrewInfo info;
        info.centerX = robotObj.x;
        info.centerY = robotObj.y;
        info.centerZ = robotObj.z;
        info.axialDirX = screw.axialDir.x;
        info.axialDirY = screw.axialDir.y;
        info.axialDirZ = screw.axialDir.z;
        info.rotateAngle = screw.rotateAngle;
        detectionResult.screwInfoList.push_back(info);

        if(debugParam.enableDebug && debugParam.printDetailLog){
            LOG_INFO("[Algo Thread] Screw %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
                     i, screw.center.x, screw.center.y, screw.center.z);
            LOG_INFO("[Algo Thread] Screw %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, Angle=%.2f\n",
                     i, pos.x, pos.y, pos.z, pos.roll);
            LOG_INFO("[Algo Thread] Screw %zu Axial Dir: X=%.3f, Y=%.3f, Z=%.3f\n",
                     i, screw.axialDir.x, screw.axialDir.y, screw.axialDir.z);
        }
    }

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