//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_NEWTON_SOLVER_HPP
#define BOOST_MATH_TOOLS_NEWTON_SOLVER_HPP

#ifdef _MSC_VER
#pragma once
#endif
#include <boost/multiprecision/detail/number_base.hpp> // test for multiprecision types.
#include <boost/type_traits/is_complex.hpp> // test for complex types

#include <iostream>
#include <utility>
#include <boost/config/no_tr1/cmath.hpp>
#include <stdexcept>

#include <boost/math/tools/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/assert.hpp>
#include <boost/throw_exception.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4512)
#endif
#include <boost/math/tools/tuple.hpp>
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#include <boost/math/special_functions/sign.hpp>
#include <boost/math/tools/toms748_solve.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{ namespace math{ namespace tools{

namespace detail{

namespace dummy{

   template<int n, class T>
   typename T::value_type get(const T&) BOOST_MATH_NOEXCEPT(T);
}

template <class Tuple, class T>
void unpack_tuple(const Tuple& t, T& a, T& b) BOOST_MATH_NOEXCEPT(T)
{
   using dummy::get;
   // Use ADL to find the right overload for get:
   a = get<0>(t);
   b = get<1>(t);
}
template <class Tuple, class T>
void unpack_tuple(const Tuple& t, T& a, T& b, T& c) BOOST_MATH_NOEXCEPT(T)
{
   using dummy::get;
   // Use ADL to find the right overload for get:
   a = get<0>(t);
   b = get<1>(t);
   c = get<2>(t);
}

template <class Tuple, class T>
inline void unpack_0(const Tuple& t, T& val) BOOST_MATH_NOEXCEPT(T)
{
   using dummy::get;
   // Rely on ADL to find the correct overload of get:
   val = get<0>(t);
}

template <class T, class U, class V>
inline void unpack_tuple(const std::pair<T, U>& p, V& a, V& b) BOOST_MATH_NOEXCEPT(T)
{
   a = p.first;
   b = p.second;
}
template <class T, class U, class V>
inline void unpack_0(const std::pair<T, U>& p, V& a) BOOST_MATH_NOEXCEPT(T)
{
   a = p.first;
}

template <class F, class T>
void handle_zero_derivative(F f,
                            T& last_f0,
                            const T& f0,
                            T& delta,
                            T& result,
                            T& guess,
                            const T& min,
                            const T& max) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   if(last_f0 == 0)
   {
      // this must be the first iteration, pretend that we had a
      // previous one at either min or max:
      if(result == min)
      {
         guess = max;
      }
      else
      {
         guess = min;
      }
      unpack_0(f(guess), last_f0);
      delta = guess - result;
   }
   if(sign(last_f0) * sign(f0) < 0)
   {
      // we've crossed over so move in opposite direction to last step:
      if(delta < 0)
      {
         delta = (result - min) / 2;
      }
      else
      {
         delta = (result - max) / 2;
      }
   }
   else
   {
      // move in same direction as last step:
      if(delta < 0)
      {
         delta = (result - max) / 2;
      }
      else
      {
         delta = (result - min) / 2;
      }
   }
}

} // namespace

template <class F, class T, class Tol, class Policy>
std::pair<T, T> bisect(F f, T min, T max, Tol tol, boost::uintmax_t& max_iter, const Policy& pol) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<Policy>::value && BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   T fmin = f(min);
   T fmax = f(max);
   if(fmin == 0)
   {
      max_iter = 2;
      return std::make_pair(min, min);
   }
   if(fmax == 0)
   {
      max_iter = 2;
      return std::make_pair(max, max);
   }

   //
   // Error checking:
   //
   static const char* function = "boost::math::tools::bisect<%1%>";
   if(min >= max)
   {
      return boost::math::detail::pair_from_single(policies::raise_evaluation_error(function,
         "Arguments in wrong order in boost::math::tools::bisect (first arg=%1%)", min, pol));
   }
   if(fmin * fmax >= 0)
   {
      return boost::math::detail::pair_from_single(policies::raise_evaluation_error(function,
         "No change of sign in boost::math::tools::bisect, either there is no root to find, or there are multiple roots in the interval (f(min) = %1%).", fmin, pol));
   }

   //
   // Three function invocations so far:
   //
   boost::uintmax_t count = max_iter;
   if(count < 3)
      count = 0;
   else
      count -= 3;

   while(count && (0 == tol(min, max)))
   {
      T mid = (min + max) / 2;
      T fmid = f(mid);
      if((mid == max) || (mid == min))
         break;
      if(fmid == 0)
      {
         min = max = mid;
         break;
      }
      else if(sign(fmid) * sign(fmin) < 0)
      {
         max = mid;
         fmax = fmid;
      }
      else
      {
         min = mid;
         fmin = fmid;
      }
      --count;
   }

   max_iter -= count;

#ifdef BOOST_MATH_INSTRUMENT
   std::cout << "Bisection iteration, final count = " << max_iter << std::endl;

   static boost::uintmax_t max_count = 0;
   if(max_iter > max_count)
   {
      max_count = max_iter;
      std::cout << "Maximum iterations: " << max_iter << std::endl;
   }
#endif

   return std::make_pair(min, max);
}

template <class F, class T, class Tol>
inline std::pair<T, T> bisect(F f, T min, T max, Tol tol, boost::uintmax_t& max_iter)  BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value && BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return bisect(f, min, max, tol, max_iter, policies::policy<>());
}

template <class F, class T, class Tol>
inline std::pair<T, T> bisect(F f, T min, T max, Tol tol) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value && BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return bisect(f, min, max, tol, m, policies::policy<>());
}

template <class F, class T>
T newton_raphson_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   BOOST_MATH_STD_USING

   T f0(0), f1, last_f0(0);
   T result = guess;

   T factor = static_cast<T>(ldexp(1.0, 1 - digits));
   T delta = tools::max_value<T>();
   T delta1 = tools::max_value<T>();
   T delta2 = tools::max_value<T>();

   boost::uintmax_t count(max_iter);

   do{
      last_f0 = f0;
      delta2 = delta1;
      delta1 = delta;
      detail::unpack_tuple(f(result), f0, f1);
      --count;
      if(0 == f0)
         break;
      if(f1 == 0)
      {
         // Oops zero derivative!!!
#ifdef BOOST_MATH_INSTRUMENT
         std::cout << "Newton iteration, zero derivative found" << std::endl;
#endif
         detail::handle_zero_derivative(f, last_f0, f0, delta, result, guess, min, max);
      }
      else
      {
         delta = f0 / f1;
      }
#ifdef BOOST_MATH_INSTRUMENT
      std::cout << "Newton iteration, delta = " << delta << std::endl;
#endif
      if(fabs(delta * 2) > fabs(delta2))
      {
         // last two steps haven't converged.
         T shift = (delta > 0) ? (result - min) / 2 : (result - max) / 2;
         if ((result != 0) && (fabs(shift) > fabs(result)))
         {
            delta = sign(delta) * result; // protect against huge jumps!
         }
         else
            delta = shift;
         // reset delta1/2 so we don't take this branch next time round:
         delta1 = 3 * delta;
         delta2 = 3 * delta;
      }
      guess = result;
      result -= delta;
      if(result <= min)
      {
         delta = 0.5F * (guess - min);
         result = guess - delta;
         if((result == min) || (result == max))
            break;
      }
      else if(result >= max)
      {
         delta = 0.5F * (guess - max);
         result = guess - delta;
         if((result == min) || (result == max))
            break;
      }
      // update brackets:
      if(delta > 0)
         max = guess;
      else
         min = guess;
   }while(count && (fabs(result * factor) < fabs(delta)));

   max_iter -= count;

#ifdef BOOST_MATH_INSTRUMENT
   std::cout << "Newton Raphson iteration, final count = " << max_iter << std::endl;

   static boost::uintmax_t max_count = 0;
   if(max_iter > max_count)
   {
      max_count = max_iter;
      std::cout << "Maximum iterations: " << max_iter << std::endl;
   }
#endif

   return result;
}

template <class F, class T>
inline T newton_raphson_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return newton_raphson_iterate(f, guess, min, max, digits, m);
}

namespace detail{

