//
// Created by xtkuang on 2025/7/8.
//

#ifndef INERTIAL_H
#define INERTIAL_H


#pragma once

#include <eigen3/Eigen/Core>

#include "utils/math/se3.h"

namespace cmvr::dyn {

class Inertial;

class Inertial {
public:
    using MatrixType = Eigen::Matrix<double, 6, 6>;

    static MatrixType I(double mass);

    static MatrixType I(double mass, double ixx, double iyy, double izz,
                        const math::SE3::MatrixType& T = math::SE3::Identity());

    static MatrixType I(double mass, double ixx, double iyy, double izz,
                        const Eigen::Vector3d& com);

    static MatrixType I(double mass, double ixx, double iyy, double izz, double ixy, double ixz, double iyz,
                        const math::SE3::MatrixType& T = math::SE3::Identity());

    static MatrixType I(double mass, double ixx, double iyy, double izz, double ixy, double ixz, double iyz,
                        const Eigen::Vector3d& com);

    static MatrixType I(double mass, const Eigen::Vector3d& inertia,
                        const math::SE3::MatrixType& T = math::SE3::Identity());

    static MatrixType I(double mass, const Eigen::Matrix<double, 6, 1>& inertia,
                        const math::SE3::MatrixType& T = math::SE3::Identity());

    static MatrixType Transform(const math::SE3::MatrixType& T, const MatrixType& I);

    static Eigen::Vector3d GetCOM(const MatrixType& I);

    static double GetMass(const MatrixType& I);

    // [I_{xx}, I_{yy}, I_{zz}, I_{xy}, I_{xz}, I_{yz}]^T
    static Eigen::Vector<double, 6> GetInertia(const MatrixType& I);
};

}
#endif //INERTIAL_H