//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
int
main()
{
  using namespace boost;
  GraphvizGraph g_dot;
  read_graphviz("figs/telephone-network.dot", g_dot);

  typedef adjacency_list < vecS, vecS, undirectedS, no_property,
    property < edge_weight_t, int > > Graph;
  Graph g(num_vertices(g_dot));
  property_map < GraphvizGraph, edge_attribute_t >::type
    edge_attr_map = get(edge_attribute, g_dot);
  graph_traits < GraphvizGraph >::edge_iterator ei, ei_end;
  for (tie(ei, ei_end) = edges(g_dot); ei != ei_end; ++ei) {
    int weight = lexical_cast < int >(edge_attr_map[*ei]["label"]);
    property < edge_weight_t, int >edge_property(weight);
    add_edge(source(*ei, g_dot), target(*ei, g_dot), edge_property, g);
  }

  typedef graph_traits < Graph >::vertex_descriptor Vertex;
  std::vector < Vertex > parent(num_vertices(g));
  property_map < Graph, edge_weight_t >::type weight = get(edge_weight, g);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  property_map<Graph, vertex_index_t>::type indexmap = get(vertex_index, g);
  std::vector<std::size_t> distance(num_vertices(g));
  prim_minimum_spanning_tree(g, *vertices(g).first, &parent[0], &distance[0],
                             weight, indexmap, default_dijkstra_visitor());
#else
  prim_minimum_spanning_tree(g, &parent[0]);
#endif

  int total_weight = 0;
  for (int v = 0; v < num_vertices(g); ++v)
    if (parent[v] != v)
      total_weight += get(weight, edge(parent[v], v, g).first);
  std::cout << "total weight: " << total_weight << std::endl;

  for (int u = 0; u < num_vertices(g); ++u)
    if (parent[u] != u)
      edge_attr_map[edge(parent[u], u, g_dot).first]["color"] = "black";
  std::ofstream out("figs/telephone-mst-prim.dot");
  graph_property < GraphvizGraph, graph_edge_attribute_t >::type &
    graph_edge_attr_map = get_property(g_dot, graph_edge_attribute);
  graph_edge_attr_map["color"] = "gray";
  write_graphviz(out, g_dot);

  return EXIT_SUCCESS;
}


/**  PrecompData_multi.h

	Copyright 2016 Pietro Mele
	Apache License 2.0
*/

#ifndef PRECOMP_DATA_MULTI_H
#define PRECOMP_DATA_MULTI_H

//#define PRECOMPDATA_DEVICE

#include <string>
#include <boost/multi_array.hpp>

#ifdef PRECOMPDATA_DEVICE
#include <vector>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/container/vector.hpp>
#endif

namespace Utilities {

/** PrecompData
	Set of points approximating a multidimensional function/hypersurface
	f: X --> Y
  */

template<
    typename TX = float,   /* data type of independent vector */
    typename TY = float,   /* data type of dependent vector   */
    int nx,                /* number of dimensions of the independent vector */
    int ny                 /* number of dimensions of the dependent vector   */
>
class PrecompData
{
public:

	// Data types for the dependent and independent data and indices
	typedef boost::multi_array<TX, nx> XData;
	typedef boost::multi_array<TY, ny> YData;
	typedef typename XData::index X;
	typedef typename YData::index Y;

public:

	PrecompData();
	PrecompData(const std::string _funcName);

	int          SetFunctionName(const std::string &_funcName);
	int          SetComment(const std::string &_comment);
	std::string  FunctionName() const;
	std::string  Comment()      const;
	int          SetOversampling(float ovs);
	float        Oversamping()  const { return overSampling; }

	int          nxDimensions() const { return nx; }
	int          nyDimensions() const { return ny; }

	// Precompute constant values
	int PreComputeValues();

	// Coordinate <--> index transformation
	size_t VectorToIndex(X x)      const;     // vector --> index
	size_t ScalarToIndex(TX x)     const;     // scalar --> index
	X      IndexToVector(size_t i) const;     // index  --> vector
	TX     IndexToScalar(size_t i) const;     // index  --> scalar

	/// Data loading

	// Regular grid, computed
	size_t  Set(Y (*Func)(X x), X xmin, X xmax, size_t nPoints);
	size_t  Set(TY (*Func)(TX x), TX xmin, TX xmax, size_t nPoints);

	// Automatic irregular grid, computed
	size_t  AutoSet(Y (*Func)(X x), X xmin, X xmax, size_t nPoints = 100);
	size_t  AutoSet(TY (*Func)(TX x), TX xmin, TX xmax, size_t nPoints = 100);

	// Regular grid, load from file
	size_t  Set(const std::string &dataFilename, X xmin, X xmax);

	// Irregular grid, load from file
	size_t  Set(const std::string &dataFilename);      // grid contained in the file

	/// Data retrieval

	// Range UNchecked, 0 degree interpolation accessors
	Y  operator()(X x)  const;
	TY operator()(TX x) const;

	// Range checked accessors; check Status()
	Y get(X x);

	// Range checked accessors, interpolated; check Status()
	Y  Interpolate(X x);
	TY Interpolate(TX x);

	int Status() const { return status; }

	int  Interpolation() const { return interpolation; }
	void Interpolation(int order);

	int RangeCheck(X x);
	int RangeCheck(TX x);

	// Get the whole value set
	int Get(std::vector<X> &_xData, std::vector<Y> &_yData) const;
	int Get(std::vector<TX> &_xData, std::vector<TY> &_yData) const;
	int Dump(int n = 0) const;
	int DumpElement(size_t j) const;

	// Evaluate error
	Y  EvaluateErrorKnownData() const;              // error on each dimension on known data
	TY EvaluateAbsErrorKnownData() const;           // absolute error on known data
	Y  EvaluateError(int nTestPoints) const;        // error on each dimension on random points
	TY EvaluateAbsError(int nTestPoints) const;     // absolute error on random points

	/// GPGPU

#ifdef PRECOMPDATA_DEVICE
	int InitDevice();

	int CopyOnDevice(boost::compute::device         &device,
	                 boost::compute::context        &context,
	                 boost::compute::command_queue  &queue,
	                 boost::compute::vector<T>      &device_line);

	int CopyOnDevice(boost::compute::device         **device      = nullptr,
	                 boost::compute::context        **context     = nullptr,
	                 boost::compute::command_queue  **queue       = nullptr,
	                 boost::compute::vector<T>      **device_line = nullptr);

	// Copy a subset
	int CopyOnDevice(T xbeg, T xend);
	int CopyOnDevice(T xbeg, T xend, T ybeg, T yend);
	int CopyOnDevice(T xbeg, T xend, T ybeg, T yend, T zbeg, T zend);
#endif // PRECOMPDATA_DEVICE

public:
	// Error return values
	static const int err_no_data              = -1,
	                 err_device_not_available = -2;

	// Warning return values
	static const int wrn_x_less_than_min      = -101,
	                 wrn_x_more_than_max      = -102,
	                 wrn_invalid_oversampling = -103;

public:
	// Test
	friend class PrecompData_test;

protected:
	TY Norm(const Y&) const;
	TY FirstDerivative(TX x1, TY y1, TX x2, TY y2) const;
	TY SecondDerivative(TX x1, TY y1, TX x2, TY y2, TX x3, TY y3) const;
	int PickBestPoints(Y (*Func)(X x), const size_t nPoints, const float overSampling = 2.0f);
	int PickBestPoints(TY (*Func)(TX x), const size_t nPoints, const float overSampling = 2.0f);

private:

	std::string     funcName, comment;
	int             interpolation;
	int             status;

	Y  (*FuncX)(X x);
	TY (*FuncTX)(TX x);

	XData   xData;
	YData   yData;
	X       min, max, step;
	X       kRealInt, kIntReal;     // conversion factors

	bool    regularGrid;            // true if points are equally spaced on all axes
	float   overSampling;

#ifdef PRECOMPDATA_DEVICE
	boost::compute::vector<T>  device_line;
#endif

};

} // Utilities

// Implementation include files
#include "PrecompData_impl.h"
#include "PrecompDataDevice_impl.h"
#include "PrecompData_dump.h"

#endif // PRECOMP_DATA_MULTI_H


#pragma once

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

template <class Scalar>
class Twist {
  using Vector3x = Eigen::Matrix<Scalar, 3, 1>;

 public:
  Twist() {}
  Twist(const Vector3x& rotation, const Vector3x& translation)
      : rotation_(rotation), translation_(translation) {}

  Twist(const Twist& twist)
      : rotation_(twist.rotation_), translation_(twist.translation_) {}

  const Vector3x& Translation() const { return translation_; }

  Vector3x& Translation() { return translation_; }

  const Vector3x& Rotation() const { return rotation_; }

  Vector3x& Rotation() { return rotation_; }

 private:
  Vector3x rotation_;
  Vector3x translation_;
};

template <class Scalar>
Twist<Scalar> operator*(const Twist<Scalar>& twist, const float scale) {
  return Twist<Scalar>{twist.Rotation() * scale, twist.Translation() * scale};
}

template <class Scalar>
Twist<Scalar> operator*(const float scale, const Twist<Scalar>& twist) {
  return Twist<Scalar>{twist.Rotation() * scale, twist.Translation() * scale};
}

/**
 * se3 -> SE3
 */
template <class Scalar>
Eigen::Transform<Scalar, 3, Eigen::Isometry> Exp(const Twist<Scalar>& T) {
  const auto& w = T.Rotation();
  const auto& v = T.Translation();

  const float t2 = w.dot(w);
  const float t = std::sqrt(t2);
  const bool is_small_angle = (t <= std::numeric_limits<float>::epsilon());
  const Eigen::AngleAxis<Scalar> rotation(t, is_small_angle ? w : w / t);

  if (is_small_angle) {
    const Eigen::Matrix<Scalar, 3, 1> position = v + 0.5 * w.cross(v);
    return Eigen::Translation<Scalar, 3>{position} * rotation;
  }

  const float ct = std::cos(t);
  const float st = std::sin(t);
  const float awxv = (1.0 - ct) / t2;
  const float av = (st / t);
  const float aw = (1.0 - av) / t2;

  const Eigen::Matrix<Scalar, 3, 1> position =
      av * v + aw * w.dot(v) * w + awxv * w.cross(v);

  return Eigen::Translation<Scalar, 3>{position} * rotation;
}

/**
 * SE3 -> se3
 */
template <class Scalar>
Twist<Scalar> Log(const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T) {
  Twist<Scalar> out;

  // Extract components.
  const Eigen::AngleAxis<Scalar> axa{T.linear()};
  const Eigen::Matrix<Scalar, 3, 1>& w = axa.axis() * axa.angle();
  const Eigen::Matrix<Scalar, 3, 1>& p = T.translation();

  // Set intermediate values.
  const float t = axa.angle();
  const float t2 = t * t;
  const bool is_small_angle = (t <= std::numeric_limits<float>::epsilon());

  if (is_small_angle) {
    const Eigen::Matrix<Scalar, 3, 1>& v = p + 0.5 * p.cross(w);
    return Twist<Scalar>{w, v};
  }

  const float st = std::sin(t);
  const float ct = std::cos(t);
  const float alpha = (t * st / (2.0 * (1.0 - ct)));
  const float beta = (1.0 / t2 - st / (2.0 * t * (1.0 - ct)));
  const Eigen::Matrix<Scalar, 3, 1>& v =
      (alpha * p - 0.5 * w.cross(p) + beta * w.dot(p) * w);
  return Twist<Scalar>{w, v};
}


