//  Copyright (c) 2006 Xiaogang Zhang
//  Copyright (c) 2006 John Maddock
//  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)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to correctly
//  handle the various corner cases.
//

#ifndef BOOST_MATH_ELLINT_3_HPP
#define BOOST_MATH_ELLINT_3_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/ellint_rf.hpp>
#include <boost/math/special_functions/ellint_rj.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/atanh.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/workaround.hpp>
#include <boost/math/special_functions/round.hpp>

// Elliptic integrals (complete and incomplete) of the third kind
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math {

namespace detail{

template <typename T, typename Policy>
T ellint_pi_imp(T v, T k, T vc, const Policy& pol);

// Elliptic integral (Legendre form) of the third kind
template <typename T, typename Policy>
T ellint_pi_imp(T v, T phi, T k, T vc, const Policy& pol)
{
   // Note vc = 1-v presumably without cancellation error.
   BOOST_MATH_STD_USING

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

   T sphi = sin(fabs(phi));
   T result = 0;

   if (k * k * sphi * sphi > 1)
   {
      return policies::raise_domain_error<T>(function,
         "Got k = %1%, function requires |k| <= 1", k, pol);
   }
   // Special cases first:
   if(v == 0)
   {
      // A&S 17.7.18 & 19
      return (k == 0) ? phi : ellint_f_imp(phi, k, pol);
   }
   if((v > 0) && (1 / v < (sphi * sphi)))
   {
      // Complex result is a domain error:
      return policies::raise_domain_error<T>(function,
         "Got v = %1%, but result is complex for v > 1 / sin^2(phi)", v, pol);
   }

   if(v == 1)
   {
      if (k == 0)
         return tan(phi);

      // http://functions.wolfram.com/08.06.03.0008.01
      T m = k * k;
      result = sqrt(1 - m * sphi * sphi) * tan(phi) - ellint_e_imp(phi, k, pol);
      result /= 1 - m;
      result += ellint_f_imp(phi, k, pol);
      return result;
   }
   if(phi == constants::half_pi<T>())
   {
      // Have to filter this case out before the next
      // special case, otherwise we might get an infinity from
      // tan(phi).
      // Also note that since we can't represent PI/2 exactly
      // in a T, this is a bit of a guess as to the users true
      // intent...
      //
      return ellint_pi_imp(v, k, vc, pol);
   }
   if((phi > constants::half_pi<T>()) || (phi < 0))
   {
      // Carlson's algorithm works only for |phi| <= pi/2,
      // use the integrand's periodicity to normalize phi
      //
      // Xiaogang's original code used a cast to long long here
      // but that fails if T has more digits than a long long,
      // so rewritten to use fmod instead:
      //
      // See http://functions.wolfram.com/08.06.16.0002.01
      //
      if(fabs(phi) > 1 / tools::epsilon<T>())
      {
         if(v > 1)
            return policies::raise_domain_error<T>(
            function,
            "Got v = %1%, but this is only supported for 0 <= phi <= pi/2", v, pol);
         //
         // Phi is so large that phi%pi is necessarily zero (or garbage),
         // just return the second part of the duplication formula:
         //
         result = 2 * fabs(phi) * ellint_pi_imp(v, k, vc, pol) / constants::pi<T>();
      }
      else
      {
         T rphi = boost::math::tools::fmod_workaround(T(fabs(phi)), T(constants::half_pi<T>()));
         T m = boost::math::round((fabs(phi) - rphi) / constants::half_pi<T>());
         int sign = 1;
         if((m != 0) && (k >= 1))
         {
            return policies::raise_domain_error<T>(function, "Got k=1 and phi=%1% but the result is complex in that domain", phi, pol);
         }
         if(boost::math::tools::fmod_workaround(m, T(2)) > 0.5)
         {
            m += 1;
            sign = -1;
            rphi = constants::half_pi<T>() - rphi;
         }
         result = sign * ellint_pi_imp(v, rphi, k, vc, pol);
         if((m > 0) && (vc > 0))
            result += m * ellint_pi_imp(v, k, vc, pol);
      }
      return phi < 0 ? T(-result) : result;
   }
   if(k == 0)
   {
      // A&S 17.7.20:
      if(v < 1)
      {
         T vcr = sqrt(vc);
         return atan(vcr * tan(phi)) / vcr;
      }
      else
      {
         // v > 1:
         T vcr = sqrt(-vc);
         T arg = vcr * tan(phi);
         return (boost::math::log1p(arg, pol) - boost::math::log1p(-arg, pol)) / (2 * vcr);
      }
   }
   if((v < 0) && fabs(k) <= 1)
   {
      //
      // If we don't shift to 0 <= v <= 1 we get
      // cancellation errors later on.  Use
      // A&S 17.7.15/16 to shift to v > 0.
      //
      // Mathematica simplifies the expressions
      // given in A&S as follows (with thanks to
      // Rocco Romeo for figuring these out!):
      //
      // V = (k2 - n)/(1 - n)
      // Assuming[(k2 >= 0 && k2 <= 1) && n < 0, FullSimplify[Sqrt[(1 - V)*(1 - k2 / V)] / Sqrt[((1 - n)*(1 - k2 / n))]]]
      // Result: ((-1 + k2) n) / ((-1 + n) (-k2 + n))
      //
      // Assuming[(k2 >= 0 && k2 <= 1) && n < 0, FullSimplify[k2 / (Sqrt[-n*(k2 - n) / (1 - n)] * Sqrt[(1 - n)*(1 - k2 / n)])]]
      // Result : k2 / (k2 - n)
      //
      // Assuming[(k2 >= 0 && k2 <= 1) && n < 0, FullSimplify[Sqrt[1 / ((1 - n)*(1 - k2 / n))]]]
      // Result : Sqrt[n / ((k2 - n) (-1 + n))]
      //
      T k2 = k * k;
      T N = (k2 - v) / (1 - v);
      T Nm1 = (1 - k2) / (1 - v);
      T p2 = -v * N;
      T t;
      if(p2 <= tools::min_value<T>())
         p2 = sqrt(-v) * sqrt(N);
      else
         p2 = sqrt(p2);
      T delta = sqrt(1 - k2 * sphi * sphi);
      if(N > k2)
      {
         result = ellint_pi_imp(N, phi, k, Nm1, pol);
         result *= v / (v - 1);
         result *= (k2 - 1) / (v - k2);
      }

      if(k != 0)
      {
         t = ellint_f_imp(phi, k, pol);
         t *= k2 / (k2 - v);
         result += t;
      }
      t = v / ((k2 - v) * (v - 1));
      if(t > tools::min_value<T>())
      {
         result += atan((p2 / 2) * sin(2 * phi) / delta) * sqrt(t);
      }
      else
      {
         result += atan((p2 / 2) * sin(2 * phi) / delta) * sqrt(fabs(1 / (k2 - v))) * sqrt(fabs(v / (v - 1)));
      }
      return result;
   }
   if(k == 1)
   {
      // See http://functions.wolfram.com/08.06.03.0013.01
      result = sqrt(v) * atanh(sqrt(v) * sin(phi)) - log(1 / cos(phi) + tan(phi));
      result /= v - 1;
      return result;
   }
#if 0  // disabled but retained for future reference: see below.
   if(v > 1)
   {
      //
      // If v > 1 we can use the identity in A&S 17.7.7/8
      // to shift to 0 <= v <= 1.  In contrast to previous
      // revisions of this header, this identity does now work
      // but appears not to produce better error rates in
      // practice.  Archived here for future reference...
      //
      T k2 = k * k;
      T N = k2 / v;
      T Nm1 = (v - k2) / v;
      T p1 = sqrt((-vc) * (1 - k2 / v));
      T delta = sqrt(1 - k2 * sphi * sphi);
      //
      // These next two terms have a large amount of cancellation
      // so it's not clear if this relation is useable even if
      // the issues with phi > pi/2 can be fixed:
      //
      result = -ellint_pi_imp(N, phi, k, Nm1, pol);
      result += ellint_f_imp(phi, k, pol);
      //
      // This log term gives the complex result when
      //     n > 1/sin^2(phi)
      // However that case is dealt with as an error above,
      // so we should always get a real result here:
      //
      result += log((delta + p1 * tan(phi)) / (delta - p1 * tan(phi))) / (2 * p1);
      return result;
   }
#endif
   //
   // Carlson's algorithm works only for |phi| <= pi/2,
   // by the time we get here phi should already have been
   // normalised above.
   //
   BOOST_ASSERT(fabs(phi) < constants::half_pi<T>());
   BOOST_ASSERT(phi >= 0);
   T x, y, z, p, t;
   T cosp = cos(phi);
   x = cosp * cosp;
   t = sphi * sphi;
   y = 1 - k * k * t;
   z = 1;
   if(v * t < 0.5)
      p = 1 - v * t;
   else
      p = x + vc * t;
   result = sphi * (ellint_rf_imp(x, y, z, pol) + v * t * ellint_rj_imp(x, y, z, p, pol) / 3);

   return result;
}

// Complete elliptic integral (Legendre form) of the third kind
template <typename T, typename Policy>
T ellint_pi_imp(T v, T k, T vc, const Policy& pol)
{
    // Note arg vc = 1-v, possibly without cancellation errors
    BOOST_MATH_STD_USING
    using namespace boost::math::tools;

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

    if (abs(k) >= 1)
    {
       return policies::raise_domain_error<T>(function,
            "Got k = %1%, function requires |k| <= 1", k, pol);
    }
    if(vc <= 0)
    {
       // Result is complex:
       return policies::raise_domain_error<T>(function,
            "Got v = %1%, function requires v < 1", v, pol);
    }

    if(v == 0)
    {
       return (k == 0) ? boost::math::constants::pi<T>() / 2 : ellint_k_imp(k, pol);
    }

    if(v < 0)
    {
       // Apply A&S 17.7.17:
       T k2 = k * k;
       T N = (k2 - v) / (1 - v);
       T Nm1 = (1 - k2) / (1 - v);
       T result = 0;
       result = boost::math::detail::ellint_pi_imp(N, k, Nm1, pol);
       // This next part is split in two to avoid spurious over/underflow:
       result *= -v / (1 - v);
       result *= (1 - k2) / (k2 - v);
       result += ellint_k_imp(k, pol) * k2 / (k2 - v);
       return result;
    }

    T x = 0;
    T y = 1 - k * k;
    T z = 1;
    T p = vc;
    T value = ellint_rf_imp(x, y, z, pol) + v * ellint_rj_imp(x, y, z, p, pol) / 3;

    return value;
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type ellint_3(T1 k, T2 v, T3 phi, const boost::false_type&)
{
   return boost::math::ellint_3(k, v, phi, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type ellint_3(T1 k, T2 v, const Policy& pol, const boost::true_type&)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_pi_imp(
         static_cast<value_type>(v),
         static_cast<value_type>(k),
         static_cast<value_type>(1-v),
         pol), "boost::math::ellint_3<%1%>(%1%,%1%)");
}

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type ellint_3(T1 k, T2 v, T3 phi, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_pi_imp(
         static_cast<value_type>(v),
         static_cast<value_type>(phi),
         static_cast<value_type>(k),
         static_cast<value_type>(1-v),
         pol), "boost::math::ellint_3<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
typename detail::ellint_3_result<T1, T2, T3>::type ellint_3(T1 k, T2 v, T3 phi)
{
   typedef typename policies::is_policy<T3>::type tag_type;
   return detail::ellint_3(k, v, phi, tag_type());
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type ellint_3(T1 k, T2 v)
{
   return ellint_3(k, v, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_3_HPP


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/literal.hpp>
#include <fcppt/no_init.hpp>
#include <fcppt/algorithm/fold.hpp>
#include <fcppt/cast/size.hpp>
#include <fcppt/cast/to_signed.hpp>
#include <fcppt/container/grid/fill.hpp>
#include <fcppt/container/grid/object.hpp>
#include <fcppt/math/vector/comparison.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <functional>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	container_grid_fill
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::container::grid::object<
		int,
		2
	>
	int2_grid;

	int2_grid test(
		int2_grid::dim(
			2u,
			2u
		),
		fcppt::no_init{}
	);

	typedef
	int2_grid::pos
	pos;

	fcppt::container::grid::fill(
		test,
		[](
			pos const _pos
		)
		{
			return
				fcppt::cast::size<
					int
				>(
					fcppt::cast::to_signed(
						fcppt::algorithm::fold(
							_pos.storage(),
							fcppt::literal<
								pos::value_type
							>(
								0
							),
							std::plus<
								pos::value_type
							>()
						)
					)
				);
		}
	);

	BOOST_CHECK_EQUAL(
		test[
			pos(
				0u,
				0u
			)
		],
		0
	);

	BOOST_CHECK_EQUAL(
		test[
			pos(
				1u,
				0u
			)
		],
		1
	);

	BOOST_CHECK_EQUAL(
		test[
			pos(
				0u,
				1u
			)
		],
		1
	);

	BOOST_CHECK_EQUAL(
		test[
			pos(
				1u,
				1u
			)
		],
		2
	);
}


//
// Copyright 2019 Mateusz Loskot <mateusz at loskot dot net>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>

#include <tuple>
#include <type_traits>

#define BOOST_TEST_MODULE test_ext_numeric_colvolve_cols
#include "unit_test.hpp"
#include "unit_test_utility.hpp"
#include "test_fixture.hpp"
#include "core/image/test_fixture.hpp"

namespace gil = boost::gil;
namespace fixture = boost::gil::test::fixture;

BOOST_AUTO_TEST_SUITE(convolve_cols)

BOOST_AUTO_TEST_CASE_TEMPLATE(image_1x1_kernel_1x1_identity, Image, fixture::image_types)
{
    auto const img = fixture::create_image<Image>(1, 1, 7);
    auto img_out = fixture::create_image<Image>(1, 1, 0);

    using pixel_t = typename Image::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const kernel = fixture::create_kernel<channel_t>({1});
    gil::convolve_cols<pixel_t>(const_view(img), kernel, view(img_out));

    // 1x1 kernel reduces convolution to multiplication
    BOOST_TEST(gil::const_view(img).front() == gil::const_view(img_out).front());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(image_1x1_kernel_3x3_identity, Image, fixture::image_types)
{
    auto const img = fixture::create_image<Image>(1, 1, 7);
    auto img_out = fixture::create_image<Image>(1, 1, 0);

    using pixel_t = typename Image::value_type;
    using channel_t = typename gil::channel_type<pixel_t>::type;
    auto const kernel = fixture::create_kernel<channel_t>({0, 0, 0, 0, 1, 0, 0, 0, 0});
    gil::convolve_cols<pixel_t>(const_view(img), kernel, view(img_out));

    BOOST_TEST(gil::const_view(img).front() == gil::const_view(img_out).front());
}

BOOST_AUTO_TEST_SUITE_END()


/*****************************************************************************
 *
 * ALPS DMFT Project
 *
 * Copyright (C) 2005 - 2009 by Emanuel Gull <gull@phys.columbia.edu>
 *                              Philipp Werner <werner@itp.phys.ethz.ch>,
 *                              Sebastian Fuchs <fuchs@theorie.physik.uni-goettingen.de>
 *                              Matthias Troyer <troyer@comp-phys.org>
 *
 *
 * This software is part of the ALPS Applications, published under the ALPS
 * Application License; you can use, redistribute it and/or modify it under
 * the terms of the license, either version 1 or (at your option) any later
 * version.
 *
 * You should have received a copy of the ALPS Application License along with
 * the ALPS Applications; see the file LICENSE.txt. If not, the license is also
 * available from http://alps.comp-phys.org/.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
 * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 *
 *****************************************************************************/

#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/bindings/lapack/driver/gesv.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <math.h>
#include "fouriertransform.h"

using namespace Eigen;

typedef boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> dense_matrix;

FourierTransformer::FourierTransformer(const double beta,
                                       const VectorXd &ftail)
    : beta_(beta), c_(ftail) {
  if (c_.size() < 3)
    throw std::length_error("The tail should contains up to at least the third order");
}

// backward_ft G(iwn)->G(tau) works for cluster Green's functions
void FourierTransformer::backward_ft(const VectorXcd &G_omega,
                                     VectorXd &G_tau, const int N_tau) const {
  unsigned int N_omega = G_omega.size();
  VectorXcd G_omega_no_model(G_omega);
  double dt = beta_/N_tau;

  G_tau.resize(N_tau+1);
  if (c_(0) == 0 && c_(1) == 0 && c_(2) == 0) {
    //nothing happening in this gf.
    for (int i = 0; i <= N_tau; i++) {
      G_tau(i)=0.;
    }
  } else {
    for (int k = 0; k < N_omega; k++) {
      std::complex<double> iw(0,(2*k+1)*M_PI/beta_);
      G_omega_no_model(k) -= f_omega(iw, c_(0),c_(1), c_(2));
    }
    for (int i=0; i<N_tau; i++) {
      G_tau(i) = f_tau(i*dt, beta_, c_(0), c_(1), c_(2));
      for (int k=0; k<N_omega; k++) {
        double wt((2*k+1)*i*M_PI/N_tau);
        G_tau(i) += 2/beta_*(cos(wt)*G_omega_no_model(k).real()+
                             sin(wt)*G_omega_no_model(k).imag());
      }
    }
    G_tau(N_tau) = -c_[0];
    G_tau(N_tau)-= G_tau(0);
  }
}

void generate_spline_matrix(dense_matrix & spline_matrix, double dt) {
  // spline_matrix has dimension (N+1)x(N+1)
  int Np1 = spline_matrix.size1();
  //std::cout<<"spline matrix size is: "<<Np1<<std::endl;
  // A is the matrix whose inverse defines spline_matrix
  //
  //      6                   6
  //      1  4  1
  //         1  4  1
  // A =        ...
  //
  //                    1  4  1
  //     -2 -1     0       1  2
  spline_matrix.clear();
  dense_matrix A = 4*dt/6.*boost::numeric::ublas::identity_matrix<double>(Np1);

  for (int i=1; i<Np1-1; i++) {
    A(i,i-1) = dt/6.;
    A(i,i+1) = dt/6.;
  }
  A(0,0) = 1.;
  A(0, Np1-1) = 1.;
  A(Np1-1, 0) = -2.*dt/6.;
  A(Np1-1, 1) = -1.*dt/6.;
  A(Np1-1, Np1-2) = 1*dt/6.;
  A(Np1-1, Np1-1) = 2*dt/6.;

  // solve A*spline_matrix=I
  // gesv solves A*X=B, input for B is I, output (=solution X) is spline_matrix
  spline_matrix = boost::numeric::ublas::identity_matrix<double>(Np1);
  boost::numeric::ublas::vector<fortran_int_t> ipivot(A.size1());
  boost::numeric::bindings::lapack::gesv(A, ipivot,spline_matrix);
}

void evaluate_second_derivatives(double dt, dense_matrix & spline_matrix,
    std::vector<double> & g, std::vector<double> & second_derivatives,
    const double c1g, const double c2g, const double c3g) {
  // g, rhs and second_derivatives have dimension N+1
  int Np1 = spline_matrix.size1();
  //assert(c1g==1);
  // rhs is the vector containing the data of the curve y = g(tau), which allows to
  // compute the vector of second derivatives y'' at times tau_n by evaluating
  // y'' = spline_matrix * rhs(y)
  //
  //                         0
  //                         y0 - 2*y1 + y2
  //                         y1 - 2*y2 + y3
  // rhs = 6/(delta_tau)^2 * ...
  //
  //                         yNp1-3 - 2*yNp1-2 + yNp1-1
  //                         y0 - y1 + yNp1-2 - yNp1-1

  std::vector<double> rhs(Np1, 0);
  std::cout<<"constants: "<<c1g<<" "<<c2g<<" "<<c3g<<std::endl;
  rhs[0] = -c3g; //G''(0)+G''(beta)=-c3
  for (int i=1; i<Np1-1; i++) {
    rhs[i] = (g[i-1]-2*g[i]+g[i+1])/dt;
  }
  rhs[Np1-1] = c2g -1./dt*(-g[0] + g[1] -g[Np1-2] + g[Np1-1]);

  for (int i=0; i<Np1; i++) {
    second_derivatives[i]=0;
    for (int j=0; j<Np1; j++) {
      second_derivatives[i] += spline_matrix(i,j)*rhs[j];
    }
  }
}

//
// calculations following Armin Comadac's thesis (see the thesis for details)
// S1 = G'(0) + G'(beta) = y'(0) + y'(L)
// S2 = G''(0) + G''(beta) = y''(0) + y''(L)
// remember that S1 and S2 can be different from the Green's function's tail
// in details: S1 = G2
//             S2 = -G3
//
VectorXd ArminGetSecondDerivative(const VectorXd &y,
    const double beta, const double S1, const double S2) {
  const int L = y.size()-1;
  double dx = beta/L;

  VectorXd X(L+1), Y(L), Z(L);
  // coefficients for L, U matrices (Armin's notations)
  VectorXd a(L), b(L), d(L);
  Y.setZero();
  Z.setZero();
  Y(0) = 6. * ((y[1]-y[0]+y[L]-y[L-1]-S1*dx) / (dx*dx) + S2/3.);
  Z(0) = Y(0);
  a(0) = 4.;
  b(0) = -1./a(0);
  d(0) = -1.;
  for (int i = 1; i < L-1; ++i) {
    a(i) = 4. - 1./a(i-1);
    b(i) = -b(i-1)/a(i);
    d(i) = -d(i-1)/a(i-1);
    Y(i) = 6. * (y(i+1) - 2*y(i) + y(i-1)) / (dx*dx);
    Z(i) = Y(i) - Z(i-1)/a(i-1);
  }
  Y(L-1) = 6. * ((y(L) - 2*y(L-1) + y(L-2)) / (dx*dx) - S2 / 6.);
  a(L-1) = 4.;
  b(L-2) = (1.-b(L-3))/a(L-2);
  d(L-2) = 1. - d(L-3)/a(L-3);
  for (int i = 0; i < L-1; ++i)
    a(L-1) -= b(i)*d(i);
  Z(L-1) = Y(L-1);
  for (int i = 0; i < L-1; ++i)
    Z(L-1) -= b(i)*Z(i);

  X(L-1) = Z(L-1) / a(L-1);
  X(L-2) = (Z(L-2)-d(L-2)*X(L-1)) / a(L-2);
  for (int i = L-3; i >= 0; --i)
    X(i) = (Z(i) - X(i+1) - d(i)*X(L-1))/a(i);
  X(L) = S2 - X(0);

  return X;
}

// forward_ft G(tau)->G(iwn) works for cluster Green's functions
void FourierTransformer::Armin_forward_ft(const VectorXd &gtau, VectorXcd &gomega,
                                          const int num_matsubara) const {
  int N = gtau.size()-1;
  double dt = beta_/N;
  VectorXcd v_omega(num_matsubara);
  gomega.resize(num_matsubara);
  VectorXd v2(ArminGetSecondDerivative(gtau, beta_, c_(1), -c_(2)));

  // DEBUG
//  const VectorXd &v(gtau);
//  std::cout<<"c1 is: "<<c_(0)<<" computed: "<<-v[0]-v[N]<<std::endl;
//  std::cout<<"c2 is: "<<c_(1)<<" computed: "<<(v[1]-v[0]+v[N]-v[N-1])/dt<<std::endl;
//  std::cout<<"c3 is: "<<c_(2)<<" computed: "<<-(v[2]-2*v[1]+v[0]+v[N-2]-2*v[N-1]+v[N])/(dt*dt)<<std::endl;

  v_omega.setZero();
  for (int k = 0; k < num_matsubara; ++k) {
    std::complex<double> iw(0, M_PI*(2*k+1)/beta_);
    for (int n = 1; n < N; n++) {
      //partial integration, four times.
      //Then approximate the fourth derivative by finite differences
      v_omega(k) += exp(iw*(n*dt))*(v2(n+1)-2*v2(n)+v2(n-1));
    }
    // the third derivative, on the boundary
    v_omega(k) += (v2(1) - v2(0) + v2(N) - v2(N-1));
    v_omega(k) *= 1./(dt*iw*iw*iw*iw);
    // the boundary terms of the first, second, and third partial integration.
    v_omega(k) += f_omega(iw, c_(0), c_(1), c_(2));
    //std::cout<<"derivative at boundary: "<<-v2[1] + v2[0] + v2[N] - v2[N-1]<<" divisor: "<< 1./(dt*iw*iw*iw*iw)<<std::endl;
    // the proper convention for the self consistency loop here.
    gomega(k)=v_omega(k);
  }
}

// forward_ft G(tau)->G(iwn) works for cluster Green's functions
void FourierTransformer::forward_ft(const VectorXd &gtau, VectorXcd &gomega,
                                    const int num_matsubara) const {
  std::vector<double> v(gtau.size());
  std::vector<std::complex<double> > v_omega(num_matsubara);
  int Np1 = v.size();
  int N = Np1-1;
  int N_omega = v_omega.size();
  double dt = beta_/N;

  gomega.resize(num_matsubara);
  for(int tau=0;tau<Np1;++tau) {
    v[tau]=gtau(tau);
  }

  dense_matrix spline_matrix(Np1, Np1);
  generate_spline_matrix(spline_matrix, dt);
  // matrix containing the second derivatives y''
  // of interpolated y=v[tau] at points tau_n
  std::vector<double> v2(Np1, 0);
  evaluate_second_derivatives(dt/*,beta_*/, spline_matrix, v, v2,
      c_(0), c_(1), c_(2));
  v_omega.assign(N_omega, 0);
  for (int k=0; k<N_omega; k++) {
    std::complex<double> iw(0, M_PI*(2*k+1)/beta_);
    for (int n=1; n<N; n++) {
      //partial integration, four times.
      //Then approximate the fourth derivative by finite differences
      v_omega[k] += exp(iw*(n*dt))*(v2[n+1]-2*v2[n]+v2[n-1]);
    }
    // the third derivative, on the boundary
    v_omega[k] += (v2[1] - v2[0] + v2[N] - v2[N-1]);
    v_omega[k] *= 1./(dt*iw*iw*iw*iw);
    // the boundary terms of the first, second, and third partial integration.
    v_omega[k] += f_omega(iw, c_(0), c_(1), c_(2));
    //std::cout<<"derivative at boundary: "<<-v2[1] + v2[0] + v2[N] - v2[N-1]<<" divisor: "<< 1./(dt*iw*iw*iw*iw)<<std::endl;
    // the proper convention for the self consistency loop here.
    gomega(k)=v_omega[k];
  }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_REMAINDER_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_REMAINDER_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/remainder.hpp>
#include <boost/simd/include/functions/scalar/abs.hpp>
#include <boost/simd/include/functions/scalar/negate.hpp>
#include <boost/simd/include/functions/scalar/idivround.hpp>
#include <boost/simd/include/functions/scalar/divround.hpp>
/////////////////////////////////////////////////////////////////////////////
// The remainder() function computes the remainder of dividing x by y.  The
// return value is x-n*y, where n is the value x / y, rounded to the nearest
// integer.  If the boost::simd::absolute value of x-n*y is 0.5, n is chosen to be even.
// The drem function is just an alias for the same thing.
/////////////////////////////////////////////////////////////////////////////

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::remainder_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)(scalar_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (!a1) return a0;
      return a0-idivround(a0, a1)*a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::remainder_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)(scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0-divround(a0, a1)*a1;
    }
  };
} } }

#endif


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

// clang-format off
#include <ndt_2d_slam/scan_matcher_2d.hpp>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <algorithm>
#include <tuple>
#include <vector>
// clang-format on

namespace NDT2DSLAM {

CovarianceMat correctMatrix(const CovarianceMat& original, double smaller,
                            double bigger, double minimum) {
    double nsmaller = smaller;
    if (smaller < minimum)
        nsmaller = minimum;
    else
        return original;
    double dsum = (nsmaller * nsmaller - bigger * bigger) / (nsmaller - bigger);
    double a11 = dsum / 2;
    double a22 = dsum - a11;
    double a212 = sqrt(bigger * bigger - dsum * bigger + a11 * a22);

    CovarianceMat corrected;
    corrected << a11, a212, a212, a22;

    return corrected;
}

void Cell::addPoint(double x, double y) {
    Eigen::Index nCol = points.cols();
    for (auto i = 0; i < nCol; ++i)  // avoid adding same point
        if (points(0, i) == x && points(1, i) == y) return;
    points.conservativeResize(Eigen::NoChange, nCol + 1);
    points(0, nCol) = x;
    points(1, nCol) = y;
}

void Cell::ndt() {
    if (points.cols() > 0) mean = points.rowwise().mean();

    if (points.cols() < 3) {
        validity = false;
        return;
    }

    Eigen::MatrixXd diff = points.colwise() - mean;
    covariance =
        (diff * diff.adjoint()) / static_cast<double>(points.cols() - 1);

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver(covariance);
    Eigen::Vector2d evs = solver.eigenvalues();
    // std::cout << "evs: \n" << evs << std::endl;
    // std::cout << "evec \n" << solver.eigenvectors() << std::endl;

    const double seThreshold = 0.001;
    double smaller = std::min(evs(0), evs(1));
    double bigger = std::max(evs(0), evs(1));
    double minimum = bigger * seThreshold;
    if (smaller < minimum) {  // correction required
        covariance = correctMatrix(covariance, smaller, bigger, minimum);
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> fixed(covariance);
        // std::cout << "fixed cov: \n" << covariance << std::endl;
        // std::cout << "fixed evs: \n" << fixed.eigenvalues() << std::endl;
        // std::cout << "fixed evec: \n" << fixed.eigenvectors() <<
        // std::endl;
    }
    iCovariance = covariance.inverse();

    validity = true;
}

void Cell::calcCenterOfGravity() {
    if (points.cols() > 0) mean = points.rowwise().mean();
}

CellGridFilter::CellGridFilter(double gridSize, const Scan2D& scan,
                               std::ostream& outStream)
    : _gridSize(gridSize), _outStream(outStream) {
    std::cerr << "[cell grid filter]" << std::endl;
    std::cerr << "scan size: " << scan.size() << std::endl;
    // process reference scan
    const std::vector<double>& xv = scan.getXVec();
    _minX = *std::min_element(xv.begin(), xv.end());
    _maxX = *std::max_element(xv.begin(), xv.end());
    const std::vector<double>& yv = scan.getYVec();
    _minY = *std::min_element(yv.begin(), yv.end());
    _maxY = *std::max_element(yv.begin(), yv.end());

    // range of pointcloud
    double rangeX = _maxX - _minX;
    double rangeY = _maxY - _minY;

    // number of cells in both direction
    _nCellX = 1 + static_cast<int>(rangeX / _gridSize);
    _nCellY = 1 + static_cast<int>(rangeY / _gridSize);

    _cells.resize(_nCellX * _nCellY);

    // distribute points to each cells
    for (size_t i = 0; i < scan.size(); ++i) {
        double x = scan.points(0, i);
        double y = scan.points(1, i);
        auto [ix, iy] = xy2cellXYIdx(x, y);
        int idx = iy * _nCellX + ix;
        if (idx >= 0 && idx < static_cast<int>(_cells.size()))
            _cells[idx].addPoint(x, y);
    }

    std::vector<double> xs, ys;
    for (auto& cell : _cells) {
        if (cell.points.size() == 0) continue;
        cell.calcCenterOfGravity();
        xs.emplace_back(cell.mean(0));
        ys.emplace_back(cell.mean(1));
    }
    _result = Scan2D(xs, ys);
}

std::tuple<int, int> CellGridFilter::xy2cellXYIdx(double x, double y) const {
    int xIdx = static_cast<int>((x - _minX) / _gridSize) + 1;
    int yIdx = static_cast<int>((y - _minY) / _gridSize) + 1;

    return {xIdx, yIdx};
}

// NDTScanMatcher implementation
NDTScanMatcher::NDTScanMatcher(double gridSize, double epsilonTranslation,
                               double epsilonRotation, const Scan2D& refScan,
                               std::ostream& outStream = std::cout)
    : _debug(false),
      _debugOut(""),
      _gridSize(gridSize),
      _halfGridSize(gridSize / 2.0),
      _epsilonTranslation(epsilonTranslation),
      _epsilonRotation(epsilonRotation),
      _outStream(outStream) {
    // process reference scan
    const std::vector<double>& xv = refScan.getXVec();
    _minX = *std::min_element(xv.begin(), xv.end());
    _maxX = *std::max_element(xv.begin(), xv.end());
    const std::vector<double>& yv = refScan.getYVec();
    _minY = *std::min_element(yv.begin(), yv.end());
    _maxY = *std::max_element(yv.begin(), yv.end());

    // range of pointcloud
    double rangeX = _maxX - _minX;
    double rangeY = _maxY - _minY;
    // number of cells in both direction
    _nCellX =
        2 + static_cast<int>((rangeX + _halfGridSize - _halfGridSize * 0.01) /
                             _halfGridSize);
    _nCellY =
        2 + static_cast<int>((rangeY + _halfGridSize - _halfGridSize * 0.01) /
                             _halfGridSize);

    _cells.resize(_nCellX * _nCellY);
    // distribute points to each cells
    for (Eigen::Index i = 0; i < refScan.points.cols(); ++i) {
        double x = refScan.points(0, i);
        double y = refScan.points(1, i);
        // base cell index for x, y direction
        auto [bcxIdx, bcyIdx] = xy2cellXYIdx(x, y);
        // each point must be corresponds to 4 cells
        for (int j = 0; j < 4; ++j) {
            try {
                int idx = cellXYIdx2Idx({bcxIdx + _dx[j], bcyIdx + _dy[j]});
                _cells[idx].addPoint(x, y);
            } catch (const std::out_of_range& e) {
                // just skip
            }
        }
    }

    // transform points to normal distribution on each cell
    for (auto& cell : _cells) cell.ndt();

    return;

    _outStream << "[init]" << std::endl;
    int cnt = 0;
    for (auto& cell : _cells) {
        if (cell.validity) {
            _outStream << "idx: " << cnt << std::endl;
            _outStream << "pnum: " << cell.points.cols() << std::endl;
            _outStream << "points:\n" << cell.points << std::endl;
            _outStream << "mean: " << cell.mean.transpose() << std::endl;
            _outStream << "covariance:\n" << cell.covariance << std::endl;
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> fixed(
                cell.covariance);
            _outStream << "fixed evs: " << fixed.eigenvalues().transpose()
                       << std::endl;
            return;

            // std::cout << "fixed evec: \n" << fixed.eigenvectors() <<
        }
        cnt++;
    }
}

Transform2D NDTScanMatcher::match(const Transform2D& initialGuess,
                                  const Scan2D& currentScan) {
    _outStream << "[matching]" << std::endl;
    _outStream << initialGuess << std::endl;
    Transform2D currentGuess = initialGuess;

    iterationCount = 0;
    const int iterMax = 50;
    while (iterationCount++ < iterMax) {
        auto transformedCurrentScan = currentScan.transformBy(currentGuess);

        double sinT = sin(currentGuess.theta);
        double cosT = cos(currentGuess.theta);
        Eigen::MatrixXd partialRP(3, 2);
        partialRP << 1, 0, 0, 1, 0, 0;
        Eigen::MatrixXd hesse1 = Eigen::MatrixXd::Zero(3, 3);
        Eigen::MatrixXd hesse23 = Eigen::MatrixXd::Zero(3, 3);

        double score = 0.0;  // of ndt scan matching
        Eigen::VectorXd gradient = Eigen::VectorXd::Zero(3);
        Eigen::MatrixXd hessian = Eigen::MatrixXd::Zero(3, 3);

        int cnt = 0;
        for (size_t i = 0; i < transformedCurrentScan.size(); ++i) {
            const double ox = currentScan.getX(i), oy = currentScan.getY(i);
            const double tx = transformedCurrentScan.getX(i);
            const double ty = transformedCurrentScan.getY(i);

            Eigen::Vector2d partialRP3(1, 2);
            // partialRP3 << -sinT * ox - cosT * oy, cosT * ox - sinT * oy;
            partialRP(2, 0) = -sinT * ox - cosT * oy;
            partialRP(2, 1) = cosT * ox - sinT * oy;
            Eigen::MatrixXd elem33(1, 2);
            elem33 << -cosT * ox + sinT * oy, -sinT * ox - cosT * oy;

            // base cell index for x, y direction
            auto [bcxIdx, bcyIdx] = xy2cellXYIdx(tx, ty);
            for (int j = 0; j < 4; ++j) {
                try {
                    int idx = cellXYIdx2Idx({bcxIdx + _dx[j], bcyIdx + _dy[j]});
                    // _cells[idx] is the corresponding cell to point(xy)
                    if (!_cells[idx].validity) continue;
                    cnt++;

                    // from cell
                    Eigen::Vector2d residual =
                        Eigen::Vector2d{tx, ty} - _cells[idx].mean;
                    Eigen::Matrix2d invCovariance = _cells[idx].iCovariance;
                    auto icovRes = invCovariance * residual;

                    double expin = -0.5 * residual.transpose() * icovRes;
                    double expN = std::exp(expin);

                    // update score
                    score += expN;
                    // update gradient
                    gradient += partialRP * icovRes * expN;

                    // update Hessian
                    auto elem33ans = elem33 * icovRes;
                    // assert(elem33.cols() == 1 && elem33.rows() == 1);
                    hesse1(2, 2) = elem33ans(0, 0);

                    for (int ii = 0; ii < 3; ++ii) {
                        auto ppi = partialRP.block<1, 2>(ii, 0);
                        auto ppiicov = ppi * invCovariance;
                        auto ppiicovRes = ppi * icovRes;
                        for (int jj = 0; jj < 3; ++jj) {
                            auto ppj = partialRP.block<1, 2>(jj, 0);
                            auto hresult = ppiicov * ppj.transpose() +
                                           ppiicovRes * ppj * icovRes;
                            // assert(hresult.cols() == 1 &&
                            // hresult.rows() == 1);
                            hesse23(ii, jj) = hresult(0, 0);
                        }
                    }

                    hessian += expN * (hesse1 + hesse23);
                } catch (const std::out_of_range& e) {
                    // just skip
                }
            }
        }
        assert(score == score);
        _outStream << "\nscore: " << score << std::endl;
        _outStream << "cnt: " << cnt << std::endl;
        _outStream << "gradient: " << gradient.transpose() << std::endl;
        // _debugOut << "hessian:\n" << hessian << std::endl;
        Eigen::EigenSolver<Eigen::Matrix3d> fixed;
        fixed.compute(hessian, false);
        _outStream << "h eig: " << fixed.eigenvalues().transpose() << std::endl;
        double min_eigenvalue = 0;
        for (int kk = 0; kk < 3; kk++)
            if (fixed.eigenvalues()[kk].real() < min_eigenvalue)
                min_eigenvalue = fixed.eigenvalues()[kk].real();
        if (min_eigenvalue < 0) {
            double lambda = 1.1 * min_eigenvalue - 1;
            hessian += Eigen::Vector3d(-lambda, -lambda, -lambda).asDiagonal();
            fixed.compute(hessian, false);
            _outStream << "h eig fixed: " << fixed.eigenvalues().transpose()
                       << std::endl;
        }

        _outStream << "hessian inv:\n" << hessian.inverse() << std::endl;
        auto dp = -hessian.inverse() * gradient;
        _outStream << "dp: " << dp.transpose() << std::endl;

        currentGuess += NDT2DSLAM::Transform2D(dp(0), dp(1), dp(2));
        _outStream << currentGuess << std::endl;

        // terminate if criteria is met
        if ((std::max(fabs(dp(0)), fabs(dp(1))) < _epsilonTranslation) ||
            (fabs(dp(2)) < _epsilonRotation)) {
            break;
        }
    }
    _outStream << "iterCnt: " << iterationCount << std::endl;
    _outStream << "final " << currentGuess << std::endl;

    return currentGuess;
}

int NDTScanMatcher::cellXYIdx2Idx(std::tuple<int, int> xIdx_yIdx) {
    int xIdx = std::get<0>(xIdx_yIdx);
    int yIdx = std::get<1>(xIdx_yIdx);
    int idx = yIdx * _nCellX + xIdx;
    if (idx < 0 || static_cast<int>(_cells.size()) <= idx)
        throw std::out_of_range("no corresponding cell exists");
    return idx;
}

std::tuple<int, int> NDTScanMatcher::xy2cellXYIdx(double x, double y) {
    int xIdx = static_cast<int>((x - _minX) / _halfGridSize) + 1;
    int yIdx = static_cast<int>((y - _minY) / _halfGridSize) + 1;

    return {xIdx, yIdx};
}

double NDTScanMatcher::scoreDifference(const Eigen::Vector2d& residual,
                                       const Eigen::Matrix2d& invCovariance) {
    double inExp = -0.5 * residual.transpose() * invCovariance * residual;
    return std::exp(inExp);
}

}  // namespace NDT2DSLAM


#include <iostream>
#include <fstream>

#include <boost/program_options.hpp>

#include <mav_trajectory_generation/polynomial_optimization_linear.h>
#include <mav_trajectory_generation/polynomial_optimization_nonlinear.h>

using namespace Eigen;

// see https://stackoverflow.com/questions/34247057/how-to-read-csv-file-and-assign-to-eigen-matrix
template<typename M>
M load_csv (const std::string & path) {
  std::ifstream indata;
  indata.open(path);
  std::string line;
  std::vector<double> values;
  uint rows = 0;
  while (std::getline(indata, line)) {
    std::stringstream lineStream(line);
    std::string cell;
    while (std::getline(lineStream, cell, ',')) {
      values.push_back(std::stod(cell));
    }
    ++rows;
  }
  return Map<const Matrix<typename M::Scalar, M::RowsAtCompileTime, M::ColsAtCompileTime, RowMajor>>(values.data(), rows, values.size()/rows);
}

int main(int argc, char **argv)
{
  std::string inputFile;
  std::string outputFile;
  double v_max;
  double a_max;

  namespace po = boost::program_options;

  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("input,i", po::value<std::string>(&inputFile)->required(), "path to list of waypoints")
    ("output,o", po::value<std::string>(&outputFile)->required(), "path to output files")
    ("v_max", po::value<double>(&v_max)->default_value(1.0), "maximum velocity [m/s]")
    ("a_max", po::value<double>(&a_max)->default_value(1.0), "maximum velocity [m/s^2]")
  ;

  try
  {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
  }
  catch(po::error& e)
  {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  mav_trajectory_generation::Vertex::Vector vertices;
  const int dimension = 3;
  const int derivative_to_optimize = mav_trajectory_generation::derivative_order::JERK;

  // create vertices with their constraints
  auto input = load_csv<Matrix<double, Dynamic, 3> >(inputFile);

  if (input.rows() < 2) {
    std::cerr << "Not enough datapoints given!" << std::endl;
    return 1;
  }

  if (input.rows() == 2) {
    mav_trajectory_generation::Vertex v1(dimension);
    v1.makeStartOrEnd(input.row(0), derivative_to_optimize);
    vertices.push_back(v1);

    mav_trajectory_generation::Vertex v2(dimension);
    auto middle = (input.row(1) - input.row(0)) * 0.5 + input.row(0);
    v2.addConstraint(mav_trajectory_generation::derivative_order::POSITION, middle);
    vertices.push_back(v2);

    mav_trajectory_generation::Vertex v3(dimension);
    v3.makeStartOrEnd(input.row(1), derivative_to_optimize);
    vertices.push_back(v3);
  } else {
    // at least 3 points given
    for (int row = 0; row < input.rows(); ++row) {
      // std::cout << input.row(row) << std::endl;
      mav_trajectory_generation::Vertex vertex(dimension);
      if (row == 0 || row == input.rows() - 1) {
        vertex.makeStartOrEnd(input.row(row), derivative_to_optimize);
      } else {
        vertex.addConstraint(mav_trajectory_generation::derivative_order::POSITION, input.row(row));
      }
      vertices.push_back(vertex);
    }
  }

  // compute segment times
  std::vector<double> segment_times;
  const double magic_fabian_constant = 6.5; // A tuning parameter.
  segment_times = estimateSegmentTimes(vertices, v_max, a_max, magic_fabian_constant);

  // solve
  const int N = 8;
  mav_trajectory_generation::Segment::Vector segments;

#if SOLVE_LINEAR
  mav_trajectory_generation::PolynomialOptimization<N> opt(dimension);
  opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
  opt.solveLinear();

  // Obtain the polynomial segments.
  opt.getSegments(&segments);
#else
  mav_trajectory_generation::NonlinearOptimizationParameters parameters;
  parameters.max_iterations = 1000;
  parameters.f_rel = 0.05;
  parameters.x_rel = 0.1;
  parameters.time_penalty = 500.0;
  parameters.initial_stepsize_rel = 0.1;
  parameters.inequality_constraint_tolerance = 0.1;

  mav_trajectory_generation::PolynomialOptimizationNonLinear<N> opt(dimension, parameters, false);
  opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
  opt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::VELOCITY, v_max);
  opt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::ACCELERATION, a_max);
  opt.optimize();

  // Obtain the polynomial segments.
  opt.getPolynomialOptimizationRef().getSegments(&segments);
#endif

  std::ofstream output(outputFile);

  output << "Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7" << std::endl;
  Eigen::IOFormat csv_fmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "", "");

  for (const auto& segment : segments) {
    output << segment.getTime() << ",";
    for (const auto& polynomial : segment.getPolynomialsRef()) {
      Eigen::VectorXd coefficients = polynomial.getCoefficients();
      output << coefficients.format(csv_fmt) << ",";
    }
    output << "0,0,0,0,0,0,0,0" << std::endl;
  }

  return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file KalmanFilter.cpp
 *
 * @brief Simple linear Kalman filter.
 * Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
 *
 * @date Sep 3, 2011
 * @author Stephen Williams
 * @author Frank Dellaert
 */

#include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/Testable.h>

#include <boost/make_shared.hpp>
#include <boost/assign/list_of.hpp>

using namespace boost::assign;
using namespace std;

namespace gtsam {

/* ************************************************************************* */
// Auxiliary function to solve factor graph and return pointer to root conditional
KalmanFilter::State //
KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {

  // Eliminate the graph using the provided Eliminate function
  Ordering ordering(factorGraph.keys());
  GaussianBayesNet::shared_ptr bayesNet = //
      factorGraph.eliminateSequential(ordering, function_);

  // As this is a filter, all we need is the posterior P(x_t).
  // This is the last GaussianConditional in the resulting BayesNet
  GaussianConditional::shared_ptr posterior = *(--bayesNet->end());
  return boost::make_shared<GaussianDensity>(*posterior);
}

/* ************************************************************************* */
// Auxiliary function to create a small graph for predict or update and solve
KalmanFilter::State //
KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {

  // Create a factor graph
  GaussianFactorGraph factorGraph;
  factorGraph += p, newFactor;

  // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
  return solve(factorGraph);
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x0,
    const SharedDiagonal& P0) const {

  // Create a factor graph f(x0), eliminate it into P(x0)
  GaussianFactorGraph factorGraph;
  factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma
  return solve(factorGraph);
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const {

  // Create a factor graph f(x0), eliminate it into P(x0)
  GaussianFactorGraph factorGraph;
  factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
  return solve(factorGraph);
}

/* ************************************************************************* */
void KalmanFilter::print(const string& s) const {
  cout << "KalmanFilter " << s << ", dim = " << n_ << endl;
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
    const Matrix& B, const Vector& u, const SharedDiagonal& model) const {

  // The factor related to the motion model is defined as
  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
  Key k = step(p);
  return fuse(p,
      boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
    const Matrix& B, const Vector& u, const Matrix& Q) const {

#ifndef NDEBUG
  DenseIndex n = F.cols();
  assert(F.rows() == n);
  assert(B.rows() == n);
  assert(B.cols() == u.size());
  assert(Q.rows() == n);
  assert(Q.cols() == n);
#endif

  // The factor related to the motion model is defined as
  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
  // See documentation in HessianFactor, we have A1 = -F,  A2 = I_, b = B*u:
  // TODO: starts to seem more elaborate than straight-up KF equations?
  Matrix M = Q.inverse(), Ft = trans(F);
  Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
  Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
  double f = dot(b, g2);
  Key k = step(p);
  return fuse(p,
      boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
    const Matrix& A1, const Vector& b, const SharedDiagonal& model) const {
  // Nhe factor related to the motion model is defined as
  // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
  Key k = step(p);
  return fuse(p, boost::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
    const Vector& z, const SharedDiagonal& model) const {
  // The factor related to the measurements would be defined as
  // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
  //    = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
  Key k = step(p);
  return fuse(p, boost::make_shared<JacobianFactor>(k, H, z, model));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
    const Vector& z, const Matrix& Q) const {
  Key k = step(p);
  Matrix M = Q.inverse(), Ht = trans(H);
  Matrix G = Ht * M * H;
  Vector g = Ht * M * z;
  double f = dot(z, M * z);
  return fuse(p, boost::make_shared<HessianFactor>(k, G, g, f));
}

/* ************************************************************************* */

} // \namespace gtsam


#ifndef ISOMON_TEST_MONEY_CALC_HPP
#define ISOMON_TEST_MONEY_CALC_HPP

#include "money_calc.hpp"

#include <boost/test/unit_test.hpp>

using namespace std;
using namespace boost;
using namespace boost::unit_test;
using namespace isomon;

BOOST_AUTO_TEST_CASE( roundhalfout_test )
{
  money m(2, 0, "USD");

  m = round(m * 1.5);
  BOOST_CHECK_EQUAL(m.total_minors(), 300);

  money m2 = round(m * 1.001);
  BOOST_CHECK_EQUAL(m2.total_minors(), 300);

  m2 = round(m * 1.002);
  BOOST_CHECK_EQUAL(m2.total_minors(), 301);
}

BOOST_AUTO_TEST_CASE( multiply_test )
{
  money m(3, 0, "USD");
  money_calc<double> mc(1.5, "USD");

  BOOST_CHECK_EQUAL( round(m * 2.0), round(mc * 4) );
}

BOOST_AUTO_TEST_CASE( addition_test )
{
  money_calc<double> one_third_dollar( 1.0/3.0, "USD" );
  money buck(1, 0, "USD");

  BOOST_CHECK_EQUAL( buck, round(3 * one_third_dollar) );
  money_calc<double> sum = one_third_dollar + one_third_dollar;
  sum += one_third_dollar;
  BOOST_CHECK_EQUAL( buck, round(sum) );
}

BOOST_AUTO_TEST_CASE( negation_test )
{
  money thirty_three_cents(0, 33, "USD");
  money neg_thirty_three_cents(0, -33, "USD");
  money buck(1, 0, "USD");
  money_calc<double> one_third_dollar( 1.0/3.0, "USD" );
  money_calc<double> neg_one_third_dollar( -1.0/3.0, "USD" );

  BOOST_CHECK_EQUAL( -thirty_three_cents, neg_thirty_three_cents );
  BOOST_CHECK_EQUAL( buck - thirty_three_cents, money(0, 67, "USD") );
  BOOST_CHECK_EQUAL( -buck + thirty_three_cents, -money(0, 67, "USD") );
  BOOST_CHECK_EQUAL( round(-one_third_dollar), round(neg_one_third_dollar) );
}

BOOST_AUTO_TEST_CASE( mutlicurrency_nonaddition_test )
{
  money bad = money(1, 0, "USD") + money(1, 0, "EUR");
  BOOST_CHECK( bad.unit().is_no_currency() );
}

#endif


//Written by John G Baker NASA-GSFC (2017)
//This is to test the shaped gaussian proposal distribution.

#include "proposal_distribution.hh"
#include "chain.hh"
#include <Eigen/Eigen>

shared_ptr<Random> globalRNG;//used for some debugging...

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

  int ndim=5;
  int ndraws=2100000;
  double temp=10.;

  //Randomly construct a covariance matrix
  Eigen::MatrixXd cov(ndim,ndim);
  cov=Eigen::MatrixXd::Random(ndim,ndim);
  //cov.Random();        //draw a random matrix
  cov=cov.transpose()*cov; //and turn it into something symmetric and positive semidefinite.
  cov*=temp;

  //Next we define the proposal
  gaussian_prop p(cov);

  //We need a chain, to allow draws, just a dummy, but some setup is needed.
  ProbabilityDist::setSeed(.224);
  stateSpace ss(ndim);
  state s(&ss,vector<double>(ndim,0));
  gaussian_dist_product prior(&ss);
  MH_chain c(&prior,&prior);
  c.resetTemp(1/temp);

  //perform draws compute stats
  Eigen::MatrixXd covsum=Eigen::MatrixXd::Zero(ndim,ndim);
  Eigen::VectorXd sum=Eigen::VectorXd::Zero(ndim);
  Eigen::VectorXd min=Eigen::VectorXd::Constant(ndim,+1e100);
  Eigen::VectorXd max=Eigen::VectorXd::Constant(ndim,-1e100);
  valarray<double> statedata;
  valarray<double> statedata0;
  s.get_params_array(statedata0);
  Eigen::Map<Eigen::VectorXd> vec0(&statedata0[0],statedata0.size());
  state draw;

  for(int n=1;n<=ndraws;n++){
    draw=p.draw(s,&c);
    draw.get_params_array(statedata);
    Eigen::Map<Eigen::VectorXd> vec(&statedata[0],statedata.size());
    draw.get_params_array(statedata);
    //Eigen::Map<Eigen::VectorXd> vec0(&statedata0[0],statedata0.size());
    //cout<<"vec0:("<<vec0.rows()<<","<<vec0.cols()<<") vec:("<<vec.rows()<<","<<vec.cols()<<")"<<endl;
    Eigen::VectorXd dvec=vec-vec0;
    Eigen::MatrixXd covdelta(ndim,ndim);
    //cout<<"dvec=\n"<<dvec<<endl;
    sum+=dvec;
    for(int i=0;i<ndim;i++)for(int j=0;j<ndim;j++)covdelta(i,j)=dvec(i)*dvec(j);
    covsum+=covdelta;
    for(int i=0;i<ndim;i++){
      if(min(i)>vec(i))min(i)=vec(i);
      if(max(i)<vec(i))max(i)=vec(i);
    }
    if((n & (n - 1))==0){
      Eigen::MatrixXd coverr(ndim,ndim);
      coverr=covsum/n-cov;
      cout<<"cov=\n"<<coverr+cov<<endl;
      cout<<"coverr=\n"<<coverr<<endl;
      double coverrnorm=(coverr*coverr).trace();
      double coverrdiagnorm=0;
      for(int i=0;i<ndim;i++)coverrdiagnorm+=coverr(i,i)*coverr(i,i);
      cout<<"min:"<<min.transpose()<<endl;
      cout<<"max:"<<max.transpose()<<endl;
      cout<<n<<": errnorm="<<coverrnorm<<"\terrdiagnorm="<<coverrdiagnorm<<endl;

    }
  }
}


#include <memory>
#define CATCH_CONFIG_MAIN
#include <catch.hpp>
#include <tbb/tbb.h>

#include <Eigen/Dense>
#include <Eigen/src/Eigenvalues/ComplexEigenSolver.h>
#include <Eigen/src/QR/HouseholderQR.h>
#include <random>

#include "EDP/ConstructSparseMat.hpp"
#include "EDP/LocalHamiltonian.hpp"

#include "common.hpp"

#include "yavque/Circuit.hpp"
#include "yavque/operators.hpp"

using namespace Eigen;

tbb::global_control gc(tbb::global_control::max_allowed_parallelism, 2);

TEST_CASE("test single qubit operator", "[single-qubit-operator]")
{
	using namespace yavque;
	constexpr uint32_t N = 3;
	constexpr uint32_t dim = 1u << N;
	constexpr cx_double I(0, 1.0);
	std::random_device rd;
	std::default_random_engine re{rd()};
	std::uniform_int_distribution<uint32_t> index_dist(0, N - 1);

	// test using sparse matrix construction
	for(uint32_t instance_idx = 0; instance_idx < 100; ++instance_idx)
	{
		auto op = random_unitary(4, re);
		auto i = index_dist(re);
		auto j = index_dist(re);
		while(i == j)
		{
			j = index_dist(re);
		}
		auto m1 = TwoQubitOperator(op, N, i, j);

		auto st = random_vector(dim, re);

		edp::LocalHamiltonian<cx_double> lh(N, 2);
		lh.addTwoSiteTerm({i, j}, op.sparseView());
		auto m = edp::constructSparseMat<cx_double>(dim, lh);

		REQUIRE((m1.apply_right(st) - m * st).norm() < 1e-6);

		m1.dagger_in_place();
		REQUIRE((m1.apply_right(st) - m.adjoint() * st).norm() < 1e-6);
	}
}


#include "lwtnn/LightweightNeuralNetwork.hh"

#include <Eigen/Dense>

#include <iostream>

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

  std::vector<double> weights{
    0, 0, 0, 1,
    0, 0, 1, 0,
    0, 1, 0, 0,
    1, 0, 0, 0};
  lwt::LayerConfig layer1{weights};
  std::vector<lwt::Input> input_conf{
    {"1", 0, 1}, {"2", 0, 1}, {"3", 0, 1}, {"4", 0, 1}};
  std::vector<std::string> outputs{"1", "2", "3", "4"};

  lwt::LightweightNeuralNetwork tagger(input_conf, {layer1}, outputs);
  lwt::ValueMap input{ {"1", 1}, {"2", 2}, {"3", 3}, {"4", 4} };
  auto out = tagger.compute(input);
  for (const auto& op: out) {
    std::cout << op.first << " " << op.second << std::endl;
  }
  return 0;
}


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

    @Copyright 2016 Numscale SAS

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_RSQRT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/refine_rsqrt.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/ratio.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::raw_tag
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (raw_tag const&
                                    , const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_cvtps_pd(_mm_rsqrt_ps(_mm_cvtpd_ps(a0))); //The maximum error for this approximation is 1.5e-12
    }
  };

   BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const A0 & a00) const BOOST_NOEXCEPT
    {
      // To obtain accuracy we need 3 Newton steps or one Halley step followed by one Newton from the raw estimate
      // the second method is a bit faster by half a cycle
      A0 a0 =  raw_(rsqrt)(a00);
      A0 y = sqr(a0)*a00;
      a0 = a0*Ratio<A0, 1, 8>()*fnms(y, fnms(A0(3), y, A0(10)), A0(15)); //this is Halley cubically convergent iteration
      a0 = refine_rsqrt(a00, a0);
#ifndef BOOST_SIMD_NO_INFINITIES
      a0 = if_zero_else(a00 == Inf<A0>(),a0);
#endif
      return if_else(is_eqz(a00), Inf<A0>(), a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pedantic_tag
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (pedantic_tag const&
                                    ,const A0 & a00) const BOOST_NOEXCEPT
    {
      A0 a01 =  a00;
      auto is_den = bs::abs(a00) < Smallestposval<A0>();
      #ifndef BOOST_SIMD_NO_DENORMALS
      a01 *= if_else(is_den, Denormalfactor<A0>(), One<A0>());
      #endif
      A0 a0 =  raw_(rsqrt)(a01);
      A0 y = sqr(a0)*a01;
      a0 = a0*Ratio<A0, 1, 8>()*fnms(y, fnms(A0(3), y, A0(10)), A0(15)); //this is Halley cubically convergent iteration
      a0 = refine_rsqrt(a00, a0);
      #ifndef BOOST_SIMD_NO_DENORMALS
      a0 *= if_else(is_den, Denormalsqrtfactor<A0>(), One<A0>());
      #endif

      #ifndef BOOST_SIMD_NO_INFINITIES
      a0 = if_zero_else(a00 == Inf<A0>(),a0);
      #endif
      return if_else(is_eqz(a00), Inf<A0>(), a0);
    }
  };
} } }

