///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2013 - 2016.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)
//

// This file contains fixed_point details.

#ifndef FIXED_POINT_DETAIL_2015_05_23_HPP_
  #define FIXED_POINT_DETAIL_2015_05_23_HPP_

  #include <array>
  #include <cstdint>

  #include <boost/config.hpp>
  #include <boost/fixed_point/detail/fixed_point_detail_cstdfloat.hpp>

  #if (   (!defined(BOOST_FIXED_POINT_FLOAT32_C))   \
       && (!defined(BOOST_FIXED_POINT_FLOAT64_C))   \
       && (!defined(BOOST_FIXED_POINT_FLOAT80_C))   \
       && (!defined(BOOST_FIXED_POINT_FLOAT128_C)))
    #error Configuration error: Sorry, fixed_point can not detect any IEEE-754 built-in floating-point types!
  #endif

  // Do not produce Doxygen indexing of items in namespace detail unless specifically required.
  // The section between \cond and \endcond can be included by adding its section label DETAIL
  // to the ENABLED_SECTIONS configuration option.
  // If the section label is omitted, the section will be excluded from processing unconditionally.

  //! \cond DETAIL

  namespace boost { namespace fixed_point { namespace detail {

  template<typename UnsignedIntegralType>
  UnsignedIntegralType left_shift_helper(const UnsignedIntegralType& u, const int shift_count)
  {
    #if !defined(BOOST_FIXED_POINT_ENABLE_GMP_BACKENDS)

      static_assert(    (std::numeric_limits<UnsignedIntegralType>::is_integer == true)
                     && (std::numeric_limits<UnsignedIntegralType>::is_signed  == false),
                     "The UnsignedIntegralType for left shift must be an unsigned integral type.");

    #endif

    return ((shift_count > 0) ? UnsignedIntegralType(u << +shift_count)
                              : UnsignedIntegralType(u >> -shift_count));
  }

  template<typename UnsignedIntegralType>
  UnsignedIntegralType right_shift_helper(const UnsignedIntegralType& u, const int shift_count)
  {
    #if !defined(BOOST_FIXED_POINT_ENABLE_GMP_BACKENDS)

      static_assert(    (std::numeric_limits<UnsignedIntegralType>::is_integer == true)
                     && (std::numeric_limits<UnsignedIntegralType>::is_signed  == false),
                     "The UnsignedIntegralType for right shift must be an unsigned integral type.");

    #endif

    return ((shift_count > 0) ? UnsignedIntegralType(u >> +shift_count)
                              : UnsignedIntegralType(u << -shift_count));
  }

  #if !defined(BOOST_FIXED_POINT_DISABLE_MULTIPRECISION)

    template<const std::uint32_t BitCount,
             typename EnableType = void>
    struct integer_type_helper
    {
    private:
      static BOOST_CONSTEXPR_OR_CONST std::uint32_t bit_count_nearest_power_of_two =
        (BitCount <= std::uint32_t(UINT32_C(1) <<  7)) ? std::uint32_t(UINT32_C(1) <<  7) :
        (BitCount <= std::uint32_t(UINT32_C(1) <<  8)) ? std::uint32_t(UINT32_C(1) <<  8) :
        (BitCount <= std::uint32_t(UINT32_C(1) <<  9)) ? std::uint32_t(UINT32_C(1) <<  9) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 10)) ? std::uint32_t(UINT32_C(1) << 10) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 11)) ? std::uint32_t(UINT32_C(1) << 11) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 12)) ? std::uint32_t(UINT32_C(1) << 12) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 13)) ? std::uint32_t(UINT32_C(1) << 13) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 14)) ? std::uint32_t(UINT32_C(1) << 14) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 15)) ? std::uint32_t(UINT32_C(1) << 15) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 16)) ? std::uint32_t(UINT32_C(1) << 16) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 17)) ? std::uint32_t(UINT32_C(1) << 17) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 18)) ? std::uint32_t(UINT32_C(1) << 18) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 19)) ? std::uint32_t(UINT32_C(1) << 19) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 20)) ? std::uint32_t(UINT32_C(1) << 20) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 21)) ? std::uint32_t(UINT32_C(1) << 21) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 22)) ? std::uint32_t(UINT32_C(1) << 22) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 23)) ? std::uint32_t(UINT32_C(1) << 23) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 24)) ? std::uint32_t(UINT32_C(1) << 24) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 25)) ? std::uint32_t(UINT32_C(1) << 25) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 26)) ? std::uint32_t(UINT32_C(1) << 26) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 27)) ? std::uint32_t(UINT32_C(1) << 27) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 28)) ? std::uint32_t(UINT32_C(1) << 28) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 29)) ? std::uint32_t(UINT32_C(1) << 29) :
        (BitCount <= std::uint32_t(UINT32_C(1) << 30)) ? std::uint32_t(UINT32_C(1) << 30) :
                    (std::uint32_t(UINT32_C(1) << 31));

      #if defined(BOOST_FIXED_POINT_ENABLE_GMP_BACKENDS)

        typedef boost::multiprecision::gmp_int signed_integral_backend_type;
        typedef boost::multiprecision::gmp_int unsigned_integral_backend_type;

      #else

      typedef boost::multiprecision::cpp_int_backend<unsigned(bit_count_nearest_power_of_two),
                                                     unsigned(bit_count_nearest_power_of_two),
                                                     boost::multiprecision::signed_magnitude,
                                                     boost::multiprecision::unchecked,
                                                     void>
      signed_integral_backend_type;

      typedef boost::multiprecision::cpp_int_backend<unsigned(bit_count_nearest_power_of_two),
                                                     unsigned(bit_count_nearest_power_of_two),
                                                     boost::multiprecision::unsigned_magnitude,
                                                     boost::multiprecision::unchecked,
                                                     void>
      unsigned_integral_backend_type;

      #endif

    public:
      typedef boost::multiprecision::number<signed_integral_backend_type,
                                            boost::multiprecision::et_off> exact_signed_type;

      typedef boost::multiprecision::number<unsigned_integral_backend_type,
                                            boost::multiprecision::et_off> exact_unsigned_type;
    };

  #endif // !BOOST_FIXED_POINT_DISABLE_MULTIPRECISION

  #if defined(BOOST_FIXED_POINT_DISABLE_MULTIPRECISION)

    template<const std::uint32_t BitCount,
             typename EnableType = void>
    struct integer_type_helper
    {
    private:
      typedef signed long long   exact_signed_type;
      typedef unsigned long long exact_unsigned_type;
    };

  #endif // BOOST_FIXED_POINT_DISABLE_MULTIPRECISION

  template<const std::uint32_t BitCount>
  struct integer_type_helper<BitCount,
                             typename std::enable_if<(BitCount <= UINT32_C(8))>::type>
  {
    typedef std::int8_t  exact_signed_type;
    typedef std::uint8_t exact_unsigned_type;
  };

  template<const std::uint32_t BitCount>
  struct integer_type_helper<BitCount,
                             typename std::enable_if<   (BitCount >  UINT32_C( 8))
                                                     && (BitCount <= UINT32_C(16))>::type>
  {
    typedef std::int16_t  exact_signed_type;
    typedef std::uint16_t exact_unsigned_type;
  };

  template<const std::uint32_t BitCount>
  struct integer_type_helper<BitCount,
                             typename std::enable_if<   (BitCount >  UINT32_C(16))
                                                     && (BitCount <= UINT32_C(32))>::type>
  {
    typedef std::int32_t  exact_signed_type;
    typedef std::uint32_t exact_unsigned_type;
  };

  template<const std::uint32_t BitCount>
  struct integer_type_helper<BitCount,
                             typename std::enable_if<   (BitCount >  UINT32_C(32))
                                                     && (BitCount <= UINT32_C(64))>::type>
  {
    typedef std::int64_t  exact_signed_type;
    typedef std::uint64_t exact_unsigned_type;
  };

  #if !defined(BOOST_FIXED_POINT_DISABLE_MULTIPRECISION)

    template<const std::uint32_t BitCount,
             typename EnableType = void>
    struct float_type_helper
    {
    private:

      #if defined(BOOST_FIXED_POINT_ENABLE_GMP_BACKENDS)

        typedef boost::multiprecision::gmp_float<unsigned((static_cast<long long>(static_cast<long long>(BitCount) * 301LL) + 500LL) / 1000LL)> floating_point_backend_type;

      #else

        typedef boost::multiprecision::backends::cpp_bin_float<unsigned(BitCount),
                                                               boost::multiprecision::backends::digit_base_2>
        floating_point_backend_type;

      #endif

    public:
      typedef boost::multiprecision::number<floating_point_backend_type,
                                            boost::multiprecision::et_off> exact_float_type;
    };

  #endif // !BOOST_FIXED_POINT_DISABLE_MULTIPRECISION

  #if defined(BOOST_FIXED_POINT_DISABLE_MULTIPRECISION)

    template<const std::uint32_t BitCount,
             typename EnableType = void>
    struct float_type_helper
    {
      // Here multiprecision is disabled. We have no choice
      // other than using built-in long double here
      // (even if built-in long double does not have
      // enough bits).
      typedef long double exact_float_type;
    };

  #endif

  #if defined(BOOST_FIXED_POINT_FLOAT32_C)

    template<const std::uint32_t BitCount>
    struct float_type_helper<BitCount,
                             typename std::enable_if<(BitCount <= UINT32_C(24))>::type>
    {
      typedef boost::fixed_point::detail::float32_t exact_float_type;
    };

  #endif // BOOST_FIXED_POINT_FLOAT32_C

  #if defined(BOOST_FIXED_POINT_FLOAT64_C)

    // For 64-bit float, we have two cases.
    //
    // * !float32_t : float64_t covers bits  0...53
    // *  float32_t : float64_t covers bits 24...53

    template<const std::uint32_t BitCount>
    struct float_type_helper<BitCount,
    #if defined(BOOST_FIXED_POINT_FLOAT32_C)
                             typename std::enable_if<   (BitCount >  UINT32_C(24))
                                                     && (BitCount <= UINT32_C(53))>::type>
    #else
                             typename std::enable_if<   (BitCount <= UINT32_C(53))>::type>
    #endif
    {
      typedef boost::fixed_point::detail::float64_t exact_float_type;
    };

  #endif // BOOST_FIXED_POINT_FLOAT64_C

  #if defined(BOOST_FIXED_POINT_FLOAT80_C)

    // For 80-bit float, we have four cases (2^2 = 4). There are
    // two redundant cases.
    //
    // * !float32_t && !float64_t : float80_t covers bits  0...64
    // *  float32_t && !float64_t : float80_t covers bits 24...64
    // *  float32_t &&  float64_t : float80_t covers bits 53...64
    // * !float32_t &&  float64_t : float80_t covers bits 53...64

    template<const std::uint32_t BitCount>
    struct float_type_helper<BitCount,
    #if   (!defined(BOOST_FIXED_POINT_FLOAT32_C) && !defined(BOOST_FIXED_POINT_FLOAT64_C))
                             typename std::enable_if<   (BitCount <= UINT32_C(64))>::type>
    #elif ( defined(BOOST_FIXED_POINT_FLOAT32_C) && !defined(BOOST_FIXED_POINT_FLOAT64_C))
                             typename std::enable_if<   (BitCount >  UINT32_C(24))
                                                     && (BitCount <= UINT32_C(64))>::type>
    #else
                             typename std::enable_if<   (BitCount >  UINT32_C(53))
                                                     && (BitCount <= UINT32_C(64))>::type>
    #endif
    {
      typedef boost::fixed_point::detail::float80_t exact_float_type;
    };

  #endif // BOOST_FIXED_POINT_FLOAT80_C

  #if defined(BOOST_FIXED_POINT_FLOAT128_C)

    // For 128-bit float, we have eight cases (2^3 = 8). There are two groups
    // of redundancies. These contain two and four redundant results, respectively.
    //
    // * !float32_t && !float64_t && !float80_t : float128_t covers bits  0...113
    // *  float32_t && !float64_t && !float80_t : float128_t covers bits 24...113
    // * !float32_t &&  float64_t && !float80_t : float128_t covers bits 53...113
    // *  float32_t &&  float64_t && !float80_t : float128_t covers bits 53...113
    // * !float32_t && !float64_t &&  float80_t : float128_t covers bits 64...113
    // *  float32_t && !float64_t &&  float80_t : float128_t covers bits 64...113
    // * !float32_t &&  float64_t &&  float80_t : float128_t covers bits 64...113
    // *  float32_t &&  float64_t &&  float80_t : float128_t covers bits 64...113

    template<const std::uint32_t BitCount>
    struct float_type_helper<BitCount,
    #if   (!defined(BOOST_FIXED_POINT_FLOAT32_C) && !defined(BOOST_FIXED_POINT_FLOAT64_C) && !defined(BOOST_FIXED_POINT_FLOAT80_C))
                             typename std::enable_if<   (BitCount <= UINT32_C(113))>::type>
    #elif ( defined(BOOST_FIXED_POINT_FLOAT32_C) && !defined(BOOST_FIXED_POINT_FLOAT64_C) && !defined(BOOST_FIXED_POINT_FLOAT80_C))
                             typename std::enable_if<   (BitCount >  UINT32_C( 24))
                                                     && (BitCount <= UINT32_C(113))>::type>
    #elif ( defined(BOOST_FIXED_POINT_FLOAT64_C) && !defined(BOOST_FIXED_POINT_FLOAT80_C))
                             typename std::enable_if<   (BitCount >  UINT32_C( 53))
                                                     && (BitCount <= UINT32_C(113))>::type>
    #else
                             typename std::enable_if<   (BitCount >  UINT32_C( 64))
                                                     && (BitCount <= UINT32_C(113))>::type>
    #endif
    {
      typedef boost::fixed_point::detail::float128_t exact_float_type;
    };

  #endif // BOOST_FIXED_POINT_FLOAT128_C

  #if !defined(BOOST_FIXED_POINT_DISABLE_MULTIPRECISION)

    template<typename UnsignedIntegralType,
             typename FloatingPointType,
             typename EnableType = void>
    struct conversion_helper
    {
      static void convert_floating_point_to_unsigned_integer(const FloatingPointType& floating_point_source,
                                                             UnsignedIntegralType& unsigned_destination)
      {
        unsigned_destination = floating_point_source.template convert_to<UnsignedIntegralType>();
      }
    };

    template<typename UnsignedIntegralType,
             typename FloatingPointType>
    struct conversion_helper<UnsignedIntegralType,
                             FloatingPointType,
                             typename std::enable_if<std::is_floating_point<FloatingPointType>::value>::type>
    {
      static void convert_floating_point_to_unsigned_integer(const FloatingPointType& floating_point_source,
                                                             UnsignedIntegralType& unsigned_destination)
      {
        unsigned_destination = static_cast<UnsignedIntegralType>(floating_point_source);
      }
    };

    #if !defined(BOOST_FIXED_POINT_FLOAT80_C)

      // Here is a somewhat significant work-around for the conversion of
      // cpp_bin_float to uint64_t. It is used for unique cases when
      // float80_t is not available, but we are still trying to convert
      // a small multiprecision floating point type to uint64_t.

      // Say, for instsance, that float64_t is the largest width built-in
      // floating-point type. It only has, however, 53 bits of precision.
      // In this case, we might use a multiprecision type for precision
      // ranging from 54...64 bits such as cpp_bin_float<64, digit_base_2>.
      // When being converted to uint64_t in this precision range,
      // multiprecision does not yet handle this case. In particular,
      // see the TODO in the comment at line 1113 of cpp_bin_float.hpp
      // from Boost 1.58. Hence we need this work-around.

      template<typename FloatingPointType>
      struct conversion_helper<std::uint64_t,
                               FloatingPointType,
                               typename std::enable_if<std::is_floating_point<FloatingPointType>::value == false>::type>
      {
      private:
        typedef boost::fixed_point::detail::float_type_helper<UINT32_C(64)>::exact_float_type floating_point_type;

      public:
        static void convert_floating_point_to_unsigned_integer(const floating_point_type& floating_point_source,
                                                               std::uint64_t& unsigned_destination)
        {
          std::stringstream ss;

          ss << std::fixed << floating_point_source;

          std::string str(ss.str());

          const std::string::size_type position_of_dot = str.find(".");

          if(position_of_dot != std::string::npos)
          {
            str = str.substr(std::string::size_type(0U), position_of_dot);
          }

          unsigned_destination = boost::lexical_cast<std::uint64_t>(str);
        }
      };

    #endif // !BOOST_FIXED_POINT_FLOAT80_C

  #endif // !BOOST_FIXED_POINT_DISABLE_MULTIPRECISION

  #if defined(BOOST_FIXED_POINT_DISABLE_MULTIPRECISION)

    template<typename UnsignedIntegralType,
             typename FloatingPointType,
             typename EnableType = void>
    struct conversion_helper
    {
      static void convert_floating_point_to_unsigned_integer(const FloatingPointType& floating_point_source,
                                                             UnsignedIntegralType& unsigned_destination)
      {
        unsigned_destination = static_cast<UnsignedIntegralType>(floating_point_source);
      }
    };

  #endif // BOOST_FIXED_POINT_DISABLE_MULTIPRECISION

  template<typename UnsignedIntegralType>
  std::uint_fast16_t msb_helper(UnsignedIntegralType& u,
                                UnsignedIntegralType& mask,
                                const std::uint_fast16_t bit_count)
  {
    // Use O(log2[N]) binary-halving search algorithm to find the msb.
    // The binary-halving search algorithm uses a recursive function call.

    static_assert(   (std::numeric_limits<UnsignedIntegralType>::is_signed  == false)
                  && (std::numeric_limits<UnsignedIntegralType>::is_integer == true),
                  "The UnsignedIntegralType for msb_helper must be an unsigned integral type.");

    if(u < 2U)
    {
      return UINT32_C(0);
    }

    const UnsignedIntegralType hi_part = (u >> (bit_count / 2U));

    mask = (mask >> (bit_count / 2U));

    if((hi_part & mask) != 0)
    {
      u = hi_part;

      return (bit_count / 2U) + msb_helper(u, mask, std::uint_fast16_t(bit_count / 2U));
    }
    else
    {
      u = (u & mask);

      return msb_helper(u, mask, (bit_count / 2U));
    }
  }

  // Make a template specialization of msb_helper() for std::uint32_t.
  template<>
  std::uint_fast16_t msb_helper(std::uint32_t& u,
                                std::uint32_t&,
                                const std::uint_fast16_t)
  {
    std::uint_fast8_t r(0);

    // Use O(log2[N]) binary-halving in an unrolled loop to find the msb.
    if((u & UINT32_C(0xFFFF0000)) != UINT32_C(0)) { u >>= 16; r |= UINT8_C(16); }
    if((u & UINT32_C(0x0000FF00)) != UINT32_C(0)) { u >>=  8; r |= UINT8_C( 8); }
    if((u & UINT32_C(0x000000F0)) != UINT32_C(0)) { u >>=  4; r |= UINT8_C( 4); }
    if((u & UINT32_C(0x0000000C)) != UINT32_C(0)) { u >>=  2; r |= UINT8_C( 2); }
    if((u & UINT32_C(0x00000002)) != UINT32_C(0)) { u >>=  1; r |= UINT8_C( 1); }

    return std::uint_fast16_t(r);
  }

  // Make a template specialization of msb_helper() for std::uint16_t.
  template<>
  std::uint_fast16_t msb_helper(std::uint16_t& u,
                                std::uint16_t&,
                                const std::uint_fast16_t)
  {
    std::uint_fast8_t r(0);

    // Use O(log2[N]) binary-halving in an unrolled loop to find the msb.
    if((u & UINT16_C(0xFF00)) != UINT16_C(0)) { u >>= 8; r |= UINT8_C(8); }
    if((u & UINT16_C(0x00F0)) != UINT16_C(0)) { u >>= 4; r |= UINT8_C(4); }
    if((u & UINT16_C(0x000C)) != UINT16_C(0)) { u >>= 2; r |= UINT8_C(2); }
    if((u & UINT16_C(0x0002)) != UINT16_C(0)) { u >>= 1; r |= UINT8_C(1); }

    return std::uint_fast16_t(r);
  }

  // Make a template specialization of msb_helper() for std::uint8_t.
  template<>
  std::uint_fast16_t msb_helper(std::uint8_t& u,
                                std::uint8_t&,
                                const std::uint_fast16_t)
  {
    std::uint_fast8_t r(0);

    // Use O(log2[N]) binary-halving in an unrolled loop to find the msb.
    if((u & UINT8_C(0xF0)) != UINT8_C(0)) { u >>= 4; r |= UINT8_C(4); }
    if((u & UINT8_C(0x0C)) != UINT8_C(0)) { u >>= 2; r |= UINT8_C(2); }
    if((u & UINT8_C(0x02)) != UINT8_C(0)) { u >>= 1; r |= UINT8_C(1); }

    return std::uint_fast16_t(r);
  }

  template<typename ArithmeticType>
  ArithmeticType power_of_two_helper(int p2)
  {
    if(p2 == 0)
    {
      return ArithmeticType(1);
    }
    else if(p2 < 0)
    {
      return ArithmeticType(1) / power_of_two_helper<ArithmeticType>(-p2);
    }
    else
    {
      // The variable xn stores the binary powers of x.
      ArithmeticType the_result(((p2 % 2) != 0) ? ArithmeticType(2) : ArithmeticType(1));

      ArithmeticType xn(2);

      while((p2 /= 2) != 0)
      {
        // Square xn for each binary power.
        xn *= xn;

        const bool has_binary_power = ((p2 % 2) != 0);

        if(has_binary_power)
        {
          // Multiply the result with each binary power contained in the exponent.
          the_result *= xn;
        }
      }

      return the_result;
    }
  }

  template<typename UnsignedLargeType,
           typename UnsignedSmallType = typename integer_type_helper<std::numeric_limits<UnsignedLargeType>::digits / 2>::exact_unsigned_type>
  UnsignedSmallType lo_part(const UnsignedLargeType& lt)
  {
    static_assert(   (std::numeric_limits<UnsignedLargeType>::is_integer == true)
                  && (std::numeric_limits<UnsignedLargeType>::is_signed  == false),
                  "The UnsignedLargeType for lo_part must be an unsigned integral type.");

    static_assert(   (std::numeric_limits<UnsignedSmallType>::is_integer == true)
                  && (std::numeric_limits<UnsignedSmallType>::is_signed  == false),
                  "The UnsignedSmallType for lo_part must be an unsigned integral type.");

    return UnsignedSmallType(lt);
  }

  template<typename UnsignedLargeType,
           typename UnsignedSmallType = typename integer_type_helper<std::numeric_limits<UnsignedLargeType>::digits / 2>::exact_unsigned_type>
  UnsignedSmallType hi_part(const UnsignedLargeType& lt)
  {
    static_assert(   (std::numeric_limits<UnsignedLargeType>::is_integer == true)
                  && (std::numeric_limits<UnsignedLargeType>::is_signed  == false),
                  "The UnsignedLargeType for hi_part must be an unsigned integral type.");

    static_assert(   (std::numeric_limits<UnsignedSmallType>::is_integer == true)
                  && (std::numeric_limits<UnsignedSmallType>::is_signed  == false),
                  "The UnsignedSmallType for hi_part must be an unsigned integral type.");

    return UnsignedSmallType(lt >> std::numeric_limits<UnsignedSmallType>::digits);
  }

  template<typename UnsignedLargeType,
           typename UnsignedSmallType = typename integer_type_helper<std::numeric_limits<UnsignedLargeType>::digits / 2>::exact_unsigned_type>
  UnsignedLargeType make_large(const UnsignedSmallType& lo, const UnsignedSmallType& hi)
  {
    static_assert(   (std::numeric_limits<UnsignedLargeType>::is_integer == true)
                  && (std::numeric_limits<UnsignedLargeType>::is_signed  == false),
                  "The UnsignedLargeType for make_large must be an unsigned integral type.");

    static_assert(   (std::numeric_limits<UnsignedSmallType>::is_integer == true)
                  && (std::numeric_limits<UnsignedSmallType>::is_signed  == false),
                  "The UnsignedSmallType for make_large must be an unsigned integral type.");

    return UnsignedLargeType( (UnsignedLargeType(hi) << std::numeric_limits<UnsignedSmallType>::digits)
                             | UnsignedLargeType(lo));
  }

  template<typename UnsignedSmallType,
           typename UnsignedHalfType = typename integer_type_helper<std::numeric_limits<UnsignedSmallType>::digits / 2>::exact_unsigned_type>
  void two_component_multiply(const UnsignedSmallType& u,
                              const UnsignedSmallType& v,
                                    UnsignedSmallType& result_lo,
                                    UnsignedSmallType& result_hi)
  {
    // Multiply u * v, where u and v are both unsigned
    // and both have 2^n bits. The result has 2*(2^n) bits.
    // The result is stored in a pair of two unsigned integers,
    // result_lo and result_hi.

    // Use an elementary school multiplication algorithm.

    // For example, multiply:
    //   uint64_t * uint64_t --> uint128_t result,
    // where result is stored in a (uint64_t, uint64_t) pair.

    // The result of the multiplication of (u * v) is:
    //   (u_hi_v_hi << n) + (uv_cross << (n/2)) + u_lo_v_lo,
    // where
    //   u_hi_v_hi = (u_hi * v_hi),
    // and
    //   uv_cross  = (u_lo * v_hi) + (u_hi * v_lo),
    // and
    //   u_lo_v_lo = (u_lo * v_lo).

    // Care is taken to properly handle shifts and carries.

    static_assert(   (std::numeric_limits<UnsignedSmallType>::is_integer == true)
                  && (std::numeric_limits<UnsignedSmallType>::is_signed  == false),
                  "The UnsignedSmallType for two_component_multiply must be an unsigned integral type.");

    static_assert(   (std::numeric_limits<UnsignedHalfType>::is_integer == true)
                  && (std::numeric_limits<UnsignedHalfType>::is_signed  == false),
                  "The UnsignedHalfType for two_component_multiply must be an unsigned integral type.");

    static_assert((std::numeric_limits<UnsignedHalfType>::digits * 2) == std::numeric_limits<UnsignedSmallType>::digits,
                  "The UnsignedSmallType for two_component_multiply must have exactly twice as many digits as the UnsignedHalfType.");

    typedef UnsignedSmallType local_unsigned_small_type;
    typedef UnsignedHalfType  local_unsigned_half_type;

    BOOST_CONSTEXPR_OR_CONST int digits_half = std::numeric_limits<local_unsigned_half_type>::digits;

    const local_unsigned_half_type u_lo(static_cast<local_unsigned_half_type>(u));
    const local_unsigned_half_type v_lo(static_cast<local_unsigned_half_type>(v));

    const local_unsigned_half_type u_hi(static_cast<local_unsigned_half_type>(u >> digits_half));
    const local_unsigned_half_type v_hi(static_cast<local_unsigned_half_type>(v >> digits_half));

    const local_unsigned_small_type u_lo_v_lo(u_lo * local_unsigned_small_type(v_lo));
    const local_unsigned_small_type u_hi_v_lo(u_hi * local_unsigned_small_type(v_lo));
    const local_unsigned_small_type u_lo_v_hi(u_lo * local_unsigned_small_type(v_hi));

    const local_unsigned_small_type uv_cross_lo =
          local_unsigned_small_type(   local_unsigned_small_type(lo_part(u_lo_v_hi)))
        + local_unsigned_small_type(   local_unsigned_small_type(lo_part(u_hi_v_lo))
                                    +  local_unsigned_small_type(hi_part(u_lo_v_lo)));

    const local_unsigned_small_type uv_cross_hi =
          local_unsigned_small_type(   local_unsigned_small_type(hi_part(u_hi_v_lo)))
        + local_unsigned_small_type(   local_unsigned_small_type(hi_part(u_lo_v_hi))
                                    +  local_unsigned_small_type(hi_part(uv_cross_lo)));

    result_hi = local_unsigned_small_type(local_unsigned_small_type(u_hi * local_unsigned_small_type(v_hi)) + uv_cross_hi);

    result_lo = local_unsigned_small_type(  local_unsigned_small_type(uv_cross_lo << digits_half)
                                          | local_unsigned_small_type(lo_part(u_lo_v_lo)));
  }

  template<typename UnsignedSmallType,
           typename UnsignedHalfType = typename integer_type_helper<std::numeric_limits<UnsignedSmallType>::digits / 2>::exact_unsigned_type>
  void two_component_divide(const UnsignedSmallType& u_lo,
                            const UnsignedSmallType& u_hi,
                            const UnsignedSmallType& v_lo,
                                  UnsignedSmallType& result_lo,
                                  UnsignedSmallType& result_hi)
  {
    // Divide the pair (u_lo, u_hi) in the numerator
    // by v_lo in the denominator. The result is stored
    // in the pair (result_lo, result_hi).

    // Here the numerator has 2*(2^n) bits and the denominator
    // has up to 2^n bits. The result has up to 2*(2^n) bits.

    // For example, divide:
    //   uint128_t numerator / uint64_t --> uint128_t result,
    // where the both the numerator as well as the result are
    // stored in a (uint64_t, uint64_t) pair.

    // A simplified version of Knuth's long division algorithm
    // is used. The loop-ordering of Knuth's algorithm has been
    // reversed. Some internal loops of the algorithm have been
    // manually unrolled to improve efficiency.

    // The division algorithm is carried out with arrays
    // of limbs having a type that is half as wide
    // as the type of the input parameters.

    // See also:
    // D.E. Knuth, "The Art of Computer Programming, Volume 2:
    // Seminumerical Algorithms", Addison-Wesley (1998),
    // Section 4.3.1 Algorithm D and Exercise 16.

    static_assert(   (std::numeric_limits<UnsignedSmallType>::is_integer == true)
                  && (std::numeric_limits<UnsignedSmallType>::is_signed  == false),
                  "The UnsignedSmallType for two_component_divide must be an unsigned integral type.");

    static_assert(   (std::numeric_limits<UnsignedHalfType>::is_integer == true)
                  && (std::numeric_limits<UnsignedHalfType>::is_signed  == false),
                  "The UnsignedHalfType for two_component_divide must be an unsigned integral type.");

    static_assert((std::numeric_limits<UnsignedHalfType>::digits * 2) == std::numeric_limits<UnsignedSmallType>::digits,
                  "The UnsignedSmallType for two_component_divide must have exactly twice as many digits as the UnsignedHalfType.");

    typedef UnsignedSmallType local_unsigned_small_type;
    typedef UnsignedHalfType  local_unsigned_half_type;

    std::array<local_unsigned_half_type, 4U + 1U> u_tmp =
    {{
      lo_part(u_lo),
      hi_part(u_lo),
      lo_part(u_hi),
      hi_part(u_hi),
      local_unsigned_half_type(0U)
    }};

    std::array<local_unsigned_half_type, 2U> v_tmp =
    {{
      lo_part(v_lo),
      hi_part(v_lo)
    }};

    std::array<local_unsigned_half_type, 4U> result =
    {{
      local_unsigned_half_type(0U),
      local_unsigned_half_type(0U),
      local_unsigned_half_type(0U),
      local_unsigned_half_type(0U)
    }};

    // Handling zero numerator and/or zero denominator
    // has already been done by the function that calls
    // this subroutine.

    if(v_tmp[1U] == 0U)
    {
      // The denominator has one single limb.
      // Use a simplified linear division algorithm.
      // The loop has been unrolled.

      result[3U] = u_tmp[3U] / v_tmp[0U];

      local_unsigned_small_type val
                  = local_unsigned_small_type(u_tmp[2U]) + local_unsigned_small_type(local_unsigned_small_type(local_unsigned_small_type(u_tmp[3U]) - local_unsigned_small_type(local_unsigned_small_type(v_tmp[0U]) * result[3U])) << std::numeric_limits<local_unsigned_half_type>::digits);
      result[2U]  = lo_part(local_unsigned_small_type(val / local_unsigned_small_type(v_tmp[0U])));

      val         = local_unsigned_small_type(u_tmp[1U]) + local_unsigned_small_type(local_unsigned_small_type(val - local_unsigned_small_type(local_unsigned_small_type(v_tmp[0U]) * result[2U])) << std::numeric_limits<local_unsigned_half_type>::digits);
      result[1U]  = lo_part(local_unsigned_small_type(val / local_unsigned_small_type(v_tmp[0U])));

      val         = local_unsigned_small_type(u_tmp[0U]) + local_unsigned_small_type(local_unsigned_small_type(val - local_unsigned_small_type(local_unsigned_small_type(v_tmp[0U]) * result[1U])) << std::numeric_limits<local_unsigned_half_type>::digits);
      result[0U]  = lo_part(local_unsigned_small_type(val / local_unsigned_small_type(v_tmp[0U])));

      result_lo = make_large<local_unsigned_small_type>(result[0U], result[1U]);
      result_hi = make_large<local_unsigned_small_type>(result[2U], result[3U]);

      return;
    }

    // Calculate the number of significant limbs in u.
    std::uint_fast8_t sig_limbs_u = 4U;

    for(std::uint_fast8_t i = std::uint_fast8_t(4U - 1U); ((std::int_fast8_t(i) >= std::int_fast8_t(0)) && (u_tmp[i] == 0U)); --i)
    {
      --sig_limbs_u;
    }

    // The result of the division is 0 if the
    // denominator is larger than the numerator.

    // The result of the division is 1 if the
    // denominator is equal to the numerator.

    // Check if the denominator is larger than
    // or equal to the numerator.

    // At this point in the subroutine, we know
    // that v has exactly 2 significant limbs.

    bool b_zero = false;

    if(sig_limbs_u < 2U)
    {
      // The denominator is larger than the numerator.
      // The result of the division is 0.
      b_zero = true;
    }
    else if(sig_limbs_u == 2U)
    {
      const bool u1_v1_are_equal = (u_tmp[1U] <  v_tmp[1U]);

      if(    u1_v1_are_equal
         || (u1_v1_are_equal && (u_tmp[0U] < v_tmp[0U])))
      {
        // The denominator is larger than the numerator.
        // The result of the division is 0.
        b_zero = true;
      }
      else if(u1_v1_are_equal && (u_tmp[0U] == v_tmp[0U]))
      {
        // The denominator is equal to the numerator.
        // The result of the division is 1.
        result_lo = 1U;
        result_hi = 0U;

        return;
      }
    }

    if(b_zero)
    {
      // The result is 0.
      result_lo = 0U;
      result_hi = 0U;

      return;
    }

    // Now use the simplified version of Knuth's
    // long division algorithm.

    {
      // Compute the normalization factor.

      const local_unsigned_half_type norm =
        lo_part(local_unsigned_small_type(local_unsigned_small_type(1U) << std::numeric_limits<local_unsigned_half_type>::digits) / (local_unsigned_small_type(v_tmp[1U]) + 1U));

      if(norm != 1U)
      {
        // Step D1(b): Multiply u with the normalization.

        local_unsigned_half_type carry(0U);

        std::uint_fast8_t i;

        for(i = 0U; i < sig_limbs_u; ++i)
        {
          const local_unsigned_small_type val(local_unsigned_small_type(local_unsigned_small_type(u_tmp[i]) * norm) + local_unsigned_small_type(carry));

          u_tmp[i] = lo_part(val);
          carry    = hi_part(val);
        }

        u_tmp[i] = carry;

        // Step D1(c): Multiply v with the normalization.
        // The loop has been unrolled.

        local_unsigned_small_type val
                  = local_unsigned_small_type(local_unsigned_small_type(v_tmp[0U]) * norm);
        v_tmp[0U] = lo_part(val);

        val       = local_unsigned_small_type(local_unsigned_small_type(v_tmp[1U]) * norm) + local_unsigned_small_type(hi_part(val));
        v_tmp[1U] = lo_part(val);
      }
    }

    // Steps D2 and D7.
    for(std::uint_fast8_t j = 0U; j <= (sig_limbs_u - 2U); ++j)
    {
      // Step D3: Calculate q_guess.

      // Here q_guess is the initial guess of the next
      // iteration in the result of the long division.

      const std::uint_fast8_t uj(sig_limbs_u - j);

      const local_unsigned_small_type uj_ujm1(make_large<local_unsigned_small_type>(u_tmp[uj - 1U], u_tmp[uj]));

      local_unsigned_small_type q_guess =
        ((u_tmp[uj] == v_tmp[1U]) ? local_unsigned_small_type((std::numeric_limits<local_unsigned_half_type>::max)())
                                  : local_unsigned_small_type(uj_ujm1 / local_unsigned_small_type(v_tmp[1U])));

      // Decrease q_guess as needed.
      for( ; ; --q_guess)
      {
        const local_unsigned_small_type val(uj_ujm1 - local_unsigned_small_type(q_guess * v_tmp[1U]));

        if(hi_part(val) != 0U)
        {
          break;
        }

        if(local_unsigned_small_type(q_guess * v_tmp[0U]) <= make_large<local_unsigned_small_type>(u_tmp[uj - 2U], lo_part(val)))
        {
          break;
        }

        // If either of the two break statements above has
        // been executed, then q_guess is not decremented.
      }

      // Step D4: Multiply and subtract.
      // The loop has been unrolled.

      std::array<local_unsigned_half_type, 2U + 1U> n_tmp;

      std::uint_fast8_t borrow;

      {
        // Multiply.
        local_unsigned_small_type val
                  = local_unsigned_small_type(v_tmp[0U] * q_guess);
        n_tmp[0U] = lo_part(val);

        val       = local_unsigned_small_type(v_tmp[1U] * q_guess) + local_unsigned_small_type(hi_part(val));
        n_tmp[1U] = lo_part(val);

        n_tmp[2U] = hi_part(val);

        // Subtract.
        val            = local_unsigned_small_type(local_unsigned_small_type(u_tmp[uj - 2U]) - local_unsigned_small_type(n_tmp[0U]));
        u_tmp[uj - 2U] = lo_part(val);
        borrow         = ((hi_part(val) != 0U) ? 1U : 0U);

        val            = local_unsigned_small_type(local_unsigned_small_type(u_tmp[uj - 1U]) - local_unsigned_small_type(n_tmp[1U])) - borrow;
        u_tmp[uj - 1U] = lo_part(val);
        borrow         = ((hi_part(val) != 0U) ? 1U : 0U);

        val            = local_unsigned_small_type(local_unsigned_small_type(u_tmp[uj - 0U]) - local_unsigned_small_type(n_tmp[2U])) - borrow;
        u_tmp[uj - 0U] = lo_part(val);
        borrow         = ((hi_part(val) != 0U) ? 1U : 0U);
      }

      // Get the result data.
      result[uj - 2U] = lo_part(q_guess);

      // Step D5: Test the remainder.
      if(borrow != 0U)
      {
        // Step D6: Add v to u and decrement the result.
        // The loop has been unrolled.

        local_unsigned_small_type val
                       = local_unsigned_small_type(local_unsigned_small_type(u_tmp[uj - 2U]) + local_unsigned_small_type(n_tmp[0U]));
        u_tmp[uj - 2U] = lo_part(val);

        val            = local_unsigned_small_type(local_unsigned_small_type(u_tmp[uj - 1U]) + local_unsigned_small_type(n_tmp[1U])) + std::uint_fast8_t((hi_part(val) != 0U) ? 1U : 0U);
        u_tmp[uj - 1U] = lo_part(val);

        val            = local_unsigned_small_type(local_unsigned_small_type(u_tmp[uj - 0U]) + local_unsigned_small_type(n_tmp[2U])) + std::uint_fast8_t((hi_part(val) != 0U) ? 1U : 0U);
        u_tmp[uj - 0U] = lo_part(val);

        u_tmp[uj - 2U] += std::uint_fast8_t((hi_part(val) != 0U) ? 1U : 0U);

        --result[uj - 2U];
      }
    }

    // Compose the low and high parts of the result.
    result_lo = make_large<local_unsigned_small_type>(result[0U], result[1U]);
    result_hi = make_large<local_unsigned_small_type>(result[2U], result[3U]);
  }

  } } } // namespace boost::fixed_point::detail
  //! \endcond // DETAIL