//=====================================================
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//=====================================================
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#ifndef EIGEN3_INTERFACE_HH
#define EIGEN3_INTERFACE_HH

#include <Eigen/Eigen>
#include <vector>
#include "btl.hh"

using namespace Eigen;

template<class real, int SIZE=Dynamic>
class eigen3_interface
{

public :

  enum {IsFixedSize = (SIZE!=Dynamic)};

  typedef real real_type;

  typedef std::vector<real> stl_vector;
  typedef std::vector<stl_vector> stl_matrix;

  typedef Eigen::Matrix<real,SIZE,SIZE> gene_matrix;
  typedef Eigen::Matrix<real,SIZE,1> gene_vector;

  static inline std::string name( void )
  {
    return EIGEN_MAKESTRING(BTL_PREFIX);
  }

  static void free_matrix(gene_matrix & /*A*/, int /*N*/) {}

  static void free_vector(gene_vector & /*B*/) {}

  static BTL_DONT_INLINE void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){
    A.resize(A_stl[0].size(), A_stl.size());

    for (unsigned int j=0; j<A_stl.size() ; j++){
      for (unsigned int i=0; i<A_stl[j].size() ; i++){
        A.coeffRef(i,j) = A_stl[j][i];
      }
    }
  }

  static BTL_DONT_INLINE  void vector_from_stl(gene_vector & B, stl_vector & B_stl){
    B.resize(B_stl.size(),1);

    for (unsigned int i=0; i<B_stl.size() ; i++){
      B.coeffRef(i) = B_stl[i];
    }
  }

  static BTL_DONT_INLINE  void vector_to_stl(gene_vector & B, stl_vector & B_stl){
    for (unsigned int i=0; i<B_stl.size() ; i++){
      B_stl[i] = B.coeff(i);
    }
  }

  static BTL_DONT_INLINE  void matrix_to_stl(gene_matrix & A, stl_matrix & A_stl){
    int  N=A_stl.size();

    for (int j=0;j<N;j++){
      A_stl[j].resize(N);
      for (int i=0;i<N;i++){
        A_stl[j][i] = A.coeff(i,j);
      }
    }
  }

  static inline void matrix_matrix_product(const gene_matrix & A, const gene_matrix & B, gene_matrix & X, int  /*N*/){
    X.noalias() = A*B;
  }

  static inline void transposed_matrix_matrix_product(const gene_matrix & A, const gene_matrix & B, gene_matrix & X, int  /*N*/){
    X.noalias() = A.transpose()*B.transpose();
  }

//   static inline void ata_product(const gene_matrix & A, gene_matrix & X, int  /*N*/){
//     X.noalias() = A.transpose()*A;
//   }

  static inline void aat_product(const gene_matrix & A, gene_matrix & X, int  /*N*/){
    X.template triangularView<Lower>().setZero();
    X.template selfadjointView<Lower>().rankUpdate(A);
  }

  static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int  /*N*/){
    X.noalias() = A*B;
  }

  static inline void symv(const gene_matrix & A, const gene_vector & B, gene_vector & X, int  /*N*/){
    X.noalias() = (A.template selfadjointView<Lower>() * B);
//     internal::product_selfadjoint_vector<real,0,LowerTriangularBit,false,false>(N,A.data(),N, B.data(), 1, X.data(), 1);
  }

  template<typename Dest, typename Src> static void triassign(Dest& dst, const Src& src)
  {
    typedef typename Dest::Scalar Scalar;
    typedef typename internal::packet_traits<Scalar>::type Packet;
    const int PacketSize = sizeof(Packet)/sizeof(Scalar);
    int size = dst.cols();
    for(int j=0; j<size; j+=1)
    {
//       const int alignedEnd = alignedStart + ((innerSize-alignedStart) & ~packetAlignedMask);
      Scalar* A0 = dst.data() + j*dst.stride();
      int starti = j;
      int alignedEnd = starti;
      int alignedStart = (starti) + internal::first_aligned(&A0[starti], size-starti);
      alignedEnd = alignedStart + ((size-alignedStart)/(2*PacketSize))*(PacketSize*2);

      // do the non-vectorizable part of the assignment
      for (int index = starti; index<alignedStart ; ++index)
      {
        if(Dest::Flags&RowMajorBit)
          dst.copyCoeff(j, index, src);
        else
          dst.copyCoeff(index, j, src);
      }

      // do the vectorizable part of the assignment
      for (int index = alignedStart; index<alignedEnd; index+=PacketSize)
      {
        if(Dest::Flags&RowMajorBit)
          dst.template copyPacket<Src, Aligned, Unaligned>(j, index, src);
        else
          dst.template copyPacket<Src, Aligned, Unaligned>(index, j, src);
      }

      // do the non-vectorizable part of the assignment
      for (int index = alignedEnd; index<size; ++index)
      {
        if(Dest::Flags&RowMajorBit)
          dst.copyCoeff(j, index, src);
        else
          dst.copyCoeff(index, j, src);
      }
      //dst.col(j).tail(N-j) = src.col(j).tail(N-j);
    }
  }

  static EIGEN_DONT_INLINE void syr2(gene_matrix & A,  gene_vector & X, gene_vector & Y, int  N){
    // internal::product_selfadjoint_rank2_update<real,0,LowerTriangularBit>(N,A.data(),N, X.data(), 1, Y.data(), 1, -1);
    for(int j=0; j<N; ++j)
      A.col(j).tail(N-j) += X[j] * Y.tail(N-j) + Y[j] * X.tail(N-j);
  }

  static EIGEN_DONT_INLINE void ger(gene_matrix & A,  gene_vector & X, gene_vector & Y, int  N){
    for(int j=0; j<N; ++j)
      A.col(j) += X * Y[j];
  }

  static EIGEN_DONT_INLINE void rot(gene_vector & A,  gene_vector & B, real c, real s, int  /*N*/){
    internal::apply_rotation_in_the_plane(A, B, JacobiRotation<real>(c,s));
  }

  static inline void atv_product(gene_matrix & A, gene_vector & B, gene_vector & X, int  /*N*/){
    X.noalias() = (A.transpose()*B);
  }

  static inline void axpy(real coef, const gene_vector & X, gene_vector & Y, int  /*N*/){
    Y += coef * X;
  }

  static inline void axpby(real a, const gene_vector & X, real b, gene_vector & Y, int  /*N*/){
    Y = a*X + b*Y;
  }

  static EIGEN_DONT_INLINE void copy_matrix(const gene_matrix & source, gene_matrix & cible, int  /*N*/){
    cible = source;
  }

  static EIGEN_DONT_INLINE void copy_vector(const gene_vector & source, gene_vector & cible, int  /*N*/){
    cible = source;
  }

  static inline void trisolve_lower(const gene_matrix & L, const gene_vector& B, gene_vector& X, int  /*N*/){
    X = L.template triangularView<Lower>().solve(B);
  }

  static inline void trisolve_lower_matrix(const gene_matrix & L, const gene_matrix& B, gene_matrix& X, int  /*N*/){
    X = L.template triangularView<Upper>().solve(B);
  }

  static inline void trmm(const gene_matrix & L, const gene_matrix& B, gene_matrix& X, int  /*N*/){
    X.noalias() = L.template triangularView<Lower>() * B;
  }

  static inline void cholesky(const gene_matrix & X, gene_matrix & C, int  /*N*/){
    C = X;
    internal::llt_inplace<real,Lower>::blocked(C);
    //C = X.llt().matrixL();
//     C = X;
//     Cholesky<gene_matrix>::computeInPlace(C);
//     Cholesky<gene_matrix>::computeInPlaceBlock(C);
  }

  static inline void lu_decomp(const gene_matrix & X, gene_matrix & C, int  /*N*/){
    C = X.fullPivLu().matrixLU();
  }

  static inline void partial_lu_decomp(const gene_matrix & X, gene_matrix & C, int  N){
    Matrix<DenseIndex,1,Dynamic> piv(N);
    DenseIndex nb;
    C = X;
    internal::partial_lu_inplace(C,piv,nb);
//     C = X.partialPivLu().matrixLU();
  }

  static inline void tridiagonalization(const gene_matrix & X, gene_matrix & C, int  N){
    typename Tridiagonalization<gene_matrix>::CoeffVectorType aux(N-1);
    C = X;
    internal::tridiagonalization_inplace(C, aux);
  }

  static inline void hessenberg(const gene_matrix & X, gene_matrix & C, int  /*N*/){
    C = HessenbergDecomposition<gene_matrix>(X).packedMatrix();
  }

};

#endif


/* Boost example/horner.cpp
 * example of unprotecting rounding for a whole function computation
 *
 * Copyright Guillaume Melquiond 2002-2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors nor Polytechnic University make any
 * representation about the suitability of this software for any
 * purpose. It is provided "as is" without express or implied warranty.
 *
 * $Id: horner.cpp,v 1.2 2003/02/05 17:34:34 gmelquio Exp $
 */

#include <boost/numeric/interval.hpp>
#include <boost/numeric/interval/io.hpp>
#include <iostream>

// I is an interval class, the polynom is a simple array
template<class I>
I horner(const I& x, const I p[], int n) {

  // initialize and restore the rounding mode
  typename I::traits_type::rounding rnd;

  // define the unprotected version of the interval type
  typedef typename boost::numeric::interval_lib::unprotect<I>::type R;

  const R& a = x;
  R y = p[n - 1];
  for(int i = n - 2; i >= 0; i--) {
    y = y * a + (const R&)(p[i]);
  }
  return y;

  // restore the rounding mode with the destruction of rnd
}

int main() {
  typedef boost::numeric::interval<double> I;
  I p[3] = { -1.0, 0, 1.0 };
  I x = 1.0;
  std::cout << horner(x, p, 3) << std::endl;
  return 0;
}


#include <iostream>
#include <boost/random.hpp>

int main()
{
	std::size_t seed = 0;

	boost::mt19937 gen(seed);
	boost::random::uniform_int_distribution<> dist(0, 10);

	for (int i = 0; i < 3; ++i)
	{
		std::cout << dist(gen) << std::endl;
	}

	std::cout << "-- seed 재설정 --" << std::endl;

	gen.seed(seed);

	for (int i = 0; i < 3; ++i) {
		std::cout << dist(gen) << std::endl;
	}

	return 0;
}


/*SLAMBOOK2里面，使用智能指针实现G2O的版本*/

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // 重置
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }

  // 更新
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(update);
  }

  // 存盘和读盘：留空
  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

  // 计算曲线模型误差
  virtual void computeError() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x 值， y 值为 _measurement
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

  // 往图中增加边
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                // 设置连接的顶点
    edge->setMeasurement(y_data[i]);      // 观测数值
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
    optimizer.addEdge(edge);
  }

  // 执行优化
  cout << "start optimization" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}


/**
 * Interface class for all the modules
 */

