#ifndef run_time_fixed_pt_hpp
#define run_time_fixed_pt_hpp

#include <boost/constrained_value.hpp>
#include <boost/operators.hpp>
#include <cmath> // ldexp
#include <iostream>
#include <boost/serialization/nvp.hpp>
#include <limits>

namespace cv = boost::constrained_value;

namespace {
template<typename base_type=int>
struct rnd {
  static base_type apply (base_type x, int frac_bits, base_type max) {
    base_type x1 = x >> (frac_bits-1);
    if (x1 == max)
      return x1 >> 1;
    else
      return (x1 + 1) >> 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);
  }
}

}

namespace boost { namespace constrained_value {
template<typename V, typename C, typename E>
cv::constrained<V,C,E> operator- (cv::constrained<V,C,E> c) {
  V tmp (c.value());
  c = -tmp;
  return c;
}
template<typename V, typename C, typename E>
cv::constrained<V,C,E> operator~ (cv::constrained<V,C,E> c) {
  V tmp (c.value());
  c = ~tmp;
  return c;
}
  }}

//! signed fixed pt
template<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<base_type,error_policy,round_policy>, base_type>,
  boost::ordered_euclidian_ring_operators< fixed_pt<base_type,error_policy,round_policy>, double>,
  boost::ordered_euclidian_ring_operators< fixed_pt<base_type,error_policy,round_policy> >,
  boost::shiftable<fixed_pt<base_type,error_policy,round_policy>, base_type>

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

  typedef base_type base_t;	// export

  typedef typename cv::bounded<base_type,base_type,base_type,error_policy>::type val_t;
  
  int int_bits;
  int frac_bits;
  val_t val;
  static const bool is_signed = std::numeric_limits<base_type>::is_signed;

  int total_bits() const { return int_bits + frac_bits; }
  base_type max() const { return is_signed ? (~(base_type(-1) << (total_bits()-1))) : (~(base_type (-1) << total_bits())) ; }
  base_type min() const { return is_signed ? ((base_type(-1) << (total_bits()-1))) : 0 ; }
  typedef typename cv::bounded<base_type,base_type,base_type,error_policy>::type::constraint_type constraint_type;

  /* explicit */ fixed_pt () {}

  /* explicit */ fixed_pt (int _int_bits, int _frac_bits, val_t v) :
    int_bits (_int_bits),
    frac_bits (_frac_bits),
    val (v) {}

  /* explicit */ fixed_pt (int _int_bits, int _frac_bits, base_type x, bool scale=true) :
    int_bits (_int_bits),
    frac_bits (_frac_bits),
    val (scale ? (shiftup_or_rnddn (x, frac_bits)) : x, constraint_type (min(), max())) {}

  /* explicit */ fixed_pt (int _int_bits, int _frac_bits) :
    int_bits (_int_bits),
    frac_bits (_frac_bits),
    val (constraint_type (min(), max()))
    {}

  template<typename _base_type, typename _error_policy, typename _round_policy>
  /* explicit */ fixed_pt (int _int_bits, int _frac_bits, fixed_pt<_base_type, _error_policy, _round_policy>const& other) :
    int_bits (_int_bits),
    frac_bits (_frac_bits),
    val (shiftup_or_rnddn (base_type (other.val), frac_bits - other.frac_bits),
	 constraint_type (min(), max()))
  {}

  self&  operator= (self const& other) {
    val = (shiftup_or_rnddn (base_type (other.val), frac_bits - other.frac_bits));
    return *this;
  }

  template<typename _base_type, typename _error_policy, typename _round_policy>
  self&  operator= (fixed_pt<_base_type, _error_policy, _round_policy>const& other) {
    val = (shiftup_or_rnddn (base_type (other.val), frac_bits - other.frac_bits));
    return *this;
  }

  /* explicit */ fixed_pt (int _int_bits, int _frac_bits, double x) :
    int_bits (_int_bits),
    frac_bits (_frac_bits),
    val (nint<base_type> (x * pow(2, frac_bits)), constraint_type (min(), max())){};


  // void set_bits (int _int_bits, int _frac_bits) {
  //   base_type old_val = val.value();
  //   int_bits = _int_bits;
  //   frac_bits = _frac_bits;
  //   val = val_t (nint<base_type> (old_val * (1 << frac_bits)), constraint_type (min(), max()));
  // }

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

  //! convert other fixed_pt to our scaling
  base_type convert (self const& x) const {
    return shiftup_or_rnddn (base_type (x.val), frac_bits - x.frac_bits);
  }

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

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

  self from_base (base_type x) const {
    return self (int_bits, frac_bits, x);
  }

  self from_double (double x) const {
    return self (int_bits, frac_bits, x);
  }

  // Don't do these, force conversion to fixed_pt and use above
  self& operator+=(base_type x) { *this += from_base (x) ; return *this; }
  self& operator-=(base_type x) { *this -= from_base (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 == convert (x); }
  bool operator<(self const& x) const { return val < convert (x); }

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

  //  self operator %=(base_type x) { val %= x; return *this; } What should this do?
  self operator ~() const { self f = *this; f.val = ~f.val; return f; }
  self operator -() const { 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_base() const { return shiftup_or_rnddn (val, -frac_bits); }

  base_type shiftup_or_rnddn (base_type x, int bits) const {
    if (bits > 0)
      return shift_left (x, bits);
    else if (bits < 0)
      return round_policy::apply (x, -bits, max());
    else
      return x;
  }

  friend class boost::serialization::access;

  template<typename Archive>
  void serialize (Archive & ar, const unsigned int /* file_verison */) {
    ar & boost::serialization::make_nvp ("int_bits", int_bits);
    ar & boost::serialization::make_nvp ("frac_bits", frac_bits);
    ar & boost::serialization::make_nvp ("val", val); // this isn't useful till val_t has serialization
  }

};

template<typename base_type, typename error_policy, typename round_policy>
std::ostream& operator<< (std::ostream& os, fixed_pt<base_type,error_policy,round_policy> const& fp) {
  os << '[' << fp.val << ',' << fp.int_bits << ',' << fp.frac_bits << ']';
  return os;
}

#endif