#endif // FIXED_POINT_DETAIL_2015_05_23_HPP_


/*
 * workModel.hh
 *
 *  Created on: May 4, 2015
 *      Author: sunayana_ghosh
 */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2015-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#ifndef KASKADE_TIMESTEPPING_EULERSDC_WORKMODEL_HH_
#define KASKADE_TIMESTEPPING_EULERSDC_WORKMODEL_HH_

//includes from current project
#include "norm.hh"
#include "util.hh"

//includes from c, c++
#include <cmath>
#include <iostream>
#include <vector>

//includes from boost
#include <boost/math/special_functions/round.hpp>
#include <boost/math/tools/minima.hpp>   //used for Brent's method to compute minima of a function

//includes from DUNE
#include "dune/common/dynmatrix.hh"
#include "dune/common/dynvector.hh"

namespace Kaskade {
  //========================================================================================
  //     Implementation for enum class WorkModelType
  //========================================================================================

  /**
   * \ingroup eulerSDC
   * \brief Enum class to define the norm type is class Norm.
   */

  //--C++11 strongly typed enums.
  enum class WorkModelType {
    ITERATION,      //!< ITERATION
    FED             //!< FED
  };

  /**
   * \ingroup eulerSDC
   * \brief Abstract base class for different work models
   *
   * This class represents a generic base class for an arbitrary work model
   */