#pragma once

#include <Eigen/Dense>

namespace DeepLearningFramework {
class Module {
public:
  virtual ~Module() = default;

  virtual void forward(Eigen::MatrixXf &out, const Eigen::MatrixXf &x) = 0;

  virtual void backward(Eigen::MatrixXf &ddout,
                        const Eigen::MatrixXf &dout) = 0;

  virtual void printDescription() = 0;

  virtual void setLR(float lr) = 0;

  virtual uint32_t getParametersCount() = 0;
};
}; // namespace DeepLearningFramework


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

#ifndef ITL_CG_INCLUDE
#define ITL_CG_INCLUDE

#include <cmath>
#include <cassert>
#include <iostream>
#include <boost/mpl/bool.hpp>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/itl/itl_fwd.hpp>
#include <boost/numeric/itl/iteration/basic_iteration.hpp>
#include <boost/numeric/itl/pc/identity.hpp>
#include <boost/numeric/itl/pc/is_identity.hpp>
#include <boost/numeric/mtl/operation/dot.hpp>
#include <boost/numeric/mtl/operation/unary_dot.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/operation/lazy.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace itl {

/// Conjugate Gradients without preconditioning
template < typename LinearOperator, typename HilbertSpaceX, typename HilbertSpaceB,
	   typename Iteration >
int cg(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
       Iteration& iter)
{
    mtl::vampir_trace<7001> tracer;
    using std::abs; using mtl::conj; using mtl::lazy;
    typedef HilbertSpaceX Vector;
    typedef typename mtl::Collection<HilbertSpaceX>::value_type Scalar;
    typedef typename Iteration::real                            Real;

    Scalar rho(0), rho_1(0), alpha(0), alpha_1(0);
    Vector p(resource(x)), q(resource(x)), r(resource(x)), z(resource(x));

    r = b - A*x;
    rho = dot(r, r);
    while (! iter.finished(Real(sqrt(abs(rho))))) {
	++iter;
	if (iter.first())
	    p = r;
	else
	    p = r + (rho / rho_1) * p;

	// q = A * p; alpha = rho / dot(p, q);
	(lazy(q)= A * p) || (lazy(alpha_1)= lazy_dot(p, q));
	alpha= rho / alpha_1;

	x += alpha * p;
	rho_1 = rho;
	(lazy(r) -= alpha * q) || (lazy(rho) = lazy_unary_dot(r));
    }

    return iter;
}

/// Conjugate Gradients
template < typename LinearOperator, typename HilbertSpaceX, typename HilbertSpaceB,
	   typename Preconditioner, typename Iteration >
int cg(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
       const Preconditioner& L, Iteration& iter)
{
    using pc::is_identity;
    if (is_identity(L))
	return cg(A, x, b, iter);

    mtl::vampir_trace<7002> tracer;
    using std::abs; using mtl::conj; using mtl::lazy;
    typedef HilbertSpaceX Vector;
    typedef typename mtl::Collection<HilbertSpaceX>::value_type Scalar;
    typedef typename Iteration::real                            Real;

    Scalar rho(0), rho_1(0), rr, alpha(0), alpha_1;
    Vector p(resource(x)), q(resource(x)), r(resource(x)), z(resource(x));

    r = b - A*x;
    rr = dot(r, r);
    while (! iter.finished(Real(sqrt(abs(rr))))) {
	++iter;
	(lazy(z)= solve(L, r)) || (lazy(rho)= lazy_dot(r, z));

	if (iter.first())
	    p = z;
	else
	    p = z + (rho / rho_1) * p;

	(lazy(q)= A * p) || (lazy(alpha_1)= lazy_dot(p, q));
	alpha= rho / alpha_1;

	x += alpha * p;
	rho_1 = rho;
	(lazy(r) -= alpha * q) || (lazy(rr) = lazy_unary_dot(r));
    }
    return iter;
}

/// Conjugate Gradients with ignored right preconditioner to unify interface
template < typename LinearOperator, typename HilbertSpaceX, typename HilbertSpaceB,
	   typename Preconditioner, typename RightPreconditioner, typename Iteration >
int cg(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
       const Preconditioner& L, const RightPreconditioner&, Iteration& iter)
{
    return cg(A, x, b, L, iter);
}

/// Solver class for CG method; right preconditioner ignored (prints warning if not identity)
template < typename LinearOperator, typename Preconditioner,
	   typename RightPreconditioner>
class cg_solver
{
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit cg_solver(const LinearOperator& A) : A(A), L(A)
    {
	if (!pc::static_is_identity<RightPreconditioner>::value)
	    std::cerr << "Right Preconditioner ignored!" << std::endl;
    }

    /// Construct solver from a linear operator and (left) preconditioner
    cg_solver(const LinearOperator& A, const Preconditioner& L) : A(A), L(L)
    {
	if (!pc::static_is_identity<RightPreconditioner>::value)
	    std::cerr << "Right Preconditioner ignored!" << std::endl;
    }

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceB, typename HilbertSpaceX, typename Iteration >
    int solve(const HilbertSpaceB& b, HilbertSpaceX& x, Iteration& iter) const
    {
	return cg(A, x, b, L, iter);
    }

    /// Perform one CG iteration on linear system
    template < typename HilbertSpaceB, typename HilbertSpaceX >
    int solve(const HilbertSpaceB& b, HilbertSpaceX& x) const
    {
	itl::basic_iteration<double> iter(x, 1, 0, 0);
	return solve(b, x, iter);
    }

  private:
    const LinearOperator& A;
    Preconditioner        L;
};

} // namespace itl

#endif // ITL_CG_INCLUDE


#pragma once

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics.hpp>

namespace nifty{
namespace features{

    namespace bacc = boost::accumulators;

    template<class T>
    class DefaultAccumulatedStatistics{
    public:

        typedef bacc::accumulator_set<
            T,
            bacc::stats<
                bacc::tag::count,
                bacc::tag::mean,
                bacc::tag::min,
                bacc::tag::max,
                bacc::tag::moment<2>,
                bacc::tag::moment<3>,
                bacc::tag::tail_quantile<bacc::right>
            >
        > AccType;

        typedef std::integral_constant<int, 1>  NPasses;
        typedef std::integral_constant<int, 11> NFeatures;

        DefaultAccumulatedStatistics(const std::size_t rightTailCacheSize = 1000)
        :   acc_(bacc::right_tail_cache_size = rightTailCacheSize){

        }
        DefaultAccumulatedStatistics & acc(const T & val, const std::size_t pass=0){
            acc_(val);
            return *this;
        }

        template<class RESULT_ITER>
        void result(RESULT_ITER rBegin, RESULT_ITER rEnd){
            using namespace boost::accumulators;
            const auto count = extract_result< tag::count>(acc_);
            const auto d = std::distance(rBegin,rEnd);
            NIFTY_ASSERT_OP(NFeatures::value,==,d);
            // 11 features
            auto mean = extract_result< tag::mean >(acc_);
            rBegin[0]  = mean;
            rBegin[1]  = mean*d;
            rBegin[2]  = extract_result< tag::min >(acc_);
            rBegin[3]  = extract_result< tag::max >(acc_);
            rBegin[4]  = replaceRotten(extract_result< tag::moment<2> >(acc_),0.0);
            rBegin[5]  = replaceRotten(extract_result< tag::moment<3> >(acc_),0.0);
            rBegin[6]  = replaceRotten(quantile(acc_, quantile_probability = 0.1 ), mean);
            rBegin[7]  = replaceRotten(quantile(acc_, quantile_probability = 0.25 ),mean);
            rBegin[8]  = replaceRotten(quantile(acc_, quantile_probability = 0.5 ), mean);
            rBegin[9]  = replaceRotten(quantile(acc_, quantile_probability = 0.75 ),mean);
            rBegin[10] = replaceRotten(quantile(acc_, quantile_probability = 0.90 ),mean);

        }

        std::size_t requiredPasses()const{
            return 1;
        }
        std::size_t nFeatures()const{
            return NFeatures::value;
        }
    private:

        T replaceRotten(const T & val, const T & replaceVal){
            if(std::isfinite(val))
                return val;
            else
                return replaceVal;
        }

        AccType acc_;

    };

}
}


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

/*
 Copyright (C) 2015 Klaus Spanderen

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

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

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

/*! \file analytichestonadengine.hpp
    \brief analytic Heston-model engine
*/

#ifndef quantlib_analytic_heston_ad_engine_hpp
#define quantlib_analytic_heston_ad_engine_hpp

#include <ql/math/integrals/integral.hpp>
#include <ql/math/integrals/gaussianadquadratures.hpp>
#include <ql/pricingengines/genericmodelengine.hpp>
#include <ql/models/equity/hestonmodel.hpp>
#include <ql/instruments/vanillaoption.hpp>

#include <boost/function.hpp>
#include <cppad/cppad.hpp>

#include <complex>

namespace QuantLib {

    //! analytic Heston-model engine based on Fourier transform
    class AnalyticHestonADEngine
        : public GenericModelEngine<HestonModel,
                                    VanillaOption::arguments,
                                    VanillaOption::results> {
      public:
        class Integration;
        enum ComplexLogFormula { Gatheral, BranchCorrection };

        // Constructor using Laguerre integration
        // and Gatheral's version of complex log.
        AnalyticHestonADEngine(const boost::shared_ptr<HestonModel>& model,
                               Size integrationOrder = 144);

        void calculate() const;
        Size numberOfEvaluations() const;

        static void doCalculation(Real riskFreeDiscount,
                                             Real dividendDiscount,
                                             Real spotPrice,
                                             Real strikePrice,
                                             Real term,
                                             Real kappa, Real theta, Real sigma, Real v0, Real rho,
                                             const TypePayoff& type,
                                             const Integration& integration,
                                             const ComplexLogFormula cpxLog,
                                             const AnalyticHestonADEngine* const enginePtr,
                                             VanillaOption::results& results,
                                             Size& evaluations);

      protected:

      private:
        class Fj_Helper;

        mutable Size evaluations_;
        const ComplexLogFormula cpxLog_;
        const boost::shared_ptr<Integration> integration_;
    };

    class AnalyticHestonADEngine::Integration {
      public:
        // non adaptive integration algorithms based on Gaussian quadrature
        static Integration gaussLaguerre    (Size integrationOrder = 128);
        static Integration gaussLegendre    (Size integrationOrder = 128);
        static Integration gaussChebyshev   (Size integrationOrder = 128);
        static Integration gaussChebyshev2nd(Size integrationOrder = 128);

        CppAD::AD<Real> calculate(
        	const boost::function<CppAD::AD<Real>(Real)>& f) const;

        Size numberOfEvaluations() const;

      private:
        enum Algorithm
            { GaussLaguerre, GaussLegendre,
              GaussChebyshev, GaussChebyshev2nd };

        Integration(Algorithm intAlgo,
                    const boost::shared_ptr<GaussianADQuadrature>& quadrature);

        const Algorithm intAlgo_;
        const boost::shared_ptr<GaussianADQuadrature> gaussianQuadrature_;
    };
}

#endif


#include <Eigen/Dense>
#include <iostream>
#include <Eigen/Sparse>
using namespace std;
using namespace Eigen;

