/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_SDK_MEMORY_DETAILS_TIMES_HPP_INCLUDED
#define NT2_SDK_MEMORY_DETAILS_TIMES_HPP_INCLUDED

////////////////////////////////////////////////////////////////////////////////
// Activates operator * on mpl integral with integral
////////////////////////////////////////////////////////////////////////////////
#include <boost/mpl/times.hpp>
#include <nt2/sdk/meta/mpl.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/tr1/functional.hpp>
#include <nt2/sdk/functor/operators.hpp>
#include <nt2/sdk/functor/preprocessor/call.hpp>

////////////////////////////////////////////////////////////////////////////////
// Make nultiplies works with mpl::integral types
////////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH ( tag::multiplies_ , tag::cpu_
                      , (A0)(A1)
                      , (mpl_integral_< integer_<A0> >)(integer_<A1>)
                      )

NT2_REGISTER_DISPATCH ( tag::multiplies_ , tag::cpu_
                      , (A0)(A1)
                      , (integer_<A0>)(mpl_integral_< integer_<A1> >)
                      )

NT2_REGISTER_DISPATCH ( tag::multiplies_ , tag::cpu_
                      , (A0)(A1)
                      , (mpl_integral_< integer_<A0> >)
                        (mpl_integral_< integer_<A1> >)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct  call< tag::multiplies_( tag::mpl_integral_<tag::integer_>
                                , tag::integer_
                                )
              , tag::cpu_, Dummy
              > : callable
  {
    template<class Sig> struct result;
    template<class This,class A0, class A1>  struct
    result<This(A0,A1)>
    {
      static A1 a1;
      typedef typename meta::strip<A0>::type arg0;
      BOOST_TYPEOF_NESTED_TYPEDEF_TPL(nested,arg0::value*a1)
      typedef typename nested::type type;
    };

    NT2_FUNCTOR_CALL(2) { return A0::value*a1; }
  };

  template<class Dummy>
  struct  call< tag::multiplies_( tag::integer_
                                , tag::mpl_integral_<tag::integer_>
                                )
              , tag::cpu_, Dummy
              > : callable
  {
    template<class Sig> struct result;
    template<class This,class A0, class A1>  struct
    result<This(A0,A1)>
    {
      static A0 a0;
      typedef typename meta::strip<A1>::type arg1;
      BOOST_TYPEOF_NESTED_TYPEDEF_TPL(nested,arg1::value*a0)
      typedef typename nested::type type;
    };

    NT2_FUNCTOR_CALL(2) { return a0*A1::value; }
  };

  template<class Dummy>
  struct  call< tag::multiplies_( tag::mpl_integral_<tag::integer_>
                                , tag::mpl_integral_<tag::integer_>
                                )
              , tag::cpu_, Dummy
              > : callable
  {
    template<class Sig> struct result;
    template<class This,class A0, class A1>
    struct  result<This(A0,A1)>
          : boost::mpl::times < typename meta::strip<A0>::type
                              , typename meta::strip<A1>::type
                              >
    {};

    NT2_FUNCTOR_CALL(2)
    {
      return typename NT2_RETURN_TYPE(2)::type();
    }
  };
} }

#endif


#include "mpfr/mpfr.hpp"

#include <boost/multiprecision/mpfr.hpp>
#include "nanobench.h"

template <int N> using scalar_t = mpfr::mp_float_t<mpfr::digits10{N}>;
template <int N>
using bscalar_t = boost::multiprecision::number<
    boost::multiprecision::mpfr_float_backend<N, boost::multiprecision::allocate_stack>,
    boost::multiprecision::et_off>;

template <typename T, typename Rhs>
void bench_mul_div(ankerl::nanobench::Bench& bench, Rhs x, std::string const& name) {
  T a = sqrt(T{2.0});
  T c{};

  ankerl::nanobench::doNotOptimizeAway(&a);
  ankerl::nanobench::doNotOptimizeAway(&c);

  bench.run("100 digits10: mul 128" + name, [&] {
    c = a * x;
    ankerl::nanobench::clobberMemory();
  });
  bench.run("100 digits10: div 128" + name, [&] {
    c = a / x;
    ankerl::nanobench::clobberMemory();
  });
}

