#pragma once

#include <psemek/math/constants.hpp>

#include <cmath>
#include <algorithm>
#include <optional>

namespace psemek::math
{

	template <typename T>
	T sign(T const & x)
	{
		if (x > 0)
			return 1;
		else if (x < 0)
			return -1;
		else
			return 0;
	}

	template <typename T>
	auto sqr(T const & x)
	{
		return x * x;
	}

	template <typename T>
	auto frac(T const & x)
	{
		return x - std::floor(x);
	}

	template <typename T>
	T deg(T x)
	{
		return static_cast<T>((180 * x) / static_cast<T>(pi));
	}

	template <typename T>
	T rad(T x)
	{
		return static_cast<T>((x * static_cast<T>(pi)) / 180);
	}

	template <typename X, typename T>
	auto lerp(X const & x0, X const & x1, T const & t) -> decltype(x0 * t)
	{
		return x0 * (T(1) - t) + x1 * t;
	}

	template <typename T>
	T sin_over_x(T const & x)
	{
		if (std::abs(x) < std::numeric_limits<T>::epsilon())
			return T{1};
		return std::sin(x) / x;
	}

	template <typename T>
	T acos_over_sqrt_1_minus_x2(T const & x)
	{
		auto t = std::sqrt(std::max(T{0}, T{1} - x * x));
		if (t < std::numeric_limits<T>::epsilon())
			return T{1};
		return std::acos(x) / t;
	}

	namespace detail
	{

		template <typename P>
		auto lerpn_impl(P &, P const &)
		{}

		template <typename P, typename T, typename ... Args>
		auto lerpn_impl(P & res, P const & o, P const & p, T const & t, Args const & ... args)
		{
			res += t * (p - o);
			lerpn_impl(res, o, args...);
		}

	}

	template <typename P, typename T, typename ... Args>
	auto lerpn(P const & p, T const &, Args const & ... args)
	{
		P res = p;
		detail::lerpn_impl(res, p, args...);
		return res;
	}

	// roots of ax^2 + bx + c = 0
	// in increasing order
	template <typename T>
	std::optional<std::pair<T, T>> solve_quadratic(T const & a, T const & b, T const & c)
	{
		auto const D = b * b - 4 * a * c;
		if (D < 0) return std::nullopt;

		auto t = - (b + (b > 0 ? 1 : -1) * std::sqrt(D)) / 2;
		auto x1 = t / a;
		auto x2 = c / t;

		if (x1 > x2) std::swap(x1, x2);
		return std::pair{x1, x2};
	}

	// roots of ax^2 + bx + c = 0
	// in increasing order
	// the quadratic is assumed to have real roots
	template <typename T>
	std::pair<T, T> solve_quadratic_positive(T const & a, T const & b, T const & c)
	{
		auto const D = b * b - 4 * a * c;
		if (D <= 0)
		{
			auto x = - b / (2 * a);
			return std::pair{x, x};
		}

		auto t = - (b + (b > 0 ? 1 : -1) * std::sqrt(D)) / 2;
		auto x1 = t / a;
		auto x2 = c / t;

		if (x1 > x2) std::swap(x1, x2);
		return std::pair{x1, x2};
	}

	// Moves the angle to the range [-pi, pi]
	template <typename T>
	T normalize_angle(T a)
	{
		while (a < -static_cast<T>(pi))
			a += static_cast<T>(2 * pi);
		while (a > static_cast<T>(pi))
			a -= static_cast<T>(2 * pi);
		return a;
	}

	// returns (a1 - a0)
	template <typename T>
	T angle_difference(T a0, T a1)
	{
		return normalize_angle(a1 - a0);
	}

	template <typename T>
	T rotate_towards(T value, T target, T max_difference)
	{
		auto d = angle_difference(value, target);
		if (d > max_difference)
			d = max_difference;
		if (d < -max_difference)
			d = -max_difference;
		return value + d;
	}

	template <typename T>
	T imod(T x, T m)
	{
		static_assert(std::is_integral_v<T>);
		return ((x % m) + m) % m;
	}

	template <typename T>
	T idiv(T x, T m)
	{
		static_assert(std::is_integral_v<T>);
		return (x - imod(x, m)) / m;
	}

	template <typename T>
	T div_ceil(T x, T y)
	{
		return (x / y) + ((x % y) == 0 ? 0 : 1);
	}

	template <typename T>
	T fmod(T x, T m)
	{
		return (x >= 0) ? std::fmod(x, m) : (m - std::fmod(-x, m));
	}

	template <typename T>
	bool make_min(T & target, T const & source)
	{
		if (target > source)
		{
			target = source;
			return true;
		}
		return false;
	}

	template <typename T>
	bool make_max(T & target, T const & source)
	{
		if (target < source)
		{
			target = source;
			return true;
		}
		return false;
	}

	template <typename T>
	bool compare_swap(T & min, T & max)
	{
		if (min > max)
		{
			std::swap(min, max);
			return true;
		}
		return false;
	}

	template <typename T>
	T smoothstep(T x)
	{
		return x * x * (T{3} - T{2} * x);
	}

	template <typename T>
	T smootherstep(T x)
	{
		auto x2 = x * x;
		return x * x2 * (T{10} - x * T{15} + x2 * T{6});
	}

	template <typename T>
	T inverse_smoothstep(T x)
	{
		return T{1} / T{2} - std::sin(std::asin(T{1} - T{2} * x) / T{3});
	}

}
