#ifndef RASTERIZER_HPP
#define RASTERIZER_HPP

#include <algorithm>
#include <boost/geometry/geometries/geometries.hpp>
#include <limits>

namespace rasterize {

namespace bg = boost::geometry;

/*  rasterize a polygon implementation  */

template <typename point_t, typename float_t = double, typename int_t = int64_t>
struct Edge {
  typedef bg::model::segment<point_t> segment_t;
  float_t x, xNorm, yNorm;
  float_t minX, maxX;
  float_t slope;
  int_t yMax, yMin; // min-max of edge
  float_t dX, dY;
  segment_t line;
  Edge(const point_t &a, const point_t &b) : line{a, b} {
    auto &pMin = (bg::get<1>(a) < bg::get<1>(b) ? a : b);
    auto &pMax = (bg::get<1>(a) < bg::get<1>(b) ? b : a);

    yMax = bg::get<1>(pMax);
    yMin = bg::get<1>(pMin);

    x = bg::get<0>(pMin);                          // start x
    minX = std::min(bg::get<0>(a), bg::get<0>(b)); // start x
    maxX = std::max(bg::get<0>(a), bg::get<0>(b)); // start x

    dX = (bg::get<0>(pMax) - bg::get<0>(pMin));
    dY = (bg::get<1>(pMax) - bg::get<1>(pMin));
    slope = dY != 0 ? slope = dX / dY : 0;

    auto yOff = yMin - std::floor(yMin);
    auto xOff = x - std::floor(x);

    yNorm = std::round(yMin) + 0.5;
    xNorm = x + (yNorm - yMin) * slope;
  }

