///////////////////////////////////////////////////////////////////////////////
//  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_ */


#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


#include <iostream>
#include <unordered_map>
#include <elemental.hpp>
#include <boost/mpi.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <skylark.hpp>

namespace bmpi =  boost::mpi;
namespace bpo = boost::program_options;
namespace skybase = skylark::base;
namespace skysketch =  skylark::sketch;
namespace skynla = skylark::nla;
namespace skyalg = skylark::algorithms;
namespace skyml = skylark::ml;
namespace skyutil = skylark::utility;

struct simple_unweighted_graph_t {

    simple_unweighted_graph_t(const std::string &gf);

    ~simple_unweighted_graph_t() {
        delete[] _out;
    }

    int num_vertices() const { return _num_vertices; }
    int num_edges() const { return _num_edges; }
    int degree(int vertex) const { return _nodepairs.at(vertex).first; }
    const int *adjanct(int vertex) const { return _nodepairs.at(vertex).second; }

private:
    typedef std::pair<int, int *> nodepair_t;

    std::unordered_map<int, nodepair_t> _nodepairs;
    int *_out;
    int _num_vertices;
    int _num_edges;
};

simple_unweighted_graph_t::simple_unweighted_graph_t(const std::string &gf) {

    std::ifstream in(gf);
    std::string line, token;

    _num_edges = 0;
    while(true) {
        getline(in, line);
        if (in.eof())
            break;
        if (line[0] == '#')
            continue;

        std::istringstream tokenstream(line);
        tokenstream >> token;
        int i = atoi(token.c_str());
        tokenstream >> token;
        int j = atoi(token.c_str());

        if (i == j)
            continue;

        _nodepairs[i].first++;
        _nodepairs[j].first++;
        _num_edges += 2;
    }

    _num_vertices = _nodepairs.size();

    std::cout << "Finished first pass. Vertices = " << _num_vertices
              << " Edges = " << _num_edges << std::endl;
    _out = new int[_num_edges];

    // Set pointers and zero degrees.
    int count = 0;
    for(auto it = _nodepairs.begin(); it != _nodepairs.end(); it++) {
        int nodeid = it->first;
        int deg = it->second.first;
        _nodepairs[nodeid] = nodepair_t(0, _out + count);
        count += deg;
    }

    // Second pass
    in.clear();
    in.seekg(0, std::ios::beg);
    while(!in.eof()) {
        getline(in, line);
        if (line[0] == '#')
            continue;

        std::istringstream tokenstream(line);
        tokenstream >> token;
        int i = atoi(token.c_str());
        tokenstream >> token;
        int j = atoi(token.c_str());

        if (i == j)
            continue;

        nodepair_t &npi = _nodepairs[i];
        npi.second[npi.first] = j;
        npi.first++;

        nodepair_t &npj = _nodepairs[j];
        npj.second[npj.first] = i;
        npj.first++;
    }

    std::cout << "Finished reading... ";
    in.close();
}

