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

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

using namespace tinyxml2;

namespace
{
constexpr int kDefaultHandEyeEulerOrder = 11; // Tool/hand-eye default: extrinsic ZYX.
}

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

    // 1. 解析相机设备列表
    ConfigXmlUtils::LoadCameraList(root, configResult.cameraList);

    // 2. 解析算法参数
    XMLElement* algoElement = root->FirstChildElement("AlgorithmParams");
    if (algoElement)
    {
        // 2.1 解析棒材检测参数 (RodParam)
        XMLElement* rodElement = algoElement->FirstChildElement("RodParam");
        if (rodElement)
        {
            rodElement->QueryDoubleAttribute("rodDiameter", &configResult.algorithmParams.rodParam.rodDiameter);
            rodElement->QueryDoubleAttribute("rodLen", &configResult.algorithmParams.rodParam.rodLen);

            LOG_INFO("RodParam: rodDiameter=%.1f, rodLen=%.1f\n",
                     configResult.algorithmParams.rodParam.rodDiameter,
                     configResult.algorithmParams.rodParam.rodLen);
        }

        // 2.2 解析离群点滤波参数 (FilterParam)
        XMLElement* filterElement = algoElement->FirstChildElement("FilterParam");
        if (filterElement)
        {
            filterElement->QueryDoubleAttribute("continuityTh", &configResult.algorithmParams.filterParam.continuityTh);
            filterElement->QueryDoubleAttribute("outlierTh", &configResult.algorithmParams.filterParam.outlierTh);

            LOG_INFO("FilterParam: continuityTh=%.1f, outlierTh=%.1f\n",
                     configResult.algorithmParams.filterParam.continuityTh,
                     configResult.algorithmParams.filterParam.outlierTh);
        }

        // 2.3 解析角点检测参数 (CornerParam)
        XMLElement* cornerElement = algoElement->FirstChildElement("CornerParam");
        if (cornerElement)
        {
            cornerElement->QueryDoubleAttribute("minEndingGap", &configResult.algorithmParams.cornerParam.minEndingGap);
            cornerElement->QueryDoubleAttribute("minEndingGap_z", &configResult.algorithmParams.cornerParam.minEndingGap_z);
            cornerElement->QueryDoubleAttribute("scale", &configResult.algorithmParams.cornerParam.scale);
            cornerElement->QueryDoubleAttribute("cornerTh", &configResult.algorithmParams.cornerParam.cornerTh);
            cornerElement->QueryDoubleAttribute("jumpCornerTh_1", &configResult.algorithmParams.cornerParam.jumpCornerTh_1);
            cornerElement->QueryDoubleAttribute("jumpCornerTh_2", &configResult.algorithmParams.cornerParam.jumpCornerTh_2);

            LOG_INFO("CornerParam: minEndingGap=%.1f, cornerTh=%.1f\n",
                     configResult.algorithmParams.cornerParam.minEndingGap,
                     configResult.algorithmParams.cornerParam.cornerTh);
        }

        // 2.4 解析树生长参数 (GrowParam)
        XMLElement* growElement = algoElement->FirstChildElement("GrowParam");
        if (growElement)
        {
            growElement->QueryDoubleAttribute("yDeviation_max", &configResult.algorithmParams.growParam.yDeviation_max);
            growElement->QueryDoubleAttribute("zDeviation_max", &configResult.algorithmParams.growParam.zDeviation_max);
            growElement->QueryIntAttribute("maxLineSkipNum", &configResult.algorithmParams.growParam.maxLineSkipNum);
            growElement->QueryDoubleAttribute("maxSkipDistance", &configResult.algorithmParams.growParam.maxSkipDistance);
            growElement->QueryDoubleAttribute("minLTypeTreeLen", &configResult.algorithmParams.growParam.minLTypeTreeLen);
            growElement->QueryDoubleAttribute("minVTypeTreeLen", &configResult.algorithmParams.growParam.minVTypeTreeLen);

            LOG_INFO("GrowParam: yDeviation_max=%.1f, zDeviation_max=%.1f\n",
                     configResult.algorithmParams.growParam.yDeviation_max,
                     configResult.algorithmParams.growParam.zDeviation_max);
        }

        // 2.5 解析平面标定参数 (PlaneCalibParams)
        ConfigXmlUtils::LoadPlaneCalibParams(algoElement, configResult.algorithmParams.planeCalibParam);

        XMLElement* positioningElement = algoElement->FirstChildElement("PositioningParam");
        if (positioningElement)
        {
            positioningElement->QueryDoubleAttribute("distToSideRatio", &configResult.algorithmParams.distToSideRatio);
            positioningElement->QueryDoubleAttribute("groundRemoveOffset", &configResult.algorithmParams.groundRemoveOffset);
            positioningElement->QueryDoubleAttribute("clusterNDist", &configResult.algorithmParams.clusterNDist);
        }

        configResult.algorithmParams.pointCloudFilterParams.clear();
        for (XMLElement* filter = algoElement->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);
        }
    }

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

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

    // 5. 解析网络参数
    XMLElement* networkElement = root->FirstChildElement("NetworkConfig");
    if (networkElement)
    {
        networkElement->QueryIntAttribute("eulerOrder", &configResult.eulerOrder);
        networkElement->QueryIntAttribute("byteOrder", &configResult.byteOrder);

        LOG_INFO("Network config: protocolPoseOrder=%d, byteOrder=%d\n",
                 configResult.eulerOrder, configResult.byteOrder);
    }

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

    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;

    // 创建 XML 声明
    XMLDeclaration* declaration = doc.NewDeclaration();
    doc.InsertFirstChild(declaration);

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

    // 1. 保存相机设备列表
    ConfigXmlUtils::SaveCameraList(doc, root, configResult.cameraList);

    // 2. 保存算法参数
    XMLElement* algoElement = doc.NewElement("AlgorithmParams");

    // 2.1 保存棒材检测参数
    XMLElement* rodElement = doc.NewElement("RodParam");
    rodElement->SetAttribute("rodDiameter", configResult.algorithmParams.rodParam.rodDiameter);
    rodElement->SetAttribute("rodLen", configResult.algorithmParams.rodParam.rodLen);
    algoElement->InsertEndChild(rodElement);

    // 2.2 保存离群点滤波参数
    XMLElement* filterElement = doc.NewElement("FilterParam");
    filterElement->SetAttribute("continuityTh", configResult.algorithmParams.filterParam.continuityTh);
    filterElement->SetAttribute("outlierTh", configResult.algorithmParams.filterParam.outlierTh);
    algoElement->InsertEndChild(filterElement);

    // 2.3 保存角点检测参数
    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);

    // 2.4 保存树生长参数
    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);

    // 2.5 保存平面标定参数
    ConfigXmlUtils::SavePlaneCalibParams(doc, algoElement, configResult.algorithmParams.planeCalibParam);

    XMLElement* positioningElement = doc.NewElement("PositioningParam");
    positioningElement->SetAttribute("distToSideRatio", configResult.algorithmParams.distToSideRatio);
    positioningElement->SetAttribute("groundRemoveOffset", configResult.algorithmParams.groundRemoveOffset);
    positioningElement->SetAttribute("clusterNDist", configResult.algorithmParams.clusterNDist);
    algoElement->InsertEndChild(positioningElement);

    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);
        algoElement->InsertEndChild(filterElement);
    }

    root->InsertEndChild(algoElement);

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

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

    // 5. 保存网络参数
    XMLElement* networkElement = doc.NewElement("NetworkConfig");
    networkElement->SetAttribute("eulerOrder", configResult.eulerOrder);
    networkElement->SetAttribute("byteOrder", configResult.byteOrder);
    root->InsertEndChild(networkElement);

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

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

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

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


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