  //================================================================================================
  //      Implementation of the abstract base class WorkModel to compute the local tolerances.
  //================================================================================================

  /**
   * \ingroup eulerSDC
   * \brief Abstract base class WorkModel which provides an interface for different kinds of work models for
   * computation of the cost function, maximum number of iterations and local tolerances.
   */

   template<class Vector, class Norm, class Utils>
   class WorkModel
   {
   public:
     using field_type = typename Vector::value_type;
     using RealVector = Dune::DynamicVector<double>;
     using RealMatrix = Dune::DynamicMatrix<double>;

     //pure virtual functions

       /**
        * \ingroup eulerSDC
        * \brief Pure virtual function implemented in derived classes for different work models. Computes the
        *        local tolerances for every time point and every iteration step given global tolerance.
        *
        * @param yPrev      An (i-1)-th iterative approximation to the solution using sdc Iteration step.
        * @param yCurrent   An i-th iterative approximation to the solution using sdc Iteration step.
        * @param yNext      An (i+1)-th iterative approximation to the solution using sdc Iteration step.
        * @return Returns the matrix of local tolerances.
        */
       virtual RealMatrix const& computeLocalTolerances(std::vector<Vector> const& yPrev,
                                                        std::vector<Vector> const& yCurrent,
                                                        std::vector<Vector> const& yNext) = 0;