  friend std::ostream &operator<<(std::ostream &stream, const Edge &edge) {
    return std::cout << "Edge bucket: " << edge.yMin << "=>" << edge.yMax
                     << "dx=" << edge.dX << "; dy=" << edge.dY
                     << "; x=" << edge.x << ";" << std::endl;
  }
};

template <typename point_t, typename float_t = double> struct Rasterizer {
  typedef bg::model::polygon<point_t> polygon_t;
  typedef Edge<point_t, float_t> edge_t;
  struct {
    bool operator()(const edge_t &a, const edge_t &b) const {
      return a.yMin < b.yMin;
    }
  } yMinCompare;

  enum { STEP_INTERSECT, STEP_RASTERIZE };
  std::list<edge_t> ET; // edge table
  std::list<edge_t> AL; // active list

  int scanline;

  std::pair<bool, int> custom_scanline;

  void set_scanline(int _scanline) {
    custom_scanline = std::make_pair(true, _scanline);
  }

  void clear_scanline() { custom_scanline = std::make_pair(false, 0); }

  void clear() {
    clear_scanline();
    ET.clear();
    AL.clear();
  }

  void init(const polygon_t &p) {
    clear();
    if (p.outer().size() < 3) // don't render invalid polygons
      return;
    // build edge table containing all edges n=>0, 0=>1, ...n-1=>n
    edge_t e = {p.outer().back(), p.outer()[0]};
    if (e.dY != 0)
      ET.push_back(e);
    for (size_t i = 0; i < p.outer().size() - 1; i++) {
      e = {p.outer()[i], p.outer()[i + 1]};
      if (e.dY != 0)
        ET.push_back(e);
    }
    // sort according to yMin
    ET.sort(yMinCompare);

    //	std::cout << bg::wkt(p) << std::endl;
    if (ET.empty())
      return;

    if (!custom_scanline.first) {
      scanline = ET.front().yMin;
    } else {
      auto _scanline = custom_scanline.second;
      if (ET.front().yMin != _scanline)
        //	    std::cout << "Starting at different scanlines: " <<
        // ET.front().yMin << " vs." << scanline << std::endl;
        scanline = ET.front().yMin;
      //	    std::cout << "silent step should now do " << scanline << "
      // until " << _scanline << std::endl;
      while (scanline < _scanline) {
        //	      std::cout << "silent step:" << scanline << " towards " <<
        //_scanline << std::endl;
        step_intersect([](int x, int y) {});
      }
    }
  }

  bool done() { return (ET.empty() && AL.empty()); }
  template <typename func> void step_intersect(func putpixel) {
#ifdef DEBUG_LOG
    std::cout << "scanline(" << scanline << "):" << std::endl;
#endif
    // all edges that start here are moved from ET to AL
    for (auto it = ET.begin(); it != ET.end();) {
      if (std::floor(it->yMin - 1) <= scanline) {
        AL.push_back(*it);
#ifdef DEBUG_LOG
        std::cout << "erase (" << it->yMin << ") ";
#endif
        it = ET.erase(it);
      } else
        it++;
    }
#ifdef DEBUG_LOG
    std::cout << "active(" << AL.size() << ") ";
#endif
    // all edges that end here, are removed from AL
    for (auto it = AL.begin(); it != AL.end();) {
      if (std::ceil(it->yMax + 1) < scanline) {
        it = AL.erase(it);
      } else
        it++;
    }

    // theoretic: for multipolygons, this could become empty. However, for
    // polygons it should always have at least two active edges
    if (AL.empty()) {
      scanline++;
      return;
    }

    // sort according to X coordinate
    AL.sort([](const edge_t &a, const edge_t &b) {
      // it is an iterator pointing to Edge
      // sort by x and slope
      return (a.x < b.x || (a.x == b.x && a.slope < b.slope));
    });
#ifdef DEBUG_LOG
    std::cout << "-> (" << AL.size() << "):" << std::endl;
    for (auto &a : AL) {
      std::cout << "{(" << a.line.first.get<0>() << "," << a.line.first.get<1>()
                << "),"
                << "(" << a.line.second.get<0>() << ","
                << a.line.second.get<1>() << ")},";
    }
#endif

    // prepare scanline filling
    auto minX = std::floor(AL.front().minX - 1);
    auto maxX = std::ceil(AL.back().maxX);
    auto line = typename edge_t::segment_t(point_t(minX, scanline + 0.5),
                                           point_t(maxX, scanline + 0.5));

    std::vector<point_t> output;
    for (auto &e : AL) {
      bg::intersection(line, e.line, output);
#ifdef DEBUG_LOG
      std::cout << "[";
      for (auto i = output.begin(); i != output.end(); ++i)
        std::cout << "(" << bg::get<0>(*i) << ',' << bg::get<1>(*i) << "),";
      std::cout << "], ";
#endif
    }
#ifdef DEBUG_LOG
    std::cout << " | sum: " << output.size() << std::endl;
#endif
    // assert(output.size() % 2 == 0);
    std::sort(output.begin(), output.end(),
              ([](const point_t &a, const point_t &b) {
                // sort by x
                return bg::get<0>(a) < bg::get<0>(b);
              }));
#ifdef BORDERS_ONLY
    // Just the borders
    auto index = 0;
    for (auto &i : output) {
      //	float_t pixelX = bg::get<0>(i) - std::floor(bg::get<0>(i));

      int x = index % 2 == 0 ? std::round(bg::get<0>(i))
                             : std::floor(bg::get<0>(i) - 0.5);

      putpixel(x, scanline);
      index++;
    }
#else
    if (output.size() > 0) {

      for (auto i = 1; i < output.size(); i += 2) {
        auto xstart = std::round(bg::get<0>(output[i - 1]));
        auto xend = std::floor(bg::get<0>(output[i]) - 0.5);
#ifdef DEBUG_LOG
        std::cout << "xstart: " << xstart << " xend: " << xend << std::endl;
#endif
        for (auto x = xstart; x <= xend; x++)
          putpixel(x, scanline);
      }
    }
#endif

    scanline++;
    return;
  }
  template <typename func> void step_rasterize(func putpixel) {
//#define DEBUG_LOG
#ifdef DEBUG_LOG
    std::cout << "scanline(" << scanline << "):" << std::endl;
#endif
    // all edges that start here are moved from ET to AL
    for (auto it = ET.begin(); it != ET.end();) {
      if (std::floor(it->yNorm) <= scanline) {
        AL.push_back(*it);
#ifdef DEBUG_LOG
        std::cout << "erase (" << it->yMin << ") ";
#endif
        it = ET.erase(it);
      } else
        it++;
    }
#ifdef DEBUG_LOG
    std::cout << "active(" << AL.size() << ") ";
#endif
    // all edges that end here, are removed from AL
    for (auto it = AL.begin(); it != AL.end();) {
      if (std::ceil(it->yMax - 1) < scanline) {
        it = AL.erase(it);
      } else
        it++;
    }

    // theoretic: for multipolygons, this could become empty. However, for
    // polygons it should always have at least two active edges
    if (AL.empty()) {
      scanline++;
      return;
    }

    // sort according to X coordinate
    AL.sort([](const edge_t &a, const edge_t &b) {
      // it is an iterator pointing to Edge
      // sort by x and slope
      return ((a.xNorm < b.xNorm) || (a.xNorm == b.xNorm && a.slope < b.slope));
    });
#ifdef DEBUG_LOG
    std::cout << "-> (" << AL.size() << "):" << std::endl;
    for (auto &a : AL) {
      std::cout << "{(" << a.line.first.get<0>() << "," << a.line.first.get<1>()
                << "),"
                << "(" << a.line.second.get<0>() << ","
                << a.line.second.get<1>() << ")},";
    }
#endif

    // prepare scanline filling
    auto minX = std::floor(AL.front().minX - 2);
    auto maxX = std::ceil(AL.back().maxX);
    typename edge_t::segment_t line(point_t(minX, scanline + 0.5),
                                    point_t(maxX, scanline + 0.5));
    auto it = AL.begin();
    bool inside = false;
#ifdef DEBUG_LOG
    std::cout << "[" << std::endl;
#endif
    for (float_t x = minX; x <= maxX; x++) {
      auto go = true;
      while (go) {
        go = false;
        if (!inside) {
          if (x + 0.5 >= it->xNorm && it != AL.end()) {
            inside = !inside;
            it++;
            go = true;
          }
        } else {
          if ((x + 0.499999) > it->xNorm) {
#ifdef DEBUG_LOG
            std::cout.precision(std::numeric_limits<float_t>::max_digits10);
            std::cout << std::endl
                      << "(" << x + 0.5 << " > " << it->xNorm << ")"
                      << std::endl;
            std::cout.precision(2);
#endif
            inside = !inside;
            it++;
            go = true;
          }
        }
        if (it == AL.end())
          break;
      }
#ifdef DEBUG_LOG
      std::cout << "n:" << it->xNorm << "~" << it->x << " x:" << x
                << " in:" << inside << ", ";
#endif

      if (inside)
        putpixel(x, scanline);
    }
#ifdef DEBUG_LOG
    std::cout << "]" << std::endl;
#endif

    // increment all the X based on slope
    for (auto &e : AL) {
      if (e.dX != 0)
        e.xNorm += e.slope;
    }

    scanline++;
    return;
  }

  void rasterize(const polygon_t &p,
                 std::function<void(float_t, float_t)> putpixel,
                 int step_id = STEP_RASTERIZE, int steps = -1) {
    init(p);

    switch (step_id) {
    case STEP_INTERSECT:
      while (!done()) {
        step_intersect(putpixel);
      }
      break;
    case STEP_RASTERIZE:
      while (!done()) {
        step_rasterize(putpixel);
      }
      break;
    }
    // Process ET=>AL=>(void)
  }
};

} // namespace rasterize