   struct halley_step
   {
      template <class T>
      static T step(const T& /*x*/, const T& f0, const T& f1, const T& f2) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T))
      {
         using std::fabs;
         T denom = 2 * f0;
         T num = 2 * f1 - f0 * (f2 / f1);
         T delta;

         BOOST_MATH_INSTRUMENT_VARIABLE(denom);
         BOOST_MATH_INSTRUMENT_VARIABLE(num);

         if((fabs(num) < 1) && (fabs(denom) >= fabs(num) * tools::max_value<T>()))
         {
            // possible overflow, use Newton step:
            delta = f0 / f1;
         }
         else
            delta = denom / num;
         return delta;
      }
   };

   template <class Stepper, class F, class T>
   T second_order_root_finder(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
   {
      BOOST_MATH_STD_USING

         T f0(0), f1, f2;
      T result = guess;

      T factor = ldexp(static_cast<T>(1.0), 1 - digits);
      T delta = (std::max)(T(10000000 * guess), T(10000000));  // arbitarily large delta
      T last_f0 = 0;
      T delta1 = delta;
      T delta2 = delta;
      bool out_of_bounds_sentry = false;

#ifdef BOOST_MATH_INSTRUMENT
      std::cout << "Second order root iteration, limit = " << factor << std::endl;
#endif

      boost::uintmax_t count(max_iter);

      do{
         last_f0 = f0;
         delta2 = delta1;
         delta1 = delta;
         detail::unpack_tuple(f(result), f0, f1, f2);
         --count;

         BOOST_MATH_INSTRUMENT_VARIABLE(f0);
         BOOST_MATH_INSTRUMENT_VARIABLE(f1);
         BOOST_MATH_INSTRUMENT_VARIABLE(f2);

         if(0 == f0)
            break;
         if(f1 == 0)
         {
            // Oops zero derivative!!!
#ifdef BOOST_MATH_INSTRUMENT
            std::cout << "Second order root iteration, zero derivative found" << std::endl;
#endif
            detail::handle_zero_derivative(f, last_f0, f0, delta, result, guess, min, max);
         }
         else
         {
            if(f2 != 0)
            {
               delta = Stepper::step(result, f0, f1, f2);
               if(delta * f1 / f0 < 0)
               {
                  // Oh dear, we have a problem as Newton and Halley steps
                  // disagree about which way we should move.  Probably
                  // there is cancelation error in the calculation of the
                  // Halley step, or else the derivatives are so small
                  // that their values are basically trash.  We will move
                  // in the direction indicated by a Newton step, but
                  // by no more than twice the current guess value, otherwise
                  // we can jump way out of bounds if we're not careful.
                  // See https://svn.boost.org/trac/boost/ticket/8314.
                  delta = f0 / f1;
                  if(fabs(delta) > 2 * fabs(guess))
                     delta = (delta < 0 ? -1 : 1) * 2 * fabs(guess);
               }
            }
            else
               delta = f0 / f1;
         }
#ifdef BOOST_MATH_INSTRUMENT
         std::cout << "Second order root iteration, delta = " << delta << std::endl;
#endif
         T convergence = fabs(delta / delta2);
         if((convergence > 0.8) && (convergence < 2))
         {
            // last two steps haven't converged.
            delta = (delta > 0) ? (result - min) / 2 : (result - max) / 2;
            if ((result != 0) && (fabs(delta) > result))
               delta = sign(delta) * result; // protect against huge jumps!
            // reset delta2 so that this branch will *not* be taken on the
            // next iteration:
            delta2 = delta * 3;
            delta1 = delta * 3;
            BOOST_MATH_INSTRUMENT_VARIABLE(delta);
         }
         guess = result;
         result -= delta;
         BOOST_MATH_INSTRUMENT_VARIABLE(result);

         // check for out of bounds step:
         if(result < min)
         {
            T diff = ((fabs(min) < 1) && (fabs(result) > 1) && (tools::max_value<T>() / fabs(result) < fabs(min)))
               ? T(1000)
               : (fabs(min) < 1) && (fabs(tools::max_value<T>() * min) < fabs(result))
               ? ((min < 0) != (result < 0)) ? -tools::max_value<T>() : tools::max_value<T>() : T(result / min);
            if(fabs(diff) < 1)
               diff = 1 / diff;
            if(!out_of_bounds_sentry && (diff > 0) && (diff < 3))
            {
               // Only a small out of bounds step, lets assume that the result
               // is probably approximately at min:
               delta = 0.99f * (guess - min);
               result = guess - delta;
               out_of_bounds_sentry = true; // only take this branch once!
            }
            else
            {
               delta = (guess - min) / 2;
               result = guess - delta;
               if((result == min) || (result == max))
                  break;
            }
         }
         else if(result > max)
         {
            T diff = ((fabs(max) < 1) && (fabs(result) > 1) && (tools::max_value<T>() / fabs(result) < fabs(max))) ? T(1000) : T(result / max);
            if(fabs(diff) < 1)
               diff = 1 / diff;
            if(!out_of_bounds_sentry && (diff > 0) && (diff < 3))
            {
               // Only a small out of bounds step, lets assume that the result
               // is probably approximately at min:
               delta = 0.99f * (guess - max);
               result = guess - delta;
               out_of_bounds_sentry = true; // only take this branch once!
            }
            else
            {
               delta = (guess - max) / 2;
               result = guess - delta;
               if((result == min) || (result == max))
                  break;
            }
         }
         // update brackets:
         if(delta > 0)
            max = guess;
         else
            min = guess;
      } while(count && (fabs(result * factor) < fabs(delta)));

      max_iter -= count;

#ifdef BOOST_MATH_INSTRUMENT
      std::cout << "Second order root iteration, final count = " << max_iter << std::endl;
#endif

      return result;
   }

}

template <class F, class T>
T halley_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return detail::second_order_root_finder<detail::halley_step>(f, guess, min, max, digits, max_iter);
}

template <class F, class T>
inline T halley_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return halley_iterate(f, guess, min, max, digits, m);
}

namespace detail{

   struct schroder_stepper
   {
      template <class T>
      static T step(const T& x, const T& f0, const T& f1, const T& f2) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T))
      {
         using std::fabs;
         T ratio = f0 / f1;
         T delta;
         if((x != 0) && (fabs(ratio / x) < 0.1))
         {
            delta = ratio + (f2 / (2 * f1)) * ratio * ratio;
            // check second derivative doesn't over compensate:
            if(delta * ratio < 0)
               delta = ratio;
         }
         else
            delta = ratio;  // fall back to Newton iteration.
         return delta;
      }
   };

}

template <class F, class T>
T schroder_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return detail::second_order_root_finder<detail::schroder_stepper>(f, guess, min, max, digits, max_iter);
}

template <class F, class T>
inline T schroder_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return schroder_iterate(f, guess, min, max, digits, m);
}
//
// These two are the old spelling of this function, retained for backwards compatibity just in case:
//
template <class F, class T>
T schroeder_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return detail::second_order_root_finder<detail::schroder_stepper>(f, guess, min, max, digits, max_iter);
}

template <class F, class T>
inline T schroeder_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return schroder_iterate(f, guess, min, max, digits, m);
}

#ifndef BOOST_NO_CXX11_AUTO_DECLARATIONS
/*
 * Why do we set the default maximum number of iterations to the number of digits in the type?
 * Because for double roots, the number of digits increases linearly with the number of iterations,
 * so this default should recover full precision even in this somewhat pathological case.
 * For isolated roots, the problem is so rapidly convergent that this doesn't matter at all.
 */
