/*
 * Copyright (c) 1999-2019 Hartmut Seichter
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */
#ifndef PW_CORE_MATH_HPP
#define PW_CORE_MATH_HPP

#include <cmath>
#include <algorithm>
//#include <pw/core/matrix.hpp>
//#include <pw/core/quaternion.hpp>

namespace pw {

constexpr double __PW_PI = 3.1415926535897932384626433832795028841971693993751058209;
constexpr double __PW_PI_DOUBLE = 2.0 * __PW_PI;

template <typename T>
inline const T pi() { return static_cast<T>(__PW_PI); }

template <typename T>
inline const T one_over_pi() { return static_cast<T>(1 / __PW_PI); }

template <typename T>
inline T rad_to_deg(const T& angle_in_radian) {
	return angle_in_radian * static_cast<T>(180) * one_over_pi<T>();
}

template <typename T>
inline T deg_to_rad(const T& angle_in_degree) {
	return angle_in_degree * pi<T>() / static_cast<T>(180);
}

template <typename T>
inline T repeat(const T& t, const T& length) {
	return std::clamp(t - std::floor(t / length) * length, T(0), length);
}

template <typename T>
inline T ping_pong(const T& t,const T& length) {
	auto tn = repeat(t, length * T(2));
	return length - std::abs(tn - length);
}

template <typename T>
inline T wrap_angle(const T& angle_in_radian) {
	using std::floor;
	return angle_in_radian - __PW_PI_DOUBLE * floor( angle_in_radian / __PW_PI_DOUBLE );
}

//void extractRotation(const matrix &A, Quaterniond &q,const unsigned int maxIter)
//{
//	for (auto iter = 0; iter < maxIter; iter++){
//		auto R = q.matrix();
//		Vector3d omega = (R.col(0).cross(A.col(0)) + R.col(1).cross(A.col(1)) + R.col(2).cross(A.col(2)))*(1.0 / fabs(R.col(0).dot(A.col(0)) + R.col(1).dot(A.col(1)) + R.col(2).dot(A.col(2))) +1.0e-9);double w = omega.norm();if (w < 1.0e-9)break;q = Quaterniond(AngleAxisd(w, (1.0 / w)*omega))*q;q.normalize();}}

}

#endif