auto main() -> int {

  auto bench = ankerl::nanobench::Bench();

  bench.minEpochTime(std::chrono::milliseconds{100UL});

  bench_mul_div<scalar_t<100>>(bench, 128.0, "d");
  bench_mul_div<scalar_t<100>>(bench, 128, "i");
  bench_mul_div<bscalar_t<100>>(bench, 128.0, "d boost");
  bench_mul_div<bscalar_t<100>>(bench, 128, "i boost");

  bench_mul_div<scalar_t<1000>>(bench, 128.0, "d");
  bench_mul_div<scalar_t<1000>>(bench, 128, "i");
  bench_mul_div<bscalar_t<1000>>(bench, 128.0, "d boost");
  bench_mul_div<bscalar_t<1000>>(bench, 128, "i boost");
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_LDEXP_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_LDEXP_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief ldexp generic tag

     Represents the ldexp function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct ldexp_ : ext::elementwise_<ldexp_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<ldexp_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_ldexp_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::ldexp_, Site> dispatching_ldexp_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::ldexp_, Site>();
   }
   template<class... Args>
   struct impl_ldexp_;
  }
  /*!
    The function multiply a floating entry \f$a_0\f$
    by \f$2^{a_1}\f$

    @par Semantic:

    @code
    T r = ldexp(a0,a1);
    @endcode

    is similar to:

    @code
    T r = a0*pow(2, a1);
    @endcode

    @param a0

    @param a1

    @return a value of same type as the inputs
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::ldexp_, ldexp, 2)
} }

#endif


/**
 * @date Tue Oct 11 12:18:23 2011 +0200
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <vector>
#include <algorithm>
#include <boost/random.hpp>
#include <cmath>

#include <bob.learn.em/EMPCATrainer.h>
#include <bob.core/array_copy.h>
#include <bob.core/check.h>
#include <bob.math/linear.h>
#include <bob.math/det.h>
#include <bob.math/inv.h>
#include <bob.math/pinv.h>
#include <bob.math/stats.h>

bob::learn::em::EMPCATrainer::EMPCATrainer(bool compute_likelihood):
  m_compute_likelihood(compute_likelihood),
  m_rng(new boost::mt19937()),
  m_S(0,0),
  m_z_first_order(0,0), m_z_second_order(0,0,0),
  m_inW(0,0), m_invM(0,0), m_sigma2(0), m_f_log2pi(0),
  m_tmp_dxf(0,0), m_tmp_d(0), m_tmp_f(0),
  m_tmp_dxd_1(0,0), m_tmp_dxd_2(0,0),
  m_tmp_fxd_1(0,0), m_tmp_fxd_2(0,0),
  m_tmp_fxf_1(0,0), m_tmp_fxf_2(0,0)
{
}

bob::learn::em::EMPCATrainer::EMPCATrainer(const bob::learn::em::EMPCATrainer& other):
  m_compute_likelihood(other.m_compute_likelihood),
  m_rng(other.m_rng),
  m_S(bob::core::array::ccopy(other.m_S)),
  m_z_first_order(bob::core::array::ccopy(other.m_z_first_order)),
  m_z_second_order(bob::core::array::ccopy(other.m_z_second_order)),
  m_inW(bob::core::array::ccopy(other.m_inW)),
  m_invM(bob::core::array::ccopy(other.m_invM)),
  m_sigma2(other.m_sigma2), m_f_log2pi(other.m_f_log2pi),
  m_tmp_dxf(bob::core::array::ccopy(other.m_tmp_dxf)),
  m_tmp_d(bob::core::array::ccopy(other.m_tmp_d)),
  m_tmp_f(bob::core::array::ccopy(other.m_tmp_f)),
  m_tmp_dxd_1(bob::core::array::ccopy(other.m_tmp_dxd_1)),
  m_tmp_dxd_2(bob::core::array::ccopy(other.m_tmp_dxd_2)),
  m_tmp_fxd_1(bob::core::array::ccopy(other.m_tmp_fxd_1)),
  m_tmp_fxd_2(bob::core::array::ccopy(other.m_tmp_fxd_2)),
  m_tmp_fxf_1(bob::core::array::ccopy(other.m_tmp_fxf_1)),
  m_tmp_fxf_2(bob::core::array::ccopy(other.m_tmp_fxf_2))
{
}

bob::learn::em::EMPCATrainer::~EMPCATrainer()
{
}

bob::learn::em::EMPCATrainer& bob::learn::em::EMPCATrainer::operator=
  (const bob::learn::em::EMPCATrainer& other)
{
  if (this != &other)
  {
    m_rng                   = other.m_rng;
	m_compute_likelihood    = other.m_compute_likelihood;
    m_S = bob::core::array::ccopy(other.m_S);
    m_z_first_order = bob::core::array::ccopy(other.m_z_first_order);
    m_z_second_order = bob::core::array::ccopy(other.m_z_second_order);
    m_inW = bob::core::array::ccopy(other.m_inW);
    m_invM = bob::core::array::ccopy(other.m_invM);
    m_sigma2 = other.m_sigma2;
    m_f_log2pi = other.m_f_log2pi;
    m_tmp_dxf = bob::core::array::ccopy(other.m_tmp_dxf);
    m_tmp_d = bob::core::array::ccopy(other.m_tmp_d);
    m_tmp_f = bob::core::array::ccopy(other.m_tmp_f);
    m_tmp_dxd_1 = bob::core::array::ccopy(other.m_tmp_dxd_1);
    m_tmp_dxd_2 = bob::core::array::ccopy(other.m_tmp_dxd_2);
    m_tmp_fxd_1 = bob::core::array::ccopy(other.m_tmp_fxd_1);
    m_tmp_fxd_2 = bob::core::array::ccopy(other.m_tmp_fxd_2);
    m_tmp_fxf_1 = bob::core::array::ccopy(other.m_tmp_fxf_1);
    m_tmp_fxf_2 = bob::core::array::ccopy(other.m_tmp_fxf_2);
  }
  return *this;
}

bool bob::learn::em::EMPCATrainer::operator==
  (const bob::learn::em::EMPCATrainer &other) const
{
  return m_compute_likelihood == other.m_compute_likelihood &&
        m_rng                   == other.m_rng &&
        bob::core::array::isEqual(m_S, other.m_S) &&
        bob::core::array::isEqual(m_z_first_order, other.m_z_first_order) &&
        bob::core::array::isEqual(m_z_second_order, other.m_z_second_order) &&
        bob::core::array::isEqual(m_inW, other.m_inW) &&
        bob::core::array::isEqual(m_invM, other.m_invM) &&
        m_sigma2 == other.m_sigma2 &&
        m_f_log2pi == other.m_f_log2pi;
}

bool bob::learn::em::EMPCATrainer::operator!=
  (const bob::learn::em::EMPCATrainer &other) const
{
  return !(this->operator==(other));
}

bool bob::learn::em::EMPCATrainer::is_similar_to
  (const bob::learn::em::EMPCATrainer &other, const double r_epsilon,
   const double a_epsilon) const
{
  return m_compute_likelihood == other.m_compute_likelihood &&
         m_rng                == other.m_rng &&
         bob::core::array::isClose(m_S, other.m_S, r_epsilon, a_epsilon) &&
         bob::core::array::isClose(m_z_first_order, other.m_z_first_order, r_epsilon, a_epsilon) &&
         bob::core::array::isClose(m_z_second_order, other.m_z_second_order, r_epsilon, a_epsilon) &&
         bob::core::array::isClose(m_inW, other.m_inW, r_epsilon, a_epsilon) &&
         bob::core::array::isClose(m_invM, other.m_invM, r_epsilon, a_epsilon) &&
         bob::core::isClose(m_sigma2, other.m_sigma2, r_epsilon, a_epsilon) &&
         bob::core::isClose(m_f_log2pi, other.m_f_log2pi, r_epsilon, a_epsilon);
}

void bob::learn::em::EMPCATrainer::initialize(bob::learn::linear::Machine& machine,
  const blitz::Array<double,2>& ar)
{
  // reinitializes array members and checks dimensionality
  initMembers(machine, ar);

  // computes the mean and the covariance if required
  computeMeanVariance(machine, ar);

  // Random initialization of W and sigma2
  initRandomWSigma2(machine);

  // Computes the product m_inW = W^T.W
  computeWtW(machine);
  // Computes inverse(M), where M = Wt * W + sigma2 * Id
  computeInvM();
}

void bob::learn::em::EMPCATrainer::initMembers(
  const bob::learn::linear::Machine& machine,
  const blitz::Array<double,2>& ar)
{
  // Gets dimensions
  const size_t n_samples = ar.extent(0);
  const size_t n_features = ar.extent(1);

  // Checks that the dimensions are matching
  const size_t n_inputs = machine.inputSize();
  const size_t n_outputs = machine.outputSize();

  // Checks that the dimensions are matching
  if (n_inputs != n_features) {
    boost::format m("number of inputs (%u) does not match the number of features (%u)");
    m % n_inputs % n_features;
    throw std::runtime_error(m.str());
  }

  // Covariance matrix S is only required to compute the log likelihood
  if (m_compute_likelihood)
    m_S.resize(n_features,n_features);
  else
    m_S.resize(0,0);
  m_z_first_order.resize(n_samples, n_outputs);
  m_z_second_order.resize(n_samples, n_outputs, n_outputs);
  m_inW.resize(n_outputs, n_outputs);
  m_invM.resize(n_outputs, n_outputs);
  m_sigma2 = 0.;
  m_f_log2pi = n_features * log(2*M_PI);

  // Cache
  m_tmp_dxf.resize(n_outputs, n_features);
  m_tmp_d.resize(n_outputs);
  m_tmp_f.resize(n_features);

  m_tmp_dxd_1.resize(n_outputs, n_outputs);
  m_tmp_dxd_2.resize(n_outputs, n_outputs);
  m_tmp_fxd_1.resize(n_features, n_outputs);
  m_tmp_fxd_2.resize(n_features, n_outputs);
  // The following large cache matrices are only required to compute the
  // log likelihood.
  if (m_compute_likelihood)
  {
    m_tmp_fxf_1.resize(n_features, n_features);
    m_tmp_fxf_2.resize(n_features, n_features);
  }
  else
  {
    m_tmp_fxf_1.resize(0,0);
    m_tmp_fxf_2.resize(0,0);
  }
}

void bob::learn::em::EMPCATrainer::computeMeanVariance(bob::learn::linear::Machine& machine,
  const blitz::Array<double,2>& ar)
{
  size_t n_samples = ar.extent(0);
  blitz::Array<double,1> mu = machine.updateInputSubtraction();
  blitz::Range all = blitz::Range::all();
  if (m_compute_likelihood)
  {
    // Mean and scatter computation
    bob::math::scatter(ar, m_S, mu);
    // divides scatter by N-1
    m_S /= static_cast<double>(n_samples-1);
  }
  else
  {
    // computes the mean and updates mu
    mu = 0.;
    for (size_t i=0; i<n_samples; ++i)
      mu += ar(i,all);
    mu /= static_cast<double>(n_samples);
  }
}

void bob::learn::em::EMPCATrainer::initRandomWSigma2(bob::learn::linear::Machine& machine)
{
  // Initializes the random number generator
  boost::uniform_01<> range01;
  boost::variate_generator<boost::mt19937&, boost::uniform_01<> > die(*m_rng, range01);

  // W initialization (TODO: add method in core)
  blitz::Array<double,2> W = machine.updateWeights();
  double ratio = 2.; /// Follows matlab implementation using a ratio of 2
  for (int i=0; i<W.extent(0); ++i)
    for (int j=0; j<W.extent(1); ++j)
      W(i,j) = die() * ratio;
  // sigma2 initialization
  m_sigma2 = die() * ratio;
}

void bob::learn::em::EMPCATrainer::computeWtW(bob::learn::linear::Machine& machine)
{
  const blitz::Array<double,2> W = machine.getWeights();
  const blitz::Array<double,2> Wt = W.transpose(1,0);
  bob::math::prod(Wt, W, m_inW);
}

void bob::learn::em::EMPCATrainer::computeInvM()
{
  // Compute inverse(M), where M = W^T * W + sigma2 * Id
  bob::math::eye(m_tmp_dxd_1); // m_tmp_dxd_1 = Id
  m_tmp_dxd_1 *= m_sigma2; // m_tmp_dxd_1 = sigma2 * Id
  m_tmp_dxd_1 += m_inW; // m_tmp_dxd_1 = M = W^T * W + sigma2 * Id
  bob::math::inv(m_tmp_dxd_1, m_invM); // m_invM = inv(M)
}

void bob::learn::em::EMPCATrainer::eStep(bob::learn::linear::Machine& machine, const blitz::Array<double,2>& ar)
{
  // Gets mu and W from the machine
  const blitz::Array<double,1>& mu = machine.getInputSubtraction();
  const blitz::Array<double,2>& W = machine.getWeights();
  const blitz::Array<double,2> Wt = W.transpose(1,0); // W^T

  // Computes the statistics
  blitz::Range a = blitz::Range::all();
  for(int i=0; i<ar.extent(0); ++i)
  {
    /// 1/ First order statistics: \f$z_first_order_i = inv(M) W^T (t - \mu)\f$
    // m_tmp_f = t (sample) - mu (normalized sample)
    m_tmp_f = ar(i,a) - mu;
    // m_tmp_dxf = inv(M) * W^T
    bob::math::prod(m_invM, Wt, m_tmp_dxf);
    blitz::Array<double,1> z_first_order_i = m_z_first_order(i,blitz::Range::all());
    // z_first_order_i = inv(M) * W^T * (t - mu)
    bob::math::prod(m_tmp_dxf, m_tmp_f, z_first_order_i);

    /// 2/ Second order statistics:
    ///     z_second_order_i = sigma2 * inv(M) + z_first_order_i * z_first_order_i^T
    blitz::Array<double,2> z_second_order_i = m_z_second_order(i,blitz::Range::all(),blitz::Range::all());
    // m_tmp_dxd = z_first_order_i * z_first_order_i^T
    bob::math::prod(z_first_order_i, z_first_order_i, m_tmp_dxd_1); // outer product
    // z_second_order_i = sigma2 * inv(M)
    z_second_order_i = m_invM;
    z_second_order_i *= m_sigma2;
    // z_second_order_i = sigma2 * inv(M) + z_first_order_i * z_first_order_i^T
    z_second_order_i += m_tmp_dxd_1;
  }
}

void bob::learn::em::EMPCATrainer::mStep(bob::learn::linear::Machine& machine, const blitz::Array<double,2>& ar)
{
  // 1/ New estimate of W
  updateW(machine, ar);

  // 2/ New estimate of sigma2
  updateSigma2(machine, ar);

  // Computes the new value of inverse(M), where M = Wt * W + sigma2 * Id
  computeInvM();
}

void bob::learn::em::EMPCATrainer::updateW(bob::learn::linear::Machine& machine, const blitz::Array<double,2>& ar) {
  // Get the mean mu and the projection matrix W
  const blitz::Array<double,1>& mu = machine.getInputSubtraction();
  blitz::Array<double,2>& W = machine.updateWeights();
  const blitz::Array<double,2> Wt = W.transpose(1,0); // W^T

  // Compute W = sum{ (t_{i} - mu) z_first_order_i^T} * inv( sum{z_second_order_i} )
  m_tmp_fxd_1 = 0.;
  m_tmp_dxd_1 = 0.;
  blitz::Range a = blitz::Range::all();
  for(int i=0; i<ar.extent(0); ++i)
  {
    // m_tmp_f = t (sample) - mu (normalized sample)
    m_tmp_f = ar(i,a) - mu;
    // first order statistics of sample i
    blitz::Array<double,1> z_first_order_i = m_z_first_order(i,blitz::Range::all());
    // m_tmp_fxd_2 = (t - mu)*z_first_order_i
    bob::math::prod(m_tmp_f, z_first_order_i, m_tmp_fxd_2);
    m_tmp_fxd_1 += m_tmp_fxd_2;

    // second order statistics of sample i
    blitz::Array<double,2> z_second_order_i = m_z_second_order(i,blitz::Range::all(),blitz::Range::all());
    m_tmp_dxd_1 += z_second_order_i;
  }

  // m_tmp_dxd_2 = inv( sum(E(x_i.x_i^T)) )
  bob::math::inv(m_tmp_dxd_1, m_tmp_dxd_2);
  // New estimates of W
  bob::math::prod(m_tmp_fxd_1, m_tmp_dxd_2, W);
  // Updates W'*W as well
  bob::math::prod(Wt, W, m_inW);
}

void bob::learn::em::EMPCATrainer::updateSigma2(bob::learn::linear::Machine& machine, const blitz::Array<double,2>& ar) {
  // Get the mean mu and the projection matrix W
  const blitz::Array<double,1>& mu = machine.getInputSubtraction();
  const blitz::Array<double,2>& W = machine.getWeights();
  const blitz::Array<double,2> Wt = W.transpose(1,0); // W^T

  m_sigma2 = 0.;
  blitz::Range a = blitz::Range::all();
  for(int i=0; i<ar.extent(0); ++i)
  {
    // a. sigma2 += || t - mu ||^2
    // m_tmp_f = t (sample) - mu (normalized sample)
    m_tmp_f = ar(i,a) - mu;
    // sigma2 += || t - mu ||^2
    m_sigma2 += blitz::sum(blitz::pow2(m_tmp_f));

    // b. sigma2 -= 2*E(x_i)^T*W^T*(t - mu)
    // m_tmp_d = W^T*(t - mu)
    bob::math::prod(Wt, m_tmp_f, m_tmp_d);
    // first order of i
    blitz::Array<double,1> z_first_order_i = m_z_first_order(i,blitz::Range::all());
    // sigma2 -= 2*E(x_i)^T*W^T*(t - mu)
    m_sigma2 -= 2*bob::math::dot(z_first_order_i, m_tmp_d);

    // c. sigma2 += trace( E(x_i.x_i^T)*W^T*W )
    // second order of i
    blitz::Array<double,2> z_second_order_i = m_z_second_order(i,blitz::Range::all(),blitz::Range::all());
    // m_tmp_dxd_1 = E(x_i.x_i^T)*W^T*W
    bob::math::prod(z_second_order_i, m_inW, m_tmp_dxd_1);
    // sigma2 += trace( E(x_i.x_i^T)*W^T*W )
    m_sigma2 += bob::math::trace(m_tmp_dxd_1);
  }
  // Normalization factor
  m_sigma2 /= (static_cast<double>(ar.extent(0)) * mu.extent(0));
}

double bob::learn::em::EMPCATrainer::computeLikelihood(bob::learn::linear::Machine& machine)
{
  // Get W projection matrix
  const blitz::Array<double,2>& W = machine.getWeights();
  const blitz::Array<double,2> Wt = W.transpose(1,0); // W^T
  const size_t n_features = m_S.extent(0);

  // 1/ Compute det(C), where C = sigma2.I + W.W^T
  //            det(C) = det(sigma2 * C / sigma2) = det(sigma2 * Id) * det(C / sigma2)
  //    We are using Sylvester's determinant theorem to compute a dxd
  //    determinant rather than a fxf one: det(I + A.B) = det(I + B.A)
  //            det(C) = sigma2^n_features * det(I + W.W^T/sigma2)
  //                   = sigma2^n_features * det(I + W^T.W/sigma2) (cf. Bishop Appendix C)
  // detC = det( eye(n_features) * sigma2 )

  // detC = sigma2^n_features
  double detC = pow(m_sigma2, n_features);
  // m_tmp_dxd_1 = Id
  bob::math::eye(m_tmp_dxd_1);
  // m_tmp_dxd_2 = W^T.W
  bob::math::prod(Wt, W, m_tmp_dxd_2);
  // m_tmp_dxd_2 = W^T.W / sigma2
  m_tmp_dxd_2 /= m_sigma2;
  // m_tmp_dxd_1 = Id + W^T.W / sigma2
  m_tmp_dxd_1 += m_tmp_dxd_2;
  // detC = sigma2^n_features * det(I + W^T.W/sigma2)
  detC *= bob::math::det(m_tmp_dxd_1);

  // 2/ Compute inv(C), where C = sigma2.I + W.W^T
  //    We are using the following identity (Property C.7 of Bishop's book)
  //      (A + B.D^-1.C)^-1 = A^-1 - A^-1.B(D+C.A^-1.B)^-1.C.A^-1
  //    Hence, inv(C) = sigma2^-1 .(I - W.M^-1.W^T)

  // Compute inverse(M), where M = Wt * W + sigma2 * Id
  computeInvM();
  // m_tmp_fxf_1 = I = eye(n_features)
  bob::math::eye(m_tmp_fxf_1);
  // m_tmp_fxd_1 = W * inv(M)
  bob::math::prod(W, m_invM, m_tmp_fxd_1);
  // m_tmp_fxf_2 = (W * inv(M) * Wt)
  bob::math::prod(m_tmp_fxd_1, Wt, m_tmp_fxf_2);
  // m_tmp_fxd_1 = inv(C) = (I - W.M^-1.W^T) / sigma2
  m_tmp_fxf_1 -= m_tmp_fxf_2;
  m_tmp_fxf_1 /= m_sigma2;

  // 3/ Compute inv(C).S
  bob::math::prod(m_tmp_fxf_1, m_S, m_tmp_fxf_2);

  // 4/ Use previous values to compute the log likelihood:
  // Log likelihood =  - N/2*{ d*ln(2*PI) + ln |detC| + tr(C^-1.S) }
  double llh = - static_cast<double>(m_z_first_order.extent(0)) / 2. *
    ( m_f_log2pi + log(fabs(detC)) + bob::math::trace(m_tmp_fxf_2) );

  return llh;
}


#ifndef AGRO_DAEMON_USER_SESSION_ID_IMPL_HPP__
#define AGRO_DAEMON_USER_SESSION_ID_IMPL_HPP__

#include "../session_id.hpp"
#include <random>
#include <sstream>

namespace Agro{

//https://stackoverflow.com/a/48592524
template<unsigned Size>
std::string generate_session_id() noexcept
{
    std::random_device seeder;
    std::mt19937 rng(seeder());
    std::uniform_int_distribution<char> gen('!', '~');	//All printable range
    std::stringstream ss;
    for(unsigned i = 0; i < Size; ++i)
    {
    	ss << gen(rng);
    }

   return ss.str();
}

//https://www.boost.org/doc/libs/1_77_0/libs/random/example/password.cpp

//#include <boost/random/random_device.hpp>
//#include <boost/random/uniform_int_distribution.hpp>

//template<unsigned Size>
//std::string create_session_id()
//{
//    /*<< We first define the characters that we're going
//         to allow.  This is pretty much just the characters
//         on a standard keyboard.
//    >>*/
//    std::string chars(
//        "abcdefghijklmnopqrstuvwxyz"
//        "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
//        "1234567890"
//        "!@#$%^&*()"
//        "`~-_=+[{]}\\|;:'\",<.>/? ");
//    /*<< We use __random_device as a source of entropy, since we want
//         passwords that are not predictable.
//    >>*/
//    boost::random::random_device rng;
//    /*<< Finally we select 8 random characters from the
//         string and print them to cout.
//    >>*/
//    boost::random::uniform_int_distribution<> index_dist(0, chars.size() - 1);
//    std::stringstream ss;
//    for(unsigned i = 0; i < Size; ++i)
//    {
//        ss << chars[index_dist(rng)];
//    }
//    return ss.str();
//}

}//Agro