template<class Complex, class F>
Complex complex_newton(F g, Complex guess, int max_iterations=std::numeric_limits<typename Complex::value_type>::digits)
{
    typedef typename Complex::value_type Real;
    using std::norm;
    using std::abs;
    using std::max;
    // z0, z1, and z2 cannot be the same, in case we immediately need to resort to Muller's Method:
    Complex z0 = guess + Complex(1,0);
    Complex z1 = guess + Complex(0,1);
    Complex z2 = guess;

    do {
       auto pair = g(z2);
       if (norm(pair.second) == 0)
       {
           // Muller's method. Notation follows Numerical Recipes, 9.5.2:
           Complex q = (z2 - z1)/(z1 - z0);
           auto P0 = g(z0);
           auto P1 = g(z1);
           Complex qp1 = static_cast<Complex>(1)+q;
           Complex A = q*(pair.first - qp1*P1.first + q*P0.first);

           Complex B = (static_cast<Complex>(2)*q+static_cast<Complex>(1))*pair.first - qp1*qp1*P1.first +q*q*P0.first;
           Complex C = qp1*pair.first;
           Complex rad = sqrt(B*B - static_cast<Complex>(4)*A*C);
           Complex denom1 = B + rad;
           Complex denom2 = B - rad;
           Complex correction = (z1-z2)*static_cast<Complex>(2)*C;
           if (norm(denom1) > norm(denom2))
           {
               correction /= denom1;
           }
           else
           {
               correction /= denom2;
           }

           z0 = z1;
           z1 = z2;
           z2 = z2 + correction;
       }
       else
       {
           z0 = z1;
           z1 = z2;
           z2 = z2  - (pair.first/pair.second);
       }

       // See: https://math.stackexchange.com/questions/3017766/constructing-newton-iteration-converging-to-non-root
       // If f' is continuous, then convergence of x_n -> x* implies f(x*) = 0.
       // This condition approximates this convergence condition by requiring three consecutive iterates to be clustered.
       Real tol = max(abs(z2)*std::numeric_limits<Real>::epsilon(), std::numeric_limits<Real>::epsilon());
       bool real_close = abs(z0.real() - z1.real()) < tol && abs(z0.real() - z2.real()) < tol && abs(z1.real() - z2.real()) < tol;
       bool imag_close = abs(z0.imag() - z1.imag()) < tol && abs(z0.imag() - z2.imag()) < tol && abs(z1.imag() - z2.imag()) < tol;
       if (real_close && imag_close)
       {
           return z2;
       }

   } while(max_iterations--);

    // The idea is that if we can get abs(f) < eps, we should, but if we go through all these iterations
    // and abs(f) < sqrt(eps), then roundoff error simply does not allow that we can evaluate f to < eps
    // This is somewhat awkward as it isn't scale invariant, but using the Daubechies coefficient example code,
    // I found this condition generates correct roots, whereas the scale invariant condition discussed here:
    // https://scicomp.stackexchange.com/questions/30597/defining-a-condition-number-and-termination-criteria-for-newtons-method
    // allows nonroots to be passed off as roots.
    auto pair = g(z2);
    if (abs(pair.first) < sqrt(std::numeric_limits<Real>::epsilon()))
    {
        return z2;
    }

    return {std::numeric_limits<Real>::quiet_NaN(),
            std::numeric_limits<Real>::quiet_NaN()};
}
#endif

#if !defined(BOOST_NO_CXX17_IF_CONSTEXPR)
// https://stackoverflow.com/questions/48979861/numerically-stable-method-for-solving-quadratic-equations/50065711
namespace detail
{
    template<class T>
    inline T discriminant(T const & a, T const & b, T const & c)
    {
        T w = 4*a*c;
        T e = std::fma(-c, 4*a, w);
        T f = std::fma(b, b, -w);
        return f + e;
    }
}

template<class T>
auto quadratic_roots(T const& a, T const& b, T const& c)
{
    using std::copysign;
    using std::sqrt;
    if constexpr (std::is_integral<T>::value)
    {
        // What I want is to write:
        // return quadratic_roots(double(a), double(b), double(c));
        // but that doesn't compile.
        double nan = std::numeric_limits<double>::quiet_NaN();
        if(a==0)
        {
            if (b==0 && c != 0)
            {
                return std::pair<double, double>(nan, nan);
            }
            else if (b==0 && c==0)
            {
                return std::pair<double, double>(0,0);
            }
            return std::pair<double, double>(-c/b, -c/b);
        }
        if (b==0)
        {
            double x0_sq = -double(c)/double(a);
            if (x0_sq < 0) {
                return std::pair<double, double>(nan, nan);
            }
            double x0 = sqrt(x0_sq);
            return std::pair<double, double>(-x0,x0);
        }
        double discriminant = detail::discriminant(double(a), double(b), double(c));
        if (discriminant < 0)
        {
            return std::pair<double, double>(nan, nan);
        }
        double q = -(b + copysign(sqrt(discriminant), double(b)))/T(2);
        double x0 = q/a;
        double x1 = c/q;
        if (x0 < x1) {
            return std::pair<double, double>(x0, x1);
        }
        return std::pair<double, double>(x1, x0);
    }
    else if constexpr (std::is_floating_point<T>::value)
    {
        T nan = std::numeric_limits<T>::quiet_NaN();
        if(a==0)
        {
            if (b==0 && c != 0)
            {
                return std::pair<T, T>(nan, nan);
            }
            else if (b==0 && c==0)
            {
                return std::pair<T, T>(0,0);
            }
            return std::pair<T, T>(-c/b, -c/b);
        }
        if (b==0)
        {
            T x0_sq = -c/a;
            if (x0_sq < 0) {
                return std::pair<T, T>(nan, nan);
            }
            T x0 = sqrt(x0_sq);
            return std::pair<T, T>(-x0,x0);
        }
        T discriminant = detail::discriminant(a, b, c);
        // Is there a sane way to flush very small negative values to zero?
        // If there is I don't know of it.
        if (discriminant < 0)
        {
            return std::pair<T, T>(nan, nan);
        }
        T q = -(b + copysign(sqrt(discriminant), b))/T(2);
        T x0 = q/a;
        T x1 = c/q;
        if (x0 < x1)
        {
            return std::pair<T, T>(x0, x1);
        }
        return std::pair<T, T>(x1, x0);
    }
    else if constexpr (boost::is_complex<T>::value || boost::multiprecision::number_category<T>::value == boost::multiprecision::number_kind_complex)
    {
        typename T::value_type nan = std::numeric_limits<typename T::value_type>::quiet_NaN();
        if(a.real()==0 && a.imag() ==0)
        {
            using std::norm;
            if (b.real()==0 && b.imag() && norm(c) != 0)
            {
                return std::pair<T, T>({nan, nan}, {nan, nan});
            }
            else if (b.real()==0 && b.imag() && c.real() ==0 && c.imag() == 0)
            {
                return std::pair<T, T>({0,0},{0,0});
            }
            return std::pair<T, T>(-c/b, -c/b);
        }
        if (b.real()==0 && b.imag() == 0)
        {
            T x0_sq = -c/a;
            T x0 = sqrt(x0_sq);
            return std::pair<T, T>(-x0, x0);
        }
        // There's no fma for complex types:
        T discriminant = b*b - T(4)*a*c;
        T q = -(b + sqrt(discriminant))/T(2);
        return std::pair<T, T>(q/a, c/q);
    }
    else // Most likely the type is a boost.multiprecision.
    {    //There is no fma for multiprecision, and in addition it doesn't seem to be useful, so revert to the naive computation.
        T nan = std::numeric_limits<T>::quiet_NaN();
        if(a==0)
        {
            if (b==0 && c != 0)
            {
                return std::pair<T, T>(nan, nan);
            }
            else if (b==0 && c==0)
            {
                return std::pair<T, T>(0,0);
            }
            return std::pair<T, T>(-c/b, -c/b);
        }
        if (b==0)
        {
            T x0_sq = -c/a;
            if (x0_sq < 0) {
                return std::pair<T, T>(nan, nan);
            }
            T x0 = sqrt(x0_sq);
            return std::pair<T, T>(-x0,x0);
        }
        T discriminant = b*b - 4*a*c;
        if (discriminant < 0)
        {
            return std::pair<T, T>(nan, nan);
        }
        T q = -(b + copysign(sqrt(discriminant), b))/T(2);
        T x0 = q/a;
        T x1 = c/q;
        if (x0 < x1)
        {
            return std::pair<T, T>(x0, x1);
        }
        return std::pair<T, T>(x1, x0);
    }
}
#endif

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_NEWTON_SOLVER_HPP


