#include "VrConfig.h"
#include <iostream>
#include <vector>
#include <string>
#include <cstdio>

#include "VrLog.h"

using namespace tinyxml2;

CVrConfig::CVrConfig() : m_pNotify(nullptr)
{
    fprintf(stderr, "[DIAG-VrConfig] CVrConfig constructor\n"); fflush(stderr);
}

CVrConfig::~CVrConfig()
{
    // 析构函数
}

int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
{
    fprintf(stderr, "[DIAG-VrConfig] LoadConfig entry, path=%s\n", filePath.c_str()); fflush(stderr);

    // 使用tinyxml2库加载XML文件
    fprintf(stderr, "[DIAG-VrConfig] Creating XMLDocument...\n"); fflush(stderr);
    XMLDocument doc;
    fprintf(stderr, "[DIAG-VrConfig] XMLDocument created, calling LoadFile...\n"); fflush(stderr);
    XMLError err = doc.LoadFile(filePath.c_str());
    fprintf(stderr, "[DIAG-VrConfig] LoadFile returned %d\n", (int)err); fflush(stderr);
    if (err != XML_SUCCESS)
    {
        LOG_ERR("open config file failed: %s\n", filePath.c_str());
        fprintf(stderr, "[DIAG-VrConfig] LoadFile failed, returning LOAD_CONFIG_FILE_NOT_FOUND\n"); fflush(stderr);
        return LOAD_CONFIG_FILE_NOT_FOUND;
    }

    // 获取根元素
    fprintf(stderr, "[DIAG-VrConfig] Getting root element...\n"); fflush(stderr);
    XMLElement* root = doc.RootElement();
    if (!root || std::string(root->Name()) != "HoleDetectionConfig")
    {
        std::cerr << "config file format error: root element is not HoleDetectionConfig" << std::endl;
        fprintf(stderr, "[DIAG-VrConfig] Root element invalid\n"); fflush(stderr);
        return LOAD_CONFIG_INVALID_FORMAT;
    }
    fprintf(stderr, "[DIAG-VrConfig] Root element OK, parsing sections...\n"); fflush(stderr);

    // 解析摄像头列表
    fprintf(stderr, "[DIAG-VrConfig] Parsing Cameras...\n"); fflush(stderr);
    XMLElement* camerasElement = root->FirstChildElement("Cameras");
    if (camerasElement)
    {
        XMLElement* cameraElement = camerasElement->FirstChildElement("Camera");
        while (cameraElement)
        {
            DeviceInfo camera;
            if (cameraElement->Attribute("name"))
                camera.name = cameraElement->Attribute("name");

            if (cameraElement->Attribute("ip"))
                camera.ip = cameraElement->Attribute("ip");

            configResult.cameraList.push_back(camera);
            cameraElement = cameraElement->NextSiblingElement("Camera");
        }
    }

    // 解析设备列表
    fprintf(stderr, "[DIAG-VrConfig] Parsing Devices...\n"); fflush(stderr);
    XMLElement* devicesElement = root->FirstChildElement("Devices");
    if (devicesElement)
    {
        XMLElement* deviceElement = devicesElement->FirstChildElement("Device");
        while (deviceElement)
        {
            DeviceInfo device;
            if (deviceElement->Attribute("name"))
                device.name = deviceElement->Attribute("name");

            if (deviceElement->Attribute("ip"))
                device.ip = deviceElement->Attribute("ip");

            configResult.deviceList.push_back(device);
            deviceElement = deviceElement->NextSiblingElement("Device");
        }
    }

    // 解析算法参数
    fprintf(stderr, "[DIAG-VrConfig] Parsing AlgorithmParams...\n"); fflush(stderr);
    XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
    if (algoParamsElement)
    {
        // 解析孔洞检测参数（14个）
        XMLElement* detectionParamElement = algoParamsElement->FirstChildElement("DetectionParam");
        if (detectionParamElement)
        {
            if (detectionParamElement->Attribute("neighborCount"))
                configResult.algorithmParams.detectionParam.neighborCount = detectionParamElement->IntAttribute("neighborCount");
            if (detectionParamElement->Attribute("angleThresholdPos"))
                configResult.algorithmParams.detectionParam.angleThresholdPos = detectionParamElement->DoubleAttribute("angleThresholdPos");
            if (detectionParamElement->Attribute("angleThresholdNeg"))
                configResult.algorithmParams.detectionParam.angleThresholdNeg = detectionParamElement->DoubleAttribute("angleThresholdNeg");
            if (detectionParamElement->Attribute("minPitDepth"))
                configResult.algorithmParams.detectionParam.minPitDepth = detectionParamElement->DoubleAttribute("minPitDepth");
            if (detectionParamElement->Attribute("minRadius"))
                configResult.algorithmParams.detectionParam.minRadius = detectionParamElement->DoubleAttribute("minRadius");
            if (detectionParamElement->Attribute("maxRadius"))
                configResult.algorithmParams.detectionParam.maxRadius = detectionParamElement->DoubleAttribute("maxRadius");
            if (detectionParamElement->Attribute("expansionSize1"))
                configResult.algorithmParams.detectionParam.expansionSize1 = detectionParamElement->IntAttribute("expansionSize1");
            if (detectionParamElement->Attribute("expansionSize2"))
                configResult.algorithmParams.detectionParam.expansionSize2 = detectionParamElement->IntAttribute("expansionSize2");
            if (detectionParamElement->Attribute("minVTransitionPoints"))
                configResult.algorithmParams.detectionParam.minVTransitionPoints = detectionParamElement->IntAttribute("minVTransitionPoints");
        }

        // 解析孔洞过滤参数
        XMLElement* filterParamElement = algoParamsElement->FirstChildElement("FilterParam");
        if (filterParamElement)
        {
            if (filterParamElement->Attribute("maxEccentricity"))
                configResult.algorithmParams.filterParam.maxEccentricity = filterParamElement->DoubleAttribute("maxEccentricity");
            if (filterParamElement->Attribute("minAngularCoverage"))
                configResult.algorithmParams.filterParam.minAngularCoverage = filterParamElement->DoubleAttribute("minAngularCoverage");
            if (filterParamElement->Attribute("maxRadiusFitRatio"))
                configResult.algorithmParams.filterParam.maxRadiusFitRatio = filterParamElement->DoubleAttribute("maxRadiusFitRatio");
            if (filterParamElement->Attribute("minQualityScore"))
                configResult.algorithmParams.filterParam.minQualityScore = filterParamElement->DoubleAttribute("minQualityScore");
            if (filterParamElement->Attribute("maxPlaneResidual"))
                configResult.algorithmParams.filterParam.maxPlaneResidual = filterParamElement->DoubleAttribute("maxPlaneResidual");
            if (filterParamElement->Attribute("maxAngularGap"))
                configResult.algorithmParams.filterParam.maxAngularGap = filterParamElement->DoubleAttribute("maxAngularGap");
            if (filterParamElement->Attribute("minInlierRatio"))
                configResult.algorithmParams.filterParam.minInlierRatio = filterParamElement->DoubleAttribute("minInlierRatio");
        }

        // 解析排序模式
        XMLElement* sortModeElement = algoParamsElement->FirstChildElement("SortMode");
        if (sortModeElement)
        {
            if (sortModeElement->Attribute("value"))
                configResult.algorithmParams.sortMode = sortModeElement->IntAttribute("value");
        }

        // 解析多相机平面校准参数
        XMLElement* planeCalibParamsElement = algoParamsElement->FirstChildElement("PlaneCalibParams");
        if (planeCalibParamsElement)
        {
            XMLElement* cameraCalibParamElement = planeCalibParamsElement->FirstChildElement("CameraCalibParam");
            while (cameraCalibParamElement)
            {
                VrCameraPlaneCalibParam cameraParam;

                // 读取相机基础信息
                if (cameraCalibParamElement->Attribute("cameraIndex"))
                    cameraParam.cameraIndex = cameraCalibParamElement->IntAttribute("cameraIndex");
                if (cameraCalibParamElement->Attribute("cameraName"))
                    cameraParam.cameraName = cameraCalibParamElement->Attribute("cameraName");
                if (cameraCalibParamElement->Attribute("isCalibrated"))
                    cameraParam.isCalibrated = cameraCalibParamElement->BoolAttribute("isCalibrated");
                if (cameraCalibParamElement->Attribute("planeHeight"))
                    cameraParam.planeHeight = cameraCalibParamElement->DoubleAttribute("planeHeight");

                // 读取旋转矩阵planeCalib[9] (3x3矩阵)
                if (cameraCalibParamElement->Attribute("planeCalib_00"))
                    cameraParam.planeCalib[0] = cameraCalibParamElement->DoubleAttribute("planeCalib_00");
                if (cameraCalibParamElement->Attribute("planeCalib_01"))
                    cameraParam.planeCalib[1] = cameraCalibParamElement->DoubleAttribute("planeCalib_01");
                if (cameraCalibParamElement->Attribute("planeCalib_02"))
                    cameraParam.planeCalib[2] = cameraCalibParamElement->DoubleAttribute("planeCalib_02");
                if (cameraCalibParamElement->Attribute("planeCalib_10"))
                    cameraParam.planeCalib[3] = cameraCalibParamElement->DoubleAttribute("planeCalib_10");
                if (cameraCalibParamElement->Attribute("planeCalib_11"))
                    cameraParam.planeCalib[4] = cameraCalibParamElement->DoubleAttribute("planeCalib_11");
                if (cameraCalibParamElement->Attribute("planeCalib_12"))
                    cameraParam.planeCalib[5] = cameraCalibParamElement->DoubleAttribute("planeCalib_12");
                if (cameraCalibParamElement->Attribute("planeCalib_20"))
                    cameraParam.planeCalib[6] = cameraCalibParamElement->DoubleAttribute("planeCalib_20");
                if (cameraCalibParamElement->Attribute("planeCalib_21"))
                    cameraParam.planeCalib[7] = cameraCalibParamElement->DoubleAttribute("planeCalib_21");
                if (cameraCalibParamElement->Attribute("planeCalib_22"))
                    cameraParam.planeCalib[8] = cameraCalibParamElement->DoubleAttribute("planeCalib_22");

                // 读取逆旋转矩阵invRMatrix[9] (3x3矩阵)
                if (cameraCalibParamElement->Attribute("invRMatrix_00"))
                    cameraParam.invRMatrix[0] = cameraCalibParamElement->DoubleAttribute("invRMatrix_00");
                if (cameraCalibParamElement->Attribute("invRMatrix_01"))
                    cameraParam.invRMatrix[1] = cameraCalibParamElement->DoubleAttribute("invRMatrix_01");
                if (cameraCalibParamElement->Attribute("invRMatrix_02"))
                    cameraParam.invRMatrix[2] = cameraCalibParamElement->DoubleAttribute("invRMatrix_02");
                if (cameraCalibParamElement->Attribute("invRMatrix_10"))
                    cameraParam.invRMatrix[3] = cameraCalibParamElement->DoubleAttribute("invRMatrix_10");
                if (cameraCalibParamElement->Attribute("invRMatrix_11"))
                    cameraParam.invRMatrix[4] = cameraCalibParamElement->DoubleAttribute("invRMatrix_11");
                if (cameraCalibParamElement->Attribute("invRMatrix_12"))
                    cameraParam.invRMatrix[5] = cameraCalibParamElement->DoubleAttribute("invRMatrix_12");
                if (cameraCalibParamElement->Attribute("invRMatrix_20"))
                    cameraParam.invRMatrix[6] = cameraCalibParamElement->DoubleAttribute("invRMatrix_20");
                if (cameraCalibParamElement->Attribute("invRMatrix_21"))
                    cameraParam.invRMatrix[7] = cameraCalibParamElement->DoubleAttribute("invRMatrix_21");
                if (cameraCalibParamElement->Attribute("invRMatrix_22"))
                    cameraParam.invRMatrix[8] = cameraCalibParamElement->DoubleAttribute("invRMatrix_22");

                // 添加到结果中
                configResult.algorithmParams.planeCalibParam.cameraCalibParams.push_back(cameraParam);

                // 移动到下一个相机配置
                cameraCalibParamElement = cameraCalibParamElement->NextSiblingElement("CameraCalibParam");
            }
        }
    }

    // 解析调试参数（在AlgorithmParams外面）
    fprintf(stderr, "[DIAG-VrConfig] Parsing DebugParam...\n"); fflush(stderr);
    XMLElement* debugParamElement = root->FirstChildElement("DebugParam");
    if (debugParamElement)
    {
        if (debugParamElement->Attribute("enableDebug"))
            configResult.debugParam.enableDebug = debugParamElement->BoolAttribute("enableDebug");
        if (debugParamElement->Attribute("savePointCloud"))
            configResult.debugParam.savePointCloud = debugParamElement->BoolAttribute("savePointCloud");
        if (debugParamElement->Attribute("saveDebugImage"))
            configResult.debugParam.saveDebugImage = debugParamElement->BoolAttribute("saveDebugImage");
        if (debugParamElement->Attribute("printDetailLog"))
            configResult.debugParam.printDetailLog = debugParamElement->BoolAttribute("printDetailLog");
        if (debugParamElement->Attribute("debugOutputPath"))
            configResult.debugParam.debugOutputPath = debugParamElement->Attribute("debugOutputPath");
    }

    // 解析串口配置
    fprintf(stderr, "[DIAG-VrConfig] Parsing SerialConfig...\n"); fflush(stderr);
    XMLElement* serialConfigElement = root->FirstChildElement("SerialConfig");
    if (serialConfigElement)
    {
        if (serialConfigElement->Attribute("portName"))
            configResult.serialConfig.portName = serialConfigElement->Attribute("portName");
        if (serialConfigElement->Attribute("baudRate"))
            configResult.serialConfig.baudRate = serialConfigElement->IntAttribute("baudRate");
        if (serialConfigElement->Attribute("dataBits"))
            configResult.serialConfig.dataBits = serialConfigElement->IntAttribute("dataBits");
        if (serialConfigElement->Attribute("stopBits"))
            configResult.serialConfig.stopBits = serialConfigElement->IntAttribute("stopBits");
        if (serialConfigElement->Attribute("parity"))
            configResult.serialConfig.parity = serialConfigElement->IntAttribute("parity");
        if (serialConfigElement->Attribute("flowControl"))
            configResult.serialConfig.flowControl = serialConfigElement->IntAttribute("flowControl");
        if (serialConfigElement->Attribute("enabled"))
            configResult.serialConfig.enabled = serialConfigElement->BoolAttribute("enabled");
    }

    // 解析手眼标定矩阵列表（支持多相机）
    fprintf(stderr, "[DIAG-VrConfig] Parsing HandEyeCalibMatrixList...\n"); fflush(stderr);
    XMLElement* handEyeListElement = root->FirstChildElement("HandEyeCalibMatrixList");
    if (handEyeListElement)
    {
        XMLElement* handEyeElement = handEyeListElement->FirstChildElement("HandEyeCalibMatrix");
        while (handEyeElement)
        {
            VrHandEyeCalibMatrix calibMatrix;
            for (int i = 0; i < 16; i++)
            {
                char attrName[32];
                sprintf(attrName, "m%d", i);
                if (handEyeElement->Attribute(attrName))
                    calibMatrix.matrix[i] = handEyeElement->DoubleAttribute(attrName);
            }
            // 读取旋转顺序，如果不存在则使用默认值11（外旋ZYX）
            if (handEyeElement->Attribute("eulerOrder"))
                calibMatrix.eulerOrder = handEyeElement->IntAttribute("eulerOrder");
            else
                calibMatrix.eulerOrder = 11;  // 默认外旋ZYX

            configResult.handEyeCalibMatrixList.push_back(calibMatrix);
            handEyeElement = handEyeElement->NextSiblingElement("HandEyeCalibMatrix");
        }
    }
    else
    {
        // 兼容旧版本：单个 HandEyeCalibMatrix 元素
        XMLElement* handEyeElement = root->FirstChildElement("HandEyeCalibMatrix");
        if (handEyeElement)
        {
            VrHandEyeCalibMatrix calibMatrix;
            for (int i = 0; i < 16; i++)
            {
                char attrName[32];
                sprintf(attrName, "m%d", i);
                if (handEyeElement->Attribute(attrName))
                    calibMatrix.matrix[i] = handEyeElement->DoubleAttribute(attrName);
            }
            if (handEyeElement->Attribute("eulerOrder"))
                calibMatrix.eulerOrder = handEyeElement->IntAttribute("eulerOrder");
            else
                calibMatrix.eulerOrder = 11;
            configResult.handEyeCalibMatrixList.push_back(calibMatrix);
        }
    }

    // 解析TCP服务端配置
    fprintf(stderr, "[DIAG-VrConfig] Parsing TcpServerConfig...\n"); fflush(stderr);
    XMLElement* tcpServerConfigElement = root->FirstChildElement("TcpServerConfig");
    if (tcpServerConfigElement)
    {
        // 解析TCP协议端口
        if (tcpServerConfigElement->Attribute("tcpServerPort"))
            configResult.plcRobotServerConfig.tcpServerPort = tcpServerConfigElement->IntAttribute("tcpServerPort");
        else
            configResult.plcRobotServerConfig.tcpServerPort = 7800;  // 默认7800

        // 解析姿态输出顺序配置
        if (tcpServerConfigElement->Attribute("poseOutputOrder"))
            configResult.plcRobotServerConfig.poseOutputOrder = tcpServerConfigElement->IntAttribute("poseOutputOrder");
        else
            configResult.plcRobotServerConfig.poseOutputOrder = POSE_ORDER_RPY;  // 默认RPY

        // 解析方向向量反向配置
        if (tcpServerConfigElement->Attribute("dirVectorInvert"))
            configResult.plcRobotServerConfig.dirVectorInvert = tcpServerConfigElement->IntAttribute("dirVectorInvert");
        else
            configResult.plcRobotServerConfig.dirVectorInvert = DIR_INVERT_YZ;  // 默认YZ反向
    }

    fprintf(stderr, "[DIAG-VrConfig] LoadConfig completed successfully\n"); fflush(stderr);
    return LOAD_CONFIG_SUCCESS;
}

bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResult)
{
    // 创建XML文档
    XMLDocument doc;

    // 添加声明
    XMLDeclaration* declaration = doc.NewDeclaration("xml version=\"1.0\" encoding=\"UTF-8\"");
    doc.InsertFirstChild(declaration);

    // 创建根元素
    XMLElement* root = doc.NewElement("HoleDetectionConfig");
    doc.InsertEndChild(root);

    // 添加摄像头列表
    XMLElement* camerasElement = doc.NewElement("Cameras");
    root->InsertEndChild(camerasElement);

    for (const auto& camera : configResult.cameraList)
    {
        XMLElement* cameraElement = doc.NewElement("Camera");
        cameraElement->SetAttribute("name", camera.name.c_str());
        cameraElement->SetAttribute("ip", camera.ip.c_str());
        camerasElement->InsertEndChild(cameraElement);
    }

    // 添加设备列表
    XMLElement* devicesElement = doc.NewElement("Devices");
    root->InsertEndChild(devicesElement);

    for (const auto& device : configResult.deviceList)
    {
        XMLElement* deviceElement = doc.NewElement("Device");
        deviceElement->SetAttribute("name", device.name.c_str());
        deviceElement->SetAttribute("ip", device.ip.c_str());
        devicesElement->InsertEndChild(deviceElement);
    }

    // 添加算法参数
    XMLElement* algoParamsElement = doc.NewElement("AlgorithmParams");
    root->InsertEndChild(algoParamsElement);

    // 添加孔洞检测参数
    XMLElement* detectionParamElement = doc.NewElement("DetectionParam");
    detectionParamElement->SetAttribute("neighborCount", configResult.algorithmParams.detectionParam.neighborCount);
    detectionParamElement->SetAttribute("angleThresholdPos", configResult.algorithmParams.detectionParam.angleThresholdPos);
    detectionParamElement->SetAttribute("angleThresholdNeg", configResult.algorithmParams.detectionParam.angleThresholdNeg);
    detectionParamElement->SetAttribute("minPitDepth", configResult.algorithmParams.detectionParam.minPitDepth);
    detectionParamElement->SetAttribute("minRadius", configResult.algorithmParams.detectionParam.minRadius);
    detectionParamElement->SetAttribute("maxRadius", configResult.algorithmParams.detectionParam.maxRadius);
    detectionParamElement->SetAttribute("expansionSize1", configResult.algorithmParams.detectionParam.expansionSize1);
    detectionParamElement->SetAttribute("expansionSize2", configResult.algorithmParams.detectionParam.expansionSize2);
    detectionParamElement->SetAttribute("minVTransitionPoints", configResult.algorithmParams.detectionParam.minVTransitionPoints);
    algoParamsElement->InsertEndChild(detectionParamElement);

    // 添加孔洞过滤参数
    XMLElement* filterParamElement = doc.NewElement("FilterParam");
    filterParamElement->SetAttribute("maxEccentricity", configResult.algorithmParams.filterParam.maxEccentricity);
    filterParamElement->SetAttribute("minAngularCoverage", configResult.algorithmParams.filterParam.minAngularCoverage);
    filterParamElement->SetAttribute("maxRadiusFitRatio", configResult.algorithmParams.filterParam.maxRadiusFitRatio);
    filterParamElement->SetAttribute("minQualityScore", configResult.algorithmParams.filterParam.minQualityScore);
    filterParamElement->SetAttribute("maxPlaneResidual", configResult.algorithmParams.filterParam.maxPlaneResidual);
    filterParamElement->SetAttribute("maxAngularGap", configResult.algorithmParams.filterParam.maxAngularGap);
    filterParamElement->SetAttribute("minInlierRatio", configResult.algorithmParams.filterParam.minInlierRatio);
    algoParamsElement->InsertEndChild(filterParamElement);

    // 添加排序模式
    XMLElement* sortModeElement = doc.NewElement("SortMode");
    sortModeElement->SetAttribute("value", configResult.algorithmParams.sortMode);
    algoParamsElement->InsertEndChild(sortModeElement);

    // 添加多相机平面校准参数
    XMLElement* planeCalibParamsElement = doc.NewElement("PlaneCalibParams");

    for (const auto& cameraParam : configResult.algorithmParams.planeCalibParam.cameraCalibParams)
    {
        XMLElement* cameraCalibParamElement = doc.NewElement("CameraCalibParam");
        cameraCalibParamElement->SetAttribute("cameraIndex", cameraParam.cameraIndex);
        cameraCalibParamElement->SetAttribute("cameraName", cameraParam.cameraName.c_str());
        cameraCalibParamElement->SetAttribute("isCalibrated", cameraParam.isCalibrated);
        cameraCalibParamElement->SetAttribute("planeHeight", cameraParam.planeHeight);

        // 保存旋转矩阵planeCalib[9] (3x3矩阵)
        cameraCalibParamElement->SetAttribute("planeCalib_00", cameraParam.planeCalib[0]);
        cameraCalibParamElement->SetAttribute("planeCalib_01", cameraParam.planeCalib[1]);
        cameraCalibParamElement->SetAttribute("planeCalib_02", cameraParam.planeCalib[2]);
        cameraCalibParamElement->SetAttribute("planeCalib_10", cameraParam.planeCalib[3]);
        cameraCalibParamElement->SetAttribute("planeCalib_11", cameraParam.planeCalib[4]);
        cameraCalibParamElement->SetAttribute("planeCalib_12", cameraParam.planeCalib[5]);
        cameraCalibParamElement->SetAttribute("planeCalib_20", cameraParam.planeCalib[6]);
        cameraCalibParamElement->SetAttribute("planeCalib_21", cameraParam.planeCalib[7]);
        cameraCalibParamElement->SetAttribute("planeCalib_22", cameraParam.planeCalib[8]);

        // 保存逆旋转矩阵invRMatrix[9] (3x3矩阵)
        cameraCalibParamElement->SetAttribute("invRMatrix_00", cameraParam.invRMatrix[0]);
        cameraCalibParamElement->SetAttribute("invRMatrix_01", cameraParam.invRMatrix[1]);
        cameraCalibParamElement->SetAttribute("invRMatrix_02", cameraParam.invRMatrix[2]);
        cameraCalibParamElement->SetAttribute("invRMatrix_10", cameraParam.invRMatrix[3]);
        cameraCalibParamElement->SetAttribute("invRMatrix_11", cameraParam.invRMatrix[4]);
        cameraCalibParamElement->SetAttribute("invRMatrix_12", cameraParam.invRMatrix[5]);
        cameraCalibParamElement->SetAttribute("invRMatrix_20", cameraParam.invRMatrix[6]);
        cameraCalibParamElement->SetAttribute("invRMatrix_21", cameraParam.invRMatrix[7]);
        cameraCalibParamElement->SetAttribute("invRMatrix_22", cameraParam.invRMatrix[8]);

        planeCalibParamsElement->InsertEndChild(cameraCalibParamElement);
    }

    algoParamsElement->InsertEndChild(planeCalibParamsElement);

    // 添加调试参数（在AlgorithmParams外面）
    XMLElement* debugParamElement = doc.NewElement("DebugParam");
    debugParamElement->SetAttribute("enableDebug", configResult.debugParam.enableDebug);
    debugParamElement->SetAttribute("savePointCloud", configResult.debugParam.savePointCloud);
    debugParamElement->SetAttribute("saveDebugImage", configResult.debugParam.saveDebugImage);
    debugParamElement->SetAttribute("printDetailLog", configResult.debugParam.printDetailLog);
    debugParamElement->SetAttribute("debugOutputPath", configResult.debugParam.debugOutputPath.c_str());
    root->InsertEndChild(debugParamElement);

    // 添加串口配置
    XMLElement* serialConfigElement = doc.NewElement("SerialConfig");
    serialConfigElement->SetAttribute("portName", configResult.serialConfig.portName.c_str());
    serialConfigElement->SetAttribute("baudRate", configResult.serialConfig.baudRate);
    serialConfigElement->SetAttribute("dataBits", configResult.serialConfig.dataBits);
    serialConfigElement->SetAttribute("stopBits", configResult.serialConfig.stopBits);
    serialConfigElement->SetAttribute("parity", configResult.serialConfig.parity);
    serialConfigElement->SetAttribute("flowControl", configResult.serialConfig.flowControl);
    serialConfigElement->SetAttribute("enabled", configResult.serialConfig.enabled);
    root->InsertEndChild(serialConfigElement);

    // 添加手眼标定矩阵列表（支持多相机）
    XMLElement* handEyeListElement = doc.NewElement("HandEyeCalibMatrixList");
    for (size_t idx = 0; idx < configResult.handEyeCalibMatrixList.size(); idx++)
    {
        const auto& calibMatrix = configResult.handEyeCalibMatrixList[idx];
        XMLElement* handEyeElement = doc.NewElement("HandEyeCalibMatrix");
        handEyeElement->SetAttribute("cameraIndex", static_cast<int>(idx + 1));
        for (int i = 0; i < 16; i++)
        {
            char attrName[32];
            sprintf(attrName, "m%d", i);
            handEyeElement->SetAttribute(attrName, calibMatrix.matrix[i]);
        }
        // 保存旋转顺序
        handEyeElement->SetAttribute("eulerOrder", calibMatrix.eulerOrder);
        handEyeListElement->InsertEndChild(handEyeElement);
    }
    root->InsertEndChild(handEyeListElement);

    // 添加TCP服务端配置
    XMLElement* tcpServerConfigElement = doc.NewElement("TcpServerConfig");
    tcpServerConfigElement->SetAttribute("tcpServerPort", configResult.plcRobotServerConfig.tcpServerPort);
    // 保存姿态输出顺序配置
    tcpServerConfigElement->SetAttribute("poseOutputOrder", configResult.plcRobotServerConfig.poseOutputOrder);
    // 保存方向向量反向配置
    tcpServerConfigElement->SetAttribute("dirVectorInvert", configResult.plcRobotServerConfig.dirVectorInvert);
    root->InsertEndChild(tcpServerConfigElement);

    // 保存到文件
    XMLError err = doc.SaveFile(filePath.c_str());
    if (err != XML_SUCCESS)
    {
        std::cerr << "无法保存配置文件: " << filePath << std::endl;
        return false;
    }

    // 触发配置改变通知
    if (m_pNotify)
    {
        m_pNotify->OnConfigChanged(configResult);
    }

    return true;
}

// 设置配置改变通知回调
void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
{
    m_pNotify = notify;
}


/**
 * @brief 创建实例
 * @return 实例
 */
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
{
    *ppVrConfig = new CVrConfig();
    return *ppVrConfig != nullptr;
}