       /**
        * \ingroup eulerSDC
        *
        * @param yPrev
        * @param yCurrent
        * @param rho
        *
        * @return
        */
       virtual field_type lowerBoundIterJ(std::vector<Vector> const& yPrev,
                                          std::vector<Vector> const& yCurrent,
                                          field_type rho) = 0;

       //virtual destructor
       virtual ~WorkModel() {}
   };

   //========================================================================================================
   //             Implementation of the derived class Iteration representing the iteration work model.
   //========================================================================================================

   template<class Vector, class Norm, class Utils>
   class Iteration : public WorkModel<Vector, Norm, Utils>
   {
   public:
     using field_type = typename Vector::value_type;
     using RealVector = Dune::DynamicVector<double>;
     using RealMatrix = Dune::DynamicMatrix<double>;

     //constructor for the derived class Iteration
     Iteration(field_type tol_,
               std::vector<Vector> const& y0_, std::vector<Vector> const& y1_,
               Norm& norm_, Utils& util_,
               RealVector const& timePts_, RealMatrix const& integrationMatrix_,
               field_type tolNewton_, int maxIterNewton_,
               field_type rhoit_, int mmin_);

     // function to compute cost
     /**
      * \ingroup eulerSDC
      * @param yPrev
      * @param yCurrent
      * @param yNext
      * @param tolMat
      *
      * @return
      */