/**
 * @file layer_traits_test.cpp
 * @author Marcus Edel
 *
 * Test the LayerTraits class. Because all of the values are known at compile
 * time, this test is meant to ensure that uses of LayerTraits still compile
 * okay and react as expected.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/layer_traits.hpp>
#include <mlpack/methods/ann/layer/bias_layer.hpp>
#include <mlpack/methods/ann/layer/multiclass_classification_layer.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(LayerTraitsTest);

// Be careful!  When writing new tests, always get the boolean value and store
// it in a temporary, because the Boost unit test macros do weird things and
// will cause bizarre problems.

// Test the defaults.
BOOST_AUTO_TEST_CASE(DefaultsTraitsTest)
{
  // An irrelevant non-connection type class is used here so that the default
  // implementation of ConnectionTraits is chosen.
  bool b = LayerTraits<int>::IsBinary;
  BOOST_REQUIRE_EQUAL(b, false);

  b =  LayerTraits<int>::IsOutputLayer;
  BOOST_REQUIRE_EQUAL(b, false);

  b =  LayerTraits<int>::IsBiasLayer;
  BOOST_REQUIRE_EQUAL(b, false);
}

// Test the BiasLayer traits.
BOOST_AUTO_TEST_CASE(BiasLayerTraitsTest)
{
  bool b = LayerTraits<BiasLayer<> >::IsBinary;
  BOOST_REQUIRE_EQUAL(b, false);

  b = LayerTraits<BiasLayer<> >::IsOutputLayer;
  BOOST_REQUIRE_EQUAL(b, false);

  b = LayerTraits<BiasLayer<> >::IsBiasLayer;
  BOOST_REQUIRE_EQUAL(b, true);
}

// Test the MulticlassClassificationLayer traits.
BOOST_AUTO_TEST_CASE(MulticlassClassificationLayerTraitsTest)
{
  bool b = LayerTraits<MulticlassClassificationLayer<> >::IsBinary;
  BOOST_REQUIRE_EQUAL(b, false);

  b = LayerTraits<MulticlassClassificationLayer<> >::IsOutputLayer;
  BOOST_REQUIRE_EQUAL(b, true);

  b = LayerTraits<MulticlassClassificationLayer<> >::IsBiasLayer;
  BOOST_REQUIRE_EQUAL(b, false);
}

BOOST_AUTO_TEST_SUITE_END();


// Example originates from the boost.context repo:
// https://github.com/boostorg/context/blob/develop/example/callcc/fibonacci.cpp

#include <cstdlib>
#include <iostream>
#include <memory>

#include <boost/context/continuation.hpp>

namespace ctx = boost::context;

int main() {
    int a;
    ctx::continuation c=ctx::callcc(
        [&a](ctx::continuation && c){
            a=0;
            int b=1;
            for(;;){
                c=c.resume();
                int next=a+b;
                a=b;
                b=next;
            }
            return std::move( c);
        });
    for ( int j = 0; j < 10; ++j) {
        std::cout << a << " ";
        c=c.resume();
    }
    std::cout << std::endl;
    std::cout << "main: done" << std::endl;
    return EXIT_SUCCESS;
}


#pragma once

#include <string>
#include <boost/numeric/conversion/cast.hpp>
#include <QString>
#include <QTextStream>
#include <QFile>
#include <SAMSON.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <ADNConfig.hpp>

namespace ublas = boost::numeric::ublas;

using Position3D = SBPosition3;

namespace ADNAuxiliary {
  using namespace ublas;

	enum SelectionFlag {
		Unselected = 0,
		Selected = 1
	};

	enum VisibilityFlag {
		Invisible = 0,
		Visible = 1
	};

  enum SortingType {
    StrandID = 0,
    Length = 1,
    GCCont = 2,
    MeltTemp = 3,
    Gibbs = 4
  };

  enum OverlayType {
    NoOverlay = 0,
    Crossovers = 1,
    PossibleCrossovers = 2
  };

  enum ScaffoldSeq {
    m13mp18 = 0,
    p7249 = 1,
    Custom = 2,
  };

  struct SphericalCoord {
    double phi;
    double theta;
    double r;
  };

  enum OxDNAInteractionType {
    DNA = 0,
    DNA2 = 1,
    Patchy = 2,
    LJ = 3,
  };

  enum OxDNASimulationType {
    MD = 0,
    MC = 1,
    VMMC = 2,
  };

  enum OxDNABackend {
    CPU = 0,
    CUDA = 1,
  };

  struct OxDNAOptions {
    OxDNAInteractionType interactionType_;
    OxDNASimulationType simType_;
    OxDNABackend backend_;
    bool debug_;
    unsigned int steps_;
    std::string temp_;
    double saltConcentration_ = 0.0;
    double mdTimeStep_ = 0.0;
    double mcDeltaTranslation_ = 0.0;
    double mcDeltaRotation_ = 0.0;
    double boxSizeX_ = 0.0;  // in nm
    double boxSizeY_ = 0.0;
    double boxSizeZ_ = 0.0;
  };

	double mapRange(double input, double input_start, double input_end, double output_start, double output_end);
	//char getBaseSymbol(SBNode* node);
	char getBaseSymbol(std::string name);
	char getBaseSymbol(size_t numAtoms);
  void getHeatMapColor(double val, double min, double max, int* color);

  std::string UblasVectorToString(ublas::vector<double> vec);
  std::string VectorToString(std::vector<int> vec);
  std::string SBPositionToString(Position3D pos);
  std::string SBVector3ToString(SBVector3 vec);

  ublas::vector<double> StringToUblasVector(std::string vec);
  Position3D StringToSBPosition(std::string vec);
  SBVector3 StringToSBVector(std::string vec);
  std::vector<int> StringToVector(std::string vec);

  Position3D UblasVectorToSBPosition(ublas::vector<double> pos);
  SBVector3 UblasVectorToSBVector(ublas::vector<double> pos);
  ublas::vector<double> SBVectorToUblasVector(SBVector3 v);
  ublas::vector<double> SBPositionToUblas(Position3D pos);
  std::vector<double> SBPositionToVector(Position3D pos);

  /*! Converts a 3D std::vector<double> to SBPosition3
      (in nanometers)
  */
  SBPosition3 VectorToSBPosition(std::vector<double> v);
  /*! Converts a 3D std::vector<double> to SBVector3
  */
  SBVector3 VectorToSBVector(std::vector<double> v);

  bool ValidateSequence(std::string seq);

  QString AdenitaCitation();
};


#include <crs_perception/model_to_point_cloud.hpp>

#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/common/centroid.h>
#include <pcl/common/time.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/conversions.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <pcl_conversions/pcl_conversions.h>

#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <crs_msgs/srv/load_part.hpp>
#include <crs_msgs/srv/localize_to_part.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <rclcpp/rclcpp.hpp>

#include <boost/filesystem.hpp>
#include <boost/format.hpp>

#include <rclcpp_components/register_node_macro.hpp>

//#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
//#define EIGEN_DONT_VECTORIZE

static const double VIEW_POINT_Z = 1000.0;
static const std::string CROP_BOXES_PARAM_PREFIX = "crop_boxes.box";
static const std::string CROP_BOXES_MARKER_NS = "crop_boxes";

struct IcpConfig
{
  bool use_cog_alignment = false;
  bool use_correspondences = false;
  double max_correspondence_dist = 0.01;
  int max_iter = 200;
  double transformation_eps = 1e-2;
  double rotation_eps = 1e-6;
  double euclidean_fitness = 1.0;
  double ransac_threshold = 0.008;
};

struct SACAlignConfig
{
  double normal_est_rad = 0.01;
  double feature_est_rad = 0.025;
  double max_iters = 50000;
  int num_samples = 3;
  int correspondence_rand = 5;
  double similarity_threshold = 0.9;
  double max_correspondence_dist = 0.5;
  double inlier_fraction = 0.25;
};

struct CropBoxConfig
{
  std::vector<double> xyz;
  std::vector<double> size;
  bool reverse;
};

using Cloud = pcl::PointCloud<pcl::PointXYZ>;

visualization_msgs::msg::MarkerArray createMarkers(const std::vector<CropBoxConfig>& configs,
                                                   const std::string& frame_id,
                                                   const std::string& ns)
{
  visualization_msgs::msg::MarkerArray markers;
  for (std::size_t i = 0; i < configs.size(); i++)
  {
    auto& cfg = configs[i];
    Eigen::Isometry3d pose =
        Eigen::Translation3d(Eigen::Vector3d(cfg.xyz[0], cfg.xyz[1], cfg.xyz[2])) * Eigen::Isometry3d::Identity();

    visualization_msgs::msg::Marker marker;
    marker.header.frame_id = frame_id;
    marker.ns = ns;
    marker.id = i;
    marker.type = visualization_msgs::msg::Marker::CUBE;
    marker.action = visualization_msgs::msg::Marker::ADD;
    marker.pose.position.x = pose.translation().x();
    marker.pose.position.y = pose.translation().y();
    marker.pose.position.z = pose.translation().z();
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    marker.scale.x = cfg.size[0];
    marker.scale.y = cfg.size[1];
    marker.scale.z = cfg.size[2];

    // setting color
    marker.color.a = 0.1;
    if (cfg.reverse)
    {
      marker.color.r = 1.0;
      marker.color.g = 1.0;
      marker.color.b = 0.0;
    }
    else
    {
      marker.color.r = 0.0;
      marker.color.g = 1.0;
      marker.color.b = 0.0;
    }

    markers.markers.push_back(marker);
  }

  return markers;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cropBox(const CropBoxConfig& cfg, pcl::PointCloud<pcl::PointXYZ>::Ptr input)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr output = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

  pcl::CropBox<pcl::PointXYZ> box_filter;
  double x_min = cfg.xyz[0] - 0.5 * cfg.size[0];
  double y_min = cfg.xyz[1] - 0.5 * cfg.size[1];
  double z_min = cfg.xyz[2] - 0.5 * cfg.size[2];

  double x_max = cfg.xyz[0] + 0.5 * cfg.size[0];
  double y_max = cfg.xyz[1] + 0.5 * cfg.size[1];
  double z_max = cfg.xyz[2] + 0.5 * cfg.size[2];

  box_filter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0));
  box_filter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0));
  box_filter.setNegative(!cfg.reverse);
  box_filter.setInputCloud(input);
  box_filter.filter(*output);
  return output;
}