#endif /* AGRO_DAEMON_USER_SESSION_ID_IMPL_HPP__ */


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, Eigen::Dynamic, 1> test_double_D_1(const Eigen::Matrix<double, Eigen::Dynamic, 1> & M)
{
	return M;
}
void export_double_D_1()
{
	boost::python::def("test_double_D_1",test_double_D_1);
}


/*
Copyright 2015 Glen Joseph Fernandes
(glenjofe@gmail.com)

Distributed under the Boost Software License, Version 1.0.
(http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_ALIGN_DETAIL_ALIGN_UP_HPP
#define BOOST_ALIGN_DETAIL_ALIGN_UP_HPP

#include <boost/align/detail/is_alignment.hpp>
#include <boost/assert.hpp>

namespace boost {
namespace alignment {

inline void*
align_up(void* ptr, std::size_t alignment) BOOST_NOEXCEPT
{
    BOOST_ASSERT(detail::is_alignment(alignment));
    return reinterpret_cast<void*>((reinterpret_cast<std::
        size_t>(ptr) + alignment - 1) & ~(alignment - 1));
}

} /* alignment */
} /* boost */

#endif


#include <algorithm>
#include <vector>
#include <iostream>
#include <set>
#include <cmath>
#include <unordered_set>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <igl/active_set.h>
#include <igl/edge_collapse_is_valid.h>
#include <igl/circulation.h>
#include <igl/min_quad_with_fixed.h>
#include <igl/writeDMAT.h>
#include <igl/barycentric_coordinates.h>
#include <igl/point_simplex_squared_distance.h>
#include <chrono>

#include "cost_and_placement.h"
#include "eiquadprog.h"
#include "neighbor_faces_and_boundary.h"

namespace {
	const double eps = 1e-8;
	enum SOLVER_TYPE {
		IGL_SOLVER = 0,
		EIQUADPROG
	} solver = EIQUADPROG;
}

const double DINF = std::numeric_limits<double>::infinity();
// #define DEBUG_MODE

// get plane from three points, and express it as ax + by + cz + d = 0.
// return coefficients a,b,c,d as result
Eigen::Vector4d face_from_three_points (
	const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, const Eigen::Vector3d& v3)
{
	Eigen::Vector3d n = (v2-v1).cross(v3-v1);
	n = n/n.norm();
	double d = -n.dot(v1);

	Eigen::Vector4d res;
	res << n(0), n(1), n(2), d;

	assert( res != Eigen::Vector4d::Zero() && "Three points are colinear." );
	return res;
}

