//  duration.hpp  --------------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009-2010 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*

This code was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.9 Time utilities [time] of the C++ committee's working paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

#ifndef BOOST_EX_CHRONO_DURATION_HPP
#define BOOST_EX_CHRONO_DURATION_HPP

#include "config.hpp"
#include "static_assert.hpp"

//~ #include <iostream>

#include <climits>
#include <limits>

#include <boost/mpl/logical.hpp>
#include <boost/ratio/ratio.hpp>
#include <boost/type_traits/common_type.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/type_traits/is_unsigned.hpp>
#include <boost/type_traits/is_arithmetic.hpp>

#include <boost/cstdint.hpp>
#if (defined(BOOST_MSVC) && (BOOST_MSVC == 1500)) || defined(__IBMCPP__)
#else
#include <boost/utility/enable_if.hpp>
#endif
#include <boost/detail/workaround.hpp>
#include <boost/integer_traits.hpp>

#if !defined(BOOST_NO_STATIC_ASSERT) || !defined(BOOST_EX_CHRONO_USES_MPL_ASSERT)
#define BOOST_EX_CHRONO_A_DURATION_REPRESENTATION_CAN_NOT_BE_A_DURATION        "A duration representation can not be a duration"
#define BOOST_EX_CHRONO_SECOND_TEMPLATE_PARAMETER_OF_DURATION_MUST_BE_A_STD_RATIO "Second template parameter of duration must be a boost::ratio"
#define BOOST_EX_CHRONO_DURATION_PERIOD_MUST_BE_POSITIVE "duration period must be positive"
#define BOOST_EX_CHRONO_SECOND_TEMPLATE_PARAMETER_OF_TIME_POINT_MUST_BE_A_BOOST_EX_CHRONO_DURATION "Second template parameter of time_point must be a boost_ex::chrono::duration"
#endif

//----------------------------------------------------------------------------//
//                                                                            //
//                        20.9 Time utilities [time]                          //
//                                 synopsis                                   //
//                                                                            //
//----------------------------------------------------------------------------//

namespace boost_ex {
    using boost::ratio;

namespace chrono {

  template <class Rep, class Period = ratio<1> >
    class duration;

    namespace detail
    {
    template <class T>
      struct is_duration
        : boost::false_type {};

    template <class Rep, class Period>
      struct is_duration<duration<Rep, Period> >
        : boost::true_type  {};
    //template <class T>
    //  struct is_duration
    //    : is_duration<typename boost::remove_cv<T>::type> {};

    template <class Duration, class Rep, bool = is_duration<Rep>::value>
    struct duration_divide_result
    {
    };

    template <class Duration, class Rep2,
        bool = (
                    (boost::is_convertible<typename Duration::rep,
                        typename boost::common_type<typename Duration::rep, Rep2>::type>::value)
                &&  (boost::is_convertible<Rep2,
                        typename boost::common_type<typename Duration::rep, Rep2>::type>::value)
                )
        >
    struct duration_divide_imp
    {
    };

    template <class Rep1, class Period, class Rep2>
    struct duration_divide_imp<duration<Rep1, Period>, Rep2, true>
    {
        typedef duration<typename boost::common_type<Rep1, Rep2>::type, Period> type;
    };

    template <class Rep1, class Period, class Rep2>
    struct duration_divide_result<duration<Rep1, Period>, Rep2, false>
        : duration_divide_imp<duration<Rep1, Period>, Rep2>
    {
    };

///
    template <class Rep, class Duration, bool = is_duration<Rep>::value>
    struct duration_divide_result2
    {
    };

    template <class Rep, class Duration,
        bool = (
                    (boost::is_convertible<typename Duration::rep,
                        typename boost::common_type<typename Duration::rep, Rep>::type>::value)
                &&  (boost::is_convertible<Rep,
                        typename boost::common_type<typename Duration::rep, Rep>::type>::value)
                )
        >
    struct duration_divide_imp2
    {
    };

    template <class Rep1, class Rep2, class Period >
    struct duration_divide_imp2<Rep1, duration<Rep2, Period>, true>
    {
        //typedef typename boost::common_type<Rep1, Rep2>::type type;
        typedef double type;
    };

    template <class Rep1, class Rep2, class Period >
    struct duration_divide_result2<Rep1, duration<Rep2, Period>, false>
        : duration_divide_imp2<Rep1, duration<Rep2, Period> >
    {
    };

///
    template <class Duration, class Rep, bool = is_duration<Rep>::value>
    struct duration_modulo_result
    {
    };

    template <class Duration, class Rep2,
        bool = (
                    //boost::is_convertible<typename Duration::rep,
                        //typename boost::common_type<typename Duration::rep, Rep2>::type>::value
                //&&
    boost::is_convertible<Rep2,
                        typename boost::common_type<typename Duration::rep, Rep2>::type>::value
                )
        >
    struct duration_modulo_imp
    {
    };

    template <class Rep1, class Period, class Rep2>
    struct duration_modulo_imp<duration<Rep1, Period>, Rep2, true>
    {
        typedef duration<typename boost::common_type<Rep1, Rep2>::type, Period> type;
    };

    template <class Rep1, class Period, class Rep2>
    struct duration_modulo_result<duration<Rep1, Period>, Rep2, false>
        : duration_modulo_imp<duration<Rep1, Period>, Rep2>
    {
    };

  } // namespace detail
} // namespace chrono
}

namespace boost {
// common_type trait specializations

template <class Rep1, class Period1, class Rep2, class Period2>
  struct common_type<boost_ex::chrono::duration<Rep1, Period1>,
                     boost_ex::chrono::duration<Rep2, Period2> >;

}
namespace boost_ex {

namespace chrono {

  // customization traits
  template <class Rep> struct treat_as_floating_point;
  template <class Rep> struct duration_values;

  // duration arithmetic
//  template <class Rep1, class Period1, class Rep2, class Period2>
//    typename common_type<duration<Rep1, Period1>, duration<Rep2, Period2> >::type
//    operator+(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs);
//  template <class Rep1, class Period1, class Rep2, class Period2>
//    typename common_type<duration<Rep1, Period1>, duration<Rep2, Period2> >::type
//    operator-(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs);
//  template <class Rep1, class Period, class Rep2>
//    typename boost::enable_if_c
//    <
//      boost::is_convertible<Rep1, typename common_type<Rep1, Rep2>::type>::value
//        && boost::is_convertible<Rep2, typename common_type<Rep1, Rep2>::type>::value,
//      duration<typename common_type<Rep1, Rep2>::type, Period>
//    >::type
//   operator*(const duration<Rep1, Period>& d, const Rep2& s);
//  template <class Rep1, class Period, class Rep2>
//    typename boost::enable_if_c
//    <
//      boost::is_convertible<Rep1, typename common_type<Rep1, Rep2>::type>::value
//        && boost::is_convertible<Rep2, typename common_type<Rep1, Rep2>::type>::value,
//      duration<typename common_type<Rep1, Rep2>::type, Period>
//    >::type
//    operator*(const Rep1& s, const duration<Rep2, Period>& d);

//  template <class Rep1, class Period, class Rep2>
//    typename boost::disable_if <detail::is_duration<Rep2>,
//      typename detail::duration_divide_result<duration<Rep1, Period>, Rep2>::type
//    >::type
//    operator/(const duration<Rep1, Period>& d, const Rep2& s);

//  template <class Rep1, class Period1, class Rep2, class Period2>
//    typename common_type<Rep1, Rep2>::type
//    operator/(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs);

  // duration comparisons
//  template <class Rep1, class Period1, class Rep2, class Period2>
//    bool operator==(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs);
//  template <class Rep1, class Period1, class Rep2, class Period2>
//    bool operator!=(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs);
//  template <class Rep1, class Period1, class Rep2, class Period2>
//    bool operator< (const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs);
//  template <class Rep1, class Period1, class Rep2, class Period2>
//    bool operator<=(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs);
//  template <class Rep1, class Period1, class Rep2, class Period2>
//    bool operator> (const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs);
//  template <class Rep1, class Period1, class Rep2, class Period2>
//    bool operator>=(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs);

  // duration_cast

  //template <class ToDuration, class Rep, class Period>
  //  ToDuration duration_cast(const duration<Rep, Period>& d);

  // convenience typedefs
  typedef duration<boost::int_least64_t, boost::nano> nanoseconds;    // at least 64 bits needed
  typedef duration<boost::int_least64_t, boost::micro> microseconds;  // at least 55 bits needed
  typedef duration<boost::int_least64_t, boost::milli> milliseconds;  // at least 45 bits needed
  typedef duration<boost::int_least64_t> seconds;              // at least 35 bits needed
  typedef duration<boost::int_least32_t, ratio< 60> > minutes; // at least 29 bits needed
  typedef duration<boost::int_least32_t, ratio<3600> > hours;  // at least 23 bits needed

//----------------------------------------------------------------------------//
//                          duration helpers                                  //
//----------------------------------------------------------------------------//

  namespace detail
  {

    // duration_cast

    // duration_cast is the heart of this whole prototype.  It can convert any
    //   duration to any other.  It is also (implicitly) used in converting
    //   time_points.  The conversion is always exact if possible.  And it is
    //   always as efficient as hand written code.  If different representations
    //   are involved, care is taken to never require implicit conversions.
    //   Instead static_cast is used explicitly for every required conversion.
    //   If there are a mixture of integral and floating point representations,
    //   the use of common_type ensures that the most logical "intermediate"
    //   representation is used.
    template <class FromDuration, class ToDuration,
              class Period = typename boost::ratio_divide<typename FromDuration::period,
              typename ToDuration::period>::type,
              bool = Period::num == 1,
              bool = Period::den == 1>
    struct duration_cast;

    // When the two periods are the same, all that is left to do is static_cast from
    //   the source representation to the target representation (which may be a no-op).
    //   This conversion is always exact as long as the static_cast from the source
    //   representation to the destination representation is exact.
    template <class FromDuration, class ToDuration, class Period>
    struct duration_cast<FromDuration, ToDuration, Period, true, true>
    {
        ToDuration operator()(const FromDuration& fd) const
        {
            return ToDuration(static_cast<typename ToDuration::rep>(fd.count()));
        }
    };

    // When the numerator of FromPeriod / ToPeriod is 1, then all we need to do is
    //   divide by the denominator of FromPeriod / ToPeriod.  The common_type of
    //   the two representations is used for the intermediate computation before
    //   static_cast'ing to the destination.
    //   This conversion is generally not exact because of the division (but could be
    //   if you get lucky on the run time value of fd.count()).
    template <class FromDuration, class ToDuration, class Period>
    struct duration_cast<FromDuration, ToDuration, Period, true, false>
    {
        ToDuration operator()(const FromDuration& fd) const
        {
            typedef typename boost::common_type<
                typename ToDuration::rep,
                typename FromDuration::rep,
                boost::intmax_t>::type C;
            return ToDuration(static_cast<typename ToDuration::rep>(
                              static_cast<C>(fd.count()) / static_cast<C>(Period::den)));
        }
    };

    // When the denomenator of FromPeriod / ToPeriod is 1, then all we need to do is
    //   multiply by the numerator of FromPeriod / ToPeriod.  The common_type of
    //   the two representations is used for the intermediate computation before
    //   static_cast'ing to the destination.
    //   This conversion is always exact as long as the static_cast's involved are exact.
    template <class FromDuration, class ToDuration, class Period>
    struct duration_cast<FromDuration, ToDuration, Period, false, true>
    {
        ToDuration operator()(const FromDuration& fd) const
        {
            typedef typename boost::common_type<
              typename ToDuration::rep,
              typename FromDuration::rep,
              boost::intmax_t>::type C;
            return ToDuration(static_cast<typename ToDuration::rep>(
                              static_cast<C>(fd.count()) * static_cast<C>(Period::num)));
        }
    };

