#pragma once

#include <cmath>
#include <cstdint>
#include <concepts>
#include <limits>

namespace psemek::util
{

	/** A base template class implementing fixed-point arithmetic.
	 *
	 *  unsigned_fixed_point<I, F> with I integer bits and F fractional bits is
	 *  represented as an (I+F)-bit-wide standard unsigned integer type, and the
	 *  stored value is interpreted as representing the fraction (value / 2^F).
	 *  It is capable of representing values in the range from 0 to (2^(I+F) - 1) / 2^F,
	 *  with a fixed precision of 1 / 2^F.
	 *
	 *  signed_fixed_point<I, F> with I integer bits and F fractional bits is
	 *  represented as an (I+F)-bit-wide standard signed integer type, and the
	 *  stored value is interpreted as representing the fraction (value / 2^F).
	 *  It is capable of representing values in the range from (- 2^(I+F-1)) / 2^F
	 *  to (2^(I+F-1) - 1) / 2^F, with a fixed precision of 1 / 2^F.
	 *
	 *  For example,
	 *
	 *      unsigned_fixed_point<8, 8> can represent the range [0, 255.996094]
	 *      with a fixed precision of 0.00390625
	 *
	 *      signed_fixed_point<16, 16> can represent the range [-32768, 32767.9999847]
	 *      with a fixed precision of 0.00001525878.
	 *
	 *  Note that for both the signed and unsigned versions, (I+F) must be equal to
	 *  8, 16 or 32. To add more options, add a specialization for the
	 *  detail::fixed_point_traits class below.
	 *
	 *  Addition and subtraction are implemented by casting to unsigned types,
	 *  meaning integer overflow doesn't invoke undefined behavior, but instead
	 *  always overflows correctly (whether this is a meaningful behavior is a
	 *  separate question...).
	 *
	 *  Multiplication and division are implemented by casting to a 2x wider integer type,
	 *  to increase precision and prevent overflows. Note that the result is still truncated
	 *  to the original size, i.e. multiplying two 16.16 fixed-points gives a 16.16 fixed-point
	 *  as well.
	 *
	 *  Typical usage of this class would be to make a type alias like
	 *
	 *      using fp = signed_fixed_point<16, 16>;
	 *
	 *  and then using this alias in place of other arithmetic types;
	 *
	 *  Only explicit casts from and to other standard types are allowed, to prevent
	 *  accidental conversions. For example, this will compile:
	 *
	 *      fp x(1);
	 *      fp y(3.1415f);
	 *      x = static_cast<fp>(1.618033);
	 *      float z = static_cast<float>(y);
	 *
	 *  while this won't compile:
	 *
	 *      fp x = 1;
	 *      fp y = 3.1415f;
	 *      x = 1.618033;
	 *      float z = y;
	 *
	 *  To initialize the fixed_point value directly from the internal representation type,
	 *  use the static fixed_point::from_rep() method.
	 */

	namespace detail
	{

		template <unsigned int TotalBits>
		struct fixed_point_traits;

		template <>
		struct fixed_point_traits<8>
		{
			using signed_rep_type = std::int8_t;
			using unsigned_rep_type = std::uint8_t;

			using signed_wide_type = std::int16_t;
			using unsigned_wide_type = std::uint16_t;
		};

		template <>
		struct fixed_point_traits<16>
		{
			using signed_rep_type = std::int16_t;
			using unsigned_rep_type = std::uint16_t;

			using signed_wide_type = std::int32_t;
			using unsigned_wide_type = std::uint32_t;
		};

		template <>
		struct fixed_point_traits<32>
		{
			using signed_rep_type = std::int32_t;
			using unsigned_rep_type = std::uint32_t;

			using signed_wide_type = std::int64_t;
			using unsigned_wide_type = std::uint64_t;
		};

		template <typename T, typename H>
		constexpr H clamp(T x, H min, H max)
		{
			if (x < static_cast<T>(min))
				return min;
			if (x > static_cast<T>(max))
				return max;
			return static_cast<H>(x);
		}

		template <typename FloatingPoint, typename Function, typename FixedPoint1, typename ... FixedPoint>
		auto apply_via_floating_point(Function && function, FixedPoint1 const & x1, FixedPoint const & ... xs)
		{
			static_assert((std::is_same_v<FixedPoint1, FixedPoint> && ...));
			return static_cast<FixedPoint1>(std::forward<Function>(function)(static_cast<FloatingPoint>(x1), static_cast<FloatingPoint>(xs) ...));
		}

	}

	template <unsigned int IntegerBits, unsigned int FractionalBits, bool Signed>
	struct fixed_point;