pcl::PointCloud<pcl::PointXYZ> downsampleCloud(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud, double leaf_size)
{
  using namespace pcl;
  pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
  voxelgrid.setInputCloud(cloud);
  voxelgrid.setLeafSize(leaf_size, leaf_size, leaf_size);
  pcl::PointCloud<pcl::PointXYZ> out;  // = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
  voxelgrid.filter(out);
  return out;
}

namespace crs_perception
{
class LocalizeToPart : public rclcpp::Node
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  LocalizeToPart(const rclcpp::NodeOptions& options)
    : Node("localize_to_part", options)
    , part_loaded_(false)
    , world_frame_("world")
    , clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME))
    , tf_buffer_(clock_)
    , tf_listener_(tf_buffer_)
  {
    using namespace std::chrono_literals;

    part_point_cloud_ = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

    if (!loadParameters())
    {
      throw std::runtime_error("Failed to load parameters");
    }

    // parameter updates subscription
    static const std::string PARAMETER_EVENTS_TOPIC = "parameter_events";
    parameter_event_subs_ = this->create_subscription<rcl_interfaces::msg::ParameterEvent>(
        PARAMETER_EVENTS_TOPIC, rclcpp::QoS(1), [this](const rcl_interfaces::msg::ParameterEvent::SharedPtr) {
          loadParameters();
        });

    // services
    load_part_service_ = this->create_service<crs_msgs::srv::LoadPart>("load_part",
                                                                       std::bind(&LocalizeToPart::handleLoadPart,
                                                                                this,
                                                                                std::placeholders::_1,
                                                                                std::placeholders::_2,
                                                                                std::placeholders::_3));
    localize_to_part_service_ =
        this->create_service<crs_msgs::srv::LocalizeToPart>("localize_to_part",
                                                            std::bind(&LocalizeToPart::handleLocalizeToPart,
                                                                      this,
                                                                      std::placeholders::_1,
                                                                      std::placeholders::_2,
                                                                      std::placeholders::_3));

    // debug visualization publishers
    if (enable_debug_visualizations_)
    {
      // todo(ayoungs: investigate if this behaves like a latched topic
      loaded_part_pc_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("loaded_part_point_cloud",
                                                                                rclcpp::QoS(1).transient_local());
      tf_loaded_part_pc_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("tf_loaded_part_point_cloud",
                                                                                rclcpp::QoS(1).transient_local());
      combined_pc_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("combined_scanned_point_clouds",
                                                                               rclcpp::QoS(1).transient_local());

      markers_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(CROP_BOXES_MARKER_NS, 1);
      markers_timer_ = this->create_wall_timer(500ms, [this]() -> void { markers_pub_->publish(cropbox_markers_); });
    }
  }

