#include "DetectPresenter.h"
#include "workpieceHolePositioning_Export.h"
#include "SG_baseAlgo_Export.h"
#include "SG_errCode.h"
#include "AlgoParamConverter.h"
#include "IHandEyeCalib.h"
#include "WorkpieceProduct.h"
#include <cmath>
#include <fstream>
#include <memory>

namespace {

HECEulerOrder ToHandEyeEulerOrder(int eulerOrder)
{
    switch (eulerOrder) {
    case 10: return HECEulerOrder::XYZ;
    case 11: return HECEulerOrder::ZYX;
    case 12: return HECEulerOrder::ZXY;
    case 13: return HECEulerOrder::YXZ;
    case 14: return HECEulerOrder::YZX;
    case 15: return HECEulerOrder::XZY;
    default:
        LOG_WARNING("Unsupported tool euler order %d, fallback to 11 (ZYX)\n", eulerOrder);
        return HECEulerOrder::ZYX;
    }
}

void ApplyToolRotationToEyeAxes(IHandEyeCalib& handEyeCalib,
                                HECEulerOrder eulerOrder,
                                const VrToolParam& toolParam,
                                std::vector<HECPoint3D>& axes)
{
    if (axes.size() != 3 ||
        (std::fabs(toolParam.rotX) < 1e-9 &&
         std::fabs(toolParam.rotY) < 1e-9 &&
         std::fabs(toolParam.rotZ) < 1e-9)) {
        return;
    }

    HECRotationMatrix toolRotation;
    handEyeCalib.EulerToRotationMatrix(
        HECEulerAngles::fromDegrees(toolParam.rotX, toolParam.rotY, toolParam.rotZ),
        eulerOrder,
        toolRotation);

    const std::vector<HECPoint3D> originalAxes = axes;
    for (int column = 0; column < 3; ++column) {
        axes[column] = (originalAxes[0] * toolRotation.at(0, column)
                      + originalAxes[1] * toolRotation.at(1, column)
                      + originalAxes[2] * toolRotation.at(2, column)).normalized();
    }
}

bool HasValidZPoint(const std::vector<SVzNL3DPosition>& line)
{
    for (const SVzNL3DPosition& point : line) {
        if (point.pt3D.z > 1e-4) {
            return true;
        }
    }
    return false;
}

void TrimZeroBoundaryLines(std::vector<std::vector<SVzNL3DPosition>>& scanLines)
{
    size_t firstValidLine = 0;
    while (firstValidLine < scanLines.size() && !HasValidZPoint(scanLines[firstValidLine])) {
        ++firstValidLine;
    }
    if (firstValidLine == scanLines.size()) {
        scanLines.clear();
        return;
    }

    size_t lastValidLine = scanLines.size();
    while (lastValidLine > firstValidLine && !HasValidZPoint(scanLines[lastValidLine - 1])) {
        --lastValidLine;
    }

    scanLines.erase(scanLines.begin() + lastValidLine, scanLines.end());
    scanLines.erase(scanLines.begin(), scanLines.begin() + firstValidLine);
}

} // namespace

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

DetectPresenter::~DetectPresenter()
{
}