	template <unsigned int IntegerBits, unsigned int FractionalBits>
	using unsigned_fixed_point = fixed_point<IntegerBits, FractionalBits, false>;

	template <unsigned int IntegerBits, unsigned int FractionalBits>
	using signed_fixed_point = fixed_point<IntegerBits, FractionalBits, true>;

	template <unsigned int IntegerBits, unsigned int FractionalBits, bool Signed>
	struct fixed_point
	{
		static constexpr unsigned int integer_bits = IntegerBits;
		static constexpr unsigned int fractional_bits = FractionalBits;
		static constexpr unsigned int total_bits = integer_bits + fractional_bits;

		using traits = detail::fixed_point_traits<total_bits>;

		using rep_type = std::conditional_t<Signed, typename traits::signed_rep_type, typename traits::unsigned_rep_type>;
		using wide_type = std::conditional_t<Signed, typename traits::signed_wide_type, typename traits::unsigned_wide_type>;
		using unsigned_rep_type = typename traits::unsigned_rep_type;
		using unsigned_wide_type = typename traits::unsigned_wide_type;

		constexpr fixed_point() = default;
		constexpr fixed_point(fixed_point const &) = default;

		constexpr fixed_point & operator = (fixed_point const &) = default;

		template <std::integral T>
		constexpr explicit fixed_point(T value);

		template <std::floating_point T>
		constexpr explicit fixed_point(T value);

		template <unsigned int I2, unsigned int F2, bool S2>
		constexpr explicit fixed_point(fixed_point<I2, F2, S2> value);

		template <std::integral T>
		constexpr explicit operator T () const;

		template <std::floating_point T>
		constexpr explicit operator T () const;

		constexpr fixed_point & operator += (fixed_point);
		constexpr fixed_point & operator -= (fixed_point);
		constexpr fixed_point & operator *= (fixed_point);
		constexpr fixed_point & operator /= (fixed_point);

		constexpr rep_type rep() const { return rep_; }

		static constexpr fixed_point from_rep(rep_type rep);

		static constexpr fixed_point min();
		static constexpr fixed_point max();

	private:
		rep_type rep_;
	};

	template <unsigned int I, unsigned int F, bool S>
	template <std::integral T>
	constexpr fixed_point<I, F, S>::fixed_point(T value)
		: rep_(static_cast<rep_type>(value) << F)
	{}

	template <unsigned int I, unsigned int F, bool S>
	template <std::floating_point T>
	constexpr fixed_point<I, F, S>::fixed_point(T value)
		: rep_(static_cast<rep_type>(detail::clamp(std::round(value * (static_cast<rep_type>(1) << F)), min().rep(), max().rep())))
	{}

	template <unsigned int I, unsigned int F, bool S>
	template <unsigned int I2, unsigned int F2, bool S2>
	constexpr fixed_point<I, F, S>::fixed_point(fixed_point<I2, F2, S2> value)
	{
		// TODO: what to do with signs?
		if constexpr (F >= F2)
		{
			rep_ = static_cast<rep_type>(value.rep_) << (F - F2);
		}
		else
		{
			rep_ = static_cast<rep_type>(value.rep_) >> (F - F2);
		}
	}

	template <unsigned int I, unsigned int F, bool S>
	template <std::integral T>
	constexpr fixed_point<I, F, S>::operator T () const
	{
		return static_cast<T>(rep_) >> F;
	}

