#ifndef DEPTHGUIDE_MATH_HELPER_HPP
#define DEPTHGUIDE_MATH_HELPER_HPP

#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtx/transform.hpp>

#include <Eigen/Geometry>

// r in radius
inline glm::mat4 to_transform_mat(glm::vec3 t, glm::vec3 r) {
    static constexpr auto unit_x = glm::vec3(1.0f, 0.0f, 0.0f);
    static constexpr auto unit_y = glm::vec3(0.0f, 1.0f, 0.0f);
    static constexpr auto unit_z = glm::vec3(0.0f, 0.0f, 1.0f);
    auto rot = glm::angleAxis(r.x, unit_x)
               * glm::angleAxis(r.y, unit_y)
               * glm::angleAxis(r.z, unit_z);
    auto offset = glm::translate(t);
    return offset * glm::mat4_cast(rot);
}

inline glm::mat4 to_transform_mat(glm::vec3 t, glm::mat3 r) {
    auto ret = glm::mat4(r);
    ret[3] = glm::vec4(t, 1.f);
    return ret;
}

template<typename Scalar, int Mode>
inline glm::mat4 to_mat4(const Eigen::Transform<Scalar, 3, Mode> &m) {
    auto ret = glm::mat4();
    auto &mat = m.matrix();
    for (auto j = 0; j < 4; ++j)
        for (auto i = 0; i < 4; ++i) {
            ret[j][i] = mat(i, j);
        }
    return ret;
}

template<typename Scalar, int Mode>
inline auto to_eigen_transform(const glm::mat4 &m) {
    auto ret = Eigen::Transform<Scalar, 3, Mode>();
    auto &mat = ret.matrix();
    for (auto j = 0; j < 4; ++j)
        for (auto i = 0; i < 4; ++i) {
            mat(i, j) = m[j][i];
        }
    return ret;
}

template<typename Scalar, int Mode>
inline void to_eigen_transform(const glm::mat4 &m,
                               Eigen::Transform<Scalar, 3, Mode> &ret) {
    ret = to_eigen_transform<Scalar, Mode>(m);
}

template<typename T>
concept Vec2Type = requires(T t) {
    { t.x } -> std::convertible_to<float>;
    { t.y } -> std::convertible_to<float>;
};

template<Vec2Type T>
inline auto to_vec2(const T &v) {
    return glm::vec2(v.x, v.y);
}

template<typename T>
concept Vec3Type = requires(T t) {
    { t.x } -> std::convertible_to<float>;
    { t.y } -> std::convertible_to<float>;
    { t.z } -> std::convertible_to<float>;
};

template<Vec3Type T>
inline auto to_vec3(const T &v) {
    return glm::vec3(v.x, v.y, v.z);
}

inline auto to_homo(const glm::vec3 &v) {
    return glm::vec4(v, 1.f);
}

inline auto to_homo(const glm::vec2 &v) {
    return glm::vec3(v, 1.f);
}

inline auto from_homo(const glm::vec4 &v) {
    return glm::vec3(v) / v.w;
}

inline auto from_homo(const glm::vec3 &v) {
    return glm::vec2(v) / v.z;
}

// transform point
inline glm::vec3 transform_p(const glm::mat4 &mat, const glm::vec3 &point) {
    return from_homo(mat * to_homo(point));
}

inline glm::vec2 transform_p(const glm::mat3 &mat, const glm::vec2 &point) {
    return from_homo(mat * to_homo(point));
}

// transform vector
inline glm::vec3 transform_v(const glm::mat4 &mat, const glm::vec3 &vec) {
    return glm::mat3(mat) * vec;
}

#endif //DEPTHGUIDE_MATH_HELPER_HPP