void cost_and_placement_qslim5d_halfedge (
	const Bundle & e,
	const Eigen::MatrixXd & V,
	const Eigen::MatrixXi & F,
	const Eigen::MatrixXd & TC,
	const Eigen::MatrixXi & FT,
	const EdgeMap & seam_edges,
	const MapV5d  & Vmetrics,
	const int seam_aware_degree,
	double &                cost,
	placement_info_5d &     new_placement
	)
{

	// Assign this to 0 rather than, say, -1 so that deleted elements will get
	// draw as degenerate elements at vertex 0 (which should always exist and
	// never get collapsed to anything else since it is the smallest index)
	using namespace Eigen;
	using namespace std;

	// If one of the endpoints is the special vertex at infinity, don't touch it.
	assert( e.size() == 2 );		//  each edge has two half-edge
	const bool has_infinity_vertex = V.row( V.rows()-1 ).minCoeff() == DINF;
	if( has_infinity_vertex
	  	&& (/* F(e[0].fi, e[0].ki) == V.rows()-1
	   		|| F(e[1].fi, e[1].ki) == V.rows()-1
	   		||*/ e[0].p[0].vi == V.rows()-1
	   		|| e[0].p[1].vi == V.rows()-1) ) {
		cost = DINF;
		return;
	}

#ifdef DEBUG_MODE
	cout << "No infinite vertex." << endl;
#endif

	solver = EIQUADPROG;
	MatrixXd new_metric;

	// two vertex indices on one side of e
	const int vi[2] = {e[0].p[0].vi, e[0].p[1].vi};
	// If vi[0] and vi[1] are in seam_edges, but (vi[0], vi[1]) is not, return infinite cost.
	if( seam_edges.count( vi[0] ) && seam_edges.count( vi[1] ) && !contains_edge( seam_edges, vi[0], vi[1] ) ) {
	    cost = DINF;
	    return;
	}

	/// case 1:
	// (vi[0], vi[1]) in seam_edges, compute each half edge
	if( contains_edge( seam_edges, vi[0], vi[1] ) ) {
		VertexBundle e_p0[2];		// two Vertex5d for both sides at one end
		VertexBundle e_p1[2];		// two Vertex5d for both sides at the other end
		MatrixXd m[2];				// two metrics
		for(int side=0; side<2; side++) {
			e_p0[side] = e[side].p[0];
			e_p1[side] = e[side].p[1];
			m[side] = Vmetrics.at(e_p0[side].vi).at(e_p0[side].tci)
					+ Vmetrics.at(e_p1[side].vi).at(e_p1[side].tci);
		}

		// TODO: Check that the seam edges are collinear with
		//       their neighbors along the seam. If they are not, return DINF.
		const auto & is_collinear = [&TC](const int tci_1, const int tci_2, const int tci_3)
		{
			assert( tci_1 >= 0 && tci_1 <TC.rows() );
			assert( tci_2 >= 0 && tci_2 <TC.rows() );
			assert( tci_3 >= 0 && tci_3 <TC.rows() );
			Eigen::RowVector2d uv1 = TC.row(tci_1);
			Eigen::RowVector2d uv2 = TC.row(tci_2);
			Eigen::RowVector2d uv3 = TC.row(tci_3);
			Eigen::RowVector2d n1 = (uv2-uv1)/(uv2-uv1).norm();
			Eigen::RowVector2d n2 = (uv3-uv1)/(uv3-uv1).norm();
			return 1-fabs(n1.dot(n2)) < eps;
		};
		const auto & edge_ratio = [&TC](const int tci_1, const int tci_2, const int tci_3)
		{
			assert( tci_1 >= 0 && tci_1 <TC.rows() );
			assert( tci_2 >= 0 && tci_2 <TC.rows() );
			assert( tci_3 >= 0 && tci_3 <TC.rows() );
			Eigen::RowVector2d uv1 = TC.row(tci_1);
			Eigen::RowVector2d uv2 = TC.row(tci_2);
			Eigen::RowVector2d uv3 = TC.row(tci_3);
			if( (uv3-uv2).norm() == 0 ) 	return DINF;
			return (uv2-uv1).norm()/(uv3-uv2).norm();
		};

		bool is_free[2] = {false, false};	// Correspond to vi[2]
		// for each end of one side, search its neighbor seam edges to check if anyone is collinear with e
		for(int end=0; end<2; end++) {
			// An end is free only if it has exactly two neighboring seam edges
			if(seam_edges.at(vi[end]).size() != 2)	continue;
			for(auto vj : seam_edges.at(vi[end])) {		// all the neighboring seam vertices.
				// test if exist one vertex which has two uvs collinear with both sides of e's uvs
				if( vj == vi[1-end] )	continue;
                double ratio[2] = {DINF, DINF};
				for(auto item : Vmetrics.at(vj)) {		// all the tci for one neighboring seam vertex.
					int tcj = item.first;
					if(is_collinear(tcj, e_p0[0].tci, e_p1[0].tci)) {
						ratio[0] = edge_ratio(tcj, e_p0[0].tci, e_p1[0].tci);
					}
					if(is_collinear(tcj, e_p0[1].tci, e_p1[1].tci)) {
						ratio[1] = edge_ratio(tcj, e_p1[1].tci, e_p0[1].tci);
					}
				}
				switch (seam_aware_degree) {
					case 0: is_free[end] = true;
						break;
					case 1: if (ratio[0]!=DINF && ratio[1]!=DINF) is_free[end] = true;
						break;
					case 2: if (ratio[0]!=DINF && ratio[1]!=DINF && abs(ratio[0]-ratio[1]) <= 1e-3) is_free[end] = true;
						break;
				}
			}
		}

		// neither end has collinear neighbor seam edge, shouldn't touch it.
		if( !is_free[0] && !is_free[1] ) {
			cost = DINF;
			return;
		}
		// Otherwise, if any end is not free, collapse e to it.
		for(int end=0; end<2; end++) {
			if( !is_free[end] ) {
				cost = 0;
				new_placement.tcs.resize(2);
				new_placement.metrics.resize(2);
				for(int side=0; side<2; side++) {
					RowVectorXd v(6);
					v.setOnes();
					assert(e[side].p[end].vi == vi[end] || e[side].p[end].vi == vi[1-end]);
					v.head(3) = V.row(vi[end]);
					v.segment(3,2) = e[side].p[end].vi == vi[end] ? TC.row(e[side].p[end].tci) : TC.row(e[side].p[1-end].tci);
					cost += v*m[side]*v.transpose();
					new_placement.p = v.head(3);
					new_placement.tcs[side] = v.segment(3,2);
					new_placement.metrics[side] = m[side];
				}
				return;
			}
		}
		// Finally, if both ends are free. Collapse the edge to a point on the seam.
		// And the uv on both sides should be proportional.
		VectorXd Z;
		// The unknowns are x,y,z,u0,v0,u1,v1,1
		MatrixXd G(8,8);
		G.setZero();
		// combine both sides' metric of e
		if( solver == EIQUADPROG ) {
			// build new metric
			G.block(0,0,3,3) = m[0].block(0,0,3,3) + m[1].block(0,0,3,3);
			G.block(3,0,2,3) = m[0].block(3,0,2,3);
			G.block(0,3,3,2) = m[0].block(0,3,3,2);
			G.block(3,3,2,2) = m[0].block(3,3,2,2);
			G.block(5,0,2,3) = m[1].block(3,0,2,3);
			G.block(0,5,3,2) = m[1].block(0,3,3,2);
			G.block(5,5,2,2) = m[1].block(3,3,2,2);
			RowVectorXd b(7);	// b is the linear term in combined metric
			b.segment(0,3) = m[0].block(5,0,1,3) + m[1].block(5,0,1,3);
			b.segment(3,2) = m[0].block(5,3,1,2);
			b.segment(5,2) = m[1].block(5,3,1,2);
			G.block(7,0,1,7) = b;
			G.block(0,7,7,1) = b.transpose();
			G(7,7) = m[0](5,5) + m[1](5,5);

			// regularizer
			const double w = 1e-6;
			MatrixXd reg(8,8);
			reg.setIdentity();
			G.block(0,0,8,8) = G.block(0,0,8,8) + w*reg;
			VectorXd g0(8);
			g0.setOnes();
			g0.segment(0,3) = (V.row(e_p0[0].vi)+V.row(e_p1[0].vi))/2;
			g0.segment(3,2) = (TC.row(e_p0[0].tci)+TC.row(e_p1[0].tci))/2;
			g0.segment(5,2) = (TC.row(e_p0[1].tci)+TC.row(e_p1[1].tci))/2;
			g0 = -w*g0;

			// Add the constraint that uv0 and uv1 stay on the same uv-space line
			// and the new position should have the same parameter along each edge.
			// equality constraints:
			// x(7) - 1 = 0
			// (x(3) - e0_u0) - t(e0_u1 - e0_u0) = 0
			// (x(4) - e0_v0) - t(e0_v1 - e0_v0) = 0
			// (x(5) - e1_u0) - t(e1_u1 - e1_u0) = 0
			// (x(6) - e1_v0) - t(e1_v1 - e1_v0) = 0
			RowVector2d vec[2] = {TC.row(e_p1[0].tci) - TC.row(e_p0[0].tci),
								  TC.row(e_p1[1].tci) - TC.row(e_p0[1].tci)};
			assert( vec[0].norm() != 0 && vec[1].norm() != 0 );
			MatrixXd CE(8,4);
			VectorXd ce0(4);
			CE.setZero();
			CE(7,0) = 1.0;
			ce0(0) = -1;
			if( vec[0](0) != 0 ) {		 // t = (x(3) - e0_u0)/(e0_u1 - e0_u0)
				CE(3,1) = -vec[0](1);
				CE(4,1) = vec[0](0);
				ce0(1) = vec[0](1)*TC.row(e_p0[0].tci)(0) - vec[0](0)*TC.row(e_p0[0].tci)(1);
				CE(3,2) = -vec[1](0);
				CE(5,2) = vec[0](0);
				ce0(2) = vec[1](0)*TC.row(e_p0[0].tci)(0) - vec[0](0)*TC.row(e_p0[1].tci)(0);
				CE(3,3) = -vec[1](1);
				CE(6,3) = vec[0](0);
				ce0(3) = vec[1](1)*TC.row(e_p0[0].tci)(0) - vec[0](0)*TC.row(e_p0[1].tci)(1);
			}
			else {						// t = (x(4) - e0_v0)/(e0_v1 - e0_v0)
				assert( vec[0](1) != 0 );
				CE(4,1) = -vec[0](0);
				CE(3,1) = vec[0](1);
				ce0(1) = vec[0](0)*TC.row(e_p0[0].tci)(1) - vec[0](1)*TC.row(e_p0[0].tci)(0);
				CE(4,2) = -vec[1](0);
				CE(5,2) = vec[0](1);
				ce0(2) = vec[1](0)*TC.row(e_p0[0].tci)(1) - vec[0](1)*TC.row(e_p0[1].tci)(0);
				CE(4,3) = -vec[1](1);
				CE(6,3) = vec[0](1);
				ce0(3) = vec[1](1)*TC.row(e_p0[0].tci)(1) - vec[0](1)*TC.row(e_p0[1].tci)(1);
			}

			// inequality constraints:
			// t >= 0 && t <= 1
			MatrixXd CI(8,2);
			VectorXd ci0(2);
			CI.setZero();
			if( vec[0](0) != 0 ) {
				double sign = vec[0](0) > 0 ? 1 : -1;
				CI(3,0) = sign;
				ci0(0) = -sign*TC.row(e_p0[0].tci)(0);
				CI(3,1) = -sign;
				ci0(1) = sign*(TC.row(e_p0[0].tci)(0)+vec[0](0));
			}
			else {
				double sign = vec[0](1) > 0 ? 1 : -1;
				CI(4,0) = sign;
				ci0(0) = -sign*TC.row(e_p0[0].tci)(1);
				CI(4,1) = -sign;
				ci0(1) = sign*(TC.row(e_p0[0].tci)(1)+vec[0](1));
			}

			solve_quadprog(G,g0,CE,ce0,CI,ci0,Z);
		} else {
			assert( false && "Unknown solver type" );
		}
			// set UV coordinates to be the middle point
		assert( isfinite(Z(0)) && isfinite(Z(1)) && isfinite(Z(2))
		     && isfinite(Z(3)) && isfinite(Z(4)) && isfinite(Z(5)) && isfinite(Z(6)));
		new_placement.p = Z.head(3);
		new_placement.tcs = {Z.segment(3,2), Z.segment(5,2)};
		new_placement.metrics = {m[0], m[1]};
		RowVectorXd v = Z;
		// Multiply by one half because we added two energy terms, shall we?
		cost = v*G*v.transpose();

		return;
	}

	/// case 2:
	// new metric is the summation of the collapsed vertices' metrics
	assert( e[0].p[0] == e[1].p[1] && e[0].p[1] == e[1].p[0] );
	const int tci[2] = {e[0].p[0].tci, e[0].p[1].tci};
	new_metric = Vmetrics.at(vi[0]).at(tci[0]) + Vmetrics.at(vi[1]).at(tci[1]);
	assert( new_metric.transpose() == new_metric ); // all the metrics are symmetric

	// If one vertex is on a seam, it will stay fixed. Use Q as the cost.
	for(int end=0; end<2; end++) {
		if(seam_edges.count(vi[end]) && !seam_edges.count(vi[1-end])) {
			RowVectorXd v(6);
			v.setOnes();
			v.head(3) = V.row(vi[end]);
			v.segment(3,2) = TC.row(tci[end]);
			cost = v*new_metric*v.transpose();
			new_placement.p = V.row(vi[end]);
			new_placement.tcs = { TC.row(tci[end]) };
			new_placement.metrics = { new_metric };
			return;
		}
	}

	/// case 3: no attachment to seam
	// solve
	VectorXd Z;
	if( solver == EIQUADPROG ) {
		const double w = 1e-6;
		MatrixXd G = new_metric;
		// regularizer
		MatrixXd reg(6,6);
		reg.setIdentity();
		G = G + w*reg;
		MatrixXd CE(6,1);
		CE.setZero();
		CE(5,0) = 1.0;
		VectorXd g0(6), ce0(1);
		g0.setOnes();
		g0.segment(0,3) = (V.row(vi[0])+V.row(vi[1]))/2;
		g0.segment(3,2) = (TC.row(tci[0])+TC.row(tci[1]))/2;
		g0 = -w*g0;
		ce0 << -1;
		MatrixXd CI;
		VectorXd ci0;
		solve_quadprog(G,g0,CE,ce0,CI,ci0,Z);
	} else {
		assert( false && "Unknown solver type" );
	}

	// set UV coordinates to be the middle point
	assert( isfinite(Z(0)) && isfinite(Z(1)) && isfinite(Z(2)) && isfinite(Z(3)) && isfinite(Z(4)));
	new_placement.p = Z.head(3);
	new_placement.tcs = {Z.segment(3,2)};
	new_placement.metrics = {new_metric};
	RowVectorXd v = Z;
	cost = v*new_metric*v.transpose();

}