private:
  bool loadParameters()
  {
    // general parameters
    mesh_num_samples_ = this->get_parameter("general.mesh_num_samples").as_int();
    leaf_size_ = this->get_parameter("general.leaf_size").as_double();
    part_leaf_size_ = this->get_parameter("general.part_leaf_size").as_double();
    part_frame_ = this->get_parameter("general.part_frame").as_string();
    enable_debug_visualizations_ = this->get_parameter("general.enable_debug_visualizations").as_bool();

    // icp parameters
    std::map<std::string, rclcpp::Parameter> params;
    if (this->get_parameters("icp", params))
    {
      icp_config_.use_cog_alignment = params["use_cog_alignment"].as_bool();
      icp_config_.euclidean_fitness = params["euclidean_fitness"].as_double();
      icp_config_.max_correspondence_dist = params["max_correspondence_dist"].as_double();
      icp_config_.max_iter = params["max_iter"].as_int();
      icp_config_.transformation_eps = params["euclidean_fitness"].as_double();
      icp_config_.rotation_eps = params["rotation_eps"].as_double();
      icp_config_.ransac_threshold = params["ransac_threshold"].as_double();
      icp_config_.use_correspondences = params["use_correspondences"].as_bool();
      RCLCPP_INFO_STREAM(this->get_logger(), "Loaded icp parameters");
    }
    else
    {
      std::string err_msg = "Failed to find icp parameters";
      RCLCPP_ERROR_STREAM(this->get_logger(), err_msg);
      return false;
    }

    // sac parameters
    params.clear();
    if (this->get_parameters("sac", params))
    {
      sac_align_config_.normal_est_rad = params["normal_est_rad"].as_double();
      sac_align_config_.feature_est_rad = params["feature_est_rad"].as_double();
      sac_align_config_.max_iters = params["max_iters"].as_int();
      sac_align_config_.num_samples = params["num_samples"].as_int();
      sac_align_config_.correspondence_rand = params["correspondence_rand"].as_int();
      sac_align_config_.similarity_threshold = params["similarity_threshold"].as_double();
      sac_align_config_.max_correspondence_dist = params["max_correspondence_dist"].as_double();
      sac_align_config_.inlier_fraction = params["inlier_fraction"].as_double();
      RCLCPP_INFO_STREAM(this->get_logger(), "Loaded sac parameters");
    }
    else
    {
      std::string err_msg = "Failed to find sac parameters";
      RCLCPP_ERROR_STREAM(this->get_logger(), err_msg);
      return false;
    }

    // crop boxes
    std::size_t box_counter = 1;
    std::map<std::string, rclcpp::Parameter> crop_box_params;
    std::string cropbox_param_ns = CROP_BOXES_PARAM_PREFIX + std::to_string(box_counter);
    while (this->get_parameters(cropbox_param_ns, crop_box_params))
    {
      CropBoxConfig cb_config;
      cb_config.xyz = crop_box_params["xyz"].as_double_array();
      cb_config.size = crop_box_params["size"].as_double_array();
      cb_config.reverse = crop_box_params["reverse"].as_bool();
      crop_boxes_.push_back(cb_config);
      RCLCPP_INFO(this->get_logger(), "Added cropbox %i", box_counter++);
      cropbox_param_ns = CROP_BOXES_PARAM_PREFIX + std::to_string(box_counter);
      crop_box_params.clear();
    }

    if (crop_boxes_.empty())
    {
      std::string err_msg =
          boost::str(boost::format("Failed to find crop boxes under namespace %s") % CROP_BOXES_PARAM_PREFIX);
      RCLCPP_ERROR_STREAM(this->get_logger(), err_msg);
      return false;
    }

    cropbox_markers_ = createMarkers(crop_boxes_, world_frame_, CROP_BOXES_MARKER_NS);

    return true;
  }

  void handleLoadPart(const std::shared_ptr<rmw_request_id_t> request_header,
                      const std::shared_ptr<crs_msgs::srv::LoadPart::Request> request,
                      const std::shared_ptr<crs_msgs::srv::LoadPart::Response> response)
  {
    namespace fs = boost::filesystem;
    using namespace std::chrono_literals;

    (void)request_header;

    part_loaded_ = false;
    response->success = false;
    if (!fs::exists(fs::path(request->path_to_part)))
    {
      response->error = boost::str(boost::format("Part file %s does not exists") % request->path_to_part);
      RCLCPP_ERROR_STREAM(this->get_logger(), response->error);
      return;
    }

    RCLCPP_INFO(this->get_logger(), "Loading part from %s", request->path_to_part.c_str());

    // converting mesh into cloud
    ModelToPointCloud mtpc(mesh_num_samples_, part_leaf_size_);
    if (!mtpc.convertToPCL(request->path_to_part, part_point_cloud_, response->error))
    {
      RCLCPP_ERROR_STREAM(this->get_logger(), response->error);
      return;
    }

    // transforming point cloud
    Eigen::Isometry3d part_seed_transform_ = Eigen::Isometry3d::Identity();
    part_seed_msg_ = request->part_origin;
    tf2::fromMsg(part_seed_msg_, part_seed_transform_);
    pcl::PointCloud<pcl::PointXYZ>::Ptr original_cloud = part_point_cloud_->makeShared();
    pcl::transformPointCloud(*original_cloud, *part_point_cloud_, part_seed_transform_.matrix());
    part_loaded_ = true;
    response->success = true;

    if (enable_debug_visualizations_)
    {
      publish_timer_.reset();

      sensor_msgs::msg::PointCloud2 point_cloud;
      pcl::toROSMsg(*part_point_cloud_, point_cloud);
      point_cloud.header.stamp = this->now();
      point_cloud.header.frame_id = world_frame_;

      sensor_msgs::msg::PointCloud2 empty_cloud;
      empty_cloud.header.stamp = this->now();
      empty_cloud.header.frame_id = world_frame_;

      publish_timer_ = this->create_wall_timer(500ms, [this, point_cloud, empty_cloud]() -> void {
        loaded_part_pc_pub_->publish(point_cloud);
        combined_pc_pub_->publish(empty_cloud);
      });
    }
  }

  Eigen::Isometry3d findTransform(const pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud,
                                  const pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud,
                                  const bool pos_only = true)
  {
    using namespace Eigen;
    auto compute_centroid_transform = [](const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) -> Eigen::Isometry3d {
      Eigen::Vector3f center, x_axis, y_axis, z_axis;
      pcl::MomentOfInertiaEstimation<pcl::PointXYZ> moi;
      moi.setInputCloud(cloud);
      moi.compute();
      moi.getEigenVectors(x_axis, y_axis, z_axis);
      moi.getMassCenter(center);

      Eigen::Isometry3d transform;
      transform.setIdentity();
      transform.translation() = center.cast<double>();
      transform.linear().col(0) = x_axis.normalized().cast<double>();
      transform.linear().col(1) = y_axis.normalized().cast<double>();
      transform.linear().col(2) = z_axis.normalized().cast<double>();
      return transform;
    };

    Eigen::Isometry3d src_transform = compute_centroid_transform(src_cloud);
    Eigen::Isometry3d target_transform = compute_centroid_transform(target_cloud);
    Eigen::Isometry3d pose = target_transform * src_transform.inverse();
    if (pos_only)
    {
      pose.linear() = Quaterniond::Identity().toRotationMatrix().matrix();
    }

    Eigen::Vector3d angles = pose.linear().eulerAngles(0, 1, 2);
    RCLCPP_INFO_STREAM(this->get_logger(), "Euler angles: " << angles.transpose());
    return pose;
  }

  bool alignIcpNormals(Cloud::Ptr target_cloud, Cloud::Ptr src_cloud, Eigen::Isometry3d& transform)
  {
    using PointNT = pcl::PointNormal;
    using CloudN = pcl::PointCloud<PointNT>;

    // initial alignment
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    Eigen::Isometry3d init_transform = findTransform(src_cloud, target_cloud, false);
    pcl::transformPointCloud(*src_cloud, *temp_cloud, init_transform.cast<float>());
    *src_cloud = *temp_cloud;

    CloudN::Ptr src_normals = boost::make_shared<CloudN>();
    CloudN::Ptr target_normals = boost::make_shared<CloudN>();

    // copy cloud data
    pcl::copyPointCloud(*src_cloud, *src_normals);
    pcl::copyPointCloud(*target_cloud, *target_normals);

    // Estimate normals
    pcl::console::print_highlight("Estimating scene normals...\n");
    pcl::NormalEstimationOMP<PointNT, PointNT> nest;
    nest.setViewPoint(0.0, 0.0, VIEW_POINT_Z);
    nest.setRadiusSearch(sac_align_config_.normal_est_rad);
    nest.setInputCloud(src_normals);
    nest.compute(*src_normals);
    nest.setInputCloud(target_normals);
    nest.compute(*target_normals);

    // icp
    pcl::IterativeClosestPointWithNormals<PointNT, PointNT> icp;
    icp.setUseReciprocalCorrespondences(icp_config_.use_correspondences);
    icp.setMaxCorrespondenceDistance(icp_config_.max_correspondence_dist);
    icp.setMaximumIterations(icp_config_.max_iter);
    icp.setTransformationEpsilon(icp_config_.transformation_eps);
    // TODO: Uncomment this after migrating to pcl 1.9.1
    // //icp.setTransformationRotationEpsilon(icp_config_.rotation_eps);
    icp.setEuclideanFitnessEpsilon(icp_config_.euclidean_fitness);
    icp.setRANSACOutlierRejectionThreshold(icp_config_.ransac_threshold);
    icp.setInputSource(src_normals);
    icp.setInputTarget(target_normals);
    pcl::PointCloud<PointNT> final;
    icp.align(final);

    if (!icp.hasConverged())
    {
      return false;
    }

    transform.matrix() = icp.getFinalTransformation().cast<double>();
    transform = init_transform * transform;
    return true;
  }

  bool alignIcp(Cloud::Ptr target_cloud, Cloud::Ptr src_cloud, Eigen::Isometry3d& transform)
  {
    Eigen::Isometry3d init_transform = Eigen::Isometry3d::Identity();
    if (icp_config_.use_cog_alignment)
    {
      // initial alignment
      pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
      init_transform = findTransform(src_cloud, target_cloud, false);
      pcl::transformPointCloud(*src_cloud, *temp_cloud, init_transform.cast<float>());
      *src_cloud = *temp_cloud;
    }

    // icp
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setUseReciprocalCorrespondences(icp_config_.use_correspondences);
    icp.setMaxCorrespondenceDistance(icp_config_.max_correspondence_dist);
    icp.setMaximumIterations(icp_config_.max_iter);
    icp.setTransformationEpsilon(icp_config_.transformation_eps);
    // TODO: Uncomment this after migrating to pcl 1.9.1
    // icp.setTransformationRotationEpsilon(icp_config_.rotation_eps);
    icp.setEuclideanFitnessEpsilon(icp_config_.euclidean_fitness);
    icp.setRANSACOutlierRejectionThreshold(icp_config_.ransac_threshold);
    icp.setInputSource(src_cloud);
    icp.setInputTarget(target_cloud);
    pcl::PointCloud<pcl::PointXYZ> final;
    icp.align(final);

    if (!icp.hasConverged())
    {
      return false;
    }

    transform.matrix() = icp.getFinalTransformation().cast<double>();
    transform = init_transform * transform;
    return true;
  }

  /**
   * @brief aligs a point cloud using the SampleConsensusPrerejective algorithm, see
   * https://pcl-tutorials.readthedocs.io/en/latest/alignment_prerejective.html#alignment-prerejective
   */
  bool alignSac(Cloud::Ptr target_cloud, Cloud::Ptr src_cloud, Eigen::Isometry3d& transform)
  {
    using PointNT = pcl::PointNormal;
    using CloudN = pcl::PointCloud<PointNT>;
    using FeatureT = pcl::FPFHSignature33;
    using FeatureCloudT = pcl::PointCloud<FeatureT>;
    using FeatureEstimationT = pcl::FPFHEstimationOMP<PointNT, PointNT, FeatureT>;

    CloudN::Ptr object = boost::make_shared<CloudN>();
    CloudN::Ptr object_aligned = boost::make_shared<CloudN>();
    CloudN::Ptr scene = boost::make_shared<CloudN>();
    FeatureCloudT::Ptr object_features = boost::make_shared<FeatureCloudT>();
    FeatureCloudT::Ptr scene_features = boost::make_shared<FeatureCloudT>();

    // initial alignment
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    Eigen::Isometry3d init_transform = findTransform(src_cloud, target_cloud, true);
    pcl::transformPointCloud(*src_cloud, *temp_cloud, init_transform.cast<float>());
    *src_cloud = *temp_cloud;

    // copy cloud data
    pcl::copyPointCloud(*src_cloud, *object);
    pcl::copyPointCloud(*target_cloud, *scene);

    // Estimate normals for scene
    pcl::console::print_highlight("Estimating scene normals...\n");
    pcl::NormalEstimationOMP<PointNT, PointNT> nest;
    nest.setViewPoint(0.0, 0.0, VIEW_POINT_Z);
    nest.setRadiusSearch(sac_align_config_.normal_est_rad);
    nest.setInputCloud(scene);
    nest.compute(*scene);

    // Estimate normals for object
    pcl::console::print_highlight("Estimating object normals...\n");
    nest.setInputCloud(object);
    nest.compute(*object);

    // Estimate features
    pcl::console::print_highlight("Estimating features...\n");
    FeatureEstimationT fest;
    fest.setRadiusSearch(sac_align_config_.feature_est_rad);
    fest.setInputCloud(object);
    fest.setInputNormals(object);
    fest.compute(*object_features);
    fest.setInputCloud(scene);
    fest.setInputNormals(scene);
    fest.compute(*scene_features);

    // Perform alignment
    pcl::console::print_highlight("Starting alignment...\n");
    pcl::SampleConsensusPrerejective<PointNT, PointNT, FeatureT> align;
    align.setInputSource(object);
    align.setSourceFeatures(object_features);
    align.setInputTarget(scene);
    align.setTargetFeatures(scene_features);
    align.setMaximumIterations(sac_align_config_.max_iters);  // Number of RANSAC iterations
    align.setNumberOfSamples(sac_align_config_.num_samples);  // Number of points to sample for generating/prerejecting
                                                              // a pose
    align.setCorrespondenceRandomness(sac_align_config_.correspondence_rand);  // Number of nearest features to use
    align.setSimilarityThreshold(sac_align_config_.similarity_threshold);  // Polygonal edge length similarity threshold
    align.setMaxCorrespondenceDistance(sac_align_config_.max_correspondence_dist);  // Inlier threshold
    align.setInlierFraction(sac_align_config_.inlier_fraction);  // Required inlier fraction for accepting a pose
                                                                 // hypothesis
    {
      pcl::ScopeTime t("Alignment");
      align.align(*object_aligned);
    }

    if (!align.hasConverged())
    {
      return false;
    }

    transform.setIdentity();
    transform.matrix() = align.getFinalTransformation().cast<double>();
    transform = transform * init_transform;
    return true;
  }

  void handleLocalizeToPart(const std::shared_ptr<rmw_request_id_t> request_header,
                            const std::shared_ptr<crs_msgs::srv::LocalizeToPart::Request> request,
                            const std::shared_ptr<crs_msgs::srv::LocalizeToPart::Response> response)
  {
    using namespace std::chrono_literals;

    (void)request_header;

    if (enable_debug_visualizations_)
    {
      publish_timer_.reset();

      sensor_msgs::msg::PointCloud2 loaded_cloud;
      pcl::toROSMsg(*part_point_cloud_, loaded_cloud);
      loaded_cloud.header.stamp = this->now();
      loaded_cloud.header.frame_id = world_frame_;

      sensor_msgs::msg::PointCloud2 empty_cloud;
      empty_cloud.header.stamp = this->now();
      empty_cloud.header.frame_id = world_frame_;

      publish_timer_ = this->create_wall_timer(500ms, [this, loaded_cloud, empty_cloud]() -> void {
        loaded_part_pc_pub_->publish(loaded_cloud);
        combined_pc_pub_->publish(empty_cloud);
      });
    }

    if (!part_loaded_)
    {
      response->success = false;
      response->error = "Missing part. Please load a part first.";
      RCLCPP_ERROR_STREAM(this->get_logger(), response->error);
      return;
    }

    // getting part seed transform
    Eigen::Isometry3d part_seed_transform_;
    tf2::fromMsg(part_seed_msg_, part_seed_transform_);

    // TODO: better input verification
    if (request->point_clouds.empty() && request->transforms.size() != request->point_clouds.size())
    {
      response->success = false;
      response->error = "Request has invalid data";
      RCLCPP_ERROR_STREAM(this->get_logger(), response->error);
      return;
    }

    // combining all clouds
    pcl::PointCloud<pcl::PointXYZ>::Ptr combined_point_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    for (std::size_t i = 0; i < request->point_clouds.size(); i++)
    {
      auto& point_cloud_msg = request->point_clouds[i];
      geometry_msgs::msg::TransformStamped& transform = request->transforms[i];

      pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
      pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

      pcl::fromROSMsg(point_cloud_msg, *point_cloud);
      pcl::transformPointCloud(*point_cloud, *transformed_cloud, tf2::transformToEigen(transform).matrix());

      *combined_point_cloud += *transformed_cloud;
    }

    RCLCPP_INFO(this->get_logger(), "Combined point cloud has %lu points", combined_point_cloud->size());

    // transforming to requested frame
    std::string src_frame_id = request->transforms.front().header.frame_id;
    if (src_frame_id != request->frame)
    {
      try
      {
        RCLCPP_INFO(
            this->get_logger(), "Transforming cloud from frame %s to %s", src_frame_id.c_str(), request->frame.c_str());
        geometry_msgs::msg::TransformStamped transform =
            tf_buffer_.lookupTransform(request->frame, src_frame_id, tf2::TimePointZero);
        pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
        ;
        pcl::transformPointCloud(*combined_point_cloud, *transformed_cloud, tf2::transformToEigen(transform).matrix());
        *combined_point_cloud = *transformed_cloud;
      }
      catch (tf2::TransformException ex)
      {
        RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
        response->success = false;
        response->error =
            "Failed to transform point cloud from '" + src_frame_id + "' to '" + request->frame + "' frame";
        return;
      }
    }

    // cropping
    for (auto& cfg : crop_boxes_)
    {
      combined_point_cloud = cropBox(cfg, combined_point_cloud);
    }
    RCLCPP_INFO(this->get_logger(), "Combined point cloud has %lu points after cropping", combined_point_cloud->size());

    if (combined_point_cloud->empty())
    {
      response->success = false;
      response->error = "No point remains after cropping";
      RCLCPP_ERROR_STREAM(this->get_logger(), response->error);
      return;
    }

    if (enable_debug_visualizations_)
    {
      publish_timer_.reset();

      sensor_msgs::msg::PointCloud2 scanned_cloud;
      pcl::toROSMsg(*combined_point_cloud, scanned_cloud);
      scanned_cloud.header.stamp = this->now();
      scanned_cloud.header.frame_id = world_frame_;

      sensor_msgs::msg::PointCloud2 loaded_cloud;
      pcl::toROSMsg(*part_point_cloud_, loaded_cloud);
      loaded_cloud.header.stamp = this->now();
      loaded_cloud.header.frame_id = world_frame_;

      publish_timer_ = this->create_wall_timer(500ms, [this, loaded_cloud, scanned_cloud]() -> void {
        loaded_part_pc_pub_->publish(loaded_cloud);
        combined_pc_pub_->publish(scanned_cloud);
      });
    }

    // downsampling
    pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    *src_cloud = downsampleCloud(part_point_cloud_, part_leaf_size_);
    *target_cloud = downsampleCloud(combined_point_cloud, leaf_size_);
    RCLCPP_INFO(this->get_logger(), "Combined point cloud has %lu points after downsampling", target_cloud->size());

    // aligning
    Eigen::Isometry3d transform;
    response->success = alignIcp(target_cloud, src_cloud, transform);
    RCLCPP_INFO_EXPRESSION(this->get_logger(), response->success, "ICP converged");
    RCLCPP_ERROR_EXPRESSION(this->get_logger(), !response->success, "ICP failed to converge");

    // printing transform
    Eigen::Vector3d angles = transform.linear().eulerAngles(0, 1, 2);
    RCLCPP_INFO(this->get_logger(),
                "Transform : p(%f, %f, %f), r(%f, %f, %f)",
                response->transform.transform.translation.x,
                response->transform.transform.translation.y,
                response->transform.transform.translation.z,
                angles(0),
                angles(1),
                angles(2));

    response->transform = tf2::eigenToTransform(transform * part_seed_transform_);  // applying seed
    response->transform.header.stamp = this->now();
    response->transform.header.frame_id = request->frame;
    response->transform.child_frame_id = part_frame_;

    if (enable_debug_visualizations_)
    {
      publish_timer_.reset();

      sensor_msgs::msg::PointCloud2 scanned_cloud;
      pcl::toROSMsg(*target_cloud, scanned_cloud);
      scanned_cloud.header.stamp = this->now();
      scanned_cloud.header.frame_id = world_frame_;

      pcl::transformPointCloud(*part_point_cloud_, *src_cloud, transform.cast<float>());
      sensor_msgs::msg::PointCloud2 registered_cloud;
      pcl::toROSMsg(*src_cloud, registered_cloud);
      registered_cloud.header.stamp = this->now();
      registered_cloud.header.frame_id = world_frame_;

      publish_timer_ = this->create_wall_timer(500ms, [this, registered_cloud, scanned_cloud]() -> void {
        loaded_part_pc_pub_->publish(registered_cloud);
        combined_pc_pub_->publish(scanned_cloud);
      });
    }
  }

  // parameters
  int mesh_num_samples_;
  double leaf_size_, part_leaf_size_;
  bool enable_debug_visualizations_;
  std::string part_frame_;
  IcpConfig icp_config_;
  SACAlignConfig sac_align_config_;
  std::vector<CropBoxConfig> crop_boxes_;

  // subscriber parameters
  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_subs_;

  // publishers
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr loaded_part_pc_pub_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr tf_loaded_part_pc_pub_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr combined_pc_pub_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markers_pub_;

  // services
  rclcpp::Service<crs_msgs::srv::LoadPart>::SharedPtr load_part_service_;
  rclcpp::Service<crs_msgs::srv::LocalizeToPart>::SharedPtr localize_to_part_service_;

  // timers
  rclcpp::TimerBase::SharedPtr publish_timer_;
  rclcpp::TimerBase::SharedPtr markers_timer_;

  // data
  geometry_msgs::msg::Pose part_seed_msg_;
  bool part_loaded_;
  std::string world_frame_;
  pcl::PointCloud<pcl::PointXYZ>::Ptr part_point_cloud_;
  visualization_msgs::msg::MarkerArray cropbox_markers_;

  // other
  rclcpp::Clock::SharedPtr clock_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
};

}  // namespace crs_perception