    // When neither the numerator or denominator of FromPeriod / ToPeriod is 1, then we need to
    //   multiply by the numerator and divide by the denominator of FromPeriod / ToPeriod.  The
    //   common_type of the two representations is used for the intermediate computation before
    //   static_cast'ing to the destination.
    //   This conversion is generally not exact because of the division (but could be
    //   if you get lucky on the run time value of fd.count()).
    template <class FromDuration, class ToDuration, class Period>
    struct duration_cast<FromDuration, ToDuration, Period, false, false>
    {
        ToDuration operator()(const FromDuration& fd) const
        {
            typedef typename boost::common_type<
              typename ToDuration::rep,
              typename FromDuration::rep,
              boost::intmax_t>::type C;
            return ToDuration(static_cast<typename ToDuration::rep>(
               static_cast<C>(fd.count()) * static_cast<C>(Period::num)
                 / static_cast<C>(Period::den)));
        }
    };

  } // namespace detail

//----------------------------------------------------------------------------//
//                                                                            //
//      20.9.2 Time-related traits [time.traits]                              //
//                                                                            //
//----------------------------------------------------------------------------//
//----------------------------------------------------------------------------//
//      20.9.2.1 treat_as_floating_point [time.traits.is_fp]                        //
//      Probably should have been treat_as_floating_point. Editor notifed.    //
//----------------------------------------------------------------------------//

  // Support bidirectional (non-exact) conversions for floating point rep types
  //   (or user defined rep types which specialize treat_as_floating_point).
  template <class Rep>
    struct treat_as_floating_point : boost::is_floating_point<Rep> {};

//----------------------------------------------------------------------------//
//      20.9.2.2 duration_values [time.traits.duration_values]                //
//----------------------------------------------------------------------------//

  namespace detail {
    template <class T, bool = boost::is_arithmetic<T>::value>
    struct chrono_numeric_limits {
        static T lowest() throw() {return (std::numeric_limits<T>::min)  ();}
    };

    template <class T>
    struct chrono_numeric_limits<T,true> {
        static T lowest() throw() {return (std::numeric_limits<T>::min)  ();}
    };

    template <>
    struct chrono_numeric_limits<float,true> {
        static float lowest() throw() {return -(std::numeric_limits<float>::max) ();}
    };

    template <>
    struct chrono_numeric_limits<double,true> {
        static double lowest() throw() {return -(std::numeric_limits<double>::max) ();}
    };

    template <>
    struct chrono_numeric_limits<long double,true> {
        static long double lowest() throw() {return -(std::numeric_limits<long double>::max)();}
    };

    template <class T>
    struct numeric_limits : chrono_numeric_limits<typename boost::remove_cv<T>::type> {};

  }
  template <class Rep>
  struct duration_values
  {
      static  Rep zero() {return Rep(0);}
      static  Rep max BOOST_PREVENT_MACRO_SUBSTITUTION ()  {return (std::numeric_limits<Rep>::max)();}

      static  Rep min BOOST_PREVENT_MACRO_SUBSTITUTION ()  {return detail::numeric_limits<Rep>::lowest();}
  };

}  // namespace chrono
}
//----------------------------------------------------------------------------//
//      20.9.2.3 Specializations of common_type [time.traits.specializations] //
//----------------------------------------------------------------------------//
namespace boost {
template <class Rep1, class Period1, class Rep2, class Period2>
struct common_type<boost_ex::chrono::duration<Rep1, Period1>,
                   boost_ex::chrono::duration<Rep2, Period2> >
{
  typedef boost_ex::chrono::duration<typename common_type<Rep1, Rep2>::type,
                      typename boost::ratio_gcd<Period1, Period2>::type> type;
};

}
//----------------------------------------------------------------------------//
//                                                                            //
//         20.9.3 Class template duration [time.duration]                     //
//                                                                            //
//----------------------------------------------------------------------------//

namespace boost_ex {

namespace chrono {

    template <class Rep, class Period>
    class duration
    {
    BOOST_EX_CHRONO_STATIC_ASSERT(!boost_ex::chrono::detail::is_duration<Rep>::value, BOOST_EX_CHRONO_A_DURATION_REPRESENTATION_CAN_NOT_BE_A_DURATION, ());
    BOOST_EX_CHRONO_STATIC_ASSERT(boost::ratio_detail::is_ratio<Period>::value, BOOST_EX_CHRONO_SECOND_TEMPLATE_PARAMETER_OF_DURATION_MUST_BE_A_STD_RATIO, ());
    BOOST_EX_CHRONO_STATIC_ASSERT(Period::num>0, BOOST_EX_CHRONO_DURATION_PERIOD_MUST_BE_POSITIVE, ());
    public:
        typedef Rep rep;
        typedef Period period;
    private:
        rep rep_;
    public:

         duration() { } // = default;
        template <class Rep2>
         explicit duration(const Rep2& r
#if (defined(BOOST_MSVC) && (BOOST_MSVC == 1500)) || defined(__IBMCPP__)
#else
	 , typename boost::enable_if <
                    boost::mpl::and_ <
                        boost::is_convertible<Rep2, rep>,
                        boost::mpl::or_ <
                            treat_as_floating_point<rep>,
                            boost::mpl::and_ <
                                boost::mpl::not_ < treat_as_floating_point<rep> >,
                                boost::mpl::not_ < treat_as_floating_point<Rep2> >
                            >
                        >
                    >
                >::type* = 0
#endif
	 )
                  : rep_(r) { }
        ~duration() {} //= default;
        duration(const duration& rhs) : rep_(rhs.rep_) {} // = default;
        duration& operator=(const duration& rhs) // = default;
        {
            if (&rhs != this) rep_= rhs.rep_;
            return *this;
        }

        // conversions
        template <class Rep2, class Period2>
         duration(const duration<Rep2, Period2>& d
#if (defined(BOOST_MSVC) && (BOOST_MSVC == 1500)) || defined(__IBMCPP__)
#else
		, typename boost::enable_if <
                    boost::mpl::or_ <
                        treat_as_floating_point<rep>,
                        boost::mpl::and_ <
                            boost::mpl::bool_ < boost::ratio_divide<Period2, period>::type::den == 1>,
                            boost::mpl::not_ < treat_as_floating_point<Rep2> >
                        >
                    >
                >::type* = 0
#endif
		)
//~ #ifdef        __GNUC__
            // GCC 4.2.4 refused to accept a definition at this point,
            // yet both VC++ 9.0 SP1 and Intel ia32 11.0 accepted the definition
            // without complaint. VC++ 9.0 SP1 refused to accept a later definition,
            // although that was fine with GCC 4.2.4 and Intel ia32 11.0. Thus we
            // have to support both approaches.
            //~ ;
//~ #else
            //~ : rep_(chrono::duration_cast<duration>(d).count()) {}
            : rep_(chrono::detail::duration_cast<duration<Rep2, Period2>, duration>()(d).count()) {}
//~ #endif

        // observer

         rep count() const {return rep_;}

        // arithmetic

        duration  operator+() const {return *this;}
        duration  operator-() const {return duration(-rep_);}
        duration& operator++()      {++rep_; return *this;}
        duration  operator++(int)   {return duration(rep_++);}
        duration& operator--()      {--rep_; return *this;}
        duration  operator--(int)   {return duration(rep_--);}

        duration& operator+=(const duration& d) {rep_ += d.count(); return *this;}
        duration& operator-=(const duration& d) {rep_ -= d.count(); return *this;}

        duration& operator*=(const rep& rhs) {rep_ *= rhs; return *this;}
        duration& operator/=(const rep& rhs) {rep_ /= rhs; return *this;}
        duration& operator%=(const rep& rhs) {rep_ %= rhs; return *this;}
        duration& operator%=(const duration& rhs) {rep_ %= rhs.count(); return *this;};
        // 20.9.3.4 duration special values [time.duration.special]

        static  duration zero() {return duration(duration_values<rep>::zero());}
        static  duration min BOOST_PREVENT_MACRO_SUBSTITUTION ()  {return duration((duration_values<rep>::min)());}
        static  duration max BOOST_PREVENT_MACRO_SUBSTITUTION ()  {return duration((duration_values<rep>::max)());}
    };

//----------------------------------------------------------------------------//
//      20.9.3.5 duration non-member arithmetic [time.duration.nonmember]     //
//----------------------------------------------------------------------------//

  // Duration +

  template <class Rep1, class Period1, class Rep2, class Period2>
  inline
  typename boost::common_type<duration<Rep1, Period1>, duration<Rep2, Period2> >::type
  operator+(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs)
  {
      typename boost::common_type<duration<Rep1, Period1>,
        duration<Rep2, Period2> >::type result = lhs;
      result += rhs;
      return result;
  }

  // Duration -

  template <class Rep1, class Period1, class Rep2, class Period2>
  inline
  typename boost::common_type<duration<Rep1, Period1>, duration<Rep2, Period2> >::type
  operator-(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs)
  {
      typename boost::common_type<duration<Rep1, Period1>,
        duration<Rep2, Period2> >::type result = lhs;
      result -= rhs;
      return result;
  }

  // Duration *

  template <class Rep1, class Period, class Rep2>
  inline
#if (defined(BOOST_MSVC) && (BOOST_MSVC == 1500)) || defined(__IBMCPP__)
    duration<typename boost::common_type<Rep1, Rep2>::type, Period>
#else
typename boost::enable_if <
    boost::mpl::and_ <
        boost::is_convertible<Rep1, typename boost::common_type<Rep1, Rep2>::type>,
        boost::is_convertible<Rep2, typename boost::common_type<Rep1, Rep2>::type>
    >,
    duration<typename boost::common_type<Rep1, Rep2>::type, Period>
  >::type
#endif
  operator*(const duration<Rep1, Period>& d, const Rep2& s)
  {
      typedef typename boost::common_type<Rep1, Rep2>::type CR;
      duration<CR, Period> r = d;
      r *= static_cast<CR>(s);
      return r;
  }

  template <class Rep1, class Period, class Rep2>
  inline
#if (defined(BOOST_MSVC) && (BOOST_MSVC == 1500)) || defined(__IBMCPP__)
    duration<typename boost::common_type<Rep1, Rep2>::type, Period>
#else
  typename boost::enable_if <
    boost::mpl::and_ <
        boost::is_convertible<Rep1, typename boost::common_type<Rep1, Rep2>::type>,
        boost::is_convertible<Rep2, typename boost::common_type<Rep1, Rep2>::type>
    >,
    duration<typename boost::common_type<Rep1, Rep2>::type, Period>
  >::type
#endif
  operator*(const Rep1& s, const duration<Rep2, Period>& d)
  {
      return d * s;
  }

  // Duration /

  template <class Rep1, class Period, class Rep2>
  inline
  typename boost::disable_if <boost_ex::chrono::detail::is_duration<Rep2>,
    typename boost_ex::chrono::detail::duration_divide_result<duration<Rep1, Period>, Rep2>::type
  >::type
  operator/(const duration<Rep1, Period>& d, const Rep2& s)
  {
      typedef typename boost::common_type<Rep1, Rep2>::type CR;
      duration<CR, Period> r = d;
      r /= static_cast<CR>(s);
      return r;
  }