#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include "cPathFinder.h"
using namespace boost;

void cPathFinder::read(
    const std::string &fname)
{
    std::ifstream inf(fname);
    if (!inf.is_open())
    {
        std::cout << "cannot open " << fname << "\n";
        exit(1);
    }
    std::string line;
    while (std::getline(inf, line))
    {
        std::cout << line << "\n";
        auto token = ParseSpaceDelimited(line);
        if (!token.size())
            continue;
        switch (token[0][0])
        {
        case 'l':
            if (token.size() != 4)
                throw std::runtime_error("cPathFinder::read bad link line");
            addLink(
                atoi(token[1].c_str()),
                atoi(token[2].c_str()),
                atof(token[3].c_str()));
            break;

        case 's':
            if (token.size() != 2)
                throw std::runtime_error("cPathFinder::read bad start line");
            myStart = atoi(token[1].c_str());
            break;

        case 'e':
            if (token.size() != 2)
                throw std::runtime_error("cPathFinder::read bad end line");
            myEnd = atoi(token[1].c_str());
            break;
        }
    }
}

void cPathFinder::paths(int start)
{
    // std::cout << "->cPathFinder::path " << num_vertices(myGraph)
    //     <<" " << myStart << "\n";
    // run dijkstra algorithm
    myPred.resize(num_vertices(myGraph));
    myDist.resize(num_vertices(myGraph));
    dijkstra_shortest_paths(
        myGraph,
        start,
        weight_map(get(&cEdge::myCost, myGraph))
            .predecessor_map(boost::make_iterator_property_map(
                myPred.begin(), get(boost::vertex_index, myGraph)))
            .distance_map(boost::make_iterator_property_map(
                myDist.begin(), get(boost::vertex_index, myGraph))));
    // std::cout << "<-cPathFinder::path ";
}

void cPathFinder::path()
{
    paths(myStart);
    pathPick(myEnd);
}

void cPathFinder::span()
{
    typedef graph_traits<graph_t>::edge_descriptor edge_t;
    std::vector<edge_t> spanning_tree;
    kruskal_minimum_spanning_tree(
        myGraph,
        std::back_inserter(spanning_tree),
        weight_map(get(&cEdge::myCost, myGraph)));

    std::cout << "spanning_tree " << spanning_tree.size() << "\n";
    mySpanCost = 0;
    mySpan.clear();
    for (auto e : spanning_tree)
    {
        mySpanCost += myGraph[e].myCost;
        std::vector<int> ve{
            (int)source(e, myGraph),
            (int)target(e, myGraph)};
        mySpan.push_back(ve);
    }
}

void cPathFinder::pathPick(int end)
{
    // std::cout << "->cPathFinder::pathPick "
    //     << myStart <<" " << myEnd << "\n";
    // pick out path, starting at goal and finishing at start
    myPath.push_back(end);
    int prev = end;
    while (1)
    {
        //std::cout << prev << " " << myPred[prev] << ", ";
        int next = myPred[prev];
        myPath.push_back(next);
        if (next == myStart)
            break;
        prev = next;
    }

    // reverse so path goes from start to goal
    std::reverse(myPath.begin(), myPath.end());
}

void cPathFinder::addLink(
    int u,
    int v,
    float cost)
{
    myGraph[add_edge(u, v, myGraph).first].myCost = cost;
}

std::vector<std::string> cPathFinder::ParseSpaceDelimited(
    const std::string &l)
{
    std::vector<std::string> token;
    std::stringstream sst(l);
    std::string a;
    while (getline(sst, a, ' '))
        token.push_back(a);

    token.erase(
        remove_if(
            token.begin(),
            token.end(),
            [](std::string t) {
                return (t.empty());
            }),
        token.end());

    return token;
}

int cPathFinder::findoradd(const std::string &name)
{
    int n = find(name);
    if (n < 0)
        n = add_vertex(name, myGraph);
    return n;
}

int cPathFinder::find(const std::string &name)
{
    for (int n = 0; n < num_vertices(myGraph); n++)
    {
        //std::cout << myGraph[n].myName << " ";
        if (myGraph[n].myName == name)
        {
            return n;
        }
    }
    //std::cout << name << " not found\n";
    return -1;
}

std::string cPathFinder::linksText()
{
    std::stringstream ss;
    graph_traits<graph_t>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(myGraph); ei != ei_end; ++ei)
    {
        std::string un = myGraph[source(*ei, myGraph)].myName;
        if (un == "???")
            un = std::to_string(source(*ei, myGraph));
        std::string vn = myGraph[target(*ei, myGraph)].myName;
        if (vn == "???")
            vn = std::to_string(target(*ei, myGraph));
        ss << "("
           << un << ","
           << vn << ","
           << myGraph[*ei].myCost
           << ") ";
    }
    ss << "\n";
    return ss.str();
}

std::string cPathFinder::namestring( int n )
{
        std::string sn = myGraph[n].myName;
        if (sn == "???")
            sn = std::to_string(n);
        return sn;
}

std::string cPathFinder::pathText()
{
    std::stringstream ss;
    for (auto n : myPath)
    {
        std::string sn = myGraph[n].myName;
        if (sn == "???")
            sn = std::to_string(n);
        ss << sn << " -> ";
    }
    std::cout << "\n";
    ss << "\n";
    return ss.str();
}

std::string cPathFinder::spanText()
{
    std::cout << "spanText " << mySpan.size()
        << " cost "<< mySpanCost << "\n";

    std::stringstream ss;
    for (auto e : mySpan)
    {
        ss << namestring( e[0] )
            << " - "
            << namestring( e[1] )
            << ", ";
    }
    ss << "\n";
    return ss.str();
}

std::string cPathFinder::pathViz()
{
    std::stringstream f;
    f << "graph G {\n";
    for (int v = *vertices(myGraph).first; v != *vertices(myGraph).second; ++v)
        f << std::to_string(v) << ";\n";

    graph_traits<graph_t>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(myGraph); ei != ei_end; ++ei)
    {
        int src = source(*ei, myGraph);
        int dst = target(*ei, myGraph);
        f << std::to_string(src) << "--"
          << std::to_string(dst) << " ";
        auto pathItsrc = std::find(myPath.begin(), myPath.end(), src);
        auto pathItdst = std::find(myPath.begin(), myPath.end(), dst);
        if (pathItsrc != myPath.end() && pathItdst != myPath.end())
        {
            if (pathItsrc == pathItdst + 1 || pathItsrc == pathItdst - 1)
            {
                f << "[color=\"red\"] ";
            }
        }
        f << ";\n";
    }
    f << "}\n";
    return f.str();
}

std::string cPathFinder::spanViz(bool all)
{
    std::stringstream f;
    f << "graph G {\n";
    for (int v = *vertices(myGraph).first; v != *vertices(myGraph).second; ++v) {
        auto sn = myGraph[v].myName;
        int x = atoi( sn.substr(1).c_str() );
        int y = atoi( sn.substr( sn.find("x")+1).c_str());
        f << sn
            << " [pos=\""
            << std::to_string(x)
            <<","
            << std::to_string(y)
            << "\"];\n";
    }

    graph_traits<graph_t>::edge_iterator ei, ei_end;
    for (tie(ei, ei_end) = edges(myGraph); ei != ei_end; ++ei)
    {
        auto src = myGraph[source(*ei, myGraph)].myName;
        auto dst = myGraph[target(*ei, myGraph)].myName;
        bool span = false;
        for (auto &se : mySpan)
        {
            if (src == myGraph[se[0]].myName && dst == myGraph[se[1]].myName ||
                src == myGraph[se[1]].myName && dst == myGraph[se[0]].myName)
            {
                span = true;
                break;
            }
        }
        if (all)
        {
            f << src << "--"
              << dst << " ";
            if (span)
                f << "[color=\"red\"]";
            f << ";\n";
        }
        else if (span)
        {
            f << src << "--"
              << dst
              << " [color=\"red\"];\n";
        }
    }
    f << "}\n";
    return f.str();
}


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

#include "backend/kernel_compiler/cpu/eigen/solve_triangular_cpu_kernel.h"
#define EIGEN_NO_MALLOC
#include <Eigen/Dense>
#include <vector>
#include <string>
#include <type_traits>

