//  boost fixed_math.hpp header file  ----------------------------------------//

//  (C) Copyright Stephen Nutt 2002. Permission to copy, use, modify, sell
//  and distribute this software is granted provided this copyright
//  notice appears in all copies. This software is provided "as is" without
//  express or implied warranty, and with no claim as to its suitability for
//  any purpose.

//  See http://www.boost.org for most recent version including documentation.

//  Revision History
//   10 Sep 02  Initial version

#ifndef BOOST_FIXED_MATH_HPP
#define BOOST_FIXED_MATH_HPP

#include <boost/integer.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/ice.hpp>

namespace boost
{
  namespace type_traits
  {
    template <int i1, int i2>
    struct ice_max
    {
      BOOST_STATIC_CONSTANT (int, value = i1 > i2 ? i1 : i2);
    };
  };
};

namespace boost
{
  template <typename IntegerType>
  IntegerType FixedSqrt (IntegerType x, unsigned int fractionalBits);

  template <typename integer_type>
  struct set_sign
  {
    template <bool is_signed> struct sign {};
    template <> struct sign <true>
    {
      typedef int_t<std::numeric_limits<integer_type>::digits -1> type;
    };

    template <> struct sign <false>
    {
      typedef uint_t<std::numeric_limits<integer_type>::digits -1> type;
    };
  };

  template <typename integer_type, typename result_type>
  result_type saturate (const integer_type val)
  {
	  if (std::numeric_limits<integer_type>::is_signed && val < std::numeric_limits<result_type>::min())
		  return std::numeric_limits<result_type>::min();

	  if (val > std::numeric_limits<result_type>::max())
		  return std::numeric_limits<result_type>::max();

	  return val;
  }

  // shift
  // shifts an integer of type integer_type either left or right, optionally
  // saturating the result to the upper or lower limit of result_type.
  // The number of bits to be shifted can either be set at compile time
  // via the optional template parameter bits, or passed in the member scale
  template <typename arg_type, bool saturate, int bits = 0, typename result_type = arg_type>
  class shift
  {
	template <typename type, typename ret_type = type>
	struct simple_shift
	{
	private:
		template <bool is_exact> struct shift_detail {};
		template <> struct shift_detail <true>
		{
			template <int direction> struct shift_helper {};
			template <> struct shift_helper <0>
			{
				static inline ret_type shift (const type val)
				{ return val >> -bits; }

				static inline ret_type shift (const type val, const int scale)
				{ return val >> -scale; }
			};
			
			template <> struct shift_helper <1>
			{
				static inline ret_type shift (const type val)
				{ return val; }

				static inline ret_type shift (const type val, const int scale)
				{ return val; }
			};

			template <> struct shift_helper <2>
			{
				static inline ret_type shift (const type val)
				{ return ret_type (val) << bits; }

				static inline ret_type shift (const type val, const int scale)
				{ return ret_type (val) << scale; }
			};
		};

		template <> struct shift_detail <false>
		{
			typedef unsigned long factor_type;
			static inline factor_type factor (void)
			{ return factor_type (1) << (bits < 0 ? -bits : bits); }

			static inline factor_type factor (const int scale)
			{ return factor_type (1) << scale; }

			template <int direction> struct shift_helper {};
			template <> struct shift_helper <0>
			{
				static inline ret_type shift (const type val)
				{ return val / factor(); }

				static inline ret_type shift (const type val, const int scale)
				{ return val / factor (-scale); }
			};
			
			template <> struct shift_helper <1>
			{
				static inline ret_type shift (const type val)
				{ return val; }

				static inline ret_type shift (const type val, const int scale)
				{ return val; }
			};

			template <> struct shift_helper <2>
			{
				static inline ret_type shift (const type val)
				{ return val * factor(); }

				static inline ret_type shift (const type val, const int scale)
				{ return val * factor (scale); }
			};
		};

	public:
		static inline ret_type shift (const type value)
		{
			return shift_detail<std::numeric_limits<arg_type>::is_exact>::shift_helper <
				(bits < 0) + (bits <= 0)> (value);
		}
	};

	template <bool saturate> struct saturated_shift {};
	template <> struct saturated_shift <false>
	{
		static inline result_type bit_shift (const arg_type value)
		{ return simple_shift<arg_type, result_type>::shift (value); }
	};

	template <> struct saturated_shift <true>
	{
		static inline result_type bit_shift (const arg_type value)
		{ 
			if (val < simple_shift<arg_type, result_type, -bits> (std::numeric_limits<result_type>::min() >> scale))
				return std::numeric_limits<result_type>::min();

			if (val > (std::numeric_limits<result_type>::max() >> scale))
				return std::numeric_limits<result_type>::max();

			return result_type (val) << scale;
		}
	};