	template <unsigned int I, unsigned int F, bool S>
	template <std::floating_point T>
	constexpr fixed_point<I, F, S>::operator T () const
	{
		return rep_ / static_cast<T>(static_cast<rep_type>(1) << F);
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> fixed_point<I, F, S>::from_rep(fixed_point<I, F, S>::rep_type rep)
	{
		fixed_point<I, F, S> result;
		result.rep_ = rep;
		return result;
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> fixed_point<I, F, S>::min()
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		return FP::from_rep(std::numeric_limits<rep_type>::min());
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> fixed_point<I, F, S>::max()
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		return FP::from_rep(std::numeric_limits<rep_type>::max());
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> operator - (fixed_point<I, F, S> x)
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		using unsigned_rep_type = typename FP::unsigned_rep_type;
		return FP::from_rep(static_cast<rep_type>(static_cast<unsigned_rep_type>(-x.rep())));
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> operator + (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		using unsigned_rep_type = typename FP::unsigned_rep_type;
		return FP::from_rep(static_cast<rep_type>(static_cast<unsigned_rep_type>(x1.rep()) + static_cast<unsigned_rep_type>(x2.rep())));
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> operator - (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		using unsigned_rep_type = typename FP::unsigned_rep_type;
		return FP::from_rep(static_cast<rep_type>(static_cast<unsigned_rep_type>(x1.rep()) - static_cast<unsigned_rep_type>(x2.rep())));
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> operator * (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		using unsigned_wide_type = typename FP::unsigned_wide_type;
		return FP::from_rep(static_cast<rep_type>((static_cast<unsigned_wide_type>(x1.rep()) * static_cast<unsigned_wide_type>(x2.rep())) >> F));
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> operator / (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		using wide_type = typename FP::wide_type;
		return FP::from_rep(static_cast<rep_type>((static_cast<wide_type>(x1.rep()) << F) / static_cast<wide_type>(x2.rep())));
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> & fixed_point<I, F, S>::operator += (fixed_point<I, F, S> x)
	{
		(*this) = (*this) + x;
		return (*this);
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> & fixed_point<I, F, S>::operator -= (fixed_point<I, F, S> x)
	{
		(*this) = (*this) - x;
		return (*this);
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> & fixed_point<I, F, S>::operator *= (fixed_point<I, F, S> x)
	{
		(*this) = (*this) * x;
		return (*this);
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> & fixed_point<I, F, S>::operator /= (fixed_point<I, F, S> x)
	{
		(*this) = (*this) / x;
		return (*this);
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr bool operator == (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
	{
		return x1.rep() == x2.rep();
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr auto operator <=> (fixed_point<I, F, S> x1, fixed_point<I, F, S> x2)
	{
		return x1.rep() <=> x2.rep();
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> abs(fixed_point<I, F, S> x)
	{
		using FP = fixed_point<I, F, S>;
		if constexpr (S)
			return FP::from_rep(std::abs(x.rep()));
		else
			return x;
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> floor(fixed_point<I, F, S> x)
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		constexpr auto fixed_point_one = static_cast<rep_type>(1) << F;
		constexpr auto fractional_mask = fixed_point_one - static_cast<rep_type>(1);
		return FP::from_rep(x.rep() & ~fractional_mask);
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> ceil(fixed_point<I, F, S> x)
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		constexpr auto fixed_point_one = static_cast<rep_type>(1) << F;
		constexpr auto fractional_mask = fixed_point_one - static_cast<rep_type>(1);
		return FP::from_rep(((x.rep() - static_cast<rep_type>(1)) & ~fractional_mask) + fixed_point_one);
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> round(fixed_point<I, F, S> x)
	{
		if constexpr (F == 0)
		{
			return x;
		}
		else
		{
			using FP = fixed_point<I, F, S>;
			using rep_type = typename FP::rep_type;
			constexpr auto fixed_point_half = static_cast<rep_type>(1) << (F - 1);
			if ((x.rep() & fixed_point_half) == 0)
				return floor(x);
			else
				return ceil(x);
		}
	}

	template <unsigned int I, unsigned int F, bool S>
	fixed_point<I, F, S> sqrt(fixed_point<I, F, S> x)
	{
		using FP = fixed_point<I, F, S>;
		using rep_type = typename FP::rep_type;
		using wide_type = typename FP::wide_type;
		return FP::from_rep(static_cast<rep_type>(std::round(std::sqrt(static_cast<wide_type>(x.rep()) << F))));
	}

	template <unsigned int I, unsigned int F, bool S>
	constexpr fixed_point<I, F, S> pow(fixed_point<I, F, S> x, int p)
	{
		if (p < 0)
			return fixed_point<I, F, S>(1) / pow(x, -p);

		// Binary exponentiation algorithm

		fixed_point<I, F, S> result(1);
		while (p > 0)
		{
			if ((p & 1) == 1)
				result *= x;
			x *= x;
			p >>= 1;
		}

		return result;
	}

	template <unsigned int I, unsigned int F, bool S>
	fixed_point<I, F, S> sin(fixed_point<I, F, S> x)
	{
		return detail::apply_via_floating_point<double>(&std::sin, x);
	}

	template <unsigned int I, unsigned int F, bool S>
	fixed_point<I, F, S> cos(fixed_point<I, F, S> x)
	{
		return detail::apply_via_floating_point<double>(&std::cos, x);
	}

	template <unsigned int I, unsigned int F, bool S>
	fixed_point<I, F, S> exp(fixed_point<I, F, S> x)
	{
		return detail::apply_via_floating_point<double>(&std::exp, x);
	}

	template <unsigned int I, unsigned int F, bool S>
	fixed_point<I, F, S> log(fixed_point<I, F, S> x)
	{
		return detail::apply_via_floating_point<double>(&std::log, x);
	}

}