  template <class Rep1, class Period1, class Rep2, class Period2>
  inline
  typename boost::common_type<Rep1, Rep2>::type
  operator/(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs)
  {
      typedef typename boost::common_type<duration<Rep1, Period1>,
                                   duration<Rep2, Period2> >::type CD;
      return CD(lhs).count() / CD(rhs).count();
  }

  template <class Rep1, class Rep2, class Period>
  inline
  typename boost::disable_if <boost_ex::chrono::detail::is_duration<Rep1>,
    typename boost_ex::chrono::detail::duration_divide_result2<Rep1, duration<Rep2, Period> >::type
  >::type
  operator/(const Rep1& s, const duration<Rep2, Period>& d)
  {
      typedef typename boost::common_type<Rep1, Rep2>::type CR;
      duration<CR, Period> r = d;
      //return static_cast<CR>(r.count()) / static_cast<CR>(s);
      return  static_cast<CR>(s)/r.count();
  }

  // Duration %

  template <class Rep1, class Period, class Rep2>
  typename boost::disable_if <boost_ex::chrono::detail::is_duration<Rep2>,
    typename boost_ex::chrono::detail::duration_modulo_result<duration<Rep1, Period>, Rep2>::type
  >::type
  operator%(const duration<Rep1, Period>& d, const Rep2& s) {
    typedef typename boost::common_type<Rep1, Rep2>::type CR;
    duration<CR, Period> r = d;
    r %= static_cast<CR>(s);
    return r;
  }

  template <class Rep1, class Period1, class Rep2, class Period2>
  typename boost::common_type<duration<Rep1, Period1>, duration<Rep2, Period2> >::type
  operator%(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs) {
    typedef typename boost::common_type<duration<Rep1, Period1>,
                                 duration<Rep2, Period2> >::type CD;
    CD r(lhs);
    r%=CD(rhs);
    return r;
  }

//----------------------------------------------------------------------------//
//      20.9.3.6 duration comparisons [time.duration.comparisons]             //
//----------------------------------------------------------------------------//

  namespace detail
  {
    template <class LhsDuration, class RhsDuration>
    struct duration_eq
    {
        bool operator()(const LhsDuration& lhs, const RhsDuration& rhs)
            {
                typedef typename boost::common_type<LhsDuration, RhsDuration>::type CD;
                return CD(lhs).count() == CD(rhs).count();
            }
    };

    template <class LhsDuration>
    struct duration_eq<LhsDuration, LhsDuration>
    {
        bool operator()(const LhsDuration& lhs, const LhsDuration& rhs)
            {return lhs.count() == rhs.count();}
    };

    template <class LhsDuration, class RhsDuration>
    struct duration_lt
    {
        bool operator()(const LhsDuration& lhs, const RhsDuration& rhs)
            {
                typedef typename boost::common_type<LhsDuration, RhsDuration>::type CD;
                return CD(lhs).count() < CD(rhs).count();
            }
    };

    template <class LhsDuration>
    struct duration_lt<LhsDuration, LhsDuration>
    {
        bool operator()(const LhsDuration& lhs, const LhsDuration& rhs)
            {return lhs.count() < rhs.count();}
    };

  } // namespace detail

  // Duration ==

  template <class Rep1, class Period1, class Rep2, class Period2>
  inline
  bool
  operator==(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs)
  {
      return boost_ex::chrono::detail::duration_eq<duration<Rep1, Period1>, duration<Rep2, Period2> >()(lhs, rhs);
  }

  // Duration !=

  template <class Rep1, class Period1, class Rep2, class Period2>
  inline
  bool
  operator!=(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs)
  {
      return !(lhs == rhs);
  }

  // Duration <

  template <class Rep1, class Period1, class Rep2, class Period2>
  inline
  bool
  operator< (const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs)
  {
      return boost_ex::chrono::detail::duration_lt<duration<Rep1, Period1>, duration<Rep2, Period2> >()(lhs, rhs);
  }

  // Duration >

  template <class Rep1, class Period1, class Rep2, class Period2>
  inline
  bool
  operator> (const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs)
  {
      return rhs < lhs;
  }

  // Duration <=

  template <class Rep1, class Period1, class Rep2, class Period2>
  inline
  bool
  operator<=(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs)
  {
      return !(rhs < lhs);
  }

  // Duration >=

  template <class Rep1, class Period1, class Rep2, class Period2>
  inline
  bool
  operator>=(const duration<Rep1, Period1>& lhs, const duration<Rep2, Period2>& rhs)
  {
      return !(lhs < rhs);
  }

//----------------------------------------------------------------------------//
//      20.9.3.7 duration_cast [time.duration.cast]                           //
//----------------------------------------------------------------------------//

  // Compile-time select the most efficient algorithm for the conversion...
  template <class ToDuration, class Rep, class Period>
  inline
#if (defined(BOOST_MSVC) && (BOOST_MSVC == 1500)) || defined(__IBMCPP__)
    ToDuration
#else
  typename boost::enable_if <boost_ex::chrono::detail::is_duration<ToDuration>, ToDuration>::type
#endif
  duration_cast(const duration<Rep, Period>& fd)
  {
      return boost_ex::chrono::detail::duration_cast<duration<Rep, Period>, ToDuration>()(fd);
  }

}
}
#endif // BOOST_EX_CHRONO_DURATION_HPP


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Rice University
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Rice University nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Ryan Luna */

#include "ompl/datastructures/AdjacencyList.h"
#include "ompl/datastructures/BinaryHeap.h"
#include "ompl/util/Console.h"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/pending/disjoint_sets.hpp>

#include <boost/graph/incremental_components.hpp>  // needed?

#include <iostream>

using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                                    boost::property<boost::vertex_index_t, int,
                                                    boost::property<boost::vertex_rank_t, int,  // disjoint sets
                                                                    boost::property<boost::vertex_predecessor_t, int>>>,
                                    boost::property<boost::edge_weight_t, double>>;

using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
using Edge = boost::graph_traits<Graph>::edge_descriptor;
using VIterator = boost::graph_traits<Graph>::vertex_iterator;
using EIterator = boost::graph_traits<Graph>::edge_iterator;
using AdjIterator = boost::graph_traits<Graph>::adjacency_iterator;

using DisjointSets = boost::disjoint_sets<boost::property_map<Graph, boost::vertex_rank_t>::type,
                                          boost::property_map<Graph, boost::vertex_predecessor_t>::type>;

#define graph_ reinterpret_cast<Graph *>(graphRaw_)
#define disjointSets_ reinterpret_cast<DisjointSets *>(disjointSetsRaw_)

ompl::AdjacencyList::AdjacencyList()
{
    graphRaw_ = new Graph();
    disjointSetsRaw_ =
        new DisjointSets(boost::get(boost::vertex_rank, *graph_), boost::get(boost::vertex_predecessor, *graph_));
}

ompl::AdjacencyList::AdjacencyList(int n)
{
    graphRaw_ = new Graph(n);
    disjointSetsRaw_ =
        new DisjointSets(boost::get(boost::vertex_rank, *graph_), boost::get(boost::vertex_predecessor, *graph_));

    std::pair<VIterator, VIterator> vRange = boost::vertices(*graph_);
    for (VIterator it = vRange.first; it != vRange.second; ++it)
        disjointSets_->make_set(*it);
}

ompl::AdjacencyList::~AdjacencyList()
{
    delete graph_;
    delete disjointSets_;
    graphRaw_ = NULL;
    disjointSetsRaw_ = NULL;
}

void ompl::AdjacencyList::clear()
{
    boost::mutex::scoped_lock lock(lock_);
    graph_->clear();
}

int ompl::AdjacencyList::addVertex()
{
    boost::mutex::scoped_lock lock(lock_);
    Vertex v = boost::add_vertex(*graph_);
    disjointSets_->make_set(v);

    boost::property_map<Graph, boost::vertex_index_t>::type vertexIndexMap = get(boost::vertex_index, *graph_);
    return vertexIndexMap[v];
}

int ompl::AdjacencyList::numVertices() const
{
    return boost::num_vertices(*graph_);
}

bool ompl::AdjacencyList::vertexExists(int v) const
{
    return v >= 0 && v < numVertices();
}

bool ompl::AdjacencyList::inSameComponent(int v1, int v2) const
{
    if (!vertexExists(v1) || !vertexExists(v2))
        return false;

    return boost::same_component(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *disjointSets_);
}

int ompl::AdjacencyList::numConnectedComponents() const
{
    std::pair<VIterator, VIterator> vRange = boost::vertices(*graph_);
    return disjointSets_->count_sets(vRange.first, vRange.second);
}

int ompl::AdjacencyList::getComponentID(int vtx) const
{
    return disjointSets_->find_set(vtx);
}

bool ompl::AdjacencyList::addEdge(int v1, int v2, double weight)
{
    boost::mutex::scoped_lock lock(lock_);

    // If either of the vertices do not exist, don't add an edge
    if (v1 < 0 || v1 >= numVertices() || v2 < 0 || v2 >= numVertices())
        return false;

    if (edgeExists(v1, v2))
        return false;

    // No self-transitions
    if (v1 == v2)
        return false;

    if (weight < 0)
    {
        std::cout << "weight = " << weight << std::endl;
        throw std::runtime_error("addEdge: Edge weight must be >= 0");
    }

    Edge e;
    bool added = false;
    Vertex vt1 = boost::vertex(v1, *graph_);
    Vertex vt2 = boost::vertex(v2, *graph_);
    tie(e, added) = boost::add_edge(vt1, vt2, *graph_);

    assert(added);  // this should never fail

    // Set edge weight
    boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
    weights[e] = weight;

    disjointSets_->union_set(vt1, vt2);

    return added;
}

bool ompl::AdjacencyList::removeEdge(int v1, int v2)
{
    boost::mutex::scoped_lock lock(lock_);

    Edge e;
    bool exists;
    boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);

    if (exists)
        boost::remove_edge(e, *graph_);
    return exists;
}

int ompl::AdjacencyList::numEdges() const
{
    return boost::num_edges(*graph_);
}

double ompl::AdjacencyList::getEdgeWeight(int v1, int v2) const
{
    Edge e;
    bool exists;
    boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);

    if (exists)
    {
        boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
        return weights[e];
    }

    throw std::runtime_error("Edge does not exist");
}

bool ompl::AdjacencyList::setEdgeWeight(int v1, int v2, double weight)
{
    boost::mutex::scoped_lock lock(lock_);

    if (weight < 0)
    {
        std::cout << "Weight: " << weight << std::endl;
        throw std::runtime_error("setEdgeWeight: Edge weight must be >= 0");
    }

    Edge e;
    bool exists;
    boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);

    if (exists)
    {
        boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);
        weights[e] = weight;
        return true;
    }

    return false;
}

bool ompl::AdjacencyList::edgeExists(int v1, int v2) const
{
    Edge e;
    bool exists;
    boost::tie(e, exists) = boost::edge(boost::vertex(v1, *graph_), boost::vertex(v2, *graph_), *graph_);

    return exists;
}

int ompl::AdjacencyList::numNeighbors(int vtx) const
{
    return boost::degree(boost::vertex(vtx, *graph_), *graph_);
}