     field_type computeCost(std::vector<Vector> const& yPrev,
                            std::vector<Vector> const& yCurrent,
                            std::vector<Vector> const& yNext,
                            RealMatrix const& tolMat);

     /**
      * \ingroup eulerSDC
      * @param yPrev
      * @param yCurrent
      * @param yNext
      * @param rho
      *
      * @return
      */
     field_type getIterJ(std::vector<Vector> const& yPrev,
                         std::vector<Vector> const& yCurrent,
                         std::vector<Vector> const& yNext,
                         field_type rho);

     virtual RealMatrix const& computeLocalTolerances(std::vector<Vector> const& yPrev,
                                                      std::vector<Vector> const& yCurrent,
                                                      std::vector<Vector> const& yNext)
     {
       //To compute optimal local tolerances we first need the value of maxSDCIterations
       auto rho = util.sdcContractionFactor(yPrev, yCurrent, yNext, norm);
       //compute maxSDCIterations
       auto maxSDCIterations = getIterJ(yPrev, yCurrent, yNext, rho);
       //then we need to compute the value of mu which depends on maxSDCIterations
       auto mu = computeMu(yPrev, yCurrent, rho, maxSDCIterations, nIntervals, tol, norm);
       //compute the value of alphaVec
       auto alphaVec = util.computeAlphaVec(timePts, integrationMatrix, yPrev, yCurrent, yNext);
       //initialize the matrix
       int itJ = (int) maxSDCIterations;
       xijMat = RealMatrix(nIntervals, itJ, 0.0);
       for (auto i = 0; i < nIntervals; ++i)
       {
         for (auto j = 0; j < itJ; ++j)
         {
           xijMat[i][j] = -1/(mu * std::pow(rho, itJ-1-j) * alphaVec[i]);
         }
       }
       return xijMat;
     }

     //function computes the lower bound of maxSDCIterations
     virtual field_type lowerBoundIterJ(std::vector<Vector> const& yPrev,
                                        std::vector<Vector> const& yCurrent,
                                        field_type rho)
     {
       auto val = Kaskade::normVecDiff(yPrev,yCurrent,norm);
       auto lboundJ = std::log((1-rho) * tol / val) / std::log(rho);
       return boost::math::round(lboundJ);
     }

     //delete later
     field_type static f (std::vector<Vector> const& yPrev,
         std::vector<Vector> const& yCurrent,
         std::vector<Vector> const& yNext,
         double rho)
       {
         auto yTotal = yPrev[0] + yCurrent[0] + yNext[0];
         return (yTotal[0] + yTotal[1]) * rho;
       }

     //destructor
     virtual ~Iteration(){}

   private:
     field_type tol;
     std::vector<Vector> y0;
     std::vector<Vector> y1;
     Norm norm;
     Utils util;
     field_type normy01;
     RealVector timePts;
     RealMatrix integrationMatrix;
     int nIntervals;
     field_type maxPrecisionBrent = 20;  //computation with 20 bit precision
     field_type rhoit;
     int mmin;
     RealMatrix xijMat;

     //private cost function