QString DetectPresenter::GetAlgoVersion()
{
    const char* ver = wd_workpieceHolePositioningVersion();
    return ver ? QString::fromUtf8(ver) : QString();
}


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

    const VrWorkpieceAlgorithmProfile* profile =
        algorithmParams.FindWorkpieceProfile(workpieceNumber);
    if (!profile) {
        LOG_ERROR("Algorithm profile is missing for workpiece %d\n", workpieceNumber);
        return ERR_CODE(DEV_CONFIG_ERR);
    }
    const VrToolParam& toolParam = profile->toolParam;
    const HECEulerOrder toolEulerOrder = ToHandEyeEulerOrder(toolParam.eulerOrder);

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

    // debug保存点云已由 BasePresenter::DetectTask() 统一处理，此处不再重复保存
    std::string timeStamp = CVrDateUtils::GetNowTime();

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

    const size_t originalLineCount = xyzData.size();
    TrimZeroBoundaryLines(xyzData);
    if (xyzData.empty()) {
        LOG_WARNING("No valid scan lines after trimming zero boundary lines\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }
    LOG_INFO("Trimmed zero boundary lines: %zu -> %zu\n",
             originalLineCount, xyzData.size());

    // 算法接口使用非 const 引用，因此单独准备算法工作数据。
    // xyzData 始终保留原始点云用于绘图，所有算法结果保持原样进入公共处理流程。
    std::vector<std::vector<SVzNL3DPosition>> algorithmData = xyzData;

    // 工件孔参数
    const VrWorkpieceHoleParam& workpieceParam = profile->workpieceHoleParam;
    WD_workpieceHoleParam workpiecePara = AlgoParamConverter::ToAlgoParam(workpieceParam);

    // 线段分割参数
    SSG_lineSegParam lineSegPara = AlgoParamConverter::ToAlgoParam(profile->lineSegParam);

    // 滤波参数
    SSG_outlierFilterParam filterParam = AlgoParamConverter::ToAlgoParam(profile->filterParam);

    // 树生长参数
    SSG_treeGrowParam growParam = AlgoParamConverter::ToAlgoParam(profile->growParam);

    LOG_INFO("Workpiece %d profile: hole=%.1f, H=%.1f, bigHole=%.1f, line=(%.1f,%.1f,%.1f), "
             "filter=(%.1f,%.1f), grow=(%d,%.1f,%.1f,%.1f,%.1f,%.1f)\n",
             workpieceNumber, workpieceParam.holeDiameter, workpieceParam.H,
             workpieceParam.bigHoleDiameter,
             lineSegPara.distScale, lineSegPara.segGapTh_y, lineSegPara.segGapTh_z,
             filterParam.continuityTh, filterParam.outlierTh,
             growParam.maxLineSkipNum, growParam.yDeviation_max,
             growParam.maxSkipDistance, growParam.zDeviation_max,
             growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
    LOG_INFO("Workpiece %d tool params: eulerOrder=%d, rot=(%.3f,%.3f,%.3f), offset=(%.3f,%.3f,%.3f)\n",
             workpieceNumber, toolParam.eulerOrder,
             toolParam.rotX, toolParam.rotY, toolParam.rotZ,
             toolParam.offsetX, toolParam.offsetY, toolParam.offsetZ);

    // 准备平面校准参数
    SSG_planeCalibPara groundCalibPara = AlgoParamConverter::ToAlgoPlaneCalibParam(cameraCalibParam);

    if(debugParam.enableDebug && debugParam.printDetailLog)
    {
        AlgoParamConverter::LogAlgoParams("[Algo Thread]",
            workpiecePara, lineSegPara, filterParam, growParam, groundCalibPara, clibMatrix);
    }
#if 0
    // 数据预处理：调平和去除地面（使用当前相机的调平参数）
    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, -1);
        }
    }
