#pragma once

#include "math_defs.h"
#include "eigen_alias.hpp"

static constexpr auto epsilon     = std::numeric_limits<double>::epsilon() * 1e6;
static constexpr auto pi          = 3.14159265358979323846;
static constexpr auto two_pi      = pi * 2;
static constexpr auto pi_div_2    = pi / 2;
static constexpr auto inv_pi      = 1. / pi;
static constexpr auto inv_two_pi  = 1. / two_pi;
static const auto     x_direction = Eigen::Vector3d{1.0, 0.0, 0.0};

inline void vec3d_conversion(const vector3d& src, Eigen::Ref<Eigen::Vector3d> dst)
{
    dst = Eigen::Map<const Eigen::Vector3d>(&src.x);
}

inline void vec3d_conversion(vector3d&& src, Eigen::Ref<Eigen::Vector3d> dst) { std::move(&src.x, &src.x + 3, dst.data()); }

inline double sign(const double t) { return t >= 0.0 ? 1.0 : -1.0; }

// Eigen has a type Isometry which supports similar operations as this
// but we use AffineCompact to get lower storage cost
// so we have to implement a helper function to apply the operation of Isometry to AffineCompact
inline auto inversed_affine_transform(const Eigen::Transform<double, 3, Eigen::AffineCompact>& trs)
{
    Eigen::Transform<double, 3, Eigen::AffineCompact> result;

    auto linear_part                                = result.matrix().template topLeftCorner<3, 3>();
    linear_part                                     = trs.linear().transpose();
    result.matrix().template topRightCorner<3, 1>() = -linear_part * trs.translation();
    result.makeAffine();

    return result;
}