// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_SFUNCTOR_INCLUDE
#define MTL_SFUNCTOR_INCLUDE

#include <cmath>
#include <complex>

#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/concept/static_functor.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/type_traits.hpp>

namespace mtl { namespace sfunctor {

template <typename Value1, typename Value2>
struct plus
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Addable<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
  return v1 + v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
  vampir_trace<23> tracer;
  return v1 + v2;
    }
};

template <typename Value1, typename Value2>
struct minus
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Subtractable<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
  return v1 - v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
  vampir_trace<24> tracer;
  return v1 - v2;
    }
};

template <typename Value1, typename Value2>
struct times
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Multiplicable<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
  return v1 * v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
  vampir_trace<25> tracer;
  return v1 * v2;
    }
};

template <typename Value1, typename Value2>
struct divide
{
    typedef const Value1&                                 first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef typename Divisible<Value1, Value2>::result_type result_type;

    static inline result_type apply(const Value1& v1, const Value2& v2)
    {
  return v1 / v2;
    }

    result_type operator() (const Value1& v1, const Value2& v2) const
    {
  vampir_trace<26> tracer;
  return v1 / v2;
    }
};

template <typename Value1, typename Value2>
struct assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1= Value1(v2);
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<27> tracer;
  return v1= v2;
    }
};

template <typename Value1, typename Value2>
struct plus_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1+= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<28> tracer;
  return v1+= v2;
    }
};

template <typename Value1, typename Value2>
struct minus_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1-= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<29> tracer;
  return v1-= v2;
    }
};

template <typename Value1, typename Value2>
struct times_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1*= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<30> tracer;
  return v1*= v2;
    }
};

template <typename Value1, typename Value2>
struct divide_assign
{
    typedef Value1&                                       first_argument_type;
    typedef const Value2&                                 second_argument_type;
    typedef Value1&                                       result_type;

    static inline result_type apply(Value1& v1, const Value2& v2)
    {
  return v1/= v2;
    }

    result_type operator() (Value1& v1, const Value2& v2) const
    {
  vampir_trace<31> tracer;
  return v1/= v2;
    }
};

// Might be helpful for surplus functor arguments
template <typename Value>
struct identity
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
  return v;
    }

    result_type operator() (const Value& v) const
    {
  vampir_trace<32> tracer;
  return v;
    }
};

template <typename Value>
struct abs
{
    typedef const Value&                                  argument_type;
    typedef typename Magnitude<Value>::type               result_type;

    static inline result_type apply(const Value& v)
    {
  using std::abs;
  return abs(v);
    }

    result_type operator() (const Value& v)  const
    {
  vampir_trace<33> tracer;
  return apply(v);
    }
};

template <typename Value>
struct sqrt
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
  using std::sqrt;
  return sqrt(v);
    }

    result_type operator() (const Value& v)  const
    {
  vampir_trace<34> tracer;
  return apply(v);
    }
};

template <typename Value>
struct square
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
  return v * v;
    }

    result_type operator() (const Value& v) const
    {
  vampir_trace<35> tracer;
  return apply(v);
    }
};

template <typename Value>
struct negate
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v) { return -v;  }
    result_type operator() (const Value& v) const
    {
  vampir_trace<36> tracer;
  return -v;
    }
};

template <typename Value>
struct acos
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::acos;
        return acos(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct acosh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::acosh;
        return acosh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};
# endif

template <typename Value>
struct asin
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::asin;
        return asin(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct asinh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::asinh;
        return asinh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};
# endif

template <typename Value>
struct atan
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::atan;
        return atan(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct atanh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::atanh;
        return atanh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};
# endif

template <typename Value>
struct cos
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::cos;
        return cos(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct cosh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::cosh;
        return cosh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct sin
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::sin;
        return sin(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct sinh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::sinh;
        return sinh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct tan
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::tan;
        return tan(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct tanh
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::tanh;
        return tanh(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

// Rounding functions

template <typename Value>
struct ceil
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        return apply(v, boost::is_integral<Value>());
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }

private:
    static inline result_type apply(const Value& v, boost::integral_constant<bool, false>)
    {
        using std::ceil;
        return ceil(v);
    }

    // return value directly for integer values
    static inline result_type apply(const Value& v, boost::integral_constant<bool, true>)
    {
        return v;
    };
};

template <typename Value>
struct floor
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        return apply(v, boost::is_integral<Value>());
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }

private:
    static inline result_type apply(const Value& v, boost::integral_constant<bool, false>)
    {
        using std::floor;
        return floor(v);
    }

    // return value directly for integer values
    static inline result_type apply(const Value& v, boost::integral_constant<bool, true>)
    {
        return v;
    };
};

# ifdef MTL_WITH_MATH_ELEVEN

    template <typename Value>
    struct round
    {
        typedef const Value&                                  argument_type;
        typedef Value                                         result_type;

        static inline result_type apply(const Value& v)
        {
            return apply(v, boost::is_integral<Value>());
        }
        result_type operator() (const Value& v) const
        {
            return apply(v);
        }

    private:
        static inline result_type apply(const Value& v, boost::integral_constant<bool, false>)
        {
            using std::round;
            return round(v);
        }

        // return value directly for integer values
        static inline result_type apply(const Value& v, boost::integral_constant<bool, true>)
        {
            return v;
        };
    };

    template <typename Value>
    struct trunc
    {
        typedef const Value&                                  argument_type;
        typedef Value                                         result_type;

        static inline result_type apply(const Value& v)
        {
            return apply(v, boost::is_integral<Value>());
        }
        result_type operator() (const Value& v) const
        {
            return apply(v);
        }

    private:
        static inline result_type apply(const Value& v, boost::integral_constant<bool, false>)
        {
            using std::trunc;
            return trunc(v);
        }

        // return value directly for integer values
        static inline result_type apply(const Value& v, boost::integral_constant<bool, true>)
        {
            return v;
        };
    };

# endif

// Logarithmic functions

template <typename Value>
struct log
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::log;
        return log(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct log2
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::log2;
        return log2(v);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};
# endif

template <typename Value>
struct log10
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::log10;
        return log10(v);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};

// Exponential functions

template <typename Value>
struct exp
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::exp;
        return exp(v);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct exp2
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::exp2;
        return exp2(v);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};
#endif

template <typename Value>
struct exp10
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::exp;
        return exp(v * 2.302585092994045684017991454684364207601101488628772976033);
    }
    result_type operator() (const Value& v) const
    {
        return apply(v);
    }
};

// Inverse square root functions

template <typename Value>
struct rsqrt
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::pow;
        return pow(v, -0.5);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

// Error functions

# ifdef MTL_WITH_MATH_ELEVEN
template <typename Value>
struct erf
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::erf;
        return erf(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};

template <typename Value>
struct erfc
{
    typedef const Value&                                  argument_type;
    typedef Value                                         result_type;

    static inline result_type apply(const Value& v)
    {
        using std::erfc;
        return erfc(v);
    }
    result_type operator() (const Value& v) const
    {
  return apply(v);
    }
};
# endif

/// Compose functors \p F and \p G, i.e. compute f(g(x)).
/** Functors must be models of StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticUnaryFunctor<F>::argument_type.
    Under these conditions compose<F, G> will be a model of StaticUnaryFunctor.
**/
template <typename F, typename G>
struct compose
{
    typedef typename StaticUnaryFunctor<G>::argument_type argument_type;
    typedef typename StaticUnaryFunctor<F>::result_type   result_type;

    static inline result_type apply(argument_type x)
    {
  return F::apply(G::apply(x));
    }

    result_type operator()(argument_type x)
    {
  vampir_trace<37> tracer;
  return apply(x);
    }
};

/// Compose functors \p F and \p G with G in F's first argument, i.e. compute f(g(x), y).
/** F/G must be models of StaticBinaryFunctor/StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticBinaryFunctor<F>::first_argument_type.
    Under these conditions compose_first<F, G> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G>
struct compose_first
{
    typedef typename StaticUnaryFunctor<G>::argument_type         first_argument_type;
    typedef typename StaticBinaryFunctor<F>::second_argument_type second_argument_type;
    typedef typename StaticBinaryFunctor<F>::result_type          result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
  return F::apply(G::apply(x), y);
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
  vampir_trace<38> tracer;
  return apply(x, y);
    }
};

/// Compose functors \p F and \p G with G in F's second argument, i.e. compute f(x, g(y)).
/** F/G must be models of StaticBinaryFunctor/StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticBinaryFunctor<F>::second_argument_type.
    Under these conditions compose_second<F, G> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G>
struct compose_second
{
    typedef typename StaticBinaryFunctor<F>::first_argument_type  first_argument_type;
    typedef typename StaticUnaryFunctor<G>::argument_type         second_argument_type;
    typedef typename StaticBinaryFunctor<F>::result_type          result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
  return F::apply(x, G::apply(y));
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
  vampir_trace<39> tracer;
  return apply(x, y);
    }
};

/// Compose functors \p F, \p G, and \p H with G/H in F's first/second argument, i.e. compute f(g(x), h(y)).
/** F/G must be models of StaticBinaryFunctor/StaticUnaryFunctor,
    StaticUnaryFunctor<G>::result_type must be convertible to
    StaticBinaryFunctor<F>::first_argument_type and
    StaticUnaryFunctor<H>::result_type must be convertible to
    StaticBinaryFunctor<F>::second_argument_type.
    Under these conditions compose_both<F, G, H> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G, typename H>
struct compose_both
{
    typedef typename StaticUnaryFunctor<G>::argument_type         first_argument_type;
    typedef typename StaticUnaryFunctor<H>::argument_type         second_argument_type;
    typedef typename StaticBinaryFunctor<F>::result_type          result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
  return F::apply(G::apply(x), H::apply(y));
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
  vampir_trace<40> tracer;
  return apply(x, y);
    }
};

/// Compose unary functor \p F with binary functor \p G, i.e. compute f(g(x, y)).
/** F/G must be models of StaticUnaryFunctor/StaticBinaryFunctor,
    StaticBinaryFunctor<G>::result_type must be convertible to
    StaticUnaryFunctor<F>::argument_type.
    Under these conditions compose_binary<F, G> will be a model of StaticBinaryFunctor.
**/
template <typename F, typename G>
struct compose_binary
{
    typedef typename StaticBinaryFunctor<G>::first_argument_type  first_argument_type;
    typedef typename StaticBinaryFunctor<G>::second_argument_type second_argument_type;
    typedef typename StaticUnaryFunctor<F>::result_type           result_type;

