#include "DetectPresenter.h"
#include "HoleDetection.h"
#include "HoleDetectionParams.h"
#include <fstream>
#include "PointCloudImageUtils.h"
#include "CoordinateTransform.h"
#include "VrConvert.h"
#include "VrTimeUtils.h"
#include "VrDateUtils.h"

// 辅助函数：向量叉积
static CTVec3D crossProduct(const CTVec3D& a, const CTVec3D& b) {
    return CTVec3D(
        a.y * b.z - a.z * b.y,
        a.z * b.x - a.x * b.z,
        a.x * b.y - a.y * b.x
    );
}

DetectPresenter::DetectPresenter()
{
    LOG_DEBUG("DetectPresenter Init (HoleDetection)\n");
}

DetectPresenter::~DetectPresenter()
{
}

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

    // 保存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);
    }

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

    // 将 xyzData 转换为 SVzNLPointXYZ* 网格数据
    int rows = static_cast<int>(xyzData.size());
    int cols = 0;
    for (const auto& row : xyzData) {
        if (static_cast<int>(row.size()) > cols) {
            cols = static_cast<int>(row.size());
        }
    }

    if (rows <= 0 || cols <= 0) {
        LOG_WARNING("Invalid grid dimensions: rows=%d, cols=%d\n", rows, cols);
        return ERR_CODE(DEV_DATA_INVALID);
    }

    LOG_INFO("[Algo Thread] Grid dimensions: rows=%d, cols=%d\n", rows, cols);

    // 分配网格数组（行优先）
    SVzNLPointXYZ* points = new SVzNLPointXYZ[rows * cols];
    memset(points, 0, sizeof(SVzNLPointXYZ) * rows * cols);

    // 填充有效点，缺失点用 {0,0,0} 填充
    for (int r = 0; r < rows; r++) {
        int rowCols = static_cast<int>(xyzData[r].size());
        for (int c = 0; c < cols; c++) {
            int idx = r * cols + c;
            if (c < rowCols) {
                points[idx].x = static_cast<float>(xyzData[r][c].pt3D.x);
                points[idx].y = static_cast<float>(xyzData[r][c].pt3D.y);
                points[idx].z = static_cast<float>(xyzData[r][c].pt3D.z);
            } else {
                points[idx].x = 0.0f;
                points[idx].y = 0.0f;
                points[idx].z = 0.0f;
            }
        }
    }

    // 映射 VrHoleDetectionParam -> SHoleDetectionParams
    SHoleDetectionParams detectionParams;
    detectionParams.neighborCount       = algorithmParams.detectionParam.neighborCount;
    detectionParams.angleThresholdPos   = static_cast<float>(algorithmParams.detectionParam.angleThresholdPos);
    detectionParams.angleThresholdNeg   = static_cast<float>(algorithmParams.detectionParam.angleThresholdNeg);
    detectionParams.minPitDepth         = static_cast<float>(algorithmParams.detectionParam.minPitDepth);
    detectionParams.minRadius           = static_cast<float>(algorithmParams.detectionParam.minRadius);
    detectionParams.maxRadius           = static_cast<float>(algorithmParams.detectionParam.maxRadius);
    detectionParams.expansionSize1      = algorithmParams.detectionParam.expansionSize1;
    detectionParams.expansionSize2      = algorithmParams.detectionParam.expansionSize2;
    detectionParams.minVTransitionPoints = algorithmParams.detectionParam.minVTransitionPoints;

    // 映射 VrHoleFilterParam -> SHoleFilterParams
    SHoleFilterParams filterParams;
    filterParams.maxEccentricity    = static_cast<float>(algorithmParams.filterParam.maxEccentricity);
    filterParams.minAngularCoverage = static_cast<float>(algorithmParams.filterParam.minAngularCoverage);
    filterParams.maxRadiusFitRatio  = static_cast<float>(algorithmParams.filterParam.maxRadiusFitRatio);
    filterParams.minQualityScore    = static_cast<float>(algorithmParams.filterParam.minQualityScore);
    filterParams.maxPlaneResidual   = static_cast<float>(algorithmParams.filterParam.maxPlaneResidual);
    filterParams.maxAngularGap      = static_cast<float>(algorithmParams.filterParam.maxAngularGap);
    filterParams.minInlierRatio     = static_cast<float>(algorithmParams.filterParam.minInlierRatio);

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] DetectionParams: neighborCount=%d, angleThresholdPos=%.1f, angleThresholdNeg=%.1f, minPitDepth=%.1f\n",
            detectionParams.neighborCount, detectionParams.angleThresholdPos,
            detectionParams.angleThresholdNeg, detectionParams.minPitDepth);
        LOG_INFO("[Algo Thread] DetectionParams: minRadius=%.1f, maxRadius=%.1f, expansionSize1=%d, expansionSize2=%d\n",
            detectionParams.minRadius, detectionParams.maxRadius,
            detectionParams.expansionSize1, detectionParams.expansionSize2);
        LOG_INFO("[Algo Thread] DetectionParams: minVTransitionPoints=%d\n",
            detectionParams.minVTransitionPoints);

        LOG_INFO("[Algo Thread] FilterParams: maxEccentricity=%.5f, minAngularCoverage=%.1f, maxRadiusFitRatio=%.2f\n",
            filterParams.maxEccentricity, filterParams.minAngularCoverage, filterParams.maxRadiusFitRatio);
        LOG_INFO("[Algo Thread] FilterParams: minQualityScore=%.2f, maxPlaneResidual=%.1f, maxAngularGap=%.1f, minInlierRatio=%.2f\n",
            filterParams.minQualityScore, filterParams.maxPlaneResidual,
            filterParams.maxAngularGap, filterParams.minInlierRatio);
    }

    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before DetectMultipleHoles\n");

    // 调用孔洞检测算法
    SMultiHoleResult multiResult;
    int errCode = DetectMultipleHoles(
        points,
        rows,
        cols,
        detectionParams,
        filterParams,
        &multiResult
    );

    LOG_DEBUG("after DetectMultipleHoles\n");

    LOG_INFO("DetectMultipleHoles: found %d holes (candidates=%d, filtered=%d), err=%d runtime=%.3fms\n",
        multiResult.holeCount, multiResult.totalCandidates, multiResult.filteredCount,
        errCode, oTimeUtils.GetElapsedTimeInMilliSec());

    // 记录统计信息到检测结果
    detectionResult.totalCandidates = multiResult.totalCandidates;
    detectionResult.filteredCount = multiResult.filteredCount;

    if (errCode != HD_SUCCESS) {
        LOG_ERROR("DetectMultipleHoles failed with error code: %d\n", errCode);
        delete[] points;
        FreeMultiHoleResult(&multiResult);
        return errCode;
    }

    // 构建手眼标定齐次变换矩阵 T_end_to_cam（从4x4 clibMatrix数组）
    CTHomogeneousMatrix handEyeHM;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            handEyeHM.at(i, j) = clibMatrix[i * 4 + j];
        }
    }

    // 构建机器人法兰位姿齐次变换矩阵 T_base_to_end
    CTRobotPose ctRobotPose = CTRobotPose::fromDegrees(
        robotPose.x, robotPose.y, robotPose.z,
        robotPose.roll, robotPose.pitch, robotPose.yaw);
    CTHomogeneousMatrix T_robot = ctRobotPose.toHomogeneousMatrix();

    // 眼在手上：T_total = T_base_to_end × T_end_to_cam
    CTHomogeneousMatrix T_total = T_robot * handEyeHM;

    // 构建用于可视化的孔洞中心点列表
    std::vector<std::pair<SVzNL3DPointF, float>> holeVisData;  // center + radius

    // 处理每个检测到的孔洞
    for (int i = 0; i < multiResult.holeCount; i++) {
        const SHoleResult& hole = multiResult.holes[i];

        // 保存孔洞详细信息
        HoleResultData holeData;
        holeData.centerX = hole.center.x;
        holeData.centerY = hole.center.y;
        holeData.centerZ = hole.center.z;
        holeData.normalX = hole.normal.x;
        holeData.normalY = hole.normal.y;
        holeData.normalZ = hole.normal.z;
        holeData.radius = hole.radius;
        holeData.depth = hole.depth;
        holeData.eccentricity = hole.eccentricity;
        holeData.radiusVariance = hole.radiusVariance;
        holeData.angularSpan = hole.angularSpan;
        holeData.qualityScore = hole.qualityScore;
        detectionResult.holeDetails.push_back(holeData);

        // 六轴转换：位置+姿态（转换到机械臂坐标系）
        // 1. 位置转换：使用齐次变换矩阵进行坐标变换
        CTVec3D ptEye(hole.center.x, hole.center.y, hole.center.z);
        CTVec3D ptRobot = T_total.transformPoint(ptEye);

        // 2. 姿态转换：从法向量构建旋转矩阵
        // Z轴 = 法向量（归一化）
        CTVec3D Z_axis(hole.normal.x, hole.normal.y, hole.normal.z);
        double normZ = Z_axis.norm();
        if (normZ > 1e-6) {
            Z_axis = Z_axis * (1.0 / normZ);
        }

        // 选择 X 轴：使用上方向 (0,1,0) 叉乘 Z_axis
        CTVec3D up(0.0, 1.0, 0.0);
        CTVec3D X_axis = crossProduct(up, Z_axis);
        double normX = X_axis.norm();

        // 如果 up 和 Z_axis 平行，使用 (1,0,0) 作为备选
        if (normX < 1e-6) {
            up = CTVec3D(1.0, 0.0, 0.0);
            X_axis = crossProduct(up, Z_axis);
            normX = X_axis.norm();
        }

        if (normX > 1e-6) {
            X_axis = X_axis * (1.0 / normX);
        }

        // Y轴 = Z × X
        CTVec3D Y_axis = crossProduct(Z_axis, X_axis);

        // 构建方向向量列表（eye坐标系）
        CTVec3D dirVectors_eye[3];
        dirVectors_eye[0] = X_axis;
        dirVectors_eye[1] = Y_axis;
        dirVectors_eye[2] = Z_axis;

        // 根据配置决定方向向量反向
        switch (dirVectorInvert) {
            case DIR_INVERT_NONE:
                break;
            case DIR_INVERT_XY:
                dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
                dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
                break;
            case DIR_INVERT_XZ:
                dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
                dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
                break;
            case DIR_INVERT_YZ:
            default:
                dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
                dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
                break;
        }

        // 使用齐次变换矩阵对方向向量进行旋转变换（仅旋转，不平移）
        CTVec3D dirVectors_robot[3];
        for (int j = 0; j < 3; j++) {
            dirVectors_robot[j] = T_total.transformVector(dirVectors_eye[j]);
        }

        // 构建旋转矩阵
        CTRotationMatrix R_pose;
        R_pose.at(0, 0) = dirVectors_robot[0].x;
        R_pose.at(0, 1) = dirVectors_robot[1].x;
        R_pose.at(0, 2) = dirVectors_robot[2].x;
        R_pose.at(1, 0) = dirVectors_robot[0].y;
        R_pose.at(1, 1) = dirVectors_robot[1].y;
        R_pose.at(1, 2) = dirVectors_robot[2].y;
        R_pose.at(2, 0) = dirVectors_robot[0].z;
        R_pose.at(2, 1) = dirVectors_robot[1].z;
        R_pose.at(2, 2) = dirVectors_robot[2].z;

        // 使用 CCoordinateTransform::rotationMatrixToEuler 转换为欧拉角（外旋ZYX，即RZ-RY-RX）
        CTEulerAngles robotRpyRad = CCoordinateTransform::rotationMatrixToEuler(R_pose, CTEulerOrder::sZYX);
        double roll_deg, pitch_deg, yaw_deg;
        robotRpyRad.toDegrees(roll_deg, pitch_deg, yaw_deg);

        // 将机器人坐标系下的位姿添加到positions列表
        HolePosition centerRobotPos;
        centerRobotPos.x = ptRobot.x;
        centerRobotPos.y = ptRobot.y;
        centerRobotPos.z = ptRobot.z;
        centerRobotPos.roll  = roll_deg;
        centerRobotPos.pitch = pitch_deg;
        centerRobotPos.yaw   = yaw_deg;

        detectionResult.positions.push_back(centerRobotPos);

        // 保存可视化数据
        holeVisData.push_back(std::make_pair(hole.center, hole.radius));

        if (debugParam.enableDebug && debugParam.printDetailLog) {
            LOG_INFO("[Algo Thread] Hole[%d] Normal (eye): (%.3f, %.3f, %.3f)\n",
                    i, hole.normal.x, hole.normal.y, hole.normal.z);
            LOG_INFO("[Algo Thread] Hole[%d] Direction vectors (robot): X=(%.3f,%.3f,%.3f), Y=(%.3f,%.3f,%.3f), Z=(%.3f,%.3f,%.3f)\n",
                    i,
                    dirVectors_robot[0].x, dirVectors_robot[0].y, dirVectors_robot[0].z,
                    dirVectors_robot[1].x, dirVectors_robot[1].y, dirVectors_robot[1].z,
                    dirVectors_robot[2].x, dirVectors_robot[2].y, dirVectors_robot[2].z);
            LOG_INFO("[Algo Thread] Hole[%d] Center Eye   Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
                    i, hole.center.x, hole.center.y, hole.center.z);
            LOG_INFO("[Algo Thread] Hole[%d] Center Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, R=%.4f, P=%.4f, Y=%.4f\n",
                    i, ptRobot.x, ptRobot.y, ptRobot.z,
                    centerRobotPos.roll, centerRobotPos.pitch, centerRobotPos.yaw);
            LOG_INFO("[Algo Thread] Hole[%d] Radius=%.2f, Depth=%.2f, Quality=%.3f\n",
                    i, hole.radius, hole.depth, hole.qualityScore);
        }
    }

    // 释放算法结果内存
    FreeMultiHoleResult(&multiResult);

    // 从点云数据生成投影图像（热力图着色 + 孔位标记）
    {
        std::vector<HoleMarkerInfo> holes;
        for (const auto& vis : holeVisData) {
            HoleMarkerInfo h;
            h.center.x = vis.first.x;
            h.center.y = vis.first.y;
            h.center.z = vis.first.z;
            h.radius = vis.second;
            holes.push_back(h);
        }
        detectionResult.image = PointCloudImageUtils::GenerateHoleDetectionImage(xyzData, holes);
    }

    if (debugParam.enableDebug && debugParam.saveDebugImage) {
        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");
        }
    }

    // 释放网格数组
    delete[] points;

    return nRet;
}