int main(int argc, char** argv) {

    elem::Initialize(argc, argv);

    boost::mpi::timer timer;

    // Parse options
    double gamma, alpha, epsilon;
    bool recursive, interactive;
    std::string graphfile, indexfile;
    std::vector<std::string> seedss;
    std::vector<int> seeds;
    bpo::options_description
        desc("Options:");
    desc.add_options()
        ("help,h", "produce a help message")
        ("graphfile,g",
            bpo::value<std::string>(&graphfile),
            "File holding the graph. REQUIRED.")
        ("indexfile,d",
            bpo::value<std::string>(&indexfile)->default_value(""),
            "Index files mapping node-ids to strings. OPTIONAL.")
        ("interactive,i", "Whether to run in interactive mode.")
        ("seed,s",
            bpo::value<std::vector<std::string> >(&seedss),
            "Seed node. Use multiple times for multiple seeds. REQUIRED. ")
        ("recursive,r",
            bpo::value<bool>(&recursive)->default_value(true),
            "Whether to try to recursively improve clusters "
            "(use cluster found as a seed)" )
        ("gamma",
            bpo::value<double>(&gamma)->default_value(5.0),
            "Time to derive the diffusion. As gamma->inf we get closer to ppr.")
        ("alpha",
            bpo::value<double>(&alpha)->default_value(0.85),
            "PPR component parameter. alpha=1 will result in pure heat-kernel.")
        ("epsilon",
            bpo::value<double>(&epsilon)->default_value(0.001),
            "Accuracy parameter for convergence.");

    bpo::variables_map vm;
    try {
        bpo::store(bpo::command_line_parser(argc, argv)
            .options(desc).run(), vm);

        if (vm.count("help")) {
            std::cout << desc;
            return 0;
        }

        interactive = vm.count("interactive");

        if (!vm.count("graphfile")) {
            std::cout << "Input graph-file is required." << std::endl;
            return -1;
        }

        if (!interactive && !vm.count("seed")) {
            std::cout << "A seed is required in non-interactive mode."
                      << std::endl;
            return -1;
        }

        bpo::notify(vm);
    } catch(bpo::error& e) {
        std::cerr << e.what() << std::endl;
        std::cerr << desc << std::endl;
        return -1;
    }

    std::cout << "Reading the adjacency matrix... " << std::endl;
    std::cout.flush();
    timer.restart();
    simple_unweighted_graph_t G(graphfile);
    std::cout <<"took " << boost::format("%.2e") % timer.elapsed() << " sec\n";

    bool use_index = !indexfile.empty();
    std::unordered_map<int, std::string> id_to_name_map;
    std::unordered_map<std::string, int> name_to_id_map;
    if (use_index) {
        std::cout << "Reading index files... ";
        std::cout.flush();
        timer.restart();

        std::ifstream in(indexfile);
        std::string line, token;

        while(true) {
            getline(in, line);
            if (in.eof())
                break;

            if (line[0] == '#')
                continue;

            std::istringstream tokenstream(line);
            tokenstream >> token;
            std::string name = token;
            tokenstream >> token;
            int node = atoi(token.c_str());

            id_to_name_map[node] = name;
            name_to_id_map[name] = node;
        }

        in.close();

        std::cout <<"took " << boost::format("%.2e") % timer.elapsed() << " sec\n";
    }

    do {
        if (interactive) {
            std::cout << "Please input seeds: ";
            std::string line;
            std::getline(std::cin, line);
            if (line.empty())
                break;

            seeds.clear();
            std::stringstream strs(line);
            if (use_index) {
                std::string seed;
                int c = 0;
                while (strs >> seed) {
                    seeds.push_back(name_to_id_map[seed]);
                    c++;
                    if (c == 200)
                        exit(-1);
                }
            } else {
                int seed;
                int c = 0;
                while(strs >> seed) {
                    seeds.push_back(seed);
                    c++;
                    if (c == 200)
                        exit(-1);
                }
            }
        } else {
            for(auto it = seedss.begin(); it != seedss.end(); it++)
                if (use_index)
                    seeds.push_back(name_to_id_map[*it]);
                else
                    seeds.push_back(atoi(it->c_str()));
        }

        timer.restart();
        std::vector<int> cluster;
        double cond = skyml::FindLocalCluster(G, seeds, cluster,
            alpha, gamma, epsilon, recursive);
        std::cout <<"Analysis complete! Took "
                  << boost::format("%.2e") % timer.elapsed() << " sec\n";
        std::cout << "Cluster found:" << std::endl;
        for (auto it = cluster.begin(); it != cluster.end(); it++)
            if (use_index)
                std::cout << id_to_name_map[*it] << std::endl;
            else
                std::cout << *it << " ";
        if (!use_index)
            std::cout << std::endl;
        std::cout << "Conductivity = " << cond << std::endl;
    } while (interactive);

    return 0;
}


#ifndef STAN_MATH_REV_MAT_FUN_QUAD_FORM_SYM_HPP
#define STAN_MATH_REV_MAT_FUN_QUAD_FORM_SYM_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/typedefs.hpp>
#include <stan/math/rev/mat/fun/typedefs.hpp>
#include <stan/math/prim/mat/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/quad_form.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/rev/mat/fun/quad_form.hpp>

namespace stan {
  namespace math {

