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

#include "VrLog.h"

using namespace tinyxml2;

namespace
{
DebugDataStorageType ParseDebugStorageType(const char* value)
{
    if (!value)
    {
        return DebugDataStorageType::Text;
    }
    const std::string type(value);
    return type == "1" || type == "binary" || type == "Binary"
        ? DebugDataStorageType::Binary : DebugDataStorageType::Text;
}

const char* DebugStorageTypeName(DebugDataStorageType type)
{
    return type == DebugDataStorageType::Binary ? "binary" : "text";
}
}

CVrConfig::CVrConfig() : m_pNotify(nullptr)
{
    // 构造函数
}

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

int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
{
    // 使用tinyxml2库加载XML文件
    XMLDocument doc;
    XMLError err = doc.LoadFile(filePath.c_str());
    if (err != XML_SUCCESS)
    {
        LOG_ERR("open config file failed: %s\n", filePath.c_str());
        return LOAD_CONFIG_FILE_NOT_FOUND;
    }

    // 获取根元素
    XMLElement* root = doc.RootElement();
    if (!root || std::string(root->Name()) != "StatorWorkpiecePositionConfig")
    {
        std::cerr << "config file format error: root element is not StatorWorkpiecePositionConfig" << std::endl;
        return LOAD_CONFIG_INVALID_FORMAT;
    }

    // 重读时清除按相机索引保存的数据，避免旧条目改变相机编号或覆盖新配置。
    configResult.cameraList.clear();
    configResult.algorithmParams.toolParamsByCamera.clear();
    configResult.handEyeCalibMatricesByCamera.clear();
    configResult.algorithmParams.toolParam = VrToolParam();
    configResult.handEyeCalibMatrix = VrHandEyeCalibMatrix();

    // 解析摄像头列表
    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");
        }
    }

    // 解析设备列表
    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");
        }
    }

    // 解析算法参数
    configResult.algorithmParams.pointCloudFilterParams.clear();
    configResult.algorithmParams.modelGraySelection = VrModelGraySelectionParam();
    configResult.algorithmParams.modelDetectionRegion = VrModelDetectionRegion();
    XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
    if (algoParamsElement)
    {
        for (XMLElement* filter = algoParamsElement->FirstChildElement("PointCloudFilterParam");
             filter != nullptr;
             filter = filter->NextSiblingElement("PointCloudFilterParam"))
        {
            VrPointCloudFilterParam param;
            filter->QueryIntAttribute("cameraIndex", &param.cameraIndex);
            if (const char* name = filter->Attribute("cameraName"))
            {
                param.cameraName = name;
            }
            filter->QueryBoolAttribute("enabled", &param.enabled);
            filter->QueryDoubleAttribute("minX", &param.minX);
            filter->QueryDoubleAttribute("maxX", &param.maxX);
            filter->QueryDoubleAttribute("minY", &param.minY);
            filter->QueryDoubleAttribute("maxY", &param.maxY);
            filter->QueryDoubleAttribute("minZ", &param.minZ);
            filter->QueryDoubleAttribute("maxZ", &param.maxZ);
            configResult.algorithmParams.pointCloudFilterParams.push_back(param);
        }

        // 解析工具姿态与目标点补偿参数（与 WorkpieceHole 配置方式一致）
        XMLElement* toolParamElement = algoParamsElement->FirstChildElement("ToolParam");
        if (toolParamElement)
        {
            if (toolParamElement->Attribute("eulerOrder"))
                configResult.algorithmParams.toolParam.eulerOrder = toolParamElement->IntAttribute("eulerOrder");
            if (toolParamElement->Attribute("rotX"))
                configResult.algorithmParams.toolParam.rotX = toolParamElement->DoubleAttribute("rotX");
            if (toolParamElement->Attribute("rotY"))
                configResult.algorithmParams.toolParam.rotY = toolParamElement->DoubleAttribute("rotY");
            if (toolParamElement->Attribute("rotZ"))
                configResult.algorithmParams.toolParam.rotZ = toolParamElement->DoubleAttribute("rotZ");
            if (toolParamElement->Attribute("offsetX"))
                configResult.algorithmParams.toolParam.offsetX = toolParamElement->DoubleAttribute("offsetX");
            if (toolParamElement->Attribute("offsetY"))
                configResult.algorithmParams.toolParam.offsetY = toolParamElement->DoubleAttribute("offsetY");
            if (toolParamElement->Attribute("offsetZ"))
                configResult.algorithmParams.toolParam.offsetZ = toolParamElement->DoubleAttribute("offsetZ");
        }
        XMLElement* toolParamsElement = algoParamsElement->FirstChildElement("ToolParamsByCamera");
        if (toolParamsElement)
        {
            for (XMLElement* item = toolParamsElement->FirstChildElement("ToolParam"); item; item = item->NextSiblingElement("ToolParam"))
            {
                const int cameraIndex = item->IntAttribute("cameraIndex", 0);
                VrToolParam value = cameraIndex == 1 ? configResult.algorithmParams.toolParam : VrToolParam();
                value.eulerOrder = item->IntAttribute("eulerOrder", value.eulerOrder);
                value.rotX = item->DoubleAttribute("rotX", value.rotX);
                value.rotY = item->DoubleAttribute("rotY", value.rotY);
                value.rotZ = item->DoubleAttribute("rotZ", value.rotZ);
                value.offsetX = item->DoubleAttribute("offsetX", value.offsetX);
                value.offsetY = item->DoubleAttribute("offsetY", value.offsetY);
                value.offsetZ = item->DoubleAttribute("offsetZ", value.offsetZ);
                if (cameraIndex > 0)
                {
                    configResult.algorithmParams.toolParamsByCamera.push_back(std::make_pair(cameraIndex, value));
                }
            }
        }

        // 类别映射独立于有序的曝光/增益/阈值参数列表。
        XMLElement* modelParamElement = algoParamsElement->FirstChildElement("WorkpieceClassMapping");
        if (!modelParamElement)
        {
            // 兼容旧配置中的单项节点，读取后不再写回该节点。
            modelParamElement = algoParamsElement->FirstChildElement("WorkpieceModelParam");
        }
        if (modelParamElement)
        {
            VrWorkpieceModelParam& modelParam = configResult.algorithmParams.modelParam;
            if (modelParamElement->Attribute("exposure"))
                modelParam.exposure = modelParamElement->UnsignedAttribute("exposure");
            if (modelParamElement->Attribute("gain"))
                modelParam.gain = modelParamElement->UnsignedAttribute("gain");
            if (modelParamElement->Attribute("minimumScore"))
                modelParam.minimumScore = modelParamElement->DoubleAttribute("minimumScore");
            if (modelParamElement->Attribute("class0WorkpieceType"))
                modelParam.class0WorkpieceType = modelParamElement->IntAttribute("class0WorkpieceType");
            if (modelParamElement->Attribute("class1WorkpieceType"))
                modelParam.class1WorkpieceType = modelParamElement->IntAttribute("class1WorkpieceType");
            if (modelParamElement->Attribute("class2WorkpieceType"))
                modelParam.class2WorkpieceType = modelParamElement->IntAttribute("class2WorkpieceType");
        }
        configResult.algorithmParams.modelParams.clear();
        XMLElement* modelParamsElement = algoParamsElement->FirstChildElement("WorkpieceModelParams");
        if (modelParamsElement)
        {
            if (XMLElement* regionElement = modelParamsElement->FirstChildElement("DetectionRegion"))
            {
                VrModelDetectionRegion region;
                regionElement->QueryBoolAttribute("enabled", &region.enabled);
                regionElement->QueryIntAttribute("left", &region.left);
                regionElement->QueryIntAttribute("top", &region.top);
                regionElement->QueryIntAttribute("right", &region.right);
                regionElement->QueryIntAttribute("bottom", &region.bottom);
                if (region.isValid())
                {
                    configResult.algorithmParams.modelDetectionRegion = region;
                }
            }
            VrModelGraySelectionParam graySelection;
            modelParamsElement->QueryDoubleAttribute("targetGray", &graySelection.targetGray);
            modelParamsElement->QueryDoubleAttribute("maxGrayDeviation", &graySelection.maxGrayDeviation);
            if (graySelection.isValid())
            {
                configResult.algorithmParams.modelGraySelection = graySelection;
            }
            for (XMLElement* item = modelParamsElement->FirstChildElement("Model"); item;
                 item = item->NextSiblingElement("Model"))
            {
                VrWorkpieceModelParam model;
                if (item->Attribute("exposure"))
                    model.exposure = item->UnsignedAttribute("exposure");
                if (item->Attribute("gain"))
                    model.gain = item->UnsignedAttribute("gain");
                if (item->Attribute("minimumScore"))
                    model.minimumScore = item->DoubleAttribute("minimumScore");
                configResult.algorithmParams.modelParams.push_back(model);
            }
        }
        XMLElement* fullImageCaptureElement = algoParamsElement->FirstChildElement("FullImageCaptureParam");
        if (fullImageCaptureElement)
        {
            VrFullImageCaptureParam& captureParam = configResult.algorithmParams.fullImageCaptureParam;
            if (fullImageCaptureElement->Attribute("exposure"))
                captureParam.exposure = fullImageCaptureElement->UnsignedAttribute("exposure");
            if (fullImageCaptureElement->Attribute("gain"))
                captureParam.gain = fullImageCaptureElement->UnsignedAttribute("gain");
        }
        if (modelParamsElement != nullptr && !configResult.algorithmParams.modelParams.empty())
        {
            // 兼容仍使用 modelParam 的调用方，但曝光、增益和阈值以列表首项为准。
            const VrWorkpieceModelParam& firstModel = configResult.algorithmParams.modelParams.front();
            configResult.algorithmParams.modelParam.exposure = firstModel.exposure;
            configResult.algorithmParams.modelParam.gain = firstModel.gain;
            configResult.algorithmParams.modelParam.minimumScore = firstModel.minimumScore;
        }
        else
        {
            // 没有列表时兼容旧的全图采集参数，再迁移为单个列表项。
            if (fullImageCaptureElement)
            {
                configResult.algorithmParams.modelParam.exposure =
                    configResult.algorithmParams.fullImageCaptureParam.exposure;
                configResult.algorithmParams.modelParam.gain =
                    configResult.algorithmParams.fullImageCaptureParam.gain;
            }
            configResult.algorithmParams.modelParams.push_back(configResult.algorithmParams.modelParam);
        }

        XMLElement* binParamElement = algoParamsElement->FirstChildElement("BinParam");
        if (binParamElement)
        {
            VrBinParam& binParam = configResult.algorithmParams.binParam;
            if (binParamElement->Attribute("useConfiguredBin"))
                binParam.useConfiguredBin = binParamElement->BoolAttribute("useConfiguredBin");
            if (binParamElement->Attribute("binHeight"))
                binParam.binHeight = binParamElement->DoubleAttribute("binHeight");
            if (binParamElement->Attribute("bin_zSliceRange"))
                binParam.bin_zSliceRange = binParamElement->DoubleAttribute("bin_zSliceRange");
            if (binParamElement->Attribute("length"))
                binParam.length = binParamElement->DoubleAttribute("length");
            if (binParamElement->Attribute("width"))
                binParam.width = binParamElement->DoubleAttribute("width");
            if (binParamElement->Attribute("binTopZ"))
                binParam.binTopZ = binParamElement->DoubleAttribute("binTopZ");

            auto readVec3 = [binParamElement](const char* prefix, double values[3]) {
                char attrName[64];
                for (int i = 0; i < 3; ++i) {
                    sprintf(attrName, "%s%d", prefix, i);
                    if (binParamElement->Attribute(attrName))
                        values[i] = binParamElement->DoubleAttribute(attrName);
                }
            };
            readVec3("center", binParam.center);
            readVec3("bottomNormal", binParam.bottomNormal);
            readVec3("xDir", binParam.xDir);
            readVec3("yDir", binParam.yDir);
            for (int vertex = 0; vertex < 4; ++vertex) {
                char attrName[64];
                for (int axis = 0; axis < 3; ++axis) {
                    sprintf(attrName, "minRectVertex%d_%d", vertex, axis);
                    if (binParamElement->Attribute(attrName))
                        binParam.minRectVertex[vertex][axis] =
                            binParamElement->DoubleAttribute(attrName);
                }
            }
        }

        XMLElement* rotorCoreParamElement = algoParamsElement->FirstChildElement("RotorCoreParam");
        if (rotorCoreParamElement)
        {
            VrRotorCoreParam& rotorParam = configResult.algorithmParams.rotorCoreParam;
            if (rotorCoreParamElement->Attribute("radius"))
                rotorParam.radius = rotorCoreParamElement->DoubleAttribute("radius");
            if (rotorCoreParamElement->Attribute("height"))
                rotorParam.height = rotorCoreParamElement->DoubleAttribute("height");
            if (rotorCoreParamElement->Attribute("spacerLength"))
            {
                rotorParam.spacerLength = rotorCoreParamElement->DoubleAttribute("spacerLength");
            }
            if (rotorCoreParamElement->Attribute("spacerWidth"))
            {
                rotorParam.spacerWidth = rotorCoreParamElement->DoubleAttribute("spacerWidth");
            }
            if (rotorCoreParamElement->Attribute("layerRatio"))
                rotorParam.layerRatio = rotorCoreParamElement->DoubleAttribute("layerRatio");
            if (rotorCoreParamElement->Attribute("distToSideRatio"))
                rotorParam.distToSideRatio = rotorCoreParamElement->DoubleAttribute("distToSideRatio");
            if (rotorCoreParamElement->Attribute("groundRemoveOffset"))
                rotorParam.groundRemoveOffset = rotorCoreParamElement->DoubleAttribute("groundRemoveOffset");

            // 旧配置或无效枚举回退到工件自身法向，仅 1 表示使用调平后的地面法向。
            rotorParam.useGroundCalibNormal = rotorCoreParamElement->IntAttribute("useGroundCalibNormal") == 1 ? 1 : 0;

            rotorParam.workpieceSizes.clear();
            XMLElement* sizesElement = rotorCoreParamElement->FirstChildElement("WorkpieceSizes");
            if (sizesElement)
            {
                rotorParam.defaultWorkpieceIndex = sizesElement->IntAttribute("defaultIndex");
                for (XMLElement* item = sizesElement->FirstChildElement("Workpiece"); item;
                     item = item->NextSiblingElement("Workpiece"))
                {
                    VrRotorWorkpieceSize size;
                    size.index = item->IntAttribute("index");
                    size.radius = item->DoubleAttribute("radius");
                    size.height = item->DoubleAttribute("height");
                    rotorParam.workpieceSizes.push_back(size);
                }
                // 无效列表保留供界面修正，启动检测时拒绝使用，不能静默换成默认尺寸。
                if (!rotorParam.isWorkpieceListValid())
                {
                    LOG_WARNING("Invalid rotor WorkpieceSizes; detection requires a valid size list\n");
                }
            }
            else
            {
                // 旧配置迁移为 1 号工件，保留现场原有半径和高度。
                VrRotorWorkpieceSize size;
                size.radius = rotorParam.radius;
                size.height = rotorParam.height;
                rotorParam.defaultWorkpieceIndex = 1;
                rotorParam.workpieceSizes.push_back(size);
            }
        }

        // 解析多相机平面校准参数
        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外面）
    XMLElement* debugParamElement = root->FirstChildElement("DebugParam");
    if (debugParamElement)
    {
        if (debugParamElement->Attribute("crash"))
        {
            configResult.debugParam.crash = debugParamElement->BoolAttribute("crash");
        }
        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");
        configResult.debugParam.storageType = ParseDebugStorageType(
            debugParamElement->Attribute("storageType"));
    }

    // 解析串口配置
    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");
    }

    // 解析手眼标定矩阵
    XMLElement* handEyeElement = root->FirstChildElement("HandEyeCalibMatrix");
    if (handEyeElement)
    {
        for (int i = 0; i < 16; i++)
        {
            char attrName[32];
            sprintf(attrName, "m%d", i);
            if (handEyeElement->Attribute(attrName))
                configResult.handEyeCalibMatrix.matrix[i] = handEyeElement->DoubleAttribute(attrName);
        }
        // 读取旋转顺序，如果不存在则使用默认值11（外旋ZYX）
        if (handEyeElement->Attribute("eulerOrder"))
            configResult.handEyeCalibMatrix.eulerOrder = handEyeElement->IntAttribute("eulerOrder");
        else
            configResult.handEyeCalibMatrix.eulerOrder = 11;  // 默认外旋ZYX
    }
    XMLElement* handEyeMatricesElement = root->FirstChildElement("HandEyeCalibMatrices");
    if (handEyeMatricesElement)
    {
        for (XMLElement* item = handEyeMatricesElement->FirstChildElement("HandEyeCalibMatrix");
             item;
             item = item->NextSiblingElement("HandEyeCalibMatrix"))
        {
            const int cameraIndex = item->IntAttribute("cameraIndex", 0);
            if (cameraIndex <= 0)
            {
                continue;
            }
            VrHandEyeCalibMatrix value;
            for (int i = 0; i < 16; ++i)
            {
                char attrName[32];
                sprintf(attrName, "m%d", i);
                if (item->Attribute(attrName))
                {
                    value.matrix[i] = item->DoubleAttribute(attrName);
                }
            }
            value.eulerOrder = item->IntAttribute("eulerOrder", 11);
            configResult.handEyeCalibMatricesByCamera.push_back(std::make_pair(cameraIndex, value));
        }
    }

    // 解析 TCP/IP 文本服务端端口和 Modbus 寄存器配置
    XMLElement* tcpServerConfigElement = root->FirstChildElement("TcpServerConfig");
    if (tcpServerConfigElement)
    {
        if (tcpServerConfigElement->Attribute("robotServerPort"))
            configResult.plcRobotServerConfig.robotServerPort = tcpServerConfigElement->IntAttribute("robotServerPort");

        // 解析Modbus寄存器地址配置
        if (tcpServerConfigElement->Attribute("addrTrigger"))
            configResult.plcRobotServerConfig.registerConfig.addrTrigger = tcpServerConfigElement->IntAttribute("addrTrigger");
        if (tcpServerConfigElement->Attribute("addrWorkStatus"))
            configResult.plcRobotServerConfig.registerConfig.addrWorkStatus = tcpServerConfigElement->IntAttribute("addrWorkStatus");
        if (tcpServerConfigElement->Attribute("addrCoordDataStart"))
            configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart = tcpServerConfigElement->IntAttribute("addrCoordDataStart");

        // 解析姿态输出顺序配置
        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反向（兼容原有行为）

        // 解析数据字节序配置
        if (tcpServerConfigElement->Attribute("byteOrder"))
            configResult.plcRobotServerConfig.byteOrder = tcpServerConfigElement->IntAttribute("byteOrder");
        else
            configResult.plcRobotServerConfig.byteOrder = BYTE_ORDER_BIG_ENDIAN;  // 默认大端序
    }

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

    for (const VrPointCloudFilterParam& param : configResult.algorithmParams.pointCloudFilterParams)
    {
        XMLElement* filterElement = doc.NewElement("PointCloudFilterParam");
        filterElement->SetAttribute("cameraIndex", param.cameraIndex);
        filterElement->SetAttribute("cameraName", param.cameraName.c_str());
        filterElement->SetAttribute("enabled", param.enabled);
        filterElement->SetAttribute("minX", param.minX);
        filterElement->SetAttribute("maxX", param.maxX);
        filterElement->SetAttribute("minY", param.minY);
        filterElement->SetAttribute("maxY", param.maxY);
        filterElement->SetAttribute("minZ", param.minZ);
        filterElement->SetAttribute("maxZ", param.maxZ);
        algoParamsElement->InsertEndChild(filterElement);
    }

    // 添加工具姿态与目标点补偿参数
    XMLElement* toolParamElement = doc.NewElement("ToolParam");
    toolParamElement->SetAttribute("eulerOrder", configResult.algorithmParams.toolParam.eulerOrder);
    toolParamElement->SetAttribute("rotX", configResult.algorithmParams.toolParam.rotX);
    toolParamElement->SetAttribute("rotY", configResult.algorithmParams.toolParam.rotY);
    toolParamElement->SetAttribute("rotZ", configResult.algorithmParams.toolParam.rotZ);
    toolParamElement->SetAttribute("offsetX", configResult.algorithmParams.toolParam.offsetX);
    toolParamElement->SetAttribute("offsetY", configResult.algorithmParams.toolParam.offsetY);
    toolParamElement->SetAttribute("offsetZ", configResult.algorithmParams.toolParam.offsetZ);
    algoParamsElement->InsertEndChild(toolParamElement);
    if (!configResult.algorithmParams.toolParamsByCamera.empty())
    {
        XMLElement* list = doc.NewElement("ToolParamsByCamera");
        for (const auto& entry : configResult.algorithmParams.toolParamsByCamera)
        {
            XMLElement* item = doc.NewElement("ToolParam");
            item->SetAttribute("cameraIndex", entry.first);
            item->SetAttribute("eulerOrder", entry.second.eulerOrder);
            item->SetAttribute("rotX", entry.second.rotX);
            item->SetAttribute("rotY", entry.second.rotY);
            item->SetAttribute("rotZ", entry.second.rotZ);
            item->SetAttribute("offsetX", entry.second.offsetX);
            item->SetAttribute("offsetY", entry.second.offsetY);
            item->SetAttribute("offsetZ", entry.second.offsetZ);
            list->InsertEndChild(item);
        }
        algoParamsElement->InsertEndChild(list);
    }

    const auto& modelParams = configResult.algorithmParams.modelParams;
    const VrWorkpieceModelParam& legacyModelParam = configResult.algorithmParams.modelParam;

    XMLElement* modelParamElement = doc.NewElement("WorkpieceClassMapping");
    modelParamElement->SetAttribute("class0WorkpieceType",
        legacyModelParam.class0WorkpieceType);
    modelParamElement->SetAttribute("class1WorkpieceType",
        legacyModelParam.class1WorkpieceType);
    modelParamElement->SetAttribute("class2WorkpieceType",
        legacyModelParam.class2WorkpieceType);
    algoParamsElement->InsertEndChild(modelParamElement);

    XMLElement* modelParamsElement = doc.NewElement("WorkpieceModelParams");
    modelParamsElement->SetAttribute("targetGray", configResult.algorithmParams.modelGraySelection.targetGray);
    modelParamsElement->SetAttribute("maxGrayDeviation", configResult.algorithmParams.modelGraySelection.maxGrayDeviation);
    const VrModelDetectionRegion& region = configResult.algorithmParams.modelDetectionRegion;
    XMLElement* regionElement = doc.NewElement("DetectionRegion");
    regionElement->SetAttribute("enabled", region.enabled);
    regionElement->SetAttribute("left", region.left);
    regionElement->SetAttribute("top", region.top);
    regionElement->SetAttribute("right", region.right);
    regionElement->SetAttribute("bottom", region.bottom);
    modelParamsElement->InsertEndChild(regionElement);
    std::vector<VrWorkpieceModelParam> modelParamsToWrite = modelParams;
    if (modelParamsToWrite.empty())
    {
        modelParamsToWrite.push_back(VrWorkpieceModelParam());
    }
    // 其他共用配置页面仍编辑兼容字段，列表首项同步该值；转子页面会从列表更新兼容字段。
    modelParamsToWrite.front().exposure = legacyModelParam.exposure;
    modelParamsToWrite.front().gain = legacyModelParam.gain;
    modelParamsToWrite.front().minimumScore = legacyModelParam.minimumScore;
    int modelIndex = 1;
    for (const auto& model : modelParamsToWrite)
    {
        XMLElement* item = doc.NewElement("Model");
        item->SetAttribute("index", modelIndex++);
        item->SetAttribute("exposure", model.exposure);
        item->SetAttribute("gain", model.gain);
        item->SetAttribute("minimumScore", model.minimumScore);
        modelParamsElement->InsertEndChild(item);
    }
    algoParamsElement->InsertEndChild(modelParamsElement);

    XMLElement* binParamElement = doc.NewElement("BinParam");
    const VrBinParam& binParam = configResult.algorithmParams.binParam;
    binParamElement->SetAttribute("useConfiguredBin", binParam.useConfiguredBin);
    binParamElement->SetAttribute("binHeight", binParam.binHeight);
    binParamElement->SetAttribute("bin_zSliceRange", binParam.bin_zSliceRange);
    binParamElement->SetAttribute("length", binParam.length);
    binParamElement->SetAttribute("width", binParam.width);
    binParamElement->SetAttribute("binTopZ", binParam.binTopZ);
    auto writeVec3 = [binParamElement](const char* prefix, const double values[3]) {
        char attrName[64];
        for (int i = 0; i < 3; ++i) {
            sprintf(attrName, "%s%d", prefix, i);
            binParamElement->SetAttribute(attrName, values[i]);
        }
    };
    writeVec3("center", binParam.center);
    writeVec3("bottomNormal", binParam.bottomNormal);
    writeVec3("xDir", binParam.xDir);
    writeVec3("yDir", binParam.yDir);
    for (int vertex = 0; vertex < 4; ++vertex) {
        char attrName[64];
        for (int axis = 0; axis < 3; ++axis) {
            sprintf(attrName, "minRectVertex%d_%d", vertex, axis);
            binParamElement->SetAttribute(attrName, binParam.minRectVertex[vertex][axis]);
        }
    }
    algoParamsElement->InsertEndChild(binParamElement);

    XMLElement* rotorCoreParamElement = doc.NewElement("RotorCoreParam");
    rotorCoreParamElement->SetAttribute("radius", configResult.algorithmParams.rotorCoreParam.radius);
    rotorCoreParamElement->SetAttribute("height", configResult.algorithmParams.rotorCoreParam.height);
    rotorCoreParamElement->SetAttribute("spacerLength", configResult.algorithmParams.rotorCoreParam.spacerLength);
    rotorCoreParamElement->SetAttribute("spacerWidth", configResult.algorithmParams.rotorCoreParam.spacerWidth);
    rotorCoreParamElement->SetAttribute("layerRatio", configResult.algorithmParams.rotorCoreParam.layerRatio);
    rotorCoreParamElement->SetAttribute("distToSideRatio", configResult.algorithmParams.rotorCoreParam.distToSideRatio);
    rotorCoreParamElement->SetAttribute("groundRemoveOffset", configResult.algorithmParams.rotorCoreParam.groundRemoveOffset);
    rotorCoreParamElement->SetAttribute("useGroundCalibNormal", configResult.algorithmParams.rotorCoreParam.useGroundCalibNormal);
    XMLElement* sizesElement = doc.NewElement("WorkpieceSizes");
    sizesElement->SetAttribute("defaultIndex", configResult.algorithmParams.rotorCoreParam.defaultWorkpieceIndex);
    for (const auto& size : configResult.algorithmParams.rotorCoreParam.workpieceSizes)
    {
        XMLElement* item = doc.NewElement("Workpiece");
        item->SetAttribute("index", size.index);
        item->SetAttribute("radius", size.radius);
        item->SetAttribute("height", size.height);
        sizesElement->InsertEndChild(item);
    }
    rotorCoreParamElement->InsertEndChild(sizesElement);
    algoParamsElement->InsertEndChild(rotorCoreParamElement);

    // 添加多相机平面校准参数
    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("crash", configResult.debugParam.crash);
    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());
    debugParamElement->SetAttribute("storageType", DebugStorageTypeName(configResult.debugParam.storageType));
    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* handEyeElement = doc.NewElement("HandEyeCalibMatrix");
    for (int i = 0; i < 16; i++)
    {
        char attrName[32];
        sprintf(attrName, "m%d", i);
        handEyeElement->SetAttribute(attrName, configResult.handEyeCalibMatrix.matrix[i]);
    }
    // 保存旋转顺序
    handEyeElement->SetAttribute("eulerOrder", configResult.handEyeCalibMatrix.eulerOrder);
    root->InsertEndChild(handEyeElement);
    if (!configResult.handEyeCalibMatricesByCamera.empty())
    {
        XMLElement* handEyeMatricesElement = doc.NewElement("HandEyeCalibMatrices");
        for (const auto& entry : configResult.handEyeCalibMatricesByCamera)
        {
            XMLElement* item = doc.NewElement("HandEyeCalibMatrix");
            item->SetAttribute("cameraIndex", entry.first);
            for (int i = 0; i < 16; ++i)
            {
                char attrName[32];
                sprintf(attrName, "m%d", i);
                item->SetAttribute(attrName, entry.second.matrix[i]);
            }
            item->SetAttribute("eulerOrder", entry.second.eulerOrder);
            handEyeMatricesElement->InsertEndChild(item);
        }
        root->InsertEndChild(handEyeMatricesElement);
    }

    // 添加 TCP/IP 文本服务端端口和 Modbus 寄存器配置
    XMLElement* tcpServerConfigElement = doc.NewElement("TcpServerConfig");
    tcpServerConfigElement->SetAttribute("robotServerPort", configResult.plcRobotServerConfig.robotServerPort);
    // 保存Modbus寄存器地址配置
    tcpServerConfigElement->SetAttribute("addrTrigger", configResult.plcRobotServerConfig.registerConfig.addrTrigger);
    tcpServerConfigElement->SetAttribute("addrWorkStatus", configResult.plcRobotServerConfig.registerConfig.addrWorkStatus);
    tcpServerConfigElement->SetAttribute("addrCoordDataStart", configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart);
    // 保存姿态输出顺序配置
    tcpServerConfigElement->SetAttribute("poseOutputOrder", configResult.plcRobotServerConfig.poseOutputOrder);
    // 保存方向向量反向配置
    tcpServerConfigElement->SetAttribute("dirVectorInvert", configResult.plcRobotServerConfig.dirVectorInvert);
    // 保存数据字节序配置
    tcpServerConfigElement->SetAttribute("byteOrder", configResult.plcRobotServerConfig.byteOrder);
    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;
}