#include <Eigen/Dense>
#include <roplat_cpp_msg/src/lib.rs.h>

roplat::Vec2 to_rust(const Eigen::Vector2d &vec)
{
    roplat::Vec2 rust_vec;
    rust_vec.data[0] = vec(0);
    rust_vec.data[1] = vec(1);
    return rust_vec;
}

roplat::Vec3 to_rust(const Eigen::Vector3d &vec)
{
    roplat::Vec3 rust_vec;
    rust_vec.data[0] = vec(0);
    rust_vec.data[1] = vec(1);
    rust_vec.data[2] = vec(2);
    return rust_vec;
}

roplat::Vec4 to_rust(const Eigen::Vector4d &vec)
{
    roplat::Vec4 rust_vec;
    rust_vec.data[0] = vec(0);
    rust_vec.data[1] = vec(1);
    rust_vec.data[2] = vec(2);
    rust_vec.data[3] = vec(3);
    return rust_vec;
}

roplat::Vec5 to_rust(const Eigen::Matrix<double, 5, 1> &vec)
{
    roplat::Vec5 rust_vec;
    for (int i = 0; i < 5; ++i)
    {
        rust_vec.data[i] = vec(i);
    }
    return rust_vec;
}

roplat::Vec6 to_rust(const Eigen::Matrix<double, 6, 1> &vec)
{
    roplat::Vec6 rust_vec;
    for (int i = 0; i < 6; ++i)
    {
        rust_vec.data[i] = vec(i);
    }
    return rust_vec;
}

roplat::VecX to_rust(const Eigen::VectorXd &vec)
{
    roplat::VecX res;
    res.data.reserve(vec.size());
    for (int i = 0; i < vec.size(); ++i)
    {
        res.data.push_back(vec(i));
    }
    return res;
}

roplat::Mat to_rust(const Eigen::MatrixXd &mat)
{
    roplat::Mat res;
    res.rows = mat.rows();
    res.cols = mat.cols();
    size_t total_size = res.rows * res.cols;
    res.data.reserve(total_size);
    const double *ptr = mat.data();
    for (size_t i = 0; i < total_size; ++i)
    {
        res.data.push_back(ptr[i]);
    }
    return res;
}