#endif


#pragma once

#include <functional>
#include <Eigen/Dense>

#include "vector/vector.hpp"
#include "polyline/interpolation.hpp"
#include "polyline/polyline_2d.hpp"
#include "spline/basis.hpp"

namespace euklid::spline {

template<typename Base, typename T>
class SplineCurve {
    public:
        SplineCurve(PolyLine2D);

        Vector2D get(double);
        PolyLine2D get_sequence(size_t);

        PolyLine2D controlpoints;
        T copy() const;

        static T fit(const PolyLine2D&, size_t);

        void set_numpoints(size_t);
        int get_numpoints();

    private:
        Base& get_base();
        Base base;
};

class BezierCurve : public SplineCurve<BezierBase, BezierCurve> {
        using SplineCurve<BezierBase, BezierCurve>::SplineCurve;
};

template<size_t degree>
class BSplineCurve : public SplineCurve<BSplineBase<degree>, BSplineCurve<degree>> {
    using SplineCurve<BSplineBase<degree>, BSplineCurve<degree>>::SplineCurve;

    public:
        Interpolation get_curvature(size_t) const;
        typename std::conditional<(degree>1), BSplineCurve<degree-1>, BSplineCurve<1>>::type get_derivate() const;
};

template<typename SplineClass, typename T>
class SymmetricSpline {
    public:
        SymmetricSpline(PolyLine2D);

