//  boost quaternion.hpp header file

//  (C) Copyright Hubert Holin 2001.
//  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)

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_QUATERNION_HPP
#define BOOST_QUATERNION_HPP

#include <boost/config.hpp> // for BOOST_NO_STD_LOCALE
#include <boost/math_fwd.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <boost/utility/enable_if.hpp>
#ifndef    BOOST_NO_STD_LOCALE
#  include <locale>                                    // for the "<<" operator
#endif /* BOOST_NO_STD_LOCALE */

#include <complex>
#include <iosfwd>                                    // for the "<<" and ">>" operators
#include <sstream>                                    // for the "<<" operator

#include <boost/math/special_functions/sinc.hpp>    // for the Sinus cardinal
#include <boost/math/special_functions/sinhc.hpp>    // for the Hyperbolic Sinus cardinal
#include <boost/math/tools/cxx03_warn.hpp>

#if defined(BOOST_NO_CXX11_NOEXCEPT) || defined(BOOST_NO_CXX11_RVALUE_REFERENCES) || defined(BOOST_NO_SFINAE_EXPR)
#include <boost/type_traits/is_pod.hpp>
#endif

namespace boost
{
   namespace math
   {

      namespace detail {

#if !defined(BOOST_NO_CXX11_NOEXCEPT) && !defined(BOOST_NO_CXX11_RVALUE_REFERENCES) && !defined(BOOST_NO_SFINAE_EXPR)

         template <class T>
         struct is_trivial_arithmetic_type_imp
         {
            typedef boost::integral_constant<bool,
               noexcept(std::declval<T&>() += std::declval<T>())
               && noexcept(std::declval<T&>() -= std::declval<T>())
               && noexcept(std::declval<T&>() *= std::declval<T>())
               && noexcept(std::declval<T&>() /= std::declval<T>())
            > type;
         };

         template <class T>
         struct is_trivial_arithmetic_type : public is_trivial_arithmetic_type_imp<T>::type {};
#else

         template <class T>
         struct is_trivial_arithmetic_type : public boost::is_pod<T> {};

#endif

      }

#ifndef BOOST_NO_CXX14_CONSTEXPR
      namespace constexpr_detail
      {
         template <class T>
         constexpr void swap(T& a, T& b)
         {
            T t(a);
            a = b;
            b = t;
         }
       }
#endif

       template<typename T>
        class quaternion
        {
        public:

            typedef T value_type;

            // constructor for H seen as R^4
            // (also default constructor)

            BOOST_CONSTEXPR explicit            quaternion( T const & requested_a = T(),
                                            T const & requested_b = T(),
                                            T const & requested_c = T(),
                                            T const & requested_d = T())
            :   a(requested_a),
                b(requested_b),
                c(requested_c),
                d(requested_d)
            {
                // nothing to do!
            }

            // constructor for H seen as C^2

            BOOST_CONSTEXPR explicit            quaternion( ::std::complex<T> const & z0,
                                            ::std::complex<T> const & z1 = ::std::complex<T>())
            :   a(z0.real()),
                b(z0.imag()),
                c(z1.real()),
                d(z1.imag())
            {
                // nothing to do!
            }

            // UNtemplated copy constructor
            BOOST_CONSTEXPR quaternion(quaternion const & a_recopier)
               : a(a_recopier.R_component_1()),
               b(a_recopier.R_component_2()),
               c(a_recopier.R_component_3()),
               d(a_recopier.R_component_4()) {}
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
            BOOST_CONSTEXPR quaternion(quaternion && a_recopier)
               : a(std::move(a_recopier.R_component_1())),
               b(std::move(a_recopier.R_component_2())),
               c(std::move(a_recopier.R_component_3())),
               d(std::move(a_recopier.R_component_4())) {}
#endif

            // templated copy constructor

            template<typename X>
            BOOST_CONSTEXPR explicit            quaternion(quaternion<X> const & a_recopier)
            :   a(static_cast<T>(a_recopier.R_component_1())),
                b(static_cast<T>(a_recopier.R_component_2())),
                c(static_cast<T>(a_recopier.R_component_3())),
                d(static_cast<T>(a_recopier.R_component_4()))
            {
                // nothing to do!
            }

            // destructor
            // (this is taken care of by the compiler itself)

            // accessors
            //
            // Note:    Like complex number, quaternions do have a meaningful notion of "real part",
            //            but unlike them there is no meaningful notion of "imaginary part".
            //            Instead there is an "unreal part" which itself is a quaternion, and usually
            //            nothing simpler (as opposed to the complex number case).
            //            However, for practicality, there are accessors for the other components
            //            (these are necessary for the templated copy constructor, for instance).

            BOOST_CONSTEXPR T real() const
            {
               return(a);
            }

            BOOST_CONSTEXPR quaternion<T> unreal() const
            {
               return(quaternion<T>(static_cast<T>(0), b, c, d));
            }

            BOOST_CONSTEXPR T R_component_1() const
            {
               return(a);
            }

            BOOST_CONSTEXPR T R_component_2() const
            {
               return(b);
            }

            BOOST_CONSTEXPR T R_component_3() const
            {
               return(c);
            }

            BOOST_CONSTEXPR T R_component_4() const
            {
               return(d);
            }

            BOOST_CONSTEXPR ::std::complex<T> C_component_1() const
            {
               return(::std::complex<T>(a, b));
            }

            BOOST_CONSTEXPR ::std::complex<T> C_component_2() const
            {
               return(::std::complex<T>(c, d));
            }

            BOOST_CXX14_CONSTEXPR void swap(quaternion& o)
            {
#ifndef BOOST_NO_CXX14_CONSTEXPR
               using constexpr_detail::swap;
#else
               using std::swap;
#endif
               swap(a, o.a);
               swap(b, o.b);
               swap(c, o.c);
               swap(d, o.d);
            }

            // assignment operators

