GrabBag/App/WeldSeam/WeldSeamConfig/Src/VrConfig.cpp

#include "VrConfig.h"

#include <iostream>
#include <string>
#include <vector>

#include "ConfigXmlUtils.h"
#include "VrLog.h"

using namespace tinyxml2;

namespace {

// 读取 <AlgorithmParams> 下的子节点到 VrAlgorithmParams
void LoadAlgorithmParams(XMLElement* algoElement, VrAlgorithmParams& params)
{
    if (!algoElement) return;

    if (XMLElement* e = algoElement->FirstChildElement("ValleyParam")) {
        e->QueryDoubleAttribute("chkWinWidth", &params.valleyParam.chkWinWidth);
        e->QueryDoubleAttribute("maxWidth", &params.valleyParam.maxWidth);
        e->QueryDoubleAttribute("minHeight", &params.valleyParam.minHeight);
    }

    if (XMLElement* e = algoElement->FirstChildElement("TreeGrowParam")) {
        e->QueryIntAttribute("maxLineSkipNum", &params.treeGrowParam.maxLineSkipNum);
        e->QueryDoubleAttribute("yDeviationMax", &params.treeGrowParam.yDeviationMax);
        e->QueryDoubleAttribute("maxSkipDistance", &params.treeGrowParam.maxSkipDistance);
        e->QueryDoubleAttribute("zDeviationMax", &params.treeGrowParam.zDeviationMax);
        e->QueryDoubleAttribute("minLTypeTreeLen", &params.treeGrowParam.minLTypeTreeLen);
        e->QueryDoubleAttribute("minVTypeTreeLen", &params.treeGrowParam.minVTypeTreeLen);
    }

    if (XMLElement* e = algoElement->FirstChildElement("WeldSeamParam")) {
        e->QueryIntAttribute("weldRefPoints", &params.weldSeamParam.weldRefPoints);
    }

    // 解析多相机平面校准参数
    ConfigXmlUtils::LoadPlaneCalibParams(algoElement, params.planeCalibParam);
}

void SaveAlgorithmParams(XMLDocument& doc, XMLElement* algoElement, const VrAlgorithmParams& params)
{
    XMLElement* valleyElement = doc.NewElement("ValleyParam");
    valleyElement->SetAttribute("chkWinWidth", params.valleyParam.chkWinWidth);
    valleyElement->SetAttribute("maxWidth", params.valleyParam.maxWidth);
    valleyElement->SetAttribute("minHeight", params.valleyParam.minHeight);
    algoElement->InsertEndChild(valleyElement);

    XMLElement* growElement = doc.NewElement("TreeGrowParam");
    growElement->SetAttribute("maxLineSkipNum", params.treeGrowParam.maxLineSkipNum);
    growElement->SetAttribute("yDeviationMax", params.treeGrowParam.yDeviationMax);
    growElement->SetAttribute("maxSkipDistance", params.treeGrowParam.maxSkipDistance);
    growElement->SetAttribute("zDeviationMax", params.treeGrowParam.zDeviationMax);
    growElement->SetAttribute("minLTypeTreeLen", params.treeGrowParam.minLTypeTreeLen);
    growElement->SetAttribute("minVTypeTreeLen", params.treeGrowParam.minVTypeTreeLen);
    algoElement->InsertEndChild(growElement);

    XMLElement* weldElement = doc.NewElement("WeldSeamParam");
    weldElement->SetAttribute("weldRefPoints", params.weldSeamParam.weldRefPoints);
    algoElement->InsertEndChild(weldElement);

    // 多相机平面校准参数
    ConfigXmlUtils::SavePlaneCalibParams(doc, algoElement, params.planeCalibParam);
}

} // namespace

CVrConfig::CVrConfig() : m_pNotify(nullptr)
{
}

CVrConfig::~CVrConfig()
{
}