void ompl::AdjacencyList::getNeighbors(int vtx, std::vector<int> &nbrs) const
{
    nbrs.resize(numNeighbors(vtx));

    std::pair<AdjIterator, AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(vtx, *graph_), *graph_);
    boost::property_map<Graph, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
    int count = 0;
    for (AdjIterator iter = iterators.first; iter != iterators.second; ++iter, ++count)
        nbrs[count] = vertices[*iter];
}

void ompl::AdjacencyList::getNeighbors(int vtx, std::vector<std::pair<int, double>> &nbrs) const
{
    nbrs.resize(numNeighbors(vtx));

    std::pair<AdjIterator, AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(vtx, *graph_), *graph_);
    boost::property_map<Graph, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
    boost::property_map<Graph, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);

    int count = 0;
    for (AdjIterator iter = iterators.first; iter != iterators.second; ++iter, ++count)
    {
        Edge e;
        bool exists;
        boost::tie(e, exists) = boost::edge(boost::vertex(vtx, *graph_), *iter, *graph_);
        assert(exists);
        nbrs[count] = std::make_pair(vertices[*iter], weights[e]);
    }
}

bool ompl::AdjacencyList::dijkstra(int v1, int v2, std::vector<int> &path) const
{
    std::vector<double> distances;
    std::vector<int> predecessors;
    dijkstra(v1, predecessors, distances);

    // no solution when predecessor of (non-start) vertex is itself
    if (v2 != v1 && predecessors[v2] == v2)
        return false;

    boost::property_map<Graph, boost::vertex_index_t>::type indexMap = get(boost::vertex_index, *graph_);

    path.clear();
    // Extracting solution path
    Vertex v = boost::vertex(v2, *graph_);
    Vertex start = boost::vertex(v1, *graph_);
    while (v != start)
    {
        path.insert(path.begin(), indexMap[v]);
        v = predecessors[v];
    }
    path.insert(path.begin(), indexMap[v]);
    return true;
}

void ompl::AdjacencyList::dijkstra(int vtx, std::vector<int> &predecessors, std::vector<double> &distances) const
{
    // Locking the graph, since modifications to the graph during search are not supported
    // This call is also the reason why lock_ must be mutable
    boost::mutex::scoped_lock lock(lock_);

    distances.resize(numVertices());
    predecessors.resize(numVertices());

    boost::dijkstra_shortest_paths(*graph_, boost::vertex(vtx, *graph_),
                                   boost::predecessor_map(&predecessors[0]).distance_map(&distances[0]));
}


/* boost validate.cpp
 *
 * Copyright Jens Maurer 2000
 * 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)
 *
 * $Id$
 */

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
#pragma warning( disable : 4786 )
#endif

#include <iostream>
#include <sstream>
#include <string>
#include <cmath>
#include <iterator>
#include <vector>
#include <boost/random.hpp>
#include <boost/config.hpp>

#include <boost/test/test_tools.hpp>
#include <boost/test/included/test_exec_monitor.hpp>

#ifdef BOOST_NO_STDC_NAMESPACE
  namespace std { using ::abs; using ::fabs; using ::pow; }
#endif

/*
 * General portability note:
 * MSVC mis-compiles explicit function template instantiations.
 * For example, f<A>() and f<B>() are both compiled to call f<A>().
 * BCC is unable to implicitly convert a "const char *" to a std::string
 * when using explicit function template instantiations.
 *
 * Therefore, avoid explicit function template instantiations.
 */

/*
 * Validate correct implementation
 */

// own run
bool check(unsigned long x, const boost::mt11213b&) { return x == 3809585648U; }

// validation by experiment from mt19937.c
bool check(unsigned long x, const boost::mt19937&) { return x == 4123659995U; }

// validation values from the publications
bool check(int x, const boost::minstd_rand0&) { return x == 1043618065; }

// validation values from the publications
bool check(int x, const boost::minstd_rand&) { return x == 399268537; }

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)
// by experiment from lrand48()
bool check(unsigned long x, const boost::rand48&) { return x == 1993516219; }
#endif

// ????
bool check(unsigned long x, const boost::taus88&) { return x == 3535848941U; }

// ????
bool check(int x, const boost::ecuyer1988&) { return x == 2060321752; }

// validation by experiment from Harry Erwin's generator.h (private e-mail)
bool check(unsigned int x, const boost::kreutzer1986&) { return x == 139726; }

bool check(double x, const boost::lagged_fibonacci607&) { return std::abs(x-0.401269) < 1e-5; }

// principal operation validated with CLHEP, values by experiment
bool check(unsigned long x, const boost::ranlux3&) { return x == 5957620; }
bool check(unsigned long x, const boost::ranlux4&) { return x == 8587295; }

bool check(float x, const boost::ranlux3_01&)
{ return std::abs(x-5957620/std::pow(2.0f,24)) < 1e-6; }
bool check(float x, const boost::ranlux4_01&)
{ return std::abs(x-8587295/std::pow(2.0f,24)) < 1e-6; }

bool check(double x, const boost::ranlux64_3_01&)
{ return std::abs(x-0.838413) < 1e-6; }
bool check(double x, const boost::ranlux64_4_01&)
{ return std::abs(x-0.59839) < 1e-6; }

template<class PRNG>
void validate(const std::string & name, const PRNG &)
{
  std::cout << "validating " << name << ": ";
  PRNG rng;  // default ctor
  for(int i = 0; i < 9999; i++)
    rng();
  typename PRNG::result_type val = rng();
  // make sure the validation function is a static member
  bool result = check(val, rng);

  // allow for a simple eyeball check for MSVC instantiation brokenness
  // (if the numbers for all generators are the same, it's obviously broken)
  std::cout << val << std::endl;
  BOOST_CHECK(result);
}

void validate_all()
{
  using namespace boost;
#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)
  validate("rand48", rand48());
#endif
  validate("minstd_rand", minstd_rand());
  validate("minstd_rand0", minstd_rand0());
  validate("ecuyer combined", ecuyer1988());
  validate("mt11213b", mt11213b());
  validate("mt19937", mt19937());
  validate("kreutzer1986", kreutzer1986());
  validate("ranlux3", ranlux3());
  validate("ranlux4", ranlux4());
  validate("ranlux3_01", ranlux3_01());
  validate("ranlux4_01", ranlux4_01());
  validate("ranlux64_3_01", ranlux64_3_01());
  validate("ranlux64_4_01", ranlux64_4_01());
  validate("taus88", taus88());
  validate("lagged_fibonacci607", lagged_fibonacci607());
}

int test_main(int, char*[])
{
  validate_all();
  return 0;
}


/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/

#include "Pinhole.h"

#include <boost/serialization/export.hpp>

//BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::Pinhole)

namespace ORB_SLAM3 {
//BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")

    long unsigned int GeometricCamera::nNextId=0;

    cv::Point2f Pinhole::project(const cv::Point3f &p3D) {
        return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2],
                           mvParameters[1] * p3D.y / p3D.z + mvParameters[3]);
    }

    cv::Point2f Pinhole::project(const cv::Mat &m3D) {
        const float* p3D = m3D.ptr<float>();

        return this->project(cv::Point3f(p3D[0],p3D[1],p3D[2]));
    }

    Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) {
        Eigen::Vector2d res;
        res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2];
        res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3];

        return res;
    }

    cv::Mat Pinhole::projectMat(const cv::Point3f &p3D) {
        cv::Point2f point = this->project(p3D);
        return (cv::Mat_<float>(2,1) << point.x, point.y);
    }

    float Pinhole::uncertainty2(const Eigen::Matrix<double,2,1> &p2D)
    {
        return 1.0;
    }

    cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) {
        return cv::Point3f((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1],
                           1.f);
    }

    cv::Mat Pinhole::unprojectMat(const cv::Point2f &p2D){
        cv::Point3f ray = this->unproject(p2D);
        return (cv::Mat_<float>(3,1) << ray.x, ray.y, ray.z);
    }

    cv::Mat Pinhole::projectJac(const cv::Point3f &p3D) {
        cv::Mat Jac(2, 3, CV_32F);
        Jac.at<float>(0, 0) = mvParameters[0] / p3D.z;
        Jac.at<float>(0, 1) = 0.f;
        Jac.at<float>(0, 2) = -mvParameters[0] * p3D.x / (p3D.z * p3D.z);
        Jac.at<float>(1, 0) = 0.f;
        Jac.at<float>(1, 1) = mvParameters[1] / p3D.z;
        Jac.at<float>(1, 2) = -mvParameters[1] * p3D.y / (p3D.z * p3D.z);

        return Jac;
    }

    Eigen::Matrix<double, 2, 3> Pinhole::projectJac(const Eigen::Vector3d &v3D) {
        Eigen::Matrix<double, 2, 3> Jac;
        Jac(0, 0) = mvParameters[0] / v3D[2];
        Jac(0, 1) = 0.f;
        Jac(0, 2) = -mvParameters[0] * v3D[0] / (v3D[2] * v3D[2]);
        Jac(1, 0) = 0.f;
        Jac(1, 1) = mvParameters[1] / v3D[2];
        Jac(1, 2) = -mvParameters[1] * v3D[1] / (v3D[2] * v3D[2]);

        return Jac;
    }

    cv::Mat Pinhole::unprojectJac(const cv::Point2f &p2D) {
        cv::Mat Jac(3, 2, CV_32F);
        Jac.at<float>(0, 0) = 1 / mvParameters[0];
        Jac.at<float>(0, 1) = 0.f;
        Jac.at<float>(1, 0) = 0.f;
        Jac.at<float>(1, 1) = 1 / mvParameters[1];
        Jac.at<float>(2, 0) = 0.f;
        Jac.at<float>(2, 1) = 0.f;

        return Jac;
    }

    bool Pinhole::ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12,
                                 cv::Mat &R21, cv::Mat &t21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated){
        if(!tvr){
            cv::Mat K = this->toK();
            tvr = new TwoViewReconstruction(K);
        }

        return tvr->Reconstruct(vKeys1,vKeys2,vMatches12,R21,t21,vP3D,vbTriangulated);
    }

    cv::Mat Pinhole::toK() {
        cv::Mat K = (cv::Mat_<float>(3, 3)
                << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f);
        return K;
    }

    bool Pinhole::epipolarConstrain(GeometricCamera* pCamera2,  const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc) {
        //Compute Fundamental Matrix
        cv::Mat t12x = SkewSymmetricMatrix(t12);
        cv::Mat K1 = this->toK();
        cv::Mat K2 = pCamera2->toK();
        cv::Mat F12 = K1.t().inv()*t12x*R12*K2.inv();

        // Epipolar line in second image l = x1'F12 = [a b c]
        const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
        const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
        const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);

        const float num = a*kp2.pt.x+b*kp2.pt.y+c;

        const float den = a*a+b*b;

        if(den==0)
            return false;

        const float dsqr = num*num/den;

        return dsqr<3.84*unc;
    }

    std::ostream & operator<<(std::ostream &os, const Pinhole &ph) {
        os << ph.mvParameters[0] << " " << ph.mvParameters[1] << " " << ph.mvParameters[2] << " " << ph.mvParameters[3];
        return os;
    }

    std::istream & operator>>(std::istream &is, Pinhole &ph) {
        float nextParam;
        for(size_t i = 0; i < 4; i++){
            assert(is.good());  //Make sure the input stream is good
            is >> nextParam;
            ph.mvParameters[i] = nextParam;

        }
        return is;
    }

    cv::Mat Pinhole::SkewSymmetricMatrix(const cv::Mat &v)
    {
        return (cv::Mat_<float>(3,3) <<             0, -v.at<float>(2), v.at<float>(1),
                v.at<float>(2),               0,-v.at<float>(0),
                -v.at<float>(1),  v.at<float>(0),              0);
    }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IF_ELSE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IF_ELSE_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/traits.hpp>