namespace mindspore {
namespace kernel {
using Eigen::ColMajor;
using Eigen::Dynamic;
using Eigen::Lower;
using Eigen::Map;
using Eigen::MatrixBase;
using Eigen::RowMajor;
using Eigen::UnitLower;
using Eigen::UnitUpper;
using Eigen::Upper;
template <typename T, int Major>
using Matrix = Eigen::Matrix<T, Dynamic, Dynamic, Major>;
constexpr auto kSolveTriangularInputsNum = 2;
constexpr auto kSolveTriangularOutputsNum = 1;
constexpr auto kAVectorxDimNum = 1;
constexpr auto kAMatrixDimNum = 2;
template <typename T>
void SolveTriangularCPUKernel<T>::InitKernel(const CNodePtr &kernel_node) {
  auto A_shape = AnfAlgo::GetPrevNodeOutputInferShape(kernel_node, 0);
  auto b_shape = AnfAlgo::GetPrevNodeOutputInferShape(kernel_node, 1);

  CHECK_KERNEL_INPUTS_NUM(A_shape.size(), kAMatrixDimNum, kernel_name_);
  if (A_shape[kDim0] != A_shape[kDim1]) {
    MS_LOG(EXCEPTION) << "wrong array shape, A should be a squre matrix, but got [" << A_shape[kDim0] << " X "
                      << A_shape[kDim1] << "]";
  }
  m_ = A_shape[kDim0];

  if (b_shape.size() != kAVectorxDimNum && b_shape.size() != kAMatrixDimNum) {
    MS_LOG(EXCEPTION) << "wrong array shape, b should be 1D or 2D, but got [" << b_shape.size() << "] dimensions";
  }
  if (SizeToInt(b_shape[kDim0]) != m_) {
    MS_LOG(EXCEPTION) << "wrong array shape, b should match the shape of A, excepted [" << m_ << "] but got ["
                      << b_shape[kDim0] << "]";
  }
  if (b_shape.size() == kAVectorxDimNum || (b_shape.size() == kAMatrixDimNum && b_shape[kDim1] == 1)) {
    n_ = 1;
  } else {
    n_ = b_shape[kDim1];
  }
  lower_ = AnfAlgo::GetNodeAttr<bool>(kernel_node, LOWER);
  unit_diagonal_ = AnfAlgo::GetNodeAttr<bool>(kernel_node, UNIT_DIAGONAL);
  const std::string trans = AnfAlgo::GetNodeAttr<std::string>(kernel_node, TRANS);
  if (trans == "N") {
    trans_ = false;
  } else if (trans == "T") {
    trans_ = true;
  } else {
    MS_LOG(EXCEPTION) << "trans should be in [N, T], but got [" << trans << "]";
  }
}

template <typename Derived_A, typename Derived_b, typename T>
inline void solve(const MatrixBase<Derived_A> &A, const MatrixBase<Derived_b> &b, T *output_addr, int m, int n,
                  bool lower, bool unit_diagonal) {
  Map<Matrix<T, RowMajor>> output(output_addr, m, n);
  if (unit_diagonal) {
    if (lower) {
      output.noalias() = A.template triangularView<UnitLower>().solve(b);
    } else {
      output.noalias() = A.template triangularView<UnitUpper>().solve(b);
    }
  } else {
    if (lower) {
      output.noalias() = A.template triangularView<Lower>().solve(b);
    } else {
      output.noalias() = A.template triangularView<Upper>().solve(b);
    }
  }
}

template <typename T>
bool SolveTriangularCPUKernel<T>::Launch(const std::vector<AddressPtr> &inputs,
                                         const std::vector<AddressPtr> &workspace,
                                         const std::vector<AddressPtr> &outputs) {
  CHECK_KERNEL_INPUTS_NUM(inputs.size(), kSolveTriangularInputsNum, kernel_name_);
  CHECK_KERNEL_OUTPUTS_NUM(outputs.size(), kSolveTriangularOutputsNum, kernel_name_);

  auto A_addr = reinterpret_cast<T *>(inputs[0]->addr);
  auto b_addr = reinterpret_cast<T *>(inputs[1]->addr);
  auto output_addr = reinterpret_cast<T *>(outputs[0]->addr);

  Map<Matrix<T, RowMajor>> b(b_addr, m_, n_);

  if (trans_) {
    Map<Matrix<T, ColMajor>> A(A_addr, m_, m_);
    solve(A, b, output_addr, m_, n_, !lower_, unit_diagonal_);
  } else {
    Map<Matrix<T, RowMajor>> A(A_addr, m_, m_);
    solve(A, b, output_addr, m_, n_, lower_, unit_diagonal_);
  }

  return true;
}
}  // namespace kernel
}  // namespace mindspore


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "dqs.h"
#include <Eigen/Geometry>
template <
  typename DerivedV,
  typename DerivedW,
  typename Q,
  typename QAlloc,
  typename T,
  typename DerivedU>
IGL_INLINE void igl::dqs(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedW> & W,
  const std::vector<Q,QAlloc> & vQ,
  const std::vector<T> & vT,
  Eigen::PlainObjectBase<DerivedU> & U)
{
  using namespace std;
  assert(V.rows() <= W.rows());
  assert(W.cols() == (int)vQ.size());
  assert(W.cols() == (int)vT.size());
  // resize output
  U.resizeLike(V);

  // Convert quats + trans into dual parts
  vector<Q> vD(vQ.size());
  for(int c = 0;c<W.cols();c++)
  {
    const Q & q = vQ[c];
    vD[c].w() = -0.5*( vT[c](0)*q.x() + vT[c](1)*q.y() + vT[c](2)*q.z());
    vD[c].x() =  0.5*( vT[c](0)*q.w() + vT[c](1)*q.z() - vT[c](2)*q.y());
    vD[c].y() =  0.5*(-vT[c](0)*q.z() + vT[c](1)*q.w() + vT[c](2)*q.x());
    vD[c].z() =  0.5*( vT[c](0)*q.y() - vT[c](1)*q.x() + vT[c](2)*q.w());
  }

  // Loop over vertices
  const int nv = V.rows();
#pragma omp parallel for if (nv>10000)

  for(int i = 0;i<nv;i++)
  {
    Q b0(0,0,0,0);
    Q be(0,0,0,0);
    // Loop over handles
    for(int c = 0;c<W.cols();c++)
    {
      b0.coeffs() += W(i,c) * vQ[c].coeffs();
      be.coeffs() += W(i,c) * vD[c].coeffs();
    }

    Q ce = be;
    ce.coeffs() /= b0.norm();
    Q c0 = b0;
    c0.coeffs() /= b0.norm();
    // See algorithm 1 in "Geometric skinning with approximate dual quaternion
    // blending" by Kavan et al
    T v = V.row(i);
    T d0 = c0.vec();
    T de = ce.vec();
    typename Q::Scalar a0 = c0.w();
    typename Q::Scalar ae = ce.w();
    U.row(i) =  v + 2*d0.cross(d0.cross(v) + a0*v) + 2*(a0*de - ae*d0 + d0.cross(de));
  }

}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::dqs<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Quaternion<double, 0>, Eigen::aligned_allocator<Eigen::Quaternion<double, 0> >, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, std::vector<Eigen::Quaternion<double, 0>, Eigen::aligned_allocator<Eigen::Quaternion<double, 0> > > const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


#include <iostream>
#include "gtest/gtest.h"
#include <Eigen/Dense>
#include "shmfw/variable.h"
#include "shmfw/serialization/eigen.h"
#include "shmfw/serialization/io_file.h"

namespace {

// The fixture for testing class Foo.
class EigenTest : public ::testing::Test {
protected:
    // You can remove any or all of the following functions if its body
    // is empty.

    EigenTest()
        : shmSegmentName_ ( "ShmTestSegment" )
        , shmSegmentSize_ ( 65536 ) {
        srand ( time ( NULL ) );
        // You can do set-up work for each test here.
    }

    virtual ~EigenTest() {
        // You can do clean-up work that doesn't throw exceptions here.
    }

    // If the constructor and destructor are not enough for setting up
    // and cleaning up each test, you can define the following methods:

    virtual void SetUp() {
        // Code here will be called immediately after the constructor (right
        // before each test).
    }

    virtual void TearDown() {
        // Code here will be called immediately after each test (right
        // before the destructor).
    }

    std::string shmSegmentName_;
    int shmSegmentSize_;
    double rand_01() {
        return ( double ) rand() / RAND_MAX;
    }
    double randf ( double fmin, double fmax ) {
        return rand_01() * ( fmax - fmin ) + fmin;
    }
    double randAngle() {
        return rand_01() * 2 * M_PI - M_PI;
    }
    double randf() {
        return randf ( -10, 10 );
    }
};

TEST_F ( EigenTest, ShmVector3d ) {

    std::string name ( "var0" );
    ShmFw::HandlerPtr shmHdl = ShmFw::Handler::create ( shmSegmentName_, shmSegmentSize_ );
    ShmFw::Var<Eigen::Vector3d> a ( name,shmHdl );
    ShmFw::Var<Eigen::Vector3d> b ( name,shmHdl );
    *a << rand_01(), rand_01(), rand_01();
    EXPECT_EQ ( *a, *b );
    shmHdl->removeSegment();
}
TEST_F ( EigenTest, Matrix3d ) {

    std::string name ( "var0" );
    ShmFw::HandlerPtr shmHdl = ShmFw::Handler::create ( shmSegmentName_, shmSegmentSize_ );
    ShmFw::Var<Eigen::Matrix3d> a ( name,shmHdl );
    ShmFw::Var<Eigen::Matrix3d> b ( name,shmHdl );
    *a = Eigen::Matrix3d::Random();
    EXPECT_EQ ( *a, *b );
    shmHdl->removeSegment();
}
TEST_F ( EigenTest, serializeVector3dXML ) {
    std::string filename ( "/tmp/Vector3d.xml" );
    Eigen::Vector3d a (rand_01(), rand_01(), rand_01()), b;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeMatrix3dXML ) {
    std::string filename ( "/tmp/Matrix3d.xml" );
    Eigen::Matrix3d a = Eigen::Matrix3d::Random(), b;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeVector3dBin ) {
    std::string filename ( "/tmp/Vector3d.bin" );
    Eigen::Vector3d a (rand_01(), rand_01(), rand_01()), b;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeMatrix3dBin ) {
    std::string filename ( "/tmp/Matrix3d.bin" );
    Eigen::Matrix3d a = Eigen::Matrix3d::Random(), b;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeVector3dTxt ) {
    std::string filename ( "/tmp/Vector3d.txt" );
    Eigen::Vector3d a (rand_01(), rand_01(), rand_01()), b;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeMatrix3dTxt ) {
    std::string filename ( "/tmp/Matrix3d.txt" );
    Eigen::Matrix3d a = Eigen::Matrix3d::Random(), b;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeVectorXdXML ) {
    std::string filename ( "/tmp/VectorXd.xml" );
    Eigen::VectorXd a (3), b;
    a << rand_01(), rand_01(), rand_01();;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeMatrixXdXML ) {
    std::string filename ( "/tmp/MatrixXd.xml" );
    Eigen::MatrixXd a = Eigen::MatrixXd::Random ( 3,3 ), b;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeVectorXdBin ) {
    std::string filename ( "/tmp/VectorXd.bin" );
    Eigen::VectorXd a (3), b;
    a << rand_01(), rand_01(), rand_01();;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeMatrixXdBin ) {
    std::string filename ( "/tmp/MatrixXd.bin" );
    Eigen::MatrixXd a = Eigen::MatrixXd::Random ( 3,3 ), b;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeVectorXdTxt ) {
    std::string filename ( "/tmp/VectorXd.txt" );
    Eigen::VectorXd a (3), b;
    a << rand_01(), rand_01(), rand_01();;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}
TEST_F ( EigenTest, serializeMatrixXdTxt ) {
    std::string filename ( "/tmp/MatrixXd.txt" );
    Eigen::MatrixXd a = Eigen::MatrixXd::Random ( 3,3 ), b;
    ShmFw::write(filename, a);
    ShmFw::read (filename, b);
    EXPECT_EQ ( a, b );
}

}  // namespace