    static inline result_type apply(first_argument_type x, second_argument_type y)
    {
  return F::apply(G::apply(x, y));
    }

    result_type operator()(first_argument_type x, second_argument_type y)
    {
  vampir_trace<41> tracer;
  return apply(x, y);
    }
};

/// Templatized example of composition, computes l_2 norm in 2D, i.e. sqrt(abs(x*x + y*y))
template <typename T>
struct l_2_2D
  : public compose_binary<sqrt<typename abs<T>::result_type>,
        compose_binary<abs<T>,
           compose_both<plus<T, T>,
                  square<T>,
                  square<T>  >
                                        >
                         >
{};

}} // namespace mtl::sfunctor

#endif // MTL_SFUNCTOR_INCLUDE


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_PRIMES_HPP
#define CRYPTO3_DETAIL_PRIMES_HPP

#include <boost/integer.hpp>

namespace boost {
    namespace crypto3 {
        namespace detail {

            template<int Bits>
            struct all_ones {
                typedef typename boost::uint_t<Bits>::least type;
                static type const value = (all_ones<Bits - 1>::value << 1) | 1;
            };
            template<>
            struct all_ones<0> {
                typedef boost::uint_t<0>::least type;
                static type const value = 0;
            };

            template<int Bits>
            struct largest_prime;

#define CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(B, D)                              \
    template<>                                                                         \
    struct largest_prime<B> {                                                          \
        constexpr static boost::uint_t<B>::least const value = all_ones<B>::value - D; \
    };                                                                                \
    constexpr boost::uint_t<B>::least const largest_prime<B>::value;