#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_andnot.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/is_nez.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;

   BOOST_DISPATCH_OVERLOAD_IF ( if_else_
                              , (typename A0,typename A1,typename X)
                              , (detail::is_native<X>)
                              , bd::cpu_
                              , bs::pack_<bs::logical_<A0>,X>
                              , bs::pack_<bd::fundamental_<A1>,X>
                              , bs::pack_<bd::fundamental_<A1>,X>
                              )

  {
    BOOST_FORCEINLINE A1 operator()(const A0& a0, const A1& a1, const A1& a2) const BOOST_NOEXCEPT
    {
      auto m = genmask(a0);
      return bitwise_or(bitwise_and(a1,m), bitwise_andnot(a2,m));
    }
  };

   BOOST_DISPATCH_OVERLOAD_IF ( if_else_
                              , (typename A0,typename A1,typename X)
                              , (detail::is_native<X>)
                              , bd::cpu_
                              , bs::pack_<bd::arithmetic_<A0>,X>
                              , bs::pack_<bd::fundamental_<A1>,X>
                              , bs::pack_<bd::fundamental_<A1>,X>
                              )
  {
    BOOST_FORCEINLINE A1 operator()(const A0& a0, const A1& a1, const A1&a2) const BOOST_NOEXCEPT
    {
      return if_else(is_nez(a0), a1, a2);
    }
  };
} } }

#endif


#include <unistd.h>
#include <sys/time.h>
#include <vector>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <posit/posit>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <omp.h>

// libcxl
extern "C" {
#include "libcxl.h"
}

#include "defines.hpp"
#include "batch.hpp"
#include "utils.hpp"
#include "psl.hpp"
#include "debug_values.hpp"
#include "pairhmm_posit.hpp"
#include "pairhmm_float.hpp"

#define BILLION    1000000000L

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

int main(int argc, char *argv[]) {
    // Times
    double start, stop;
    double t_fill_batch, t_create_core, t_fpga, t_sw, t_float, t_dec = 0.0;

    struct cxl_afu_h *afu;
    void *batch;
    t_result *result_hw;
    t_workload *workload;
    t_batch *batches;

    float f_hw = 125e6;
    float max_cups = f_hw * (float)16;

    unsigned long pairs, x, y = 0;
    int initial_constant_power = 1;
    bool calculate_sw = true;
    bool show_results = false;
    bool show_table = false;

    DEBUG_PRINT("Parsing input arguments...\n");
    if (argc > 4) {
            pairs = strtoul(argv[1], NULL, 0);
            x = strtoul(argv[2], NULL, 0);
            y = strtoul(argv[3], NULL, 0);
            initial_constant_power = strtoul(argv[4], NULL, 0);

            workload = gen_workload(pairs, x, y);

            BENCH_PRINT("M, ");
            BENCH_PRINT("%8d, %8d, %8d, ", workload->pairs, x, y);
    } else {
        fprintf(stderr,
                "ERROR: Correct usage is: %s <pairs> <X> <Y> <initial constant power>\n",
                "pairhmm");
        return (-1);
    }

    DEBUG_PRINT("Total workload bytes: %17d \n", (unsigned int) workload->bytes);
    DEBUG_PRINT("CUPS required       : %17lu \n", workload->cups_req);
    DEBUG_PRINT("Allocating memory for %d batches and %d results...\n", (unsigned int) workload->batches,
                (unsigned int) workload->pairs);

    if (posix_memalign((void **) &batch, CACHELINE_BYTES, workload->bytes)) {
        perror("Could not allocate memory to store the batches.\n");
        return (-1);
    }

    if (posix_memalign((void **) &result_hw, CACHELINE_BYTES, sizeof(t_result) * workload->batches * PIPE_DEPTH)) {
        perror("Could not allocate memory to store hardware results.\n");
        return (-1);
    }

    DEBUG_PRINT("Clearing batch and host result memory ...\n");
    memset(batch, 0x00, workload->bytes);

    DEBUG_PRINT("Filling batches...\n");

    void *batch_cur = batch;

    start = omp_get_wtime();
    batches = (t_batch *) malloc(sizeof(t_batch) * workload->batches);

    // Generate random basepair strings for reads and haplotypes
    std::string x_string = randomBasepairs(16 + pbp(px(x, y)));
    std::string y_string = randomBasepairs(16 + pbp(py(y)));

    for (int q = 0; q < workload->batches; q++) {
        init_batch_address(&batches[q], batch_cur, workload->bx[q], workload->by[q]);
        fill_batch(&batches[q], x_string, y_string, workload->bx[q], workload->by[q], powf(2.0, initial_constant_power));
        // print_batch_info(&batches[q]);
        batch_cur = (void *) ((uint64_t) batch_cur + (uint64_t) workload->bbytes[q]);
    }
    stop = omp_get_wtime();
    t_fill_batch = stop - start;

    PairHMMPosit pairhmm_posit(workload, show_results, show_table);
    PairHMMFloat<float> pairhmm_float(workload, show_results, show_table);
    PairHMMFloat<cpp_dec_float_100> pairhmm_dec50(workload, show_results, show_table);

    if (calculate_sw) {
        DEBUG_PRINT("Calculating on host...\n");

        start = omp_get_wtime();
        pairhmm_posit.calculate(batches);
        stop = omp_get_wtime();
        t_sw = stop - start;

        start = omp_get_wtime();
        pairhmm_float.calculate(batches);
        stop = omp_get_wtime();
        t_float = stop - start;

        start = omp_get_wtime();
        pairhmm_dec50.calculate(batches);
        stop = omp_get_wtime();
        t_dec = stop - start;
    }

    DEBUG_PRINT("Clearing HW result memory\n");
    memset(result_hw, 0xFF, sizeof(t_result) * workload->batches * PIPE_DEPTH);

    DEBUG_PRINT("Opening device: %s ...\n", DEVICE);
    afu = cxl_afu_open_dev((char *) (DEVICE));
    if (!afu) {
        perror("cxl_afu_open_dev");
        return (-1);
    }

    DEBUG_PRINT("Setting up Work Element Descriptor...\n");
    struct wed *wed0 = NULL;
    if (posix_memalign((void **) &(wed0), CACHELINE_BYTES, sizeof(struct wed))) {
        perror("posix_memalign");
        return (-1);
    }

    wed0->status = 0;
    wed0->source = (__u64 *) batch;
    wed0->destination = (__u64 *) result_hw;
    wed0->batch_size = (__u32) 0;
    wed0->pair_size = (__u32) 0;
    wed0->padded_size = (__u32) 0;
    wed0->batches = (__u32) workload->batches;

    DEBUG_PRINT("Attaching to AFU and passing WED address...\n");
    if (cxl_afu_attach(afu, (__u64) wed0) < 0) {
        perror("cxl_afu_attach");
        return (-1);
    }

    DEBUG_PRINT("Mapping MMIO...\n");
    if ((cxl_mmio_map(afu, CXL_MMIO_BIG_ENDIAN)) < 0) {
        perror("cxl_mmio_map");
        return (-1);
    }

    DEBUG_PRINT("Waiting for last result...\n");

    start = omp_get_wtime();
    while (!wed0->status) {
        usleep(1); //sleep(5);
         for (int i = 0; i < workload->batches * PIPE_DEPTH; i++) {
             // Interpret 32-bit value from HW as posits and print
             posit<NBITS, ES> res0, res1, res2, res3;
             res0.set_raw_bits(result_hw[i].b[0]);
             res1.set_raw_bits(result_hw[i].b[1]);
             res2.set_raw_bits(result_hw[i].b[2]);
             res3.set_raw_bits(result_hw[i].b[3]);

           cout << i << ": " << hexstring(res0.collect()) << " " << hexstring(res1.collect()) << " "
                << hexstring(res2.collect()) << " " << hexstring(res3.collect()) << endl;
         }
    }
    stop = omp_get_wtime();
    t_fpga = stop - start;

for (int i = 0; i < workload->batches * PIPE_DEPTH; i++) {
             // Interpret 32-bit value from HW as posits and print
             posit<NBITS, ES> res0, res1, res2, res3;
             res0.set_raw_bits(result_hw[i].b[0]);
             res1.set_raw_bits(result_hw[i].b[1]);
             res2.set_raw_bits(result_hw[i].b[2]);
             res3.set_raw_bits(result_hw[i].b[3]);

           cout << i << ": " << hexstring(res0.collect()) << " " << hexstring(res1.collect()) << " "
                << hexstring(res2.collect()) << " " << hexstring(res3.collect()) << endl;
         }

    // Check for errors with SW calculation
    if (calculate_sw) {
        DebugValues<posit<NBITS, ES>> hw_debug_values;

        for (int i = 0; i < workload->batches * PIPE_DEPTH; i++) {
            // Store HW posit result for decimal accuracy calculation
            posit<NBITS, ES> res_hw;
            res_hw.set_raw_bits(result_hw[i].b[0]);
            hw_debug_values.debugValue(res_hw, "result[0][%d]", i);
        }
	cout << "Writing benchmark..." << endl;
        writeBenchmark(pairhmm_dec50, pairhmm_float, pairhmm_posit, hw_debug_values,
                       "pairhmm_stream_es" + std::to_string(ES) + "_" + std::to_string(pairs) + "_" + std::to_string(x) + "_" + std::to_string(y) + "_" + std::to_string(initial_constant_power) + ".txt",
                       false, true);

        int errs_posit = 0;
	cout << "Counting errors..." << endl;
        errs_posit = pairhmm_posit.count_errors((uint32_t *) result_hw);
        DEBUG_PRINT("Posit errors: %d\n", errs_posit);
    }

    // Release the afu
    cxl_mmio_unmap(afu);
    cxl_afu_free(afu);

    float p_fpga      = ((double)workload->cups_req / (double)t_fpga)  / 1000000; // in MCUPS
    float p_sw        = ((double)workload->cups_req / (double)t_sw)    / 1000000; // in MCUPS
    float p_float     = ((double)workload->cups_req / (double)t_float) / 1000000; // in MCUPS
    float p_dec       = ((double)workload->cups_req / (double)t_dec)   / 1000000; // in MCUPS
    float utilization = ((double)workload->cups_req / (double)t_fpga)  / max_cups;
    float speedup     = t_sw / t_fpga;

    cout << "Adding timing data..." << endl;
    time_t t = chrono::system_clock::to_time_t(chrono::system_clock::now());
    ofstream outfile("pairhmm_stream_es" + std::to_string(ES) + "_" + std::to_string(pairs) + "_" + std::to_string(x) + "_" + std::to_string(y) + "_" + std::to_string(initial_constant_power) + ".txt", ios::out | ios::app);
    outfile << endl << "===================" << endl;
    outfile << ctime(&t) << endl;
    outfile << "Pairs = " << pairs << endl;
    outfile << "X = " << x << endl;
    outfile << "Y = " << y << endl;
    outfile << "Initial Constant = " << initial_constant_power << endl;
    outfile << "cups,t_fill_batch,t_fpga,p_fpga,t_sw,p_sw,t_float,p_float,t_dec,p_dec,utilization,speedup" << endl;
    outfile << setprecision(20) << fixed << (unsigned long)workload->cups_req <<","<< t_fill_batch <<","<< t_fpga <<","<< p_fpga <<","<< t_sw <<","<< p_sw <<","<< t_float <<","<< p_float <<","<< t_dec <<","<< p_dec <<","<< utilization <<","<< speedup << endl;
    outfile.close();

    free(workload);
    free(result_hw);
    free(batch);
    free(wed0);

    return (0);

} // main