template <class scalar>
struct TripletMatrix;

vector <Triplet<double>> triplet;
triplet=
SparseMatrix<double, row major> spMat(size_t n,size_t m);

Nummerical Methods for CSE/PS3/triplet.cpp


/** @file
 *****************************************************************************

 Arithmetic in the finite field Fp, for prime p of fixed length using
 NTL as the backend.

 *****************************************************************************
 * @author     Samir Menon, Brennan Shacklett, and David J. Wu
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/

#ifndef NTLFP_HPP_
#define NTLFP_HPP_

#include <cstddef>
#include <iostream>
#include <NTL/ZZ_p.h>
#include <libsnark/common/libsnark_serialization.hpp>
#include <libff/algebra/fields/bigint.hpp>

namespace libsnark {

template<unsigned long modulus>
class NTLFp_model;

template<unsigned long modulus>
std::ostream& operator<<(std::ostream &, const NTLFp_model<modulus>&);

template<unsigned long modulus>
std::istream& operator>>(std::istream &, NTLFp_model<modulus> &);

template<unsigned long modulus>
class NTLFp_model {
private:
    NTL::ZZ_p value;
public:
    static const constexpr unsigned long& mod = modulus;
    static size_t s; // log2(modulus) OR modulus = 2^s * t + 1
    static size_t t; // with t odd
    static NTLFp_model<modulus> multiplicative_generator; // generator of Fp^*
    static NTLFp_model<modulus> root_of_unity; // generator^((modulus-1)/2^s)m
    static size_t num_bits;

    NTLFp_model();
    NTLFp_model(long x);
    NTLFp_model(const NTLFp_model &other);
    NTLFp_model(const NTL::ZZ_p &value) : value(value) {}

    NTL::ZZ_p as_ZZ_p() const { return this->value; }
    static NTL::ZZ mod_zz() { return NTL::ZZ(modulus); }

    bool operator==(const NTLFp_model& other) const;
    bool operator!=(const NTLFp_model& other) const;
    bool is_zero() const;

    void print() const;

    NTLFp_model& operator+=(const NTLFp_model& other);
    NTLFp_model& operator-=(const NTLFp_model& other);
    NTLFp_model& operator*=(const NTLFp_model& other);
    NTLFp_model& operator/=(const NTLFp_model& other);
    NTLFp_model& operator^=(const NTLFp_model& other);
    NTLFp_model& operator^=(unsigned long pwr);
    NTLFp_model& operator^=(const libff::bigint<1>& pwr);

    NTLFp_model operator+(const NTLFp_model& other) const;
    NTLFp_model operator-(const NTLFp_model& other) const;
    NTLFp_model operator*(const NTLFp_model& other) const;
    NTLFp_model operator/(const NTLFp_model& other) const;
    NTLFp_model operator-() const;

    NTLFp_model squared() const;
    NTLFp_model& invert();
    NTLFp_model inverse() const;
    NTLFp_model sqrt() const;
    void get_s_and_t(unsigned long& s, unsigned long& t);

    NTLFp_model operator^(unsigned long pwr) const;
    NTLFp_model operator^(const libff::bigint<1>& pwr) const;
    NTLFp_model operator^(const NTLFp_model& other) const;

    static size_t size_in_bits() { return num_bits; }
    static size_t capacity() { return num_bits - 1; }
    static unsigned long field_char() { return modulus; }
    static NTLFp_model<modulus> geometric_generator() { return NTLFp_model<modulus>::multiplicative_generator; }
    static NTLFp_model<modulus> arithmetic_generator() { return 1; }

    static NTLFp_model<modulus> zero();
    static NTLFp_model<modulus> one();
    static NTLFp_model<modulus> random_element();

    friend std::ostream& operator<< <modulus>(std::ostream &out, const NTLFp_model<modulus> &p);
    friend std::istream& operator>> <modulus>(std::istream &in, NTLFp_model<modulus> &p);
};

template<unsigned long modulus>
size_t NTLFp_model<modulus>::num_bits;

template<unsigned long modulus>
size_t NTLFp_model<modulus>::s;

template<unsigned long modulus>
size_t NTLFp_model<modulus>::t;

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::multiplicative_generator;

template<unsigned long modulus>
NTLFp_model<modulus> NTLFp_model<modulus>::root_of_unity;

} // libsnark

#include "ntlfp.tcc"

#endif // NTLFP_HPP_


#include <Eigen/Core>
#include "mex.h"

using namespace Eigen;
using namespace std;

// TODO openmp version

// NOTE: mxSetProperty and possibly mxGetProperty make copies, even with
// classdef < handle! lame! I believe mxGetField does NOT make a copy, though I
// haven't tested it recently

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{

    /* SETUP */
    if (nrhs != 3) { mexErrMsgTxt("wrong number of arguments\n"); }

    //// pull out inputs

    // data
    if (!mxGetField(prhs[0],0,"data")) { mexErrMsgTxt("data missing 'data' field\n"); }
    int8_t *alldata = (int8_t *) mxGetData(mxGetField(prhs[0],0,"data"));
    int bigT = mxGetN(mxGetField(prhs[0],0,"data")); // total length of the data
    int num_subparts = mxGetM(mxGetField(prhs[0],0,"data")); // number of sub-parts

    if (!mxGetField(prhs[0],0,"resources")) { mexErrMsgTxt("data missing 'resources' field\n"); }
    int16_t *allresources = (int16_t *) mxGetData(mxGetField(prhs[0],0,"resources"));

    if (!mxGetField(prhs[0],0,"starts")) { mexErrMsgTxt("data missing 'starts' field\n"); }
    int32_t *starts = (int32_t *) mxGetData(mxGetField(prhs[0],0,"starts"));
    int num_sequences = max(mxGetN(mxGetField(prhs[0],0,"starts")),
            mxGetM(mxGetField(prhs[0],0,"starts")));

    if (!mxGetField(prhs[0],0,"lengths")) { mexErrMsgTxt("data missing 'lengths' field\n"); }
    int32_t *lengths = (int32_t *) mxGetData(mxGetField(prhs[0],0,"lengths"));

    // parameters struct
    if (!mxGetField(prhs[1],0,"learns")) { mexErrMsgTxt("model missing 'learns' field\n"); }
    double *learns = mxGetPr(mxGetField(prhs[1],0,"learns"));
    int num_resources = max(mxGetM(mxGetField(prhs[1],0,"learns")),
            mxGetN(mxGetField(prhs[1],0,"learns")));

    if (!mxGetField(prhs[1],0,"forgets")) { mexErrMsgTxt("model missing 'forgets' field\n"); }
    double *forgets = mxGetPr(mxGetField(prhs[1],0,"forgets"));

    if (!mxGetField(prhs[1],0,"guesses")) { mexErrMsgTxt("model missing 'guesses' field\n"); }
    double *guess = mxGetPr(mxGetField(prhs[1],0,"guesses"));

    if (!mxGetField(prhs[1],0,"slips")) { mexErrMsgTxt("model missing 'slips' field\n"); }
    double *slip = mxGetPr(mxGetField(prhs[1],0,"slips"));

    if (!mxGetField(prhs[1],0,"prior")) { mexErrMsgTxt("model missing 'prior' field\n"); }
    double prior = mxGetScalar(mxGetField(prhs[1],0,"prior"));

    Array2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        As.col(2*n) << 1-learns[n], learns[n];
        As.col(2*n+1) << forgets[n], 1-forgets[n];
    }

    // forward messages
    double *all_forward_messages = mxGetPr(prhs[2]);

    //// outputs

    // lhs outputs
    if (nlhs != 1) { mexErrMsgTxt("must have one output\n"); }
    plhs[0] = mxCreateDoubleMatrix(2,bigT,mxREAL);
    double *all_predictions = mxGetPr(plhs[0]);
    Map<Array2Xd,Aligned> predictions(mxGetPr(plhs[0]),2,bigT);

    /* COMPUTATION */

    for (int sequence_index=0; sequence_index < num_sequences; sequence_index++) {
        // NOTE: -1 because Matlab indexing starts at 1
        int sequence_start = ((int) starts[sequence_index]) - 1;
        int T = (int) lengths[sequence_index];

        int16_t *resources = allresources + sequence_start;
        Map<MatrixXd> forward_messages(all_forward_messages + 2*sequence_start,2,T);
        Map<MatrixXd> predictions(all_predictions + 2*sequence_start,2,T);

        predictions.col(0) = initial_distn;
        for (int t=0; t<T-1; t++) {
            predictions.col(t+1) = As.block(0,2*(resources[t]-1),2,2) * forward_messages.col(t);
        }
    }
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "vlasovpp/field.h"
#include "vlasovpp/weno.h"
#include "vlasovpp/fft.h"
#include "vlasovpp/array_view.h"
#include "vlasovpp/poisson.h"

#ifndef SIGMA
#define SIGMA (2.*std::sqrt(2.))
#endif
#define E_MAX (0.6)

/*
template <unsigned int i>
struct phi
{
  static std::complex<double>
  operator () ( std::complex<double> const & z ) {
    static std::valarray<std::complex<double>> coeff(i);
    coeff[0] = 1.;

    for ( unsigned int k=1 ; k<coeff.size() ; ++k ) {
      coeff[k] = coeff[k-1] * z / (double(k));
    }

    return (std::exp(z) - std::accumulate( std::begin(coeff) , std::end(coeff) , std::complex<double>(0.,0.) ))/(std::pow(z,i));
  }
};
*/
template <unsigned int i>
std::complex<double>
phi ( std::complex<double> const & _z )
{
  std::valarray<std::complex<double>> coeff(i);
  coeff[0] = 1.;

  std::complex<double> z = _z;
  if ( _z == 0. ) { z = std::complex<double>(1.,0.); }

  for ( unsigned int k=1 ; k<coeff.size() ; ++k ) {
    coeff[k] = coeff[k-1] * z / (double(k));
  }
  //std::copy(std::begin(coeff),std::end(coeff),std::ostream_iterator<std::complex<double>>(std::cout," . "));
  //std::cout << std::endl;

  if ( z != 0. ) {
    return (std::exp(z) - std::accumulate( std::begin(coeff) , std::end(coeff) , std::complex<double>(0.,0.) ))/(std::pow(z,i));
  }
  return coeff[i-1];
}

namespace o2 {
  template < typename _T , std::size_t NumDimsV >
  auto
  trp_v ( field<_T,NumDimsV> const & u , ublas::vector<_T> const& E )
  {
    field<_T,NumDimsV> trp(tools::array_view<const std::size_t>(u.shape(),NumDimsV+1));

    { auto k=0, km1=trp.size(0)-1;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[km1][i])/(2.*u.step.dv) );
      }
    }
    for ( auto k=1 ; k<trp.size(0)-1 ; ++k ) {
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }
    { auto k=trp.size(0)-1, kp1=0;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[kp1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }

    return trp;
  }
}

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*f.step.dx+f.range.x_min)
#define Vk(k) (k*f.step.dv+f.range.v_min)