#endif

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("Before workpiece positioning algorithm, workpiece=%d\n", workpieceNumber);

    std::vector<WD_workpieceInfo> workpiecePositioning;
    const char* algorithmName = nullptr;
    const WorkpieceProduct::ProductInfo* product = WorkpieceProduct::Find(workpieceNumber);
    if (!product || !product->detectionSupported) {
        LOG_ERROR("No product or detection algorithm for software workpiece %d\n", workpieceNumber);
        return ERR_CODE(FUN_UNSUPPORT);
    }

    switch (product->algorithmApi) {
    case WorkpieceProduct::AlgorithmApi::Api1:
        algorithmName = "wd_workpieceHolePositioning";
        wd_workpieceHolePositioning(
            algorithmData, workpiecePara, lineSegPara, filterParam, growParam,
            groundCalibPara, workpiecePositioning, &errCode);
        break;
    case WorkpieceProduct::AlgorithmApi::Api2:
        algorithmName = "wd_workpieceHolePositioning_2";
        wd_workpieceHolePositioning_2(
            algorithmData, workpiecePara, lineSegPara, filterParam, growParam,
            groundCalibPara, workpiecePositioning, &errCode);
        break;
    case WorkpieceProduct::AlgorithmApi::Api3:
        algorithmName = "wd_workpieceHolePositioning_3";
        wd_workpieceHolePositioning_3(
            algorithmData, workpiecePara, workpieceParam.bigHoleDiameter,
            lineSegPara, filterParam, growParam, groundCalibPara,
            workpiecePositioning, &errCode);
        break;
    default:
        LOG_ERROR("Unsupported algorithm API for software workpiece %d\n", workpieceNumber);
        return ERR_CODE(FUN_UNSUPPORT);
    }

    LOG_INFO("%s: found %zu workpieces, err=%d runtime=%.3fms\n",
             algorithmName, workpiecePositioning.size(), errCode,
             oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    // 创建手眼标定实例
    std::unique_ptr<IHandEyeCalib, decltype(&DestroyHandEyeCalibInstance)> handEyeCalib(
                                                CreateHandEyeCalibInstance(),
                                                DestroyHandEyeCalibInstance);
    if (!handEyeCalib) {
        LOG_ERROR("Failed to create HandEyeCalib instance\n");
        return ERR_CODE(DEV_NOT_FIND);
    }

    const HECCalibResult calibResult = HECCalibResult::fromHomogeneousArray(clibMatrix);

    // 处理每个工件的检测结果
    for (size_t i = 0; i < workpiecePositioning.size(); i++) {
        const WD_workpieceInfo& workpiece = workpiecePositioning[i];

        // 保存工件类型（仅保存第一个工件的类型）
        if (i == 0) {
            detectionResult.workpieceType = workpiece.workpieceType;
        }
        if (IsReversedWorkpieceType(workpiece.workpieceType)) {
            // 算法执行成功，但反向工件属于异常物料；保留结果继续生成可视化图片。
            detectionResult.errorCode = SX_ERR_UNKNOWN_OBJECT;
            LOG_WARNING("Detected reversed workpiece %zu (workpieceType=%d)\n",
                        i + 1, workpiece.workpieceType);
        }

        // 六轴转换：位置+姿态（转换到机械臂坐标系）
        HECPoint3D eyeCenter(workpiece.center.x, workpiece.center.y, workpiece.center.z);

        // 构建方向向量（眼坐标系）
        std::vector<HECPoint3D> dirVectors_eye(3);
        dirVectors_eye[0] = HECPoint3D(workpiece.x_dir.x, workpiece.x_dir.y, workpiece.x_dir.z);
        dirVectors_eye[1] = HECPoint3D(workpiece.y_dir.x, workpiece.y_dir.y, workpiece.y_dir.z);
        dirVectors_eye[2] = HECPoint3D(workpiece.z_dir.x, workpiece.z_dir.y, workpiece.z_dir.z);

        // 根据配置决定方向向量反向
        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;
        }

        // 工具姿态补偿在 Eye 坐标系内合成，再进入手眼旋转链。
        ApplyToolRotationToEyeAxes(*handEyeCalib, toolEulerOrder, toolParam, dirVectors_eye);

        // 1. 位置转换：R * P_eye + T
        HECPoint3D ptRobot;
        handEyeCalib->TransformPoint(calibResult.R, calibResult.T, eyeCenter, ptRobot);

        // 2. 方向向量旋转变换（仅旋转，不平移）
        std::vector<HECPoint3D> dirVectors_robot(3);
        for (int j = 0; j < 3; j++) {
            handEyeCalib->RotatePoint(calibResult.R, dirVectors_eye[j], dirVectors_robot[j]);
        }

        // 3. 构建旋转矩阵（列向量形式）
        double R_pose[3][3];
        R_pose[0][0] = dirVectors_robot[0].x;
        R_pose[0][1] = dirVectors_robot[1].x;
        R_pose[0][2] = dirVectors_robot[2].x;
        R_pose[1][0] = dirVectors_robot[0].y;
        R_pose[1][1] = dirVectors_robot[1].y;
        R_pose[1][2] = dirVectors_robot[2].y;
        R_pose[2][0] = dirVectors_robot[0].z;
        R_pose[2][1] = dirVectors_robot[1].z;
        R_pose[2][2] = dirVectors_robot[2].z;

        // 4. 保持既有 ZYX 输出分解约定，确保零工具补偿时输出完全兼容。
        // toolParam.eulerOrder 只控制上面的工具补偿旋转合成顺序。
        const SSG_EulerAngles robotRpy = rotationMatrixToEulerZYX(R_pose);

        // 将机器人坐标系下的位姿添加到positions列表
        // 根据 poseOutputOrder 重映射欧拉角输出顺序
        HolePosition centerRobotPos;
        centerRobotPos.workpieceType = workpiece.workpieceType;
        centerRobotPos.x = ptRobot.x + toolParam.offsetX;
        centerRobotPos.y = ptRobot.y + toolParam.offsetY;
        centerRobotPos.z = ptRobot.z + toolParam.offsetZ;
        switch (poseOutputOrder) {
            case POSE_ORDER_RPY:
                centerRobotPos.roll  = robotRpy.roll;
                centerRobotPos.pitch = robotRpy.pitch;
                centerRobotPos.yaw   = robotRpy.yaw;
                break;
            case POSE_ORDER_RYP:
                centerRobotPos.roll  = robotRpy.roll;
                centerRobotPos.pitch = robotRpy.yaw;
                centerRobotPos.yaw   = robotRpy.pitch;
                break;
            case POSE_ORDER_PRY:
                centerRobotPos.roll  = robotRpy.pitch;
                centerRobotPos.pitch = robotRpy.roll;
                centerRobotPos.yaw   = robotRpy.yaw;
                break;
            case POSE_ORDER_PYR:
                centerRobotPos.roll  = robotRpy.pitch;
                centerRobotPos.pitch = robotRpy.yaw;
                centerRobotPos.yaw   = robotRpy.roll;
                break;
            case POSE_ORDER_YRP:
                centerRobotPos.roll  = robotRpy.yaw;
                centerRobotPos.pitch = robotRpy.roll;
                centerRobotPos.yaw   = robotRpy.pitch;
                break;
            case POSE_ORDER_YPR:
                centerRobotPos.roll  = robotRpy.yaw;
                centerRobotPos.pitch = robotRpy.pitch;
                centerRobotPos.yaw   = robotRpy.roll;
                break;
            default:
                centerRobotPos.roll  = robotRpy.roll;
                centerRobotPos.pitch = robotRpy.pitch;
                centerRobotPos.yaw   = robotRpy.yaw;
                break;
        }

        detectionResult.positions.push_back(centerRobotPos);

        if(debugParam.enableDebug && debugParam.printDetailLog){
            LOG_INFO("[Algo Thread] Direction vectors (eye): X=(%.3f,%.3f,%.3f), Y=(%.3f,%.3f,%.3f), Z=(%.3f,%.3f,%.3f)\n",
                    dirVectors_eye[0].x, dirVectors_eye[0].y, dirVectors_eye[0].z,
                    dirVectors_eye[1].x, dirVectors_eye[1].y, dirVectors_eye[1].z,
                    dirVectors_eye[2].x, dirVectors_eye[2].y, dirVectors_eye[2].z);
            LOG_INFO("[Algo Thread] Direction vectors (robot): X=(%.3f,%.3f,%.3f), Y=(%.3f,%.3f,%.3f), Z=(%.3f,%.3f,%.3f)\n",
                    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] Center Eye   Coords: X=%.3f, Y=%.3f, Z=%.3f\n",
                    workpiece.center.x, workpiece.center.y, workpiece.center.z);
            LOG_INFO("[Algo Thread] Center Robot Coords: X=%.3f, Y=%.3f, Z=%.3f, R=%.4f, P=%.4f, Y=%.4f\n",
                    centerRobotPos.x, centerRobotPos.y, centerRobotPos.z,
                    centerRobotPos.roll, centerRobotPos.pitch, centerRobotPos.yaw);
        }
    }

    // 使用 PointCloudCanvas 生成点云图像（正向红点、反向蓝点 + 中心点编号）
    {
        PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
        for (size_t i = 0; i < workpiecePositioning.size(); i++) {
            const WD_workpieceInfo& workpiece = workpiecePositioning[i];
            const QColor markerColor = IsReversedWorkpieceType(workpiece.workpieceType)
                                           ? QColor(0, 0, 255)
                                           : QColor(255, 0, 0);
            // 小圆点标记每个孔
            for (size_t j = 0; j < workpiece.holes.size(); j++) {
                canvas.drawPoint(workpiece.holes[j].x, workpiece.holes[j].y, markerColor, 4);
            }
            // 中心点标记 + 白色编号
            canvas.drawPoint(workpiece.center.x, workpiece.center.y, markerColor, 4);
            canvas.drawText(workpiece.center.x, workpiece.center.y,
                            QString("%1").arg(i + 1), Qt::white, 14);
        }
        detectionResult.image = canvas.image();
    }

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