RCLCPP_COMPONENTS_REGISTER_NODE(crs_perception::LocalizeToPart)

int main(int argc, char** argv)
{
  // force flush of the stdout buffer.
  // this ensures a correct sync of all prints
  // even when executed simultaneously within the launch file.
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);

  static const rclcpp::Logger LOGGER = rclcpp::get_logger("localize_to_part");

  RCLCPP_INFO(LOGGER, "Initialize node");
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  // This enables loading undeclared parameters
  // best practice would be to declare parameters in the corresponding classes
  // and provide descriptions about expected use
  node_options.automatically_declare_parameters_from_overrides(true);
  std::shared_ptr<crs_perception::LocalizeToPart> node = std::make_shared<crs_perception::LocalizeToPart>(node_options);

  rclcpp::spin(node);
  return 0;
}


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_DEALII_MATRIX_FREE_OPERATOR_HPP
#define MFMG_DEALII_MATRIX_FREE_OPERATOR_HPP

#include <mfmg/common/operator.hpp>
#include <mfmg/dealii/dealii_matrix_free_mesh_evaluator.hpp>

#include <deal.II/base/subscriptor.h>
#include <deal.II/lac/diagonal_matrix.h>

namespace mfmg
{
// NOTE Must derive from dealii::Subscriptor and provide vmult(dst, src), m(),
// n(), and el(i, j) to be used as the MatrixType template argument of
// dealii::PreconditionerChebyshev.  The methods n() and el() aren't actually
// called, they throw a not implemented exception.
template <int dim, typename VectorType>
class DealIIMatrixFreeOperator : public Operator<VectorType>,
                                 public dealii::Subscriptor
{
public:
  using vector_type = VectorType;
  using size_type = typename VectorType::size_type;
  using value_type = typename VectorType::value_type;

  DealIIMatrixFreeOperator(std::shared_ptr<DealIIMatrixFreeMeshEvaluator<dim>>
                               matrix_free_mesh_evaluator);

  virtual ~DealIIMatrixFreeOperator() override = default;

  void apply(vector_type const &x, vector_type &y,
             OperatorMode mode = OperatorMode::NO_TRANS) const override final;

  std::shared_ptr<Operator<VectorType>> transpose() const override final;

  std::shared_ptr<Operator<VectorType>>
  multiply(std::shared_ptr<Operator<VectorType> const> b) const override final;

  std::shared_ptr<Operator<VectorType>> multiply_transpose(
      std::shared_ptr<Operator<VectorType> const> b) const override final;

  void vmult(vector_type &dst, vector_type const &src) const;

  size_type m() const;

  size_type n() const;

  value_type el(size_type i, size_type j) const;

  std::shared_ptr<vector_type> build_domain_vector() const override final;

  std::shared_ptr<vector_type> build_range_vector() const override final;

  size_t grid_complexity() const override final;

  size_t operator_complexity() const override final;

  std::shared_ptr<dealii::DiagonalMatrix<vector_type>>
  get_diagonal_inverse() const;

private:
  std::shared_ptr<DealIIMatrixFreeMeshEvaluator<dim>> _mesh_evaluator;
};
} // namespace mfmg