     field_type static costFunction(std::vector<Vector> const& yPrev,
                                    std::vector<Vector> const& yCurrent,
                                    std::vector<Vector> const& yNext,
                                    field_type maxSDCIterations,
                                    Norm& norm,
                                    Utils& util,
                                    field_type normy01,
                                    RealVector const& timePts,
                                    RealMatrix const& integrationMatrix,
                                    int nIntervals, field_type tol);

     field_type static computeMu(std::vector<Vector> const& yPrev,
                          std::vector<Vector> const& yCurrent,
                          field_type rho,
                          field_type maxSDCIterations,
                          int nIntervals,
                          field_type tol,
                          Norm& norm);

     field_type computeDerivativeMu(std::vector<Vector> const& yPrev,
                                    std::vector<Vector> const& yCurrent,
                                    field_type rho,
                                    field_type maxSDCIterations);

     field_type derivativeCostFunction(std::vector<Vector> const& yPrev,
                                       std::vector<Vector> const& yCurrent,
                                       field_type rho,
                                       field_type alphaProd,
                                       field_type c,
                                       field_type maxSDCIterations);

     //compute product of entries of alphaVec
     field_type static computeProduct(RealVector const& alpha);

     //This method is based on the ***assumption*** that cost function is monotonically decreasing
     field_type computeIterJ(std::vector<Vector> const& yPrev,
                       std::vector<Vector> const& yCurrent,
                       std::vector<Vector> const& yNext,
                       field_type rho);

   };

   //=======================================================================================================
   //   Implementation of the derived class FiniteElementDiscretization representing the FED work model.
   //======================================================================================================

   template<class Vector, class Norm, class Utils>
   class FiniteElementDiscretization : public WorkModel<Vector, Norm, Utils>
   {
   public:
     using field_type = typename Vector::value_type;
     using RealVector = Dune::DynamicVector<double>;
     using RealMatrix = Dune::DynamicMatrix<double>;

     //constructor for the derived class FiniteElementDiscretization
     FiniteElementDiscretization(field_type tol_,
                                 std::vector<Vector> const& y0_, std::vector<Vector> const& y1_,
                                 Norm& norm_, Utils& util_,
                                 RealVector const& timePts_, RealMatrix const& integrationMatrix_,
                                 int dim_);

     //function to compute cost
     //(cannot be a pure virtual function since the parameters are different for different work models.)
     field_type computeCost(RealMatrix const& tolMat);

     field_type getIterJ(std::vector<Vector> const& yPrev,
                         std::vector<Vector> const& yCurrent,
                         field_type rho);

     virtual RealMatrix const& computeLocalTolerances(std::vector<Vector> const& yPrev,
                                                      std::vector<Vector> const& yCurrent,
                                                      std::vector<Vector> const& yNext)
     {
       field_type d1 = 0.0;
       field_type d2 = -1.0 / ((double) dim + 1.0);
       auto rho = util.sdcContractionFactor(yPrev, yCurrent, yNext, norm);
       //std::cout << "rho = " <<  rho << std::endl;
       //compute max sdc iterations
       maxSDCIterations = getIterJ(yPrev, yCurrent, rho);
       auto alphaVec = util.computeAlphaVec(timePts, integrationMatrix, yPrev, yCurrent, yNext);
       auto lambda = computeLambda(yPrev, yCurrent, yNext, maxSDCIterations);
       //std::cout << "lambda = " << lambda << std::endl;
       //initialize the matrix
       int itJ = (int) maxSDCIterations;
       xijMat = RealMatrix(nIntervals, itJ, 0.0);
       for (auto i = 0; i < nIntervals; ++i)
       {
         auto val = std::pow(alphaVec[i], d2);
         for (auto j = 0; j < itJ; ++j)
         {
           d1 = (maxSDCIterations-1-j)/(d2);
           xijMat[i][j] = lambda * std::pow(rho, d1) * val;
         }
       }
       return xijMat;
     }

     //function computes the lower bound of maxSDCIterations
     virtual field_type lowerBoundIterJ(std::vector<Vector> const& yPrev,
                                        std::vector<Vector> const& yCurrent,
                                        field_type rho)
     {
       auto val = Kaskade::normVecDiff(yPrev, yCurrent, norm);
       auto lboundJ = std::log((1-rho)*tol/val)/std::log(rho);
       return boost::math::round(lboundJ);
     }

     //destructor
     virtual ~FiniteElementDiscretization(){}

   private:
     field_type tol;
     std::vector<Vector> y0;
     std::vector<Vector> y1;
     Norm norm;
     field_type normy01;
     Utils util;
     RealVector timePts;
     RealMatrix integrationMatrix;
     int nIntervals;
     int dim;
     field_type maxSDCIterations;
     RealMatrix xijMat;

     //private methods
     //computes A or B depending on the norm
     field_type computeA(std::vector<Vector> const& yPrev,
                         std::vector<Vector> const& yCurrent,
                         std::vector<Vector> const& yNext);

     field_type computeLambda(std::vector<Vector> const& yPrev,
                              std::vector<Vector> const& yCurrent,
                              std::vector<Vector> const& yNext,
                              field_type maxSDCIterations);
   };

   //=================================================================================================
   //   FUNCTION DEFINITION BEGINS FROM HERE
   //=================================================================================================

   //=================================================================================================
   //   ITERATION  WORKMODEL
   //=================================================================================================

   //=================================================================================================
   //   Constructor for the class Iteration
   //=================================================================================================

   template<class Vector, class Norm, class Utils>
   Iteration<Vector, Norm, Utils>::Iteration(field_type tol_,
               std::vector<Vector> const& y0_, std::vector<Vector> const& y1_,
               Norm& norm_, Utils& util_,
               RealVector const& timePts_, RealMatrix const& integrationMatrix_,
               field_type tolNewton_, int maxPrecisionBrent_,
               field_type rhoit_, int mmin_)
               : tol(tol_), y0(y0_), y1(y1_),
                 norm(norm_), util(util_), normy01(Kaskade::normVecDiff(y0,y1,norm)),
                 timePts(timePts_), integrationMatrix(integrationMatrix_), nIntervals(timePts.size()-1),
                 maxPrecisionBrent(maxPrecisionBrent_),
                 rhoit(rhoit_), mmin(mmin_){}

   //=================================================================================================
   //   Public member function for Iteration WorkModel to compute total cost.
   //=================================================================================================

   template<class Vector, class Norm, class Utils>
   typename Vector::value_type Iteration<Vector, Norm, Utils>::computeCost(std::vector<Vector> const& yPrev,
                                                                           std::vector<Vector> const& yCurrent,
                                                                           std::vector<Vector> const& yNext,
                                                                           RealMatrix const& tolMat)
   {
     //initialize the total cost
     field_type totalCost = 0;
     //compute SDC contraction factor
     auto rho = util.sdcContractionFactor(yPrev, yCurrent, yNext, norm);
     //compute c = log(|y1-y0|)
     auto c = std::log(normy01);
     //compute the constant k = -mmin * log(rhoit)
     auto k = -mmin * std::log(rhoit);
     //dimensions of the tolerance matrix
     auto rows = tolMat.rows();
     auto cols = tolMat.cols();
     for (auto i = 0u; i < rows; ++i)
     {
       for (auto j = 0u; j < cols; ++j)
       {
         totalCost += std::max(k, c - std::log(tolMat[i][j]/std::pow(rho,j)));
       }
     }
     return totalCost;
   }

   //=================================================================================================
   //   Public member function for Iteration WorkModel to get the maxSDCIterations.
   //=================================================================================================
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type Iteration<Vector, Norm, Utils>::getIterJ(std::vector<Vector> const& yPrev,
                                                                        std::vector<Vector> const& yCurrent,
                                                                        std::vector<Vector> const& yNext,
                                                                        field_type rho)
   {
     auto maxSDCIterations = computeIterJ(yPrev, yCurrent, yNext, rho);
     return boost::math::round(maxSDCIterations);
   }

   //=================================================================================================
   //   Private member function for Iteration WorkModel for the costFunction.
   //=================================================================================================

   //make it a static function, and do not use any private member variables.
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type Iteration<Vector, Norm, Utils>::costFunction(std::vector<Vector> const& yPrev,
                                                                            std::vector<Vector> const& yCurrent,
                                                                            std::vector<Vector> const& yNext,
                                                                            field_type maxSDCIterations,
                                                                            Norm& norm,
                                                                            Utils& util,
                                                                            field_type normy01,
                                                                            RealVector const& timePts,
                                                                            RealMatrix const& integrationMatrix,
                                                                            int nIntervals,
                                                                            field_type tol)
   {
     //compute SDC contraction factor
     auto rho = util.sdcContractionFactor(yPrev, yCurrent, yNext, norm);
     //compute c = log(|y1-y0|)
     auto c = std::log(normy01);
     //compute mu
     auto mu = computeMu(yPrev, yCurrent, rho, maxSDCIterations, nIntervals, tol, norm);
     auto alphaVec = util.computeAlphaVec(timePts, integrationMatrix, yPrev, yCurrent, yNext);
     auto alphaProd = computeProduct(alphaVec);
     auto cost = nIntervals * c + nIntervals * maxSDCIterations * std::log(rho) + nIntervals * std::log(-mu) + std::log(alphaProd);
     return cost;
   }

   //==========================================================================================================
   //   Private member function for Iteration WorkModel for computing the mu function as described in the paper
   //==========================================================================================================
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type Iteration<Vector, Norm, Utils>::computeMu(std::vector<Vector> const& yPrev,
                                                                         std::vector<Vector> const& yCurrent,
                                                                         field_type rho,
                                                                         field_type maxSDCIterations,
                                                                         int nIntervals,
                                                                         field_type tol,
                                                                         Norm& norm)
   {
     auto mu = -((1-rho) * (maxSDCIterations - 1) * nIntervals)/((1-rho) * tol - std::pow(rho, maxSDCIterations) * Kaskade::normVecDiff(yPrev,yCurrent,norm));
     return mu;
   }