int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
{
    XMLDocument doc;
    const XMLError err = doc.LoadFile(filePath.c_str());
    if (err != XML_SUCCESS)
    {
        LOG_ERR("Failed to open config file: %s\n", filePath.c_str());
        return LOAD_CONFIG_FILE_NOT_FOUND;
    }

    XMLElement* root = doc.RootElement();
    if (!root || std::string(root->Name()) != "WeldSeamConfig")
    {
        LOG_ERR("Config file format error: root element is not WeldSeamConfig\n");
        return LOAD_CONFIG_INVALID_FORMAT;
    }

    // 解析摄像头列表
    ConfigXmlUtils::LoadCameraList(root, configResult.cameraList);

    // 解析算法参数（类比 WorkpieceHole 的 <AlgorithmParams> 结构）
    XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
    LoadAlgorithmParams(algoParamsElement, configResult.algorithmParams);

    // 解析调试参数
    ConfigXmlUtils::LoadDebugParam(root, configResult.debugParam);

    // 解析串口配置
    ConfigXmlUtils::LoadSerialConfig(root, configResult.serialConfig);

    // 解析手眼标定矩阵列表（支持多相机，类比 WorkpieceHole）
    ConfigXmlUtils::LoadHandEyeCalibMatrixs(root, configResult.handEyeCalibMatrixList);

    // 补全缺失的相机矩阵槽位
    int cameraCount = static_cast<int>(configResult.cameraList.size());
    if (cameraCount <= 0) cameraCount = 1;
    for (int camIdx = 1; camIdx <= cameraCount; ++camIdx) {
        configResult.EnsureHandEyeMatrix(camIdx);
    }

    // 解析网络配置
    if (XMLElement* networkElement = root->FirstChildElement("NetworkConfig")) {
        if (networkElement->Attribute("tcpServerPort"))
            configResult.tcpPort = static_cast<uint16_t>(networkElement->IntAttribute("tcpServerPort"));
        if (networkElement->Attribute("eulerOrder"))
            configResult.eulerOrder = networkElement->IntAttribute("eulerOrder");
        if (networkElement->Attribute("outputEulerOrder"))
            configResult.outputEulerOrder = networkElement->IntAttribute("outputEulerOrder");
        if (networkElement->Attribute("poseOutputOrder"))
            configResult.poseOutputOrder = networkElement->IntAttribute("poseOutputOrder");
        if (networkElement->Attribute("byteOrder"))
            configResult.byteOrder = networkElement->IntAttribute("byteOrder");
    }

    if (XMLElement* tcpElement = root->FirstChildElement("TCP")) {
        if (tcpElement->Attribute("port"))
            configResult.tcpPort = static_cast<uint16_t>(tcpElement->IntAttribute("port"));
    }

    LOG_INFO("Config loaded successfully from: %s (cameras: %zu, handEyeMatrices: %zu)\n",
             filePath.c_str(), configResult.cameraList.size(), configResult.handEyeCalibMatrixList.size());
    return LOAD_CONFIG_SUCCESS;
}

bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResult)
{
    XMLDocument doc;

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

    XMLElement* root = doc.NewElement("WeldSeamConfig");
    doc.InsertEndChild(root);

    // 摄像头列表
    ConfigXmlUtils::SaveCameraList(doc, root, configResult.cameraList);

    // 算法参数（类比 WorkpieceHole 的 <AlgorithmParams> 结构）
    XMLElement* algoParamsElement = doc.NewElement("AlgorithmParams");
    SaveAlgorithmParams(doc, algoParamsElement, configResult.algorithmParams);
    root->InsertEndChild(algoParamsElement);

    // 调试参数
    ConfigXmlUtils::SaveDebugParam(doc, root, configResult.debugParam);

    // 串口配置
    ConfigXmlUtils::SaveSerialConfig(doc, root, configResult.serialConfig);

    // 手眼标定矩阵列表（支持多相机，类比 WorkpieceHole）
    ConfigXmlUtils::SaveHandEyeCalibMatrixs(doc, root, configResult.handEyeCalibMatrixList);

    // 网络配置
    XMLElement* networkElement = doc.NewElement("NetworkConfig");
    networkElement->SetAttribute("tcpServerPort", configResult.tcpPort);
    networkElement->SetAttribute("eulerOrder", configResult.eulerOrder);
    networkElement->SetAttribute("outputEulerOrder", configResult.outputEulerOrder);
    networkElement->SetAttribute("poseOutputOrder", configResult.poseOutputOrder);
    networkElement->SetAttribute("byteOrder", configResult.byteOrder);
    root->InsertEndChild(networkElement);

    XMLElement* tcpElement = doc.NewElement("TCP");
    tcpElement->SetAttribute("port", configResult.tcpPort);
    root->InsertEndChild(tcpElement);

    const XMLError err = doc.SaveFile(filePath.c_str());
    if (err != XML_SUCCESS)
    {
        LOG_ERR("Failed to save config file: %s\n", filePath.c_str());
        return false;
    }

    if (m_pNotify)
    {
        m_pNotify->OnConfigChanged(configResult);
    }

    LOG_INFO("Config saved successfully to: %s\n", filePath.c_str());
    return true;
}

void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
{
    m_pNotify = notify;
}

bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
{
    *ppVrConfig = new CVrConfig();
    return *ppVrConfig != nullptr;
}