int main(int,char**)
{
	std::size_t Nx = 135, Nv = 256 , Nb_iter=10;
	field<double,1> f(boost::extents[Nv][Nx]);

	f.range.v_min = -8.; f.range.v_max = 8.;
	f.step.dv = (f.range.v_max-f.range.v_min)/Nv;

  const double Kx = 0.3;
  f.range.x_min = 0.; f.range.x_max = 2./Kx*math::pi<double>();
	//f.range.x_min = 0.; f.range.x_max = 20.*math::pi<double>();
  //f.range.x_min = 0.; f.range.x_max = 4.0*math::pi<double>();
  //f.range.x_min = -8.; f.range.x_max = 8.;
	f.step.dx = (f.range.x_max-f.range.x_min)/Nx;

  // SIGMA is the CFL number 0.45 is E_max in our test case
	const double dt = 0.05;//SIGMA*f.step.dv/E_MAX; //1.606*f.step.dv/0.6; //0.005; //1.606*f.step.dv/0.6; //0.5*6.*math::pi<double>()/(Nv*f.range.v_max);

  //field<double,1> f_sol = f;
  //field<double,1> f_ini = f;

	ublas::vector<double> v (Nv,0.);
  ublas::vector<double> E (Nx,0.),rho(Nx);
  for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  //for ( std::size_t i=0 ; i<Nx ; ++i ) { E[i] = -Xi(i); }

	const double l = f.range.x_max-f.range.x_min;
	ublas::vector<double> kx(Nx);
	//for ( auto i=0 ; i<Nx/2+1 ; ++i )   { kx[i] = 2.*math::pi<double>()*i/l; }
	//for ( auto i=0 ; i<((Nx/2)) ; ++i ) { kx[i+Nx/2+1] = -kx[Nx/2-i]; }
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  double ui = 3.4;
  double alpha = 0.1;
  double Tc = 0.0001;

  //double np = 0.9 , nb = 0.2 , ui = 4.5;
  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      //f[k][i] = ( std::exp(-0.5*SQ(Vk(k)))*np/std::sqrt(2.*math::pi<double>()) + nb/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-ui)/0.25) )*(1.+0.04*std::cos(0.5*Xi(i)));

      // Bump on Tail
      f[k][i] = ( std::exp(-0.5*SQ(Vk(k)))*np/std::sqrt(2.*math::pi<double>()) + nb/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-ui)/0.25) )*(1.+0.04*std::cos(0.3*Xi(i)));
      // Landau dumpping test
      //f[k][i] = (1./std::sqrt(2*math::pi<double>()))*std::exp(-0.5*SQ(Vk(k)))*(1.+0.001*std::cos(0.5*Xi(i)));

      //f[k][i] = std::exp( -SQ(Xi(i)-6) );
      //f[k][i] = std::exp(-SQ(Xi(i)-3)/0.5 - SQ(Vk(k))/2.);
      //f_sol[k][i] = std::exp(-SQ(Xi(i)-3)/0.5 - SQ(Vk(k))/2.);
      //f[k][i] = SQ(v(k))*std::exp(-0.5*SQ(Vk(k)))/std::sqrt(2.*math::pi<double>())*(1.+0.01*std::cos(0.5*Xi(i)));
      //f_ini[k][i] = f[k][i];
      //f_sol[k][i] =  std::exp(-SQ(Xi(i)-3.-v[k]*Nb_iter*dt)/0.5 - SQ(Vk(k)-E[i]*Nb_iter*dt)/2.);

      // triple bump
      //f[k][i] = ( 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-ui)) + 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)+ui)) )*(1.+0.04*std::cos(Kx*Xi(i))) + ((1-alpha)/(std::sqrt(2.*math::pi<double>()*Tc))*std::exp(-0.5*SQ(Vk(k))/Tc));
    }
  }
  f.write("vphl/kin/init.dat");

  poisson<double> poisson_solver(Nx,l);
  rho = f.density();
  E = poisson_solver(rho);

  //double Tf = 60.;//2*math::pi<double>();
  double Tf = 40.;
  int i_t=0;

  std::cout << "Nx: " << Nx << "\n";
  std::cout << "Nv: " << Nv << "\n";
  std::cout << "v_min: " << f.range.v_min << "\n";
  std::cout << "v_max: " << f.range.v_max << "\n";
  std::cout << "x_min: " << f.range.x_min << "\n";
  std::cout << "x_max: " << f.range.x_max << "\n";
  std::cout << "dt: " << dt << "\n";
  std::cout << "dx: " << f.step.dx << "\n";
  std::cout << "dv: " << f.step.dv << "\n";
  std::cout << "Tf: " << Tf << "\n";
  std::cout << "f_0: " << "\"bot\"" << "\n";
  std::cout << std::endl;

  std::ofstream info("info.yaml");

  info << "Nx: " << Nx << "\n";
  info << "Nv: " << Nv << "\n";
  info << "v_min: " << f.range.v_min << "\n";
  info << "v_max: " << f.range.v_max << "\n";
  info << "x_min: " << f.range.x_min << "\n";
  info << "x_max: " << f.range.x_max << "\n";
  info << "dt: " << dt << "\n";
  info << "dx: " << f.step.dx << "\n";
  info << "dv: " << f.step.dv << "\n";
  info << "Tf: " << Tf << "\n";
  info << "f_0: " << "\"bot\"" << "\n";
  info << std::endl;
  info.close();

  ublas::vector<double> ee(int(std::ceil(Tf/dt))+1,0.);
  ublas::vector<double> Emax(int(std::ceil(Tf/dt))+1,0.);
  ublas::vector<double> H(int(std::ceil(Tf/dt))+1,0.);
  //ublas::vector<double> ee(Nb_iter);
  //ublas::vector<double> Emax(Nb_iter);
  //ublas::vector<double> H(Nb_iter);

  field<double,1> f1(tools::array_view<const std::size_t>(f.shape(),2)),f2(tools::array_view<const std::size_t>(f.shape(),2)),f3(tools::array_view<const std::size_t>(f.shape(),2)),f4(tools::array_view<const std::size_t>(f.shape(),2)),f5(tools::array_view<const std::size_t>(f.shape(),2));
  fft::spectrum_ hf(Nx),hf1(Nx),hf2(Nx),hf3(Nx),hf4(Nx),hf5(Nx),hEdvf(Nx),hEdvf1(Nx),hEdvf2(Nx),hEdvf3(Nx),hEdvf4(Nx);

  rho = f.density();
  E = poisson_solver(rho);
  //for ( int i=0 ; i<E.size() ; ++i ) { E[i] = 1.; }
  //for ( int i=0 ; i<E.size() ; ++i ) { std::cout << E[i] << " "; }
  //  std::cout << std::endl;

  //ee(i_t) = 0.;
  //for ( auto i=0 ; i<Nx ; ++i ) {
  //  ee(i_t) += SQ(E(i))*f.step.dx;
  //}
  //ee(i_t) = std::sqrt(ee(i_t));
  H(i_t) = energy(f,E);