        Vector2D get(double);
        PolyLine2D get_sequence(size_t);

        PolyLine2D controlpoints;
        T copy() const;

        static T fit(const PolyLine2D&, size_t);

        void set_numpoints(size_t);
        int get_numpoints();

    protected:
        void apply();
        SplineClass spline_curve;
};

template<size_t degree>
class SymmetricBSplineCurve : public SymmetricSpline<BSplineCurve<degree>, SymmetricBSplineCurve<degree>> {
    using SymmetricSpline<BSplineCurve<degree>, SymmetricBSplineCurve<degree>>::SymmetricSpline;
    public:
        Interpolation get_curvature(size_t);
};

class SymmetricBezierCurve : public SymmetricSpline<BezierCurve, SymmetricBezierCurve> {
    using SymmetricSpline<BezierCurve, SymmetricBezierCurve>::SymmetricSpline;
};

} // namespace euklid::spline


#ifndef PYTHONIC_INCLUDE_NUMPY_LOG1P_HPP
#define PYTHONIC_INCLUDE_NUMPY_LOG1P_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/log1p.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{

  namespace wrapper
  {
    template <class T>
    std::complex<T> log1p(std::complex<T> const &val)
    {
      return std::log(val + 1);
    }
    template <class T>
    auto log1p(T const &val) -> decltype(boost::simd::log1p(val))
    {
      return boost::simd::log1p(val);
    }
  }