#endif


//=======================================================================
// Copyright 2002 Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================

/*
  Adapted from the GIRTH program of the Stanford GraphBase.

  Sample output:

  This program explores the girth and diameter of Ramanujan graphs.
  The bipartite graphs have q^3-q vertices, and the non-bipartite
  graphs have half that number. Each vertex has degree p+1.
  Both p and q should be odd prime numbers;
    or you can try p = 2 with q = 17 or 43.

  Choose a branching factor, p: 2
  Ok, now choose the cube root of graph size, q: 17
  Starting at any given vertex, there are
  3 vertices at distance 1,
  6 vertices at distance 2,
  12 vertices at distance 3,
  24 vertices at distance 4,
  46 vertices at distance 5,
  90 vertices at distance 6,
  169 vertices at distance 7,
  290 vertices at distance 8,
  497 vertices at distance 9,
  634 vertices at distance 10,
  521 vertices at distance 11,
  138 vertices at distance 12,
  13 vertices at distance 13,
  3 vertices at distance 14,
  1 vertices at distance 15.
  So the diameter is 15, and the girth is 9.

 */

#include <boost/config.hpp>
#include <vector>
#include <list>
#include <iostream>
#include <boost/limits.hpp>
#include <boost/graph/stanford_graph.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/graph_utility.hpp>

typedef boost::graph_traits<Graph*> Traits;
typedef Traits::vertex_descriptor vertex_descriptor;
typedef Traits::edge_descriptor edge_descriptor;
typedef Traits::vertex_iterator vertex_iterator;