    template <typename TA, int RA, int CA, typename TB, int RB, int CB>
    inline typename
    boost::enable_if_c< boost::is_same<TA, var>::value ||
    boost::is_same<TB, var>::value,
                        Eigen::Matrix<var, CB, CB> >::type
      quad_form_sym(const Eigen::Matrix<TA, RA, CA>& A,
                    const Eigen::Matrix<TB, RB, CB>& B) {
      check_square("quad_form", "A", A);
      check_symmetric("quad_form_sym", "A", A);
      check_multiplicable("quad_form_sym",
                          "A", A,
                          "B", B);

      quad_form_vari<TA, RA, CA, TB, RB, CB> *baseVari
        = new quad_form_vari<TA, RA, CA, TB, RB, CB>(A, B, true);

      return baseVari->impl_->C_;
    }
    template <typename TA, int RA, int CA, typename TB, int RB>
    inline typename
    boost::enable_if_c< boost::is_same<TA, var>::value ||
    boost::is_same<TB, var>::value,
                        var >::type
      quad_form_sym(const Eigen::Matrix<TA, RA, CA>& A,
                    const Eigen::Matrix<TB, RB, 1>& B) {
      check_square("quad_form", "A", A);
      check_symmetric("quad_form_sym", "A", A);
      check_multiplicable("quad_form_sym",
                          "A", A,
                          "B", B);

      quad_form_vari<TA, RA, CA, TB, RB, 1> *baseVari
        = new quad_form_vari<TA, RA, CA, TB, RB, 1>(A, B, true);

      return baseVari->impl_->C_(0, 0);
    }

  }
}
#endif


// Copyright 2015-2022 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

#pragma once

/// @file
/// Code implementing the full BTE solution by the Omini-Sparavigna
/// iterative method, as implemented in ShengBTE.

#include <Eigen/Dense>
#include <processes.hpp>
#include <isotopic_scattering.hpp>

namespace alma {
/// Class implementing an iterative solution to the BTE following
/// the scheme devised by Omini and Sparavigna.
class ShengBTE_iterator {
private:
    /// Number of points in the grid.
    const std::size_t nqpoints;
    /// Number of irreducible q points.
    const std::size_t nirred;
    /// Number of branches.
    const std::size_t nbranches;
    /// Volume of the unit cell.
    const double V;
    /// Iteration number.
    std::size_t n;
    /// Angular frequencies for all points on the grid [rad / ps].
    Eigen::MatrixXd omega;
    /// Velocities for all points on the grid [nm / ps].
    Eigen::MatrixXd vg;
    /// Lifetimes for all points on the grid [nm / ps]
    Eigen::MatrixXd tau0;
    /// Value of the intermediate quantity F, measuring the distance to
    /// equilibrium, for the current iteration.
    Eigen::MatrixXd F;
    /// MPI communicator used to coordinate different processes.
    const boost::mpi::communicator& comm;

public:
    /// Basic constructor to initialize F.
    ///
    /// @param[in] poscar - description of the unit cell
    /// @param[in] grid - phonon spectrum on a regular q-point grid
    /// @param[in] syms - symmetry operations object
    /// @param[inout] threeph_procs - list of 3-phonon processes
    /// @param[inout] twoph_procs - list of 2-phonon processes
    /// @param[in] T - temperature in K
    /// @param[in] comm - communicator used to synchronize all processes
    ShengBTE_iterator(const Crystal_structure& poscar,
                      const Gamma_grid& grid,
                      const Symmetry_operations& syms,
                      std::vector<Threeph_process>& threeph_procs,
                      std::vector<Twoph_process>& twoph_procs,
                      double T,
                      const boost::mpi::communicator& comm_);

    /// Advance to the next iteration.
    ///
    /// @param[in] poscar - description of the unit cell
    /// @param[in] grid - phonon spectrum on a regular q-point grid
    /// @param[in] syms - symmetry operations object
    /// @param[inout] threeph_procs - list of 3-phonon processes
    /// @param[inout] twoph_procs - list of 2-phonon processes
    /// @param[in] T - temperature in K
    void next(const Crystal_structure& poscar,
              const Gamma_grid& grid,
              const Symmetry_operations& syms,
              std::vector<Threeph_process>& threeph_procs,
              std::vector<Twoph_process>& twoph_procs,
              double T);