#define NUMPY_NARY_FUNC_NAME log1p
#define NUMPY_NARY_FUNC_SYM wrapper::log1p
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


//   Copyright (c) 2018 Shahrzad Shirzad
//
//   Distributed under the Boost Software License, Version 1.0.0. (See accompanying
//   file LICENSE_1_0.0.txt or copy at http://www.boost.org/LICENSE_1_0.0.txt)

#include <phylanx/phylanx.hpp>
#include <hpx/hpx_init.hpp>

#include <iostream>

#include <blaze/Math.h>
#include <boost/program_options.hpp>

#include <hpx/include/agas.hpp>
#include <hpx/runtime_fwd.hpp>

#include <cstddef>
#include <cstdint>
#include <iostream>
#include <map>
#include <vector>
#include <string>
#include <utility>

///////////////////////////////////////////////////////////////////////////////
char const* const read_x_code = R"(
    //
    // Read input-data from given CSV file
    //
    define(read_x, filepath, row_start, row_stop, col_start, col_stop,
        slice(file_read_csv(filepath), make_list(row_start , row_stop),
              make_list(col_start , col_stop))
    )
    read_x
)";

char const* const als_explicit = R"(
    //
    // Alternating Least squares algorithm (ALS)
    //
    define(als_explicit, ratings, regularization, num_factors, iterations, alpha,
        enable_output,
        block(
            define(num_users, shape(ratings, 0)),
            define(num_items, shape(ratings, 1)),
            define(conf, alpha * ratings),

            define(conf_u, constant(0.0, make_list(num_items))),
            define(conf_i, constant(0.0, make_list(num_users))),

            define(c_u, constant(0.0, make_list(num_items, num_items))),
            define(c_i, constant(0.0, make_list(num_users, num_users))),
            define(p_u, constant(0.0, make_list(num_items))),
            define(p_i, constant(0.0, make_list(num_users))),

            set_seed(0),
            define(X, random(make_list(num_users, num_factors))),
            define(Y, random(make_list(num_items, num_factors))),
            define(I_f, identity(num_factors)),
            define(I_i, identity(num_items)),
            define(I_u, identity(num_users)),
            define(k, 0),
            define(i, 0),
            define(u, 0),

            define(XtX, constant(0.0, make_list(num_factors, num_factors))),
            define(YtY, constant(0.0, make_list(num_factors, num_factors))),
            define(A, constant(0.0, make_list(num_factors, num_factors))),
            define(b, constant(0.0, make_list(num_factors))),

            while(k < iterations,
                block(
                    if(enable_output,
                            block(
                                    cout("iteration ", k, u),
                                    cout("X: ",X),
                                    cout("Y: ",Y)
                            )
                    ),
                    store(YtY, dot(transpose(Y), Y) + regularization * I_f),
                    store(XtX, dot(transpose(X), X) + regularization * I_f),

                    while(u < num_users,
                        block(
                            store(conf_u, slice_row(conf, u)),
                            store(c_u, diag(conf_u)),
                            store(p_u, __ne(conf_u, 0.0, true)),
                            store(A, dot(dot(transpose(Y), c_u), Y)+ YtY),
                            store(b, dot(dot(transpose(Y),(c_u + I_i)), transpose(p_u))),
                            store(slice(X, list(u, u + 1, 1),nil), dot(inverse(A), b)),
                            store(u, u + 1)
                        )
                    ),
                    store(u, 0),
                    while(i < num_items,
                        block(
                            store(conf_i, slice_column(conf, i)),
                            store(c_i, diag(conf_i)),
                            store(p_i, __ne(conf_i, 0.0, true)),
                            store(A, dot(dot(transpose(X),c_i), X) + XtX),
                            store(b, dot(dot(transpose(X),(c_i + I_u)), transpose(p_i))),
                            store(slice(Y, list(i, i + 1, 1),nil), dot(inverse(A), b)),
                            store(i, i + 1)
                        )
                    ),
                    store(i, 0),
                    store(k, k + 1)
                )
            ),
            list(X, Y)
        )
    )
    als_explicit
)";