#include <stdexcept>
#include <boost/test/unit_test.hpp>
#include "rectangle.hpp"
#include "base-types.hpp"

const double EPSILON = 0.00000001;

BOOST_AUTO_TEST_SUITE(RectangleTest)

  BOOST_AUTO_TEST_SUITE(Rectangle_RectangleConstructor)

    BOOST_AUTO_TEST_CASE(Rectangle_NegativeNumberAsWidth_ExceptionThrown)
    {
      double invalidWidth = -4.8;
      double height = 9.5;
      samokhin::point_t center = {2.1, 3.2};
      BOOST_CHECK_THROW(samokhin::Rectangle(invalidWidth, height, center), std::invalid_argument);
    }

    BOOST_AUTO_TEST_CASE(Rectangle_NegativeNumberAsHeight_ExceptionThrown)
    {
      double width = 2.13;
      double invalidHeight = -9.0;
      samokhin::point_t center = {2.1, 3.2};
      BOOST_CHECK_THROW(samokhin::Rectangle(width, invalidHeight, center), std::invalid_argument);
    }

    BOOST_AUTO_TEST_CASE(Rectangle_PositiveNumbersAsHeightAndWidth_NoException)
    {
      double width = 1.4;
      double height = 8.9;
      samokhin::point_t center = {2.1, 3.2};
      BOOST_CHECK_NO_THROW(samokhin::Rectangle(width, height, center));
    }

    BOOST_AUTO_TEST_CASE(Rectangle_ZeroAsHeightAndWidth_NoException)
    {
      double width = 0.0;
      double height = 0.0;
      samokhin::point_t center = {2.1, 3.2};
      BOOST_CHECK_NO_THROW(samokhin::Rectangle(width, height, center));
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Rectangle_Scale)

    BOOST_AUTO_TEST_CASE(Rectangle_NegativeNumberAsScaleCoefficient_ExceptionThrown)
    {
      double width = 2.5;
      double height = 9.2;
      samokhin::point_t center = {2.0, 2.4};
      samokhin::Rectangle rectangle(width, height, center);
      double k = -4;
      BOOST_CHECK_THROW(rectangle.scale(k), std::invalid_argument);
    }

    BOOST_AUTO_TEST_CASE(Rectangle_PositiveNumberAsScaleCoefficient_NoException)
    {
      double width = 1.5;
      double height = 9.8;
      samokhin::point_t center = {2.4, 2.44};
      samokhin::Rectangle rectangle(width, height, center);
      double k = 6.1;
      BOOST_CHECK_NO_THROW(rectangle.scale(k));
    }

    BOOST_AUTO_TEST_CASE(Rectangle_ZeroAsScaleCoefficient_NoException)
    {
      double width = 1.5;
      double height = 9.8;
      samokhin::point_t center = {2.4, 2.44};
      samokhin::Rectangle rectangle(width, height, center);
      double k = 0.0;
      BOOST_CHECK_NO_THROW(rectangle.scale(k));
    }

    BOOST_AUTO_TEST_CASE(Scale_RectangleArea_SquareAreaChange)
    {
      double width = 6.3;
      double height = 5.7;
      samokhin::point_t center = {1.9, 8.33};
      samokhin::Rectangle rectangle(width, height, center);

      double area = rectangle.getArea();
      double k = 1.4;
      rectangle.scale(k);

      BOOST_CHECK_CLOSE(rectangle.getArea(), area * k * k, EPSILON);
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Rectangle_Move_ByCords)

    BOOST_AUTO_TEST_CASE(Move_RectanglesFrameAtMovingByCords_ConstantFrame)
    {
      double width = 2.4;
      double height = 9.6;
      samokhin::point_t center = {1.4, 5.24};
      samokhin::Rectangle rectangle(width, height, center);
      double dx = 3.0;
      double dy = 2.6;
      samokhin::rectangle_t frame = rectangle.getFrameRect();
      rectangle.move(dx, dy);
      samokhin::rectangle_t newFrame = rectangle.getFrameRect();
      samokhin::point_t newCentre = {frame.pos.x + dx, frame.pos.y + dy};

      BOOST_CHECK_CLOSE(frame.width, newFrame.width, EPSILON);
      BOOST_CHECK_CLOSE(frame.height, newFrame.height, EPSILON);

      BOOST_CHECK_CLOSE(newCentre.x, newFrame.pos.x, EPSILON);
      BOOST_CHECK_CLOSE(newCentre.y, newFrame.pos.y, EPSILON);
    }

    BOOST_AUTO_TEST_CASE(Move_RectanglesAreaAtMovingByCords_ConstantArea)
    {
      double width = 6.3;
      double height = 5.7;
      samokhin::point_t center = {1.9, 4.11};
      samokhin::Rectangle rectangle(width, height, center);
      double area = rectangle.getArea();
      double dx = 2.0;
      double dy = 9.1;
      rectangle.move(dx, dy);
      BOOST_CHECK_CLOSE(rectangle.getArea(), area, EPSILON);
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Rectangle_Move_AtPoint)

    BOOST_AUTO_TEST_CASE(Move_RectanglesFrameAtMovingAtPoint_ConstantFrame)
    {
      double width = 7.2;
      double height = 2.4;
      samokhin::point_t center = {-9.6, -8.1};
      samokhin::Rectangle rectangle(width, height, center);

      samokhin::point_t newCentre = {3.0, 2.6};
      samokhin::rectangle_t frame = rectangle.getFrameRect();
      rectangle.move(newCentre);
      samokhin::rectangle_t newFrame = rectangle.getFrameRect();

      BOOST_CHECK_CLOSE(frame.width, newFrame.width, EPSILON);
      BOOST_CHECK_CLOSE(frame.height, newFrame.height, EPSILON);

      BOOST_CHECK_CLOSE(newCentre.x, newFrame.pos.x, EPSILON);
      BOOST_CHECK_CLOSE(newCentre.y, newFrame.pos.y, EPSILON);
    }

    BOOST_AUTO_TEST_CASE(Move_RectanglesAreaAtMovingAtPoint_ConstantArea)
    {
      double width = 6.3;
      double height = 5.7;
      samokhin::point_t center = {1.9, 9.1};
      samokhin::Rectangle rectangle(width, height, center);

      double area = rectangle.getArea();
      samokhin::point_t newCentre = {2.0, 9.1};
      rectangle.move(newCentre);
      BOOST_CHECK_CLOSE(rectangle.getArea(), area, EPSILON);
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Rectangle_Rotate)

    BOOST_AUTO_TEST_CASE(Rotate_AnyAngleAsParameter_ConstantArea)
    {
      double width = 7.3;
      double height = 1.7;
      samokhin::point_t center = {8.9, 1.1};
      samokhin::Rectangle rectangle(width, height, center);

      double area = rectangle.getArea();
      double angle = 15;
      rectangle.rotate(angle);
      BOOST_CHECK_CLOSE(rectangle.getArea(), area, EPSILON);
    }

    BOOST_AUTO_TEST_CASE(Rotate_AnyAngleAsParameter_ConstantCenter)
    {
      double width = 5.3;
      double height = 2.7;
      samokhin::point_t center = {8.0, 2.1};
      samokhin::Rectangle rectangle(width, height, center);

      double angle = 15;
      rectangle.rotate(angle);
      samokhin::point_t newCenter = rectangle.getCenter();
      BOOST_CHECK_CLOSE(newCenter.x, center.x, EPSILON);
      BOOST_CHECK_CLOSE(newCenter.y, center.y, EPSILON);
    }

    BOOST_AUTO_TEST_CASE(Rotate_AnyAngleAsParameter_RightFrameChange)
    {
      double width = 7.1;
      double height = 2.3;
      samokhin::point_t center = {5.0, -1.1};
      samokhin::Rectangle rectangle(width, height, center);

      double angle = 90;
      samokhin::rectangle_t frame = rectangle.getFrameRect();
      rectangle.rotate(angle);
      samokhin::rectangle_t newFrame = rectangle.getFrameRect();
      BOOST_CHECK_CLOSE(frame.width, newFrame.height, EPSILON);
      BOOST_CHECK_CLOSE(frame.height, newFrame.width, EPSILON);
    }

  BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#ifndef TMC_UNIT_TESTS
#define BOOST_TEST_MODULE lib_test_layer_partitioning
#endif
#include <boost/test/included/unit_test.hpp>

#include "sample_creator.h"
#include "layer_partitioning_creator.h"
#include "border_system_creator.h"

BOOST_AUTO_TEST_SUITE( layer_partitioning_creator )

BOOST_AUTO_TEST_CASE( tmc_9 ) {
    const std::string names_file("data/tmc_paper_9/tmc_paper.names");
    const std::string data_file("data/tmc_paper_9/tmc_paper.data");
    BOOST_TEST_MESSAGE("Creating sample from file: " << data_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);

    BOOST_TEST_MESSAGE("Resulting sample: " << *pSample);
    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 4);

    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);
    pLayerPartitioningCreator->optimize();
    std::shared_ptr<LayerPartitioning> pLayerPartitioning(pLayerPartitioningCreator->get_layer_partitioning());
    BOOST_CHECK_EQUAL(pLayerPartitioning->size(), 2);
}

BOOST_AUTO_TEST_CASE( thirtysix_points ) {
    const std::string names_file("data/4layers_36points/4layers_36points.names");
    const std::string data_file("data/4layers_36points/4layers_36points.data");
    BOOST_TEST_MESSAGE("Creating sample from file: " << data_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);

    BOOST_TEST_MESSAGE("Resulting sample: " << *pSample);
    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 36);

    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->size(), 1);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->consistent(),true);
    pLayerPartitioningCreator->optimize();
    std::shared_ptr<LayerPartitioning> pLD(pLayerPartitioningCreator->get_layer_partitioning());
    BOOST_CHECK_EQUAL(pLD->size(), 4);

    BOOST_CHECK_EQUAL(pLD->consistent(),true);
    auto pLayer1 = *pLD->begin();
    auto pLayer2 = *(pLD->begin() + 1);
    auto pLayer3 = *(pLD->begin() + 2);
    auto pLayer4 = *(pLD->begin() + 3);

    BOOST_TEST_MESSAGE("First layer: " << *pLayer1);
    BOOST_TEST_MESSAGE("Second layer: " << *pLayer2);
    BOOST_TEST_MESSAGE("Third layer: " << *pLayer3);
    BOOST_TEST_MESSAGE("Fourth layer: " << *pLayer4);

    BOOST_CHECK_LE(*pLayer1, *pLayer2);
    BOOST_CHECK_LE(*pLayer1, *pLayer3);
    BOOST_CHECK_LE(*pLayer1, *pLayer4);
    BOOST_CHECK_LE(*pLayer2, *pLayer3);
    BOOST_CHECK_LE(*pLayer2, *pLayer4);
    BOOST_CHECK_LE(*pLayer3, *pLayer4);
    BOOST_CHECK_GE(*pLayer4, *pLayer3);
    BOOST_CHECK_GE(*pLayer3, *pLayer2);
    BOOST_CHECK_GE(*pLayer2, *pLayer1);

    BOOST_CHECK_EQUAL((double)pLayer1->get_neg_pos_counts().second / (double)pLayer1->get_neg_pos_counts().first, (double)1/(double)5 );
    BOOST_CHECK_EQUAL((double)pLayer2->get_neg_pos_counts().second / (double)pLayer2->get_neg_pos_counts().first, (double)1/(double)2 );
    BOOST_CHECK_EQUAL((double)pLayer3->get_neg_pos_counts().second / (double)pLayer3->get_neg_pos_counts().first, (double)2/(double)1 );
    BOOST_CHECK_EQUAL((double)pLayer4->get_neg_pos_counts().second / (double)pLayer4->get_neg_pos_counts().first, (double)5/(double)1 );

    BOOST_CHECK_EQUAL(boost::num_vertices(*pLD->get_graph(pLayer1)), pLayer1->size());
    BOOST_CHECK_EQUAL(boost::num_vertices(*pLD->get_graph(pLayer2)), pLayer2->size());
    BOOST_CHECK_EQUAL(boost::num_vertices(*pLD->get_graph(pLayer3)), pLayer3->size());
    BOOST_CHECK_EQUAL(boost::num_vertices(*pLD->get_graph(pLayer4)), pLayer4->size());

    pLD->merge_two_layers(pLD->begin()+1);
    BOOST_CHECK_EQUAL(pLD->consistent(),true);
    pLD->merge_two_layers(pLD->begin()+1);
    BOOST_CHECK_EQUAL(pLD->consistent(),true);
    pLD->merge_two_layers(pLD->begin());
    BOOST_CHECK_EQUAL(pLD->consistent(),true);

    BOOST_CHECK_EQUAL(36, boost::num_vertices(*pLD->get_graph(*pLD->begin())));
}