   //========================================================================================================================
   //   Private member function for Iteration WorkModel for computing the derivative of mu function as described in the paper
   //========================================================================================================================
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type Iteration<Vector, Norm, Utils>::computeDerivativeMu(std::vector<Vector> const& yPrev,
                                                                                std::vector<Vector> const& yCurrent,
                                                                                field_type rho,
                                                                                field_type maxSDCIterations)
   {
     auto val = Kaskade::normVecDiff(yPrev,yCurrent,norm);
     auto rhoJ = std::pow(rho, maxSDCIterations);
     auto dervMu = nIntervals * (1-rho) *
         (-(1-rho) * tol + val * rhoJ * (1 - (maxSDCIterations - 1) * std::log(rho)))/
         (std::pow((rhoJ * val - (1-rho) * tol), 2));
     return dervMu;
   }

   //========================================================================================================================
   //   Private member function for Iteration WorkModel for computing the derivative of cost Function
   //========================================================================================================================
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type Iteration<Vector, Norm, Utils>::derivativeCostFunction(std::vector<Vector> const& yPrev,
                                                                                std::vector<Vector> const& yCurrent,
                                                                                field_type rho,
                                                                                field_type alphaProd,
                                                                                field_type c,
                                                                                field_type maxSDCIterations)
   {

     auto mu = computeMu(yPrev, yCurrent, rho, maxSDCIterations, nIntervals, tol, norm);
     auto dervMu = computeDerivativeMu(yPrev, yCurrent, rho, maxSDCIterations);
     auto dervCost = nIntervals * c + nIntervals * (2*maxSDCIterations - 1) * std::log(rho) + std::log(alphaProd)
                     + nIntervals * std::log(-mu) + nIntervals * (maxSDCIterations - 1) * dervMu/mu;
     return dervCost;
   }

   //==========================================================================================================================
   //   Private member function for Iteration WorkModel for computing the product of alphaVector where all entries are positive
   //==========================================================================================================================
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type Iteration<Vector, Norm, Utils>::computeProduct(RealVector const& alpha)
   {
     field_type alphaProd = 1;
     for(auto i = 0u; i < alpha.size(); ++i)
       alphaProd *= alpha[i];

     return alphaProd;
   }

   //============================================================================================
   //   Private member function for Iteration WorkModel for computing maxSDCIterations
   //============================================================================================

//   //Use Brent's method to compute the minimum of the cost function
//   template<class Vector, class Norm, class Utils>
//   typename Vector::value_type Iteration<Vector, Norm, Utils>::computeIterJ(std::vector<Vector> const& yPrev,
//                                                                            std::vector<Vector> const& yCurrent,
//                                                                            std::vector<Vector> const& yNext,
//                                                                            field_type rho)
//   {
//     using Result = std::pair<double, double>;
//     //compute lower bound of maxSDCIterations to give as starting value for to Brent's method
//     field_type lbmaxSDCIterations = lowerBoundIterJ(yPrev, yCurrent, rho);
//     //initialize interval where the minima is searched
//     field_type left = lbmaxSDCIterations;
//     //initialize the lower bound on maxSDCIterations as input guess.
//     //auto func = std::bind(f, yPrev, yCurrent, yNext, std::placeholders::_1);
//     auto func = std::bind(costFunction, yPrev, yCurrent, yNext, std::placeholders::_1, norm, util, normy01,
//                           timePts, integrationMatrix, nIntervals, tol);
//     Result soln = boost::math::tools::brent_find_minima(func, left, left+6, maxPrecisionBrent);
//
//     return soln.first;
//   }

   //Numeric method to find minimia of the function
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type Iteration<Vector, Norm, Utils>::computeIterJ(std::vector<Vector> const& yPrev,
                                                                            std::vector<Vector> const& yCurrent,
                                                                            std::vector<Vector> const& yNext,
                                                                            field_type rho)
     {
     //compute lower bound of maxSDCIterations
     field_type lbmaxSDCIterations = lowerBoundIterJ(yPrev, yCurrent, rho);
     field_type maxSDCIterations = lbmaxSDCIterations + 1;
     //compute the cost at lbmaxSDCIterations
     field_type minCost = costFunction(yPrev, yCurrent, yNext, lbmaxSDCIterations, norm, util, normy01,
                                       timePts, integrationMatrix, nIntervals, tol);
     field_type val = costFunction(yPrev, yCurrent, yNext, maxSDCIterations, norm, util, normy01,
                                   timePts, integrationMatrix, nIntervals, tol);
     while(val > minCost)
     {
       minCost = val;
       maxSDCIterations++;
       val = costFunction(yPrev, yCurrent, yNext, maxSDCIterations, norm, util, normy01,
                          timePts, integrationMatrix, nIntervals, tol);
     }
     return maxSDCIterations;
     }

   //=================================================================================================
   //   FINITE ELEMENT DISCRETIZATION  WORKMODEL
   //=================================================================================================

   //=================================================================================================
   //   Constructor for the class FiniteElementDiscretization
   //=================================================================================================
   template<class Vector, class Norm, class Utils>
   FiniteElementDiscretization<Vector, Norm, Utils>::FiniteElementDiscretization(field_type tol_,
                                    std::vector<Vector> const& y0_, std::vector<Vector> const& y1_,
                                    Norm& norm_, Utils& util_,
                                    RealVector const& timePts_, RealMatrix const& integrationMatrix_,
                                    int dim_)
                                    : tol(tol_),
                                      y0(y0_), y1(y1_), norm(norm_), normy01(Kaskade::normVecDiff(y0, y1, norm)),
                                      util(util_),
                                      timePts(timePts_), integrationMatrix(integrationMatrix_), nIntervals(timePts.size()-1),
                                      dim(dim_){}

   //========================================================================================================
   //   Public member function computeCost for the class FiniteElementDiscretization, computes the total cost
   //========================================================================================================
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type FiniteElementDiscretization<Vector, Norm, Utils>::computeCost(RealMatrix const& tolMat)
   {
     //initialize the total cost
     field_type totalCost = 0.0;
     //dimensions of the tolerance matrix
     auto rows = tolMat.rows();
     auto cols = tolMat.cols();
     for (auto i = 0u; i < rows; ++i)
     {
       for (auto j = 0u; j < cols; ++j)
         totalCost += 1/std::pow(tolMat[i][j], dim);
     }
     return totalCost;
   }

   //===========================================================================================================
   //   Public member function getIterJ for the class FiniteElementDiscretization, returns the maxSDCIterations
   //===========================================================================================================
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type FiniteElementDiscretization<Vector, Norm, Utils>::getIterJ(std::vector<Vector> const& yPrev,
                                                                                std::vector<Vector> const& yCurrent,
                                                                                field_type rho)
   {
     auto val = Kaskade::normVecDiff(yPrev, yCurrent, norm);
     maxSDCIterations = (dim + 1)/(std::log(rho)) * std::log((1-rho)*tol / val);

     return boost::math::round(maxSDCIterations);
   }

   //===============================================================================================================
   //   Private member function for FiniteElementDiscretization WorkModel for computing A or B depending on the norm
   //===============================================================================================================
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type FiniteElementDiscretization<Vector, Norm, Utils>::computeA(std::vector<Vector> const& yPrev,
                                                                                std::vector<Vector> const& yCurrent,
                                                                                std::vector<Vector> const& yNext)
   {
     //compute alphaVec
     auto alphaVec = util.computeAlphaVec(timePts, integrationMatrix, yPrev, yCurrent, yNext);
     //initialize alphaTildeVec
     auto alphaTildeVec = RealVector(alphaVec.size(), 0.0);
     //casting dim/(dim + 1) to double
     double d = ((double) dim)/((double) dim + 1);
     field_type sumAlphaTilde = 0.0;
     //compute alphaTildeVec and A the sum of alphaTilde
     for (auto i = 0u; i < alphaVec.size(); ++i)
     {
       alphaTildeVec[i] = std::pow(alphaVec[i], d);
       sumAlphaTilde += alphaTildeVec[i];
     }
     return sumAlphaTilde;
   }

   //====================================================================================================================
   //   Private member function for FiniteElementDiscretization WorkModel for computing lambda as described in the paper.
   //====================================================================================================================
   template<class Vector, class Norm, class Utils>
   typename Vector::value_type FiniteElementDiscretization<Vector, Norm, Utils>::computeLambda
                                                                           (std::vector<Vector> const& yPrev,
                                                                            std::vector<Vector> const& yCurrent,
                                                                            std::vector<Vector> const& yNext,
                                                                            field_type maxSDCIterations)
   {
     //compute rho, sdc-contraction factor
     auto rho = util.sdcContractionFactor(yPrev, yCurrent, yNext, norm);
     //casting
     double d = ((double) dim)/((double) dim + 1);
     field_type tildeRho = std::pow(rho, d);
     auto sumAlphaTilde = computeA(yPrev, yCurrent, yNext);
     auto val = Kaskade::normVecDiff(yPrev, yCurrent, norm);
     auto lambda = ((1-rho) * tol - val * std::pow(rho, maxSDCIterations)) * (1 - tildeRho) / ((1 - rho) * (1 - std::pow(tildeRho, maxSDCIterations)) * sumAlphaTilde);

     return lambda;
   }

} //end namespace Kaskade