std::vector<std::size_t> distance_list;

typedef boost::v_property<long> dist_t;
boost::property_map<Graph*, dist_t>::type d_map;

typedef boost::u_property<vertex_descriptor> pred_t;
boost::property_map<Graph*, pred_t>::type p_map;

typedef boost::w_property<long> color_t;
boost::property_map<Graph*, color_t>::type c_map;

class diameter_and_girth_visitor : public boost::bfs_visitor<>
{
public:
  diameter_and_girth_visitor(std::size_t& k_, std::size_t& girth_)
    : k(k_), girth(girth_) { }

  void tree_edge(edge_descriptor e, Graph* g) {
    vertex_descriptor u = source(e, g), v = target(e, g);
    k = d_map[u] + 1;
    d_map[v] = k;
    ++distance_list[k];
    p_map[v] = u;
  }
  void non_tree_edge(edge_descriptor e, Graph* g) {
    vertex_descriptor u = source(e, g), v = target(e, g);
    k = d_map[u] + 1;
    if (d_map[v] + k < girth && v != p_map[u])
      girth = d_map[v]+ k;
  }
private:
  std::size_t& k;
  std::size_t& girth;
};

int
main()
{
  std::cout <<
    "This program explores the girth and diameter of Ramanujan graphs."
            << std::endl;
  std::cout <<
    "The bipartite graphs have q^3-q vertices, and the non-bipartite"
            << std::endl;
  std::cout <<
    "graphs have half that number. Each vertex has degree p+1."
            << std::endl;
  std::cout << "Both p and q should be odd prime numbers;" << std::endl;
  std::cout << "  or you can try p = 2 with q = 17 or 43." << std::endl;

  while (1) {

    std::cout << std::endl
              << "Choose a branching factor, p: ";
    long p = 0, q = 0;
    std::cin >> p;
    if (p == 0)
      break;
    std::cout << "Ok, now choose the cube root of graph size, q: ";
    std::cin >> q;
    if (q == 0)
      break;

    Graph* g;
    g = raman(p, q, 0L, 0L);
    if (g == 0) {
      std::cerr << " Sorry, I couldn't make that graph (error code "
        << panic_code << ")" << std::endl;
      continue;
    }
    distance_list.clear();
    distance_list.resize(boost::num_vertices(g), 0);

    // obtain property maps
    d_map = get(dist_t(), g);
    p_map = get(pred_t(), g);
    c_map = get(color_t(), g);

    vertex_iterator i, end;
    for (boost::tie(i, end) = boost::vertices(g); i != end; ++i)
      d_map[*i] = 0;

    std::size_t k = 0;
    std::size_t girth = (std::numeric_limits<std::size_t>::max)();
    diameter_and_girth_visitor vis(k, girth);

    vertex_descriptor s = *boost::vertices(g).first;

    boost::breadth_first_search(g, s, visitor(vis).color_map(c_map));

    std::cout << "Starting at any given vertex, there are" << std::endl;

    for (long d = 1; distance_list[d] != 0; ++d)
      std::cout << distance_list[d] << " vertices at distance " << d
                << (distance_list[d+1] != 0 ? "," : ".") << std::endl;

    std::cout << "So the diameter is " << k - 1
              << ", and the girth is " << girth
              << "." << std::endl;
  } // end while

  return 0;
}


#include <stdio.h>
#include <iostream>
#include <Eigen/Eigen>
#include "NeuralNetwork.h"

