#ifndef fixed_pt_hpp
#define fixed_pt_hpp

#include <boost/constrained_value.hpp>
#include <boost/operators.hpp>
#include <cmath> // ldexp
//#include <iostream> // DEBUG

namespace cv = boost::constrained_value;

template<typename base_type=int>
struct rnd {
  static base_type apply (base_type x, int frac_bits) {
    base_type x1 = x >> (frac_bits-1);
    if (x1 & 1)
      return (x1 + 1) >> 1;	// overflow?
    else
      return (x1 >> 1);
  }
};

template<typename int_t, typename flt_t>
inline int_t nint (flt_t x) { return (x >= 0) ? int_t (x + 0.5) : int_t (x - 0.5); }

template<typename int_t>
inline int_t shift_right (int_t x, int bits) {
  return x >> bits;
}
template<typename int_t>
inline int_t shift_left (int_t x, int bits) {
  return x << bits;
}

// positive shift is <<
template <typename T>
inline T shift(T val, int bits) {
  if (bits<0) {
    return shift_right (val, bits);
  } else { 
    return shift_left (val, bits);
  }
}

//! signed fixed pt
template<int int_bits, int frac_bits, typename base_type=int, typename error_policy=cv::throw_exception<>, typename round_policy=rnd<base_type> >
struct fixed_pt  :
  boost::ordered_euclidian_ring_operators< fixed_pt<int_bits,frac_bits,base_type,error_policy,round_policy>, base_type>,
	      boost::ordered_euclidian_ring_operators< fixed_pt<int_bits,frac_bits,base_type,error_policy,round_policy>, double>,
	      boost::ordered_euclidian_ring_operators< fixed_pt<int_bits,frac_bits,base_type,error_policy,round_policy> >,
	      boost::shiftable<fixed_pt<int_bits,frac_bits,base_type,error_policy,round_policy>, base_type>

{
  typedef fixed_pt<int_bits,frac_bits,base_type,error_policy,round_policy> self;

  static const int total_bits = int_bits + frac_bits;
  static const base_type max = ~(base_type(-1) << (total_bits-1));
  static const base_type min = (base_type(-1) << (total_bits-1));
  static const base_type int_max = ~(base_type(-1) << (int_bits-1));
  static const base_type int_min = (base_type(-1) << (int_bits-1));
  typedef typename cv::bounded_int<base_type, min, max,true,true,error_policy>::type val_t;

  val_t val;

  /* explicit */ fixed_pt (base_type x=base_type(0)) : val (x << frac_bits) {};

  template<int _int_bits, int _frac_bits, typename _base_type, typename _error_policy, typename _round_policy>
  /* explicit */ fixed_pt (fixed_pt<_int_bits, _frac_bits, _base_type, _error_policy, _round_policy>const& other) {
    val = (frac_bits >= _frac_bits) ? shift_left (base_type (other.val), frac_bits - _frac_bits) :
      round_policy::apply (base_type (other.val), _frac_bits - frac_bits);
  }

  template<int _int_bits, int _frac_bits, typename _base_type, typename _error_policy, typename _round_policy>
  self&  operator= (fixed_pt<_int_bits, _frac_bits, _base_type, _error_policy, _round_policy>const& other) {
    self tmp (other);
    *this = tmp;
    return *this;
  }

  /* explicit */ fixed_pt (double x) : val (nint<base_type> (x * (1 << frac_bits))) {};

  //operator base_type() const { return round_policy::apply (val); }

  self& operator+=(self const& x) { val += x.val; return *this; }
  self& operator-=(self const& x) { val -= x.val; return *this; }
  self& operator*=(self const& x) { 
    base_type tmp = (base_type)val * (base_type)x.val;
    val = tmp >> (frac_bits);
    return *this;
  }

  self& operator/=(self const& x) { 
    val = ((base_type)val << frac_bits) / (base_type)x.val;
    return *this; }

  // Don't do these, force conversion to fixed_pt and use above
  // self operator+=(base_type x) { val += x; return *this; }
  // self operator-=(base_type x) { val -= x; return *this; }
  // self operator*=(base_type x) { val *= x; return *this; }
  // self operator/=(base_type x) { val /= x; return *this; }

  self& operator<<=(int x) { val <<= x; return *this; }
  self& operator>>=(int x) { val >>= x; return *this; }

  bool operator==(self const& x) const { return val == x.val; }
  bool operator<(self const& x) const { return val < x.val; }

  self& operator |=(self const& x) { val |= x.val; return *this; }
  self& operator &=(self const& x) { val &= x.val; return *this; }
  self& operator ^=(self const& x) { val ^= x.val; return *this; }

  //  self operator %=(base_type x) { val %= x; return *this; } What should this do?
  self& operator ~() { self f = *this; f.val = !f.val; return f; }
  self& operator -() { self f = *this; f.val = -f.val; return f; }
  //  template<int _int_bits, int _frac_bits, typename _base_type, typename _round_policy>
  // self operator+= (fixed_pt<_int_bits, _frac_bits, _base_type, _round_policy>const& other) {
  // }

  double as_double() const { return ldexp (static_cast<double>(val), -frac_bits); }
  base_type as_int() const { return round_policy::apply (val, frac_bits); }
};


#endif