BOOST_AUTO_TEST_CASE( thirtysix_points_enforce_merge ) {
    const std::string names_file("data/4layers_36points/4layers_36points.names");
    const std::string data_file("data/4layers_36points/4layers_36points.data");
    BOOST_TEST_MESSAGE("Creating sample from file: " << data_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);

    BOOST_TEST_MESSAGE("Resulting sample: " << *pSample);
    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 36);

    boost::dynamic_bitset<> db4(36);
    db4[25] = db4[27] = db4[28] = db4[29] = db4[30] = db4[31] = db4[32] = db4[33] = db4[34] = db4[35] = true;

    std::shared_ptr<LayerPartitioning> pLD_aux = std::make_shared<LayerPartitioning>();
    pLD_aux->push_back(pSample);

    BOOST_TEST_MESSAGE("Create layer partitioning via splitting the lowest layer three times into 4 layers");
    auto it_aux = pLD_aux->begin();
    pLD_aux->split_layer(it_aux, db4);
    BOOST_CHECK_EQUAL(pLD_aux->consistent(),true);

    auto pSample1 = *pLD_aux->begin();
    auto pSample2 = *(pLD_aux->begin()+1);

    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample1);
    pLayerPartitioningCreator->push_back(pSample2);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->size(), 2);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->consistent(),true);
    pLayerPartitioningCreator->optimize();
    std::shared_ptr<LayerPartitioning> pLD(pLayerPartitioningCreator->get_layer_partitioning());
    BOOST_CHECK_EQUAL(pLD->size(), 4);

    BOOST_CHECK_EQUAL(pLD->consistent(),true);
    auto pLayer1 = *pLD->begin();
    auto pLayer2 = *(pLD->begin() + 1);
    auto pLayer3 = *(pLD->begin() + 2);
    auto pLayer4 = *(pLD->begin() + 3);

    BOOST_TEST_MESSAGE("First layer: " << *pLayer1);
    BOOST_TEST_MESSAGE("Second layer: " << *pLayer2);
    BOOST_TEST_MESSAGE("Third layer: " << *pLayer3);
    BOOST_TEST_MESSAGE("Fourth layer: " << *pLayer4);

    BOOST_CHECK_LE(*pLayer1, *pLayer2);
    BOOST_CHECK_LE(*pLayer1, *pLayer3);
    BOOST_CHECK_LE(*pLayer1, *pLayer4);
    BOOST_CHECK_LE(*pLayer2, *pLayer3);
    BOOST_CHECK_LE(*pLayer2, *pLayer4);
    BOOST_CHECK_LE(*pLayer3, *pLayer4);
    BOOST_CHECK_GE(*pLayer4, *pLayer3);
    BOOST_CHECK_GE(*pLayer3, *pLayer2);
    BOOST_CHECK_GE(*pLayer2, *pLayer1);

    BOOST_CHECK_EQUAL((double)pLayer1->get_neg_pos_counts().second / (double)pLayer1->get_neg_pos_counts().first, (double)1/(double)5 );
    BOOST_CHECK_EQUAL((double)pLayer2->get_neg_pos_counts().second / (double)pLayer2->get_neg_pos_counts().first, (double)1/(double)2 );
    BOOST_CHECK_EQUAL((double)pLayer3->get_neg_pos_counts().second / (double)pLayer3->get_neg_pos_counts().first, (double)2/(double)1 );
    BOOST_CHECK_EQUAL((double)pLayer4->get_neg_pos_counts().second / (double)pLayer4->get_neg_pos_counts().first, (double)5/(double)1 );

    BOOST_CHECK_EQUAL(boost::num_vertices(*pLD->get_graph(pLayer1)), pLayer1->size());
    BOOST_CHECK_EQUAL(boost::num_vertices(*pLD->get_graph(pLayer2)), pLayer2->size());
    BOOST_CHECK_EQUAL(boost::num_vertices(*pLD->get_graph(pLayer3)), pLayer3->size());
    BOOST_CHECK_EQUAL(boost::num_vertices(*pLD->get_graph(pLayer4)), pLayer4->size());

    pLD->merge_two_layers(pLD->begin()+1);
    BOOST_CHECK_EQUAL(pLD->consistent(),true);
    pLD->merge_two_layers(pLD->begin()+1);
    BOOST_CHECK_EQUAL(pLD->consistent(),true);
    pLD->merge_two_layers(pLD->begin());
    BOOST_CHECK_EQUAL(pLD->consistent(),true);

    BOOST_CHECK_EQUAL(36, boost::num_vertices(*pLD->get_graph(*pLD->begin())));
}

BOOST_AUTO_TEST_CASE( thirtysix_points_check_border_system ) {
    const std::string names_file("data/4layers_36points/4layers_36points.names");
    const std::string data_file("data/4layers_36points/4layers_36points.data");
    BOOST_TEST_MESSAGE("Creating sample from file: " << data_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);

    BOOST_TEST_MESSAGE("Resulting sample: " << *pSample);
    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 36);

    boost::dynamic_bitset<> db4(36);
    db4[25] = db4[27] = db4[28] = db4[29] = db4[30] = db4[31] = db4[32] = db4[33] = db4[34] = db4[35] = true;

    std::shared_ptr<LayerPartitioning> pLD_aux = std::make_shared<LayerPartitioning>();
    pLD_aux->push_back(pSample);

    BOOST_TEST_MESSAGE("Create layer partitioning via splitting the lowest layer three times into 4 layers");
    auto it_aux = pLD_aux->begin();
    pLD_aux->split_layer(it_aux, db4);
    BOOST_CHECK_EQUAL(pLD_aux->consistent(), true);

    auto pSample1 = *pLD_aux->begin();
    auto pSample2 = *(pLD_aux->begin() + 1);

    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample1);
    pLayerPartitioningCreator->push_back(pSample2);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->size(), 2);
    BOOST_CHECK_EQUAL(pLayerPartitioningCreator->get_layer_partitioning()->consistent(), true);
    pLayerPartitioningCreator->optimize();
    std::shared_ptr<LayerPartitioning> pLD(pLayerPartitioningCreator->get_layer_partitioning());
    BOOST_CHECK_EQUAL(pLD->size(), 4);
    BOOST_CHECK_EQUAL(pLD->consistent(), true);
    auto pLayer1 = *pLD->begin();
    auto pLayer2 = *(pLD->begin() + 1);
    auto pLayer3 = *(pLD->begin() + 2);
    auto pLayer4 = *(pLD->begin() + 3);

    std::shared_ptr<BorderSystemCreator> pBSC = std::make_shared<BorderSystemCreator>();
    std::shared_ptr<BorderSystem> pBS = pBSC->from_layer_partitioning(pLD);
    BOOST_CHECK_EQUAL(pBS->dim(), pLD->dim());
    BOOST_CHECK_EQUAL(pBS->size(), pLD->size());
    const Border & LowerBorder0 = *pBS->get_lower(0);
    const Border & LowerBorder1 = *pBS->get_lower(1);
    const Border & LowerBorder2 = *pBS->get_lower(2);
    const Border & LowerBorder3 = *pBS->get_lower(3);

    const Border & UpperBorder0 = *pBS->get_upper(0);
    const Border & UpperBorder1 = *pBS->get_upper(1);
    const Border & UpperBorder2 = *pBS->get_upper(2);
    const Border & UpperBorder3 = *pBS->get_upper(3);

    BOOST_TEST_MESSAGE("First lower border: " << LowerBorder0);
    BOOST_TEST_MESSAGE("Second lower border: " << LowerBorder1);
    BOOST_TEST_MESSAGE("Third lower border: " << LowerBorder2);
    BOOST_TEST_MESSAGE("Fourth lower border: " << LowerBorder3);

    BOOST_TEST_MESSAGE("First upper border: " << UpperBorder0);
    BOOST_TEST_MESSAGE("Second upper border: " << UpperBorder1);
    BOOST_TEST_MESSAGE("Third upper border: " << UpperBorder2);
    BOOST_TEST_MESSAGE("Fourth upper border: " << UpperBorder3);

    BOOST_CHECK(pLayer1->get_neg_pos_counts() == LowerBorder0.get_neg_pos_counts());
    BOOST_CHECK(pLayer1->get_neg_pos_counts() == UpperBorder0.get_neg_pos_counts());
    BOOST_CHECK(pLayer2->get_neg_pos_counts() == LowerBorder1.get_neg_pos_counts());
    BOOST_CHECK(pLayer2->get_neg_pos_counts() == UpperBorder1.get_neg_pos_counts());
    BOOST_CHECK(pLayer3->get_neg_pos_counts() == LowerBorder2.get_neg_pos_counts());
    BOOST_CHECK(pLayer3->get_neg_pos_counts() == UpperBorder2.get_neg_pos_counts());
    BOOST_CHECK(pLayer4->get_neg_pos_counts() == LowerBorder3.get_neg_pos_counts());
    BOOST_CHECK(pLayer4->get_neg_pos_counts() == UpperBorder3.get_neg_pos_counts());

    //lower borders
    BOOST_CHECK_EQUAL(LowerBorder0.size(), 5);
    BOOST_CHECK(LowerBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{9,1})));
    BOOST_CHECK(LowerBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{8,3})));
    BOOST_CHECK(LowerBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{5,4})));
    BOOST_CHECK(LowerBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{2,7})));
    BOOST_CHECK(LowerBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{0,11})));

    BOOST_CHECK_EQUAL(LowerBorder1.size(), 3);
    BOOST_CHECK(LowerBorder1.contains(std::make_shared<FeatureVector>(std::vector<double>{15,0})));
    BOOST_CHECK(LowerBorder1.contains(std::make_shared<FeatureVector>(std::vector<double>{13,2})));
    BOOST_CHECK(LowerBorder1.contains(std::make_shared<FeatureVector>(std::vector<double>{12,5})));

    BOOST_CHECK_EQUAL(LowerBorder2.size(), 3);
    BOOST_CHECK(LowerBorder2.contains(std::make_shared<FeatureVector>(std::vector<double>{11,8})));
    BOOST_CHECK(LowerBorder2.contains(std::make_shared<FeatureVector>(std::vector<double>{5,9})));
    BOOST_CHECK(LowerBorder2.contains(std::make_shared<FeatureVector>(std::vector<double>{4,10})));

    BOOST_CHECK_EQUAL(LowerBorder3.size(), 5);
    BOOST_CHECK(LowerBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{16,6})));
    BOOST_CHECK(LowerBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{13,7})));
    BOOST_CHECK(LowerBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{9,9})));
    BOOST_CHECK(LowerBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{6,13})));
    BOOST_CHECK(LowerBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{3,14})));

    // upper borders
    BOOST_CHECK_EQUAL(UpperBorder3.size(), 6);
    BOOST_CHECK(UpperBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{16,6})));
    BOOST_CHECK(UpperBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{13,7})));
    BOOST_CHECK(UpperBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{12,11})));
    BOOST_CHECK(UpperBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{9,12})));
    BOOST_CHECK(UpperBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{6,15})));
    BOOST_CHECK(UpperBorder3.contains(std::make_shared<FeatureVector>(std::vector<double>{5,17})));

    BOOST_CHECK_EQUAL(UpperBorder2.size(), 3);
    BOOST_CHECK(UpperBorder2.contains(std::make_shared<FeatureVector>(std::vector<double>{11,8})));
    BOOST_CHECK(UpperBorder2.contains(std::make_shared<FeatureVector>(std::vector<double>{8,11})));
    BOOST_CHECK(UpperBorder2.contains(std::make_shared<FeatureVector>(std::vector<double>{7,12})));

    BOOST_CHECK_EQUAL(UpperBorder1.size(), 2);
    BOOST_CHECK(UpperBorder1.contains(std::make_shared<FeatureVector>(std::vector<double>{17,5})));
    BOOST_CHECK(UpperBorder1.contains(std::make_shared<FeatureVector>(std::vector<double>{14,6})));

    BOOST_CHECK_EQUAL(UpperBorder0.size(), 5);
    BOOST_CHECK(UpperBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{14,1})));
    BOOST_CHECK(UpperBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{12,3})));
    BOOST_CHECK(UpperBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{11,7})));
    BOOST_CHECK(UpperBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{3,12})));
    BOOST_CHECK(UpperBorder0.contains(std::make_shared<FeatureVector>(std::vector<double>{2,15})));
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <map>
#include <algorithm>
#include <armadillo>
#include "sdp_branch_and_bound.h"