int main ( int argc, char **argv ) {
    ::testing::InitGoogleTest ( &argc, argv );
    return RUN_ALL_TESTS();
}


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//!
//! @file
//! @ingroup openOR_core_math

#ifndef openOR_core_Math_determinant_hpp
#define openOR_core_Math_determinant_hpp

#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/assert.hpp>

#include <openOR/Utility/conceptcheck.hpp>

#include <openOR/Math/detail/determinant_impl.hpp>

namespace openOR {
   namespace Math {

      //----------------------------------------------------------------------------
      //! @brief
      //! @ingroup openOR_core_math
      //----------------------------------------------------------------------------
      template<class Type>
         inline
         OPENOR_CONCEPT_REQUIRES( ((Concept::ConstMatrix<Type>)),
      (typename MatrixTraits<Type>::ValueType)) determinant(const Type& mat) {

         BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
         return Impl::Determinant<Type, MatrixTraits<Type>::RowDimension::value>()(mat);
      }

   }
}

#endif


#ifndef ASLAM_BACKEND_LINESEARCH_HPP
#define ASLAM_BACKEND_LINESEARCH_HPP

/*
Most of the following is a c++ translations of code from https://github.com/scipy/scipy/blob/master/scipy/optimize/linesearch.py,
the function dcstep is based on https://github.com/scipy/scipy/blob/master/scipy/optimize/minpack2/dcstep.f,
the class Dcsrch is based on https://github.com/scipy/scipy/blob/master/scipy/optimize/minpack2/dcsrch.f .

For those parts the following license applies:

SciPy project (http://www.scipy.org/):

Copyright (c) 2001, 2002 Enthought, Inc.
All rights reserved.

Copyright (c) 2003-2016 SciPy Developers.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

  a. Redistributions of source code must retain the above copyright notice,
     this list of conditions and the following disclaimer.
  b. Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in the
     documentation and/or other materials provided with the distribution.
  c. Neither the name of Enthought nor the names of the SciPy Developers
     may be used to endorse or promote products derived from this software
     without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS
BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <boost/shared_ptr.hpp>
#include <boost/optional.hpp>
#include <boost/function.hpp>

#include <aslam/Exceptions.hpp>
#include <aslam/backend/util/CostFunctionInterface.hpp>

namespace sm {
  class PropertyTree;
}

namespace aslam {
  namespace backend {

    /**
     * Computes a safeguarded step for a search procedure and updates an interval that contains a step that satisfies a sufficient decrease and a curvature condition.
     * This method is adopted from https://github.com/funsim/moola/blob/master/moola/linesearch/dcstep.py
     * @param stx     On entry stx is the best step obtained so far and is an endpoint of the interval that contains the minimizer.
     *                On exit stx is the updated best step.
     * @param fx      On entry fx is the function at stx. On exit fx is the function at stx.
     * @param dx      On entry dx is the derivative of the function at stx. The derivative must be negative in the direction of the step, that is, dx and stp - stx must have opposite signs.
     *                On exit dx is the derivative of the function at stx.
     * @param sty     On entry sty is the second endpoint of the interval that contains the minimizer. On exit sty is the updated endpoint of the interval that contains the minimizer.
     * @param fy      On entry fy is the function at sty. On exit fy is the function at sty.
     * @param dy      On entry dy is the derivative of the function at sty. On exit dy is the derivative of the function at the exit sty.
     * @param stp     On entry stp is the current step. If brackt is set to true: on input stp must be between stx and sty. On exit stp is a new trial step.
     * @param fp      On entry fp is the function at stp.
     * @param dp      dp is the the derivative of the function at stp.
     * @param stpmin  A lower bound for the step.
     * @param stpmax  An upper bound for the step.
     */
    void dcstep(double& stx, double& fx, double& dx, double& sty, double& fy, double& dy, double& stp,
                const double fp, const double dp, bool& brackt, const double stpmin, const double stpmax);

    /**
     * Finds the minimizer for a cubic polynomial that goes through the points (a,fa), (b,fb), and (c,fc) with derivative at a of fpa.
     * f(x) = A *(x-a)^3 + B*(x-a)^2 + C*(x-a) + D
     * @return Abscissa of minimizer. If no minimizer can be found return NaN.
     */
    double cubicMin(double a, double fa, double fpa, double b, double fb, double c, double fc);

    /**
     * Finds the minimizer for a quadratic polynomial that goes through the points (a,fa), (b,fb) with derivative at a of fpa.
     * f(x) = B*(x-a)^2 + C*(x-a) + D
     * @return Abscissa of minimizer. If no minimizer can be found return NaN.
     */
    double quadMin(double a, double fa, double fpa, double b, double fb);

    /**
     * @class Dcsrch
     * Computes a safeguarded step for a search procedure and updates an interval
     * that contains a step that satisfies a sufficient decrease and a curvature
     * condition.
     */
    class Dcsrch {

     public:

      enum Status { RUNNING, CONVERGED, WARNING };

      /**
       * Constructor
       * @param stepLengthInit
       * @param error Error at stepLengthInit
       * @param errorDerivative Error derivative at stepLengthInit
       * @param minStepLength Minimum allowed step length
       * @param maxStepLength Maximum allowed step length
       * @param ftol A nonnegative value for the sufficient decrease condition
       * @param xtol A nonnegative relative tolerance for an acceptable step.
       * @param gtol A nonnegative value for the curvature condition
       */
      Dcsrch(double stepLengthInit, double error, double errorDerivative, double minStepLength, double maxStepLength, double ftol, double xtol, double gtol);

      /**
       * Finds a step that satisfies a sufficient decrease condition and a curvature condition.
       * https://github.com/funsim/moola/blob/master/moola/linesearch/dcsrch.py
       * @param stepLength     On entry stepLength is the current estimate of a satisfactory step. On initial entry, a positive initial estimate must be provided.
       *                On exit stp is the current estimate of a satisfactory step if task = 'FG'. If task = 'CONV' then stp satisfies the sufficient decrease and curvature condition.
       * @param f       On initial entry f is the value of the function at 0. On subsequent entries f is the value of the function at stp.
       * @param g       On initial entry g is the derivative of the function at 0. On subsequent entries g is the derivative of the function at stp.
       */
      double updateStepLength(double error, double errorDerivative);

      /// \brief Getter for the status of the search
      Status status() const { return _status; }

     private:

      bool _brackt = false; /// \brief Specifies if a minimizer has been bracketed.
      Status _status = RUNNING; /// \brief The status of the search
      std::size_t _stage = 0; /// \brief  The stage of the search

      /**
       *  \brief A nonnegative relative tolerance for an acceptable step. The
       *  algorithm exits with a warning if the relative difference between sty
       *  and stx is less than xtol.
       */
      const double _xtol;

      const double _ftol; /// \brief A nonnegative tolerance for the sufficient decrease condition.
      const double _gtol;/// \brief A nonnegative tolerance for the curvature condition.
      const double _minStepLength;/// \brief A nonnegative lower bound for the step length.
      const double _maxStepLength;/// \brief A nonnegative upper bound for the step length.
      const double _xtrapl = 1.1;
      const double _xtrapu = 4.0;

      /**
       * \brief The current estimate of a satisfactory step. If _status is
       * CONVERGED, then _stepLength satisfies the sufficient decrease and
       * curvature condition.
       */
      double _stepLength;

      const double _finit;  /// \brief Initial function (error) value
      double _fx;           /// \brief Function (error) value at point x
      double _fy;           /// \brief Function (error) value at point y
      const double _ginit;  /// \brief Initial function (error) derivative
      const double _gtest;
      const double _ginitTimesNegGtol; /// \brief Helper variable to save computation time
      double _gx;           /// \brief Function (error) derivative at point x
      double _gy;           /// \brief Function (error) derivative at point y
      double _stx = 0.0;    /// \brief The best step obtained so far. stx is an endpoint of the interval that contains the minimizer.
      double _sty = 0.0;    /// \brief The second endpoint of the interval that contains the minimizer.
      double _stmin = 0.0;  /// \brief Interval point x
      double _stmax;        /// \brief Interval point y
      double _width;        /// \brief Width of the interval
      double _width1;       /// \brief Width of the interval
    };

    struct LineSearchOptions {
      LineSearchOptions();
      LineSearchOptions(const sm::PropertyTree& config);
      void check() const;

      double c1WolfeCondition = 1e-4; /// \brief A nonnegative tolerance for the sufficient decrease condition
      double c2WolfeCondition = 0.9;  /// \brief A nonnegative tolerance for the curvature condition
      double maxStepLength = 50;      /// \brief A nonnegative upper bound for the step.
      double minStepLength = 1e-8;    /// \brief A nonnegative lower bound for the step.
      double xtol = 1e-14;            /// \brief A nonnegative relative tolerance for an acceptable step.
      double initialStepLength = 1.0; /// \brief A nonnegative initial step length.
      std::size_t nMaxIterWolfe1 = 30; /// \brief Maximum number of iterations for method wolfe1
      std::size_t nMaxIterWolfe2 = 10; /// \brief Maximum number of iterations for method wolfe2
      std::size_t nMaxIterZoom = 10;   /// \brief Maximum number of iterations for the internal zoom method

      template<class Archive>
      inline void serialize(Archive & ar, const unsigned int version);
    };

    std::ostream& operator<<(std::ostream& out, const aslam::backend::LineSearchOptions& options);

    /**
     * \class LineSearch
     *
     * Various line-search methods
     */
    class LineSearch {

    public:// public methods

      typedef boost::shared_ptr<LineSearch> Ptr;
      typedef boost::shared_ptr<const LineSearch> ConstPtr;

      /// \brief Constructor with default options
      LineSearch(const boost::shared_ptr<CostFunctionInterface>& cf);
      /// \brief Constructor with custom options
      LineSearch(const boost::shared_ptr<CostFunctionInterface>& cf, const LineSearchOptions& options);
      /// \brief Constructor from property tree
      LineSearch(const boost::shared_ptr<CostFunctionInterface>& cf, const sm::PropertyTree& config);
      /// \brief Destructor
      ~LineSearch();

      /// \brief Mutable getter for the options.
      LineSearchOptions& options() { return _options; }

      /// \brief Const getter for the options.
      const LineSearchOptions& options() const { return _options; }

      /**
       * @brief Initialize the class. Mandatory to call before using any of the line-search methods.
       * Do not call initialize() in case you perform subsequent line-searches always starting at the endpoint of the previous search but
       * rather use setSearchDirection() in case the search direction has to be modified.
       * @param searchDirection Direction of the search. Often you want to use -gradient. You may pass it later via setSearchDirection().
       * @param error Optionally specify the error at the current point. If not supplied, initialize() will recompute it.
       * @param gradient Optionally specify the gradient at the current point. If not supplied, initialize() will recompute it.
       */
      void initialize(boost::optional<const RowVectorType&> searchDirection = boost::optional<const RowVectorType&>(),
                      boost::optional<double> error = boost::optional<double>(),
                      boost::optional<const RowVectorType&> gradient = boost::optional<const RowVectorType&>());

      /**
       * Modifies the current search direction
       * @param searchDirection New search direction
       */
      void setSearchDirection(const RowVectorType& searchDirection);

      /**
       * Set a callback when the error is evaluated
       */
      inline void setEvaluateErrorCallback(const boost::function<void(void)>& cb);

      /**
       * Set a callback when the gradient is evaluated
       */
      inline void setEvaluateGradientCallback(const boost::function<void(void)>& cb);

      /**
       * Search for a step length that satisfies strong Wolfe conditions
       * @return Successful or not
       */
      bool lineSearchWolfe1();

      /**
       * Search for a step length that satisfies strong Wolfe conditions.
       * Uses the line search algorithm to enforce strong Wolfe conditions. See Wright and Nocedal, 'Numerical Optimization', 1999, pg. 59-60.
       * Adapted from scipy https://github.com/scipy/scipy/blob/master/scipy/optimize/linesearch.py#L296
       * @return Successful or not
       */
      bool lineSearchWolfe2();

      /**
       * Search for a step length that satisfies strong Wolfe conditions
       * Same as lineSearchWolfe1, but fall back to lineSearchWolfe2 if suitable step length is not found.
       * @return Successful or not
       */
      bool lineSearchWolfe12();

      /**
       * Updates the state with a step of length s and the related error and gradient information at the new location
       * @param s step length
       * @param gradient Whether or not to update gradient related information.
       */
      void applyStateUpdate(const double s);

      /**
       * Updates the error-related information of the class
       */
      void updateError();

      /**
       * Updates the gradient at the current state
       */
      void updateGradient();

      /**
       * Updates the gradient-related information of the class
       */
      void updateErrorDerivative();

      /**
       * Computes the derivative of the error in the search direction
       * @return derivative of the error in the search direction
       */
      double computeErrorDerivative() const;

      /**
       * Return the error at the current point, checking whether the information is up to date
       * @return error at the current point
       */
      inline double getError() const;

      /**
       * Return the derivative of the error in the search direction at the current point, checking whether the information is up to date
       * @return derivative of the error in the search direction at the current point
       */
      inline double getErrorDerivative() const;

      /**
       * Return the gradient at the current point
       * @return gradient at the current point
       */
      inline const RowVectorType& getGradient() const;

      /**
       * Getter for the current step length
       */
      inline double getCurrentStepLength() const;

    private: // private methods

      /**
       * Part of the optimization algorithm in scalarSearchWolfe2.
       */
      bool zoom(double minStepLength, double maxStepLength, double error_lo, double error_hi, double derror_lo, double error0, double derror0);

    private: // private members

      /// \brief Cost function
      boost::shared_ptr<CostFunctionInterface> _costFunction;

      /// \brief current search direction
      RowVectorType _searchDirection;

      /// \brief gradient at current point
      RowVectorType _gradient;

      /// \brief current step length
      double _stepLength = 0.0;

      /// \brief Error at the current point xk
      double _error = std::numeric_limits<double>::signaling_NaN();

      /// \brief Error at the previous step xk-1, i.e. beginning of last line search
      double _errorOld = std::numeric_limits<double>::signaling_NaN();

      /// \brief Value of the gradient of the error in the search direction at the current point
      double _derror = std::numeric_limits<double>::signaling_NaN();

      /// \brief Whether an update of the error-related information is neccesary
      bool _errorOutdated = true;

      /// \brief Whether an update of the gradient-related information is neccesary
      bool _derrorOutdated = true;

      /// \brief Callback  that is called when the objective function is evaluated
      boost::function<void(void)> _evalErrorCallback;

      /// \brief Callback  that is called when the gradient is evaluated
      boost::function<void(void)> _evalGradCallback;

      /// \brief the current set of options
      LineSearchOptions _options;

    };

    // Inlined methods

    inline void LineSearch::setEvaluateErrorCallback(const boost::function<void(void)>& cb) {
      _evalErrorCallback = cb;
    }

    inline void LineSearch::setEvaluateGradientCallback(const boost::function<void(void)>& cb) {
      _evalGradCallback = cb;
    }

    inline double LineSearch::getError() const {
      SM_ASSERT_FALSE(Exception, _errorOutdated, "Missing call to updateError()");
      return _error;
    }
    inline double LineSearch::getErrorDerivative() const {
      SM_ASSERT_FALSE(Exception, _derrorOutdated, "Missing call to updateErrorDerivative()");
      return _derror;
    }
    inline const RowVectorType& LineSearch::getGradient() const {
      return _gradient;
    }
    inline double LineSearch::getCurrentStepLength() const {
      return _stepLength;
    }

  } // namespace backend
} // namespace aslam