#define L (-v(k)*I*kx[i])
//#define L (0.)
  while (  i_t*dt < Tf ) {
  //while (  i_t < Nb_iter ) {
  	//if (t%32==0) { std::cout<<"\r"<<t<<" "<<std::flush ; }
    std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt<<"\r"<<std::flush;
    Emax(i_t) = std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} ));
    ee(i_t) = 0.;
    for ( auto i=0 ; i<Nx ; ++i ) { ee(i_t) += SQ(E(i))*f.step.dx; }
    ee(i_t) = std::sqrt(ee(i_t));

    /**
    // exprk(2,2) =============================================================
    #define SCHEME "expRK22"
    // SIGMA = 0.551 (10^-2)
    E = poisson_solver(f.density());
    field<double,1> Edvf = o2::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp(-L*dt)*hf[i] + dt*phi<1>(dt*L)*hEdvf[i];
      }
      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver(f1.density());
    field<double,1> Edvf1 = o2::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&f[k][0]);
      hf1.fft(&f1[k][0]);
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf[i] = std::exp(-L*dt)*hf[i] + dt*( (phi<1>(dt*L)-phi<2>(dt*L))*hEdvf[i] + phi<2>(dt*L)*hEdvf1[i] );
      }

      hf.ifft(&(f[k][0]));
    }

    **/
    /**
    // Cox-Matthews ===========================================================
    #define SCHEME "CM"
    // SIGMA = 0.450 (10^-2)
    E = poisson_solver( f.density() );
    field<double,1> Edvf = o2::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp(-0.5*L*dt)*hf[i] + 0.5*dt*phi<1>(-0.5*L*dt)*hEdvf[i];
      }

      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver( f1.density() );
    field<double,1> Edvf1 = o2::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf2[i] = std::exp(-0.5*L*dt)*hf[i] + 0.5*dt*phi<1>(-0.5*L*dt)*hEdvf1[i];
      }

      hf2.ifft(&(f2[k][0]));
    }

    E = poisson_solver( f2.density() );
    field<double,1> Edvf2 = o2::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));
      hEdvf2.fft(&(Edvf2[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf3[i] = std::exp(-L*dt)*hf[i] + 0.5*dt*phi<1>(-0.5*L*dt)*(std::exp(-0.5*L*dt)-1.)*hEdvf[i] + dt*phi<1>(-0.5*L*dt)*hEdvf2[i];
      }

      hf3.ifft(&(f3[k][0]));
    }

    E = poisson_solver( f3.density() );
    field<double,1> Edvf3  = o2::trp_v(f3,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));
      hEdvf2.fft(&(Edvf2[k][0]));
      hEdvf3.fft(&(Edvf3[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf4[i] = std::exp(-L*dt)*hf[i] + dt*( (phi<1>(-L*dt)-3.*phi<2>(-L*dt)+4.*phi<3>(-L*dt))*hEdvf[i]
                                             + (2.*phi<2>(-L*dt)-4.*phi<3>(-L*dt))*(hEdvf1[i]+hEdvf2[i])
                                             + (-phi<2>(-L*dt)+4.*phi<3>(-L*dt))*hEdvf3[i] );
      }

      hf4.ifft(&(f[k][0]));
    }

    **/
    /**
    // Krogstad ===============================================================
    #define SCHEME "K"
    // SIGMA = 0.200 (10^-2)
    E = poisson_solver( f.density() );
    field<double,1> Edvf = o2::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp(-0.5*L*dt)*hf[i] + 0.5*dt*phi<1>(-0.5*L*dt)*hEdvf[i];
      }

      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver( f1.density() );
    field<double,1> Edvf1 = o2::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf2[i] = std::exp(-0.5*L*dt)*hf[i] + dt*(0.5*phi<1>(-0.5*L*dt)-phi<2>(-0.5*L*dt))*hEdvf[i] + dt*phi<2>(-0.5*L*dt)*hEdvf1[i];
      }

      hf2.ifft(&(f2[k][0]));
    }

    E = poisson_solver( f2.density() );
    field<double,1> Edvf2 = o2::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));
      hEdvf2.fft(&(Edvf2[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf3[i] = std::exp(-L*dt)*hf[i] + dt*(phi<1>(-L*dt)-2.*phi<2>(-0.5*L*dt))*hEdvf[i] + 2.*dt*phi<2>(-L*dt)*hEdvf2[i];
      }

      hf3.ifft(&(f3[k][0]));
    }

    E = poisson_solver( f3.density() );
    field<double,1> Edvf3  = o2::trp_v(f3,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));
      hEdvf2.fft(&(Edvf2[k][0]));
      hEdvf3.fft(&(Edvf3[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf4[i] = std::exp(-L*dt)*hf[i] + dt*( (phi<1>(-L*dt)-3.*phi<2>(-L*dt)+4.*phi<3>(-L*dt))*hEdvf[i]
                                             + (2.*phi<2>(-L*dt)-4.*phi<3>(-L*dt))*(hEdvf1[i]+hEdvf2[i])
                                             + (-phi<2>(-L*dt)+4.*phi<3>(-L*dt))*hEdvf3[i] );
      }

      hf4.ifft(&(f[k][0]));
    }

    **/
    /**
    // Hochbruck-Ostermann ====================================================
    #define SCHEME "HO"
    // SIGMA = 0.501 (10^-2)
    E = poisson_solver( f.density() );
    field<double,1> Edvf = o2::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp(-0.5*L*dt)*hf[i] + 0.5*dt*phi<1>(-0.5*L*dt)*hEdvf[i];
      }

      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver( f1.density() );
    field<double,1> Edvf1 = o2::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf2[i] = std::exp(-0.5*L*dt)*hf[i] + dt*(0.5*phi<1>(-0.5*L*dt)-phi<2>(-0.5*L*dt))*hEdvf[i] + dt*phi<2>(-0.5*L*dt)*hEdvf1[i];
      }

      hf2.ifft(&(f2[k][0]));
    }

    E = poisson_solver( f2.density() );
    field<double,1> Edvf2 = o2::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));
      hEdvf2.fft(&(Edvf2[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf3[i] = std::exp(-L*dt)*hf[i] + dt*(phi<1>(-L*dt)-2.*phi<2>(-L*dt))*hEdvf[i] + dt*phi<2>(-L*dt)*hEdvf1[i] + dt*phi<2>(-L*dt)*hEdvf2[i];
      }

      hf3.ifft(&(f3[k][0]));
    }

    E = poisson_solver( f3.density() );
    field<double,1> Edvf3  = o2::trp_v(f3,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));
      hEdvf2.fft(&(Edvf2[k][0]));
      hEdvf3.fft(&(Edvf3[k][0]));

#define a52 (0.5*phi<2>(-0.5*L*dt)-phi<3>(-L*dt)+0.25*phi<2>(-L*dt)-0.5*phi<3>(-0.5*L*dt))
#define a54 (0.25*phi<2>(-0.5*L*dt)-a52)
      for ( auto i=0 ; i<Nx ; ++i ) {
        hf4[i] = std::exp(-0.5*L*dt)*hf[i] + dt*(0.5*phi<1>(-0.5*L*dt)-2.*a52-a54)*hEdvf[i] + dt*a52*(hEdvf1[i]+hEdvf2[i]) + dt*(0.25*phi<2>(-0.5*L*dt)-a52)*hEdvf3[i];
      }
#undef a54
#undef a52

      hf4.ifft(&(f4[k][0]));
    }

    E = poisson_solver( f4.density() );
    field<double,1> Edvf4  = o2::trp_v(f4,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hf4.fft(&(f4[k][0]));
      hEdvf.fft(&(Edvf[k][0]));
      hEdvf1.fft(&(Edvf1[k][0]));
      hEdvf2.fft(&(Edvf2[k][0]));
      hEdvf3.fft(&(Edvf3[k][0]));
      hEdvf4.fft(&(Edvf4[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf5[i] = std::exp(-L*dt)*hf[i] + dt*( (phi<1>(-L*dt)-3.*phi<2>(-L*dt)+4.*phi<3>(-L*dt))*hEdvf[i]
                                             + (-phi<2>(-L*dt)+4.*phi<3>(-L*dt))*hEdvf3[i]
                                             + (4.*phi<2>(-L*dt)-8.*phi<3>(-L*dt))*hEdvf4[i] );
      }

      hf5.ifft(&(f[k][0]));
    }
    **/
    /**
    // RK(3,2) best ===========================================================
    #define SCHEME "RK32"
    // SIGMA = 2. (y_max)
    // SIGMA = 1.344 (WENO)
    E = poisson_solver(f.density());
    field<double,1> Edvf = o2::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp(0.5*L*dt)*( hf[i]-0.5*dt*hEdvf[i] );
      }
      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver(f1.density());
    Edvf = o2::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      //hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf2[i] = std::exp(0.5*L*dt)*hf[i] - 0.5*dt*hEdvf[i];
      }
      hf2.ifft(&(f2[k][0]));
    }

    E = poisson_solver(f2.density());
    Edvf = o2::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      //hf1.fft(&(f1[k][0]));
      //hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf[i] = std::exp(L*dt)*hf[i] - dt*std::exp(0.5*L*dt)*hEdvf[i];
      }
      hf.ifft(&(f[k][0]));
    }
    **/
    /**/
    // RK(3,3) ================================================================
    // RK(3,3) eq19 ===========================================================
    #define SCHEME "RK33"
    //#define SCHEME "RK33_eq19"
    // SIGMA = std::sqrt(3) (y_max)
    // SIGMA = 1.433 (WENO)
    E = poisson_solver(f.density());

    //std::cout << "\n" << L << " " << *std::max_element(E.begin(),E.end()) << " " << *std::min_element(E.begin(),E.end()) << std::endl;

    field<double,1> Edvf = weno::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp(L*dt)*( hf[i]-dt*hEdvf[i] );
        //hf1[i] = 0.5*std::exp((2./3.)*L*dt)*hf[i] + 0.5*std::exp((2./3.)*dt*L)*( hf[i] - (4./3.)*dt*hEdvf[i] );
      }
      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver(f1.density());
    Edvf = weno::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf2[i] = 0.75*std::exp(0.5*L*dt)*hf[i] + 0.25*std::exp(-0.5*L*dt)*( hf1[i]-dt*hEdvf[i] );
        //hf2[i] = (2./3.)*std::exp((2./3.)*dt*L)*hf[i] + (1./3.)*( hf1[i] - (4./3.)*dt*hEdvf[i] );
      }
      hf2.ifft(&(f2[k][0]));
    }

    E = poisson_solver(f2.density());
    Edvf = weno::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf[i] = (1./3.)*std::exp(L*dt)*hf[i] + (2./3.)*std::exp(0.5*L*dt)*( hf2[i]-dt*hEdvf[i] );
        //hf[i] = (59./128.)*std::exp(L*dt)*hf[i] + (15./128.)*std::exp(L*dt)*( 2.*hf1[i]*std::exp(-(2./3.)*L*dt) - hf[i] ) + (27./64.)*std::exp((1./3.)*dt*L)*( hf2[i] - (4./3.)*dt*hEdvf[i] );
      }
      hf.ifft(&(f[k][0]));
    }
    /**/
    /**
    // RK(4,4) ================================================================
    // RK(4,4) 3/8 rule =======================================================
    #define SCHEME "RK44"
    //#define SCHEME "RK44_38"
    // SIGMA = 2.*std::sqrt(2.) (y_max)
    // SIGMA = 1.731 (WENO)
    E = poisson_solver( f.density() );
    field<double,1> Edvf = weno::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp(0.5*L*dt)*( hf[i] - 0.5*dt*hEdvf[i] );
        //hf1[i] = std::exp((1./3.)*L*dt)*( hf[i] - (1./3.)*dt*hEdvf[i] );
      }

      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver( f1.density() );
    Edvf = weno::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf2[i] = std::exp(0.5*L*dt)*hf[i] - 0.5*dt*hEdvf[i] ;
        //hf2[i] = 2.*std::exp((2./3.)*L*dt)*hf[i] - std::exp((1./3.)*L*dt)*hf1[i] - dt*std::exp((1./3.)*L*dt)*hEdvf[i];
      }

      hf2.ifft(&(f2[k][0]));
    }

    E = poisson_solver( f2.density() );
    Edvf = weno::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf3[i] = std::exp(L*dt)*hf[i] - dt*std::exp(0.5*L*dt)*hEdvf[i];
        //hf3[i] = 2.*std::exp((2./3.)*L*dt)*hf1[i] - std::exp((1./3.)*L*dt)*hf2[i] - dt*std::exp((1./3.)*L*dt)*hEdvf[i];
      }

      hf3.ifft(&(f3[k][0]));
    }

    E = poisson_solver( f3.density() );
    Edvf  = weno::trp_v(f3,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf[i] = -(1./3.)*std::exp(L*dt)*hf[i] + (1./3.)*std::exp(0.5*L*dt)*hf1[i] + (2./3.)*std::exp(0.5*L*dt)*hf2[i] + (1./3.)*hf3[i] - (1./6.)*dt*hEdvf[i];
        //hf[i] = -(1./8.)*std::exp(L*dt)*hf[i] + 0.75*std::exp((1./3.)*L*dt)*hf2[i] + (3./8.)*hf3[i] - (1./8.)*dt*hEdvf[i];
      }

      hf.ifft(&(f[k][0]));
    }
    **/
    /**
    // RK(5,3) ================================================================
    #define SCHEME "RK53"
    E = poisson_solver(f.density());
    field<double,1> Edvf = weno::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp((1./7.)*L*dt)*hf[i] - (1./7.)*dt*std::exp((1./7.)*L*dt)*hEdvf[i];
      }
      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver(f1.density());
    Edvf = weno::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf2[i] = std::exp((3./16.)*L*dt)*hf[i] - (3./16.)*dt*std::exp((5./112.)*L*dt)*hEdvf[i];
      }
      hf2.ifft(&(f2[k][0]));
    }

    E = poisson_solver(f2.density());
    Edvf = weno::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf3[i] = std::exp((1./3.)*L*dt)*hf[i] - (1./3.)*dt*std::exp((7./48.)*L*dt)*hEdvf[i];
      }
      hf3.ifft(&(f3[k][0]));
    }

    E = poisson_solver(f3.density());
    Edvf = weno::trp_v(f3,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf4[i] = std::exp((2./3.)*L*dt)*hf[i] - (2./3.)*dt*std::exp((1./3.)*L*dt)*hEdvf[i];
      }
      hf4.ifft(&(f4[k][0]));
    }

    E = poisson_solver(f4.density());
    Edvf = weno::trp_v(f4,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf[i] = -0.75*std::exp(L*dt)*hf[i] + 1.75*std::exp((6./7.)*L*dt)*hf1[i] - 0.75*dt*std::exp((1./3.)*L*dt)*hEdvf[i];
      }
      hf.ifft(&(f[k][0]));
    }
    **/
    /**
    // DP5  ===================================================================
    #define SCHEME "DP5"
    E = poisson_solver(f.density());
    field<double,1> Edvf = weno::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp((1./5.)*L*dt)*hf[i] - (1./5.)*dt*std::exp((1./5.)*L*dt)*hEdvf[i];
      }
      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver(f1.density());
    Edvf = weno::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf2[i] = (5./8.)*std::exp((3./10.)*L*dt)*hf[i] + (3./8.)*std::exp((1./10.)*L*dt)*hf1[i] - (9./40.)*dt*std::exp((1./10.)*L*dt)*hEdvf[i];
      }
      hf2.ifft(&(f2[k][0]));
    }

    E = poisson_solver(f2.density());
    Edvf = weno::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf3[i] = (175./27.)*std::exp((4./5.)*L*dt)*hf[i] + (100./9.)*std::exp((3./5.)*L*dt)*hf1[i] - (448./27.)*std::exp(0.5*L*dt)*hf2[i] - (32./9.)*dt*std::exp(0.5*L*dt)*hEdvf[i];
      }
      hf3.ifft(&(f3[k][0]));
    }

    E = poisson_solver(f3.density());
    Edvf = weno::trp_v(f3,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf4[i] = (3551./6561.)*std::exp((8./9.)*L*dt)*hf[i] + (7420./2187.)*std::exp((31./45.)*L*dt)*hf1[i] - (37376./6561.)*std::exp((53./90.)*L*dt)*hf2[i] + (2014./729.)*std::exp((4./45.)*L*dt)*hf3[i] + (212./729.)*dt*std::exp((4./45.)*L*dt)*hEdvf[i];
      }
      hf4.ifft(&(f4[k][0]));
    }

    E = poisson_solver(f4.density());
    Edvf = weno::trp_v(f4,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hf4.fft(&(f4[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf5[i] = (313397./335808.)*std::exp(L*dt)*hf[i] + (424025./55968.)*std::exp((4./5.)*L*dt)*hf1[i] - (61400./5247.)*std::exp((7./10.)*L*dt)*hf2[i] + (96075./18656.)*std::exp((1./5.)*L*dt)*hf3[i] - (35721./37312.)*std::exp((1./9.)*L*dt)*hf4[i] + (5103./18656.)*dt*std::exp((1./9.)*L*dt)*hEdvf[i];
      }
      hf5.ifft(&(f5[k][0]));
    }

    E = poisson_solver(f5.density());
    Edvf = weno::trp_v(f5,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hf4.fft(&(f4[k][0]));
      hf5.fft(&(f5[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf[i] = -(563./3456.)*std::exp(L*dt)*hf[i] - (575./252.)*std::exp((4./5.)*L*dt)*hf1[i] + (31400./10017.)*std::exp((7./10.)*L*dt)*hf2[i] + (325./1344.)*std::exp((1./5.)*L*dt)*hf3[i] - (7533./6784.)*std::exp((1./9.)*L*dt)*hf4[i] + (33./28.)*hf5[i] - (11./84.)*dt*hEdvf[i];
      }
      hf.ifft(&(f[k][0]));
    }

    **/
    /**
    // RK(8,6) ================================================================
    #define SCHEME"RK86"
    E = poisson_solver(f.density());
    field<double,1> Edvf = weno::trp_v(f,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf1[i] = std::exp((1./9.)*L*dt)*hf[i] - (1./9.)*dt*std::exp((1./9.)*dt*L)*hEdvf[i];
      }
      hf1.ifft(&(f1[k][0]));
    }

    E = poisson_solver(f1.density());
    Edvf = weno::trp_v(f1,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf2[i] = (5./8.)*std::exp((1./6.)*L*dt)*hf[i] + (3./8.)*std::exp((1./18.)*L*dt)*hf1[i] - (1./8.)*dt*std::exp((1./18.)*L*dt)*hEdvf[i];
      }
      hf2.ifft(&(f2[k][0]));
    }

    E = poisson_solver(f2.density());
    Edvf = weno::trp_v(f2,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf3[i] = 2.*std::exp((1./3.)*L*dt)*hf[i] + 3.*std::exp((2./9.)*L*dt)*hf1[i] -4.*std::exp((1./6.)*L*dt)*hf2[i] - (2./3.)*dt*std::exp((1./6.)*L*dt)*hEdvf[i];
      }
      hf3.ifft(&(f3[k][0]));
    }

    E = poisson_solver(f3.density());
    Edvf = weno::trp_v(f3,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf4[i] = (305./1268.)*std::exp(0.5*L*dt)*hf[i] + (2817./1268.)*std::exp((7./18.)*L*dt)*hf1[i] - (927./317.)*std::exp((1./3.)*L*dt)*hf2[i] + (927./634.)*std::exp((1./6.)*L*dt)*hf3[i] - (321./1268.)*dt*std::exp((1./6.)*L*dt)*hEdvf[i];
      }
      hf4.ifft(&(f4[k][0]));
    }

    E = poisson_solver(f4.density());
    Edvf = weno::trp_v(f4,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hf4.fft(&(f4[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf5[i] = (3191./321.)*std::exp((2./3.)*L*dt)*hf[i] - (2436./107.)*std::exp((5./9.)*L*dt)*hf1[i] - (2404./107.)*std::exp(0.5*L*dt)*hf2[i] + (12330./107.)*std::exp((1./3.)*L*dt)*hf3[i] - (25340./321.)*std::exp((1./6.)*L*dt)*hf4[i] - 8.*dt*std::exp((1./6.)*L*dt)*hEdvf[i];
      }
      hf5.ifft(&(f5[k][0]));
    }

    E = poisson_solver(f5.density());
    Edvf = weno::trp_v(f5,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hf4.fft(&(f4[k][0]));
      hf5.fft(&(f5[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf6[i] = -(15130159./6286464.)*std::exp((5./6.)*L*dt)*hf[i] + (2014319./349248.)*std::exp((13./18.)*L*dt)*hf1[i] + (1194095./523872.)*std::exp((2./3.)*L*dt)*hf2[i] - (1471057./116416.)*std::exp(0.5*L*dt)*hf3[i] + (12601453./1571616.)*std::exp((1./3.)*L*dt)*hf4[i] - (433./19584.)*std::exp((1./6.)*L*dt)*hf5[i] - (33./1088.)*dt*std::exp((1./6.)*L*dt)*hEdvf[i];
      }
      hf6.ifft(&(f6[k][0]));
    }

    E = poisson_solver(f6.density());
    Edvf = weno::trp_v(f6,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hf4.fft(&(f4[k][0]));
      hf5.fft(&(f5[k][0]));
      hf6.fft(&(f6[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf7[i] = (805187./78966.)*std::exp(L*dt)*hf[i] - (2263766./48257.)*std::exp((8./9.)*L*dt)*hf1[i] + (2745422./144771.)*std::exp((5./6.)*L*dt)*hf2[i] + (2271108./48257.)*std::exp((2./3.)*L*dt)*hf3[i] - (13115270./434313.)*std::exp(0.5*L*dt)*hf4[i] - (227./2706.)*std::exp((1./3.)*L*dt)*hf5[i] + (888./451.)*std::exp((1./6.)*L*dt)*hf6[i] - (36./41.)*dt*std::exp((1./6.)*L*dt)*hEdvf[i];
      }
      hf7.ifft(&(f7[k][0]));
    }

    E = poisson_solver(f7.density());
    Edvf = weno::trp_v(f7,E);
    for ( auto k=0 ; k<f.size(0) ; ++k ) {
      hf.fft(&(f[k][0]));
      hf1.fft(&(f1[k][0]));
      hf2.fft(&(f2[k][0]));
      hf3.fft(&(f3[k][0]));
      hf4.fft(&(f4[k][0]));
      hf5.fft(&(f5[k][0]));
      hf6.fft(&(f6[k][0]));
      hf7.fft(&(f7[k][0]));
      hEdvf.fft(&(Edvf[k][0]));

      for ( auto i=0 ; i<Nx ; ++i ) {
        hf[i] = -(193999./179760.)*std::exp(L*dt)*hf[i] + (2487363./329560.)*std::exp((8./9.)*L*dt)*hf1[i] - (847909./164780.)*std::exp((5./6.)*L*dt)*hf2[i] - (1600251./329560.)*std::exp((2./3.)*L*dt)*hf3[i] + (362713./98868.)*std::exp(0.5*L*dt)*hf4[i] + (109./1232.)*std::exp((1./3.)*L*dt)*hf5[i] + (186./385.)*std::exp((1./6.)*L*dt)*hf6[i] + (41./140.)*hf7[i] - (41./840.)*dt*hEdvf[i];
      }
      hf.ifft(&(f[k][0]));
    }
    **/

    // end of time loop
    ++i_t;
    rho = f.density();
    E = poisson_solver(rho);
    //ee(i_t) = 0.;
    //for ( auto i=0 ; i<Nx ; ++i ) {
    //  ee(i_t) += SQ(E(i))*f.step.dx;
    //}
    //ee(i_t) = std::sqrt(ee(i_t));
    H(i_t) = energy(f,E);

