#include "eigen_proto_utils.h"
#include <fstream>
#include <google/protobuf/io/zero_copy_stream_impl.h>

namespace EigenProto {

bool serialize(const Eigen::MatrixXd& mat, eigen::MatrixXd* proto_mat) {
    if (!proto_mat) return false;

    proto_mat->set_rows(static_cast<int>(mat.rows()));
    proto_mat->set_cols(static_cast<int>(mat.cols()));

    auto* data_field = proto_mat->mutable_data();
    data_field->Resize(mat.size(), 0.0);

    // 方法一：使用 mutable_data()
    Eigen::Map<Eigen::VectorXd>(data_field->mutable_data(), mat.size()) =
        Eigen::Map<const Eigen::VectorXd>(mat.data(), mat.size());

    return true;
}

bool deserialize(const eigen::MatrixXd& proto_mat, Eigen::MatrixXd* mat) {
    if (!mat) return false;

    int rows = proto_mat.rows();
    int cols = proto_mat.cols();

    if (rows < 0 || cols < 0) return false;

    mat->resize(rows, cols);

    if (proto_mat.data_size() != mat->size()) {
        return false;
    }

    Eigen::Map<Eigen::VectorXd>(mat->data(), mat->size()) =
        Eigen::Map<const Eigen::VectorXd>(proto_mat.data().data(), proto_mat.data_size());

    return true;
}


void Matrix4dToProto(const Eigen::Matrix4d& mat, eigen::Matrix4d* proto_mat) {
    proto_mat->clear_data();
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            proto_mat->add_data(mat(i, j));  // 行优先顺序
        }
    }
}

void ProtoToMatrix4d(const eigen::Matrix4d& proto_mat, Eigen::Matrix4d* mat) {
    if (proto_mat.data_size() != 16) {
        throw std::runtime_error("Invalid Matrix4d proto: data size != 16");
    }

    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            (*mat)(i, j) = proto_mat.data(i * 4 + j);  // 行优先还原
        }
    }
}


bool saveToFile(const Eigen::MatrixXd& mat, const std::string& filename) {
    eigen::MatrixXd proto_mat;
    if (!serialize(mat, &proto_mat)) {
        return false;
    }

    std::ofstream ofs(filename, std::ios::binary);
    if (!ofs.is_open()) return false;

    return proto_mat.SerializeToOstream(&ofs);
}

bool loadFromFile(Eigen::MatrixXd* mat, const std::string& filename) {
    if (!mat) return false;

    std::ifstream ifs(filename, std::ios::binary);
    if (!ifs.is_open()) return false;

    eigen::MatrixXd proto_mat;
    if (!proto_mat.ParseFromIstream(&ifs)) {
        return false;
    }

    return deserialize(proto_mat, mat);
}

} // namespace EigenProto