int main(void)
{
  NeuralNetwork nn;

  float loss;
  Matrix<float, Dynamic, Dynamic> out;

#if 1
  Matrix<float, 1, 2> input[4];
  Matrix<float, 1, 1> expect[4];
  Matrix<float, Dynamic, Dynamic> W1(2,2);
  Matrix<float, Dynamic, Dynamic> W2(2,1);

  input[0] << 0, 0; expect[0] << 0;
  input[1] << 0, 1; expect[1] << 1;
  input[2] << 1, 0; expect[2] << 1;
  input[3] << 1, 1; expect[3] << 0;

  W1 << -0.0032973, -0.02393722, -0.02107663, 0.01280219 ;
  W2 << -0.01024167, -0.01514217;

  nn.createNewLayer(2,2, false, &W1);
  nn.createNewLayer(2,1, true,  &W2);

  loss = 100.f;
  nn.printnet();
  // while (loss > 0.001f)
  for(int j=0; j<10; j++)
    {
      loss = 0.f;
      for(int i=0; i<4; i++)
        {
          // std::cout << "==== " << i+1 << " ===="  << std::endl;
          out = nn.forward( input[i] );
          loss += nn.backward(0.1f, expect[i]);
          nn.printnet();
          // std::cout << "   out = " << out << std::endl;
        }
      loss /= 4;
      std::cout << "loss = " << loss << std::endl;
      nn.printnet();
    }

  for(int i=0; i<4; i++)
    {
      std::cout << "Expect = " << expect[i] << " Input  = " << input[i] << std::endl;
      out = nn.forward( input[i] );
      std::cout << "Output = " << out << std::endl;
    }

#else
  Matrix<float, 1, 2> input;
  Matrix<float, 1, 2> expect;

  input << 3, 3;
  expect << 0, 1;

  nn.createNewLayer(2, 5);
  nn.createNewLayer(5, 3);
  nn.createNewLayer(3, 2, true);

  std::cout << "tmp input = " << input << std::endl;
  std::cout << "expect = " << expect << std::endl;
  std::cout << "input * expect = " << input.array() * expect.array() << std::endl;

  std::cout << "input = " << input << std::endl;
  input = input.array() + 1;
  std::cout << "input = " << input << std::endl;

  out = nn.forward( input );
  std::cout << "out.rows = " << out.rows() << std::endl;
  std::cout << "out.cols = " << out.cols() << std::endl;
  std::cout << "out = " << out << std::endl;

  loss = 100.f;
  while (loss > 0.001f)
    {
      out = nn.forward( input );
      loss = nn.backward(0.1f, expect);
      std::cout << "out = " << out << std::endl;
      std::cout << "loss = " << loss << std::endl;
    }
#endif

  return 0;
}


#include <SDL2/SDL.h>
#include <vector>
#include <armadillo>
#include <cmath>
#include <cstdlib>
#include <cstdio>
#include <sys/types.h>
#include <unistd.h>
#include <iostream>

double cos_rule_angle(double A, double B, double C) {
  if (fabs((A + B) - C) < 0.000001) {
    return M_PI;
  }
  if (!(A + B >= C)) {
    fprintf(stderr, "Assertion Error: A + B >= C\n");
    exit(1);
  }
  return acos((A * A + B * B - C * C) / (2.0 * A * B));
}

bool concave(const arma::vec &l, const arma::vec &c, const arma::vec &r, double lrad, double crad, double rrad) {
  double dlcx = c(0) - l(0);
  double dlcy = c(1) - l(1);
  double drcx = c(0) - r(0);
  double drcy = c(1) - r(1);
  double lc = sqrt(dlcx * dlcx + dlcy * dlcy);
  double rc = sqrt(drcx * drcx + drcy * drcy);
  double theta1 = cos_rule_angle(lc, crad, lrad);
  double theta2 = cos_rule_angle(rc, crad, rrad);
  return ((theta1 + theta2) - M_PI) > -0.1;
}

bool within(const arma::vec &l, const arma::vec &c, const arma::vec &r, double lrad, double crad, double rrad) {
  if (crad == 0.0) {
    return true;
  }
  return concave(l, c, r, lrad, crad, rrad);
}

