#include "VrConfig.h"

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

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

using namespace tinyxml2;

namespace {

// 读取 CornerParam 子节点到 VrAlgorithmParams
void LoadAlgorithmParamsBlock(XMLElement* algoElement, VrAlgorithmParams& params)
{
    if (!algoElement) return;

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

void SaveAlgorithmParamsBlock(XMLDocument& doc, XMLElement* algoElement, const VrAlgorithmParams& params)
{
    XMLElement* cornerElement = doc.NewElement("CornerParam");
    cornerElement->SetAttribute("cornerTh", params.cornerParam.cornerTh);
    cornerElement->SetAttribute("scale", params.cornerParam.scale);
    cornerElement->SetAttribute("minEndingGap", params.cornerParam.minEndingGap);
    cornerElement->SetAttribute("minEndingGap_z", params.cornerParam.minEndingGap_z);
    cornerElement->SetAttribute("jumpCornerTh_1", params.cornerParam.jumpCornerTh_1);
    cornerElement->SetAttribute("jumpCornerTh_2", params.cornerParam.jumpCornerTh_2);
    algoElement->InsertEndChild(cornerElement);
}

// 读取一个 <HandEyeCalibMatrix .../> 节点的所有属性到目标对象
void LoadHandEyeMatrixAttributes(XMLElement* e, VrHandEyeCalibMatrix& m)
{
    if (!e) return;
    if (e->Attribute("cameraIndex")) m.cameraIndex = e->IntAttribute("cameraIndex");
    for (int i = 0; i < 16; ++i) {
        char attrName[16];
        std::snprintf(attrName, sizeof(attrName), "m%d", i);
        if (e->Attribute(attrName)) m.matrix[i] = e->DoubleAttribute(attrName);
    }
    if (e->Attribute("eulerOrder"))     m.eulerOrder = e->IntAttribute("eulerOrder");
    if (e->Attribute("rotX"))           m.rotX = e->DoubleAttribute("rotX");
    if (e->Attribute("rotY"))           m.rotY = e->DoubleAttribute("rotY");
    if (e->Attribute("rotZ"))           m.rotZ = e->DoubleAttribute("rotZ");
    if (e->Attribute("approachOffset")) m.approachOffset = e->DoubleAttribute("approachOffset");
    if (e->Attribute("offsetX"))        m.offsetX = e->DoubleAttribute("offsetX");
    if (e->Attribute("offsetY"))        m.offsetY = e->DoubleAttribute("offsetY");
    if (e->Attribute("offsetZ"))        m.offsetZ = e->DoubleAttribute("offsetZ");
    if (e->Attribute("outRotX"))        m.outRotX = e->DoubleAttribute("outRotX");
    if (e->Attribute("outRotY"))        m.outRotY = e->DoubleAttribute("outRotY");
    if (e->Attribute("outRotZ"))        m.outRotZ = e->DoubleAttribute("outRotZ");
}

void SaveHandEyeMatrixAttributes(XMLElement* e, const VrHandEyeCalibMatrix& m)
{
    e->SetAttribute("cameraIndex", m.cameraIndex);
    for (int i = 0; i < 16; ++i) {
        char attrName[16];
        std::snprintf(attrName, sizeof(attrName), "m%d", i);
        e->SetAttribute(attrName, m.matrix[i]);
    }
    e->SetAttribute("eulerOrder", m.eulerOrder);
    e->SetAttribute("rotX", m.rotX);
    e->SetAttribute("rotY", m.rotY);
    e->SetAttribute("rotZ", m.rotZ);
    e->SetAttribute("approachOffset", m.approachOffset);
    e->SetAttribute("offsetX", m.offsetX);
    e->SetAttribute("offsetY", m.offsetY);
    e->SetAttribute("offsetZ", m.offsetZ);
    e->SetAttribute("outRotX", m.outRotX);
    e->SetAttribute("outRotY", m.outRotY);
    e->SetAttribute("outRotZ", m.outRotZ);
}

const char* DetectionTypeToStr(DetectionType t)
{
    return (t == DETECTION_TYPE_DISC_RACK) ? "discRack" : "discHole";
}

DetectionType StrToDetectionType(const char* s)
{
    if (!s) return DETECTION_TYPE_DISC_HOLE;
    return (std::string(s) == "discRack") ? DETECTION_TYPE_DISC_RACK : DETECTION_TYPE_DISC_HOLE;
}

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

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

    // 共享平面校准：相机级配置
    ConfigXmlUtils::LoadPlaneCalibParams(root, configResult.algorithmParams.planeCalibParam);

    // 读取四套 (camera, type) 配置
    configResult.detectionConfigList.clear();
    XMLElement* setsRoot = root->FirstChildElement("DetectionConfigs");
    if (setsRoot) {
        XMLElement* setElem = setsRoot->FirstChildElement("DetectionConfig");
        while (setElem) {
            VrDetectionConfigSet item;
            if (setElem->Attribute("cameraIndex"))
                item.cameraIndex = setElem->IntAttribute("cameraIndex");
            if (setElem->Attribute("detectionType"))
                item.detectionType = StrToDetectionType(setElem->Attribute("detectionType"));

            // handEye 矩阵子节点
            if (XMLElement* heElem = setElem->FirstChildElement("HandEyeCalibMatrix")) {
                LoadHandEyeMatrixAttributes(heElem, item.handEyeMatrix);
            }
            // 算法参数子块
            LoadAlgorithmParamsBlock(setElem->FirstChildElement("AlgorithmParams"), item.algorithmParams);

            // 同步 handEyeMatrix.cameraIndex
            item.handEyeMatrix.cameraIndex = item.cameraIndex;

            configResult.detectionConfigList.push_back(item);
            setElem = setElem->NextSiblingElement("DetectionConfig");
        }
    }

    // 缺失槽位补全默认值
    int cameraCount = static_cast<int>(configResult.cameraList.size());
    if (cameraCount <= 0) cameraCount = 1;
    for (int camIdx = 1; camIdx <= cameraCount; ++camIdx) {
        configResult.EnsureDetectionConfig(camIdx, DETECTION_TYPE_DISC_HOLE);
        configResult.EnsureDetectionConfig(camIdx, DETECTION_TYPE_DISC_RACK);
    }

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

    if (XMLElement* networkElement = root->FirstChildElement("NetworkConfig")) {
        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 (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 (detection sets: %zu)\n",
             filePath.c_str(), configResult.detectionConfigList.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("DiscHolePoseConfig");
    doc.InsertEndChild(root);

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

    // 共享平面校准
    ConfigXmlUtils::SavePlaneCalibParams(doc, root, configResult.algorithmParams.planeCalibParam);

    // 四套 (camera, type) 配置
    XMLElement* setsRoot = doc.NewElement("DetectionConfigs");
    for (const auto& item : configResult.detectionConfigList) {
        XMLElement* setElem = doc.NewElement("DetectionConfig");
        setElem->SetAttribute("cameraIndex", item.cameraIndex);
        setElem->SetAttribute("detectionType", DetectionTypeToStr(item.detectionType));

        XMLElement* heElem = doc.NewElement("HandEyeCalibMatrix");
        VrHandEyeCalibMatrix syncedMatrix = item.handEyeMatrix;
        syncedMatrix.cameraIndex = item.cameraIndex;
        SaveHandEyeMatrixAttributes(heElem, syncedMatrix);
        setElem->InsertEndChild(heElem);

        XMLElement* algoElem = doc.NewElement("AlgorithmParams");
        SaveAlgorithmParamsBlock(doc, algoElem, item.algorithmParams);
        setElem->InsertEndChild(algoElem);

        setsRoot->InsertEndChild(setElem);
    }
    root->InsertEndChild(setsRoot);

    ConfigXmlUtils::SaveDebugParam(doc, root, configResult.debugParam);
    ConfigXmlUtils::SaveSerialConfig(doc, root, configResult.serialConfig);

    XMLElement* networkElement = doc.NewElement("NetworkConfig");
    networkElement->SetAttribute("tcpServerPort", configResult.tcpPort);
    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;
}