#include "implementation/LineSearchImpl.hpp"

#endif /* ASLAM_BACKEND_LINESEARCH_HPP */


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern feature spatial_domain
#include <cassert>
#include <boost/test/unit_test.hpp>
#include "fern/feature/core/point.h"
#include "fern/language/feature/core/box.h"
#include "fern/language/feature/core/spatial_domain.h"

namespace fl = fern::language;

template<
    class CoordinateType,
    size_t nr_dimensions,
    class CoordinateSystem>
inline bool operator==(
    fern::Point<CoordinateType, nr_dimensions,
        CoordinateSystem> const& lhs,
    fern::Point<CoordinateType, nr_dimensions,
        CoordinateSystem> const& rhs)
{
    return
        fern::get<0>(lhs) == fern::get<0>(rhs) &&
        fern::get<1>(lhs) == fern::get<1>(rhs);
}

template<
    class Point>
inline bool operator==(
    fl::Box<Point> const& lhs,
    fl::Box<Point> const& rhs)
{
    return
        lhs.min_corner() == rhs.min_corner() &&
        lhs.max_corner() == rhs.max_corner();
}

BOOST_AUTO_TEST_CASE(spatial_domain)
{
    using Point = fern::Point<double, 2>;
    using Box = fl::Box<Point>;
    using BoxDomain = fl::SpatialDomain<Box>;

    BoxDomain spatial_domain;

    Point south_west;
    Point north_east;
    fern::set<0>(south_west, 1.1);
    fern::set<1>(south_west, 2.2);
    fern::set<0>(north_east, 3.3);
    fern::set<1>(north_east, 4.4);
    Box box1(south_west, north_east);
    Box box2(south_west, north_east);

    // Whatever the geometry, adding one to a domain should result in a
    // unique geometry id.
    BoxDomain::GID gid1 = spatial_domain.add(box1);
    BoxDomain::GID gid2 = spatial_domain.add(box2);
    BOOST_CHECK_NE(gid1, gid2);

    // Test geometries themselves.
    BOOST_CHECK(spatial_domain.geometry(gid1) == box1);
    BOOST_CHECK(spatial_domain.geometry(gid2) == box2);
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN set_union tests

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cmath>
#include <functional>
#include <iterator>
#include <list>
#include <vector>

BOOST_AUTO_TEST_SUITE( set_union_test_suite )

BOOST_AUTO_TEST_CASE( empty_primary_input_default_compare )
{
    const std::vector<int> primary;
    const std::list<int> secondary{1, 2, 3};
    std::vector<int> output;

    std::set_union(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output));

    const std::vector<int> expected{1, 2, 3};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( empty_primary_input_custom_compare )
{
    const std::vector<int> primary;
    const std::list<int> secondary{3, 2, 1};
    std::vector<int> output;

    std::set_union(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), std::greater<int>());

    const std::vector<int> expected{3, 2, 1};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( empty_primary_input_conversion_compare )
{
    const std::vector<int> primary;
    const std::list<int> secondary{1, 2, 3};
    std::vector<int> output;

    std::set_union(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), std::less<double>());

    const std::vector<int> expected{1, 2, 3};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( same_type_inputs_default_compare )
{
    const std::vector<int> primary{1, 3, 4, 5, 6};
    const std::list<int> secondary{1, 2, 3};
    std::vector<int> output;

    std::set_union(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output));

    const std::vector<int> expected{1, 2, 3, 4, 5, 6};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( same_type_inputs_custom_compare )
{
    const std::vector<int> primary{1, -3, -4, 5, -6};
    const std::list<int> secondary{1, -2, 3};
    std::vector<int> output;

    auto comp = [](int lhs, int rhs){ return std::less<int>()(std::abs(lhs), std::abs(rhs)); };
    std::set_union(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), comp);

    const std::vector<int> expected{1, -2, -3, -4, 5, -6};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( same_type_inputs_conversion_compare )
{
    const std::vector<int> primary{1, -3, -4, 5, -6};
    const std::list<int> secondary{1, -2, 3};
    std::vector<int> output;

    auto comp = [](double lhs, double rhs){ return std::less<double>()(std::abs(lhs), std::abs(rhs)); };
    std::set_union(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), comp);

    const std::vector<int> expected{1, -2, -3, -4, 5, -6};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( different_type_inputs_default_compare )
{
    const std::vector<int> primary{1, 3, 4, 5, 6};
    const std::list<double> secondary{1.1, 2, 4.0, 4.6};
    std::vector<int> output;

    /*! For the default compare between an int and a double, the int will be
     * promoted to double first, so for example 1 will be considered different
     * to 1.1, in which case 1.1 will be reported to the output iterator. As the
     * output iterator receives ints, the value 1.1 will be implicitly cast to
     * int, which means a second "1" will appear in the output collection.
     * Similarly, 4 will be considered equal to 4.0 but different to 4.6, and
     * two "4" values will be reported. */
    std::set_union(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output));

    const std::vector<int> expected{1, 1, 2, 3, 4, 4, 5, 6};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( different_type_inputs_custom_compare )
{
    const std::vector<int> primary{1, 3, 4, 5, 6};
    const std::list<double> secondary{1.1, 2, 3.9, 4.6};
    std::vector<int> output;

    auto comp = [](int lhs, double rhs){ return std::less<double>()(static_cast<double>(lhs), std::floor(rhs)); };
    std::set_union(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output), comp);

    const std::vector<int> expected{1, 2, 3, 4, 5, 6};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_CASE( repeated_elements_default_compare )
{
    const std::vector<int> primary{1, 1, 1, 1, 2, 3, 3, 3, 3, 4, 4, 5, 6};
    const std::list<int> secondary{1, 1, 2, 3};
    std::vector<int> output;

    std::set_union(std::begin(primary), std::end(primary), std::begin(secondary), std::end(secondary), std::back_inserter(output));

    const std::vector<int> expected{1, 1, 1, 1, 2, 3, 3, 3, 3, 4, 4, 5, 6};
    BOOST_CHECK_EQUAL_COLLECTIONS(std::begin(output), std::end(output), std::begin(expected), std::end(expected));
}

BOOST_AUTO_TEST_SUITE_END( /* set_union_test_suite */ )