//#define FOLDER "lukas/vp/"
#define FOLDER "vphl/kin/"
#define SPACE_SCHEME "weno"
/*
    if ( i_t == int(15./dt) ) {
      std::stringstream ss; ss << FOLDER << "vp_" << SCHEME << "_" << SPACE_SCHEME << "_15.dat";
      f.write(ss.str());
      std::cout << std::endl;
    }
    if ( i_t == int(20./dt) ) {
      std::stringstream ss; ss << FOLDER << "vp_" << SCHEME << "_" << SPACE_SCHEME << "_20.dat";
      f.write(ss.str());
      std::cout << std::endl;
    }
    if ( i_t == int(25./dt) ) {
      std::stringstream ss; ss << FOLDER << "vp_" << SCHEME << "_" << SPACE_SCHEME << "_25.dat";
      f.write(ss.str());
      std::cout << std::endl;
    }
    if ( i_t == int(30./dt) ) {
      std::stringstream ss; ss << FOLDER << "vp_" << SCHEME << "_" << SPACE_SCHEME << "_30.dat";
      f.write(ss.str());
      std::cout << std::endl;
    }
    if ( i_t == int(35./dt) ) {
      std::stringstream ss; ss << FOLDER << "vp_" << SCHEME << "_" << SPACE_SCHEME << "_35.dat";
      f.write(ss.str());
      std::cout << std::endl;
    }
*/
	} // while (  i_t*dt < Tf )
#undef L
  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt<<"   \r"<<std::endl;

  std::stringstream ss; ss << FOLDER << "vp_" << SCHEME << "_" << SPACE_SCHEME << "_dt" << dt << ".dat";
  f.write(ss.str());

  rho = f.density();
  auto dx_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<Xi(count++)<<" "<<y; return ss.str(); };
  ss.str(std::string());
  ss << FOLDER << "rho_" << SCHEME << "_" << SPACE_SCHEME << "_" << Tf << ".dat";
  std::ofstream of;
  of.open(ss.str()); ss.str(std::string());
  std::transform( rho.begin() , rho.end() , std::ostream_iterator<std::string>(of,"\n") , dx_y );
  of.close();

  ss.str(std::string());
  //std::ofstream of;
  std::size_t count = 0;
  auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<(count++)*dt<<" "<<y; return ss.str(); };
  ss << FOLDER << "ee_" << SCHEME << "_" << SPACE_SCHEME << "_10.dat";
  of.open(ss.str()); ss.str(std::string());
  for ( auto i=0; i<ee.size() ; ++i ) {
    of << i*dt <<" " << ee[i] << "\n";
  }
  of.close();
  ss << FOLDER << "H_" << SCHEME << "_" << SPACE_SCHEME << ".dat";
  of.open(ss.str()); ss.str(std::string());
  for ( auto i=0; i<H.size() ; ++i ) {
    of << i*dt <<" " << (H[i]-H[0])/std::abs(H[0]) << "\n";
  }

  //double h = std::abs(*std::max_element( H.begin() , H.end() , [&](double a,double b){return ( std::abs((a-H[0])/std::abs(H[0])) < std::abs((b-H[0])/std::abs(H[0])) );} ));
  //std::cout << dt << " " << std::abs((h-H[0])/std::abs(H[0])) << "\n";

  of.close();
  ss << FOLDER << "Emax_" << SCHEME << "_" << SPACE_SCHEME << "_10.dat";
  of.open(ss.str()); ss.str(std::string());
  std::transform( Emax.begin() , Emax.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

	return 0;
}


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