    template <bool arg_is_exact, bool result_is_exact> struct exact {};
    template <bool arg_is_exact, bool result_is_exact> struct exact {};

    // Integer to integer conversion
    template<> struct exact <true, true>
	{
      template <int direction> struct shift_helper {};
      template <> struct shift_helper <0>
      {
        static inline result_type bit_shift (const arg_type val)
        { return val >> -bits; }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return val >> -scale; }
      };

      template <> struct shift_helper <1>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return val; }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return val; }
      };

      template <> struct shift_helper <2>
      {
        static inline result_type bit_shift (const arg_type val)
  	    { return result_type (val) << bits; }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val) << scale; }
      };

      template <> struct shift_helper <3>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return boost::saturate<arg_type, result_type> (val >> -bits); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return boost::saturate<arg_type, result_type> (val >> -scale); }
      };

      template <> struct shift_helper <4>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return boost::saturate<arg_type, result_type> (val); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return boost::saturate<arg_type, result_type> (val); }
      };

      template <> struct shift_helper <5>
      {
        static inline result_type bit_shift (const arg_type val)
		{ return bit_shift (val, bits); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    {
          if (val < (std::numeric_limits<result_type>::min() >> scale))
            return std::numeric_limits<result_type>::min();

          if (val > (std::numeric_limits<result_type>::max() >> scale))
            return std::numeric_limits<result_type>::max();

          return result_type (val) << scale;
	    }
      };
	};

    // Integer to float conversion
    template<> struct exact <true, false>
	{
      template <int direction> struct shift_helper {};
      template <> struct shift_helper <0>
      {
        static inline result_type bit_shift (const arg_type val)
        { return result_type (val) / (((unsigned long) 1) << -bits); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val) / (((unsigned long) 1) << -scale); }
      };

      template <> struct shift_helper <1>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return result_type (val); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val); }
      };

      template <> struct shift_helper <2>
      {
        static inline result_type bit_shift (const arg_type val)
  	    { return result_type (val) * (((unsigned long) 1) << bits); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val) * (((unsigned long) 1) << scale); }
      };

      template <> struct shift_helper <3>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return boost::saturate<arg_type, result_type> (val) / (1 << -bits); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return boost::saturate<arg_type, result_type> (val) / (1 << -scale); }
      };

      template <> struct shift_helper <4>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return boost::saturate<arg_type, result_type> (val); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return boost::saturate<arg_type, result_type> (val); }
      };

      template <> struct shift_helper <5>
      {
        static inline result_type bit_shift (const arg_type val)
		{ return bit_shift (val, bits); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    {
          if (val < (std::numeric_limits<result_type>::min() / (((unsigned long) 1) << scale)))
            return std::numeric_limits<result_type>::min();

          if (val > (std::numeric_limits<result_type>::max() / (((unsigned long) 1) << scale)))
            return std::numeric_limits<result_type>::max();

          return shift_helper<2>::bit_shift (val, scale);
	    }
      };
	};

    // Float to integer conversion
    template<> struct exact <false, true>
	{
      template <int direction> struct shift_helper {};
      template <> struct shift_helper <0>
      {
        static inline result_type bit_shift (const arg_type val)
        { return result_type (val / (((unsigned long) 1) << -bits)); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val / (((unsigned long) 1) << -scale)); }
      };

      template <> struct shift_helper <1>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return result_type (val); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val); }
      };

      template <> struct shift_helper <2>
      {
        static inline result_type bit_shift (const arg_type val)
  	    { return result_type (val * (((unsigned long) 1) << bits)); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val * (((unsigned long) 1) << scale)); }
      };

      template <> struct shift_helper <3>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return boost::saturate<arg_type, result_type> (val / (1 << -bits)); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return boost::saturate<arg_type, result_type> (val / (1 << -scale)); }
      };

      template <> struct shift_helper <4>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return boost::saturate<arg_type, result_type> (val); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return boost::saturate<arg_type, result_type> (val); }
      };

      template <> struct shift_helper <5>
      {
        static inline result_type bit_shift (const arg_type val)
		{ return bit_shift (val, bits); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    {
          if (val < (std::numeric_limits<result_type>::min() >> scale))
            return std::numeric_limits<result_type>::min();

          if (val > (std::numeric_limits<result_type>::max() >> scale))
            return std::numeric_limits<result_type>::max();

          return shift_helper<2>::bit_shift (val, scale);
	    }
      };
	};

    // Float to float conversion
    template<> struct exact <false, false>
	{
      template <int direction> struct shift_helper {};
      template <> struct shift_helper <0>
      {
        static inline result_type bit_shift (const arg_type val)
        { return result_type (val / (((unsigned long) 1) << -bits)); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val / (((unsigned long) 1) << -scale)); }
      };

      template <> struct shift_helper <1>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return result_type (val); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val); }
      };

      template <> struct shift_helper <2>
      {
        static inline result_type bit_shift (const arg_type val)
  	    { return result_type (val * (((unsigned long) 1) << bits)); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return result_type (val * (((unsigned long) 1) << scale)); }
      };

      template <> struct shift_helper <3>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return boost::saturate<arg_type, result_type> (val / (1 << -bits)); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return boost::saturate<arg_type, result_type> (val / (1 << -scale)); }
      };

      template <> struct shift_helper <4>
      {
        static inline result_type bit_shift (const arg_type val)
	    { return boost::saturate<arg_type, result_type> (val); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    { return boost::saturate<arg_type, result_type> (val); }
      };

      template <> struct shift_helper <5>
      {
        static inline result_type bit_shift (const arg_type val)
		{ return bit_shift (val, bits); }

        static inline result_type bit_shift (const arg_type val, const int scale)
	    {
          if (val < (std::numeric_limits<result_type>::min() >> scale))
            return std::numeric_limits<result_type>::min();

          if (val > (std::numeric_limits<result_type>::max() >> scale))
            return std::numeric_limits<result_type>::max();

          return shift_helper<2>::bit_shift (val, scale);
	    }
      };
	};

	typedef exact<
		std::numeric_limits<arg_type>::is_exact,
		std::numeric_limits<result_type>::is_exact> exact_type;
	public:
      static inline result_type bit_shift (const arg_type val)
	  {
		  return exact_type::shift_helper<
				(0 < bits) +
				(1 <= bits) +
				(saturate * 3)
			  >::bit_shift (val);
	  }

      static inline result_type bit_shift (const arg_type val, const int scale)
	  {
		  if (scale < 0)
			  return exact_type::shift_helper<saturate * 3>::bit_shift (val, scale);

		  if (scale == 0)
			  return exact_type::shift_helper<1 + saturate * 3>::bit_shift (val, scale);

		  return exact_type::shift_helper<2 + saturate * 3>::bit_shift (val, scale);
	  }
  };

  template <typename IntegerType1, int FractionalBits1,
            typename IntegerType2, int FractionalBits2>
  struct fixed_math_traits
  {
    typedef boost::type_traits::ice_or<std::numeric_limits<IntegerType1>::is_signed, std::numeric_limits<IntegerType2>::is_signed> _is_signed;
    BOOST_STATIC_CONSTANT (int, digits1 = std::numeric_limits<IntegerType1>::digits);
    BOOST_STATIC_CONSTANT (int, digits2 = std::numeric_limits<IntegerType2>::digits);
    BOOST_STATIC_CONSTANT (int, bit_count1 = digits1 + std::numeric_limits<IntegerType1>::is_signed);
    BOOST_STATIC_CONSTANT (int, bit_count2 = digits2 + std::numeric_limits<IntegerType2>::is_signed);
    BOOST_STATIC_CONSTANT (int, is_signed = _is_signed::value);
    BOOST_STATIC_ASSERT (FractionalBits1 <= digits1);
    BOOST_STATIC_ASSERT (FractionalBits2 <= digits2);

    typedef type_traits::ice_max<bit_count1, bit_count2> _digits;
    BOOST_STATIC_CONSTANT (int, digits = _digits::value - is_signed);

//    template <bool is_signed> struct result_type {};
//    template <> struct result_type <true>  { typedef  int_t<digits> type; };
//    template <> struct result_type <false> { typedef uint_t<digits> type; };

    struct result
    {
      typedef set_sign<uint_t<digits>::fast>::sign<bool (is_signed)>::type::least least;
      typedef set_sign<uint_t<digits>::fast>::sign<bool (is_signed)>::type::fast fast;
      typedef type_traits::ice_max<bit_count1 - FractionalBits1, bit_count2 - FractionalBits2> max_integral_bits;
      BOOST_STATIC_CONSTANT (int, fractional_bits = std::numeric_limits<least>::digits + is_signed - max_integral_bits::value);
      BOOST_STATIC_CONSTANT (int, is_signed = std::numeric_limits<fast>::is_signed);

      fast value;
      inline result (const fast& val) : value (val) {}
    };
  };


	template <typename IntegerType1, typename IntegerType2, bool saturate, typename ResultType>
	struct saturated_math
	{
		template<bool sat>
		struct saturated
		{};

		template<>
		struct saturated <false>
		{
			static inline ResultType add (const IntegerType1 augend, const IntegerType2 addend)
			{
				return ResultType (augend) + addend;
			}

			static inline ResultType subtract (const IntegerType1 minuend, const IntegerType2 subtrahend)
			{
				return ResultType (minuend) - subtrahend;
			}

			static inline ResultType multiply (const IntegerType1 multiplier, const IntegerType2 multiplicand)
			{
				return ResultType (multiplier) * multiplicand;
			}

			static inline ResultType divide  (const IntegerType1 dividend, const IntegerType2 divisor)
			{
				return ResultType (dividend) / divisor;
			}
		};

		template<>
		struct saturated <true>
		{
			static inline ResultType add (const IntegerType1 augend, const IntegerType2 addend)
			{
				const ResultType result = saturated<false>::add (augend, addend);
				if (augend < 0 && addend < 0 && result >= 0)
					return std::numeric_limits<ResultType>::min();

				if (augend > 0 && addend > 0 && result <= 0)
					return std::numeric_limits<ResultType>::max();

				return result;
			}

			static inline ResultType subtract (const IntegerType1 minuend, const IntegerType2 subtrahend)
			{
				return add (minuend, -subtrahend);
			}

			static inline ResultType multiply (const IntegerType1 multiplier, const IntegerType2 multiplicand)
			{
				const ResultType result = saturated<false>::multiply (multiplier, multiplicand);
				if (result / multiplicand == multiplier)
					return result;

				if ((multiplier < 0) ^ (multiplicand < 0))
					return std::numeric_limits<ResultType>::min();

				return std::numeric_limits<ResultType>::max();
			}

			static inline ResultType divide  (const IntegerType1 dividend, const IntegerType2 divisor)
			{
				return ResultType (dividend) / divisor;
			}
		};

		static inline ResultType add (const IntegerType1 augend, const IntegerType2 addend)
		{
			return saturated<saturate>::add (augend, addend);
		}

		static inline ResultType subtract (const IntegerType1 minuend, const IntegerType2 subtrahend)
		{
			return saturated<saturate>::subtract (minuend, subtrahend);
		}

		static inline ResultType multiply (const IntegerType1 multiplier, const IntegerType2 multiplicand)
		{
			return saturated<saturate>::multiply (multiplier, multiplicand);
		}

		static inline ResultType divide  (const IntegerType1 dividend, const IntegerType2 divisor)
		{
			return saturated<saturate>::divide (dividend, divisor);
		}
	};

	template <typename IntegerType1, int FractionalBits1,
			  typename IntegerType2, int FractionalBits2,
			  bool saturate = false,
			  class fixed_traits = fixed_math_traits <IntegerType1, FractionalBits1, IntegerType2, FractionalBits2> >
	struct fixed_math
	{
		typedef fixed_traits traits;
		typedef traits::result result;
		typedef result::fast fast;
		typedef set_sign<fast>::sign<std::numeric_limits<IntegerType1>::is_signed>::type::fast Type1;
		typedef set_sign<fast>::sign<std::numeric_limits<IntegerType2>::is_signed>::type::fast Type2;
		typedef saturated_math<Type1, Type2, saturate, fast> saturated;
		BOOST_STATIC_CONSTANT (int, fractional_bits = result::fractional_bits);
		BOOST_STATIC_CONSTANT (int, is_signed = traits::is_signed);
		BOOST_STATIC_CONSTANT (int, digits = traits::digits);

	protected:

		static inline Type1 normailse_type1 (Type1 val)
		{
			return shift<Type1, saturate, fractional_bits - FractionalBits1>::bit_shift (val);
		}

		static inline Type2 normailse_type2 (Type2 val)
		{
			return shift<Type1, saturate, fractional_bits - FractionalBits2>::bit_shift (val);
		}

		static fast full_multiple (const IntegerType1 multiplier, const IntegerType2 multiplicand)
		{
			fast prod1 = multiplier * (multiplicand >> FractionalBits2);
			if (FractionalBits1 > fractional_bits)
				prod1 >>= FractionalBits1 - fractional_bits;
			else
				prod1 <<= fractional_bits - FractionalBits1;

			fast prod2 = (multiplier >> FractionalBits1) * (multiplicand & ((fast (1) << FractionalBits2) -1));
			if (FractionalBits2 > fractional_bits)
				prod2 >>= FractionalBits2 - fractional_bits;
			else
				prod2 <<= fractional_bits - FractionalBits2;

			fast prod3 = (multiplier & ((fast (1) << FractionalBits1) -1)) * (multiplicand & ((fast (1) << FractionalBits2) -1));
			prod3 >>= FractionalBits1 + FractionalBits2 - fractional_bits;
			return prod1 + prod2 + prod3;
		}

		static fast full_divide (const IntegerType1 dividend, const IntegerType2 divisor)
		{
			fast quot = dividend / divisor;
			const fast rem = dividend - quot * divisor;
			if (fractional_bits > FractionalBits1 - FractionalBits2)
				quot <<= (fractional_bits - FractionalBits1 + FractionalBits2);
			else
				quot >>= (FractionalBits1 - FractionalBits2 - fractional_bits);

			return quot + (rem << (fractional_bits + FractionalBits2 - FractionalBits1)) / divisor;
		}

	public:
		static inline result add (const IntegerType1 augend, const IntegerType2 addend)
		{
			return saturated::add (normailse_type1 (augend), normailse_type2 (addend));
		}

		static inline result subtract (const IntegerType1 minuend, const IntegerType2 subtrahend)
		{
			return saturated::subtract (normailse_type1 (minuend), normailse_type2 (subtrahend));
		}

		static inline result multiply (const IntegerType1 multiplier, const IntegerType2 multiplicand)
		{
			BOOST_STATIC_CONSTANT (int, mshift = FractionalBits1 + FractionalBits2 - fractional_bits);
			BOOST_STATIC_ASSERT (mshift >= 0);
			if (std::numeric_limits<unsigned int>::digits >= traits::digits + mshift + traits::is_signed)
				if (traits::is_signed)
					return (static_cast<signed int> (multiplier) * multiplicand) >> mshift;
				else
					return (static_cast<unsigned int> (multiplier) * multiplicand) >> mshift;

			if (std::numeric_limits<unsigned long>::digits >= traits::digits + mshift + traits::is_signed)
				if (traits::is_signed)
					return (static_cast<signed long> (multiplier) * multiplicand) >> mshift;
				else
					return (static_cast<unsigned long> (multiplier) * multiplicand) >> mshift;

#ifndef BOOST_NO_INT64_T
			if (std::numeric_limits<uint64_t>::digits >= traits::digits + mshift + traits::is_signed)
				if (traits::is_signed)
					return (static_cast<int64_t> (multiplier) * multiplicand) >> mshift;
				else
					return (static_cast<uint64_t> (multiplier) * multiplicand) >> mshift;
#endif

			return full_multiple (multiplier, multiplicand);
		}

		static inline result divide  (const IntegerType1 dividend, const IntegerType2 divisor)
		{
			typedef type_traits::ice_max <0, FractionalBits2 - FractionalBits1 + fractional_bits > _dshift1;
			typedef type_traits::ice_max <0, FractionalBits2 - FractionalBits1 - fractional_bits > _dshift2;
			BOOST_STATIC_CONSTANT (int, dshift1 = _dshift1::value);
			BOOST_STATIC_CONSTANT (int, dshift2 = _dshift1::value);
			if (std::numeric_limits<unsigned int>::digits >= traits::bit_count1 + dshift1)
				if (traits::is_signed)
					return ((static_cast<signed int> (dividend) << dshift1) / divisor) >> dshift2;
				else
					return ((static_cast<unsigned int> (dividend) << dshift1) / divisor) >> dshift2;

			if (std::numeric_limits<unsigned long>::digits >= traits::bit_count1 + dshift1)
				if (traits::is_signed)
					return ((static_cast<signed long> (dividend) << dshift1) / divisor) >> dshift2;
				else
					return ((static_cast<unsigned long> (dividend) << dshift1) / divisor) >> dshift2;

#ifndef BOOST_NO_INT64_T
			if (std::numeric_limits<uint64_t>::digits >= traits::bit_count1 + dshift1)
				if (traits::is_signed)
					return ((static_cast<int64_t> (dividend) << dshift1) / divisor) >> dshift2;
				else
					return ((static_cast<uint64_t> (dividend) << dshift1) / divisor) >> dshift2;
#endif

//			BOOST_STATIC_ASSERT ((std::numeric_limits<uint64_t>::digits >= traits::bit_count1 + dshift1) || (FractionalBits2 > 0));
//			BOOST_STATIC_ASSERT ((std::numeric_limits<uint64_t>::digits >= traits::bit_count1 + dshift1) || (fractional_bits + FractionalBits2 > FractionalBits1));
			return full_divide (dividend, divisor);
		}
	};
};

#endif  // BOOST_FIXED_MATH_HPP