#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 constexpr auto sqrt_2      = 1.4142135623730951;
static constexpr auto sqrt_3      = 1.7320508075688772;
static constexpr auto infinity    = std::numeric_limits<double>::infinity();
static const auto     x_direction = Eigen::Vector3d{1.0, 0.0, 0.0};

inline auto to_vec4d_by_1(Eigen::Vector3d x)
{
    return Eigen::Vector4d{x[0], x[1], x[2], 1.};
}

inline auto to_vec4d_by_0(Eigen::Vector3d x)
{
    return Eigen::Vector4d{x[0], x[1], x[2], .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;
}

inline auto corrected_atan2(double y, double x)
{
    auto v = atan2(y, x);
    return v < 0 ? v + two_pi : v;
}