GrabBag/App/ScrewPosition/ScrewPositionConfig/Src/VrConfig.cpp

#include "VrConfig.h"

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

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

using namespace tinyxml2;

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()) != "ScrewPositionConfig")
    {
        LOG_ERR("Config file format error: root element is not ScrewPositionConfig\n");
        return LOAD_CONFIG_INVALID_FORMAT;
    }

    ConfigXmlUtils::LoadCameraList(root, configResult.cameraList);

    XMLElement* algoElement = root->FirstChildElement("AlgorithmParams");
    if (algoElement)
    {
        XMLElement* screwElement = algoElement->FirstChildElement("ScrewParam");
        if (screwElement)
        {
            if (screwElement->Attribute("rodDiameter"))
                configResult.algorithmParams.screwParam.rodDiameter = screwElement->DoubleAttribute("rodDiameter");
            if (screwElement->Attribute("isHorizonScan"))
                configResult.algorithmParams.screwParam.isHorizonScan = screwElement->BoolAttribute("isHorizonScan");
        }

        XMLElement* filterElement = algoElement->FirstChildElement("FilterParam");
        if (filterElement)
        {
            if (filterElement->Attribute("continuityTh"))
                configResult.algorithmParams.filterParam.continuityTh = filterElement->DoubleAttribute("continuityTh");
            if (filterElement->Attribute("outlierTh"))
                configResult.algorithmParams.filterParam.outlierTh = filterElement->DoubleAttribute("outlierTh");
        }

        XMLElement* cornerElement = algoElement->FirstChildElement("CornerParam");
        if (cornerElement)
        {
            if (cornerElement->Attribute("minEndingGap"))
                configResult.algorithmParams.cornerParam.minEndingGap = cornerElement->DoubleAttribute("minEndingGap");
            if (cornerElement->Attribute("minEndingGap_z"))
                configResult.algorithmParams.cornerParam.minEndingGap_z = cornerElement->DoubleAttribute("minEndingGap_z");
            if (cornerElement->Attribute("scale"))
                configResult.algorithmParams.cornerParam.scale = cornerElement->DoubleAttribute("scale");
            if (cornerElement->Attribute("cornerTh"))
                configResult.algorithmParams.cornerParam.cornerTh = cornerElement->DoubleAttribute("cornerTh");
            if (cornerElement->Attribute("jumpCornerTh_1"))
                configResult.algorithmParams.cornerParam.jumpCornerTh_1 = cornerElement->DoubleAttribute("jumpCornerTh_1");
            if (cornerElement->Attribute("jumpCornerTh_2"))
                configResult.algorithmParams.cornerParam.jumpCornerTh_2 = cornerElement->DoubleAttribute("jumpCornerTh_2");
        }

        XMLElement* growElement = algoElement->FirstChildElement("GrowParam");
        if (growElement)
        {
            if (growElement->Attribute("yDeviation_max"))
                configResult.algorithmParams.growParam.yDeviation_max = growElement->DoubleAttribute("yDeviation_max");
            if (growElement->Attribute("zDeviation_max"))
                configResult.algorithmParams.growParam.zDeviation_max = growElement->DoubleAttribute("zDeviation_max");
            if (growElement->Attribute("maxLineSkipNum"))
                configResult.algorithmParams.growParam.maxLineSkipNum = growElement->IntAttribute("maxLineSkipNum");
            if (growElement->Attribute("maxSkipDistance"))
                configResult.algorithmParams.growParam.maxSkipDistance = growElement->DoubleAttribute("maxSkipDistance");
            if (growElement->Attribute("minLTypeTreeLen"))
                configResult.algorithmParams.growParam.minLTypeTreeLen = growElement->DoubleAttribute("minLTypeTreeLen");
            if (growElement->Attribute("minVTypeTreeLen"))
                configResult.algorithmParams.growParam.minVTypeTreeLen = growElement->DoubleAttribute("minVTypeTreeLen");
        }

        ConfigXmlUtils::LoadPlaneCalibParams(algoElement, configResult.algorithmParams.planeCalibParam);
    }

    ConfigXmlUtils::LoadDebugParam(root, configResult.debugParam);
    ConfigXmlUtils::LoadSerialConfig(root, configResult.serialConfig);

    // 兼容旧配置：若存在已废弃的 eulerOrder/dirVectorInvert/longAxisDir，
    // 则一次性迁移为每台相机手眼矩阵的 eulerOrder/rotX/rotY/rotZ。
    int legacyEulerOrder = 11;
    bool hasLegacyEulerOrder = false;
    double legacyRotX = 0.0;
    double legacyRotY = 0.0;
    double legacyRotZ = 0.0;
    bool hasLegacyRotation = false;

    XMLElement* networkElement = root->FirstChildElement("NetworkConfig");
    if (networkElement)
    {
        if (networkElement->Attribute("tcpServerPort"))
            configResult.tcpPort = static_cast<uint16_t>(networkElement->IntAttribute("tcpServerPort"));
        if (networkElement->Attribute("poseOutputOrder"))
            configResult.poseOutputOrder = networkElement->IntAttribute("poseOutputOrder");
        if (networkElement->Attribute("byteOrder"))
            configResult.byteOrder = networkElement->IntAttribute("byteOrder");

        if (networkElement->Attribute("eulerOrder")) {
            legacyEulerOrder = networkElement->IntAttribute("eulerOrder");
            hasLegacyEulerOrder = true;
        }
        // 旧字段：方向向量调整（XY/XZ/YZ 反向），映射为绕对应轴 180°
        if (networkElement->Attribute("dirVectorInvert")) {
            const int dirInvert = networkElement->IntAttribute("dirVectorInvert");
            switch (dirInvert) {
                case 1: legacyRotZ = 180.0; hasLegacyRotation = true; break;  // XY 反向 ≡ Rz(180°)
                case 2: legacyRotY = 180.0; hasLegacyRotation = true; break;  // XZ 反向 ≡ Ry(180°)
                case 3: legacyRotX = 180.0; hasLegacyRotation = true; break;  // YZ 反向 ≡ Rx(180°)
                default: break;
            }
        }
        // 旧字段：长轴对应轴（Y 轴时相当于绕 Z 旋转 -90°）
        if (networkElement->Attribute("longAxisDir")) {
            const int longAxisDir = networkElement->IntAttribute("longAxisDir");
            if (longAxisDir == 1) {
                legacyRotZ += -90.0;
                hasLegacyRotation = true;
            }
        }
    }

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

    ConfigXmlUtils::LoadHandEyeCalibMatrixs(root,
                                            configResult.handEyeCalibMatrixList,
                                            legacyEulerOrder);

    if (hasLegacyEulerOrder || hasLegacyRotation) {
        for (auto& calibMatrix : configResult.handEyeCalibMatrixList) {
            if (hasLegacyEulerOrder) {
                calibMatrix.eulerOrder = legacyEulerOrder;
            }
            if (hasLegacyRotation) {
                calibMatrix.rotX = legacyRotX;
                calibMatrix.rotY = legacyRotY;
                calibMatrix.rotZ = legacyRotZ;
            }
        }
    }

    LOG_INFO("Config loaded successfully from: %s\n", filePath.c_str());
    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("ScrewPositionConfig");
    doc.InsertEndChild(root);

    ConfigXmlUtils::SaveCameraList(doc, root, configResult.cameraList);

    XMLElement* algoElement = doc.NewElement("AlgorithmParams");
    root->InsertEndChild(algoElement);

    XMLElement* screwElement = doc.NewElement("ScrewParam");
    screwElement->SetAttribute("rodDiameter", configResult.algorithmParams.screwParam.rodDiameter);
    screwElement->SetAttribute("isHorizonScan", configResult.algorithmParams.screwParam.isHorizonScan);
    algoElement->InsertEndChild(screwElement);

    XMLElement* filterElement = doc.NewElement("FilterParam");
    filterElement->SetAttribute("continuityTh", configResult.algorithmParams.filterParam.continuityTh);
    filterElement->SetAttribute("outlierTh", configResult.algorithmParams.filterParam.outlierTh);
    algoElement->InsertEndChild(filterElement);

    XMLElement* cornerElement = doc.NewElement("CornerParam");
    cornerElement->SetAttribute("cornerTh", configResult.algorithmParams.cornerParam.cornerTh);
    cornerElement->SetAttribute("scale", configResult.algorithmParams.cornerParam.scale);
    cornerElement->SetAttribute("minEndingGap", configResult.algorithmParams.cornerParam.minEndingGap);
    cornerElement->SetAttribute("minEndingGap_z", configResult.algorithmParams.cornerParam.minEndingGap_z);
    cornerElement->SetAttribute("jumpCornerTh_1", configResult.algorithmParams.cornerParam.jumpCornerTh_1);
    cornerElement->SetAttribute("jumpCornerTh_2", configResult.algorithmParams.cornerParam.jumpCornerTh_2);
    algoElement->InsertEndChild(cornerElement);

    XMLElement* growElement = doc.NewElement("GrowParam");
    growElement->SetAttribute("maxLineSkipNum", configResult.algorithmParams.growParam.maxLineSkipNum);
    growElement->SetAttribute("yDeviation_max", configResult.algorithmParams.growParam.yDeviation_max);
    growElement->SetAttribute("maxSkipDistance", configResult.algorithmParams.growParam.maxSkipDistance);
    growElement->SetAttribute("zDeviation_max", configResult.algorithmParams.growParam.zDeviation_max);
    growElement->SetAttribute("minLTypeTreeLen", configResult.algorithmParams.growParam.minLTypeTreeLen);
    growElement->SetAttribute("minVTypeTreeLen", configResult.algorithmParams.growParam.minVTypeTreeLen);
    algoElement->InsertEndChild(growElement);

    ConfigXmlUtils::SavePlaneCalibParams(doc, algoElement, configResult.algorithmParams.planeCalibParam);
    ConfigXmlUtils::SaveDebugParam(doc, root, configResult.debugParam);
    ConfigXmlUtils::SaveSerialConfig(doc, root, configResult.serialConfig);

    ConfigXmlUtils::SaveHandEyeCalibMatrixs(doc, root, configResult.handEyeCalibMatrixList);

    XMLElement* networkElement = doc.NewElement("NetworkConfig");
    networkElement->SetAttribute("tcpServerPort", configResult.tcpPort);
    networkElement->SetAttribute("poseOutputOrder", configResult.poseOutputOrder);
    networkElement->SetAttribute("byteOrder", configResult.byteOrder);
    root->InsertEndChild(networkElement);

    // Keep the legacy TCP node for backward compatibility.
    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;
}