#include <iostream>
#include <Eigen/Dense>
#include "eigen_proto_utils.h"  // 你的序列化工具

using namespace EigenProto;

int main() {
    std::cout << "=== Eigen::MatrixXd Protobuf Serialization Test ===" << std::endl;

    // Step 1: 创建测试矩阵（3x4，含各种数值）
    Eigen::MatrixXd original(3, 4);
    original << 1.5, -2.7, 0.0, 3.14159,
                100.0, -0.001, 42.42, -999.999,
                1e-6, 1e6, -3.1415926535, 0.0;

    std::cout << "\nOriginal Matrix:\n" << original << std::endl;

    // Step 2: 序列化并保存到文件
    std::string filename = "test_matrix.pb";
    if (!saveToFile(original, filename)) {
        std::cerr << "❌ Serialization failed!" << std::endl;
        return 1;
    }
    std::cout << "✅ Serialized and saved to: " << filename << std::endl;

    // Step 3: 从文件加载并反序列化
    Eigen::MatrixXd loaded;
    if (!loadFromFile(&loaded, filename)) {
        std::cerr << "❌ Deserialization failed!" << std::endl;
        return 1;
    }
    std::cout << "\nLoaded Matrix:\n" << loaded << std::endl;

    // Step 4: 比较矩阵（使用 isApprox，允许浮点误差）
    const double tolerance = 1e-12; // 高精度比较
    if (original.rows() == loaded.rows() &&
        original.cols() == loaded.cols() &&
        original.isApprox(loaded, tolerance)) {
        std::cout << "\n✅ SUCCESS: Matrices are equal within tolerance " << tolerance << "!" << std::endl;
    } else {
        std::cout << "\n❌ FAILURE: Matrices are NOT equal!" << std::endl;

        // 打印差异（可选）
        if (original.rows() == loaded.rows() && original.cols() == loaded.cols()) {
            std::cout << "Difference matrix:\n" << (original - loaded) << std::endl;
        }

        return 1;
    }

    // Step 5: 额外测试：序列化到内存再反序列化（不经过文件）
    {
        eigen::MatrixXd proto_mat;
        if (!serialize(original, &proto_mat)) {
            std::cerr << "❌ In-memory serialization failed!" << std::endl;
            return 1;
        }

        Eigen::MatrixXd in_memory_loaded;
        if (!deserialize(proto_mat, &in_memory_loaded)) {
            std::cerr << "❌ In-memory deserialization failed!" << std::endl;
            return 1;
        }

        if (original.isApprox(in_memory_loaded, tolerance)) {
            std::cout << "✅ In-memory round-trip test PASSED!" << std::endl;
        } else {
            std::cout << "❌ In-memory round-trip test FAILED!" << std::endl;
            return 1;
        }
    }

    std::cout << "\n🎉 All tests PASSED!" << std::endl;
    return 0;
}