std::string const als_direct = R"(
    //
    // Alternating Least squares algorithm (ALS) (direct implementation)
    //
    define(als_direct, ratings, regularization, num_factors, iterations, alpha,
        enable_output, als(ratings, regularization, num_factors, iterations, alpha,
        enable_output)
    )
    als_direct
)";

///////////////////////////////////////////////////////////////////////////////
// Find the line/column position in the source code from a given iterator
// pointing into it.
//
std::pair<std::size_t, std::size_t> get_pos(std::string const& code,
    std::tuple<std::size_t, std::size_t, std::int64_t> const& tags)
{
    // Column might be given directly, in that case line is given as well
    if (std::get<2>(tags) != -1)
    {
        return std::make_pair(std::get<1>(tags), std::get<2>(tags));
    }

    // Otherwise the given value is the offset into the code
    std::size_t pos = std::get<1>(tags);
    std::size_t line = 1;
    std::size_t column = 1;

    for (std::int64_t i = 0; i != pos && i != code.size(); ++i)
    {
        if (code[i] == '\r' || code[i] == '\n')    // CR/LF
        {
            ++line;
            column = 1;
        }
        else
        {
            ++column;
        }
    }

    return std::make_pair(line, column);
}

///////////////////////////////////////////////////////////////////////////////
// Find offset into code as given by the tags argument
//
std::size_t get_offset(std::string const& code,
    std::tuple<std::size_t, std::size_t, std::size_t> const& tags)
{
    // Offset might be given directly
    if (std::get<2>(tags) == -1)
    {
        return std::get<1>(tags);
    }

    // Otherwise the given value is the line/column position in the code
    std::size_t offset = 0;
    std::size_t line = 1;
    std::size_t column = 0;

    for (std::int64_t i = 0; i != code.size(); ++i, ++offset)
    {
        if (code[i] == '\r' || code[i] == '\n')    // CR/LF
        {
            ++line;
            column = 0;
        }
        else
        {
            ++column;
        }

        if (std::get<1>(tags) == line && std::get<2>(tags) == column)
        {
            break;
        }
    }

    return offset;
}