std::vector<arma::vec> get_filled_boundary(const std::vector<arma::vec> &pts) {
  arma::vec X(pts.size());
  arma::vec Y(pts.size());
  for (int i = 0; i < pts.size(); i++) {
    X(i) = pts[i](0) * cos(pts[i](1));
    Y(i) = pts[i](0) * sin(pts[i](1));
  }
  int min_x = 0;
  int min_y = 0;
  int max_x = 0;
  int max_y = 0;
  int tx;
  int ty;
  for (int i = 0; i < pts.size(); i++) {
    tx = (int)floor(X(i));
    if (tx < min_x) {
      min_x = tx;
    }
    tx = (int)ceil(X(i));
    if (tx > max_x) {
      max_x = tx;
    }
    ty = (int)floor(Y(i));
    if (ty < min_y) {
      min_y = ty;
    }
    ty = (int)ceil(Y(i));
    if (ty > max_y) {
      max_y = ty;
    }
  }
  max_x++;
  max_y++;
  // create vmap
  arma::mat vmap = arma::zeros<arma::mat>(max_y - min_y, max_x - min_x);
  std::vector<arma::vec> colored;
  std::vector<arma::vec> infection_queue = {{ 0.0, 0.0 }};
  vmap(-min_y, -min_x) = 1.0;
  int index = 0;
  while (infection_queue.size() > index) {
    double x = infection_queue[index](0);
    double y = infection_queue[index](1);

    double theta = atan2(y, x);
    if (theta < 0.0) {
      theta += 2.0 * M_PI;
    }
    double radius = sqrt(x * x + y * y);
    // binary search
    int left = 0;
    int right = pts.size() - 1;
    if (pts[right](1) <= theta || theta < pts[left](1)) {
      left = right;
      right = 0;
    } else {
      int mid = (left + right) / 2;
      while (left + 1 != right) {
        if (theta >= pts[mid](1)) {
          left = mid;
        } else {
          right = mid;
        }
        mid = (left + right) / 2;
      }
    }

    // bfs
    arma::vec l({X(left), Y(left)});
    arma::vec c({x, y});
    arma::vec r({X(right), Y(right)});
    int dx = x - min_x;
    int dy = y - min_y;
    if (within(l, c, r, pts[left](0), radius, pts[right](0))) {
      colored.push_back(c);
      if (y + 1 < max_y && vmap(dy + 1, dx) == 0.0) {
        infection_queue.push_back({x, y + 1});
        vmap(dy + 1, dx) = 1.0;
      }
      if (y - 1 >= min_y && vmap(dy - 1, dx) == 0.0) {
        infection_queue.push_back({x, y - 1});
        vmap(dy - 1, dx) = 1.0;
      }
      if (x + 1 < max_x && vmap(dy, dx + 1) == 0.0) {
        infection_queue.push_back({x + 1, y});
        vmap(dy, dx + 1) = 1.0;
      }
      if (x - 1 >= min_x && vmap(dy, dx - 1) == 0.0) {
        infection_queue.push_back({x - 1, y});
        vmap(dy, dx - 1) = 1.0;
      }
    }
    //infection_queue.erase(infection_queue.begin());
    index++;
  }
  return colored;
}

std::vector<arma::vec> lidar_read(void) {
  std::vector<arma::vec> pts;
  double theta = 0.0;
  while (theta < 360.0) {
    theta += (double)rand() / (double)RAND_MAX + 2.0;
    if (theta > 360.0) {
      break;
    }
    double radius = (double)rand() / (double)RAND_MAX * 100.0 + 100.0;
    pts.push_back(arma::vec({radius, theta * M_PI / 180.0}));
  }
  return pts;
}

int main(int argc, char *argv[]) {
  arma::vec center = { 300.0, 300.0 };

  srand(getpid());
  SDL_Init(SDL_INIT_VIDEO);
  SDL_Window *window = SDL_CreateWindow("raycast", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, 640, 640, SDL_WINDOW_SHOWN);
  SDL_Renderer *renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_ACCELERATED | SDL_RENDERER_PRESENTVSYNC);
  SDL_Surface *screen = SDL_CreateRGBSurface(0, 640, 640, 32, 0, 0, 0, 0);
  SDL_Texture *texture = SDL_CreateTextureFromSurface(renderer, screen);

  bool done = false;
  uint32_t color_black = SDL_MapRGB(screen->format, 0, 0, 0);
  uint32_t color_red = SDL_MapRGB(screen->format, 255, 0, 0);
  uint32_t color_green = SDL_MapRGB(screen->format, 0, 255, 0);
  uint32_t color_blue = SDL_MapRGB(screen->format, 0, 0, 255);
  while (true) {
    SDL_Event e;
    while (SDL_PollEvent(&e)) {
      if (e.type == SDL_QUIT) {
        done = true;
        break;
      }
    }
    if (done) {
      break;
    }

    std::vector<arma::vec> data = lidar_read();
    printf("getting raycast area...\n");
    std::vector<arma::vec> raycast_area = get_filled_boundary(data);

    SDL_FillRect(screen, NULL, color_black);
    for (arma::vec &pt : raycast_area) {
      int x = (int)(pt(0) + center(0));
      int y = (int)(-pt(1) + center(1));
      ((uint32_t *)screen->pixels)[y * 640 + x] = color_red;
    }
    for (arma::vec &pt : data) {
      int x = (int)(pt(0) * cos(pt(1)) + center(0));
      int y = (int)(-pt(0) * sin(pt(1)) + center(1));
      ((uint32_t *)screen->pixels)[y * 640 + x] = color_green;
    }
    ((uint32_t *)screen->pixels)[300 * 640 + 299] = color_blue;
    ((uint32_t *)screen->pixels)[300 * 640 + 300] = color_blue;
    ((uint32_t *)screen->pixels)[300 * 640 + 301] = color_blue;
    ((uint32_t *)screen->pixels)[299 * 640 + 300] = color_blue;
    ((uint32_t *)screen->pixels)[301 * 640 + 300] = color_blue;

    SDL_UpdateTexture(texture, NULL, screen->pixels, screen->pitch);
    SDL_RenderClear(renderer);
    SDL_RenderCopy(renderer, texture, NULL, NULL);
    SDL_RenderPresent(renderer);
    SDL_Delay(25);
  }
  SDL_DestroyTexture(texture);
  SDL_FreeSurface(screen);
  SDL_DestroyRenderer(renderer);
  SDL_DestroyWindow(window);
  SDL_Quit();
  return 0;
}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

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