#ifndef BOOST_MATH_SPECIAL_HERMITE_HPP
#define BOOST_MATH_SPECIAL_HERMITE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{
namespace math{

// Recurrence relation for Hermite polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   hermite_next(unsigned n, T1 x, T2 Hn, T3 Hnm1)
{
   return (2 * x * Hn - 2 * n * Hnm1);
}

namespace detail{

// Implement Hermite polynomials via recurrence:
template <class T>
T hermite_imp(unsigned n, T x)
{
   T p0 = 1;
   T p1 = 2 * x;

   if(n == 0)
      return p0;

   unsigned c = 1;

   while(c < n)
   {
      std::swap(p0, p1);
      p1 = hermite_next(c, x, p0, p1);
      ++c;
   }
   return p1;
}

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type
   hermite(unsigned n, T x, const Policy&)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::hermite_imp(n, static_cast<value_type>(x)), "boost::math::hermite<%1%>(unsigned, %1%)");
}

template <class T>
inline typename tools::promote_args<T>::type
   hermite(unsigned n, T x)
{
   return boost::math::hermite(n, x, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SPECIAL_HERMITE_HPP


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include <mars/nearest_cov.h>
#include <Eigen/Dense>

namespace mars
{
NearestCov::NearestCov(const Eigen::MatrixXd& covariance) : cov_mat_(covariance)
{
  // Ensure the matrix is square
  assert(covariance.rows() == covariance.cols());
}

Eigen::MatrixXd NearestCov::EigenCorrectionUsingCovariance(NearestCovMethod method)
{
  Eigen::EigenSolver<Eigen::MatrixXd> vd(cov_mat_);

  Eigen::EigenSolver<Eigen::MatrixXd>::EigenvectorsType V(vd.eigenvectors());
  Eigen::EigenSolver<Eigen::MatrixXd>::EigenvalueType D(vd.eigenvalues());

  if (!(V.imag().isZero() && D.imag().isZero()))
  {
    std::cout << "Warning: Eigenvalue decomposition has imaginary components" << std::endl;
  }

  Eigen::MatrixXd V_real(V.real());
  Eigen::VectorXd D_real(D.real());

  // determine if the matrix is already positive-semi-definite
  bool no_negative_eigenvalues = true;
  for (int k = 0; k < D_real.size(); k++)
  {
    if (D_real[k] < 0)
    {
      no_negative_eigenvalues = false;
    }
  }

  if (no_negative_eigenvalues)
  {
    return cov_mat_;
  }

  Eigen::VectorXd D_corrected(D_real);

  // Correct the covariance matrix
  switch (method)
  {
    case NearestCovMethod::abs:
      // replace negative Eigenvalues with their absolut value
      for (int k = 0; k < D_corrected.size(); k++)
      {
        if (D_corrected[k] < 0)
        {
          D_corrected[k] = std::abs(D_corrected[k]);
        }
      }
      break;

    case NearestCovMethod::zero:
      // replace negative Eigenvalues with zero
      for (int k = 0; k < D_corrected.size(); k++)
      {
        if (D_corrected[k] < 0)
        {
          D_corrected[k] = 0.0;
        }
      }
      break;

    case NearestCovMethod::delta:
      // replace negative Eigenvalues with a positive delta
      for (int k = 0; k < D_corrected.size(); k++)
      {
        if (D_corrected[k] < 0)
        {
          D_corrected[k] = delta_;
        }
      }
      break;

    default:
      std::cout << "Warning: Unexpected method for nearest_cov" << std::endl;
      break;
  }

  Eigen::MatrixXd result(V_real * D_corrected.asDiagonal() * V_real.inverse());
  return result;
}

Eigen::MatrixXd NearestCov::EigenCorrectionUsingCorrelation(NearestCovMethod method)
{
  // TODO
  return {};
}
}


#include "ExtendedKalmanFilter4d.h"

#include <Eigen/Eigenvalues>

ExtendedKalmanFilter4d::ExtendedKalmanFilter4d(const Eigen::Vector4d& state, const Eigen::Matrix2d& processNoiseStdSingleDimension, const Eigen::Matrix2d& measurementNoiseCovariances, const Eigen::Matrix2d& initialStateStdSingleDimension):
    x(state)
{
    Eigen::Matrix2d q;
    q = processNoiseStdSingleDimension.cwiseProduct(processNoiseStdSingleDimension);

    // covariance matrix of process noise (values taken from old kalman filter)
    Q << q, Eigen::Matrix2d::Zero(), Eigen::Matrix2d::Zero(), q;

    // covariance matrix of measurement noise
    Eigen::Matrix2d r;
    R << measurementNoiseCovariances;

    // inital covariance matrix of current state (values taken from old kalman filter)
    //double p = 62500;
    Eigen::Matrix2d p;
    p = initialStateStdSingleDimension.cwiseProduct(initialStateStdSingleDimension);
    P << p, Eigen::Matrix2d::Zero(), Eigen::Matrix2d::Zero(), p;

    P_pre = P;  // covariance matrix of predicted state
    P_corr = P; // covariance matrix of corrected state

    x_pre = x;
    x_corr = x;

    updateEllipses();
}

ExtendedKalmanFilter4d::~ExtendedKalmanFilter4d()
{

}

void ExtendedKalmanFilter4d::predict(const Eigen::Vector2d& u, double dt)
{
    // adapt state transition matrix
    F << 1, dt, 0, 0,
         0,  1, 0, 0,
         0,  0, 1, dt,
         0,  0, 0, 1;

    // adapt control matrix to dt
    B << dt*dt/2, 0,
         dt     , 0,
         0      , dt*dt/2,
         0      , dt;

    // predict
    x_pre = F * x + B * u;
    P_pre = F * P * F.transpose() + Q;

    x = x_pre;
    P = P_pre;

    updateEllipses();
}

void ExtendedKalmanFilter4d::update(const Eigen::Vector2d& z, const Measurement_Function_H& h)
{
    Eigen::Vector2d predicted_measurement;

    predicted_measurement = getStateInMeasurementSpace(h);

    // approximate H with central differential quotient
    H = approximateH(h);

    Eigen::Matrix2d temp1 = H * P_pre * H.transpose() + R;

    K = P_pre * H.transpose() * temp1.inverse();

    // just for debugging - begin
    x_pre = x;
    P_pre = P;
    // just for debugging - end

    x_corr = x_pre + K * (z - predicted_measurement);

    P_corr = (Eigen::Matrix4d::Identity()-K*H)*P_pre;

    x = x_corr;
    P = P_corr;

    updateEllipses();
}

Eigen::Matrix<double,2,4> ExtendedKalmanFilter4d::approximateH(const Measurement_Function_H &h) const {
    double e = 1e-4;
    Eigen::Vector2d dx1, dx2, dy1, dy2;

    dx1 = h(x(0)-e,x(2));
    dx2 = h(x(0)+e,x(2));

    dy1 = h(x(0),x(2)-e);
    dy2 = h(x(0),x(2)+e);

    Eigen::Vector2d dx = (dx2-dx1)/(2*e);
    Eigen::Vector2d dy = (dy2-dy1)/(2*e);

    Eigen::Matrix<double,2,4> H_approx;
    H_approx << dx(0), 0, dy(0), 0,
                dx(1), 0, dy(1), 0;

    return H_approx;
}

//--- setter ---//

void ExtendedKalmanFilter4d::setState(Eigen::Vector4d& state)
{
    x = state;
}

void ExtendedKalmanFilter4d::setCovarianceOfProcessNoise(const Eigen::Matrix2d& q){
    Q << q, Eigen::Matrix2d::Zero(), Eigen::Matrix2d::Zero(), q;
}

void ExtendedKalmanFilter4d::setCovarianceOfState(const Eigen::Matrix4d& p){
    P << p;
}

void ExtendedKalmanFilter4d::setCovarianceOfMeasurementNoise(const Eigen::Matrix2d& r){
    R << r;
}

//--- getter ---//
Eigen::Matrix2d ExtendedKalmanFilter4d::getStateCovarianceInMeasurementSpace(const Measurement_Function_H& h) const // horizontal, vertical
{
    Eigen::Matrix<double,2,4> H_approx;

    H_approx = approximateH(h);

    return H_approx * P * H_approx.transpose();
}

void ExtendedKalmanFilter4d::updateEllipses()
{
    Eigen::EigenSolver< Eigen::Matrix<double,2,2> > es;

    // determine error ellipse for the location
    Eigen::Matrix2d loc_cov;

    loc_cov << P(0,0), P(0,2),
               P(2,0), P(2,2);

    es.compute(loc_cov,true);

    if(std::abs(es.eigenvalues()[0]) > std::abs(es.eigenvalues()[1]))
    {
        ellipse_location.minor = std::sqrt(5.99*std::abs(es.eigenvalues()[1]));
        ellipse_location.major = std::sqrt(5.99*std::abs(es.eigenvalues()[0]));
        ellipse_location.angle = std::atan2(es.eigenvectors()(1,1).real(),es.eigenvectors()(0,1).real());
    } else {
        ellipse_location.minor = std::sqrt(5.99*std::abs(es.eigenvalues()[0]));
        ellipse_location.major = std::sqrt(5.99*std::abs(es.eigenvalues()[1]));
        ellipse_location.angle = std::atan2(es.eigenvectors()(1,0).real(),es.eigenvectors()(0,0).real());
    }

    // determine error ellipse of the velocity
    Eigen::Matrix2d vol_cov;

    vol_cov << P(1,1), P(1,3),
               P(3,1), P(3,3);

    es.compute(vol_cov,true);

    if(std::abs(es.eigenvalues()[0]) > std::abs(es.eigenvalues()[1]))
    {
        ellipse_velocity.minor = std::sqrt(5.99*std::abs(es.eigenvalues()[1]));
        ellipse_velocity.major = std::sqrt(5.99*std::abs(es.eigenvalues()[0]));
        ellipse_velocity.angle = std::atan2(es.eigenvectors()(1,1).real(),es.eigenvectors()(0,1).real());
    } else {
        ellipse_velocity.minor = std::sqrt(5.99*std::abs(es.eigenvalues()[0]));
        ellipse_velocity.major = std::sqrt(5.99*std::abs(es.eigenvalues()[1]));
        ellipse_velocity.angle = std::atan2(es.eigenvectors()(1,0).real(),es.eigenvectors()(0,0).real());
    }
}