// Extract the compile_id/tag pair from a given primitive instance name.
//
// The compile_id is a sequence number tracking invocations of the
// function phylanx::execution_tree::compile (needed to link back to the
// concrete source code compiled).
//
// The tag is an index into the array of iterators filled by
// phylanx::ast::generate_ast. It allows to find the iterator referring
// to the construct in the source code a particular primitive instance was
// created by.
//
std::tuple<std::size_t, std::size_t, std::size_t> extract_tags(
    std::string const& name)
{
    auto data = phylanx::execution_tree::compiler::parse_primitive_name(name);
    return std::make_tuple(data.compile_id, data.tag1, data.tag2);
}

// The symbolic names registered in AGAS that identify the created
// primitive instances have the following structure:
//
// /phylanx/<primitive>$<sequence-nr>[$<instance>]/<compile_id>$<tag1>[$<tag2>]
//
//  where:
//      <primitive>:   the name of primitive type representing the given
//                     node in the expression tree
//      <sequence-nr>: the sequence number of the corresponding instance
//                     of type <primitive>
//      <instance>:    (optional), some primitives have additional instance
//                     names, for instance references to function arguments
//                     have the name of the argument as their <instance>
//      <compile_id>:  the sequence number of the invocation of the
//                     function phylanx::execution_tree::compile
//      <tag1>:        if <tag2> == -1: the position inside the compiled code
//                     block where the referring to the point of usage of the
//                     primitive in the compiled source code
//                     if <tag2> != -1: the line number in the compiled code
//                     block where the referring to the point of usage of the
//                     primitive in the compiled source code
//      <tag2>:        (optional) if <tag2> != -1 or not given: the column
//                      offset in the given line (default: -1)
//
void print_instrumentation(char const* const name, int compile_id,
    std::string const& code,
    phylanx::execution_tree::compiler::function const& func,
    std::map<std::string, hpx::id_type> const& entries)
{
    std::cout << "Instrumentation information for function: " << name << "\n";

    for (auto const& e : entries)
    {
        // Extract compile_id and iterator index (tag) from the symbolic name
        auto tags = extract_tags(e.first);
        if (std::get<0>(tags) != compile_id)
            continue;

        // Find real position of given symbol in source code
        if (std::get<1>(tags) != std::size_t(-1))
        {
            auto pos = get_pos(code, tags);
            std::cout << e.first << ": " << name << "(" << pos.first << ", "
                      << pos.second << "): ";

            // Show the next (at max) 20 characters
            auto offset = get_offset(code, tags);
            auto end = code.begin() + offset;
            for (int i = 0; end != code.end() && i != 20; ++end, ++i)
            {
                if (*end == '\n' || *end == '\r')
                    break;
            }
            std::cout << std::string(code.begin() + offset, end) << " ...\n";
        }
        else
        {
            std::cout << e.first << "\n";
        }
    }

    std::cout << "\n";

    std::cout << "Tree information for function: " << name << "\n";
    std::cout << phylanx::execution_tree::newick_tree(
                     name, func.get_expression_topology())
              << "\n\n";

    std::cout << phylanx::execution_tree::dot_tree(
                     name, func.get_expression_topology())
              << "\n\n";
}

void print_performance_counter_data_csv(
    std::vector<std::string> const& existing_primitive_instances)
{
    std::cout << std::endl << "Primitive Performance Counter Data in CSV:";

    // CSV Header
    std::cout << "\nprimitive_instance,display_name,count,time,eval_direct\n";

    // Print performance data
    for (auto const& entry :
        phylanx::util::retrieve_counter_data(existing_primitive_instances))
    {
        std::cout << "\"" << entry.first << "\",\""
                  << phylanx::execution_tree::compiler::primitive_display_name(
                         entry.first)
                  << "\"";
        for (auto const& counter_value : entry.second)
        {
            std::cout << "," << counter_value;
        }
        std::cout << std::endl;
    }

    std::cout << std::endl;
}