            template<typename X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (quaternion<X> const  & a_affecter)
            {
               a = static_cast<T>(a_affecter.R_component_1());
               b = static_cast<T>(a_affecter.R_component_2());
               c = static_cast<T>(a_affecter.R_component_3());
               d = static_cast<T>(a_affecter.R_component_4());

               return(*this);
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (quaternion<T> const & a_affecter)
            {
               a = a_affecter.a;
               b = a_affecter.b;
               c = a_affecter.c;
               d = a_affecter.d;

               return(*this);
            }
#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (quaternion<T> && a_affecter)
            {
               a = std::move(a_affecter.a);
               b = std::move(a_affecter.b);
               c = std::move(a_affecter.c);
               d = std::move(a_affecter.d);

               return(*this);
            }
#endif
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (T const & a_affecter)
            {
               a = a_affecter;

               b = c = d = static_cast<T>(0);

               return(*this);
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator = (::std::complex<T> const & a_affecter)
            {
               a = a_affecter.real();
               b = a_affecter.imag();

               c = d = static_cast<T>(0);

               return(*this);
            }

            // other assignment-related operators
            //
            // NOTE:    Quaternion multiplication is *NOT* commutative;
            //            symbolically, "q *= rhs;" means "q = q * rhs;"
            //            and "q /= rhs;" means "q = q * inverse_of(rhs);"
            //
            // Note2:   Each operator comes in 2 forms - one for the simple case where
            //          type T throws no exceptions, and one exception-safe version
            //          for the case where it might.
         private:
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(T const & rhs, const boost::true_type&)
            {
               a += rhs;
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(T const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a + rhs, b, c, d); // exception guard
               swap(result);
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(std::complex<T> const & rhs, const boost::true_type&)
            {
               a += std::real(rhs);
               b += std::imag(rhs);
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(std::complex<T> const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a + std::real(rhs), b + std::imag(rhs), c, d); // exception guard
               swap(result);
               return *this;
            }
            template <class X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(quaternion<X> const & rhs, const boost::true_type&)
            {
               a += rhs.R_component_1();
               b += rhs.R_component_2();
               c += rhs.R_component_3();
               d += rhs.R_component_4();
               return *this;
            }
            template <class X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_add(quaternion<X> const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a + rhs.R_component_1(), b + rhs.R_component_2(), c + rhs.R_component_3(), d + rhs.R_component_4()); // exception guard
               swap(result);
               return *this;
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(T const & rhs, const boost::true_type&)
            {
               a -= rhs;
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(T const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a - rhs, b, c, d); // exception guard
               swap(result);
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(std::complex<T> const & rhs, const boost::true_type&)
            {
               a -= std::real(rhs);
               b -= std::imag(rhs);
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(std::complex<T> const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a - std::real(rhs), b - std::imag(rhs), c, d); // exception guard
               swap(result);
               return *this;
            }
            template <class X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(quaternion<X> const & rhs, const boost::true_type&)
            {
               a -= rhs.R_component_1();
               b -= rhs.R_component_2();
               c -= rhs.R_component_3();
               d -= rhs.R_component_4();
               return *this;
            }
            template <class X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_subtract(quaternion<X> const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a - rhs.R_component_1(), b - rhs.R_component_2(), c - rhs.R_component_3(), d - rhs.R_component_4()); // exception guard
               swap(result);
               return *this;
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_multiply(T const & rhs, const boost::true_type&)
            {
               a *= rhs;
               b *= rhs;
               c *= rhs;
               d *= rhs;
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_multiply(T const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a * rhs, b * rhs, c * rhs, d * rhs); // exception guard
               swap(result);
               return *this;
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_divide(T const & rhs, const boost::true_type&)
            {
               a /= rhs;
               b /= rhs;
               c /= rhs;
               d /= rhs;
               return *this;
            }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        do_divide(T const & rhs, const boost::false_type&)
            {
               quaternion<T> result(a / rhs, b / rhs, c / rhs, d / rhs); // exception guard
               swap(result);
               return *this;
            }
         public:

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator += (T const & rhs) { return do_add(rhs, detail::is_trivial_arithmetic_type<T>()); }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator += (::std::complex<T> const & rhs) { return do_add(rhs, detail::is_trivial_arithmetic_type<T>()); }
            template<typename X> BOOST_CXX14_CONSTEXPR quaternion<T> & operator += (quaternion<X> const & rhs) { return do_add(rhs, detail::is_trivial_arithmetic_type<T>()); }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator -= (T const & rhs) { return do_subtract(rhs, detail::is_trivial_arithmetic_type<T>()); }
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator -= (::std::complex<T> const & rhs) { return do_subtract(rhs, detail::is_trivial_arithmetic_type<T>()); }
            template<typename X> BOOST_CXX14_CONSTEXPR quaternion<T> & operator -= (quaternion<X> const & rhs) { return do_subtract(rhs, detail::is_trivial_arithmetic_type<T>()); }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator *= (T const & rhs) { return do_multiply(rhs, detail::is_trivial_arithmetic_type<T>()); }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator *= (::std::complex<T> const & rhs)
            {
                T    ar = rhs.real();
                T    br = rhs.imag();
                quaternion<T> result(a*ar - b*br, a*br + b*ar, c*ar + d*br, -c*br+d*ar);
                swap(result);
                return(*this);
            }

            template<typename X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator *= (quaternion<X> const & rhs)
            {
                T    ar = static_cast<T>(rhs.R_component_1());
                T    br = static_cast<T>(rhs.R_component_2());
                T    cr = static_cast<T>(rhs.R_component_3());
                T    dr = static_cast<T>(rhs.R_component_4());

                quaternion<T> result(a*ar - b*br - c*cr - d*dr, a*br + b*ar + c*dr - d*cr, a*cr - b*dr + c*ar + d*br, a*dr + b*cr - c*br + d*ar);
                swap(result);
                return(*this);
            }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator /= (T const & rhs) { return do_divide(rhs, detail::is_trivial_arithmetic_type<T>()); }

            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator /= (::std::complex<T> const & rhs)
            {
                T    ar = rhs.real();
                T    br = rhs.imag();
                T    denominator = ar*ar+br*br;
                quaternion<T> result((+a*ar + b*br) / denominator, (-a*br + b*ar) / denominator, (+c*ar - d*br) / denominator, (+c*br + d*ar) / denominator);
                swap(result);
                return(*this);
            }

            template<typename X>
            BOOST_CXX14_CONSTEXPR quaternion<T> &        operator /= (quaternion<X> const & rhs)
            {
                T    ar = static_cast<T>(rhs.R_component_1());
                T    br = static_cast<T>(rhs.R_component_2());
                T    cr = static_cast<T>(rhs.R_component_3());
                T    dr = static_cast<T>(rhs.R_component_4());

                T    denominator = ar*ar+br*br+cr*cr+dr*dr;
                quaternion<T> result((+a*ar+b*br+c*cr+d*dr)/denominator, (-a*br+b*ar-c*dr+d*cr)/denominator, (-a*cr+b*dr+c*ar-d*br)/denominator, (-a*dr-b*cr+c*br+d*ar)/denominator);
                swap(result);
                return(*this);
            }
        private:
           T a, b, c, d;

        };

// swap:
template <class T>
BOOST_CXX14_CONSTEXPR void swap(quaternion<T>& a, quaternion<T>& b) { a.swap(b); }

// operator+
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator + (const quaternion<T1>& a, const T2& b)
{
   return quaternion<T1>(static_cast<T1>(a.R_component_1() + b), a.R_component_2(), a.R_component_3(), a.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator + (const T1& a, const quaternion<T2>& b)
{
   return quaternion<T2>(static_cast<T2>(b.R_component_1() + a), b.R_component_2(), b.R_component_3(), b.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator + (const quaternion<T1>& a, const std::complex<T2>& b)
{
   return quaternion<T1>(a.R_component_1() + std::real(b), a.R_component_2() + std::imag(b), a.R_component_3(), a.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator + (const std::complex<T1>& a, const quaternion<T2>& b)
{
   return quaternion<T1>(b.R_component_1() + real(a), b.R_component_2() + imag(a), b.R_component_3(), b.R_component_4());
}
template <class T>
inline BOOST_CONSTEXPR quaternion<T> operator + (const quaternion<T>& a, const quaternion<T>& b)
{
   return quaternion<T>(a.R_component_1() + b.R_component_1(), a.R_component_2() + b.R_component_2(), a.R_component_3() + b.R_component_3(), a.R_component_4() + b.R_component_4());
}
// operator-
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator - (const quaternion<T1>& a, const T2& b)
{
   return quaternion<T1>(static_cast<T1>(a.R_component_1() - b), a.R_component_2(), a.R_component_3(), a.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator - (const T1& a, const quaternion<T2>& b)
{
   return quaternion<T2>(static_cast<T2>(a - b.R_component_1()), -b.R_component_2(), -b.R_component_3(), -b.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator - (const quaternion<T1>& a, const std::complex<T2>& b)
{
   return quaternion<T1>(a.R_component_1() - std::real(b), a.R_component_2() - std::imag(b), a.R_component_3(), a.R_component_4());
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator - (const std::complex<T1>& a, const quaternion<T2>& b)
{
   return quaternion<T1>(real(a) - b.R_component_1(), imag(a) - b.R_component_2(), -b.R_component_3(), -b.R_component_4());
}
template <class T>
inline BOOST_CONSTEXPR quaternion<T> operator - (const quaternion<T>& a, const quaternion<T>& b)
{
   return quaternion<T>(a.R_component_1() - b.R_component_1(), a.R_component_2() - b.R_component_2(), a.R_component_3() - b.R_component_3(), a.R_component_4() - b.R_component_4());
}

// operator*
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator * (const quaternion<T1>& a, const T2& b)
{
   return quaternion<T1>(static_cast<T1>(a.R_component_1() * b), a.R_component_2() * b, a.R_component_3() * b, a.R_component_4() * b);
}
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator * (const T1& a, const quaternion<T2>& b)
{
   return quaternion<T2>(static_cast<T2>(a * b.R_component_1()), a * b.R_component_2(), a * b.R_component_3(), a * b.R_component_4());
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator * (const quaternion<T1>& a, const std::complex<T2>& b)
{
   quaternion<T1> result(a);
   result *= b;
   return result;
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator * (const std::complex<T1>& a, const quaternion<T2>& b)
{
   quaternion<T1> result(a);
   result *= b;
   return result;
}
template <class T>
inline BOOST_CXX14_CONSTEXPR quaternion<T> operator * (const quaternion<T>& a, const quaternion<T>& b)
{
   quaternion<T> result(a);
   result *= b;
   return result;
}

// operator/
template <class T1, class T2>
inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator / (const quaternion<T1>& a, const T2& b)
{
   return quaternion<T1>(a.R_component_1() / b, a.R_component_2() / b, a.R_component_3() / b, a.R_component_4() / b);
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator / (const T1& a, const quaternion<T2>& b)
{
   quaternion<T2> result(a);
   result /= b;
   return result;
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
operator / (const quaternion<T1>& a, const std::complex<T2>& b)
{
   quaternion<T1> result(a);
   result /= b;
   return result;
}
template <class T1, class T2>
inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
operator / (const std::complex<T1>& a, const quaternion<T2>& b)
{
   quaternion<T2> result(a);
   result /= b;
   return result;
}
template <class T>
inline BOOST_CXX14_CONSTEXPR quaternion<T> operator / (const quaternion<T>& a, const quaternion<T>& b)
{
   quaternion<T> result(a);
   result /= b;
   return result;
}

        template<typename T>
        inline BOOST_CONSTEXPR const quaternion<T>&             operator + (quaternion<T> const & q)
        {
            return q;
        }

        template<typename T>
        inline BOOST_CONSTEXPR quaternion<T>                    operator - (quaternion<T> const & q)
        {
            return(quaternion<T>(-q.R_component_1(),-q.R_component_2(),-q.R_component_3(),-q.R_component_4()));
        }

        template<typename R, typename T>
        inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<R, T>::value, bool>::type operator == (R const & lhs, quaternion<T> const & rhs)
        {
            return    (
                        (rhs.R_component_1() == lhs)&&
                        (rhs.R_component_2() == static_cast<T>(0))&&
                        (rhs.R_component_3() == static_cast<T>(0))&&
                        (rhs.R_component_4() == static_cast<T>(0))
                    );
        }

        template<typename T, typename R>
        inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<R, T>::value, bool>::type operator == (quaternion<T> const & lhs, R const & rhs)
        {
           return rhs == lhs;
        }

        template<typename T>
        inline BOOST_CONSTEXPR bool                                operator == (::std::complex<T> const & lhs, quaternion<T> const & rhs)
        {
            return    (
                        (rhs.R_component_1() == lhs.real())&&
                        (rhs.R_component_2() == lhs.imag())&&
                        (rhs.R_component_3() == static_cast<T>(0))&&
                        (rhs.R_component_4() == static_cast<T>(0))
                    );
        }

        template<typename T>
        inline BOOST_CONSTEXPR bool                                operator == (quaternion<T> const & lhs, ::std::complex<T> const & rhs)
        {
           return rhs == lhs;
        }

        template<typename T>
        inline BOOST_CONSTEXPR bool                                operator == (quaternion<T> const & lhs, quaternion<T> const & rhs)
        {
            return    (
                        (rhs.R_component_1() == lhs.R_component_1())&&
                        (rhs.R_component_2() == lhs.R_component_2())&&
                        (rhs.R_component_3() == lhs.R_component_3())&&
                        (rhs.R_component_4() == lhs.R_component_4())
                    );
        }

        template<typename R, typename T> inline BOOST_CONSTEXPR bool operator != (R const & lhs, quaternion<T> const & rhs) { return !(lhs == rhs); }
        template<typename T, typename R> inline BOOST_CONSTEXPR bool operator != (quaternion<T> const & lhs, R const & rhs) { return !(lhs == rhs); }
        template<typename T> inline BOOST_CONSTEXPR bool operator != (::std::complex<T> const & lhs, quaternion<T> const & rhs) { return !(lhs == rhs); }
        template<typename T> inline BOOST_CONSTEXPR bool operator != (quaternion<T> const & lhs, ::std::complex<T> const & rhs) { return !(lhs == rhs); }
        template<typename T> inline BOOST_CONSTEXPR bool operator != (quaternion<T> const & lhs, quaternion<T> const & rhs) { return !(lhs == rhs); }

        // Note:    we allow the following formats, with a, b, c, and d reals
        //            a
        //            (a), (a,b), (a,b,c), (a,b,c,d)
        //            (a,(c)), (a,(c,d)), ((a)), ((a),c), ((a),(c)), ((a),(c,d)), ((a,b)), ((a,b),c), ((a,b),(c)), ((a,b),(c,d))
        template<typename T, typename charT, class traits>
        ::std::basic_istream<charT,traits> &    operator >> (    ::std::basic_istream<charT,traits> & is,
                                                                quaternion<T> & q)
        {

#ifdef    BOOST_NO_STD_LOCALE
#else
            const ::std::ctype<charT> & ct = ::std::use_facet< ::std::ctype<charT> >(is.getloc());
#endif /* BOOST_NO_STD_LOCALE */

            T    a = T();
            T    b = T();
            T    c = T();
            T    d = T();

            ::std::complex<T>    u = ::std::complex<T>();
            ::std::complex<T>    v = ::std::complex<T>();

            charT    ch = charT();
            char    cc;

            is >> ch;                                        // get the first lexeme

            if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
            cc = ch;
#else
            cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

            if    (cc == '(')                            // read "(", possible: (a), (a,b), (a,b,c), (a,b,c,d), (a,(c)), (a,(c,d)), ((a)), ((a),c), ((a),(c)), ((a),(c,d)), ((a,b)), ((a,b),c), ((a,b),(c)), ((a,b,),(c,d,))
            {
                is >> ch;                                    // get the second lexeme

                if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                cc = ch;
#else
                cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                if    (cc == '(')                        // read "((", possible: ((a)), ((a),c), ((a),(c)), ((a),(c,d)), ((a,b)), ((a,b),c), ((a,b),(c)), ((a,b,),(c,d,))
                {
                    is.putback(ch);

                    is >> u;                                // we extract the first and second components
                    a = u.real();
                    b = u.imag();

                    if    (!is.good())    goto finish;

                    is >> ch;                                // get the next lexeme

                    if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                    cc = ch;
#else
                    cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                    if        (cc == ')')                    // format: ((a)) or ((a,b))
                    {
                        q = quaternion<T>(a,b);
                    }
                    else if    (cc == ',')                // read "((a)," or "((a,b),", possible: ((a),c), ((a),(c)), ((a),(c,d)), ((a,b),c), ((a,b),(c)), ((a,b,),(c,d,))
                    {
                        is >> v;                            // we extract the third and fourth components
                        c = v.real();
                        d = v.imag();

                        if    (!is.good())    goto finish;

                        is >> ch;                                // get the last lexeme

                        if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                        cc = ch;
#else
                        cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                        if    (cc == ')')                    // format: ((a),c), ((a),(c)), ((a),(c,d)), ((a,b),c), ((a,b),(c)) or ((a,b,),(c,d,))
                        {
                            q = quaternion<T>(a,b,c,d);
                        }
                        else                            // error
                        {
                            is.setstate(::std::ios_base::failbit);
                        }
                    }
                    else                                // error
                    {
                        is.setstate(::std::ios_base::failbit);
                    }
                }
                else                                // read "(a", possible: (a), (a,b), (a,b,c), (a,b,c,d), (a,(c)), (a,(c,d))
                {
                    is.putback(ch);

                    is >> a;                                // we extract the first component

                    if    (!is.good())    goto finish;

                    is >> ch;                                // get the third lexeme

                    if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                    cc = ch;
#else
                    cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                    if        (cc == ')')                    // format: (a)
                    {
                        q = quaternion<T>(a);
                    }
                    else if    (cc == ',')                // read "(a,", possible: (a,b), (a,b,c), (a,b,c,d), (a,(c)), (a,(c,d))
                    {
                        is >> ch;                            // get the fourth lexeme

                        if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                        cc = ch;
#else
                        cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                        if    (cc == '(')                // read "(a,(", possible: (a,(c)), (a,(c,d))
                        {
                            is.putback(ch);

                            is >> v;                        // we extract the third and fourth component

                            c = v.real();
                            d = v.imag();

                            if    (!is.good())    goto finish;

                            is >> ch;                        // get the ninth lexeme

                            if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                            cc = ch;
#else
                            cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                            if    (cc == ')')                // format: (a,(c)) or (a,(c,d))
                            {
                                q = quaternion<T>(a,b,c,d);
                            }
                            else                        // error
                            {
                                is.setstate(::std::ios_base::failbit);
                            }
                        }
                        else                        // read "(a,b", possible: (a,b), (a,b,c), (a,b,c,d)
                        {
                            is.putback(ch);

                            is >> b;                        // we extract the second component

                            if    (!is.good())    goto finish;

                            is >> ch;                        // get the fifth lexeme

                            if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                            cc = ch;
#else
                            cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                            if    (cc == ')')                // format: (a,b)
                            {
                                q = quaternion<T>(a,b);
                            }
                            else if    (cc == ',')        // read "(a,b,", possible: (a,b,c), (a,b,c,d)
                            {
                                is >> c;                    // we extract the third component

                                if    (!is.good())    goto finish;

                                is >> ch;                    // get the seventh lexeme

                                if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                                cc = ch;
#else
                                cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                                if        (cc == ')')        // format: (a,b,c)
                                {
                                    q = quaternion<T>(a,b,c);
                                }
                                else if    (cc == ',')    // read "(a,b,c,", possible: (a,b,c,d)
                                {
                                    is >> d;                // we extract the fourth component

                                    if    (!is.good())    goto finish;

                                    is >> ch;                // get the ninth lexeme

                                    if    (!is.good())    goto finish;

#ifdef    BOOST_NO_STD_LOCALE
                                    cc = ch;
#else
                                    cc = ct.narrow(ch, char());
#endif /* BOOST_NO_STD_LOCALE */

                                    if    (cc == ')')        // format: (a,b,c,d)
                                    {
                                        q = quaternion<T>(a,b,c,d);
                                    }
                                    else                // error
                                    {
                                        is.setstate(::std::ios_base::failbit);
                                    }
                                }
                                else                    // error
                                {
                                    is.setstate(::std::ios_base::failbit);
                                }
                            }
                            else                        // error
                            {
                                is.setstate(::std::ios_base::failbit);
                            }
                        }
                    }
                    else                                // error
                    {
                        is.setstate(::std::ios_base::failbit);
                    }
                }
            }
            else                                        // format:    a
            {
                is.putback(ch);

                is >> a;                                    // we extract the first component

                if    (!is.good())    goto finish;

                q = quaternion<T>(a);
            }

            finish:
            return(is);
        }

        template<typename T, typename charT, class traits>
        ::std::basic_ostream<charT,traits> &    operator << (    ::std::basic_ostream<charT,traits> & os,
                                                                quaternion<T> const & q)
        {
            ::std::basic_ostringstream<charT,traits>    s;

            s.flags(os.flags());
#ifdef    BOOST_NO_STD_LOCALE
#else
            s.imbue(os.getloc());
#endif /* BOOST_NO_STD_LOCALE */
            s.precision(os.precision());

            s << '('    << q.R_component_1() << ','
                        << q.R_component_2() << ','
                        << q.R_component_3() << ','
                        << q.R_component_4() << ')';

            return os << s.str();
        }

        // values

        template<typename T>
        inline BOOST_CONSTEXPR T real(quaternion<T> const & q)
        {
            return(q.real());
        }

        template<typename T>
        inline BOOST_CONSTEXPR quaternion<T> unreal(quaternion<T> const & q)
        {
            return(q.unreal());
        }

        template<typename T>
        inline T sup(quaternion<T> const & q)
        {
            using    ::std::abs;
            return (std::max)((std::max)(abs(q.R_component_1()), abs(q.R_component_2())), (std::max)(abs(q.R_component_3()), abs(q.R_component_4())));
        }

        template<typename T>
        inline T l1(quaternion<T> const & q)
        {
           using    ::std::abs;
           return abs(q.R_component_1()) + abs(q.R_component_2()) + abs(q.R_component_3()) + abs(q.R_component_4());
        }

        template<typename T>
        inline T abs(quaternion<T> const & q)
        {
            using    ::std::abs;
            using    ::std::sqrt;

            T            maxim = sup(q);    // overflow protection

            if    (maxim == static_cast<T>(0))
            {
                return(maxim);
            }
            else
            {
                T    mixam = static_cast<T>(1)/maxim;    // prefer multiplications over divisions

                T a = q.R_component_1() * mixam;
                T b = q.R_component_2() * mixam;
                T c = q.R_component_3() * mixam;
                T d = q.R_component_4() * mixam;

                a *= a;
                b *= b;
                c *= c;
                d *= d;

                return(maxim * sqrt(a + b + c + d));
            }

            //return(sqrt(norm(q)));
        }

        // Note:    This is the Cayley norm, not the Euclidean norm...

        template<typename T>
        inline BOOST_CXX14_CONSTEXPR T norm(quaternion<T>const  & q)
        {
            return(real(q*conj(q)));
        }

        template<typename T>
        inline BOOST_CONSTEXPR quaternion<T> conj(quaternion<T> const & q)
        {
            return(quaternion<T>(   +q.R_component_1(),
                                    -q.R_component_2(),
                                    -q.R_component_3(),
                                    -q.R_component_4()));
        }

        template<typename T>
        inline quaternion<T>                    spherical(  T const & rho,
                                                            T const & theta,
                                                            T const & phi1,
                                                            T const & phi2)
        {
            using ::std::cos;
            using ::std::sin;

            //T    a = cos(theta)*cos(phi1)*cos(phi2);
            //T    b = sin(theta)*cos(phi1)*cos(phi2);
            //T    c = sin(phi1)*cos(phi2);
            //T    d = sin(phi2);

            T    courrant = static_cast<T>(1);

            T    d = sin(phi2);

            courrant *= cos(phi2);

            T    c = sin(phi1)*courrant;

            courrant *= cos(phi1);

            T    b = sin(theta)*courrant;
            T    a = cos(theta)*courrant;

            return(rho*quaternion<T>(a,b,c,d));
        }

        template<typename T>
        inline quaternion<T>                    semipolar(  T const & rho,
                                                            T const & alpha,
                                                            T const & theta1,
                                                            T const & theta2)
        {
            using ::std::cos;
            using ::std::sin;

            T    a = cos(alpha)*cos(theta1);
            T    b = cos(alpha)*sin(theta1);
            T    c = sin(alpha)*cos(theta2);
            T    d = sin(alpha)*sin(theta2);

            return(rho*quaternion<T>(a,b,c,d));
        }

        template<typename T>
        inline quaternion<T>                    multipolar( T const & rho1,
                                                            T const & theta1,
                                                            T const & rho2,
                                                            T const & theta2)
        {
            using ::std::cos;
            using ::std::sin;

            T    a = rho1*cos(theta1);
            T    b = rho1*sin(theta1);
            T    c = rho2*cos(theta2);
            T    d = rho2*sin(theta2);

            return(quaternion<T>(a,b,c,d));
        }

        template<typename T>
        inline quaternion<T>                    cylindrospherical(  T const & t,
                                                                    T const & radius,
                                                                    T const & longitude,
                                                                    T const & latitude)
        {
            using ::std::cos;
            using ::std::sin;

            T    b = radius*cos(longitude)*cos(latitude);
            T    c = radius*sin(longitude)*cos(latitude);
            T    d = radius*sin(latitude);

            return(quaternion<T>(t,b,c,d));
        }

        template<typename T>
        inline quaternion<T>                    cylindrical(T const & r,
                                                            T const & angle,
                                                            T const & h1,
                                                            T const & h2)
        {
            using ::std::cos;
            using ::std::sin;

            T    a = r*cos(angle);
            T    b = r*sin(angle);

            return(quaternion<T>(a,b,h1,h2));
        }

        // transcendentals
        // (please see the documentation)

        template<typename T>
        inline quaternion<T>                    exp(quaternion<T> const & q)
        {
            using    ::std::exp;
            using    ::std::cos;

            using    ::boost::math::sinc_pi;

            T    u = exp(real(q));

            T    z = abs(unreal(q));

            T    w = sinc_pi(z);

            return(u*quaternion<T>(cos(z),
                w*q.R_component_2(), w*q.R_component_3(),
                w*q.R_component_4()));
        }

        template<typename T>
        inline quaternion<T>                    cos(quaternion<T> const & q)
        {
            using    ::std::sin;
            using    ::std::cos;
            using    ::std::cosh;

            using    ::boost::math::sinhc_pi;

            T    z = abs(unreal(q));

            T    w = -sin(q.real())*sinhc_pi(z);

            return(quaternion<T>(cos(q.real())*cosh(z),
                w*q.R_component_2(), w*q.R_component_3(),
                w*q.R_component_4()));
        }

        template<typename T>
        inline quaternion<T>                    sin(quaternion<T> const & q)
        {
            using    ::std::sin;
            using    ::std::cos;
            using    ::std::cosh;

            using    ::boost::math::sinhc_pi;

            T    z = abs(unreal(q));

            T    w = +cos(q.real())*sinhc_pi(z);

            return(quaternion<T>(sin(q.real())*cosh(z),
                w*q.R_component_2(), w*q.R_component_3(),
                w*q.R_component_4()));
        }

        template<typename T>
        inline quaternion<T>                    tan(quaternion<T> const & q)
        {
            return(sin(q)/cos(q));
        }

        template<typename T>
        inline quaternion<T>                    cosh(quaternion<T> const & q)
        {
            return((exp(+q)+exp(-q))/static_cast<T>(2));
        }

        template<typename T>
        inline quaternion<T>                    sinh(quaternion<T> const & q)
        {
            return((exp(+q)-exp(-q))/static_cast<T>(2));
        }

        template<typename T>
        inline quaternion<T>                    tanh(quaternion<T> const & q)
        {
            return(sinh(q)/cosh(q));
        }

        template<typename T>
        quaternion<T>                            pow(quaternion<T> const & q,
                                                    int n)
        {
            if        (n > 1)
            {
                int    m = n>>1;

                quaternion<T>    result = pow(q, m);

                result *= result;

                if    (n != (m<<1))
                {
                    result *= q; // n odd
                }

                return(result);
            }
            else if    (n == 1)
            {
                return(q);
            }
            else if    (n == 0)
            {
                return(quaternion<T>(static_cast<T>(1)));
            }
            else    /* n < 0 */
            {
                return(pow(quaternion<T>(static_cast<T>(1))/q,-n));
            }
        }
    }
}

#endif /* BOOST_QUATERNION_HPP */


#pragma once

#include <Eigen/Core>
#include <autodiff/autodiff_types.hpp>

/**
 * @namespace ipc::rigid:
 * @brief
 */
namespace ipc::rigid {

template <typename T> using Vector2T = Eigen::Matrix<T, 2, 1>;

/**
 * @brief Given toi returns the position \f$\alpha\f$ along the
 * \f$edge_{ij}\f$ where the impact between vertex \f$k\f$ and
 * \f$edge_{ij}\f$  takes place.
 *
 *   @param[in]   V_{i,j,k}       : vertex positions
 *   @param[in]   U_{i,j,k}       : vertex displacements
 *   @param[in]   toi             : time of impact
 *
 *   @param[out]  alpha          : position along the \f$edge_{ij}\f$ where
 * the impact takes place
 *
 * @return true if a valid \f$\alpha\f$ exists.
 */
template <typename T>
bool temporal_parameterization_to_spatial(
    const Eigen::Vector2d& Vi,
    const Eigen::Vector2d& Vj,
    const Eigen::Vector2d& Vk,
    const Vector2T<T>& Ui,
    const Vector2T<T>& Uj,
    const Vector2T<T>& Uk,
    const T& toi,
    T& alpha);

/**
 * Computes the time of impact between an \f$edge_{ij}\f$ and vertex \f$k\f$
 *
 *   @param[in]   V_{i,j,k}       : vertex positions
 *   @param[in]   U_{i,j,k}       : vertex displacements
 *
 *   @param[out]  toi             : time of FIRST impact
 *
 * @return true if an impact happens at time \f$ t \in [0, 1]\f$
 */
template <typename T>
bool compute_edge_vertex_time_of_impact(
    const Eigen::Vector2d& Vi,
    const Eigen::Vector2d& Vj,
    const Eigen::Vector2d& Vk,
    const Vector2T<T>& Ui,
    const Vector2T<T>& Uj,
    const Vector2T<T>& Uk,
    T& toi,
    T& alpha);

template <typename T>
bool compute_edge_vertex_time_of_impact(
    const Eigen::Vector2d& Vi,
    const Eigen::Vector2d& Vj,
    const Eigen::Vector2d& Vk,
    const Vector2T<T>& Ui,
    const Vector2T<T>& Uj,
    const Vector2T<T>& Uk,
    T& toi)
{
    T alpha;
    return compute_edge_vertex_time_of_impact(
        Vi, Vj, Vk, Ui, Uj, Uk, toi, alpha);
}

} // namespace ipc::rigid

#include "edge_vertex_ccd.tpp"


/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

#include <boost/unordered_set.hpp>
#include <graphlab.hpp>
#include <graphlab/macros_def.hpp>
#include "../../src/graphlab/util/timer.hpp"
/**
 *
 * In this program we implement the "k-core" decomposition algorithm.
 * We use a parallel variant of
 *
 * V. Batagelj and M. Zaversnik, An O(m) algorithm for cores
 * decomposition of networks,
 *
 *  - Essentially, recursively remove everything with degree 1
 *  - Then recursively remove everything with degree 2
 *  - etc.
 */

/*
 * Each vertex maintains a "degree" count. If this value
 * is 0, the vertex is "deleted"
 */
typedef int vertex_data_type;

/*
 * Don't need any edges
 */
typedef graphlab::empty edge_data_type;

/*
 * Define the type of the graph
 */
typedef graphlab::distributed_graph<vertex_data_type,
                                    edge_data_type> graph_type;

// The current K to compute
size_t CURRENT_K;

size_t ITERATIONS = 0;

/*
 * The core K-core implementation.
 * The basic concept is simple.
 * Each vertex maintains a count of the number of adjacent edges.
 * If a vertex receives a message, the message contains the number of
 * adjacent edges deleted. The vertex then updates its counter.
 * If the counter falls below K, it deletes itself
 * (set the adjacent count to 0) and signals each of its neighbors
 * with a message of 1.
 */
class k_core :
  public graphlab::ivertex_program<graph_type,
                                   graphlab::empty, // gathers are integral
                                   int>,   // messages are integral
  public graphlab::IS_POD_TYPE  {
public:
  // the last received message
  int msg;

  /* Each vertex can only signal once. I set this flag
   * if it is the first time this vertex falls below K, so I can
   * initiate scattering
   */
  bool just_deleted;

  k_core():msg(0),just_deleted(false) { }

  /* The message contains the number of adjacent edges deleted.
   * Store the message in the program, and reset the just_deleted flag
   */
  void init(icontext_type& context, const vertex_type& vertex,
            const message_type& message) {
    msg = message;
    just_deleted = false;
  }

  // gather is never invoked
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::NO_EDGES;
  }

  /* On apply, if the vertex has not yet been deleted,
   * decrement the counter on the vertex.
   * If the adjacency count of the vertex falls below K,
   * the vertex shall be deleted.
   * We set the vertex data to 0 to designate that it is deleted
   * and Set the just_deleted flag to signal the neighbors in scatter
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& unused) {
    if (vertex.data() > 0) {
      vertex.data() -= msg;
      if (vertex.data() < CURRENT_K) {
        just_deleted = true;
        vertex.data() = 0;
      }
    }
  }

  /*
   * If the vertex is deleted, we signal all neighbors on the scatter
   */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    return just_deleted ?
      graphlab::ALL_EDGES : graphlab::NO_EDGES;
  }

  /*
   * For each neighboring vertex, if it is not yet deleted,
   * signal it.
   */
  void scatter(icontext_type& context,
               const vertex_type& vertex,
               edge_type& edge) const {
    vertex_type other = edge.source().id() == vertex.id() ?
      edge.target() : edge.source();
    if (other.data() > 0) {
      context.signal(other, 1);
    }
  }

};

// type of the synchronous_engine
typedef graphlab::synchronous_engine<k_core> engine_type;

/*
 * Called before any graph operation is performed.
 * Initializes all vertex data to the number of adjacent edges.
 * Can be called from a graph.transform_vertices()
 */
void initialize_vertex_values(graph_type::vertex_type& v) {
  v.data() = v.num_in_edges() + v.num_out_edges();
}

/*
 * Signals all non-deleted vertices with degree less than K.
 * Can be called from an engine.map_reduce_vertices()
 * We return empty since no reduction is performed. Only the map.
 */
graphlab::empty signal_vertices_at_k(engine_type::icontext_type& ctx,
                                     const graph_type::vertex_type& vertex) {
  if (vertex.data() > 0 && vertex.data() < CURRENT_K) {
    ctx.signal(vertex, 0);
  }
  return graphlab::empty();
}

/*
 * Counts the number of un-deleted vertices.
 */
size_t count_active_vertices(const graph_type::vertex_type& vertex) {
  return vertex.data() > 0;
}

/*
 * Counts the degree of each un-deleted vertex. Half of this
 * will be the size of the K-core graph.
 */
size_t double_count_active_edges(const graph_type::vertex_type& vertex) {
  return (size_t) vertex.data();
}

/*
 * Saves the graph in a tsv format with the condition that
 * the adjacent vertices have not yet been deleted.
 * This allows saving of the k-core graph.
 */
struct save_core_at_k {
  std::string save_vertex(graph_type::vertex_type) { return ""; }
  std::string save_edge(graph_type::edge_type e) {
    if (e.source().data() > 0 && e.target().data() > 0) {
      return graphlab::tostr(e.source().id()) + "\t" +
        graphlab::tostr(e.target().id()) + "\n";
    }
    else return "";
  }
};

int main(int argc, char** argv) {
  std::cout << "Computes a k-core decomposition of a graph.\n\n";

  graphlab::command_line_options clopts
    ("K-Core decomposition. This program "
     "computes the K-Core decomposition of a graph, for K ranging from [kmin] "
     "to [kmax]. The size of the remaining K-core graph at each K is printed. "
     "The [savecores] allow the saving of each K-Core graph in a TSV format"
     );
  std::string prefix, format;
  size_t kmin = 0;
  size_t kmax = (size_t)(-1);
  std::string savecores;
  clopts.attach_option("graph", prefix,
                       "Graph input. reads all graphs matching prefix*");
  clopts.attach_option("format", format,
                       "The graph format");
  clopts.attach_option("kmin", kmin,
                       "Compute the k-Core for k the range [kmin,kmax]");
  clopts.attach_option("kmax", kmax,
                       "Compute the k-Core for k the range [kmin,kmax]");
  clopts.attach_option("savecores", savecores,
                       "If non-empty, will save tsv of each core with prefix [savecores].K.");
	clopts.attach_option("iterations", ITERATIONS,
                       "If set, will force the use of the synchronous engine"
                       "overriding any engine option set by the --engine parameter. "
                       "Runs complete (non-dynamic) PageRank for a fixed "
                       "number of iterations. Also overrides the iterations "
                       "option in the engine");
	size_t powerlaw = 0;
  clopts.attach_option("powerlaw", powerlaw,
                       "Generate a synthetic powerlaw degree graph. ");
  double alpha = 2.1, beta = 2.2;
  clopts.attach_option("alpha", alpha,
                         "Power-law constant for indegree ");
  clopts.attach_option("beta", beta,
                         "Power-law constant for outdegree ");
  std::string result_file;
  clopts.attach_option("result_file", result_file,
                       "If set, will save the test result to the"
                       "specific file");

  if(!clopts.parse(argc, argv)) return EXIT_FAILURE;
  if (kmax < kmin) {
    std::cout << "kmax must be at least as large as kmin\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }

  // Initialize control plane using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  if (ITERATIONS) {
    // make sure this is the synchronous engine
    dc.cout() << "--iterations set. Forcing Synchronous engine, and running "
              << "for " << ITERATIONS << " iterations." << std::endl;
    //clopts.get_engine_args().set_option("type", "synchronous");
    clopts.get_engine_args().set_option("max_iterations", ITERATIONS);
    clopts.get_engine_args().set_option("sched_allv", true);
  }

  int ntrials = 20;
  int trial_results1[20];
  double trial_results2[20][8];
  uint32_t seed_set[20] = {1492133106, 680965948, 2040586311, 73972395, 942196338, 819390547, 1643934785, 1707678784, 401305863, 1051761031, 956889080, 1387946621, 1523349375, 1620677309, 592759340, 1459650384, 1406812251, 349206043, 255545576, 1070228652};
  for(int i = 0; i < ntrials; i++)
  {

		// random seed
		// dc.cout() << seed_set[i] << std::endl;
		clopts.get_graph_args().set_option("seed", seed_set[i]);

	  // load graph
	  graph_type graph(dc, clopts);

	  if(powerlaw > 0) { // make a synthetic graph
		  dc.cout() << "Loading synthetic Powerlaw graph." << std::endl;
		  graph.load_synthetic_powerlaw(powerlaw, alpha, beta, 100000000);
		}
		else if (prefix.length() > 0) { // Load the graph from a file
			if(dc.procid() == 24)
		  std::cout << "Loading graph in format: "<< format << std::endl;
		  graph.load_format(prefix, format);
		}
		else {
		  dc.cout() << "graph or powerlaw option must be specified" << std::endl;
		  clopts.print_description();
		  return 0;
		}

	  graphlab::timer timer;
	  timer.start();
	  graph.finalize();
	  const double ingress_time = timer.current_time();
	  dc.cout() << "Finalizing graph. Finished in "
		  << ingress_time << std::endl;
	  dc.cout() << "Number of vertices: " << graph.num_vertices() << std::endl
				<< "Number of edges:    " << graph.num_edges() << std::endl;

	  timer.start();
	  graphlab::synchronous_engine<k_core> engine(dc, graph, clopts);

	  // initialize the vertex data with the degree
	  graph.transform_vertices(initialize_vertex_values);

	  // for each K value
	  for (CURRENT_K = kmin; CURRENT_K <= kmax; CURRENT_K++) {
		// signal all vertices with degree less than K
		engine.map_reduce_vertices<graphlab::empty>(signal_vertices_at_k);
		// recursively delete all vertices with degree less than K
		engine.start();
		// count the number of vertices and edges remaining
		size_t numv = graph.map_reduce_vertices<size_t>(count_active_vertices);
		size_t nume = graph.map_reduce_vertices<size_t>(double_count_active_edges) / 2;
		if (numv == 0) break;
		// Output the size of the graph
		dc.cout() << "K=" << CURRENT_K << ":  #V = "
				  << numv << "   #E = " << nume << std::endl;

		// Saves the result if requested
		if (savecores != "") {
		  graph.save(savecores + "." + graphlab::tostr(CURRENT_K) + ".",
					 save_core_at_k(),
					 false, /* no compression */
					 false, /* do not save vertex */
					 true, /* save edge */
					 clopts.get_ncpus()); /* one file per machine */
		}
	  }
	  const double runtime = timer.current_time();

	  if(dc.procid() == 0) {
          std::cout << graph.num_replicas() << "\t"
                    << (double)graph.num_replicas()/graph.num_vertices() << "\t"
                    << graph.get_edge_balance() << "\t"
                    << graph.get_vertex_balance() << "\t"
                    << ingress_time << "\t"
					<< runtime << "\t"
                    << engine.get_exec_time() << "\t"
                    << engine.get_one_itr_time() << "\t"
                    << engine.get_compute_balance() << "\t"
                    << std::endl;
          trial_results1[i] = graph.num_replicas();
          trial_results2[i][0] = (double)graph.num_replicas()/graph.num_vertices();
          trial_results2[i][1] = graph.get_edge_balance();
          trial_results2[i][2] = graph.get_vertex_balance() ;
          trial_results2[i][3] = ingress_time;
          trial_results2[i][4] = runtime;
          trial_results2[i][5] = engine.get_exec_time();
          trial_results2[i][6] = engine.get_one_itr_time();
          trial_results2[i][7] = engine.get_compute_balance();

          if(result_file != "") {
              std::cout << "saving the result to " << result_file << std::endl;
              std::ofstream fout(result_file.c_str(), std::ios::app);
              if(powerlaw > 0) {
                  fout << "powerlaw synthetic: " << powerlaw << "\t"
                      << alpha << "\t" << beta << "\t" << dc.numprocs() << std::endl;
              }
              else {
                  fout << "real-world graph: " << prefix << "\t"
                      << graph.num_vertices() << "\t" << graph.num_edges()
                      << "\t" << dc.numprocs() << std::endl;
              }
              std::string ingress_method = "";
              clopts.get_graph_args().get_option("ingress", ingress_method);
              fout << ingress_method << "\t";
              fout << graph.num_replicas() << "\t"
                  << (double)graph.num_replicas()/graph.num_vertices() << "\t"
                  << graph.get_edge_balance() << "\t"
                  << graph.get_vertex_balance() << "\t"
                  << ingress_time << "\t"
				  << runtime << "\t"
                  << engine.get_exec_time() << "\t"
                  << engine.get_one_itr_time() << "\t"
                  << engine.get_compute_balance() << "\t"
                  << std::endl;
              fout.close();
          }
      }

  }

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // End of main


//  Copyright Benjamin Sobotta 2012

//  Use, modification and distribution are subject to 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)

#ifndef BOOST_STATS_SKEW_NORMAL_HPP
#define BOOST_STATS_SKEW_NORMAL_HPP

// http://en.wikipedia.org/wiki/Skew_normal_distribution
// http://azzalini.stat.unipd.it/SN/
// Also:
// Azzalini, A. (1985). "A class of distributions which includes the normal ones".
// Scand. J. Statist. 12: 171-178.

#include <boost/math/distributions/fwd.hpp> // TODO add skew_normal distribution to fwd.hpp!
#include <boost/math/special_functions/owens_t.hpp> // Owen's T function
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/tuple.hpp>
#include <boost/math/tools/roots.hpp> // Newton-Raphson
#include <boost/assert.hpp>
#include <boost/math/distributions/detail/generic_mode.hpp> // pdf max finder.

#include <utility>
#include <algorithm> // std::lower_bound, std::distance

namespace boost{ namespace math{

  namespace detail
  {
    template <class RealType, class Policy>
    inline bool check_skew_normal_shape(
      const char* function,
      RealType shape,
      RealType* result,
      const Policy& pol)
    {
      if(!(boost::math::isfinite)(shape))
      {
        *result =
          policies::raise_domain_error<RealType>(function,
          "Shape parameter is %1%, but must be finite!",
          shape, pol);
        return false;
      }
      return true;
    }

  } // namespace detail

  template <class RealType = double, class Policy = policies::policy<> >
  class skew_normal_distribution
  {
  public:
    typedef RealType value_type;
    typedef Policy policy_type;

    skew_normal_distribution(RealType l_location = 0, RealType l_scale = 1, RealType l_shape = 0)
      : location_(l_location), scale_(l_scale), shape_(l_shape)
    { // Default is a 'standard' normal distribution N01. (shape=0 results in the normal distribution with no skew)
      static const char* function = "boost::math::skew_normal_distribution<%1%>::skew_normal_distribution";

      RealType result;
      detail::check_scale(function, l_scale, &result, Policy());
      detail::check_location(function, l_location, &result, Policy());
      detail::check_skew_normal_shape(function, l_shape, &result, Policy());
    }

    RealType location()const
    {
      return location_;
    }

    RealType scale()const
    {
      return scale_;
    }

    RealType shape()const
    {
      return shape_;
    }

  private:
    //
    // Data members:
    //
    RealType location_;  // distribution location.
    RealType scale_;    // distribution scale.
    RealType shape_;    // distribution shape.
  }; // class skew_normal_distribution

  typedef skew_normal_distribution<double> skew_normal;

  template <class RealType, class Policy>
  inline const std::pair<RealType, RealType> range(const skew_normal_distribution<RealType, Policy>& /*dist*/)
  { // Range of permissible values for random variable x.
    using boost::math::tools::max_value;
    return std::pair<RealType, RealType>(
       std::numeric_limits<RealType>::has_infinity ? -std::numeric_limits<RealType>::infinity() : -max_value<RealType>(),
       std::numeric_limits<RealType>::has_infinity ? std::numeric_limits<RealType>::infinity() : max_value<RealType>()); // - to + max value.
  }

  template <class RealType, class Policy>
  inline const std::pair<RealType, RealType> support(const skew_normal_distribution<RealType, Policy>& /*dist*/)
  { // Range of supported values for random variable x.
    // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.

    using boost::math::tools::max_value;
    return std::pair<RealType, RealType>(-max_value<RealType>(),  max_value<RealType>()); // - to + max value.
  }

  template <class RealType, class Policy>
  inline RealType pdf(const skew_normal_distribution<RealType, Policy>& dist, const RealType& x)
  {
    const RealType scale = dist.scale();
    const RealType location = dist.location();
    const RealType shape = dist.shape();

    static const char* function = "boost::math::pdf(const skew_normal_distribution<%1%>&, %1%)";

    RealType result = 0;
    if(false == detail::check_scale(function, scale, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_location(function, location, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_skew_normal_shape(function, shape, &result, Policy()))
    {
      return result;
    }
    if((boost::math::isinf)(x))
    {
       return 0; // pdf + and - infinity is zero.
    }
    // Below produces MSVC 4127 warnings, so the above used instead.
    //if(std::numeric_limits<RealType>::has_infinity && abs(x) == std::numeric_limits<RealType>::infinity())
    //{ // pdf + and - infinity is zero.
    //  return 0;
    //}
    if(false == detail::check_x(function, x, &result, Policy()))
    {
      return result;
    }

    const RealType transformed_x = (x-location)/scale;

    normal_distribution<RealType, Policy> std_normal;

    result = pdf(std_normal, transformed_x) * cdf(std_normal, shape*transformed_x) * 2 / scale;

    return result;
  } // pdf

  template <class RealType, class Policy>
  inline RealType cdf(const skew_normal_distribution<RealType, Policy>& dist, const RealType& x)
  {
    const RealType scale = dist.scale();
    const RealType location = dist.location();
    const RealType shape = dist.shape();

    static const char* function = "boost::math::cdf(const skew_normal_distribution<%1%>&, %1%)";
    RealType result = 0;
    if(false == detail::check_scale(function, scale, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_location(function, location, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_skew_normal_shape(function, shape, &result, Policy()))
    {
      return result;
    }
    if((boost::math::isinf)(x))
    {
      if(x < 0) return 0; // -infinity
      return 1; // + infinity
    }
    // These produce MSVC 4127 warnings, so the above used instead.
    //if(std::numeric_limits<RealType>::has_infinity && x == std::numeric_limits<RealType>::infinity())
    //{ // cdf +infinity is unity.
    //  return 1;
    //}
    //if(std::numeric_limits<RealType>::has_infinity && x == -std::numeric_limits<RealType>::infinity())
    //{ // cdf -infinity is zero.
    //  return 0;
    //}
    if(false == detail::check_x(function, x, &result, Policy()))
    {
      return result;
    }

    const RealType transformed_x = (x-location)/scale;

    normal_distribution<RealType, Policy> std_normal;

    result = cdf(std_normal, transformed_x) - owens_t(transformed_x, shape)*static_cast<RealType>(2);

    return result;
  } // cdf

  template <class RealType, class Policy>
  inline RealType cdf(const complemented2_type<skew_normal_distribution<RealType, Policy>, RealType>& c)
  {
    const RealType scale = c.dist.scale();
    const RealType location = c.dist.location();
    const RealType shape = c.dist.shape();
    const RealType x = c.param;

    static const char* function = "boost::math::cdf(const complement(skew_normal_distribution<%1%>&), %1%)";

    if((boost::math::isinf)(x))
    {
      if(x < 0) return 1; // cdf complement -infinity is unity.
      return 0; // cdf complement +infinity is zero
    }
    // These produce MSVC 4127 warnings, so the above used instead.
    //if(std::numeric_limits<RealType>::has_infinity && x == std::numeric_limits<RealType>::infinity())
    //{ // cdf complement +infinity is zero.
    //  return 0;
    //}
    //if(std::numeric_limits<RealType>::has_infinity && x == -std::numeric_limits<RealType>::infinity())
    //{ // cdf complement -infinity is unity.
    //  return 1;
    //}
    RealType result = 0;
    if(false == detail::check_scale(function, scale, &result, Policy()))
      return result;
    if(false == detail::check_location(function, location, &result, Policy()))
      return result;
    if(false == detail::check_skew_normal_shape(function, shape, &result, Policy()))
      return result;
    if(false == detail::check_x(function, x, &result, Policy()))
      return result;

    const RealType transformed_x = (x-location)/scale;

    normal_distribution<RealType, Policy> std_normal;

    result = cdf(complement(std_normal, transformed_x)) + owens_t(transformed_x, shape)*static_cast<RealType>(2);
    return result;
  } // cdf complement

  template <class RealType, class Policy>
  inline RealType location(const skew_normal_distribution<RealType, Policy>& dist)
  {
    return dist.location();
  }

  template <class RealType, class Policy>
  inline RealType scale(const skew_normal_distribution<RealType, Policy>& dist)
  {
    return dist.scale();
  }

  template <class RealType, class Policy>
  inline RealType shape(const skew_normal_distribution<RealType, Policy>& dist)
  {
    return dist.shape();
  }

  template <class RealType, class Policy>
  inline RealType mean(const skew_normal_distribution<RealType, Policy>& dist)
  {
    BOOST_MATH_STD_USING  // for ADL of std functions

    using namespace boost::math::constants;

    //const RealType delta = dist.shape() / sqrt(static_cast<RealType>(1)+dist.shape()*dist.shape());

    //return dist.location() + dist.scale() * delta * root_two_div_pi<RealType>();

    return dist.location() + dist.scale() * dist.shape() / sqrt(pi<RealType>()+pi<RealType>()*dist.shape()*dist.shape()) * root_two<RealType>();
  }

  template <class RealType, class Policy>
  inline RealType variance(const skew_normal_distribution<RealType, Policy>& dist)
  {
    using namespace boost::math::constants;

    const RealType delta2 = static_cast<RealType>(1) / (static_cast<RealType>(1)+static_cast<RealType>(1)/(dist.shape()*dist.shape()));
    //const RealType inv_delta2 = static_cast<RealType>(1)+static_cast<RealType>(1)/(dist.shape()*dist.shape());

    RealType variance = dist.scale()*dist.scale()*(static_cast<RealType>(1)-two_div_pi<RealType>()*delta2);
    //RealType variance = dist.scale()*dist.scale()*(static_cast<RealType>(1)-two_div_pi<RealType>()/inv_delta2);

    return variance;
  }

  namespace detail
  {
    /*
      TODO No closed expression for mode, so use max of pdf.
    */

    template <class RealType, class Policy>
    inline RealType mode_fallback(const skew_normal_distribution<RealType, Policy>& dist)
    { // mode.
        static const char* function = "mode(skew_normal_distribution<%1%> const&)";
        const RealType scale = dist.scale();
        const RealType location = dist.location();
        const RealType shape = dist.shape();

        RealType result;
        if(!detail::check_scale(
          function,
          scale, &result, Policy())
          ||
        !detail::check_skew_normal_shape(
          function,
          shape,
          &result,
          Policy()))
        return result;

        if( shape == 0 )
        {
          return location;
        }

        if( shape < 0 )
        {
          skew_normal_distribution<RealType, Policy> D(0, 1, -shape);
          result = mode_fallback(D);
          result = location-scale*result;
          return result;
        }

        BOOST_MATH_STD_USING

        // 21 elements
        static const RealType shapes[] = {
          0.0,
          1.000000000000000e-004,
          2.069138081114790e-004,
          4.281332398719396e-004,
          8.858667904100824e-004,
          1.832980710832436e-003,
          3.792690190732250e-003,
          7.847599703514606e-003,
          1.623776739188722e-002,
          3.359818286283781e-002,
          6.951927961775606e-002,
          1.438449888287663e-001,
          2.976351441631319e-001,
          6.158482110660261e-001,
          1.274274985703135e+000,
          2.636650898730361e+000,
          5.455594781168514e+000,
          1.128837891684688e+001,
          2.335721469090121e+001,
          4.832930238571753e+001,
          1.000000000000000e+002};

        // 21 elements
        static const RealType guess[] = {
          0.0,
          5.000050000525391e-005,
          1.500015000148736e-004,
          3.500035000350010e-004,
          7.500075000752560e-004,
          1.450014500145258e-003,
          3.050030500305390e-003,
          6.250062500624765e-003,
          1.295012950129504e-002,
          2.675026750267495e-002,
          5.525055250552491e-002,
          1.132511325113255e-001,
          2.249522495224952e-001,
          3.992539925399257e-001,
          5.353553535535358e-001,
          4.954549545495457e-001,
          3.524535245352451e-001,
          2.182521825218249e-001,
          1.256512565125654e-001,
          6.945069450694508e-002,
          3.735037350373460e-002
        };

        const RealType* result_ptr = std::lower_bound(shapes, shapes+21, shape);

        typedef typename std::iterator_traits<RealType*>::difference_type diff_type;

        const diff_type d = std::distance(shapes, result_ptr);

        BOOST_ASSERT(d > static_cast<diff_type>(0));

        // refine
        if(d < static_cast<diff_type>(21)) // shape smaller 100
        {
          result = guess[d-static_cast<diff_type>(1)]
            + (guess[d]-guess[d-static_cast<diff_type>(1)])/(shapes[d]-shapes[d-static_cast<diff_type>(1)])
            * (shape-shapes[d-static_cast<diff_type>(1)]);
        }
        else // shape greater 100
        {
          result = 1e-4;
        }

        skew_normal_distribution<RealType, Policy> helper(0, 1, shape);

        result = detail::generic_find_mode_01(helper, result, function);

        result = result*scale + location;

        return result;
    } // mode_fallback

    /*
     * TODO No closed expression for mode, so use f'(x) = 0
     */
    template <class RealType, class Policy>
    struct skew_normal_mode_functor
    {
      skew_normal_mode_functor(const boost::math::skew_normal_distribution<RealType, Policy> dist)
        : distribution(dist)
      {
      }

      boost::math::tuple<RealType, RealType> operator()(RealType const& x)
      {
        normal_distribution<RealType, Policy> std_normal;
        const RealType shape = distribution.shape();
        const RealType pdf_x = pdf(distribution, x);
        const RealType normpdf_x = pdf(std_normal, x);
        const RealType normpdf_ax = pdf(std_normal, x*shape);
        RealType fx = static_cast<RealType>(2)*shape*normpdf_ax*normpdf_x - x*pdf_x;
        RealType dx = static_cast<RealType>(2)*shape*x*normpdf_x*normpdf_ax*(static_cast<RealType>(1) + shape*shape) + pdf_x + x*fx;
        // return both function evaluation difference f(x) and 1st derivative f'(x).
        return boost::math::make_tuple(fx, -dx);
      }
    private:
      const boost::math::skew_normal_distribution<RealType, Policy> distribution;
    };

  } // namespace detail

  template <class RealType, class Policy>
  inline RealType mode(const skew_normal_distribution<RealType, Policy>& dist)
  {
    const RealType scale = dist.scale();
    const RealType location = dist.location();
    const RealType shape = dist.shape();

    static const char* function = "boost::math::mode(const skew_normal_distribution<%1%>&, %1%)";

    RealType result = 0;
    if(false == detail::check_scale(function, scale, &result, Policy()))
      return result;
    if(false == detail::check_location(function, location, &result, Policy()))
      return result;
    if(false == detail::check_skew_normal_shape(function, shape, &result, Policy()))
      return result;

    if( shape == 0 )
    {
      return location;
    }

    if( shape < 0 )
    {
      skew_normal_distribution<RealType, Policy> D(0, 1, -shape);
      result = mode(D);
      result = location-scale*result;
      return result;
    }

    // 21 elements
    static const RealType shapes[] = {
      0.0,
      static_cast<RealType>(1.000000000000000e-004),
      static_cast<RealType>(2.069138081114790e-004),
      static_cast<RealType>(4.281332398719396e-004),
      static_cast<RealType>(8.858667904100824e-004),
      static_cast<RealType>(1.832980710832436e-003),
      static_cast<RealType>(3.792690190732250e-003),
      static_cast<RealType>(7.847599703514606e-003),
      static_cast<RealType>(1.623776739188722e-002),
      static_cast<RealType>(3.359818286283781e-002),
      static_cast<RealType>(6.951927961775606e-002),
      static_cast<RealType>(1.438449888287663e-001),
      static_cast<RealType>(2.976351441631319e-001),
      static_cast<RealType>(6.158482110660261e-001),
      static_cast<RealType>(1.274274985703135e+000),
      static_cast<RealType>(2.636650898730361e+000),
      static_cast<RealType>(5.455594781168514e+000),
      static_cast<RealType>(1.128837891684688e+001),
      static_cast<RealType>(2.335721469090121e+001),
      static_cast<RealType>(4.832930238571753e+001),
      static_cast<RealType>(1.000000000000000e+002)
    };

    // 21 elements
    static const RealType guess[] = {
      0.0,
      static_cast<RealType>(5.000050000525391e-005),
      static_cast<RealType>(1.500015000148736e-004),
      static_cast<RealType>(3.500035000350010e-004),
      static_cast<RealType>(7.500075000752560e-004),
      static_cast<RealType>(1.450014500145258e-003),
      static_cast<RealType>(3.050030500305390e-003),
      static_cast<RealType>(6.250062500624765e-003),
      static_cast<RealType>(1.295012950129504e-002),
      static_cast<RealType>(2.675026750267495e-002),
      static_cast<RealType>(5.525055250552491e-002),
      static_cast<RealType>(1.132511325113255e-001),
      static_cast<RealType>(2.249522495224952e-001),
      static_cast<RealType>(3.992539925399257e-001),
      static_cast<RealType>(5.353553535535358e-001),
      static_cast<RealType>(4.954549545495457e-001),
      static_cast<RealType>(3.524535245352451e-001),
      static_cast<RealType>(2.182521825218249e-001),
      static_cast<RealType>(1.256512565125654e-001),
      static_cast<RealType>(6.945069450694508e-002),
      static_cast<RealType>(3.735037350373460e-002)
    };

    const RealType* result_ptr = std::lower_bound(shapes, shapes+21, shape);

    typedef typename std::iterator_traits<RealType*>::difference_type diff_type;

    const diff_type d = std::distance(shapes, result_ptr);

    BOOST_ASSERT(d > static_cast<diff_type>(0));

    // TODO: make the search bounds smarter, depending on the shape parameter
    RealType search_min = 0; // below zero was caught above
    RealType search_max = 0.55f; // will never go above 0.55

    // refine
    if(d < static_cast<diff_type>(21)) // shape smaller 100
    {
      // it is safe to assume that d > 0, because shape==0.0 is caught earlier
      result = guess[d-static_cast<diff_type>(1)]
        + (guess[d]-guess[d-static_cast<diff_type>(1)])/(shapes[d]-shapes[d-static_cast<diff_type>(1)])
        * (shape-shapes[d-static_cast<diff_type>(1)]);
    }
    else // shape greater 100
    {
      result = 1e-4f;
      search_max = guess[19]; // set 19 instead of 20 to have a safety margin because the table may not be exact @ shape=100
    }

    const int get_digits = policies::digits<RealType, Policy>();// get digits from policy,
    boost::uintmax_t m = policies::get_max_root_iterations<Policy>(); // and max iterations.

    skew_normal_distribution<RealType, Policy> helper(0, 1, shape);

    result = tools::newton_raphson_iterate(detail::skew_normal_mode_functor<RealType, Policy>(helper), result,
      search_min, search_max, get_digits, m);

    result = result*scale + location;

    return result;
  }

  template <class RealType, class Policy>
  inline RealType skewness(const skew_normal_distribution<RealType, Policy>& dist)
  {
    BOOST_MATH_STD_USING  // for ADL of std functions
    using namespace boost::math::constants;

    static const RealType factor = four_minus_pi<RealType>()/static_cast<RealType>(2);
    const RealType delta = dist.shape() / sqrt(static_cast<RealType>(1)+dist.shape()*dist.shape());

    return factor * pow(root_two_div_pi<RealType>() * delta, 3) /
      pow(static_cast<RealType>(1)-two_div_pi<RealType>()*delta*delta, static_cast<RealType>(1.5));
  }

  template <class RealType, class Policy>
  inline RealType kurtosis(const skew_normal_distribution<RealType, Policy>& dist)
  {
    return kurtosis_excess(dist)+static_cast<RealType>(3);
  }

  template <class RealType, class Policy>
  inline RealType kurtosis_excess(const skew_normal_distribution<RealType, Policy>& dist)
  {
    using namespace boost::math::constants;

    static const RealType factor = pi_minus_three<RealType>()*static_cast<RealType>(2);

    const RealType delta2 = static_cast<RealType>(1) / (static_cast<RealType>(1)+static_cast<RealType>(1)/(dist.shape()*dist.shape()));

    const RealType x = static_cast<RealType>(1)-two_div_pi<RealType>()*delta2;
    const RealType y = two_div_pi<RealType>() * delta2;

    return factor * y*y / (x*x);
  }

  namespace detail
  {

    template <class RealType, class Policy>
    struct skew_normal_quantile_functor
    {
      skew_normal_quantile_functor(const boost::math::skew_normal_distribution<RealType, Policy> dist, RealType const& p)
        : distribution(dist), prob(p)
      {
      }

      boost::math::tuple<RealType, RealType> operator()(RealType const& x)
      {
        RealType c = cdf(distribution, x);
        RealType fx = c - prob;  // Difference cdf - value - to minimize.
        RealType dx = pdf(distribution, x); // pdf is 1st derivative.
        // return both function evaluation difference f(x) and 1st derivative f'(x).
        return boost::math::make_tuple(fx, dx);
      }
    private:
      const boost::math::skew_normal_distribution<RealType, Policy> distribution;
      RealType prob;
    };

  } // namespace detail

  template <class RealType, class Policy>
  inline RealType quantile(const skew_normal_distribution<RealType, Policy>& dist, const RealType& p)
  {
    const RealType scale = dist.scale();
    const RealType location = dist.location();
    const RealType shape = dist.shape();

    static const char* function = "boost::math::quantile(const skew_normal_distribution<%1%>&, %1%)";

    RealType result = 0;
    if(false == detail::check_scale(function, scale, &result, Policy()))
      return result;
    if(false == detail::check_location(function, location, &result, Policy()))
      return result;
    if(false == detail::check_skew_normal_shape(function, shape, &result, Policy()))
      return result;
    if(false == detail::check_probability(function, p, &result, Policy()))
      return result;

    // Compute initial guess via Cornish-Fisher expansion.
    RealType x = -boost::math::erfc_inv(2 * p, Policy()) * constants::root_two<RealType>();

    // Avoid unnecessary computations if there is no skew.
    if(shape != 0)
    {
      const RealType skew = skewness(dist);
      const RealType exk = kurtosis_excess(dist);

      x = x + (x*x-static_cast<RealType>(1))*skew/static_cast<RealType>(6)
      + x*(x*x-static_cast<RealType>(3))*exk/static_cast<RealType>(24)
      - x*(static_cast<RealType>(2)*x*x-static_cast<RealType>(5))*skew*skew/static_cast<RealType>(36);
    } // if(shape != 0)

    result = standard_deviation(dist)*x+mean(dist);

    // handle special case of non-skew normal distribution.
    if(shape == 0)
      return result;

    // refine the result by numerically searching the root of (p-cdf)

    const RealType search_min = range(dist).first;
    const RealType search_max = range(dist).second;

    const int get_digits = policies::digits<RealType, Policy>();// get digits from policy,
    boost::uintmax_t m = policies::get_max_root_iterations<Policy>(); // and max iterations.

    result = tools::newton_raphson_iterate(detail::skew_normal_quantile_functor<RealType, Policy>(dist, p), result,
      search_min, search_max, get_digits, m);

    return result;
  } // quantile

  template <class RealType, class Policy>
  inline RealType quantile(const complemented2_type<skew_normal_distribution<RealType, Policy>, RealType>& c)
  {
    const RealType scale = c.dist.scale();
    const RealType location = c.dist.location();
    const RealType shape = c.dist.shape();

    static const char* function = "boost::math::quantile(const complement(skew_normal_distribution<%1%>&), %1%)";
    RealType result = 0;
    if(false == detail::check_scale(function, scale, &result, Policy()))
      return result;
    if(false == detail::check_location(function, location, &result, Policy()))
      return result;
    if(false == detail::check_skew_normal_shape(function, shape, &result, Policy()))
      return result;
    RealType q = c.param;
    if(false == detail::check_probability(function, q, &result, Policy()))
      return result;

    skew_normal_distribution<RealType, Policy> D(-location, scale, -shape);

    result = -quantile(D, q);

    return result;
  } // quantile

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_SKEW_NORMAL_HPP


#include <vector>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <kanooth/numbers/lowlevel/generic_has_double.hpp>
#include <kanooth/numbers/lowlevel/generic_sim_double.hpp>
#include <kanooth/numbers/lowlevel/low_double_int.hpp>
#include <kanooth/number_bits.hpp>
#include <kanooth/fixed_width_ints.hpp>

#include "common/stopwatch.hpp"

boost::random::mt19937 gen;

template <typename T>
void random_array(T* begin, T* end)
{
    boost::random::uniform_int_distribution<T> rand;
    while (begin != end)
        *begin++ = rand(gen);
}

template <typename LOWLEVEL>
class tester {
public:
    tester(unsigned _length) : length(_length)
    {
        digit_type* r;
        for (unsigned k = 0; k < count; ++k) {
            r = new digit_type[length];
            random_array(r, r + length);
            a.push_back(r);
            r = new digit_type[length];
            random_array(r, r + length);
            b.push_back(r);
            c.push_back(new digit_type[length]);
        }
        random_array(&scalar, &scalar + 1);
    }

    ~tester()
    {
        for (unsigned k = 0; k < count; ++k) {
            delete[] a[k];
            delete[] b[k];
            delete[] c[k];
        }
    }

    double test_add()
    {
        stopwatch<> timer;

        unsigned reps = length > 10000 ? 1 : 10000/length;
        for (unsigned j = 0; j < reps; ++j)
            for (unsigned k = 0; k < count; ++k)
                LOWLEVEL::add(c[k], a[k], length, b[k], length);

        return 0.000000001 * count * reps * length * digit_bits / timer.seconds();
    }

    double test_sub()
    {
        stopwatch<> timer;

        unsigned reps = length > 10000 ? 1 : 10000/length;
        for (unsigned j = 0; j < reps; ++j)
            for (unsigned k = 0; k < count; ++k)
                LOWLEVEL::sub(c[k], a[k], length, b[k], length);

        return 0.000000001 * count * reps * length * digit_bits / timer.seconds();
    }

    double test_mul_1()
    {
        stopwatch<> timer;

        unsigned reps = length > 10000 ? 1 : 10000/length;
        for (unsigned j = 0; j < reps; ++j)
            for (unsigned k = 0; k < count; ++k)
                LOWLEVEL::mul_1(c[k], a[k], length, b[k][0]);

        return 0.000000001 * count * reps * length * digit_bits / timer.seconds();
    }

    void run()
    {
        std::cout.flags(std::ios::fixed);
        std::cout.precision(2);
        std::cout << "add       " << test_add() << " Gbit/s" << std::endl;
        std::cout << "sub       " << test_sub() << " Gbit/s" << std::endl;
        std::cout << "mul_1     " << test_mul_1() << " Gbit/s" << std::endl;
    }
private:
    typedef typename LOWLEVEL::digit_type digit_type;
    static const unsigned digit_bits = kanooth::number_bits<digit_type>::value;
    static const unsigned count = 10000;
    const unsigned length;
    std::vector<digit_type*> a;
    std::vector<digit_type*> b;
    std::vector<digit_type*> c;
    digit_type scalar;
};

int main()
{
    //tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::uint128_t> >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint32_t, kanooth::uint64_t> >(1000).run();
    //tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint16_t, kanooth::uint32_t> >(1000).run();
    //tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::numbers::lowlevel::low_double_int<kanooth::uint64_t> > >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_sim_double<kanooth::uint64_t> >(1000).run();

    /*tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint16_t, kanooth::uint32_t> >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint16_t, kanooth::numbers::lowlevel::low_double_int<kanooth::uint16_t> > >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_sim_double<kanooth::uint16_t> >(1000).run();*/

    /*tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::uint128_t> >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::numbers::lowlevel::low_double_int<kanooth::uint64_t> > >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_sim_double<kanooth::uint64_t> >(1000).run();*/

    return 0;
}


#pragma once
#include "math.h"
#include <boost/config.hpp>
#include <type_traits>

namespace std { template <class T> class complex; }
///

namespace coreutil {

BOOST_FORCEINLINE constexpr math_constant::math_constant(long double value)
    : value(value) {}

template <class T>
BOOST_FORCEINLINE constexpr math_constant::operator T() const {
  return value;
}

namespace math_constants_detail {

template <class T> struct is_compatible_operand;
template <class T> struct is_compatible_operand<std::complex<T>>
    : public is_compatible_operand<T> {};
template <class T> struct is_compatible_operand
    : public std::is_floating_point<T> {};

}  // namespace math_constants_detail

#define COREUTIL_MATH_TYPE_CHECK(op, t)                                 \
  static_assert(math_constants_detail::is_compatible_operand<t>(),      \
                "cannot perform '" #op "' between a math constant and non-floating point type " #t)
#define COREUTIL_MATH_DEFINE_UNOP(op)                                   \
  BOOST_FORCEINLINE constexpr math_constant operator op(math_constant a) { return math_constant(op a.value); }
#define COREUTIL_MATH_DEFINE_BINOP(op)                                  \
  BOOST_FORCEINLINE constexpr math_constant operator op(math_constant a, math_constant b) { return math_constant(a.value op b.value); } \
  template <class T> BOOST_FORCEINLINE constexpr T operator op(math_constant a, T b) { COREUTIL_MATH_TYPE_CHECK(op, T); return T(a.value) op b; } \
  template <class T> BOOST_FORCEINLINE constexpr T operator op(T a, math_constant b) { COREUTIL_MATH_TYPE_CHECK(op, T); return a op T(b.value); }
#define COREUTIL_MATH_DEFINE_OPASSIGN(op)   \
  template <class T> BOOST_FORCEINLINE constexpr T &operator op(T &a, math_constant b) { COREUTIL_MATH_TYPE_CHECK(op, T); return a op T(b.value); }
#define COREUTIL_MATH_DEFINE_COMPARISON(op)                             \
  BOOST_FORCEINLINE constexpr bool operator op(math_constant a, math_constant b) { return a.value op b.value; } \
  template <class T> BOOST_FORCEINLINE constexpr bool operator op(math_constant a, T b) { COREUTIL_MATH_TYPE_CHECK(op, T); return T(a.value) op b; } \
  template <class T> BOOST_FORCEINLINE constexpr bool operator op(T a, math_constant b) { COREUTIL_MATH_TYPE_CHECK(op, T); return a op T(b.value); }

COREUTIL_MATH_DEFINE_UNOP(+);
COREUTIL_MATH_DEFINE_UNOP(-);
COREUTIL_MATH_DEFINE_BINOP(+);
COREUTIL_MATH_DEFINE_BINOP(-);
COREUTIL_MATH_DEFINE_BINOP(*);
COREUTIL_MATH_DEFINE_BINOP(/);
COREUTIL_MATH_DEFINE_OPASSIGN(+=);
COREUTIL_MATH_DEFINE_OPASSIGN(-=);
COREUTIL_MATH_DEFINE_OPASSIGN(*=);
COREUTIL_MATH_DEFINE_OPASSIGN(/=);
COREUTIL_MATH_DEFINE_COMPARISON(==);
COREUTIL_MATH_DEFINE_COMPARISON(!=);
COREUTIL_MATH_DEFINE_COMPARISON(<);
COREUTIL_MATH_DEFINE_COMPARISON(>);
COREUTIL_MATH_DEFINE_COMPARISON(<=);
COREUTIL_MATH_DEFINE_COMPARISON(>=);

#undef COREUTIL_MATH_TYPE_CHECK
#undef COREUTIL_MATH_DEFINE_UNOP
#undef COREUTIL_MATH_DEFINE_BINOP
#undef COREUTIL_MATH_DEFINE_OPASSIGN
#undef COREUTIL_MATH_DEFINE_COMPARISON

}  // namespace coreutil