#endif


// ------------------------------------------------------------------------
//  Copyright (C)
//  Universitat Politecnica de Catalunya BarcelonaTech (UPC) - Spain
//  University of California Berkeley (UCB) - USA
//
//  Jordi Pont-Tuset <jordi.pont@upc.edu>
//  Pablo Arbelaez <arbelaez@berkeley.edu>
//  June 2014
// ------------------------------------------------------------------------
// This file is part of the MCG package presented in:
//    Arbelaez P, Pont-Tuset J, Barron J, Marques F, Malik J,
//    "Multiscale Combinatorial Grouping,"
//    Computer Vision and Pattern Recognition (CVPR) 2014.
// Please consider citing the paper if you use this code.
// ------------------------------------------------------------------------
#include "mex.h"
#include "mex_helpers.hpp"
#include "ucm2hier.hpp"

#include <iostream>
#include <vector>
#include <list>
#include <set>
#include <map>
#include <algorithm>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

void mexFunction( int nlhs, mxArray *plhs[],
        		  int nrhs, const mxArray*prhs[] )
{
    if(nrhs!=2)
        mexErrMsgTxt("There should be 2 input parameters");

    /* Input parameters */
    auto lp   = eigen_map<label_type>(prhs[0]);
    auto ucm2 = eigen_map<double>(prhs[1]);

    /* Call actual function */
    merging_sequence ms = ucm2hier(ucm2, lp);

    /* Output allocation */
    plhs[0] = mxCreateDoubleMatrix(ms.parent_labels.size(),ms.n_max_children+1,mxREAL);
    auto ms_out = eigen_map<double>(plhs[0]);
    plhs[1] = mxCreateDoubleMatrix(ms.n_regs,1,mxREAL);
    auto start_ths_out = eigen_map<double>(plhs[1]);
    plhs[2] = mxCreateDoubleMatrix(ms.n_regs,1,mxREAL);
    auto end_ths_out = eigen_map<double>(plhs[2]);

    /* Copy data to output */
    for (size_t ii=0; ii<ms.n_regs; ++ii)
        start_ths_out(ii,0) = ms.start_ths[ii];

    for (size_t ii=0; ii<ms.parent_labels.size(); ++ii)
    {
        ms_out(ii,ms.n_max_children) = ms.parent_labels[ii];
        list<label_type>::iterator it4 = ms.children_labels[ii].begin();
        for (size_t jj=0 ; it4!=ms.children_labels[ii].end(); ++it4, ++jj)
        {
            ms_out(ii,jj) = *it4;
            end_ths_out(*it4-1,0) = start_ths_out(ms.parent_labels[ii]-1,0);
        }
    }
    end_ths_out(ms.n_regs-1,0) = 1;
}


#ifndef ALGORITHM_KMEANS_HPP_7D0XW1DK
#define ALGORITHM_KMEANS_HPP_7D0XW1DK

#include <math.h>
#include <log4cxx/logger.h>
#include <log4cxx/basicconfigurator.h>
#include <log4cxx/helpers/exception.h>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/unordered_map.hpp>
#include <boost/multi_array.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace log4cxx;
using namespace log4cxx::helpers;
//namespace std {using namespace __gnu_cxx;}
using namespace std;
using namespace boost;

namespace prhlt {

    typedef vector<int> label_map;

    class Algorithm_KMeans
    {
    public:
        enum
        {
            BS=1,
            IL=2,
            NL=3
        };
        Algorithm_KMeans(cv::Mat &ex_data, cv::Mat &ex_image);
        ~Algorithm_KMeans();
        void run(int binarization_threshold);
        cv::Mat get_labels();
        int classify(cv::Mat sample);
    private:
        cv::Mat data;
        cv::Mat image;
        cv::Mat centers;
        int num_centers;
        cv::Mat data_labels;
        vector<float> center_grey_count;
        label_map center2label_map;
        double euclidean_distance(cv::Mat center,cv::Mat sample);
        void set_labels2centers(int binarization_threshold);
        void calculate_centers_grey_count(int binarization_threshold);
        void draw_result();
    };
}

#endif /* end of include guard: ALGORITHM_KMEANS_HPP_7D0XW1DK */


#include <boost/geometry/strategies/geographic/side_thomas.hpp>