GrabBag/App/TunnelChannel/TunnelChannelConfig/Src/VrConfig.cpp

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

#include "VrLog.h"

using namespace tinyxml2;

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()) != "TunnelChannelConfig")
    {
        std::cerr << "config file format error: root element is not TunnelChannelConfig" << std::endl;
        return LOAD_CONFIG_INVALID_FORMAT;
    }

    // 解析3D摄像头列表
    XMLElement* camerasElement = root->FirstChildElement("Cameras3D");
    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* hikCamerasElement = root->FirstChildElement("HikCameras");
    if (hikCamerasElement)
    {
        XMLElement* hikCameraElement = hikCamerasElement->FirstChildElement("HikCamera");
        while (hikCameraElement)
        {
            HikCameraConfig hikCamera;
            if (hikCameraElement->Attribute("ip"))
                hikCamera.ip = hikCameraElement->Attribute("ip");
            if (hikCameraElement->Attribute("port"))
                hikCamera.port = hikCameraElement->IntAttribute("port");
            if (hikCameraElement->Attribute("username"))
                hikCamera.username = hikCameraElement->Attribute("username");
            if (hikCameraElement->Attribute("password"))
                hikCamera.password = hikCameraElement->Attribute("password");
            if (hikCameraElement->Attribute("channelNo"))
                hikCamera.channelNo = hikCameraElement->IntAttribute("channelNo");
            if (hikCameraElement->Attribute("streamType"))
                hikCamera.streamType = hikCameraElement->IntAttribute("streamType");
            if (hikCameraElement->Attribute("enabled"))
                hikCamera.enabled = hikCameraElement->BoolAttribute("enabled");

            configResult.hikCameraList.push_back(hikCamera);
            hikCameraElement = hikCameraElement->NextSiblingElement("HikCamera");
        }
    }

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

    // 解析算法参数
    XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
    if (algoParamsElement)
    {
        // 解析隧道检测参数
        XMLElement* tunnelParamElement = algoParamsElement->FirstChildElement("TunnelParam");
        if (tunnelParamElement)
        {
            if (tunnelParamElement->Attribute("threshold"))
                configResult.algorithmParams.tunnelParam.threshold = tunnelParamElement->DoubleAttribute("threshold");
            if (tunnelParamElement->Attribute("minArea"))
                configResult.algorithmParams.tunnelParam.minArea = tunnelParamElement->DoubleAttribute("minArea");
            if (tunnelParamElement->Attribute("maxArea"))
                configResult.algorithmParams.tunnelParam.maxArea = tunnelParamElement->DoubleAttribute("maxArea");
            if (tunnelParamElement->Attribute("enableAlarm"))
                configResult.algorithmParams.tunnelParam.enableAlarm = tunnelParamElement->BoolAttribute("enableAlarm");
        }

        // 解析通道间距测量算法参数
        XMLElement* channelSpaceElement = algoParamsElement->FirstChildElement("ChannelSpaceParam");
        if (channelSpaceElement)
        {
            ChannelSpaceAlgoParam& param = configResult.algorithmParams.channelSpaceParam;

            // 解析角点检测参数
            XMLElement* cornerParamElement = channelSpaceElement->FirstChildElement("CornerParam");
            if (cornerParamElement)
            {
                if (cornerParamElement->Attribute("minEndingGap"))
                    param.minEndingGap = cornerParamElement->DoubleAttribute("minEndingGap");
                if (cornerParamElement->Attribute("minEndingGap_z"))
                    param.minEndingGap_z = cornerParamElement->DoubleAttribute("minEndingGap_z");
                if (cornerParamElement->Attribute("scale"))
                    param.scale = cornerParamElement->DoubleAttribute("scale");
                if (cornerParamElement->Attribute("cornerTh"))
                    param.cornerTh = cornerParamElement->DoubleAttribute("cornerTh");
                if (cornerParamElement->Attribute("jumpCornerTh_1"))
                    param.jumpCornerTh_1 = cornerParamElement->DoubleAttribute("jumpCornerTh_1");
                if (cornerParamElement->Attribute("jumpCornerTh_2"))
                    param.jumpCornerTh_2 = cornerParamElement->DoubleAttribute("jumpCornerTh_2");
            }

            // 解析离群点过滤参数
            XMLElement* outlierFilterElement = channelSpaceElement->FirstChildElement("OutlierFilterParam");
            if (outlierFilterElement)
            {
                if (outlierFilterElement->Attribute("continuityTh"))
                    param.continuityTh = outlierFilterElement->DoubleAttribute("continuityTh");
                if (outlierFilterElement->Attribute("outlierTh"))
                    param.outlierTh = outlierFilterElement->DoubleAttribute("outlierTh");
            }

            // 解析树生长参数
            XMLElement* treeGrowElement = channelSpaceElement->FirstChildElement("TreeGrowParam");
            if (treeGrowElement)
            {
                if (treeGrowElement->Attribute("yDeviationMax"))
                    param.yDeviationMax = treeGrowElement->DoubleAttribute("yDeviationMax");
                if (treeGrowElement->Attribute("zDeviationMax"))
                    param.zDeviationMax = treeGrowElement->DoubleAttribute("zDeviationMax");
                if (treeGrowElement->Attribute("maxLineSkipNum"))
                    param.maxLineSkipNum = treeGrowElement->IntAttribute("maxLineSkipNum");
                if (treeGrowElement->Attribute("maxSkipDistance"))
                    param.maxSkipDistance = treeGrowElement->DoubleAttribute("maxSkipDistance");
                if (treeGrowElement->Attribute("minLTypeTreeLen"))
                    param.minLTypeTreeLen = treeGrowElement->DoubleAttribute("minLTypeTreeLen");
                if (treeGrowElement->Attribute("minVTypeTreeLen"))
                    param.minVTypeTreeLen = treeGrowElement->DoubleAttribute("minVTypeTreeLen");
            }

            // 解析通道参数
            XMLElement* channelParamElement = channelSpaceElement->FirstChildElement("ChannelParam");
            if (channelParamElement)
            {
                if (channelParamElement->Attribute("channelWidthMin"))
                    param.channelWidthMin = channelParamElement->DoubleAttribute("channelWidthMin");
                if (channelParamElement->Attribute("channelWidthMax"))
                    param.channelWidthMax = channelParamElement->DoubleAttribute("channelWidthMax");
                if (channelParamElement->Attribute("channelSpaceMin"))
                    param.channelSpaceMin = channelParamElement->DoubleAttribute("channelSpaceMin");
                if (channelParamElement->Attribute("channelSpaceMax"))
                    param.channelSpaceMax = channelParamElement->DoubleAttribute("channelSpaceMax");
            }

            // 解析扫描方向
            if (channelSpaceElement->Attribute("horizonScan"))
                param.horizonScan = channelSpaceElement->BoolAttribute("horizonScan");
        }

        // 解析多相机平面校准参数
        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矩阵)
                for (int i = 0; i < 3; i++) {
                    for (int j = 0; j < 3; j++) {
                        char attrName[32];
                        sprintf(attrName, "planeCalib_%d%d", i, j);
                        if (cameraCalibParamElement->Attribute(attrName))
                            cameraParam.planeCalib[i * 3 + j] = cameraCalibParamElement->DoubleAttribute(attrName);
                    }
                }

                // 读取逆旋转矩阵invRMatrix[9] (3x3矩阵)
                for (int i = 0; i < 3; i++) {
                    for (int j = 0; j < 3; j++) {
                        char attrName[32];
                        sprintf(attrName, "invRMatrix_%d%d", i, j);
                        if (cameraCalibParamElement->Attribute(attrName))
                            cameraParam.invRMatrix[i * 3 + j] = cameraCalibParamElement->DoubleAttribute(attrName);
                    }
                }

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

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

    // 解析调试参数
    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");
    }

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

    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("TunnelChannelConfig");
    doc.InsertEndChild(root);

    // 添加3D摄像头列表
    XMLElement* camerasElement = doc.NewElement("Cameras3D");
    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* hikCamerasElement = doc.NewElement("HikCameras");
    root->InsertEndChild(hikCamerasElement);

    for (const auto& hikCamera : configResult.hikCameraList)
    {
        XMLElement* hikCameraElement = doc.NewElement("HikCamera");
        hikCameraElement->SetAttribute("ip", hikCamera.ip.c_str());
        hikCameraElement->SetAttribute("port", hikCamera.port);
        hikCameraElement->SetAttribute("username", hikCamera.username.c_str());
        hikCameraElement->SetAttribute("password", hikCamera.password.c_str());
        hikCameraElement->SetAttribute("channelNo", hikCamera.channelNo);
        hikCameraElement->SetAttribute("streamType", hikCamera.streamType);
        hikCameraElement->SetAttribute("enabled", hikCamera.enabled);
        hikCamerasElement->InsertEndChild(hikCameraElement);
    }

    // 添加设备列表
    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* tunnelParamElement = doc.NewElement("TunnelParam");
    tunnelParamElement->SetAttribute("threshold", configResult.algorithmParams.tunnelParam.threshold);
    tunnelParamElement->SetAttribute("minArea", configResult.algorithmParams.tunnelParam.minArea);
    tunnelParamElement->SetAttribute("maxArea", configResult.algorithmParams.tunnelParam.maxArea);
    tunnelParamElement->SetAttribute("enableAlarm", configResult.algorithmParams.tunnelParam.enableAlarm);
    algoParamsElement->InsertEndChild(tunnelParamElement);

    // 添加通道间距测量算法参数
    XMLElement* channelSpaceElement = doc.NewElement("ChannelSpaceParam");
    const ChannelSpaceAlgoParam& param = configResult.algorithmParams.channelSpaceParam;

    // 角点检测参数
    XMLElement* cornerParamElement = doc.NewElement("CornerParam");
    cornerParamElement->SetAttribute("minEndingGap", param.minEndingGap);
    cornerParamElement->SetAttribute("minEndingGap_z", param.minEndingGap_z);
    cornerParamElement->SetAttribute("scale", param.scale);
    cornerParamElement->SetAttribute("cornerTh", param.cornerTh);
    cornerParamElement->SetAttribute("jumpCornerTh_1", param.jumpCornerTh_1);
    cornerParamElement->SetAttribute("jumpCornerTh_2", param.jumpCornerTh_2);
    channelSpaceElement->InsertEndChild(cornerParamElement);

    // 离群点过滤参数
    XMLElement* outlierFilterElement = doc.NewElement("OutlierFilterParam");
    outlierFilterElement->SetAttribute("continuityTh", param.continuityTh);
    outlierFilterElement->SetAttribute("outlierTh", param.outlierTh);
    channelSpaceElement->InsertEndChild(outlierFilterElement);

    // 树生长参数
    XMLElement* treeGrowElement = doc.NewElement("TreeGrowParam");
    treeGrowElement->SetAttribute("yDeviationMax", param.yDeviationMax);
    treeGrowElement->SetAttribute("zDeviationMax", param.zDeviationMax);
    treeGrowElement->SetAttribute("maxLineSkipNum", param.maxLineSkipNum);
    treeGrowElement->SetAttribute("maxSkipDistance", param.maxSkipDistance);
    treeGrowElement->SetAttribute("minLTypeTreeLen", param.minLTypeTreeLen);
    treeGrowElement->SetAttribute("minVTypeTreeLen", param.minVTypeTreeLen);
    channelSpaceElement->InsertEndChild(treeGrowElement);

    // 通道参数
    XMLElement* channelParamElement = doc.NewElement("ChannelParam");
    channelParamElement->SetAttribute("channelWidthMin", param.channelWidthMin);
    channelParamElement->SetAttribute("channelWidthMax", param.channelWidthMax);
    channelParamElement->SetAttribute("channelSpaceMin", param.channelSpaceMin);
    channelParamElement->SetAttribute("channelSpaceMax", param.channelSpaceMax);
    channelSpaceElement->InsertEndChild(channelParamElement);

    // 扫描方向
    channelSpaceElement->SetAttribute("horizonScan", param.horizonScan);
    algoParamsElement->InsertEndChild(channelSpaceElement);

    // 添加多相机平面校准参数
    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);

        // 保存旋转矩阵
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                char attrName[32];
                sprintf(attrName, "planeCalib_%d%d", i, j);
                cameraCalibParamElement->SetAttribute(attrName, cameraParam.planeCalib[i * 3 + j]);
            }
        }

        // 保存逆旋转矩阵
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                char attrName[32];
                sprintf(attrName, "invRMatrix_%d%d", i, j);
                cameraCalibParamElement->SetAttribute(attrName, cameraParam.invRMatrix[i * 3 + j]);
            }
        }

        planeCalibParamsElement->InsertEndChild(cameraCalibParamElement);
    }

    algoParamsElement->InsertEndChild(planeCalibParamsElement);

    // 添加调试参数
    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);

    // 保存到文件
    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;
}