#ifndef H_frame_conversions
#define H_frame_conversions

#include <Eigen/Dense>
#include <cmath>

#ifndef M_PI
#define M_PI 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117068
#endif

namespace frame_conversions {
/// @brief Computes the modulo of a number with another number, ensuring that the results lies in the interval [0;b].
/// @param  a   The number to compute the modulo of.
/// @param  b   The modulo.
/// @return The modulo of a with b, ensuring that the result lies in the interval [0;b]
template <typename T>
inline T fmod_pos(T a, T b) {
    return a - b*std::floor(a/b);
}

/// @brief Maps a value x from the range (a,b) to the range (c,d).
///
/// This function takes an input value x and maps it from the input range (a,b)
/// to the output range (c,d). The resulting mapped value is returned.
///
/// @param x The value to be mapped.
/// @param a The lower bound of the input range.
/// @param b The upper bound of the input range.
/// @param c The lower bound of the output range.
/// @param d The upper bound of the output range.
///
/// @return The mapped value of x in the output range.
///
/// @note Make sure that a != b to avoid a divide-by-zero error.
double map(double x, double a, double b, double c, double d);

} // namespace frame_conversions

/// @brief Converts a spherical vector [r, theta, phi] in ISO convention (theta=0 is Z+) to a cartesian vector.
Eigen::Vector3d sph2cart(const Eigen::Vector3d & sph);

/// @brief Converts a cartesian vector to a spherical vector [r, theta, phi] in ISO convention (theta=0 is Z+).
Eigen::Vector3d cart2sph(const Eigen::Vector3d & cart);

/// @brief Converts equirectangular coordinates to spherical coordinates. 
/// @param  i       Row in the equirectangular image.
/// @param  j       Column in the equirectangular image.
/// @param  width   Width of the equirectangular image.
/// @param  height  Height of the equirectangular image.
/// @return A spherical vector [r, theta, phi] in ISO convention (theta=0 is Z+).
Eigen::Vector3d equirectangular2sph(int i, int j, unsigned int width, unsigned int height);

/// @brief Converts spherical coordinates to pixel coordinates of an equirectangular image.
/// @param  p_sph    A spherical vector [r, theta, phi] in ISO convention (theta=0 is Z+).
/// @param  width    Width of the equirectangular image.
/// @param  height   Height of the equirectangular image.
/// @return A 2D vector of floating point numbers [i, j] representing the pixel coordinates of the equirectangular image.
Eigen::Vector2d sph2equirectangular_d(Eigen::Vector3d const& p_sph, unsigned int width, unsigned int height);

/// @brief Converts spherical coordinates to pixel coordinates of an equirectangular image.
/// @param  p_sph    A spherical vector [r, theta, phi] in ISO convention (theta=0 is Z+).
/// @param  width    Width of the equirectangular image.
/// @param  height   Height of the equirectangular image.
/// @return A 2D vector of integers [i, j] representing the pixel coordinates of the equirectangular image.
Eigen::Vector2i sph2equirectangular_i(Eigen::Vector3d const& p_sph, unsigned int width, unsigned int height);

#endif