int hpx_main(boost::program_options::variables_map& vm)
{
    if (vm.count("data_csv") == 0)
    {
        std::cerr << "Please specify '--data_csv=data-file'";
        return hpx::finalize();
    }

    // compile the given code
    phylanx::execution_tree::compiler::function_list snippets_read_x;
    auto const& code_read_x =
        phylanx::execution_tree::compile("read_x", read_x_code, snippets_read_x);

    phylanx::execution_tree::compiler::function_list snippets_als;
    auto const& code_als = phylanx::execution_tree::compile(
        vm.count("direct") != 0 ? als_direct : als_explicit, snippets_als);

    // Enable collection of performance data for all existing primitives
    auto primitives = phylanx::util::enable_measurements();

    auto read_x = code_read_x.run();
    auto als = code_als.run();

    // Print instrumentation information, if enabled
    if (vm.count("instrument") != 0)
    {
        auto entries = hpx::agas::find_symbols(hpx::launch::sync, "/phylanx/*");

        print_instrumentation("als", 0,
            vm.count("direct") != 0 ? als_direct : als_explicit, als, entries);
    }

    // evaluate generated execution tree
    auto row_start = static_cast<int64_t>(0);
    auto col_start = static_cast<int64_t>(0);
    auto row_stop = vm["row_stop"].as<std::int64_t>();
    auto col_stop = vm["col_stop"].as<std::int64_t>();

    auto regularization = vm["regularization"].as<double>();
    auto iterations = vm["iterations"].as<int64_t>();
    auto num_factors = vm["factors"].as<int64_t>();
    auto alpha = vm["alpha"].as<double>();
    auto filepath = vm["data_csv"].as<std::string>();

    bool enable_output = vm.count("enable_output") != 0;

    // Read the data from the files
    auto ratings = read_x(filepath, row_start, row_stop, col_start, col_stop);

    // Measure execution time
    hpx::util::high_resolution_timer t;

    // Evaluate ALS using the read data
    auto result =
        als(ratings, regularization, num_factors, iterations, alpha, enable_output);
    auto elapsed = t.elapsed();

    // Print performance counter data in CSV
    if (vm.count("instrument") != 0)
    {
        print_performance_counter_data_csv(primitives);
    }

    // Make sure all counters are properly initialized, don't reset current
    // counter values
    hpx::reinit_active_counters(false);

    auto result_r = phylanx::execution_tree::extract_list_value(result);
    auto it = result_r.begin();
    std::cout << "X: \n"
              << phylanx::execution_tree::extract_numeric_value(*it++)
              << "\nY: \n"
              << phylanx::execution_tree::extract_numeric_value(*it)
              << std::endl;
    std::cout << "time: " << t.elapsed() << std::endl;

    return hpx::finalize();
}

int main(int argc, char* argv[])
{
    // command line handling
    boost::program_options::options_description desc("usage: als [options]");
    desc.add_options()("enable_output,e",
        "enable progress output (default: false)")("instrument,i",
        "print instrumentation information (default: false)")("direct,d",
        "use direct implementation of ALS (default: false)")("iterations,n",
        boost::program_options::value<std::int64_t>()->default_value(3),
        "number of iterations (default: 10.0)")("factors,f",
        boost::program_options::value<std::int64_t>()->default_value(10),
        "number of factors (default: 10)")("alpha,a",
        boost::program_options::value<double>()->default_value(40),
        "alpha (default: 40)")("regularization,r",
        boost::program_options::value<double>()->default_value(0.1),
        "regularization (default: 0.1)")("data_csv",
        boost::program_options::value<std::string>(),
        "file name for reading data")("row_stop",
        boost::program_options::value<std::int64_t>()->default_value(10),
        "row_stop (default: 10)")("col_stop",
        boost::program_options::value<std::int64_t>()->default_value(100),
        "col_stop (default: 100)");
    return hpx::init(desc, argc, argv);
}


/// Implement some functions that absent in the origin HElib.
#ifndef CRYPT_GMM_HELIB_HPP
#define CRYPT_GMM_HELIB_HPP
#include <vector>
#include <NTL/lzz_p.h>
class FHEcontext;
class FHESecKey;
class Ctxt;
namespace NTL { class zz_pX; class ZZX; }
/// encode and decode without using the G(X) as the EncrypedArray does.
void rawEncode(NTL::zz_pX &out,
               std::vector<NTL::zz_pX> const& slots,
               FHEcontext const& context);
void rawEncode(NTL::ZZX &out,
               std::vector<NTL::zz_pX> const& slots,
               FHEcontext const& context);
void rawDecode(std::vector<NTL::zz_pX> &out,
               NTL::zz_pX const& poly,
               FHEcontext const& context);
void rawDecode(std::vector<NTL::zz_pX> &out,
               NTL::ZZX const& poly,
               FHEcontext const& context);
void rawDecode(std::vector<NTL::ZZX> &out,
               NTL::ZZX const& poly,
               FHEcontext const& context);

struct GMMPrecompTable {
    std::vector<long> beta_powers;
    NTL::mulmod_t inv_p;
};

std::vector<GMMPrecompTable> precompute_gmm_tables(FHEcontext const& context);
/// Extract inner products from the decrypted polynomial
void extract_inner_products(std::vector<long> &out,
                            NTL::ZZX const& poly,
                            std::vector<GMMPrecompTable> const& tables,
                            FHEcontext const& context);

void faster_decrypt(NTL::Vec<long> &out, FHESecKey const& key, Ctxt const &ctx);

void extract_inner_products(std::vector<long> &out,
                            NTL::Vec<long> const& poly,
                            std::vector<GMMPrecompTable> const& tables,
                            FHEcontext const& context);
#endif //CRYPT_GMM_HELIB_HPP


#include <boost/math/distributions/skew_normal.hpp>


#define _USE_MATH_DEFINES

#include "fastRect.h"

#include <Eigen/Core>
#include <Eigen/Dense>

std::vector<std::vector<fast::calPoint>> fast::rectROI(int center_x, int center_y, double angle, int range, int distance, bool direction, int skip_pixels) {

    if (range == 0) {
        HVERROR(error, "Invalid box roi range");
    }

    if (distance == 0) {
        HVERROR(error, "Invalid box distance");
    }

    if (skip_pixels == 0) {
        HVERROR(error, "Invalid search step");
    }

    int dimension = range * distance;

    int roi_mid_width = range / 2;
    int roi_mid_height = distance / 2;

    int start_x1 = 0;
    int start_y1 = 0;
    int end_x1 = 0;
    int end_y1 = 0;

    int start_x2 = 0;
    int start_y2 = 0;
    int end_x2 = 0;
    int end_y2 = 0;

    if (direction == false) {
        start_x1 = center_x - roi_mid_width;
        start_y1 = center_y + roi_mid_height;

        start_x2 = center_x + roi_mid_width;
        start_y2 = center_y + roi_mid_height;

        end_x1 = center_x - roi_mid_width;
        end_y1 = center_y - roi_mid_height;

        end_x2 = center_x + roi_mid_width;
        end_y2 = center_y - roi_mid_height;
    }
    else {
        start_x1 = center_x - roi_mid_width;
        start_y1 = center_y - roi_mid_height;

        start_x2 = center_x + roi_mid_width;
        start_y2 = center_y - roi_mid_height;

        end_x1 = center_x - roi_mid_width;
        end_y1 = center_y + roi_mid_height;

        end_x2 = center_x + roi_mid_width;
        end_y2 = center_y + roi_mid_height;
    }

    int size = 4;

    Eigen::MatrixXd trigonometric_matrix(2, 2);

    //trigonometric_matrix
    trigonometric_matrix(0, 0) = cos(angle * M_PI / 180);
    trigonometric_matrix(0, 1) = -sin(angle * M_PI / 180);
    trigonometric_matrix(1, 0) = sin(angle * M_PI / 180);
    trigonometric_matrix(1, 1) = cos(angle * M_PI / 180);

    //rotation_matrix
    Eigen::MatrixXd rotation_matrix(2, size);

    //base_matrix
    Eigen::MatrixXd base_matrix(2, size);

    rotation_matrix(0, 0) = start_x1 - center_x; // x           Start X1
    rotation_matrix(1, 0) = start_y1 - center_y; // y

    rotation_matrix(0, 1) = start_x2 - center_x; // x
    rotation_matrix(1, 1) = start_y2 - center_y; // y

    rotation_matrix(0, 2) = end_x1 - center_x;// x
    rotation_matrix(1, 2) = end_y1 - center_y; // y

    rotation_matrix(0, 3) = end_x2 - center_x; // x
    rotation_matrix(1, 3) = end_y2 - center_y; /// y

    // No problem
    for (int index = 0; index < 4; index++) {
        base_matrix(0, index) = center_x;
        base_matrix(1, index) = center_y;
    }

    Eigen::MatrixXd result = trigonometric_matrix * rotation_matrix + base_matrix;

    double rotated_start_x1 = result(0, 0);
    double rotated_start_y1 = result(1, 0);

    double rotated_start_x2 = result(0, 1);
    double rotated_start_y2 = result(1, 1);

    double rotated_end_x1 = result(0, 2);
    double rotated_end_y1 = result(1, 2);

    double rotated_end_x2 = result(0, 3);
    double rotated_end_y2 = result(1, 3);

    //startx1 = rotated_start_x1;
    //starty1 = rotated_start_y1;
    //startx2 = rotated_start_x2;
    //starty2 = rotated_start_y2;
    //endx1 = rotated_end_x1;
    //endy1 = rotated_end_y1;
    //endx2 = rotated_end_x2;
    //endy2 = rotated_end_y2;

    double diff_start_x = rotated_start_x2 - rotated_start_x1;
    double diff_start_y = rotated_start_y2 - rotated_start_y1;

    double increase_rate_x = diff_start_x / range;
    double increase_rate_y = diff_start_y / range;

    //Start Line Points
    //Start Line Points
    //Start Line Points
    //Start Line Points
    int range_align_size = (range)+(range % 4);
    std::vector<double> range_vec(range_align_size * 2);
    for (int index = 0; index < range * 2; index += 2) {
        int current_index = index;
        range_vec[index] = current_index / 2;
        range_vec[index + 1] = current_index / 2;
    }

    std::vector<fast::calPoint> start_line_xy(range_align_size);
    std::vector<fast::calPoint> end_line_xy(range_align_size);

    std::vector<double> increase_rate_xy_vec = { increase_rate_x, increase_rate_y,increase_rate_x, increase_rate_y };
    const __m256d simd_increase_rate_xy = _mm256_load_pd(increase_rate_xy_vec.data());

    std::vector<double> simd_start_xy_vec = { rotated_start_x1 , rotated_start_y1, rotated_start_x1 , rotated_start_y1 };
    const __m256d simd_start_xy = _mm256_load_pd(simd_start_xy_vec.data());

    std::vector<double> simd_end_xy_vec = { rotated_end_x1 , rotated_end_y1, rotated_end_x1 , rotated_end_y1 };
    const __m256d simd_end_xy = _mm256_load_pd(simd_end_xy_vec.data());

    const double* range_ptr = &range_vec[0];

    const fast::calPoint* start_result_xy_ptr = start_line_xy.data();
    const fast::calPoint* end_result_xy_ptr = end_line_xy.data();

    int chunk_size = sizeof(double) * 4;
    for (int index = 0; index < range * 2; index += 4) {

        __m256d range_chunk = _mm256_load_pd(range_ptr + index);

        __m256d chunk_mul = _mm256_mul_pd(simd_increase_rate_xy, range_chunk);

        // Start Line
        __m256d start_result = _mm256_add_pd(chunk_mul, simd_start_xy);

        // End Line
        __m256d end_result = _mm256_add_pd(chunk_mul, simd_end_xy);
        memcpy(((double*)start_result_xy_ptr + index), &start_result, chunk_size);
        memcpy(((double*)end_result_xy_ptr + index), &end_result, chunk_size);
    }

    //Start Line Points
    //Start Line Points
    //Start Line Points
    //Start Line Points

    int distance_align_size = (distance)+(distance % 4);
    std::vector<double> distance_vec(distance_align_size * 2);
    for (int index = 0; index < distance * 2; index += 2) {
        int current_index = index;
        distance_vec[index] = current_index / 2;
        distance_vec[index + 1] = current_index / 2;
    }

    std::vector<std::vector<fast::calPoint>> combine_vertical_xy;

    double diff_virtical_start_x = rotated_end_x1 - rotated_start_x1;
    double diff_virtical_start_y = rotated_end_y1 - rotated_start_y1;

    double increase_virtical_rate_x = diff_virtical_start_x / distance;
    double increase_virtical_rate_y = diff_virtical_start_y / distance;

    std::vector<double> increase_vertical_rate_xy_vec = { increase_virtical_rate_x, increase_virtical_rate_y,increase_virtical_rate_x, increase_virtical_rate_y };
    const __m256d simd_increase_vertical_rate_xy = _mm256_load_pd(increase_vertical_rate_xy_vec.data());

    for (int range_index = 0; range_index < range; range_index += skip_pixels) {

        auto start_vertical_point = start_line_xy[range_index];
        std::vector<double> simd_vertical_xy_vec = { start_vertical_point.x , start_vertical_point.y, start_vertical_point.x , start_vertical_point.y };
        const __m256d simd_vertical_xy = _mm256_load_pd(simd_vertical_xy_vec.data());

        const double* distance_ptr = &distance_vec[0];

        std::vector<fast::calPoint> vertical_xy;
        vertical_xy.resize(distance_align_size);
        const fast::calPoint* start_vertical_xy_ptr = vertical_xy.data();

        for (int index = 0; index < distance * 2; index += 4) {
            __m256d distance_chunk = _mm256_load_pd(distance_ptr + index);
            __m256d chunk_mul = _mm256_mul_pd(simd_increase_vertical_rate_xy, distance_chunk);
            __m256d start_result = _mm256_add_pd(chunk_mul, simd_vertical_xy);
            memcpy((((double*)start_vertical_xy_ptr) + index), &start_result, chunk_size);
        }

        combine_vertical_xy.push_back(vertical_xy);
    }

    return combine_vertical_xy;
}