#ifndef GRADIENT_FUNCTOR_INC_HPP
#define GRADIENT_FUNCTOR_INC_HPP

#include "gradient_functor.hpp"

#include <boost/bind.hpp>
#include <boost/gil/extension/numeric/kernel.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>
#include <boost/gil/extension/io_new/tiff_read.hpp>

namespace internal {

#if !defined(M_PI)
    const double M_PI = 4.0 * atan(1.0);
#endif

    template<typename Kernel1D>
    void initKernelGaussian1D(Kernel1D& kernel, double m_sigma)
    {
	// Gaussian smoothing
        typedef	typename Kernel1D::value_type vt;
        const vt z = 1.0 / (std::sqrt(2 * M_PI) * m_sigma);
        const vt m_sigmasquared = m_sigma * m_sigma;
        vt x = -1.0 * kernel.center();
        typename Kernel1D::iterator i;
        vt sum = 0.;
        for (i=kernel.begin(); i!=kernel.end(); ++i, ++x)
        {
            *i = z * (std::exp(-0.5*(x*x/m_sigmasquared)));
            sum += *i;
        }
        for (i=kernel.begin(); i!=kernel.end(); ++i) *i /= sum;
    }

    template<typename Kernel1D>
    void initKernelGaussianDeriv1D(Kernel1D& kernel, double m_sigma)
    {
	// Gaussian derivative smoothing
        typedef	typename Kernel1D::value_type vt;
        const vt z = 1.0 / (std::sqrt(2 * M_PI) * m_sigma);
        const vt m_sigmasquared = m_sigma * m_sigma;
        vt x = -1.0 * kernel.center();
        typename Kernel1D::iterator i;
        vt sum = 0.;
        for (i=kernel.begin(); i!=kernel.end(); ++i, ++x)
        {
            *i = - (x/m_sigmasquared) * z * (std::exp(-0.5*(x*x/m_sigmasquared)));
            sum += *i * x;
        }
        for (i=kernel.begin(); i!=kernel.end(); ++i) *i /= sum;
    }

}

template<typename Image, typename View>
typename gradient_functor::result_type gradient_functor::operator()(Image& g, const View& v) const
{
    using namespace boost::gil;

    typedef typename Image::view_t g_view_t;
    typedef typename get_pixel_type<g_view_t>::type g_pixel_t;
    typedef typename kth_element_type<g_pixel_t,0>::type element_0_t;
    typedef typename kth_element_type<g_pixel_t,1>::type element_1_t;
    typedef pixel<element_0_t, gray_layout_t > g0_pixel_t;
    typedef pixel<element_1_t, gray_layout_t > g1_pixel_t;

    g.recreate(v.dimensions());

    unsigned int half_size = (unsigned int) (3* m_sigma) ;
    const size_t kws = 2 * half_size + 1;
    kernel_1d<float> ksmooth(kws, kws / 2);
    kernel_1d<float> kderiv(kws, kws / 2);
    internal::initKernelGaussian1D(ksmooth, m_sigma);
    internal::initKernelGaussianDeriv1D(kderiv, m_sigma);

    convolve_cols<g0_pixel_t> (v, ksmooth, kth_channel_view<0> (view(g)), convolve_option_extend_constant);
    convolve_rows<g0_pixel_t> (kth_channel_view<0> (view(g)), kderiv, kth_channel_view<0> (view(g)), convolve_option_extend_constant);

    convolve_rows<g1_pixel_t> (v, ksmooth, kth_channel_view<1> (view(g)), convolve_option_extend_constant);
    convolve_cols<g1_pixel_t> (kth_channel_view<1> (view(g)), kderiv, kth_channel_view<1> (view(g)), convolve_option_extend_constant);
}