#endif /* KASKADE_TIMESTEPPING_EULERSDC_WORKMODEL_HH_ */


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_not_infinite.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/quarter.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/logical.hpp>

STF_CASE_TPL (" is_not_infinite real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_not_infinite;
  using r_t = decltype(is_not_infinite(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(is_not_infinite(bs::Inf<T>()), r_t(false));
  STF_EQUAL(is_not_infinite(bs::Minf<T>()), r_t(false));
  STF_EQUAL(is_not_infinite(bs::Nan<T>()), r_t(true));
#endif
  STF_EQUAL(is_not_infinite(-bs::Zero<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::Half<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::One<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::Quarter<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::Two<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::Zero<T>()), r_t(true));
}

STF_CASE_TPL (" is_not_infinite signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_not_infinite;
  using r_t = decltype(is_not_infinite(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_not_infinite(bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::One<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::Two<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::Zero<T>()), r_t(true));
}

STF_CASE_TPL (" is_not_infinite _unsigned",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_not_infinite;
  using r_t = decltype(is_not_infinite(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_not_infinite(bs::One<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::Two<T>()), r_t(true));
  STF_EQUAL(is_not_infinite(bs::Zero<T>()), r_t(true));
}

STF_CASE ( "is_not_infinite bool")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_not_infinite;
  using r_t = decltype(is_not_infinite(bool()));

  // return type conformity test
  STF_TYPE_IS(r_t, bool);

  // specific values tests
  STF_EQUAL(is_not_infinite(true), true);
  STF_EQUAL(is_not_infinite(false), true);
}


#define PY_ARRAY_UNIQUE_SYMBOL libpreviewer_ARRAY_API

#include <iostream>
#include <boost/python.hpp>
#include <pyboostcvconverter/pyboostcvconverter.hpp>

namespace libpreviewer {

    using namespace boost::python;

    std::ostream& operator<<(std::ostream& os, const boost::python::object& o) {
        return os << boost::python::extract<std::string>(boost::python::str(o))();
    }

    class Projector {
    private:
        double scale;
        double imageMidWidth;
        double imageMidHeight;
        int previewWidth;
        int previewHeight;
        cv::Mat R_Kinv;
        cv::Mat mapX;
        cv::Mat mapY;

        /**
         * Map backward the final preview image pixel (x,y) to the
         * original equirectangular coords (u,v)
         */
        void mapBackward(double x, double y, double& u, double& v) {
            double x_ = R_Kinv.at<double>(0,0) * x + R_Kinv.at<double>(0,1) * y + R_Kinv.at<double>(0,2);
            double y_ = R_Kinv.at<double>(1,0) * x + R_Kinv.at<double>(1,1) * y + R_Kinv.at<double>(1,2);
            double z_ = R_Kinv.at<double>(2,0) * x + R_Kinv.at<double>(2,1) * y + R_Kinv.at<double>(2,2);

            // project on a spherical map

            // DEBUG
            // std::cout << "(x,y,z) = " << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
            // ENDDEBUG

            u = scale * atan2f(x_, z_) + imageMidWidth;
            v = scale * ((M_PI / 2.0) - acosf(y_ / sqrtf(x_ * x_ + y_ * y_ + z_ * z_))) + imageMidHeight;
        }

    public:
        Projector(int _imageWidth, int _imageHeight, int _previewWidth, int _previewHeight, cv::Mat _R_Kinv) :
            imageMidWidth(static_cast<double>(_imageWidth)/2),
            imageMidHeight(static_cast<double>(_imageHeight)/2),
            previewWidth(_previewWidth),
            previewHeight(_previewHeight),
            R_Kinv(_R_Kinv) {
                scale = imageMidWidth / M_PI;

                // DEBUG
                // std::cout << std::endl;
                // std::cout << "scale = " << scale << std::endl;
                // std::cout << "R_Kinv[0] = " << R_Kinv.at<double>(0,0) << ", " << R_Kinv.at<double>(0,1) << ", " << R_Kinv.at<double>(0,2) << std::endl;
                // std::cout << "R_Kinv[1] = " << R_Kinv.at<double>(1,0) << ", " << R_Kinv.at<double>(1,1) << ", " << R_Kinv.at<double>(1,2) << std::endl;
                // std::cout << "R_Kinv[2] = " << R_Kinv.at<double>(2,0) << ", " << R_Kinv.at<double>(2,1) << ", " << R_Kinv.at<double>(2,2) << std::endl;
                // std::cout << std::endl;
                // ENDDEBUG
            }

        cv::Mat get_map_x() const { return mapX; }
        cv::Mat get_map_y() const { return mapY; }

        void unproject() {
            mapX = cv::Mat(previewHeight, previewWidth, CV_32FC1);
            mapY = cv::Mat(previewHeight, previewWidth, CV_32FC1);

            for (int x = 0; x < previewWidth; ++x) {
                for (int y = 0; y < previewHeight; ++y) {
                    double u,v;
                    mapBackward(static_cast<double>(x), static_cast<double>(y), u, v);
                    // std::cout << "u,v = " << u << "," << v << std::endl;
                    mapX.at<float>(y,x) = static_cast<float>(u);
                    mapY.at<float>(y,x) = static_cast<float>(v);
                }
            }
        }
    };

    /**
     * Unproject. Basic inner matrix product using implicit matrix conversion.
     * @param leftMat left-hand matrix operand
     * @param rightMat right-hand matrix operand
     * @return an NdArray representing the dot-product of the left and right operands
     */
    // cv::Mat unproject(cv::Mat leftMat, cv::Mat rightMat) {
    //     auto c1 = leftMat.cols, r2 = rightMat.rows;
    //     if (c1 != r2) {
    //         PyErr_SetString(PyExc_TypeError,
    //                         "Incompatible sizes for matrix multiplication.");
    //         throw_error_already_set();
    //     }
    //     cv::Mat result = leftMat * rightMat;

    //     return result;
    // }

#if (PY_VERSION_HEX >= 0x03000000)
    static void *init_ar() {
#else
    static void init_ar(){
#endif
        Py_Initialize();

        import_array();
        return NUMPY_IMPORT_ARRAY_RETVAL;
    }

    BOOST_PYTHON_MODULE (libpreviewer) {
        //using namespace XM;
        init_ar();

        //initialize converters
        boost::python::type_info info = boost::python::type_id<cv::Mat>();
        const boost::python::converter::registration* reg = boost::python::converter::registry::query(info);
        if (reg == NULL || (*reg).m_to_python == NULL)  {
            to_python_converter<cv::Mat, libpreviewer::matToNDArrayBoostConverter>();
            libpreviewer::matFromNDArrayBoostConverter();
        }

        //expose module-level functions
        class_<Projector>("Projector", init<int, int, int, int, cv::Mat>())
            .def("unproject", &Projector::unproject)
            .def("get_map_x", &Projector::get_map_x)
            .def("get_map_y", &Projector::get_map_y);
    }

} //end namespace libpreviewer


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 trigonometric toolbox - rem_2pi/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of trigonometric components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 11/02/2011
///
#include <nt2/toolbox/trigonometric/include/functions/rem_2pi.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/toolbox/constant/constant.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>

NT2_TEST_CASE_TPL ( rem_2pi_real__1_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::rem_2pi;
  using nt2::tag::rem_2pi_;
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::call<rem_2pi_(vT)>::type r_t;
  typedef typename nt2::meta::call<rem_2pi_(T)>::type sr_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;

  // specific values tests
  typedef typename boost::fusion::result_of::value_at_c<r_t,0>::type r_t0;
  typedef typename boost::fusion::result_of::value_at_c<r_t,1>::type r_t1;
  {
    r_t res = rem_2pi(nt2::Zero<vT>());
    NT2_TEST_ULP_EQUAL( boost::fusion::get<0>(res)[0], nt2::Zero<r_t0>()[0], 0.5);
    NT2_TEST_ULP_EQUAL( boost::fusion::get<1>(res)[0], nt2::Zero<r_t1>()[0], 0.5);
  }
} // end of test for floating_

NT2_TEST_CASE_TPL ( rem_2pi_targeted,  NT2_SIMD_REAL_TYPES)
{

  using nt2::rem_2pi;
  using nt2::tag::rem_2pi_;
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;

  vT x = 2*nt2::Pi<vT>(), xr, xc;
  rem_2pi(x, xr, xc, nt2::meta::as_<nt2::big_>());
  NT2_TEST_ULP_EQUAL( xr[0], nt2::Zero<vT>()[0], 1.5);
  NT2_TEST_ULP_EQUAL( xc[0], nt2::Zero<vT>()[0], 1.5);

  rem_2pi(x, xr, xc, nt2::meta::as_<nt2::medium_>());
  NT2_TEST_ULP_EQUAL( xr[0], nt2::Zero<vT>()[0], 1.5);
  NT2_TEST_ULP_EQUAL( xc[0], nt2::Zero<vT>()[0], 1.5);

  rem_2pi(x, xr, xc, nt2::meta::as_<nt2::small_>());
  NT2_TEST_ULP_EQUAL( xr[0], nt2::Zero<vT>()[0], 1.5);
  NT2_TEST_ULP_EQUAL( xc[0], nt2::Zero<vT>()[0], 1.5);

  rem_2pi(x, xr, xc, nt2::meta::as_<nt2::very_small_>());
  NT2_TEST_ULP_EQUAL( xr[0], nt2::Zero<vT>()[0], 1.5);
  NT2_TEST_ULP_EQUAL( xc[0], nt2::Zero<vT>()[0], 1.5);
}