// From : https://raw.githubusercontent.com/Smorodov/Multitarget-tracker/master/KalmanFilter/Kalman.h
#pragma once
#include <opencv2/opencv.hpp>
//#include <Eigen/Geometry>
// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/
class TKalmanFilter
{
	public:
		TKalmanFilter(const cv::Point3f &p, float dt = 0.05, float Accel_noise_mag = 0.5);
		cv::Point3f GetPrediction();
		cv::Point3f Update(const cv::Point3f &p);
	//	void adjustPrediction(const Eigen::Transform<double, 3, Eigen::Isometry> &delta_robot);
		void adjustPrediction(const cv::Point3f &delta_pos);
	private:
		cv::KalmanFilter kalman;
};


//
//  Copyright Markus Rickert 2008
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/blas/level3/gemm.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/std/valarray.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

namespace bindings = boost::numeric::bindings;

int
main(int argc, char** argv)
{
  {
    // a * b' = C ; a' * b = d

    boost::numeric::ublas::vector<double> a(3);
    for(std::size_t i = 0; i < a.size(); ++i) a(i) = i;
    std::cout << "a=" << a << std::endl;

    boost::numeric::ublas::vector<double> b(3);
    for(std::size_t i = 0; i < b.size(); ++i) b(i) = i;
    std::cout << "b=" << b << std::endl;

    boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
    boost::numeric::bindings::blas::gemm(
      1.0, a, bindings::trans(b), 0.0, c
    );
    std::cout << "c=" << c << std::endl;

    boost::numeric::ublas::vector<double> d(1);
    boost::numeric::bindings::blas::gemm(
      1.0, bindings::trans(a), b, 0.0, d
    );
    std::cout << "d=" << d << std::endl;
  }

  std::cout << std::endl;

  {
    // a * b' = C ; a' * b = d

    std::vector<double> a(3);
    for(std::size_t i = 0; i < a.size(); ++i) a[i] = i;
    std::cout << "a=[" << a.size() << "](";
    for(std::size_t i = 0; i < a.size(); ++i) std::cout << (i > 0 ? "," : "") << a[i];
    std::cout << ")" << std::endl;

    std::valarray<double> b(3);
    for(std::size_t i = 0; i < b.size(); ++i) b[i] = i;
    std::cout << "b=[" << b.size() << "](";
    for(std::size_t i = 0; i < b.size(); ++i) std::cout << (i > 0 ? "," : "") << b[i];
    std::cout << ")" << std::endl;

    boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
    boost::numeric::bindings::blas::gemm(
      1.0, a, bindings::trans(b), 0.0, c
    );
    std::cout << "c=" << c << std::endl;

    std::vector<double> d(1);
    boost::numeric::bindings::blas::gemm(
      1.0, bindings::trans(a), b, 0.0, d
    );
    std::cout << "d=[" << d.size() << "](";
    for(std::size_t i = 0; i < d.size(); ++i) std::cout << (i > 0 ? "," : "") << d[i];
    std::cout << ")" << std::endl;
  }

  std::cout << std::endl;

  {
    // a * b' = C ; a' * b = d

    double a[3];
    for(std::size_t i = 0; i < 3; ++i) a[i] = i;
    std::cout << "a=[" << 3 << "](";
    for(std::size_t i = 0; i < 3; ++i) std::cout << (i > 0 ? "," : "") << a[i];
    std::cout << ")" << std::endl;

    std::vector<double> b(3);
    for(std::size_t i = 0; i < b.size(); ++i) b[i] = i;
    std::cout << "b=[" << b.size() << "](";
    for(std::size_t i = 0; i < b.size(); ++i) std::cout << (i > 0 ? "," : "") << b[i];
    std::cout << ")" << std::endl;

    boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
    boost::numeric::bindings::blas::gemm(
      1.0, a, bindings::trans(b), 0.0, c
    );
    std::cout << "c=" << c << std::endl;

    std::valarray<double> d(1);
    boost::numeric::bindings::blas::gemm(
      1.0, bindings::trans(a), b, 0.0, d
    );
    std::cout << "d=[" << d.size() << "](";
    for(std::size_t i = 0; i < d.size(); ++i) std::cout << (i > 0 ? "," : "") << d[i];
    std::cout << ")" << std::endl;
  }

  return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_2.h>
#include <CGAL/Projection_traits_xy_3.h>
#include <CGAL/boost/graph/graph_traits_Triangulation_2.h>
#include <CGAL/boost/graph/iterator.h>

#include <boost/graph/filtered_graph.hpp>
#include <boost/foreach.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Epic;
typedef CGAL::Projection_traits_xy_3<Epic>  K;
typedef K::Point_2 Point;

typedef CGAL::Triangulation_2<K> Triangulation;

typedef boost::graph_traits<Triangulation>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Triangulation>::halfedge_descriptor halfedge_descriptor;
typedef boost::graph_traits<Triangulation>::halfedge_iterator halfedge_iterator;
typedef boost::graph_traits<Triangulation>::face_descriptor face_descriptor;
typedef boost::graph_traits<Triangulation>::vertex_iterator vertex_iterator;
typedef boost::graph_traits<Triangulation>::face_iterator face_iterator;
typedef boost::graph_traits<Triangulation>::edge_iterator edge_iterator;
typedef boost::graph_traits<Triangulation>::edge_descriptor edge_descriptor;

typedef std::map<vertex_descriptor,int> VertexIndexMap;
VertexIndexMap vertex_id_map;

typedef boost::associative_property_map<VertexIndexMap> VertexIdPropertyMap;
VertexIdPropertyMap vertex_index_pmap(vertex_id_map);

typedef std::map<halfedge_descriptor,int> HalfedgeIndexMap;
HalfedgeIndexMap halfedge_id_map;

typedef boost::associative_property_map<HalfedgeIndexMap> HalfedgeIdPropertyMap;
HalfedgeIdPropertyMap halfedge_index_pmap(halfedge_id_map);

int
main(int,char*[])
{
  Triangulation t;

  t.insert(Point(0.1,0,1));
  t.insert(Point(1,0,1));
  t.insert(Point(0.2,0.2, 2));
  t.insert(Point(0,1,2));
  t.insert(Point(0,2,3));

  vertex_iterator vit, ve;
  // Associate indices to the vertices
  int index = 0;
  // boost::tie assigns the first and second element of the std::pair
  // returned by boost::vertices to the variables vit and ve
  for(boost::tie(vit,ve) = vertices(t); vit!=ve; ++vit ){
    vertex_descriptor  vd = *vit;
    if(! t.is_infinite(vd)){
      vertex_id_map[vd]= index++;
    }
  }

  std::cerr << index << " vertices" << std::endl;
  index = 0;
  face_iterator fit,fe;
  for(boost::tie(fit,fe) = faces(t); fit!= fe; ++fit){
    face_descriptor fd = *fit;
    halfedge_descriptor hd = halfedge(fd,t);
    halfedge_descriptor n = next(hd,t);

    halfedge_descriptor nn = next(n,t);
    if(next(nn,t) != hd){
      std::cerr << "the face is not a triangle" << std::endl;
    }

    ++index;
  }

  std::cerr << index << " faces" << std::endl;
  index = 0;

  edge_iterator eit,ee;
  for(boost::tie(eit,ee) = edges(t); eit!= ee; ++eit){
    edge_descriptor ed = *eit;
    vertex_descriptor vd = source(ed,t);
    CGAL_USE(vd);
    ++index;
  }

  std::cerr << index << " edges" << std::endl;
  index = 0;

  halfedge_iterator hit,he;
  for(boost::tie(hit,he) = halfedges(t); hit!= he; ++hit){
    halfedge_descriptor hd = *hit;
    vertex_descriptor vd = source(hd,t);
    CGAL_USE(vd);
    ++index;
  }
  std::cerr << index << " halfedges" << std::endl;

  std::cerr << num_vertices(t) << " " << num_edges(t) << " " << num_halfedges(t) << " " << num_faces(t) << std::endl;

  typedef boost::property_map<Triangulation, boost::vertex_point_t>::type Ppmap;
  Ppmap ppmap = get(boost::vertex_point, t);

  BOOST_FOREACH(vertex_descriptor vd, vertices_around_target(*vertices(t).first, t)){
    std::cout <<  ppmap[vd] << std::endl;
  }

  ppmap[*(++vertices(t).first)] = Point(78,1,2);
  std::cout << " changed point of vertex " << ppmap[*(++vertices(t).first)] << std::endl;

  return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <memory>
#include "../include/trainer.h"
#include "../include/model.h"
#include "../include/optimizer.h"
#include "../datasets/include/mnist.h"

void display_history(MyDL::Trainer& trainer);

int main()
{
    using namespace Eigen;
    using namespace MyDL;
    using std::cout;
    using std::endl;
    using std::shared_ptr;
    using std::make_shared;

    int batch_size = 100;
    int input_size = 28*28;
    vector<int> hidden_size = {50};
    int output_size = 10;
    double lambda = 2.0;

    double learning_rate = 0.1;
    int epochs = 2;

    auto model = make_shared<MultiLayerModel>(input_size, hidden_size, output_size, lambda);
    auto optimizer = make_shared<SGD>(learning_rate);
    auto dataset = make_shared<MnistEigenDataset>(batch_size);

    Trainer trainer(model, optimizer, dataset, epochs=epochs);

    trainer.train();

    display_history(trainer);

    return 0;
}

// util.cppを作成し、その中に記述する？
void display_history(MyDL::Trainer& trainer)
{
    using std::cout;
    using std::endl;

    cout << "Train acc history: ";
    for (const auto &item : trainer.train_acc_history)
    {
        cout << item << " ";
    }
    cout << endl;

    cout << "Test acc history: ";
    for (const auto &item : trainer.test_acc_history)
    {
        cout << item << " ";
    }
    cout << endl;
}