#endif // GRADIENT_FUNCTOR_INC_HPP


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <string>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include "DatasetAR.h"
#include "DatasetARX.h"
#include "GaussSeq.h"
#include "ARSeq.h"
#include "ARPoly.h"
#include "utils.h"
using utils::my_float;
using boost::multiprecision::cpp_bin_float_50;
using boost::random::uniform_real_distribution;

int main() {
    boost::random::mt19937 gen {};
    uniform_real_distribution<my_float> u(-1, 1);

    // prepare
    std::map<int, my_float> coeff {
        {3, 0.2},
        {20, 0.5},
    };
    std::map<int, my_float> coeff_2 {
        {3, 0.75},
        {20, -0.9},
    };
    std::vector<my_float> v_seed(20);

    // Stationary AR process
    for (auto type = 0; type < 5; type++) {
        for (auto samp = 0; samp < utils::N_SAMPLE; samp++) {
            // Seed differently
            for (auto i = 0; i < 20; i++) {
                v_seed[i] = u(gen);
            }
            ARSeq targ_seq(coeff, u(gen));
            targ_seq.seed_prev_vals(v_seed);

            DatasetAR dat("./ARStat/ARStat-" + std::to_string(type) +
                "/Sample-" + std::to_string(samp), targ_seq, type);
            dat.write_csv();
        }
    }

    // Non-stationary AR process
    for (auto type = 0; type < 5; type++) {
        for (auto samp = 0; samp < utils::N_SAMPLE; samp++) {
            // Seed differently
            for (auto i = 0; i < 20; i++) {
                v_seed[i] = u(gen);
            }
            ARSeq targ_seq(coeff_2, u(gen));
            targ_seq.seed_prev_vals(v_seed);

            DatasetAR dat("./ARNStat/ARNStat-" + std::to_string(type) +
                "/Sample-" + std::to_string(samp), targ_seq, type);
            dat.write_csv();
        }
    }

    // Non-stationary ARX process
    for (auto type = 1; type < 5; type++) {
        for (auto samp = 0; samp < utils::N_SAMPLE; samp++) {
            DatasetARX dat("./ARXNStat/ARXNStat-" + std::to_string(type) +
                "/Sample-" + std::to_string(samp), type);
            dat.write_csv();
        }
    }
    return 0;
}


/*BISHOPS - Bishops
#math #big-numbers #ad-hoc-1

Yesterday was Sam's birthday. The most interesting gift was definitely the chessboard. Sam quickly learned the rules of chess and defeated his father, all his friends, his little sister, and now no one wants to play with him any more.

So he decided to play with another birthday gift – a Book of Math Problems for Young Mathematicians. He opened the book somewhere in the middle and read the following problem: "How many knights can be placed on a chessboard without threatening each other?" After a while he realized that this was trivial and moved on to the next problem: "How many bishops can be placed on a chessboard without threatening each other?". Sam is in trouble here. He is not able to solve this problem and needs your help.

Sam's chessboard has size N x N. A bishop can move to any distance in any of the four diagonal directions. A bishop threatens another bishop if it can move to the other bishop's position. Your task is to compute the maximum number of bishops that can be placed on a chessboard in such a way that no two bishops threaten each other.
Input

The input file consists of several lines. The line number i contains a single positive integer N representing the size of the i-th chessboard. [1 <= N <= 10^100]
Output

The output file should contain the same number of lines as the input file. The i-th line should contain one number – the maximum number of bishops that can be placed on i-th chessboard without threatening each other.
Example

Input:
2
3

Output:
2
4

*/

#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>

int main()
{
    using namespace boost::multiprecision;

    cpp_int num;

    while (std::cin >> num)
    {
        std::cout << (num <= 1 ? num : 2*num - 2) << std::endl;
    }

    return 0;
}


#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};
}


/*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;
}


// 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


/* -*- 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


/** @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 <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());
    }
}