    /// Get the current estimate of the thermal conductivity tensor.
    ///
    /// @param[in] T - temperature in K
    /// @return a 3x3 matrix with all the components of kappa
    /// [W / (m K)]
    Eigen::Matrix3d calc_current_kappa(double T) const;

    /// Get the current estimate of the contribution to the thermal conductivity
    /// tensor from a single branch.
    ///
    /// In this context, a branch is defined as the set of modes with the same
    /// index
    /// when energies are sorted in ascending order at each q point.
    /// @param[in] T - temperature in K
    /// @param[in] branch - branch index
    /// @return a 3x3 matrix with all the components of kappa
    /// [W / (m K)]
    Eigen::Matrix3d calc_current_kappa_branch(double T,
                                              std::size_t branch) const;

    /// Obtain the cumulative histogram of contributions to the thermal
    /// conductivity as a function of angular frequency.
    ///
    /// @param[in] T - temperature in K
    /// @param[ticks] - upper edges of the histogram bins
    /// @return a std::vector of thermal conductivity tensors with
    /// the cumulative histogram
    std::vector<Eigen::Matrix3d> calc_cumulative_kappa_omega(
        double T,
        Eigen::ArrayXd ticks);

    /// Obtain the cumulative histogram of contributions to the thermal
    /// conductivity as a function of pseudo-mean free path.
    ///
    /// @param[in] T - temperature in K
    /// @param[ticks] - upper edges of the histogram bins
    /// @return a std::vector of thermal conductivity tensors with
    /// the cumulative histogram
    std::vector<Eigen::Matrix3d> calc_cumulative_kappa_lambda(
        double T,
        Eigen::ArrayXd ticks);

    /// Obtain a set of pseudo-scattering rates from the current estimate of the
    /// solution.
    ///
    /// Note that these are not the inverse of any real relaxation time, since
    /// no such thing exist in the full linearized BTE formalism for phonons.
    /// Substituting these into the RTA expression for kappa will not yield
    /// the right thermal conductivity. Moreover, some elements can be negative.
    /// See the ShengBTE paper for details.
    /// @return an array of pseudo-scattering rates for each q point and each
    /// mode [ps ** (-1)]
    Eigen::ArrayXXd calc_w() const;

    /// Obtain a set of pseudo-mean free paths from the current estimate of the
    /// solution.
    ///
    /// Note that these are not real MFPs, since no such thing exist in the full
    /// linearized BTE formalism for phonons. Substituting these into the RTA
    /// expression for kappa will not yield the right thermal conductivity.
    /// Moreover, some elements can be negative.See the ShengBTE paper for
    /// details.
    /// @return an array of pseudo-scattering rates for each q point and each
    /// mode [ps ** (-1)]
    Eigen::ArrayXXd calc_lambda() const;
};

/// Compute the full thermal conductivity tensor use the
/// Omini-Sparavigna
/// iterative approach.
///
/// @param[in] poscar - description of the unit cell
/// @param[in] grid - phonon spectrum on a regular q-point grid
/// @param[in] syms - symmetry operations object
/// @param[inout] threeph_procs - list of 3-phonon processes
/// @param[inout] twoph_procs - list of 2-phonon processes
/// @param[in] T - temperature in K
/// @param[in] comm - communicator used to synchronize all processes
/// @param[in] tolerance - maximum change in the norm betwwen iterations
/// used as the convergence criterion
/// @param[in] maxiter - maximum number of iterations before giving up
/// @return the thermal conductivity tensor in SI units
Eigen::MatrixXd calc_shengbte_kappa(
    const alma::Crystal_structure& poscar,
    const alma::Gamma_grid& grid,
    const alma::Symmetry_operations& syms,
    std::vector<alma::Threeph_process>& threeph_procs,
    std::vector<alma::Twoph_process>& twoph_procs,
    double T,
    const boost::mpi::communicator& comm,
    double tolerance = 1e-4,
    std::size_t maxiter = 1000);
} // namespace alma