            // http://primes.utm.edu/lists/2small/0bit.html or
            // http://www.research.att.com/~njas/sequences/A013603
            // Though those offets are from 2**b; This code is offsets from 2**b-1
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(2, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(3, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(4, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(5, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(6, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(7, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(8, 4);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(9, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(10, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(11, 8);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(12, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(13, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(14, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(15, 18);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(16, 14);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(17, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(18, 4);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(19, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(20, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(21, 8);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(22, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(23, 14);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(24, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(25, 38);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(26, 4);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(27, 38);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(28, 56);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(29, 2);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(30, 34);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(31, 0);
            CRYPTO3_HASH_DEFINE_LARGEST_PRIME_BY_OFFSET(32, 4);

        }    // namespace detail
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_HASH_DETAIL_PRIMES_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file cclgmprocess.hpp
    \brief cross currency lgm model process
           the fx processes are modeled in log spot here
           (different from BlackScholesProcess where it is mixed
           log spot / spot depending on the method)
*/

#ifndef quantlib_cclgm_process_hpp
#define quantlib_cclgm_process_hpp

#include <ql/math/matrixutilities/pseudosqrt.hpp>
#include <ql/experimental/models/cclgmparametrization.hpp>

#include <boost/unordered_map.hpp>

namespace QuantLib {

template <class Impl, class ImplFx, class ImplLgm>
class CcLgmProcess : public StochasticProcess {
  public:
    CcLgmProcess(const boost::shared_ptr<detail::CcLgmParametrization<
                     Impl, ImplFx, ImplLgm> > &parametrization,
                 const std::vector<Handle<Quote> > &fxSpots,
                 const std::vector<Handle<YieldTermStructure> > &curves);

    //! Stochastic process interface
    Size size() const;
    Size factors() const;
    Disposable<Array> initialValues() const;
    Disposable<Array> drift(Time t, const Array &x) const;      // not available
    Disposable<Matrix> diffusion(Time t, const Array &x) const; // not available
    Disposable<Array> expectation(Time t0, const Array &x0, Time dt) const;
    Disposable<Matrix> stdDeviation(Time t0, const Array &x0, Time dt) const;
    Disposable<Matrix> covariance(Time t0, const Array &x0, Time dt) const;

    //! clear cache
    void flushCache() const;

    //! curves inspector
    Handle<YieldTermStructure> termStructure(const Size i) const {
        QL_REQUIRE(i <= n_, "term structure index ("
                                << i << ") out of range 0..." << n_);
        return curves_[i];
    }

  private:
    const boost::shared_ptr<
        detail::CcLgmParametrization<Impl, ImplFx, ImplLgm> > p_;
    const std::vector<Handle<Quote> > fxSpots_;
    const std::vector<Handle<YieldTermStructure> > curves_;
    const Size n_;

    struct cache_key {
        double t0;
        double dt;
        const bool operator==(const cache_key &o) const {
            return (t0 == o.t0) && (dt == o.dt);
        }
    };

    struct cache_hasher : std::unary_function<cache_key, std::size_t> {
        std::size_t operator()(cache_key const &x) const {
            std::size_t seed = 0;
            boost::hash_combine(seed, x.t0);
            boost::hash_combine(seed, x.dt);
            return seed;
        }
    };

    mutable boost::unordered_map<cache_key, Array, cache_hasher> cache_e_;
    mutable boost::unordered_map<cache_key, Matrix, cache_hasher> cache_v_,
        cache_s_;
};

// inline

template <class Impl, class ImplFx, class ImplLgm>
void CcLgmProcess<Impl, ImplFx, ImplLgm>::flushCache() const {
    cache_v_.clear();
    cache_s_.clear();
    cache_e_.clear();
}

template <class Impl, class ImplFx, class ImplLgm>
inline Size CcLgmProcess<Impl, ImplFx, ImplLgm>::size() const {
    return 2 * n_ + 1;
}

template <class Impl, class ImplFx, class ImplLgm>
inline Size CcLgmProcess<Impl, ImplFx, ImplLgm>::factors() const {
    return 2 * n_ + 1;
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Array>
CcLgmProcess<Impl, ImplFx, ImplLgm>::initialValues() const {
    Array res(2 * n_ + 1);
    for (Size i = 0; i < n_; ++i) {
        res[i] = fxSpots_[i]->value();
    }
    for (Size i = n_; i < 2 * n_ + 1; ++i) {
        res[i] = 0.0;
    }
    return res;
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Array>
CcLgmProcess<Impl, ImplFx, ImplLgm>::drift(Time t, const Array &x) const {
    QL_FAIL("drift not implemented");
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Matrix>
CcLgmProcess<Impl, ImplFx, ImplLgm>::diffusion(Time t, const Array &x) const {
    QL_FAIL("diffusion not implemented");
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Array>
CcLgmProcess<Impl, ImplFx, ImplLgm>::expectation(Time t0, const Array &x0,
                                                 Time dt) const {
    cache_key k = {t0, dt};
    typename boost::unordered_map<cache_key, Array>::iterator it =
        cache_e_.find(k);
    Array res(2 * n_ + 1, 0.0);
    if (it == cache_e_.end()) {
        // fx
        for (Size i = 0; i < n_; ++i) {
            res[i] =
                std::log(curves_[i + 1]->discount(t0 + dt) /
                         curves_[i + 1]->discount(t0) *
                         curves_[0]->discount(t0) /
                         curves_[0]->discount(t0 + dt)) -
                0.5 * p_->int_sigma_i_sigma_j(i, i, t0, t0 + dt) +
                0.5 * (p_->H_i(0, t0 + dt) * p_->H_i(0, t0 + dt) *
                           p_->zeta_i(0, t0 + dt) -
                       p_->H_i(0, t0) * p_->H_i(0, t0) * p_->zeta_i(0, t0) -
                       p_->int_H_i_H_j_alpha_i_alpha_j(0, 0, t0, t0 + dt)) -
                0.5 * (p_->H_i(i + 1, t0 + dt) * p_->H_i(i + 1, t0 + dt) *
                           p_->zeta_i(i + 1, t0 + dt) -
                       p_->H_i(i + 1, t0) * p_->H_i(i + 1, t0) *
                           p_->zeta_i(i + 1, t0) -
                       p_->int_H_i_H_j_alpha_i_alpha_j(i + 1, i + 1, t0,
                                                       t0 + dt)) +
                p_->int_H_i_alpha_i_sigma_j(0, i, t0, t0 + dt) -
                p_->H_i(i + 1, t0 + dt) *
                    (-p_->int_H_i_alpha_i_alpha_j(i + 1, i + 1, t0, t0 + dt) +
                     p_->int_H_i_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) -
                     p_->int_alpha_i_sigma_j(i + 1, i, t0, t0 + dt)) -
                p_->int_H_i_H_j_alpha_i_alpha_j(i + 1, i + 1, t0, t0 + dt) +
                p_->int_H_i_H_j_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) -
                p_->int_H_i_alpha_i_sigma_j(i + 1, i, t0, t0 + dt);
        }
        // lgm
        for (Size i = 1; i < n_ + 1; ++i) {
            res[n_ + i] = -p_->int_H_i_alpha_i_alpha_j(i, i, t0, t0 + dt) -
                          p_->int_alpha_i_sigma_j(i, i - 1, t0, t0 + dt) +
                          p_->int_H_i_alpha_i_alpha_j(0, i, t0, t0 + dt);
        }
        cache_e_.insert(std::make_pair(k, res));
    } else {
        res = it->second;
    }
    for (Size i = 0; i < n_; ++i) {
        res[i] +=
            x0[i] + (p_->H_i(0, t0 + dt) - p_->H_i(0, t0)) * x0[n_] -
            (p_->H_i(i + 1, t0 + dt) - p_->H_i(i + 1, t0)) * x0[n_ + i + 1];
    }
    for (Size i = 0; i < n_ + 1; ++i) {
        res[n_ + i] += x0[n_ + i];
    }
    return res;
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Matrix>
CcLgmProcess<Impl, ImplFx, ImplLgm>::covariance(Time t0, const Array &x0,
                                                Time dt) const {
    cache_key k = {t0, dt};
    typename boost::unordered_map<cache_key, Matrix>::iterator i =
        cache_v_.find(k);
    if (i == cache_v_.end()) {
        Matrix res(2 * n_ + 1, 2 * n_ + 1, 0.0);
        // fx-fx
        for (Size i = 0; i < n_; ++i) {
            for (Size j = 0; j <= i; ++j) {
                res[i][j] = res[j][i] =
                    // row 1
                    p_->H_i(0, t0 + dt) * p_->H_i(0, t0 + dt) *
                        p_->int_alpha_i_alpha_j(0, 0, t0, t0 + dt) -
                    2.0 * p_->H_i(0, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(0, 0, t0, t0 + dt) +
                    p_->int_H_i_H_j_alpha_i_alpha_j(0, 0, t0, t0 + dt) -
                    // row 2
                    p_->H_i(0, t0 + dt) * p_->H_i(j + 1, t0 + dt) *
                        p_->int_alpha_i_alpha_j(0, j + 1, t0, t0 + dt) +
                    p_->H_i(j + 1, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(0, j + 1, t0, t0 + dt) +
                    p_->H_i(0, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(j + 1, 0, t0, t0 + dt) -
                    p_->int_H_i_H_j_alpha_i_alpha_j(0, j + 1, t0, t0 + dt) -
                    // row 3
                    p_->H_i(0, t0 + dt) * p_->H_i(i + 1, t0 + dt) *
                        p_->int_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) +
                    p_->H_i(i + 1, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) +
                    p_->H_i(0, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(i + 1, 0, t0, t0 + dt) -
                    p_->int_H_i_H_j_alpha_i_alpha_j(0, i + 1, t0, t0 + dt) +
                    // row 4
                    p_->H_i(0, t0 + dt) *
                        p_->int_alpha_i_sigma_j(0, j, t0, t0 + dt) -
                    p_->int_H_i_alpha_i_sigma_j(0, j, t0, t0 + dt) +
                    // row 5
                    p_->H_i(0, t0 + dt) *
                        p_->int_alpha_i_sigma_j(0, i, t0, t0 + dt) -
                    p_->int_H_i_alpha_i_sigma_j(0, i, t0, t0 + dt) -
                    // row 6
                    p_->H_i(i + 1, t0 + dt) *
                        p_->int_alpha_i_sigma_j(i + 1, j, t0, t0 + dt) +
                    p_->int_H_i_alpha_i_sigma_j(i + 1, j, t0, t0 + dt) -
                    // row 7
                    p_->H_i(j + 1, t0 + dt) *
                        p_->int_alpha_i_sigma_j(j + 1, i, t0, t0 + dt) +
                    p_->int_H_i_alpha_i_sigma_j(j + 1, i, t0, t0 + dt) +
                    // row 8
                    p_->H_i(i + 1, t0 + dt) * p_->H_i(j + 1, t0 + dt) *
                        p_->int_alpha_i_alpha_j(i + 1, j + 1, t0, t0 + dt) -
                    p_->H_i(j + 1, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(i + 1, j + 1, t0, t0 + dt) -
                    p_->H_i(i + 1, t0 + dt) *
                        p_->int_H_i_alpha_i_alpha_j(j + 1, i + 1, t0, t0 + dt) +
                    p_->int_H_i_H_j_alpha_i_alpha_j(i + 1, j + 1, t0, t0 + dt) +
                    // row 9
                    p_->int_sigma_i_sigma_j(i, j, t0, t0 + dt);
            }
        }
        // fx-lgm
        for (Size i = 0; i < n_ + 1; ++i) {
            for (Size j = 0; j < n_; ++j) {
                res[j][i + n_] = res[i + n_][j] =
                    p_->H_i(0, t0 + dt) *
                        p_->int_alpha_i_alpha_j(0, i, t0, t0 + dt) -
                    p_->int_H_i_alpha_i_alpha_j(0, i, t0, t0 + dt) -
                    p_->H_i(j + 1, t0 + dt) *
                        p_->int_alpha_i_alpha_j(j + 1, i, t0, t0 + dt) +
                    p_->int_H_i_alpha_i_alpha_j(j + 1, i, t0, t0 + dt) +
                    p_->int_alpha_i_sigma_j(i, j, t0, t0 + dt);
            }
        }
        // lgm-lgm
        for (Size i = 0; i < n_ + 1; ++i) {
            for (Size j = 0; j <= i; ++j) {
                res[i + n_][j + n_] = res[j + n_][i + n_] =
                    p_->int_alpha_i_alpha_j(i, j, t0, t0 + dt);
            }
        }
        cache_v_.insert(std::make_pair(k, res));
        return res;
    } else {
        // we need to make a copy here since a disposable is returned
        Matrix tmp = i->second;
        return tmp;
    }
}

template <class Impl, class ImplFx, class ImplLgm>
inline Disposable<Matrix>
CcLgmProcess<Impl, ImplFx, ImplLgm>::stdDeviation(Time t0, const Array &x0,
                                                  Time dt) const {
    cache_key k = {t0, dt};
    typename boost::unordered_map<cache_key, Matrix>::iterator i =
        cache_s_.find(k);
    if (i == cache_s_.end()) {
        Matrix tmp =
            pseudoSqrt(covariance(t0, x0, dt), SalvagingAlgorithm::Spectral);
        cache_s_.insert(std::make_pair(k, tmp));
        return tmp;
    } else {
        // we need to make a copy here since a disposable is returned
        Matrix tmp = i->second;
        return tmp;
    }
}

// implementation

template <class Impl, class ImplFx, class ImplLgm>
CcLgmProcess<Impl, ImplFx, ImplLgm>::CcLgmProcess(
    const boost::shared_ptr<
        detail::CcLgmParametrization<Impl, ImplFx, ImplLgm> > &parametrization,
    const std::vector<Handle<Quote> > &fxSpots,
    const std::vector<Handle<YieldTermStructure> > &curves)
    : p_(parametrization), fxSpots_(fxSpots), curves_(curves), n_(p_->n()) {

    QL_REQUIRE(fxSpots_.size() == n_,
               fxSpots_.size()
                   << " fx spots given, while parametrization suggests " << n_);
    QL_REQUIRE(curves_.size() == n_ + 1,
               curves_.size()
                   << " curves given, while parametrization suggests "
                   << (n_ + 1));

    for (Size i = 0; i < n_; ++i)
        registerWith(fxSpots_[i]);
}

} // namesapce QuantLib

#endif


#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include "ui_Mean_curvature_flow_skeleton_plugin.h"
#include "Scene_polyhedron_item.h"
#include "Scene_points_with_normal_item.h"
#include "Scene_polylines_item.h"
#include "Scene.h"

#include "Polyhedron_type.h"

#include <QApplication>
#include <QMainWindow>
#include <QInputDialog>
#include <QTime>
#include <QMessageBox>

#include <Eigen/Sparse>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Eigen_solver_traits.h>
#include <CGAL/extract_mean_curvature_flow_skeleton.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/iterator.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/boost/graph/split_graph_into_polylines.h>
#include <CGAL/mesh_segmentation.h>
#include <CGAL/Polyhedron_copy_3.h>
#include <queue>

typedef boost::graph_traits<Polyhedron>::vertex_descriptor          vertex_descriptor;
typedef boost::graph_traits<Polyhedron>::vertex_iterator            vertex_iterator;
typedef boost::graph_traits<Polyhedron>::halfedge_descriptor        halfedge_descriptor;
typedef Polyhedron::Facet_iterator                                  Facet_iterator;

typedef CGAL::Mean_curvature_flow_skeletonization<Polyhedron>      Mean_curvature_skeleton;
typedef Mean_curvature_skeleton::Skeleton Skeleton;

typedef Polyhedron::Traits         Kernel;
typedef Kernel::Point_3            Point;

struct Polyline_visitor
{
  typedef std::vector<Point> Polyline;
  typedef std::vector<std::size_t> Polyline_of_ids;

  std::list<Polyline>& polylines;
  Skeleton& skeleton;

  Polyline_visitor(std::list<Polyline>& lines, Skeleton& skeleton)
    : polylines(lines),
      skeleton(skeleton)
  {}

  void start_new_polyline()
  {
    Polyline V;
    polylines.push_back(V);
  }

  void add_node(boost::graph_traits<Skeleton>::vertex_descriptor vd)
  {
    Polyline& polyline = polylines.back();
    polyline.push_back(skeleton[vd].point);
  }

  void end_polyline(){}
};

template<class ValueType>
struct Facet_with_id_pmap
    : public boost::put_get_helper<ValueType&,
             Facet_with_id_pmap<ValueType> >
{
    typedef Polyhedron::Face_handle key_type;
    typedef ValueType value_type;
    typedef value_type& reference;
    typedef boost::lvalue_property_map_tag category;

    Facet_with_id_pmap(
      std::vector<ValueType>& internal_vector
    ) : internal_vector(internal_vector) { }

    reference operator[](key_type key) const
    { return internal_vector[key->id()]; }
private:
    std::vector<ValueType>& internal_vector;
};
using namespace CGAL::Three;
class Polyhedron_demo_mean_curvature_flow_skeleton_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
  QAction* actionMCFSkeleton;
  QAction* actionConvert_to_medial_skeleton;

public:
  // used by Polyhedron_demo_plugin_helper
  QStringList actionsNames() const {
    return QStringList() << "actionMCFSkeleton" << "actionConvert_to_medial_skeleton";
  }

  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface) {
    mcs = NULL;
    dockWidget = NULL;
    ui = NULL;

    actionMCFSkeleton = new QAction(tr("Mean Curvature Skeleton (Advanced)"), mainWindow);
    actionMCFSkeleton->setProperty("subMenuName", "Triangulated Surface Mesh Skeletonization");
    actionMCFSkeleton->setObjectName("actionMCFSkeleton");

    actionConvert_to_medial_skeleton = new QAction(tr("Extract Medial Skeleton"), mainWindow);
    actionConvert_to_medial_skeleton->setProperty("subMenuName", "Triangulated Surface Mesh Skeletonization");
    actionConvert_to_medial_skeleton->setObjectName("actionConvert_to_medial_skeleton");

    Polyhedron_demo_plugin_helper::init(mainWindow, scene_interface);

    dockWidget = new QDockWidget(mw);
    dockWidget->setVisible(false);
    ui = new Ui::Mean_curvature_flow_skeleton_plugin();
    ui->setupUi(dockWidget);
    dockWidget->setFeatures(QDockWidget::DockWidgetMovable
                          | QDockWidget::DockWidgetFloatable
                          | QDockWidget::DockWidgetClosable);
    dockWidget->setWindowTitle("Mean Curvature Flow Skeleton");
    add_dock_widget(dockWidget);

    connect(ui->pushButton_contract, SIGNAL(clicked()),
            this, SLOT(on_actionContract()));
    connect(ui->pushButton_collapse, SIGNAL(clicked()),
            this, SLOT(on_actionCollapse()));
    connect(ui->pushButton_split, SIGNAL(clicked()),
            this, SLOT(on_actionSplit()));
    connect(ui->pushButton_degeneracy, SIGNAL(clicked()),
            this, SLOT(on_actionDegeneracy()));
    connect(ui->pushButton_run, SIGNAL(clicked()),
            this, SLOT(on_actionRun()));
    connect(ui->pushButton_skeletonize, SIGNAL(clicked()),
            this, SLOT(on_actionSkeletonize()));
    connect(ui->pushButton_converge, SIGNAL(clicked()),
            this, SLOT(on_actionConverge()));
    connect(dynamic_cast<Scene*>(scene), SIGNAL(updated_bbox()),
            this, SLOT(on_actionUpdateBBox()));
    connect(ui->pushButton_segment, SIGNAL(clicked()),
            this, SLOT(on_actionSegment()));

    QObject* scene_object = dynamic_cast<QObject*>(scene);
    connect(scene_object, SIGNAL(itemAboutToBeDestroyed(CGAL::Three::Scene_item*)),
            this, SLOT(on_actionItemAboutToBeDestroyed(CGAL::Three::Scene_item*)));
  }

  virtual void closure()
  {
    dockWidget->hide();
  }

  QList<QAction*> actions() const {
    return QList<QAction*>() << actionMCFSkeleton << actionConvert_to_medial_skeleton;
  }

  bool applicable(QAction*) const {
    return qobject_cast<Scene_polyhedron_item*>(scene->item(scene->mainSelectionIndex()));
  }

  void init_ui(double diag) {
    ui->omega_H->setValue(0.1);
    ui->omega_H->setSingleStep(0.1);
    ui->omega_H->setDecimals(3);
    ui->omega_P->setValue(0.2);
    ui->omega_P->setSingleStep(0.1);
    ui->omega_P->setDecimals(3);
    ui->min_edge_length->setDecimals(7);
    ui->min_edge_length->setValue(0.002 * diag);
    ui->min_edge_length->setSingleStep(0.0000001);
    ui->delta_area->setDecimals(7);
    ui->delta_area->setValue(1e-4);
    ui->delta_area->setSingleStep(1e-5);
    ui->is_medially_centered->setChecked(false);

    ui->label_omega_H->setToolTip(QString("omega_H controls the velocity of movement and approximation quality"));
    ui->label_omega_P->setToolTip(QString("omega_P controls the smoothness of the medial approximation"));
    ui->pushButton_contract->setToolTip(QString("contract mesh based on mean curvature flow"));
    ui->pushButton_collapse->setToolTip(QString("collapse short edges"));
    ui->pushButton_split->setToolTip(QString("split obtuse triangles"));
    ui->pushButton_degeneracy->setToolTip(QString("fix degenerate points"));
    ui->pushButton_skeletonize->setToolTip(QString("Turn mesh to a skeleton curve"));
    ui->pushButton_run->setToolTip(QString("run one iteration of contract, collapse, split, detect degeneracy"));
    ui->pushButton_converge->setToolTip(QString("iteratively contract the mesh until convergence"));
  }

  bool check_item_index(int index) {
    if (index < 0)
    {
      QMessageBox msgBox;
      msgBox.setText("Please select an item first");
      msgBox.exec();
      return false;
    }
    return true;
  }

  /// \todo move this function into an include
  bool is_mesh_valid(Polyhedron *pMesh) {
    if (!pMesh->is_closed())
    {
      QMessageBox msgBox;
      msgBox.setText("The mesh is not closed.");
      msgBox.exec();
      return false;
    }
    if (!pMesh->is_pure_triangle())
    {
      QMessageBox msgBox;
      msgBox.setText("The mesh is not a pure triangle mesh.");
      msgBox.exec();
      return false;
    }

    // the algorithm is only applicable on a mesh
    // that has only one connected component
    std::size_t num_component;
    CGAL::Counting_output_iterator output_it(&num_component);
    CGAL::internal::corefinement::extract_connected_components(*pMesh, output_it);
    ++output_it;
    if (num_component != 1)
    {
      QMessageBox msgBox;
      QString str = QString("The mesh is not a single closed mesh.\n It has %1 components.").arg(num_component);
      msgBox.setText(str);
      msgBox.exec();
      return false;
    }
    return true;
  }

  /// \todo remove duplicated code
  // check if the Mean_curvature_skeleton exists
  // or has the same polyheron item
  // check if the mesh is a watertight triangle mesh
  bool check_mesh(Scene_polyhedron_item* item) {
    double omega_H = ui->omega_H->value();
    double omega_P = ui->omega_P->value();
    double min_edge_length = ui->min_edge_length->value();
    double delta_area = ui->delta_area->value();
    bool is_medially_centered = ui->is_medially_centered->isChecked();

    Polyhedron *pMesh = item->polyhedron();

    if (mcs == NULL)
    {
      if (!is_mesh_valid(pMesh))
      {
        return false;
      }

      mcs = new Mean_curvature_skeleton(*pMesh);
      meso_skeleton = new Polyhedron(*pMesh);
      input_triangle_mesh = pMesh;
      //set algorithm parameters
      mcs->set_quality_speed_tradeoff(omega_H);
      mcs->set_medially_centered_speed_tradeoff(omega_P);
      mcs->set_min_edge_length(min_edge_length);
      mcs->set_is_medially_centered(is_medially_centered);
      mcs->set_area_variation_factor(delta_area);

      Scene_polyhedron_item* contracted_item = new Scene_polyhedron_item( meso_skeleton );
      contracted_item->setName(QString("contracted mesh of %1").arg(item->name()));

      InputMeshItemIndex = scene->mainSelectionIndex();

      contractedItemIndex = scene->addItem(contracted_item);

      item->setVisible(false);

      fixedPointsItemIndex = -1;
      nonFixedPointsItemIndex = -1;
      poleLinesItemIndex = -1;
    }
    else
    {
      if (input_triangle_mesh != pMesh)
      {
        if (!is_mesh_valid(pMesh))
        {
          return false;
        }

        delete mcs;

        mcs = new Mean_curvature_skeleton(*pMesh);
        meso_skeleton = new Polyhedron(*pMesh);
        input_triangle_mesh = pMesh;
        //set algorithm parameters
        mcs->set_quality_speed_tradeoff(omega_H);
        mcs->set_medially_centered_speed_tradeoff(omega_P);
        mcs->set_min_edge_length(min_edge_length);
        mcs->set_is_medially_centered(is_medially_centered);
        mcs->set_area_variation_factor(delta_area);

        Scene_polyhedron_item* contracted_item = new Scene_polyhedron_item(meso_skeleton);
        contracted_item->setName(QString("contracted mesh of %1").arg(item->name()));

        InputMeshItemIndex = scene->mainSelectionIndex();

        contractedItemIndex = scene->addItem(contracted_item);

        item->setVisible(false);

        fixedPointsItemIndex = -1;
        nonFixedPointsItemIndex = -1;
        poleLinesItemIndex = -1;
      }
      else
      {
        mcs->set_quality_speed_tradeoff(omega_H);
        mcs->set_medially_centered_speed_tradeoff(omega_P);
        mcs->set_min_edge_length(min_edge_length);
        mcs->set_area_variation_factor(delta_area);
        mcs->set_is_medially_centered(is_medially_centered);
      }
    }
    return true;
  }

  void update_meso_skeleton()
  {
    CGAL::Polyhedron_copy_3<Mean_curvature_skeleton::Meso_skeleton, Polyhedron::HalfedgeDS> modifier(mcs->meso_skeleton());
    meso_skeleton->delegate(modifier);
    scene->item(contractedItemIndex)->invalidateOpenGLBuffers();
    scene->itemChanged(contractedItemIndex);
  }

  void update_parameters(Mean_curvature_skeleton* mcs)
  {
    double omega_H = ui->omega_H->value();
    double omega_P = ui->omega_P->value();
    double min_edge_length = ui->min_edge_length->value();
    double delta_area = ui->delta_area->value();
    bool is_medially_centered = ui->is_medially_centered->isChecked();

    mcs->set_quality_speed_tradeoff(omega_H);
    mcs->set_medially_centered_speed_tradeoff(omega_P);
    mcs->set_min_edge_length(min_edge_length);
    mcs->set_area_variation_factor(delta_area);
    mcs->set_is_medially_centered(is_medially_centered);
  }

public Q_SLOTS:
  void on_actionMCFSkeleton_triggered();
  void on_actionConvert_to_medial_skeleton_triggered();
  void on_actionContract();
  void on_actionCollapse();
  void on_actionSplit();
  void on_actionDegeneracy();
  void on_actionRun();
  void on_actionSkeletonize();
  void on_actionConverge();
  void on_actionUpdateBBox();
  void on_actionSegment();
  void on_actionItemAboutToBeDestroyed(CGAL::Three::Scene_item*);

private:
  Mean_curvature_skeleton* mcs;
  Polyhedron* meso_skeleton; // a copy of the meso_skeleton that is displayed
  Polyhedron* input_triangle_mesh;
  QDockWidget* dockWidget;
  Ui::Mean_curvature_flow_skeleton_plugin* ui;

  int fixedPointsItemIndex;
  int nonFixedPointsItemIndex;
  int poleLinesItemIndex;
  int contractedItemIndex;
  int InputMeshItemIndex;

  Skeleton skeleton_curve;
}; // end Polyhedron_demo_mean_curvature_flow_skeleton_plugin

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionMCFSkeleton_triggered()
{
  dockWidget->show();
  dockWidget->raise();

  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_polyhedron_item* item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  if(item)
  {
    Polyhedron* pMesh = item->polyhedron();

    if(!pMesh) return;

    double diag = scene->len_diagonal();
    init_ui(diag);

    fixedPointsItemIndex = -1;
    nonFixedPointsItemIndex = -1;
    poleLinesItemIndex = -1;
    contractedItemIndex = -1;
    InputMeshItemIndex = -1;
  }
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionUpdateBBox()
{
  double diag = scene->len_diagonal();
  ui->min_edge_length->setValue(0.002 * diag);
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionSegment()
{
  if (num_vertices(skeleton_curve)==0 ) on_actionSkeletonize();
  if (num_vertices(skeleton_curve)==0 ) return;

  QTime time;
  time.start();
  QApplication::setOverrideCursor(Qt::WaitCursor);

    // init the polyhedron simplex indices
  CGAL::set_halfedgeds_items_id(*input_triangle_mesh);

  //for each input vertex compute its distance to the skeleton
  std::vector<double> distances(num_vertices(*input_triangle_mesh));
  BOOST_FOREACH(boost::graph_traits<Skeleton>::vertex_descriptor v, vertices(skeleton_curve) )
  {
    const Point& skel_pt = skeleton_curve[v].point;
    BOOST_FOREACH(vertex_descriptor mesh_v, skeleton_curve[v].vertices)
    {
      const Point& mesh_pt = mesh_v->point();
      distances[mesh_v->id()] = std::sqrt(CGAL::squared_distance(skel_pt, mesh_pt));
    }
  }

  // create a property-map for sdf values
  std::vector<double> sdf_values( num_faces(*input_triangle_mesh) );
  Facet_with_id_pmap<double> sdf_property_map(sdf_values);

  // compute sdf values with skeleton
  BOOST_FOREACH(Polyhedron::Face_handle f, faces(*input_triangle_mesh))
  {
    double dist = 0;
    BOOST_FOREACH(Polyhedron::Halfedge_handle hd, halfedges_around_face(halfedge(f, *input_triangle_mesh), *input_triangle_mesh))
      dist+=distances[target(hd, *input_triangle_mesh)->id()];
    sdf_property_map[f] = dist / 3.;
  }

  // post-process the sdf values
  CGAL::sdf_values_postprocessing(*input_triangle_mesh, sdf_property_map);

  // create a property-map for segment-ids (it is an adaptor for this case)
  std::vector<std::size_t> segment_ids( num_faces(*input_triangle_mesh) );
  Facet_with_id_pmap<std::size_t> segment_property_map(segment_ids);

  // segment the mesh using default parameters
  std::cout << "Number of segments: "
            << CGAL::segmentation_from_sdf_values(*input_triangle_mesh, sdf_property_map, segment_property_map) <<"\n";

  Polyhedron* segmented_polyhedron = new Polyhedron(*input_triangle_mesh);

  int i=0;
  BOOST_FOREACH(Polyhedron::Face_handle fd, faces(*segmented_polyhedron))
  {
    fd->set_patch_id( static_cast<int>(segment_ids[i++] ));
  }

  scene->item(InputMeshItemIndex)->setVisible(false);
  Scene_polyhedron_item* item_segmentation = new Scene_polyhedron_item(segmented_polyhedron);
  item_segmentation->setItemIsMulticolor(true);
  scene->addItem(item_segmentation);
  item_segmentation->setName(QString("segmentation"));

  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionConvert_to_medial_skeleton_triggered()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_polyhedron_item* item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  if(item)
  {
    Polyhedron* pMesh = item->polyhedron();

    if ( !is_mesh_valid(pMesh) ) return;

    QTime time;
    time.start();
    QApplication::setOverrideCursor(Qt::WaitCursor);

    Skeleton skeleton;
    CGAL::extract_mean_curvature_flow_skeleton(*pMesh, skeleton);

    std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

    //create the polylines representing the skeleton
    Scene_polylines_item* skeleton_item = new Scene_polylines_item();
    skeleton_item->setColor(QColor(175, 0, 255));

    Polyline_visitor polyline_visitor(skeleton_item->polylines, skeleton);
    CGAL::split_graph_into_polylines( skeleton,
                                      polyline_visitor,
                                      CGAL::internal::IsTerminalDefault() );

    skeleton_item->setName(QString("Medial skeleton curve of %1").arg(item->name()));
    scene->addItem(skeleton_item);
    skeleton_item->invalidateOpenGLBuffers();

    item->setPointsMode();

    QApplication::restoreOverrideCursor();
  }
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionContract()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

  Scene_polyhedron_item* item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  if (!check_mesh(item))
  {
    return;
  }

  QTime time;
  time.start();
  std::cout << "Contract...\n";
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(mcs);
  mcs->contract_geometry();

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  update_meso_skeleton();
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionCollapse()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

  Scene_polyhedron_item* item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  if (!check_mesh(item))
  {
    return;
  }

  QTime time;
  time.start();
  std::cout << "Collapse...\n";
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(mcs);
  std::size_t num_collapses = mcs->collapse_edges();
  std::cout << "collapsed " << num_collapses << " edges.\n";

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  update_meso_skeleton();
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionSplit()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

  Scene_polyhedron_item* item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  if (!check_mesh(item))
  {
    return;
  }

  QTime time;
  time.start();
  std::cout << "Split...\n";
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(mcs);
  std::size_t num_split = mcs->split_faces();
  std::cout << "split " << num_split << " triangles.\n";

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  update_meso_skeleton();
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionDegeneracy()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

  Scene_polyhedron_item* item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  if (!check_mesh(item))
  {
    return;
  }

  QTime time;
  time.start();
  std::cout << "Detect degeneracy...\n";
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(mcs);
  mcs->detect_degeneracies();

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  Scene_points_with_normal_item* fixedPointsItem = new Scene_points_with_normal_item;
  fixedPointsItem->setName(QString("fixed points of %1").arg(item->name()));

  std::vector<Point> fixedPoints;
  mcs->fixed_points(fixedPoints);

  Point_set *ps = fixedPointsItem->point_set();
  for (size_t i = 0; i < fixedPoints.size(); ++i)
  {
    UI_point_3<Kernel> point(fixedPoints[i].x(), fixedPoints[i].y(), fixedPoints[i].z());
    ps->push_back(point);
  }
  ps->select_all ();

  if (fixedPointsItemIndex == -1)
  {
    fixedPointsItemIndex = scene->addItem(fixedPointsItem);
  }
  else
  {
    Scene_item* temp = scene->replaceItem(fixedPointsItemIndex, fixedPointsItem, false);
    delete temp;
  }
  // update scene
  update_meso_skeleton();
  scene->item(fixedPointsItemIndex)->invalidateOpenGLBuffers();
  scene->itemChanged(fixedPointsItemIndex);
  scene->setSelectedItem(index);
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionRun()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

  Scene_polyhedron_item* item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  if (!check_mesh(item))
  {
    return;
  }

  QTime time;
  time.start();
  QApplication::setOverrideCursor(Qt::WaitCursor);

  std::cout << "Run one iteration...\n";

  update_parameters(mcs);
  mcs->contract();

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  CGAL::Three::Scene_interface::Item_id contracted_item_index = scene->mainSelectionIndex();
  Scene_polyhedron_item* contracted_item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(contracted_item_index));

  // update scene
  Scene_points_with_normal_item* fixedPointsItem = new Scene_points_with_normal_item;
  fixedPointsItem->setName(QString("fixed points of %1").arg(contracted_item->name()));

  std::vector<Point> fixedPoints;
  mcs->fixed_points(fixedPoints);

  Point_set *ps = fixedPointsItem->point_set();
  for (size_t i = 0; i < fixedPoints.size(); ++i)
  {
    UI_point_3<Kernel> point(fixedPoints[i].x(), fixedPoints[i].y(), fixedPoints[i].z());
    ps->push_back(point);
  }
  ps->select_all();

  if (fixedPointsItemIndex == -1)
  {
    fixedPointsItemIndex = scene->addItem(fixedPointsItem);
  }
  else
  {
    Scene_item* temp = scene->replaceItem(fixedPointsItemIndex, fixedPointsItem, false);
    delete temp;
  }

//#define DRAW_NON_FIXED_POINTS
#ifdef DRAW_NON_FIXED_POINTS
  // draw non-fixed points
  Scene_points_with_normal_item* nonFixedPointsItem = new Scene_points_with_normal_item;
  nonFixedPointsItem->setName("non-fixed points");
  nonFixedPointsItem->setColor(QColor(0, 255, 0));
  std::vector<Point> nonFixedPoints;
  mcs->non_fixed_points(nonFixedPoints);
  ps = nonFixedPointsItem->point_set();
  for (size_t i = 0; i < nonFixedPoints.size(); ++i)
  {
    UI_point_3<Kernel> point(nonFixedPoints[i].x(), nonFixedPoints[i].y(), nonFixedPoints[i].z());
    ps->push_back(point);
  }
  if (nonFixedPointsItemIndex == -1)
  {
    nonFixedPointsItemIndex = scene->addItem(nonFixedPointsItem);
  }
  else
  {
    scene->replaceItem(nonFixedPointsItemIndex, nonFixedPointsItem, false);
  }
  scene->itemChanged(nonFixedPointsItemIndex);
#endif

//#define DRAW_POLE_LINE
#ifdef DRAW_POLE_LINE
  // draw lines connecting surface points and their correspondent poles
  Scene_polylines_item* poleLinesItem = new Scene_polylines_item();

  Polyhedron* pMesh = item->polyhedron();
  std::vector<Point> pole_points;
  mcs->poles(pole_points);
  vertex_iterator vb, ve;
  int id = 0;
  for (boost::tie(vb, ve) = vertices(*pMesh); vb != ve; ++vb)
  {
    std::vector<Point> line;
    line.clear();

    vertex_descriptor v = *vb;
    Point s = v->point();
    Point t = pole_points[id++];

    line.push_back(s);
    line.push_back(t);
    poleLinesItem->polylines.push_back(line);
  }

  if (poleLinesItemIndex == -1)
  {
    poleLinesItemIndex = scene->addItem(poleLinesItem);
  }
  else
  {
    scene->replaceItem(poleLinesItemIndex, poleLinesItem, false);
  }
#endif

  update_meso_skeleton();
  scene->setSelectedItem(index);
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionSkeletonize()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

  Scene_polyhedron_item* item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  if (!check_mesh(item))
  {
    return;
  }

  QTime time;
  time.start();
  QApplication::setOverrideCursor(Qt::WaitCursor);

  update_parameters(mcs);

  mcs->convert_to_skeleton(skeleton_curve);

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  //create the polylines representing the skeleton
  Scene_polylines_item* skeleton = new Scene_polylines_item();
  skeleton->setColor(QColor(175, 0, 255));

  Polyline_visitor polyline_visitor(skeleton->polylines, skeleton_curve);
  CGAL::split_graph_into_polylines( skeleton_curve,
                                    polyline_visitor,
                                    CGAL::internal::IsTerminalDefault() );

  skeleton->setName(QString("skeleton curve of %1").arg(item->name()));
  scene->addItem(skeleton);
  skeleton->invalidateOpenGLBuffers();

  // set the fixed points and contracted mesh as invisible
  if (fixedPointsItemIndex >= 0)
  {
    scene->item(fixedPointsItemIndex)->setVisible(false);
    scene->itemChanged(fixedPointsItemIndex);
  }
  scene->item(contractedItemIndex)->setVisible(false);
  scene->itemChanged(contractedItemIndex);
  // display the original mesh in transparent mode
  item->setVisible(false);
  if (InputMeshItemIndex >= 0)
  {
    scene->item(InputMeshItemIndex)->setVisible(true);
    scene->item(InputMeshItemIndex)->setPointsMode();
    scene->itemChanged(InputMeshItemIndex);
  }

  // update scene
  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionConverge()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();
  if (!check_item_index(index))
  {
    return;
  }

  Scene_polyhedron_item* item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  if (!check_mesh(item))
  {
    return;
  }

  QTime time;
  time.start();
  QApplication::setOverrideCursor(Qt::WaitCursor);

  mcs->contract_until_convergence();

  std::cout << "ok (" << time.elapsed() << " ms, " << ")" << std::endl;

  // update scene
  Scene_points_with_normal_item* fixedPointsItem = new Scene_points_with_normal_item;
  fixedPointsItem->setName(QString("fixed points of %1").arg(item->name()));

  std::vector<Point> fixedPoints;
  mcs->fixed_points(fixedPoints);

  Point_set *ps = fixedPointsItem->point_set();
  for (size_t i = 0; i < fixedPoints.size(); ++i)
  {
    UI_point_3<Kernel> point(fixedPoints[i].x(), fixedPoints[i].y(), fixedPoints[i].z());
    ps->push_back(point);
  }
  ps->select_all();

  if (fixedPointsItemIndex == -1)
  {
    fixedPointsItemIndex = scene->addItem(fixedPointsItem);
  }
  else
  {
    Scene_item* temp = scene->replaceItem(fixedPointsItemIndex, fixedPointsItem, false);
    delete temp;
  }

  scene->item(fixedPointsItemIndex)->invalidateOpenGLBuffers();
  scene->itemChanged(fixedPointsItemIndex);
  update_meso_skeleton();
  scene->setSelectedItem(index);

  QApplication::restoreOverrideCursor();
}

void Polyhedron_demo_mean_curvature_flow_skeleton_plugin::on_actionItemAboutToBeDestroyed(CGAL::Three::Scene_item* /* item */)
{
  if (mcs != NULL)
  {
    delete mcs;
    mcs = NULL;
  }
}

#include "Mean_curvature_flow_skeleton_plugin.moc"


// -----------------------------------------------------------------------------
// Copyright (c) 2022 Mohamed Aladem
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this softwareand associated documentation files(the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and /or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions :
//
// The above copyright noticeand this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
// -----------------------------------------------------------------------------

#include "micro_graph_optimizer.h"

#include <math.h>
#include <map>
#include <Eigen/Sparse>

namespace mgo
{
  namespace
  {
    struct VariableInfo
    {
    public:
      inline VariableInfo& set_dim(int val) { m_dim = val; return *this; }
      inline VariableInfo& set_idx(int val) { m_idx = val; return *this; }
      inline int dim()const { return m_dim; }
      inline int idx()const { return m_idx; }

    private:
      int m_dim;
      int m_idx; // The index of this variable in the H matrix.
    };

    struct ScratchPad
    {
      int total_variables_dim = 0;
      int total_factors_dim = 0;
      std::map<Variable*, VariableInfo> variable_lookup_table;
      inline const VariableInfo& variable_lookup(Variable* var) { MGO_ASSERT(variable_lookup_table.count(var) != 0); return variable_lookup_table[var]; }
      typedef Eigen::Triplet<double> T;
      std::vector<T> tripletList;
      Eigen::SparseMatrix<double> H;
      Eigen::VectorXd b;
    };

    // ===================================================================
    void compute_graph_info(const FactorGraph& graph, ScratchPad* pad)
    {
      const std::vector<Variable*>& variables = graph.get_variables();
      int total_variables_dim = 0;
      std::map<Variable*, VariableInfo> variable_lookup_table;
      for (int i = 0, var_idx = 0, count = variables.size(); i < count; ++i)
      {
        Variable* var = variables[i];
        if (!var->fixed)
        {
          const int v_dim = var->dim();
          total_variables_dim += v_dim;
          variable_lookup_table[var] = VariableInfo().set_dim(v_dim).set_idx(var_idx);
          var_idx += v_dim;
        }
      }
      pad->total_variables_dim = total_variables_dim;
      pad->variable_lookup_table.swap(variable_lookup_table);

      const std::vector<Factor*>& factors = graph.get_factors();
      int total_factors_dim = 0;
      for (int i = 0, count = factors.size(); i < count; ++i)
      {
        const int f_dim = factors[i]->dim();
        total_factors_dim += f_dim;
      }
      pad->total_factors_dim = total_factors_dim;

      pad->H = Eigen::SparseMatrix<double>(total_variables_dim, total_variables_dim);
      pad->b = Eigen::VectorXd::Zero(total_variables_dim);
    }

    // ===================================================================
    double compute_error_norm_squared(const FactorGraph& graph)
    {
      const std::vector<Factor*>& factors = graph.get_factors();
      double error = 0.0;
      for (size_t i = 0, count = factors.size(); i < count; ++i)
      {
        error += factors[i]->error().squaredNorm();
      }
      return error;
    }

    // ===================================================================
    void linearize_single_factor(Factor* factor, ScratchPad* pad)
    {
      // Our goal in this function is to add the contribution of a factor
      // to the linearized system (H, b).

      // First we need to calculate the jacobian of the factor wrt each of
      // its variables then stack them horizontally in Js then compute Jt*J matrix.
      const int n_rows = factor->dim();
      const int num_variables = factor->num_variables();
      std::vector<int> vars_cols(num_variables, -1);
      std::vector<int> vars_dim(num_variables, -1);
      int n_cols = 0;
      for (int i = 0; i < num_variables; ++i)
      {
        Variable* var = factor->variable_at(i);
        if (!var->fixed)
        {
          const int var_dim = pad->variable_lookup(var).dim();
          vars_cols[i] = n_cols;
          vars_dim[i] = var_dim;
          n_cols += var_dim;
        }
      }

      if (n_cols == 0)
      {
        MGO_LOG("All variables connected to this factor are fixed.");
        return;
      }

      Eigen::MatrixXd Js(n_rows, n_cols);
      for (int i = 0, start_col = 0; i < num_variables; ++i)
      {
        if (!factor->variable_at(i)->fixed)
        {
          Eigen::MatrixXd J = factor->jacobian(i);
          Js.block(0, start_col, J.rows(), J.cols()) = J;
          start_col += J.cols();
        }
      }

      Eigen::MatrixXd JtJ(Js.cols(), Js.cols());
      JtJ.noalias() = Js.transpose() * Js;

      // Now we need to add the contribution to H. Note that we are only filling the lower triangular part.
      for (int i = 0; i < num_variables; ++i)
      {
        if (factor->variable_at(i)->fixed)
        {
          continue;
        }
        const int H_col = pad->variable_lookup(factor->variable_at(i)).idx();
        const int JtJ_col = vars_cols[i];
        for (int j = i; j < num_variables; ++j)
        {
          if (factor->variable_at(j)->fixed)
          {
            continue;
          }
          const int H_row = pad->variable_lookup(factor->variable_at(j)).idx();
          const int JtJ_row = vars_cols[j];
          for (int JtJ_i = JtJ_col, H_i = H_col; JtJ_i < (JtJ_col + vars_dim[i]); ++JtJ_i, ++H_i)
          {
            for (int JtJ_j = JtJ_row, H_j = H_row; JtJ_j < (JtJ_row + vars_dim[j]); ++JtJ_j, ++H_j)
            {
              pad->tripletList.push_back(ScratchPad::T(H_j, H_i, JtJ(JtJ_j, JtJ_i)));
            }
          }
        }
      }

      // Handle the vector b.
      const Eigen::VectorXd Jtb = Js.transpose() * factor->error();
      for (int i = 0; i < num_variables; ++i)
      {
        Variable* var = factor->variable_at(i);
        if (!var->fixed)
        {
          const int var_idx = pad->variable_lookup(var).idx();
          const int var_dim = vars_dim[i];
          pad->b.segment(var_idx, var_dim) -= Jtb.segment(vars_cols[i], var_dim);
        }
      }
    }

    // ===================================================================
    bool iterate(FactorGraph* graph, ScratchPad* pad)
    {
      const std::vector<Factor*>& factors = graph->get_factors();
      pad->b.setZero();
      pad->tripletList.clear();
      for (size_t i = 0, count = factors.size(); i < count; ++i)
      {
        linearize_single_factor(factors[i], pad);
      }
      MGO_ASSERT(!pad->tripletList.empty());
      pad->H.setFromTriplets(pad->tripletList.begin(), pad->tripletList.end());

      Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> chol(pad->H);
      Eigen::VectorXd dx = chol.solve(pad->b);
      if (chol.info() == Eigen::Success)
      {
        std::vector<Variable*>& variables = graph->get_variables();
        for (int i = 0, d = 0, count = variables.size(); i < count; ++i)
        {
          Variable* var = variables[i];
          if (!var->fixed)
          {
            const int vd = pad->variable_lookup_table[var].dim();
            var->plus(dx.segment(d, vd));
            d += vd;
          }
        }
        return true;
      }
      else
      {
        MGO_LOG("Linear solver failed.");
        return false;
      }
    }

    // ===================================================================
    bool continue_iterating_check(int iter_num, double current_error, double new_error, const OptimizationParameters& params, bool* converged)
    {
      if (!std::isfinite(new_error))
      {
        return false;
      }

      if (iter_num == params.max_iterations)
      {
        MGO_LOG("Max iterations reached.");
        return false;
      }

      const double error_decrease = current_error - new_error;
      if ((error_decrease <= params.absolute_error_th) ||
        ((error_decrease / current_error) <= params.relative_error_th))
      {
        MGO_LOG("Converged.");
        *converged = true;
        return false;
      }

      return true;
    }

    // ===================================================================
  } // anonymous namespace

  bool optimize_gn(FactorGraph* graph, const OptimizationParameters& params)
  {
    MGO_LOG("Started optimization with %i factors and %i variables.", (int)graph->get_factors().size(), (int)graph->get_variables().size());
    ScratchPad pad;
    compute_graph_info(*graph, &pad);
    double current_error = 0.5 * compute_error_norm_squared(*graph);
    MGO_LOG("Initial error: %f", current_error);
    double new_error = current_error;
    int iter_num = 0;
    bool converged = false;
    bool continue_iterating = true;
    while (continue_iterating)
    {
      current_error = new_error;
      if (!iterate(graph, &pad))
      {
        return false;
      }
      ++iter_num;
      new_error = 0.5 * compute_error_norm_squared(*graph);
      MGO_LOG("New error after iteration %i: %f", iter_num, new_error);
      continue_iterating = continue_iterating_check(iter_num, current_error, new_error, params, &converged);
    }
    return converged;
  }

  // ===================================================================
  Eigen::MatrixXd Factor::jacobian(int idx)const
  {
    MGO_ASSERT(m_num_variables > 0 && idx < m_num_variables);
    return compute_numerical_jacobian(m_variables[idx]);
  }

  // ===================================================================
  Eigen::MatrixXd Factor::compute_numerical_jacobian(Variable* v)const
  {
    constexpr double h = 1e-5;
    const int N = v->dim();
    const int M = this->dim();
    Eigen::MatrixXd J = Eigen::MatrixXd::Zero(M, N);
    Eigen::VectorXd dx = Eigen::VectorXd::Zero(N);
    Eigen::VectorXd dy0 = this->error();
    constexpr double k = 1.0 / (2.0 * h);
    for (int i = 0; i < N; ++i)
    {
      dx(i) = h;
      v->plus(dx); // right
      const Eigen::VectorXd dy1 = this->subtract_error(this->error(), dy0);
      dx(i) = -2.0 * h;
      v->plus(dx); // left
      const Eigen::VectorXd dy2 = this->subtract_error(this->error(), dy0);
      dx(i) = h;
      v->plus(dx); // return to original state.
      dx(i) = 0.0;
      J.col(i) << (dy1 - dy2) * k;
    }
    return J;
  }

  // ===================================================================
  FactorGraph::FactorGraph() = default;

  FactorGraph::~FactorGraph()
  {
    for (Factor* f : m_factors)
    {
      delete f;
    }
    for (Variable* v : m_variables)
    {
      delete v;
    }
  }
}


/*
 * Update.hpp
 *
 *  Created on: Feb 9, 2014
 *      Author: Bloeschm
 */

#ifndef LWF_UPDATEMODEL_HPP_
#define LWF_UPDATEMODEL_HPP_

#include "lightweight_filtering/common.hpp"
#include "lightweight_filtering/ModelBase.hpp"
#include "lightweight_filtering/PropertyHandler.hpp"
#include "lightweight_filtering/SigmaPoints.hpp"
#include "lightweight_filtering/OutlierDetection.hpp"
#include <Eigen/StdVector>

namespace LWF{

template<typename Innovation, typename FilterState, typename Meas, typename Noise, typename OutlierDetection = OutlierDetectionDefault, bool isCoupled = false>
class Update: public ModelBase<Update<Innovation,FilterState,Meas,Noise,OutlierDetection,isCoupled>,Innovation,typename FilterState::mtState,Noise>, public PropertyHandler{
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  static_assert(!isCoupled || Noise::D_ == FilterState::noiseExtensionDim_,"Noise Size for coupled Update must match noise extension of prediction!");
  typedef ModelBase<Update<Innovation,FilterState,Meas,Noise,OutlierDetection,isCoupled>,Innovation,typename FilterState::mtState,Noise> mtModelBase;
  typedef FilterState mtFilterState;
  typedef typename mtFilterState::mtState mtState;
  typedef typename mtModelBase::mtInputTuple mtInputTuple;
  typedef typename mtFilterState::mtPredictionMeas mtPredictionMeas;
  typedef typename mtFilterState::mtPredictionNoise mtPredictionNoise;
  typedef Innovation mtInnovation;
  typedef Meas mtMeas;
  typedef Noise mtNoise;
  typedef OutlierDetection mtOutlierDetection;
  mtMeas meas_; // TODO change to pointer, or remove
  static const bool coupledToPrediction_ = isCoupled;
  bool useSpecialLinearizationPoint_;
  bool useImprovedJacobian_;
  bool hasConverged_;
  bool successfulUpdate_;
  mutable bool cancelIteration_;
  mutable int candidateCounter_;
  mutable Eigen::MatrixXd H_;
  Eigen::MatrixXd Hlin_;
  Eigen::MatrixXd boxMinusJac_;
  Eigen::MatrixXd Hn_;
  Eigen::MatrixXd updnoiP_;
  Eigen::MatrixXd noiP_;
  Eigen::MatrixXd preupdnoiP_;
  Eigen::MatrixXd C_;
  mtInnovation y_;
  mutable Eigen::MatrixXd Py_;
  Eigen::MatrixXd Pyinv_;
  typename mtInnovation::mtDifVec innVector_;
  mtInnovation yIdentity_;
  typename mtState::mtDifVec updateVec_;
  mtState linState_;
  double updateVecNorm_;
  Eigen::MatrixXd K_;
  Eigen::MatrixXd Pyx_;
  mutable typename mtState::mtDifVec difVecLinInv_;

  SigmaPoints<mtState,2*mtState::D_+1,2*(mtState::D_+mtNoise::D_)+1,0> stateSigmaPoints_;
  SigmaPoints<mtNoise,2*mtNoise::D_+1,2*(mtState::D_+mtNoise::D_)+1,2*mtState::D_> stateSigmaPointsNoi_;
  SigmaPoints<mtInnovation,2*(mtState::D_+mtNoise::D_)+1,2*(mtState::D_+mtNoise::D_)+1,0> innSigmaPoints_;
  SigmaPoints<mtNoise,2*(mtNoise::D_+mtPredictionNoise::D_)+1,2*(mtState::D_+mtNoise::D_+mtPredictionNoise::D_)+1,2*(mtState::D_)> coupledStateSigmaPointsNoi_;
  SigmaPoints<mtInnovation,2*(mtState::D_+mtNoise::D_+mtPredictionNoise::D_)+1,2*(mtState::D_+mtNoise::D_+mtPredictionNoise::D_)+1,0> coupledInnSigmaPoints_;
  SigmaPoints<LWF::VectorElement<mtState::D_>,2*mtState::D_+1,2*mtState::D_+1,0> updateVecSP_;
  SigmaPoints<mtState,2*mtState::D_+1,2*mtState::D_+1,0> posterior_;
  double alpha_;
  double beta_;
  double kappa_;
  double updateVecNormTermination_;
  int maxNumIteration_;
  int iterationNum_;
  mtOutlierDetection outlierDetection_;
  unsigned int numSequences;
  bool disablePreAndPostProcessingWarning_;
  Update(): H_((int)(mtInnovation::D_),(int)(mtState::D_)),
      Hlin_((int)(mtInnovation::D_),(int)(mtState::D_)),
      boxMinusJac_((int)(mtState::D_),(int)(mtState::D_)),
      Hn_((int)(mtInnovation::D_),(int)(mtNoise::D_)),
      updnoiP_((int)(mtNoise::D_),(int)(mtNoise::D_)),
      noiP_((int)(mtNoise::D_),(int)(mtNoise::D_)),
      preupdnoiP_((int)(mtPredictionNoise::D_),(int)(mtNoise::D_)),
      C_((int)(mtState::D_),(int)(mtInnovation::D_)),
      Py_((int)(mtInnovation::D_),(int)(mtInnovation::D_)),
      Pyinv_((int)(mtInnovation::D_),(int)(mtInnovation::D_)),
      K_((int)(mtState::D_),(int)(mtInnovation::D_)),
      Pyx_((int)(mtInnovation::D_),(int)(mtState::D_)){
    alpha_ = 1e-3;
    beta_ = 2.0;
    kappa_ = 0.0;
    updateVecNormTermination_ = 1e-6;
    maxNumIteration_  = 10;
    updnoiP_.setIdentity();
    updnoiP_ *= 0.0001;
    noiP_.setZero();
    preupdnoiP_.setZero();
    useSpecialLinearizationPoint_ = false;
    useImprovedJacobian_ = false;
    yIdentity_.setIdentity();
    updateVec_.setIdentity();
    refreshNoiseSigmaPoints();
    // refreshUKFParameter();
    mtNoise n;
    n.setIdentity();
    n.registerCovarianceToPropertyHandler_(updnoiP_,this,"UpdateNoise.");
    doubleRegister_.registerScalar("alpha",alpha_);
    doubleRegister_.registerScalar("beta",beta_);
    doubleRegister_.registerScalar("kappa",kappa_);
    doubleRegister_.registerScalar("updateVecNormTermination",updateVecNormTermination_);
    intRegister_.registerScalar("maxNumIteration",maxNumIteration_);
    outlierDetection_.setEnabledAll(false);
    numSequences = 1;
    disablePreAndPostProcessingWarning_ = false;
  };
  virtual ~Update(){};
  void refreshNoiseSigmaPoints(){
    if(noiP_ != updnoiP_){
      noiP_ = updnoiP_;
      stateSigmaPointsNoi_.computeFromZeroMeanGaussian(noiP_);
    }
  }
  void refreshUKFParameter(){
    stateSigmaPoints_.computeParameter(alpha_,beta_,kappa_);
    innSigmaPoints_.computeParameter(alpha_,beta_,kappa_);
    coupledInnSigmaPoints_.computeParameter(alpha_,beta_,kappa_);
    updateVecSP_.computeParameter(alpha_,beta_,kappa_);
    posterior_.computeParameter(alpha_,beta_,kappa_);
    stateSigmaPointsNoi_.computeParameter(alpha_,beta_,kappa_);
    stateSigmaPointsNoi_.computeFromZeroMeanGaussian(noiP_);
    coupledStateSigmaPointsNoi_.computeParameter(alpha_,beta_,kappa_);
  }
  void refreshProperties(){
    refreshPropertiesCustom();
    // refreshUKFParameter();
  }
  virtual void refreshPropertiesCustom(){}
  void eval_(mtInnovation& x, const mtInputTuple& inputs, double dt) const{
    evalInnovation(x,std::get<0>(inputs),std::get<1>(inputs));
  }
  template<int i,typename std::enable_if<i==0>::type* = nullptr>
  void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{
    jacState(F,std::get<0>(inputs));
  }
  template<int i,typename std::enable_if<i==1>::type* = nullptr>
  void jacInput_(Eigen::MatrixXd& F, const mtInputTuple& inputs, double dt) const{
    jacNoise(F,std::get<0>(inputs));
  }
  virtual void evalInnovation(mtInnovation& y, const mtState& state, const mtNoise& noise) const = 0;
  virtual void evalInnovationShort(mtInnovation& y, const mtState& state) const{
    mtNoise n; // TODO get static for Identity()
    n.setIdentity();
    evalInnovation(y,state,n);
  }
  virtual void jacState(Eigen::MatrixXd& F, const mtState& state) const = 0;
  virtual void jacNoise(Eigen::MatrixXd& F, const mtState& state) const = 0;
  virtual void preProcess(mtFilterState& filterState, const mtMeas& meas, bool& isFinished){
    isFinished = false;
    if(!disablePreAndPostProcessingWarning_){
      std::cout << "Warning: update preProcessing is not implemented!" << std::endl;
    }
  }
  virtual bool extraOutlierCheck(const mtState& state) const{
    return hasConverged_;
  }
  virtual bool generateCandidates(const mtFilterState& filterState, mtState& candidate) const{
    candidate = filterState.state_;
    candidateCounter_++;
    if(candidateCounter_<=1)
      return true;
    else
      return false;
  }
  virtual void postProcess(mtFilterState& filterState, const mtMeas& meas, const mtOutlierDetection& outlierDetection, bool& isFinished){
    isFinished = true;
    if(!disablePreAndPostProcessingWarning_){
      std::cout << "Warning: update postProcessing is not implemented!" << std::endl;
    }
  }
  int performUpdate(mtFilterState& filterState, const mtMeas& meas){
    bool isFinished = true;
    int r = 0;
    do {
      preProcess(filterState,meas,isFinished);
      if(!isFinished){
        switch(filterState.mode_){
          case ModeEKF:
            r = performUpdateEKF(filterState,meas);
            break;
          case ModeUKF:
            r = performUpdateUKF(filterState,meas);
            break;
          case ModeIEKF:
            r = performUpdateIEKF(filterState,meas);
            break;
          default:
            r = performUpdateEKF(filterState,meas);
            break;
        }
      }
      postProcess(filterState,meas,outlierDetection_,isFinished);
      filterState.state_.fix();
      enforceSymmetry(filterState.cov_);
    } while (!isFinished);
    return r;
  }
  int performUpdateEKF(mtFilterState& filterState, const mtMeas& meas){
    meas_ = meas;
    if(!useSpecialLinearizationPoint_){
      this->jacState(H_,filterState.state_);
      Hlin_ = H_;
      this->jacNoise(Hn_,filterState.state_);
      this->evalInnovationShort(y_,filterState.state_);
    } else {
      filterState.state_.boxPlus(filterState.difVecLin_,linState_);
      this->jacState(H_,linState_);
      if(useImprovedJacobian_){
        filterState.state_.boxMinusJac(linState_,boxMinusJac_);
        Hlin_ = H_*boxMinusJac_;
      } else {
        Hlin_ = H_;
      }
      this->jacNoise(Hn_,linState_);
      this->evalInnovationShort(y_,linState_);
    }

    if(isCoupled){
      C_ = filterState.G_*preupdnoiP_*Hn_.transpose();
      Py_ = Hlin_*filterState.cov_*Hlin_.transpose() + Hn_*updnoiP_*Hn_.transpose() + Hlin_*C_ + C_.transpose()*Hlin_.transpose();
    } else {
      Py_ = Hlin_*filterState.cov_*Hlin_.transpose() + Hn_*updnoiP_*Hn_.transpose();
    }
    y_.boxMinus(yIdentity_,innVector_);

    // Outlier detection // TODO: adapt for special linearization point
    outlierDetection_.doOutlierDetection(innVector_,Py_,Hlin_);
    Pyinv_.setIdentity();
    Py_.llt().solveInPlace(Pyinv_);

    // Kalman Update
    if(isCoupled){
      K_ = (filterState.cov_*Hlin_.transpose()+C_)*Pyinv_;
    } else {
      K_ = filterState.cov_*Hlin_.transpose()*Pyinv_;
    }
    filterState.cov_ = filterState.cov_ - K_*Py_*K_.transpose();
    if(!useSpecialLinearizationPoint_){
      updateVec_ = -K_*innVector_;
    } else {
      filterState.state_.boxMinus(linState_,difVecLinInv_);
      updateVec_ = -K_*(innVector_+H_*difVecLinInv_); // includes correction for offseted linearization point, dif must be recomputed (a-b != (-(b-a)))
    }
    filterState.state_.boxPlus(updateVec_,filterState.state_);
    return 0;
  }
  int performUpdateIEKF(mtFilterState& filterState, const mtMeas& meas){
    meas_ = meas;
    successfulUpdate_ = false;
    candidateCounter_ = 0;

    std::vector<double> scores;
    std::vector<mtState, Eigen::aligned_allocator<mtState>> states;
    double bestScore = -1.0;
    mtState bestState;
    MXD bestCov;

    while(generateCandidates(filterState,linState_)){
      cancelIteration_ = false;
      hasConverged_ = false;
      for(iterationNum_=0;iterationNum_<maxNumIteration_ && !hasConverged_ && !cancelIteration_;iterationNum_++){
        this->jacState(H_,linState_);
        this->jacNoise(Hn_,linState_);
        this->evalInnovationShort(y_,linState_);

        if(isCoupled){
          C_ = filterState.G_*preupdnoiP_*Hn_.transpose();
          Py_ = H_*filterState.cov_*H_.transpose() + Hn_*updnoiP_*Hn_.transpose() + H_*C_ + C_.transpose()*H_.transpose();
        } else {
          Py_ = H_*filterState.cov_*H_.transpose() + Hn_*updnoiP_*Hn_.transpose();
        }
        y_.boxMinus(yIdentity_,innVector_);

        // Outlier detection
        outlierDetection_.doOutlierDetection(innVector_,Py_,H_);
        Pyinv_.setIdentity();
        Py_.llt().solveInPlace(Pyinv_);

        // Kalman Update
        if(isCoupled){
          K_ = (filterState.cov_*H_.transpose()+C_)*Pyinv_;
        } else {
          K_ = filterState.cov_*H_.transpose()*Pyinv_;
        }
        filterState.state_.boxMinus(linState_,difVecLinInv_);
        updateVec_ = -K_*(innVector_+H_*difVecLinInv_)+difVecLinInv_; // includes correction for offseted linearization point, dif must be recomputed (a-b != (-(b-a)))
        linState_.boxPlus(updateVec_,linState_);
        updateVecNorm_ = updateVec_.norm();
        hasConverged_ = updateVecNorm_<=updateVecNormTermination_;
      }
      if(extraOutlierCheck(linState_)){
        successfulUpdate_ = true;
        double score = (innVector_.transpose()*Pyinv_*innVector_)(0);
        scores.push_back(score);
        states.push_back(linState_);
        if(bestScore == -1.0 || score < bestScore){
          bestScore = score;
          bestState = linState_;
          bestCov = filterState.cov_ - K_*Py_*K_.transpose();
        }
      }
    }

    if(successfulUpdate_){
      if(scores.size() == 1){
        filterState.state_ = bestState;
        filterState.cov_ = bestCov;
      } else {
        bool foundOtherMin = false;
        for(auto it = states.begin();it!=states.end();it++){
          bestState.boxMinus(*it,difVecLinInv_);
          if(difVecLinInv_.norm()>2*updateVecNormTermination_){
            foundOtherMin = true;
            break;
          }
        }
        if(!foundOtherMin){
          filterState.state_ = bestState;
          filterState.cov_ = bestCov;
        } else {
          successfulUpdate_ = false;
        }
      }
    }
    return 0;
  }
  int performUpdateUKF(mtFilterState& filterState, const mtMeas& meas){
    meas_ = meas;
    handleUpdateSigmaPoints<isCoupled>(filterState);
    y_.boxMinus(yIdentity_,innVector_);

    outlierDetection_.doOutlierDetection(innVector_,Py_,Pyx_);
    Pyinv_.setIdentity();
    Py_.llt().solveInPlace(Pyinv_);

    // Kalman Update
    K_ = Pyx_.transpose()*Pyinv_;
    filterState.cov_ = filterState.cov_ - K_*Py_*K_.transpose();
    updateVec_ = -K_*innVector_;

    // Adapt for proper linearization point
    updateVecSP_.computeFromZeroMeanGaussian(filterState.cov_);
    for(unsigned int i=0;i<2*mtState::D_+1;i++){
      filterState.state_.boxPlus(updateVec_+updateVecSP_(i).v_,posterior_(i));
    }
    posterior_.getMean(filterState.state_);
    posterior_.getCovarianceMatrix(filterState.state_,filterState.cov_);
    return 0;
  }
  template<bool IC = isCoupled, typename std::enable_if<(IC)>::type* = nullptr>
  void handleUpdateSigmaPoints(mtFilterState& filterState){
    coupledStateSigmaPointsNoi_.extendZeroMeanGaussian(filterState.stateSigmaPointsNoi_,updnoiP_,preupdnoiP_);
    for(unsigned int i=0;i<coupledInnSigmaPoints_.L_;i++){
      this->evalInnovation(coupledInnSigmaPoints_(i),filterState.stateSigmaPointsPre_(i),coupledStateSigmaPointsNoi_(i));
    }
    coupledInnSigmaPoints_.getMean(y_);
    coupledInnSigmaPoints_.getCovarianceMatrix(y_,Py_);
    coupledInnSigmaPoints_.getCovarianceMatrix(filterState.stateSigmaPointsPre_,Pyx_);
  }
  template<bool IC = isCoupled, typename std::enable_if<(!IC)>::type* = nullptr>
  void handleUpdateSigmaPoints(mtFilterState& filterState){
    refreshNoiseSigmaPoints();
    stateSigmaPoints_.computeFromGaussian(filterState.state_,filterState.cov_);
    for(unsigned int i=0;i<innSigmaPoints_.L_;i++){
      this->evalInnovation(innSigmaPoints_(i),stateSigmaPoints_(i),stateSigmaPointsNoi_(i));
    }
    innSigmaPoints_.getMean(y_);
    innSigmaPoints_.getCovarianceMatrix(y_,Py_);
    innSigmaPoints_.getCovarianceMatrix(stateSigmaPoints_,Pyx_);
  }
  bool testUpdateJacs(double d = 1e-6,double th = 1e-6){
    mtState state;
    mtMeas meas;
    unsigned int s = 1;
    state.setRandom(s);
    meas.setRandom(s);
    return testUpdateJacs(state,meas,d,th);
  }
  bool testUpdateJacs(const mtState& state, const mtMeas& meas, double d = 1e-6,double th = 1e-6){
    mtInputTuple inputs;
    const double dt = 1.0;
    std::get<0>(inputs) = state;
    std::get<1>(inputs).setIdentity(); // Noise is always set to zero for Jacobians
    meas_ = meas;
    return this->testJacs(inputs,d,th,dt);
  }
};

}

#endif /* LWF_UPDATEMODEL_HPP_ */