// data full path
const char *data_path;
const char *log_path;
std::ofstream log_file;

// branch and bound
double branch_and_bound_tol;
int branch_and_bound_parallel;
int branch_and_bound_max_nodes;
int branch_and_bound_visiting_strategy;

// sdp solver
// const char *sdp_solver_matlab_session;
int sdp_solver_session_threads_root;
int sdp_solver_session_threads;
const char *sdp_solver_folder;
double sdp_solver_tol;
// int sdp_solver_type;
int sdp_solver_verbose;
int sdp_solver_max_cp_iter_root;
int sdp_solver_max_cp_iter;
double sdp_solver_cp_tol;
int sdp_solver_max_ineq;
double sdp_solver_inherit_perc;
double sdp_solver_eps_ineq;
double sdp_solver_eps_active;
int sdp_solver_max_pair_ineq;
double sdp_solver_pair_perc;
int sdp_solver_max_triangle_ineq;
double sdp_solver_triangle_perc;

// heuristic
bool kmeans_sdp_based;
int kmeans_max_iter;
int kmeans_n_start;
bool kmeans_verbose;

// read data Ws
arma::mat read_data(const char *filename, int &n, int &d) {
    std::ifstream file(filename);
    if (!file) {
        std::cerr << strerror(errno) << "\n";
        exit(EXIT_FAILURE);
    }
    // read the header n, d
    file >> n >> d;
    arma::mat Ws(n, d);
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < d; j++) {
            file >> Ws(i, j);
        }
    }
    return Ws;
}

std::map<std::string, std::string> read_params(std::string &config_file) {

    std::map<std::string, std::string> config_map = {};

    std::ifstream cFile (config_file);
    if (cFile.is_open()) {
        std::string line;
        while (getline(cFile, line)){
            line.erase(std::remove_if(line.begin(), line.end(), isspace), line.end());
            if(line[0] == '#' || line.empty())
                continue;
            auto delimiterPos = line.find('=');
            auto key = line.substr(0, delimiterPos);
            auto value = line.substr(delimiterPos + 1);
            config_map.insert(std::pair<std::string, std::string>(key, value));
        }

    }
    else {
        std::cerr << "Couldn't open config file for reading.\n";
    }

    return config_map;
}

void run(int argc, char **argv) {

    std::string config_file = "config.txt";
    std::map<std::string, std::string> config_map = read_params(config_file);

    if (argc != 4) {
        std::cerr << "Input: <DATA_PATH> <K> <LOG_PATH>" << std::endl;
        exit(EXIT_FAILURE);
    }

    data_path = argv[1];
    int k = std::stoi(argv[2]);
    log_path = argv[3];
    log_file.open(log_path);

    // branch and bound
    branch_and_bound_tol = std::stod(config_map["BRANCH_AND_BOUND_TOL"]);
    branch_and_bound_parallel = std::stoi(config_map["BRANCH_AND_BOUND_PARALLEL"]);
    branch_and_bound_max_nodes = std::stoi(config_map["BRANCH_AND_BOUND_MAX_NODES"]);
    branch_and_bound_visiting_strategy = std::stoi(config_map["BRANCH_AND_BOUND_VISITING_STRATEGY"]);

    // sdp solver
    // sdp_solver_matlab_session = config_map["SDP_SOLVER_MATLAB_SESSION"].c_str();
    sdp_solver_session_threads_root = std::stoi(config_map["SDP_SOLVER_SESSION_THREADS_ROOT"]);
    sdp_solver_session_threads = std::stoi(config_map["SDP_SOLVER_SESSION_THREADS"]);
    sdp_solver_folder = config_map["SDP_SOLVER_FOLDER"].c_str();
    sdp_solver_tol = std::stod(config_map["SDP_SOLVER_TOL"]);
    sdp_solver_verbose = std::stoi(config_map["SDP_SOLVER_VERBOSE"]);
    sdp_solver_max_cp_iter_root = std::stoi(config_map["SDP_SOLVER_MAX_CP_ITER_ROOT"]);
    sdp_solver_max_cp_iter = std::stoi(config_map["SDP_SOLVER_MAX_CP_ITER"]);
    sdp_solver_cp_tol = std::stod(config_map["SDP_SOLVER_CP_TOL"]);
    sdp_solver_max_ineq = std::stoi(config_map["SDP_SOLVER_MAX_INEQ"]);
    sdp_solver_inherit_perc = std::stod(config_map["SDP_SOLVER_INHERIT_PERC"]);
    sdp_solver_eps_ineq = std::stod(config_map["SDP_SOLVER_EPS_INEQ"]);
    sdp_solver_eps_active = std::stod(config_map["SDP_SOLVER_EPS_ACTIVE"]);
    sdp_solver_max_pair_ineq = std::stoi(config_map["SDP_SOLVER_MAX_PAIR_INEQ"]);
    sdp_solver_pair_perc = std::stod(config_map["SDP_SOLVER_PAIR_PERC"]);
    sdp_solver_max_triangle_ineq = std::stoi(config_map["SDP_SOLVER_MAX_TRIANGLE_INEQ"]);
    sdp_solver_triangle_perc = std::stod(config_map["SDP_SOLVER_TRIANGLE_PERC"]);

    // kmeans
    kmeans_sdp_based = std::stoi(config_map["KMEANS_SDP_BASED"]);
    kmeans_max_iter = std::stoi(config_map["KMEANS_MAX_ITER"]);
    kmeans_n_start = std::stoi(config_map["KMEANS_N_START"]);
    kmeans_verbose = std::stoi(config_map["KMEANS_VERBOSE"]);

    int n, d;
    arma::mat Ws = read_data(data_path, n, d);

    log_file << "\n" << "DATA_PATH, n, d, k: " << data_path << " " << n << " " << d << " " << k << "\n";
    log_file << "LOG_PATH: " << log_path << "\n\n";

    log_file << "BRANCH_AND_BOUND_TOL: " << branch_and_bound_tol << "\n";
    log_file << "BRANCH_AND_BOUND_PARALLEL: " << branch_and_bound_parallel << "\n";
    log_file << "BRANCH_AND_BOUND_MAX_NODES: " <<  branch_and_bound_max_nodes << "\n";
    log_file << "BRANCH_AND_BOUND_VISITING_STRATEGY: " << branch_and_bound_visiting_strategy << "\n\n";

    // log_file << "SDP_SOLVER_MATLAB_SESSION: " << sdp_solver_matlab_session << "\n";
    log_file << "SDP_SOLVER_SESSION_THREADS_ROOT: " << sdp_solver_session_threads_root << "\n";
    log_file << "SDP_SOLVER_SESSION_THREADS: " << sdp_solver_session_threads << "\n";
    log_file << "SDP_SOLVER_FOLDER: " << sdp_solver_folder << "\n";
    log_file << "SDP_SOLVER_TOL: " << sdp_solver_tol << "\n";
    log_file << "SDP_SOLVER_VERBOSE: " << sdp_solver_verbose << "\n";
    log_file << "SDP_SOLVER_MAX_CP_ITER_ROOT: " << sdp_solver_max_cp_iter_root << "\n";
    log_file << "SDP_SOLVER_MAX_CP_ITER: " << sdp_solver_max_cp_iter << "\n";
    log_file << "SDP_SOLVER_CP_TOL: " << sdp_solver_cp_tol << "\n";
    log_file << "SDP_SOLVER_MAX_INEQ: " << sdp_solver_max_ineq << "\n";
    log_file << "SDP_SOLVER_INHERIT_PERC: " << sdp_solver_inherit_perc << "\n";
    log_file << "SDP_SOLVER_EPS_INEQ: " << sdp_solver_eps_ineq << "\n";
    log_file << "SDP_SOLVER_EPS_ACTIVE: " << sdp_solver_eps_active << "\n";
    log_file << "SDP_SOLVER_MAX_PAIR_INEQ: " << sdp_solver_max_pair_ineq << "\n";
    log_file << "SDP_SOLVER_PAIR_PERC: " << sdp_solver_pair_perc << "\n";
    log_file << "SDP_SOLVER_MAX_TRIANGLE_INEQ: " << sdp_solver_max_triangle_ineq << "\n";
    log_file << "SDP_SOLVER_TRIANGLE_PERC: " << sdp_solver_triangle_perc << "\n\n";

    log_file << "KMEANS_SDP_BASED: " << kmeans_sdp_based << "\n";
    log_file << "KMEANS_MAX_ITER: " << kmeans_max_iter << "\n";
    log_file << "KMEANS_N_START: " << kmeans_n_start << "\n";
    log_file << "KMEANS_VERBOSE: " << kmeans_verbose << "\n\n";

    sdp_branch_and_bound(k, Ws);

}

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

    run(argc, argv);

    return EXIT_SUCCESS;
}