/*=============================================================================
    Copyright (c) 2001-2011 Joel de Guzman

    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)
=============================================================================*/
#if !defined(PAWN_MAST_HPP)
#define PAWN_MAST_HPP

#include <boost/config/warning_disable.hpp>
#include <boost/variant/recursive_variant.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>
#include <boost/optional.hpp>

#include <iostream>
#include <list>
#include <map>
#include <vector>

#include <helper.hpp>

namespace client { namespace math { namespace ast
{
    struct nil {};
    struct unary;
    struct expr;

    using variable = std::string;
    using column = unsigned int;

    typedef boost::variant<
            nil
          , double
          , variable
          , column
          , boost::recursive_wrapper<unary>
          , boost::recursive_wrapper<expr>
        >
    operand;

    enum class optoken : int
    {
        plus,
        minus,
        times,
        divide,
        positive,
        negative
    };

    struct unary
    {
        optoken operator_;
        operand operand_;
    };

    struct operation
    {
        optoken operator_;
        operand operand_;
    };

    struct expr
    {
        operand first;
        std::list<operation> rest;
    };

    // print functions for debugging
    inline std::ostream& operator<<(std::ostream& out, nil) { out << "nil"; return out; }
    //inline std::ostream& operator<<(std::ostream& out, variable const& var) { out << var.name; return out; }

    struct printer {
        typedef void result_type;

        void operator()(nil) const {std::cout << '_'; }
        void operator()(double n) const { std::cout << n; }
        void operator()(variable const& x) const { std::cout << '%' << x; }
        void operator()(column const& x) const { std::cout << '$' << x; }

        void operator()(optoken const &o) const {
            switch (o)
            {
                case optoken::plus: std::cout << " add"; break;
                case optoken::minus: std::cout << " subt"; break;
                case optoken::times: std::cout << " mult"; break;
                case optoken::divide: std::cout << " div"; break;
                case optoken::positive: std::cout << " pos"; break;
                case optoken::negative: std::cout << " neg"; break;
            }
        }

        void operator()(unary const& x) const
        {
            boost::apply_visitor(*this, x.operand_);
            (*this)(x.operator_);
        }

        void operator()(operation const& x) const
        {
            boost::apply_visitor(*this, x.operand_);
            (*this)(x.operator_);
        }

        void operator()(expr const& x) const
        {
            boost::apply_visitor(*this, x.first);
            for (const auto& oper : x.rest) {
                std::cout << ' ';
                (*this)(oper);
            }
        }
    };

    struct colsEval {
    private:
      using ColIndices = client::helper::ColIndices;
      using Global = client::helper::Global;
      const ColIndices &_pre;
      const Global &_global;
      bool _isInitial {true};
      std::vector<std::string> _headers;
    public:
        using result_type = std::pair<ColIndices, std::string>;
        colsEval(const ColIndices &v, const Global &g) : _pre{v}, _global{g} {}
        void setHeaders(const std::vector<std::string>& h) { _headers = h; }
        void notInitial() { _isInitial = false; }
        result_type operator()(nil) const { BOOST_ASSERT(0); return result_type{}; }
        result_type operator()(double n) const { return result_type{}; }
        result_type operator()(variable const &x) const {
          if (_global.gVarsN.find(x) != std::end(_global.gVarsN)) return result_type{};
          auto jt = std::find(begin(_headers), end(_headers), x);
          if (jt != std::end(_headers)) return (*this)((unsigned int)(jt - std::begin(_headers)) + 1);
          auto it = std::find(std::begin(_pre.var), std::end(_pre.var), x);
          if (it == std::end(_pre.var)) return std::make_pair(ColIndices{}, "Error: " + x + " used before declaration.");
          return result_type{};
        }
        result_type operator()(column const &x) const {
          if (!_isInitial) return std::make_pair(ColIndices{}, "Can't access new input file columns via column name / index after reduce / zip.");
          ColIndices res;
          res.num.push_back(x);
          return std::make_pair(res, "");
        }
        result_type operator()(unary const& x) const {
          return boost::apply_visitor(*this, x.operand_);
        }
        result_type operator()(operation const& x) const {
            return boost::apply_visitor(*this, x.operand_);
        }
        result_type operator()(expr const& e) const {
            result_type res{};
            auto x = boost::apply_visitor(*this, e.first);
            if (x.second.size() > 0) return x;
            res.first.add(x.first);
            for (const auto& oper : e.rest) {
              x = (*this)(oper);
              if (x.second.size() > 0) return x;
              res.first.add(x.first);
            }
            return res;
        }
    };

    ///////////////////////////////////////////////////////////////////////////
    //  The AST evaluator
    ///////////////////////////////////////////////////////////////////////////
    struct whatever {
      whatever(double x) : _x{x} {}
      auto operator() (const std::vector<double>&) { return _x; }
    private:
      double _x;
    };

    struct evaluator {
    private:
        const helper::positionTeller _index;
        const helper::Global &_global;
    public:
        using retFnT = std::function<double(const std::vector<double>&)>;
        typedef retFnT result_type;

        evaluator(helper::positionTeller p, const helper::Global &g) : _index{p}, _global{g} {}
        retFnT operator()(nil) const { BOOST_ASSERT(0); return whatever{0.0}; }
        retFnT operator()(double n) const { return whatever{n}; }
        retFnT operator()(variable const &x) const {
          auto it = _global.gVarsN.find(x);
          if (it != std::end(_global.gVarsN)) {
            auto y = it->second;
            return [y](const std::vector<double> &v) { return y; };
          }
          auto y = _index.var(x);
          return [y](const std::vector<double> &v) { return v[y]; };
        }
        retFnT operator()(column const &x) const {
          auto y = _index.num(x);
          return [y](const std::vector<double> &v) { return v[y]; };
        }

        retFnT operator()(optoken const &o, retFnT const &lhs, retFnT const &rhs) const {
            switch (o)
            {
                case optoken::plus: return [lhs, rhs](const std::vector<double> &v) { return lhs(v) + rhs(v); };
                case optoken::minus: return [lhs, rhs](const std::vector<double> &v) { return lhs(v) - rhs(v); };
                case optoken::times: return [lhs, rhs](const std::vector<double> &v) { return lhs(v) * rhs(v); };
                case optoken::divide: return [lhs, rhs](const std::vector<double> &v) { return lhs(v) / rhs(v); };
                default: BOOST_ASSERT(0); return rhs;
            }
            BOOST_ASSERT(0);
            return rhs;
        }

        retFnT operator()(optoken const &o, retFnT const &rhs) const {
            switch (o)
            {
                case optoken::positive: return [rhs](const std::vector<double> &v) { return rhs(v); };
                case optoken::negative:  return [rhs](const std::vector<double> &v) { return - rhs(v); };
                default: BOOST_ASSERT(0); return rhs;
            }
            BOOST_ASSERT(0);
            return rhs;
        }

        retFnT operator()(unary const& x) const {
            retFnT rhs = boost::apply_visitor(*this, x.operand_);
            return (*this)(x.operator_, rhs);
        }

        retFnT operator()(operation const& x, retFnT const& lhs) const
        {
            retFnT rhs = boost::apply_visitor(*this, x.operand_);
            return (*this)(x.operator_, lhs, rhs);
        }
        retFnT operator()(expr const& x) const
        {
            retFnT state = boost::apply_visitor(*this, x.first);
            for (const auto& oper : x.rest) {
                state = (*this)(oper, state);
            }
            return state;
        }
    };
}}}

BOOST_FUSION_ADAPT_STRUCT(
    client::math::ast::unary,
    (client::math::ast::optoken, operator_)
    (client::math::ast::operand, operand_)
)

BOOST_FUSION_ADAPT_STRUCT(
    client::math::ast::operation,
    (client::math::ast::optoken, operator_)
    (client::math::ast::operand, operand_)
)

BOOST_FUSION_ADAPT_STRUCT(
    client::math::ast::expr,
    (client::math::ast::operand, first)
    (std::list<client::math::ast::operation>, rest)
)
#endif


// Copyright 2018 Hans Dembinski
//
// 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)

// clang-format off

//[ getting_started_listing_03

#include <boost/format.hpp>
#include <boost/histogram.hpp>
#include <cassert>
#include <iostream>
#include <sstream>

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

  /*
    Create a profile. Profiles does not only count entries in each cell, but
    also compute the mean of a sample value in each cell.
  */
  auto p = make_profile(axis::regular<>(5, 0.0, 1.0));

  /*
    Fill profile with data, usually this happens in a loop. You pass the sample
    with the `sample` helper function. The sample can be the first or last
    argument.
  */
  p(0.1, sample(1));
  p(0.15, sample(3));
  p(0.2, sample(4));
  p(0.9, sample(5));

  /*
    Iterate over bins and print profile.
  */
  std::ostringstream os;
  for (auto&& x : indexed(p)) {
    os << boost::format("bin %i [%3.1f, %3.1f) count %i mean %g\n")
          % x.index() % x.bin().lower() % x.bin().upper()
          % x->count() % x->value();
  }

  std::cout << os.str() << std::flush;
  assert(os.str() == "bin 0 [0.0, 0.2) count 2 mean 2\n"
                     "bin 1 [0.2, 0.4) count 1 mean 4\n"
                     "bin 2 [0.4, 0.6) count 0 mean 0\n"
                     "bin 3 [0.6, 0.8) count 0 mean 0\n"
                     "bin 4 [0.8, 1.0) count 1 mean 5\n");
}

//]


/**
 * @file
 * @brief We build a tensor product mesh on a torus and apply uniform refinement
 * @author Anian Ruoss
 * @date   2018-10-20 16:27:17
 * @copyright MIT License
 */

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

#include <lf/refinement/mesh_hierarchy.h>
#include "lf/io/io.h"

using size_type = lf::base::size_type;

int main(int argc, char **argv) {
  // define allowed command line arguments:
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  desc.add_options()("help", "Produce this help message")(
      "num_steps", po::value<size_t>()->default_value(4),
      "Number of uniform refinement steps")(
      "num_x_cells", po::value<size_type>()->default_value(4),
      "Number of cells in x direction")(
      "num_y_cells", po::value<size_type>()->default_value(4),
      "Number of cells in y direction")(
      "top_right_corner",
      po::value<std::vector<double>>()->multitoken()->default_value(
          std::vector<double>{1., 4.}, "1., 4."),
      "Coordinates of top right corner of rectangle with bottom left at (0,0)");

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

  if (vm.count("help") != 0u) {
    std::cout << desc << std::endl;
    return 1;
  }

  const size_t num_steps = vm["num_steps"].as<size_t>();
  const size_type num_x_cells = vm["num_x_cells"].as<size_type>();
  const size_type num_y_cells = vm["num_y_cells"].as<size_type>();
  std::vector<double> top_right_corner_coords =
      vm["top_right_corner"].as<std::vector<double>>();

  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(3);

  // build tensor product mesh by specifying rectangle from which torus will
  // be generated by identifying opposite edges
  lf::mesh::hybrid2d::TorusMeshBuilder builder(mesh_factory_ptr);
  builder.setBottomLeftCorner(Eigen::Vector2d{0., 0.});
  builder.setTopRightCorner(Eigen::Vector2d{top_right_corner_coords.data()});
  builder.setNoXCells(num_x_cells);
  builder.setNoYCells(num_y_cells);
  std::shared_ptr<lf::mesh::Mesh> mesh_ptr = builder.Build();

  // output mesh information
  const lf::mesh::Mesh &mesh = *mesh_ptr;
  lf::mesh::utils::PrintInfo(mesh, std::cout);
  std::cout << std::endl;

  // build mesh hierarchy
  lf::refinement::MeshHierarchy multi_mesh(mesh_ptr, mesh_factory_ptr);

  for (int step = 0; step < num_steps; ++step) {
    // obtain pointer to mesh on finest level
    const size_type n_levels = multi_mesh.NumLevels();
    std::shared_ptr<const lf::mesh::Mesh> mesh_fine =
        multi_mesh.getMesh(n_levels - 1);

    // print number of entities of various co-dimensions
    std::cout << "Mesh on level " << n_levels - 1 << ": "
              << mesh_fine->NumEntities(2) << " nodes, "
              << mesh_fine->NumEntities(1) << " edges, "
              << mesh_fine->NumEntities(0) << " cells," << std::endl;

    lf::io::writeMatplotlib(*mesh_fine, std::string("torus_refinement") +
                                            std::to_string(step) + ".csv");

    lf::io::VtkWriter vtk_writer(mesh_fine, std::string("torus_refinement") +
                                                std::to_string(step) + ".vtk");

    multi_mesh.RefineRegular();
  }

  return 0;
}


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

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

 * See LICENSE for the license information

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

/**
 * @file    testGaussianISAM.cpp
 * @brief   Unit tests for GaussianISAM
 * @author  Michael Kaess
 */

#include <CppUnitLite/TestHarness.h>

#include <tests/smallExample.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/inference/Ordering.h>

#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <boost/range/adaptor/map.hpp>
namespace br { using namespace boost::adaptors; using namespace boost::range; }

using namespace std;
using namespace gtsam;
using namespace example;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
TEST( ISAM, iSAM_smoother )
{
  Ordering ordering;
  for (int t = 1; t <= 7; t++) ordering += X(t);

  // Create smoother with 7 nodes
  GaussianFactorGraph smoother = createSmoother(7);

  // run iSAM for every factor
  GaussianISAM actual;
  for(boost::shared_ptr<GaussianFactor> factor: smoother) {
    GaussianFactorGraph factorGraph;
    factorGraph.push_back(factor);
    actual.update(factorGraph);
  }

  // Create expected Bayes Tree by solving smoother with "natural" ordering
  GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);

  // Verify sigmas in the bayes tree
  for(const GaussianBayesTree::sharedClique& clique: expected.nodes() | br::map_values) {
    GaussianConditional::shared_ptr conditional = clique->conditional();
    EXPECT(!conditional->get_model());
  }

  // Check whether BayesTree is correct
  EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian()));

  // obtain solution
  VectorValues e; // expected solution
  for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2));
  VectorValues optimized = actual.optimize(); // actual solution
  EXPECT(assert_equal(e, optimized));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


#include <mtl/dense1D.h>
#include <mtl/mtl.h>
#include <mtl/utils.h>

/*
  example output:

  [7,7,7,7,7,7,7,7,7,7]

  */

int
main()
{
  using namespace mtl;
  //begin
  dense1D< double > x(10,2);
  dense1D< double > y(10,3);
  add(scaled(x,2), y, y);
  print_vector(y);
  //end
  return 0;
}


#pragma once
#include <tuple>
#include <cfloat>
#include <Eigen/Core>

#include "Node.hpp"
#include "Context.hpp"

namespace geos
{
	namespace geom
	{
		class Geometry;
		class Polygon;
		class CoordinateArraySequence;
	}
	namespace operation
	{
		namespace buffer {}
		namespace distance {}
	}
}

namespace cgl
{
	namespace gg = geos::geom;
	namespace gob = geos::operation::buffer;
	namespace god = geos::operation::distance;

	bool ReadDouble(double& output, const std::string& name, const Record& record, std::shared_ptr<Context> environment);

	std::tuple<double, double> ReadVec2Packed(const PackedRecord& record);

	struct Path
	{
		std::unique_ptr<gg::CoordinateArraySequence> cs;
		std::vector<double> distances;
	};

	Path ReadPathPacked(const PackedRecord& record);

	struct Transform
	{
		using Mat3x3 = Eigen::Matrix<double, 3, 3, 0, 3, 3>;

		Transform()
		{
			init();
		}

		Transform(double px, double py, double sx = 1, double sy = 1, double angle = 0)
		{
			init(px, py, sx, sy, angle);
		}

		Transform(const Mat3x3& mat) :mat(mat) {}

		Transform(const Record& record, std::shared_ptr<Context> pEnv);

		void init(double px = 0, double py = 0, double sx = 1, double sy = 1, double angle = 0);

		Transform operator*(const Transform& other)const
		{
			return static_cast<Mat3x3>(mat * other.mat);
		}

		Eigen::Vector2d product(const Eigen::Vector2d& v)const;

		void printMat()const;

	private:
		Mat3x3 mat;
	};

	struct TransformPacked
	{
		using Mat3x3 = Eigen::Matrix<double, 3, 3, 0, 3, 3>;

		TransformPacked()
		{
			init();
		}

		TransformPacked(double px, double py, double sx = 1, double sy = 1, double angle = 0)
		{
			init(px, py, sx, sy, angle);
		}

		TransformPacked(const Mat3x3& mat) :mat(mat) {}

		TransformPacked(const PackedRecord& record, std::shared_ptr<Context> pEnv);

		void init(double px = 0, double py = 0, double sx = 1, double sy = 1, double angle = 0);

		TransformPacked operator*(const TransformPacked& other)const
		{
			return static_cast<Mat3x3>(mat * other.mat);
		}

		Eigen::Vector2d product(const Eigen::Vector2d& v)const;

		void printMat()const;

	private:
		Mat3x3 mat;
	};

	class BoundingRect
	{
	public:

		BoundingRect() = default;

		BoundingRect(const Vector<Eigen::Vector2d>& vs)
		{
			add(vs);
		}

		void add(const Eigen::Vector2d& v);

		void add(const Vector<Eigen::Vector2d>& vs);

		bool intersects(const BoundingRect& other)const
		{
			return std::max(m_min.x(), other.m_min.x()) < std::min(m_max.x(), other.m_max.x())
				&& std::max(m_min.y(), other.m_min.y()) < std::min(m_max.y(), other.m_max.y());
		}

		bool includes(const Eigen::Vector2d& point)const
		{
			return m_min.x() < point.x() && point.x() < m_max.x()
				&& m_min.y() < point.y() && point.y() < m_max.y();
		}

		Eigen::Vector2d pos()const
		{
			return m_min;
		}

		Eigen::Vector2d center()const
		{
			return (m_min + m_max)*0.5;
		}

		Eigen::Vector2d width()const
		{
			return m_max - m_min;
		}

		double area()const
		{
			const auto wh = width();
			return wh.x()*wh.y();
		}

	private:
		Eigen::Vector2d m_min = Eigen::Vector2d(DBL_MAX, DBL_MAX);
		Eigen::Vector2d m_max = Eigen::Vector2d(-DBL_MAX, -DBL_MAX);
	};

	bool ReadPolygon(Vector<Eigen::Vector2d>& output, const List& vertices, std::shared_ptr<Context> pEnv, const Transform& transform);

	void GetBoundingBoxImpl(BoundingRect& output, const List& list, std::shared_ptr<Context> pEnv, const Transform& transform);

	void GetBoundingBoxImpl(BoundingRect& output, const Record& record, std::shared_ptr<Context> pEnv, const Transform& parent = Transform());

	boost::optional<BoundingRect> GetBoundingBox(const Val& value, std::shared_ptr<Context> pEnv);

	using PolygonsStream = std::multimap<double, std::string>;

	std::string GetGeometryType(gg::Geometry* geometry);

	gg::Polygon* ToPolygon(const Vector<Eigen::Vector2d>& exterior);

	void GeosPolygonsConcat(std::vector<gg::Geometry*>& head, const std::vector<gg::Geometry*>& tail);

	std::vector<gg::Geometry*> GeosFromRecord(const Val& value, std::shared_ptr<cgl::Context> pEnv, const cgl::Transform& transform = cgl::Transform());

	std::vector<gg::Geometry*> GeosFromRecordPacked(const PackedVal& value, std::shared_ptr<cgl::Context> pEnv, const cgl::TransformPacked& transform = cgl::TransformPacked());

	Record GetPolygon(const gg::Polygon* poly, std::shared_ptr<cgl::Context> pEnv);

	List GetShapesFromGeos(const std::vector<gg::Geometry*>& polygons, std::shared_ptr<cgl::Context> pEnv);

	PackedList GetPackedShapesFromGeos(const std::vector<gg::Geometry*>& polygons);

	bool OutputSVG(std::ostream& os, const Val& value, std::shared_ptr<Context> pEnv);

	bool OutputSVG2(std::ostream& os, const Val& value, std::shared_ptr<Context> pEnv, const std::string& name);
}


// Eigen
#include <Eigen/Dense>
#include <Eigen/SVD>

// OpenCV
#include <opencv2/opencv.hpp>

// ROS
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/Float64MultiArray.h>

#include <crane_msgs/CranePendulumImagePoints.h>

namespace crane_vision
{
using namespace sensor_msgs;
using namespace message_filters::sync_policies;

using ProjectionMatrix = Eigen::Matrix<double, 3, 4>;
using Vector3d = Eigen::Matrix<double, 3, 1>;
using StateVector = Eigen::Matrix<double, 6, 1>;
using TransitionMatrix = Eigen::Matrix<double, 6, 6>;
using CovarianceMatrix = Eigen::Matrix<double, 6, 6>;
using AccelerationVector = Eigen::Vector2d;

Vector3d dlt(const Eigen::Vector2d& c0, const Eigen::Vector2d& c1, const Eigen::Vector2d& c2,
             const ProjectionMatrix& P0, const ProjectionMatrix& P1, const ProjectionMatrix P2)
{
  Eigen::Matrix<double, 6, 4> X;
  X.row(0) = c0(0) * P0.row(2) - P0.row(0);
  X.row(1) = c0(1) * P0.row(2) - P0.row(1);
  X.row(2) = c1(0) * P1.row(2) - P1.row(0);
  X.row(3) = c1(1) * P1.row(2) - P1.row(1);
  X.row(4) = c2(0) * P2.row(2) - P2.row(0);
  X.row(5) = c2(1) * P2.row(2) - P2.row(1);

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(X, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::MatrixXd V = svd.matrixV();

  Eigen::Vector3d L = Eigen::Vector3d::Zero();
  double scale = V(3, 3);
  if (scale > 1e-8)
  {
    L = V.col(3).topRows(3) / scale;
  }
  return L;
}

Vector3d findLine(const Eigen::Vector2d& c01, const Eigen::Vector2d& c02, const Eigen::Vector2d& c11,
                  const Eigen::Vector2d& c12, const Eigen::Vector2d& c21, const Eigen::Vector2d& c22,
                  const ProjectionMatrix& P0, const ProjectionMatrix& P1, const ProjectionMatrix P2)
{
  Vector3d X1 = dlt(c01, c11, c21, P0, P1, P2);
  Vector3d X2 = dlt(c02, c12, c22, P0, P1, P2);
  Vector3d L = X2 - X1;
  L.normalize();
  return L;
}

StateVector fk(StateVector x, AccelerationVector u, double w, double L)
{
  double c0 = cos(x(0));
  double c1 = cos(x(1));
  double s0 = sin(x(0));
  double s1 = sin(x(1));

  StateVector x1 = StateVector::Zero();
  x1(0) = x(2);
  x1(1) = x(3);
  x1(2) = (2.0 * x(2) * x(3) * s1 - w * s0 + (u(1) * c0 / L)) / c1;
  x1(3) = -c1 * s1 * x(2) * x(2) - (u(0) * c1 + u(1) * s0 * s1) / L - w * c0 * s1;
  return x1;
}

TransitionMatrix Fk(StateVector x, AccelerationVector u, double w, double L, double dt)
{
  double c0 = cos(x(0));
  double c1 = cos(x(1));
  double s0 = sin(x(0));
  double s1 = sin(x(1));

  TransitionMatrix X = TransitionMatrix::Identity();
  X(0, 2) = dt;
  X(1, 3) = dt;
  X(2, 0) = -(dt * (w * c0 + (u(1) * s0) / L)) / c1;
  X(2, 1) = 2.0 * dt * x(2) * x(3) + (dt * s1 * (2.0 * x(2) * x(3) * s1 - w * s0 + (u(1) * c0) / L)) / (c1 * c1);
  X(2, 2) = (2.0 * dt * x(3) * s1) / c1 + 1.0;
  X(2, 3) = (2.0 * dt * x(2) * s1) / c1;
  X(3, 0) = dt * (w * s0 * s1 - (u[1] * c0 * s1) / L);
  X(3, 1) = -dt * (x(2) * x(2) * c1 * c1 - x(2) * x(2) * s1 * s1 - (u(0) * s1 - u(1) * c1 * s0 / L) + w * c0 * c1);
  X(3, 2) = -2.0 * dt * x(2) * c1 * s1;
  return X;
}

auto ekf(Vector3d Lvec, AccelerationVector uk, Eigen::MatrixXd hat_Pkm1, Eigen::VectorXd hat_thetakm1, double r,
         double dt)
{
  int D = 10;
  double g = 9.81;
  double L = r;
  AccelerationVector u = uk;
  Eigen::VectorXd x = hat_thetakm1;
  Eigen::Matrix2d R;
  R << 0.00377597, -0.00210312, -0.00210312, 0.00125147;

  CovarianceMatrix Q = CovarianceMatrix::Zero();
  Q.diagonal() << 0.00003, 0.00003, 0.0005, 0.0005, 0.0001, 0.0001;
  Eigen::Matrix<double, 2, 6> H;
  H << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;

  TransitionMatrix Fi = Fk(x, u, g / r, L, dt);

  Eigen::Vector2d zkp1;
  zkp1 << atan2(-Lvec(1), Lvec(2)), atan2(Lvec(0), sqrt(Lvec(1) * Lvec(1) + Lvec(2) * Lvec(2)));

  for (int i = 0; i < D; ++i)
  {
    x = x + fk(x, u, g / r, L) * dt / D;
  }

  Eigen::MatrixXd barP_kp1 = (Fi * hat_Pkm1 * Fi.transpose()) + Q;
  Eigen::MatrixXd K_kp1 = barP_kp1 * H.transpose() * (R + H * barP_kp1 * H.transpose()).inverse();

  Eigen::VectorXd hat_thetak = x + K_kp1 * (zkp1 - H * x);
  Eigen::MatrixXd hat_Pk = (Eigen::Matrix<double, 6, 6>::Identity() - K_kp1 * H) * barP_kp1;

  return std::make_tuple(hat_thetak, hat_Pk);
}

struct CompareArea
{
  CompareArea(const std::vector<float>& areas) : areas_(&areas)
  {
  }
  bool operator()(int a, int b) const
  {
    return (*areas_)[a] > (*areas_)[b];
  }
  const std::vector<float>* areas_;
};

class CraneVisionEKF : public nodelet::Nodelet {

  virtual void onInit();

};

class CraneVisionNodelet : public nodelet::Nodelet
{
  virtual void onInit();

  bool extractSphereCenters(const sensor_msgs::ImageConstPtr& image_msg,
                            const sensor_msgs::CameraInfoConstPtr& info_msg, const cv::Rect& roi,
                            std::vector<double>& points, bool debug, const std::string& winname);

  void imageCb(const sensor_msgs::ImageConstPtr& image0_msg, const sensor_msgs::CameraInfoConstPtr& info0_msg,
               const sensor_msgs::ImageConstPtr& image1_msg, const sensor_msgs::CameraInfoConstPtr& info1_msg,
               const sensor_msgs::ImageConstPtr& image2_msg, const sensor_msgs::CameraInfoConstPtr& info2_msg);

  boost::shared_ptr<image_transport::ImageTransport> it_;

  /// Subscriptions
  image_transport::SubscriberFilter image0_sub_, image1_sub_, image2_sub_;
  message_filters::Subscriber<CameraInfo> image0_info_sub_, image1_info_sub_, image2_info_sub_;
  typedef ExactTime<Image, CameraInfo, Image, CameraInfo, Image, CameraInfo> ExactPolicy;
  typedef ApproximateTime<Image, CameraInfo, Image, CameraInfo, Image, CameraInfo> ApproximatePolicy;
  typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
  typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
  boost::shared_ptr<ExactSync> exact_sync_;
  boost::shared_ptr<ApproximateSync> approximate_sync_;

  image_transport::CameraSubscriber camera_sub_;
  ros::Publisher pub_;

  int hmin_, hmax_, smin_, smax_, vmin_, vmax_;
  cv::Rect roi0_, roi1_, roi2_;

  Eigen::Matrix<double, 6, 6> hat_Pkm1_;
  StateVector hat_thetakm1_;
  ProjectionMatrix P0_, P1_, P2_;

  bool debug_;
};

void CraneVisionNodelet::onInit()
{
  ros::NodeHandle& nh = getNodeHandle();
  ros::NodeHandle& private_nh = getPrivateNodeHandle();
  it_.reset(new image_transport::ImageTransport(nh));

  // Synchronize inputs. Topic subscriptions happen on demand in the connection
  // callback. Optionally do approximate synchronization.
  int queue_size;
  private_nh.param("queue_size", queue_size, 5);
  bool approx;
  private_nh.param("approximate_sync", approx, false);
  if (approx)
  {
    approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size), image0_sub_, image0_info_sub_,
                                                image1_sub_, image1_info_sub_, image2_sub_, image2_info_sub_));
    approximate_sync_->registerCallback(boost::bind(&CraneVisionNodelet::imageCb, this, _1, _2, _3, _4, _5, _6));
  }
  else
  {
    exact_sync_.reset(new ExactSync(ExactPolicy(queue_size), image0_sub_, image0_info_sub_, image1_sub_,
                                    image1_info_sub_, image2_sub_, image2_info_sub_));
    exact_sync_->registerCallback(boost::bind(&CraneVisionNodelet::imageCb, this, _1, _2, _3, _4, _5, _6));
  }

  std::vector<int> roi;
  if (private_nh.getParam("roi0", roi))
  {
    roi0_ = cv::Rect(roi[0], roi[1], roi[2], roi[3]);
  }
  else
  {
    NODELET_ERROR_STREAM("Did not find any roi0 parameters!");
  }
  if (private_nh.getParam("roi1", roi))
  {
    roi1_ = cv::Rect(roi[0], roi[1], roi[2], roi[3]);
  }
  else
  {
    NODELET_ERROR_STREAM("Did not find any roi1 parameters!");
  }
  if (private_nh.getParam("roi2", roi))
  {
    roi2_ = cv::Rect(roi[0], roi[1], roi[2], roi[3]);
  }
  else
  {
    NODELET_ERROR_STREAM("Did not find any roi2 parameters!");
  }

  private_nh.param("debug", debug_, false);

  private_nh.param("hmin", hmin_, 43);
  private_nh.param("hmax", hmax_, 73);
  private_nh.param("smin", smin_, 54);
  private_nh.param("smax", smax_, 250);
  private_nh.param("vmin", vmin_, 86);
  private_nh.param("vmax", vmax_, 255);

  image0_sub_.subscribe(*it_, "/camera0/image_raw", 1);
  image0_info_sub_.subscribe(nh, "/camera0/camera_info", 1);
  image1_sub_.subscribe(*it_, "/camera1/image_raw", 1);
  image1_info_sub_.subscribe(nh, "/camera1/camera_info", 1);
  image2_sub_.subscribe(*it_, "/camera2/image_raw", 1);
  image2_info_sub_.subscribe(nh, "/camera2/camera_info", 1);

  pub_ = private_nh.advertise<crane_msgs::CranePendulumImagePoints>("points", 1);

  P0_ << 942.0, 0.0, 623.66, 0.0, 0.0, 942.0, 345.69, 0.0, 0.0, 0.0, 1.0, 0.0;
  P1_ << 941.0, 0.0, 637.21, 220005.80000000002, 0.0, 941.0, 349.9, 0.0, 0.0, 0.0, 1.0, 0.0;
  P2_ << 937.0, 0.0, 637.21, 437579.0, 0.0, 937.0, 381.54, 0.0, 0.0, 0.0, 1.0, 0.0;

  hat_thetakm1_.setZero();
  // hat_Pkm1_ << 0.0, 0.0, 0.0, 0.0, 0.04, -0.03;
}

bool CraneVisionNodelet::extractSphereCenters(const sensor_msgs::ImageConstPtr& image_msg,
                                              const sensor_msgs::CameraInfoConstPtr& info_msg, const cv::Rect& roi,
                                              std::vector<double>& points, bool debug, const std::string& winname)
{
  using namespace std;
  using namespace cv;

  Mat bgr8_image = cv_bridge::toCvShare(image_msg, "bgr8")->image;

  Mat roi_image = bgr8_image(roi);

  Mat hsv_image;
  cvtColor(roi_image, hsv_image, CV_BGR2HSV);

  Mat thresh_image;
  inRange(hsv_image, Scalar(hmin_, smin_, vmin_), Scalar(hmax_, smax_, vmax_), thresh_image);

  using Contours = vector<vector<Point> >;
  using Hierarchy = vector<Vec4i>;
  Contours contours;
  Hierarchy hierarchy;

  /// Find contours
  findContours(thresh_image, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
  if (contours.size() > 1)
  {
    /// Get sort index based on contour area
    vector<int> sort_idx(contours.size());
    vector<float> areas(contours.size());
    for (int n = 0; n < (int)contours.size(); n++)
    {
      sort_idx[n] = n;
      areas[n] = contourArea(contours[n], false);
    }
    sort(sort_idx.begin(), sort_idx.end(), CompareArea(areas));

    /// Get the moments
    vector<Moments> mu(contours.size());
    for (int i = 0; i < contours.size(); i++)
    {
      mu[i] = moments(contours[i], false);
    }

    ///  Get the mass centers:
    vector<Point2d> mc(contours.size());
    for (int i = 0; i < contours.size(); i++)
    {
      mc[i] = Point2d(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
    }

    /// Swap to get right order
    if (mc[sort_idx[1]].y > mc[sort_idx[0]].y)
    {
      points[0] = mc[sort_idx[1]].x;
      points[1] = mc[sort_idx[1]].y;
      points[2] = mc[sort_idx[0]].x;
      points[3] = mc[sort_idx[0]].y;
    }
    else
    {
      points[0] = mc[sort_idx[0]].x;
      points[1] = mc[sort_idx[0]].y;
      points[2] = mc[sort_idx[1]].x;
      points[3] = mc[sort_idx[1]].y;
    }

    if (debug)
    {
      circle(roi_image, Point2d(points[0], points[1]), 5, Scalar(0, 0, 255), -1, 8, 0);
      circle(roi_image, Point2d(points[2], points[3]), 5, Scalar(255, 0, 0), -1, 8, 0);
      imshow(winname, roi_image);
      waitKey(30);
    }
  }
}

void CraneVisionNodelet::imageCb(const sensor_msgs::ImageConstPtr& image0_msg,
                                 const sensor_msgs::CameraInfoConstPtr& info0_msg,
                                 const sensor_msgs::ImageConstPtr& image1_msg,
                                 const sensor_msgs::CameraInfoConstPtr& info1_msg,
                                 const sensor_msgs::ImageConstPtr& image2_msg,
                                 const sensor_msgs::CameraInfoConstPtr& info2_msg)
{
  using namespace std;
  using namespace Eigen;

  vector<double> points0{ 0.0, 0.0, 0.0, 0.0 };
  vector<double> points1{ 0.0, 0.0, 0.0, 0.0 };
  vector<double> points2{ 0.0, 0.0, 0.0, 0.0 };

  extractSphereCenters(image0_msg, info0_msg, roi0_, points0, debug_, "camera0");
  extractSphereCenters(image1_msg, info1_msg, roi1_, points1, debug_, "camera1");
  extractSphereCenters(image2_msg, info2_msg, roi2_, points2, debug_, "camera2");

  Vector2d center01 = Vector2d(points0[0] + roi0_.x, points0[1] + roi0_.y);
  Vector2d center02 = Vector2d(points0[2] + roi0_.x, points0[3] + roi0_.y);
  Vector2d center11 = Vector2d(points1[0] + roi1_.x, points1[1] + roi1_.y);
  Vector2d center12 = Vector2d(points1[2] + roi1_.x, points1[3] + roi1_.x);
  Vector2d center21 = Vector2d(points2[0] + roi2_.x, points2[1] + roi2_.y);
  Vector2d center22 = Vector2d(points2[2] + roi2_.x, points2[3] + roi2_.y);

  // Vector3d Lc0 = findLine(center01, center02, center11, center12, center21, center22, P0_, P1_, P2_);

  // todo(Lars): Transform to inertial coordinatesA
  // Vector3d Lvec = Lc0;

  // std::tie(hat_thetakm1_, hat_Pkm1_) = ekf(Lvec, AccelerationVector(0.0,0.0), hat_thetakm1_, hat_Pkm1_, 1.05, 0.01);

  crane_msgs::CranePendulumImagePoints msg;
  // Reuse the synchronized time stamp
  msg.header.stamp = image0_msg->header.stamp;
  std::vector<double> data(12);
  msg.points =
      std::vector<double>{ points0[0] + roi0_.x, points0[1] + roi0_.y, points0[2] + roi0_.x, points0[3] + roi0_.y,
                           points1[0] + roi1_.y, points1[1] + roi1_.y, points1[2] + roi1_.y, points1[3] + roi1_.y,
                           points2[0] + roi2_.y, points2[1] + roi2_.y, points2[2] + roi2_.y, points2[3] + roi2_.y };
  pub_.publish(msg);
}

}  // namespace crane_vision

// Register nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(crane_vision::CraneVisionNodelet, nodelet::Nodelet)


// STL includes
#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <random>
#include <utility>
#include <thread>

// Local includes
#include "Merl.hpp"
#include <tests/Tests.hpp>
#include <tests/SH.hpp>
#include <include/Utils.hpp>
#include <include/SphericalHarmonics.hpp>
#include <include/DirectionsSampling.hpp>
#include <include/SphericalIntegration.hpp>

// GLM include
#include <glm/glm.hpp>

// Include Eigen
#include <Eigen/Core>

struct MerlProjectionThread : public std::thread {

   int order;
   std::vector<Eigen::MatrixXf> cijs;

   MerlProjectionThread(const MerlBRDF* brdf,
                        const std::vector<Vector>* ws,
                        int order, int skip, int nthread) :
      std::thread(&MerlProjectionThread::run, this, brdf, ws, skip, order, nthread) {}

   /* Pre-convolved the Zonal basis to perform smooth evaluation.
    */
   static void ApplyZonalFilter(Eigen::VectorXf& clm) {
      const auto bmax = floor(sqrt(clm.size())-1);
      for(unsigned int i=0; i<clm.size(); ++i) {
         const auto b = floor(sqrt(i));
         const auto f = exp(- pow(float(b)/float(bmax), 2));
         clm[i] *= f;
      }
   }

   void run(const MerlBRDF* brdf,
            const std::vector<Vector>* dirs,
            int skip, int order, int nthread) {

      cijs = std::vector<Eigen::MatrixXf>(6, Eigen::MatrixXf::Zero(SH::Terms(order), SH::Terms(order)));

      const int size = SH::Terms(order);
      Eigen::VectorXf ylmo(size);
      Eigen::VectorXf ylmi(size);
      for(unsigned int i=skip; i<dirs->size(); i+=nthread) {
         const Vector& wo = (*dirs)[i];

         if(skip == 0) {
            std::cout << "Progress: " << i << " / " << dirs->size() << "     \r";
            std::cout.flush();
         }

         // Skip below the horizon configuration
         if(wo.z < 0.0) continue;
         SH::FastBasis(wo, order, ylmo);

         for(unsigned int j=0; j<dirs->size(); ++j) {
            const Vector& wi = (*dirs)[j];
            // Skip below the horizon configuration
            if(wi.z < 0.0) continue;

            // Evaluate the BRDF value
            const auto rgb = brdf->value<Vector, Vector>(wi, wo);
            SH::FastBasis(wi, order, ylmi);

            // Apply filtering
            //ApplyZonalFilter(ylmo);
            //ApplyZonalFilter(ylmi);

            Eigen::MatrixXf mat = ylmo * ylmi.transpose();
#ifndef SYMMETRIZE
            mat = 0.5f*(mat + mat.transpose());
#endif
            cijs[0] += rgb[0] * mat;
            cijs[1] += rgb[1] * mat;
            cijs[2] += rgb[2] * mat;
            // Note: Here the correct weighting should be with respect to wi.z
            // but I use wo.z since it allows to reduce the ringing drastically.
#ifdef LOOKS_BETTER
            cijs[3] += rgb[0] * wo.z * mat;
            cijs[4] += rgb[1] * wo.z * mat;
            cijs[5] += rgb[2] * wo.z * mat;
#else // CORRECT
            cijs[3] += rgb[0] * wi.z * mat;
            cijs[4] += rgb[1] * wi.z * mat;
            cijs[5] += rgb[2] * wi.z * mat;
#endif
         }
      }
   }
};

int MerlProjectionMatrix(const std::string& filename,
                         int order = 15, int N = 1000) {

   // Constants
   const int size = SH::Terms(order);

   // Load the BRDF
   MerlBRDF brdf;
   if(! brdf.read_brdf(filename)) {
      std::cerr << "Failed: unable to load the MERL brdf" << std::endl;
      return 1;
   }

   const auto k = filename.rfind('.');
   std::string ofilename = filename;
   ofilename.replace(k, std::string::npos, ".mats");
   std::cout << "Will output to \"" << ofilename << "\"" << std::endl;

   // Values
   std::vector<Eigen::MatrixXf> cijs(6, Eigen::MatrixXf::Zero(size, size));
   const auto dirs = SamplingFibonacci<Vector>(N);

   const int nbthreads = std::thread::hardware_concurrency();
   std::vector<MerlProjectionThread*> threads;
   for(int k=0; k<nbthreads; ++k) {
      MerlProjectionThread* th = new MerlProjectionThread(&brdf, &dirs, order, k, nbthreads);
      threads.push_back(th);
   }

   for(MerlProjectionThread* th : threads) {
      th->join();
      cijs[0] += th->cijs[0];
      cijs[1] += th->cijs[1];
      cijs[2] += th->cijs[2];
      cijs[3] += th->cijs[3];
      cijs[4] += th->cijs[4];
      cijs[5] += th->cijs[5];
      delete th;
   }
   const float factor = 16.0*M_PI*M_PI / float(N*N);
   cijs[0] *= factor;
   cijs[1] *= factor;
   cijs[2] *= factor;
   cijs[3] *= factor;
   cijs[4] *= factor;
   cijs[5] *= factor;

   SaveMatrices(ofilename, cijs);

   // Print values
   std::string gfilename = filename;
   gfilename.replace(k, std::string::npos, ".gnuplot");
   std::ofstream file(gfilename.c_str(), std::ios_base::trunc);
   const float thetai = -0.5f*M_PI * 30.f/90.f;
   const Vector wi(sin(thetai), 0, cos(thetai));
   const auto ylmi = SH::FastBasis(wi, order);
   Vector wo;
   for(int i=0; i<90; ++i) {
      const float theta = 0.5*M_PI * i / float(90);
      wo.x = sin(theta);
      wo.y = 0;
      wo.z = cos(theta);

      // Ref
      const auto RGB = brdf.value<Vector, Vector>(wi, wo);
      const float R = RGB[0];
      const float G = RGB[1];
      const float B = RGB[2];

      // SH expansion
      const auto ylmo = SH::FastBasis(wo, order);
      const Eigen::VectorXf rlm = cijs[0] * ylmo;
      const Eigen::VectorXf glm = cijs[1] * ylmo;
      const Eigen::VectorXf blm = cijs[2] * ylmo;
      const float r = ylmi.dot(rlm);
      const float g = ylmi.dot(glm);
      const float b = ylmi.dot(blm);

      file << theta << "\t" << r << "\t" << g << "\t" << b
                    << "\t" << R << "\t" << G << "\t" << B
                    << std::endl;
   }

   int nb_fails = 0;
   return nb_fails;
}

bool parseArguments(int argc, char** argv, std::string& filename,
                    int& order, int& nb) {

   // Loop over all the different elements of the command line and search for
   // some patterns
   for(int k=0; k<argc; ++k) {
      if(argv[k] == std::string("-h") || argv[k] == std::string("--help")) {
         std::cerr << "Usage: merl2sh [options] filename.binary" << std::endl;
         return false;
      }
      if((argv[k] == std::string( "-o") || argv[k] == std::string("--order")) && k+1<argc) {
         order = std::atoi(argv[k+1]);
      }
      if((argv[k] == std::string( "-n") || argv[k] == std::string("--nb")) && k+1<argc) {
         nb = std::atoi(argv[k+1]);
      }
   }

   // The filename to convert is the last argument of the command line.
   if(argc > 1) {
      filename = argv[argc-1];
      return true;
   } else {
      return false;
   }
}

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

   int nb_fails = 0;
   std::string filename;
   int order = 3;
   int nb = 1000;
   if(! parseArguments(argc, argv, filename, order, nb)) {
      return EXIT_SUCCESS;
   }

   // Load an example
   nb_fails += MerlProjectionMatrix(argv[argc-1], order, nb);

   if(nb_fails > 0) {
      return EXIT_FAILURE;
   } else {
      return EXIT_SUCCESS;
   }
}


/*! \file demo_boxplot_simple.cpp
    \brief An example to demonstrate simplest use of boxplot. See also boxplot_full.cpp for a wider range of use.
*/

// demo_boxplot_simple.cpp
//
// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2008, 2009, 2021

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

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.

// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[boxplot_simple_1

/*`
Boxplot is a convenient way of graphically depicting groups of numerical data
through their five-number summaries.
Show 1st quartile, median and 3rd quartile as a box,
95% confidence interval as whiskers,
outliers and extreme outliers.

See [@http://en.wikipedia.org/wiki/Boxplot boxplot] and

Some Implementations of the Boxplot
Michael Frigge, David C. Hoaglin and Boris Iglewicz
The American Statistician, Vol. 43, No. 1 (Feb., 1989), pp. 50-54

First we need a few includes to use Boost.Plot.
*/

#include <vector>
 // using std::vector;
#include <cmath>
//  using ::sin;
#include <boost/svg_plot/svg_boxplot.hpp>

#include <iostream>
//  using std::cout;
//  using std::endl;
#include <exception>

/*`Use two functions, 1/x and sin(x), to simulate distributions.
*/

double f(double x)
{ // Effectively 1/x.
  return 50 / x;
}

double g(double x)
{ // Effectively sin(x).
  return 60 + 25 * sin(x * 50);
}
//] [boxplot_simple_1]

int main()
{
  using namespace boost::svg;
  try
  {
//[boxplot_simple_2]
/*`10 values are computed and stored in two `std::vector`s.
*/
  std::vector<double> data1;
  std::vector<double> data2;

  std::cout.precision(2);
  for(double i = 0.1; i < 10; i += 0.1)
  {   // Fill our vectors with 100 values:
    double fv = f(i);
    double gv = g(i);
    // std::cout << i << ' ' << fv << ' ' << gv << std::endl; // Optionally display values?
    data1.push_back(fv);
    data2.push_back(gv);
  }

/*`A new boxplot is constructed and a few settings added.
*/
  svg_boxplot my_boxplot;

  my_boxplot  // Title and axes labels.
    .title("Boxplots of 1/x and sin(x) Functions")
    .x_label("Functions")
    .y_label("Population Size")
    .background_border_color(magenta);

  my_boxplot.y_range(0, 100)  // Axis information.
    .y_major_interval(20);

/*`Add the two data series containers, and their labels, to the plot.
*/
  my_boxplot.plot(data1, "[50 / x]");
  my_boxplot.plot(data2, "[60 + 25 * sin(x * 50)]");

/*`Finally write the SVG plot to a file.
*/
  my_boxplot.write("boxplot_simple.svg");
/*`You can view the plot at boxplot_simple.svg."
*/
//] [boxplot_simple_2]
  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n  " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

Output:

Autorun "j:\Cpp\SVG\debug\demo_boxplot_simple.exe"

Build Time 0:00
Build log was saved at "file://j:\Cpp\SVG\demo_boxplot_simple\Debug\BuildLog.htm"

*/


#include <Eigen/Core>
#include <catch2/catch.hpp>

#include <finitediff.hpp>

#include <ipc/barrier/barrier.hpp>

TEST_CASE("Test barrier derivatives", "[barrier]")
{
    double dhat = GENERATE(range(-5, 2));
    dhat = pow(10, dhat);

    double d =
        GENERATE_COPY(take(10, random(dhat / 2, 0.9 * dhat))); // ∈ [0, d̂]
    Eigen::Matrix<double, 1, 1> d_vec;
    d_vec << d;

    // Check gradient

    Eigen::VectorXd fgrad(1);
    fd::finite_gradient(
        d_vec,
        [&](const Eigen::VectorXd& d) { return ipc::barrier(d[0], dhat); },
        fgrad);

    Eigen::VectorXd grad(1);
    grad << ipc::barrier_gradient(d, dhat);

    CAPTURE(dhat, d, fgrad(0), grad(0));
    CHECK(fd::compare_gradient(fgrad, grad));

    // Check hessian

    fd::finite_gradient(
        d_vec,
        [&](const Eigen::VectorXd& d) {
            return ipc::barrier_gradient(d[0], dhat);
        },
        fgrad);

    grad << ipc::barrier_hessian(d, dhat);

    CAPTURE(dhat, d, fgrad(0), grad(0));
    CHECK(fd::compare_gradient(fgrad, grad));
}


#ifndef SPIRIT_SKIP_HTST

#include <engine/HTST.hpp>
#include <engine/Sparse_HTST.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <engine/Hamiltonian_Heisenberg.hpp>
#include <utility/Constants.hpp>
#include <utility/Logging.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <GenEigsSolver.h>  // Also includes <MatOp/DenseGenMatProd.h>
#include <SymEigsSolver.h>
#include <MatOp/SparseSymMatProd.h>

#include <GenEigsRealShiftSolver.h>

#include <fmt/format.h>
#include <fmt/ostream.h>

namespace C = Utility::Constants;

namespace Engine
{
    namespace Sparse_HTST
    {

        void Sparse_Get_Lowest_Eigenvector(const SpMatrixX & matrix, int nos, scalar & lowest_evalue, VectorX & lowest_evec)
        {
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, "        Using Spectra to compute lowest eigenmode...");

            VectorX evalues;
            MatrixX evectors;

            int n_steps = std::max(2, nos);

            //  Create a Spectra solver
            Spectra::SparseSymMatProd<scalar> op(matrix);
            Spectra::SymEigsSolver< scalar, Spectra::SMALLEST_ALGE, Spectra::SparseSymMatProd<scalar> > matrix_spectrum(&op, 1, n_steps);

            matrix_spectrum.init();
            int nconv = matrix_spectrum.compute();

            if (matrix_spectrum.info() == Spectra::SUCCESSFUL)
            {
                evalues = matrix_spectrum.eigenvalues().real();
                evectors = matrix_spectrum.eigenvectors().real();
            } else {
                Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, "        Failed to calculate lowest eigenmode. Aborting!");
                return;
            }

            lowest_evalue = evalues[0];
            lowest_evec = evectors.col(0);
        }

        // Project vector such that it is orthogonal to all vectors in orth
        void _orth_project(VectorX & vector, const std::vector<VectorX> & orth)
        {
            for(const VectorX & cur : orth)
            {
                vector -= (vector.dot(cur)) * cur;
            }
        }

        void Sparse_Get_Lowest_Eigenvectors_VP(const SpMatrixX & matrix, scalar max_evalue, scalarfield & evalues, std::vector<VectorX> & evecs)
        {
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("    Computing eigenvalues smaller than {}", max_evalue));

            scalar tol = 1e-6;
            scalar evalue_epsilon = 1e-4;
            int n_log_step = 2500;
            scalar cur = 2 * tol;
            scalar m = 0.01;
            scalar step_size = 1e-4;
            int n_iter = 0;
            int nos = matrix.rows()/2;

            scalar sigma_shift = std::max(scalar(5.0), 2*scalar(max_evalue));

            VectorX gradient      = VectorX::Zero(2*nos);
            VectorX gradient_prev = VectorX::Zero(2*nos);
            VectorX velocity      = VectorX::Zero(2*nos);
            scalar cur_evalue_estimate;
            scalar fnorm2, ratio, proj;
            bool run = true;

            // We try to find the lowest n_values eigenvalue/vector pairs
            while(run)
            {
                VectorX x = VectorX::Random(2*nos); // Initialize solver with random normalized vector
                x.normalize();

                fnorm2 = 2*tol*tol;
                n_iter = 0;
                gradient_prev.setZero();
                velocity.setZero();

                bool search = true;
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST,     fmt::format("        Search for eigenpair"));

                while (search)
                {
                    // Compute gradient of unnormalized Rayleigh quotient
                    gradient = 2 * matrix * x;

                    cur_evalue_estimate = 0.5 * x.dot(gradient); // Update the current estimate of our evalue
                    for (int i=0; i<evecs.size(); i++)
                    {
                        gradient += 2 * (sigma_shift - evalues[i]) * (evecs[i].dot(x)) * evecs[i]; // Add the shift so that we dont land on the same eigenvalues we had before. Effectively H -> H + (sigma - lambda) * v^T v, where (lambda, v) is an eigenvalue, eigenvector pair
                    }

                    // Project the gradient orthogonally wrt to x and the previous eigenvectors
                    _orth_project(gradient, {x});

                    velocity = 0.5 * (gradient + gradient_prev) / m;
                    fnorm2 = gradient.squaredNorm();

                    proj = velocity.dot(gradient);
                    ratio = proj/fnorm2;

                    if (proj<=0)
                        velocity.setZero();
                    else
                        velocity = gradient*ratio;

                    // Update x and renormalize
                    x -= step_size * velocity + 0.5/m * step_size * gradient;
                    x.normalize();

                    // Update prev gradient
                    gradient_prev = gradient;

                    // Increment n_iter
                    n_iter++;

                    if(n_iter % n_log_step == 0)
                        Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, fmt::format("        ... Iteration {}: Evalue estimate = {}, Grad. norm = {} (> {})", n_iter, cur_evalue_estimate, std::sqrt(fnorm2), tol));

                    search = (std::sqrt(fnorm2) > tol);
                }

                // Ideally we have found one eigenvalue/vector pair now
                // We save the eigenvalue
                evecs.push_back(x);
                evalues.push_back(x.dot(matrix*x));

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST,     fmt::format("        Found an eigenpair after {} iterations", n_iter));
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST,     fmt::format("        ... Eigenvalue  = {}", evalues.back()));
                if(2*nos>=4)
                    Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, fmt::format("        ... Eigenvector = ({}, {}, {}, ..., {})", evecs.back()[0], evecs.back()[1], evecs.back()[2], evecs.back()[2*nos-1]));
                if (evalues.back() > max_evalue)
                {
                    Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, fmt::format("        No more eigenvalues < {} found. Stopping.", max_evalue));
                    run = false;
                }
            }
        }

        void Sparse_Get_Lowest_Eigenvector_VP(const SpMatrixX & matrix, int nos, const VectorX & init, scalar & lowest_evalue, VectorX & lowest_evec)
        {
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, "        Minimizing Rayleigh quotient to compute lowest eigenmode...");
            auto & x = lowest_evec;
            x = init;
            x.normalize();

            scalar tol = 1e-6;
            scalar cur = 2 * tol;
            scalar m = 0.01;
            scalar step_size = 1e-4;
            int n_iter = 0;

            VectorX gradient = VectorX::Zero(2*nos);
            VectorX gradient_prev = VectorX::Zero(2*nos);
            VectorX velocity = VectorX::Zero(2*nos);

            VectorX mx; // temporary for matrix * x
            scalar proj; // temporary for gradient * x
            scalar fnorm2 = 2*tol*tol, ratio;

            while (std::sqrt(fnorm2) > tol)
            {
                // Compute gradient of Rayleigh quotient
                mx = matrix * x;
                gradient = 2 * mx;
                proj = gradient.dot(x);
                gradient -= proj * x;

                velocity = 0.5 * (gradient + gradient_prev) / m;
                fnorm2 = gradient.squaredNorm();

                proj = velocity.dot(gradient);
                ratio = proj/fnorm2;

                if (proj<=0)
                    velocity.setZero();
                else
                    velocity = gradient*ratio;

                // Update x and renormalize
                x -= step_size * velocity + 0.5/m * step_size * gradient;
                x.normalize();

                // Update prev gradient
                gradient_prev = gradient;

                // Increment n_iter
                lowest_evalue = x.dot(matrix * x);
                n_iter++;
            }

            // Compute the eigenvalue
            lowest_evalue = x.dot(matrix * x);
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("        Finished after {} iterations", n_iter));
        }

        // Note the two images should correspond to one minimum and one saddle point
        // Non-extremal images may yield incorrect Hessians and thus incorrect results
        void Calculate(Data::HTST_Info & htst_info)
        {
            Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, "Sparse Prefactor calculation");
            bool lowest_mode_spectra = false;
            htst_info.sparse = true;
            htst_info.n_eigenmodes_keep = 0;

            const scalar epsilon = 1e-4;
            const scalar epsilon_force = 1e-8;

            auto& image_minimum = *htst_info.minimum->spins;
            auto& image_sp      = *htst_info.saddle_point->spins;

            int nos = image_minimum.size();

            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Saving NO eigenvectors.");

            vectorfield force_tmp(nos, {0,0,0});
            std::vector<std::string> block;

            // TODO
            bool is_afm = false;

            // The gradient (unprojected)
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluation of the gradient at the initial configuration...");
            vectorfield gradient_minimum(nos, {0,0,0});
            htst_info.minimum->hamiltonian->Gradient(image_minimum, gradient_minimum);

            // Check if the configuration is actually an extremum
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking if initial configuration is an extremum...");
            Vectormath::set_c_a(1, gradient_minimum, force_tmp);
            Manifoldmath::project_tangential(force_tmp, image_minimum);
            scalar fmax_minimum = Vectormath::max_norm(force_tmp);
            if( fmax_minimum > epsilon_force )
            {
                Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                    "HTST: the initial configuration is not a converged minimum, its max. torque is above the threshold ({} > {})!", fmax_minimum, epsilon_force ));
                return;
            }

            // The gradient (unprojected)
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluation of the gradient at the transition configuration...");
            vectorfield gradient_sp(nos, {0,0,0});
            htst_info.saddle_point->hamiltonian->Gradient(image_sp, gradient_sp);

            // Check if the configuration is actually an extremum
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking if transition configuration is an extremum...");
            Vectormath::set_c_a(1, gradient_sp, force_tmp);
            Manifoldmath::project_tangential(force_tmp, image_sp);
            scalar fmax_sp = Vectormath::max_norm(force_tmp);
            if( fmax_sp > epsilon_force )
            {
                Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                    "HTST: the transition configuration is not a converged saddle point, its max. torque is above the threshold ({} > {})!", fmax_sp, epsilon_force ));
                return;
            }

            ////////////////////////////////////////////////////////////////////////
            // Saddle point
            int n_zero_modes_sp = 0;
            scalarfield evalues_sp = scalarfield(0);
            {
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculation for the Saddle Point");

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate tangent basis ...");
                SpMatrixX tangent_basis = SpMatrixX(3*nos, 2*nos);
                Manifoldmath::sparse_tangent_basis_spherical(image_sp, tangent_basis);

                // Evaluation of the Hessian...
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate the Hessian...");
                SpMatrixX sparse_hessian_sp(3*nos, 3*nos);
                htst_info.saddle_point->hamiltonian->Sparse_Hessian(image_sp, sparse_hessian_sp);

                // Transform into geodesic Hessian
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Transform Hessian into geodesic Hessian...");
                SpMatrixX sparse_hessian_sp_geodesic_3N(3*nos, 3*nos);
                sparse_hessian_bordered_3N(image_sp, gradient_sp, sparse_hessian_sp, sparse_hessian_sp_geodesic_3N);
                SpMatrixX sparse_hessian_sp_geodesic_2N = tangent_basis.transpose() * sparse_hessian_sp_geodesic_3N * tangent_basis;

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate lowest eigenmode of the Hessian...");

                std::vector<VectorX> evecs_sp = std::vector<VectorX>(0);
                Sparse_Get_Lowest_Eigenvectors_VP(sparse_hessian_sp_geodesic_2N, epsilon, evalues_sp, evecs_sp);
                scalar lowest_evalue = evalues_sp[0];
                VectorX & lowest_evector = evecs_sp[0];

                // Check if lowest eigenvalue < 0 (else it's not a SP)
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Check if actually a saddle point...");
                if( lowest_evalue > -epsilon )
                {
                    Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                        "HTST: the transition configuration is not a saddle point, its lowest eigenvalue is above the threshold ({} > {})!", lowest_evalue, -epsilon ));
                    return;
                }
                // Check if second-lowest eigenvalue < 0 (higher-order SP)
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Check if higher order saddle point...");
                int n_negative = 0;
                for( int i=0; i < evalues_sp.size(); ++i )
                    if( evalues_sp[i] < -epsilon )
                        ++n_negative;

                if( n_negative > 1 )
                {
                    Log(Utility::Log_Level::Error, Utility::Log_Sender::All, fmt::format(
                        "HTST: the image you passed is a higher order saddle point (N={})!", n_negative ));
                    return;
                }

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Sparse LU Decomposition of geodesic Hessian...");
                Eigen::SparseLU<SpMatrixX, Eigen::COLAMDOrdering<int> > solver;
                solver.analyzePattern(sparse_hessian_sp_geodesic_2N);
                solver.factorize(sparse_hessian_sp_geodesic_2N);
                htst_info.det_sp = solver.logAbsDeterminant() - std::log(-lowest_evalue);

                // Perpendicular velocity
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Calculate dynamical contribution");

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate the dynamical matrix");
                SpMatrixX velocity(3*nos, 3*nos);
                Sparse_Calculate_Dynamical_Matrix(image_sp, htst_info.saddle_point->geometry->mu_s, sparse_hessian_sp_geodesic_3N, velocity);
                SpMatrixX projected_velocity = tangent_basis.transpose() * velocity * tangent_basis;

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Solving H^-1 V q_1 ...");
                VectorX x(2*nos);
                x = solver.solve(projected_velocity.transpose() * lowest_evector);
                htst_info.s = std::sqrt(lowest_evector.transpose() * projected_velocity * x );

                // Checking for zero modes at the saddle point...
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking for zero modes at the saddle point...");
                for( int i=0; i < evalues_sp.size(); ++i )
                {
                    if( std::abs( evalues_sp[i] ) <= epsilon)
                        ++n_zero_modes_sp;
                }
                // Deal with zero modes if any (calculate volume)
                htst_info.volume_sp = 1;
                if( n_zero_modes_sp > 0 )
                {
                    Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("ZERO MODES AT SADDLE POINT (N={})", n_zero_modes_sp));
                    htst_info.volume_sp = HTST::Calculate_Zero_Volume(htst_info.saddle_point);
                }
            }

            // TODO  // End saddle point
            ////////////////////////////////////////////////////////////////////////

            ////////////////////////////////////////////////////////////////////////
            // Initial state minimum
            int n_zero_modes_minimum = 0;
            scalarfield evalues_min = scalarfield(0);
            {
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculation for the Minimum");

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate tangent basis ...");
                SpMatrixX tangent_basis = SpMatrixX(3*nos, 2*nos);
                Manifoldmath::sparse_tangent_basis_spherical(image_minimum, tangent_basis);

                // Evaluation of the Hessian...
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Evaluate the Hessian...");
                SpMatrixX sparse_hessian_minimum = SpMatrixX(3*nos,3*nos);
                htst_info.minimum->hamiltonian->Sparse_Hessian(image_minimum, sparse_hessian_minimum);

                // Transform into geodesic Hessian
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Transforming Hessian into geodesic Hessian...");
                SpMatrixX sparse_hessian_geodesic_min_3N = SpMatrixX(3*nos, 3*nos);
                sparse_hessian_bordered_3N(image_minimum, gradient_minimum, sparse_hessian_minimum, sparse_hessian_geodesic_min_3N);
                SpMatrixX sparse_hessian_geodesic_min_2N = tangent_basis.transpose() * sparse_hessian_geodesic_min_3N * tangent_basis;

                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Sparse LU Decomposition of geodesic Hessian...");
                Eigen::SparseLU<SpMatrixX, Eigen::COLAMDOrdering<int> > solver;
                solver.analyzePattern(sparse_hessian_geodesic_min_2N);
                solver.factorize(sparse_hessian_geodesic_min_2N);
                htst_info.det_min = solver.logAbsDeterminant();

                // Calculate modes at minimum (needed for zero-mode volume)
                std::vector<VectorX> evecs_min = std::vector<VectorX>(0);
                Sparse_Get_Lowest_Eigenvectors_VP(sparse_hessian_geodesic_min_2N, epsilon, evalues_min, evecs_min);

                // Checking for zero modes at the minimum..
                Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Checking for zero modes at the minimum ...");
                for( int i=0; i < evalues_min.size(); ++i )
                {
                    if( std::abs( evalues_min[i] ) <= epsilon)
                        ++n_zero_modes_minimum;
                    if( evalues_min[i] < 0 )
                    {
                        Log(Utility::Log_Level::Warning, Utility::Log_Sender::HTST, fmt::format("    Minimum has a negative mode with eigenvalue = {}!", evalues_min[i])); // The Question is if we should terminate the calculation here or allow to continue since often the negatives cancel sqrt(-x) * sqrt(-x) = sqrt(x^2)
                    }
                }
                // Deal with zero modes if any (calculate volume)
                htst_info.volume_min = 1;
                if( n_zero_modes_minimum > 0 )
                {
                    Log(Utility::Log_Level::All, Utility::Log_Sender::HTST, fmt::format("ZERO MODES AT MINIMUM (N={})", n_zero_modes_minimum));
                    htst_info.volume_min = HTST::Calculate_Zero_Volume(htst_info.minimum);
                }
            }
            // End initial state minimum
            ////////////////////////////////////////////////////////////////////////

            ////////////////////////////////////////////////////////////////////////
            // Calculation of the prefactor...
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "Calculating prefactor...");

            // Calculate the exponent for the temperature-dependence of the prefactor
            //      The exponent depends on the number of zero modes at the different states
            htst_info.temperature_exponent = 0.5 * (n_zero_modes_minimum - n_zero_modes_sp);

            // Calculate "me"
            htst_info.me = std::pow(2*C::Pi * C::k_B, htst_info.temperature_exponent);

            // Calculate Omega_0, i.e. the entropy contribution
            htst_info.Omega_0 = std::sqrt(std::exp(htst_info.det_min - htst_info.det_sp));

            scalar zero_mode_factor = 1;
            for (int i=0; i<n_zero_modes_minimum; i++)
                zero_mode_factor /= evalues_min[i];

            for (int i=0; i<n_zero_modes_sp; i++)
                zero_mode_factor *= evalues_sp[i+1];

            zero_mode_factor = std::sqrt(zero_mode_factor);

            htst_info.Omega_0 *= zero_mode_factor;

            // Calculate the prefactor
            htst_info.prefactor_dynamical = htst_info.me * htst_info.volume_sp / htst_info.volume_min * htst_info.s;
            htst_info.prefactor = C::g_e / (C::hbar * 1e-12) * htst_info.Omega_0 * htst_info.prefactor_dynamical / ( 2*C::Pi );

            Log.SendBlock(Utility::Log_Level::All, Utility::Log_Sender::HTST,
                {
                    "---- Prefactor calculation successful!",
                    fmt::format("exponent      = {:^20e}", htst_info.temperature_exponent),
                    fmt::format("me            = {:^20e}", htst_info.me),
                    fmt::format("m = Omega_0   = {:^20e}", htst_info.Omega_0),
                    fmt::format("s             = {:^20e}", htst_info.s),
                    fmt::format("volume_sp     = {:^20e}", htst_info.volume_sp),
                    fmt::format("volume_min    = {:^20e}", htst_info.volume_min),
                    fmt::format("log |det_min| = {:^20e}", htst_info.det_min),
                    fmt::format("log |det_sp|  = {:^20e}", htst_info.det_sp),
                    fmt::format("0-mode factor = {:^20e}", zero_mode_factor),
                    fmt::format("hbar[meV*s]   = {:^20e}", C::hbar*1e-12),
                    fmt::format("v = dynamical prefactor = {:^20e}", htst_info.prefactor_dynamical),
                    fmt::format("prefactor               = {:^20e}", htst_info.prefactor)
                }, -1, -1);
        }

        void Sparse_Calculate_Dynamical_Matrix(const vectorfield & spins, const scalarfield & mu_s, const SpMatrixX & hessian, SpMatrixX & velocity)
        {
            constexpr scalar epsilon = 1e-10;
            int nos = spins.size();

            typedef Eigen::Triplet<scalar> T;
            std::vector<T> tripletList;
            tripletList.reserve(hessian.nonZeros());

            auto levi_civita = [](int i, int j, int k)
            {
                return -0.5 * (j-i) * (k-j) * (i-k);
            };

            // We first compute the effective field temporary
            auto b_eff = vectorfield(nos, {0,0,0});
            for (int k=0; k<hessian.outerSize(); ++k)
            {
                for (SpMatrixX::InnerIterator it(hessian,k); it; ++it)
                {
                    int row = it.row(), col = it.col();
                    scalar h = it.value();

                    for(int nu=0; nu<3; nu++)
                    {
                        for(int gamma=0; gamma<3; gamma++)
                        {
                            if( (row-nu) % 3 != 0 || (col-gamma) % 3 != 0 || nu>row || gamma>col)
                                continue;
                            int i = (row-nu)/3.0;
                            int j = (col-gamma)/3.0;
                            b_eff[i][nu] += h * spins[j][gamma] / mu_s[i];
                        }
                    }
                }
            }

            // Add the contributions from the effective field
            for(int i=0; i<nos; i++)
            {
                for(int alpha=0; alpha<3; alpha++)
                {
                    for(int beta=0; beta<3; beta++)
                    {
                        for(int nu=0; nu<3; nu++)
                        {
                            scalar res = levi_civita(alpha, beta, nu) * b_eff[i][nu];
                            if(std::abs(res) > epsilon)
                                tripletList.push_back( T(3*i+alpha, 3*i+beta, res) );
                        }
                    }
                }
            }

            // Iterate over non zero entries of hessian
            for (int k=0; k<hessian.outerSize(); ++k)
            {
                for (SpMatrixX::InnerIterator it(hessian,k); it; ++it)
                {
                    int row = it.row(), col = it.col();
                    scalar h = it.value();

                    for( int mu = 0; mu < 3; mu++ )
                    {
                        for( int nu = 0; nu < 3; nu++ )
                        {
                            for( int alpha = 0; alpha < 3; alpha++ )
                            {
                                for( int beta = 0; beta < 3; beta++ )
                                {
                                    if( (row-nu) % 3 != 0 || (col-beta) % 3 != 0 || nu>row || beta>col )
                                        continue;

                                    int i = (row-nu)/3.0;
                                    int j = (col-beta)/3.0;
                                    scalar res = levi_civita(alpha, mu, nu) * spins[i][mu] * h / mu_s[i];
                                    tripletList.push_back( T(3*i+alpha, 3*j+beta, res) );
                                }
                            }
                        }
                    }
                }
            }

            velocity.setFromTriplets(tripletList.begin(), tripletList.end());
        }

        void sparse_hessian_bordered_3N(const vectorfield & image, const vectorfield & gradient, const SpMatrixX & hessian, SpMatrixX & hessian_out)
        {
            // Calculates a 3Nx3N matrix in the bordered Hessian approach and transforms it into the tangent basis,
            // making the result a 2Nx2N matrix. The bordered Hessian's Lagrange multipliers assume a local extremum.

            int nos = image.size();
            VectorX lambda(nos);
            for (int i=0; i<nos; ++i)
                lambda[i] = image[i].normalized().dot(gradient[i]);

            // Construct hessian_out
            typedef Eigen::Triplet<scalar> T;
            std::vector<T> tripletList;
            tripletList.reserve( hessian.nonZeros() + 3*nos );

            // Iterate over non zero entries of hesiian
            for (int k=0; k<hessian.outerSize(); ++k)
            {
                for (SpMatrixX::InnerIterator it(hessian,k); it; ++it)
                {
                    tripletList.push_back( T(it.row(), it.col(), it.value() ) );
                }
                int j = k % 3;
                int i = (k - j) / 3;
                tripletList.push_back( T(k,k, -lambda[i]) ); // Correction to the diagonal
            }
            hessian_out.setFromTriplets(tripletList.begin(), tripletList.end());
        }

        // NOTE WE ASSUME A SELFADJOINT MATRIX
        void Sparse_Eigen_Decomposition(const SpMatrixX & matrix, VectorX & evalues, MatrixX & evectors)
        {
            // Create a Spectra solver
            Eigen::SelfAdjointEigenSolver<SpMatrixX> matrix_solver(matrix);
            evalues = matrix_solver.eigenvalues().real();
            evectors = matrix_solver.eigenvectors().real();
        }

        void Sparse_Geodesic_Eigen_Decomposition(const vectorfield & image, const vectorfield & gradient, const SpMatrixX & hessian,
            SpMatrixX & hessian_geodesic_3N, SpMatrixX & hessian_geodesic_2N, SpMatrixX & tangent_basis, VectorX & eigenvalues, MatrixX & eigenvectors)
        {
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "---------- Sparse Geodesic Eigen Decomposition");

            int nos = image.size();

            // Calculate geodesic Hessian in 3N-representation
            hessian_geodesic_3N = SpMatrixX(3*nos, 3*nos);
            sparse_hessian_bordered_3N(image, gradient, hessian, hessian_geodesic_3N);

            // Transform into geodesic Hessian
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Transforming Hessian into geodesic Hessian...");
            hessian_geodesic_2N = SpMatrixX(2*nos, 2*nos);
            hessian_geodesic_2N = tangent_basis.transpose() * hessian_geodesic_3N * tangent_basis;

            // Calculate full eigenspectrum
            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "    Calculation of full eigenspectrum..." );

            eigenvalues = VectorX::Zero(2*nos);
            eigenvectors = MatrixX::Zero(2*nos, 2*nos);

            Sparse_Eigen_Decomposition(hessian_geodesic_2N, eigenvalues, eigenvectors);

            Log(Utility::Log_Level::Info, Utility::Log_Sender::HTST, "---------- Sparse Geodesic Eigen Decomposition Done");
        }

    }
}
#endif


// std includes
#include <vector>
#include <iostream>
#include <random> // random_device, default_random_engine, uniform_real_distribution
#include <memory> // shared_ptr
#include <cmath> // sin, cos
// thirdparty includes
#include <Eigen/Dense>
// lib includes
#include "m0sh/uniform.h"
#include "m0sh/structured_sub.h"
#include "p0l/interpolation.h"
#include "v0l/bin/file_data.h"

using TypeScalar = double;
// Space
const unsigned int DIM = 3;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
// Mesh
template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMeshStructured = m0sh::Structured<TypeVector, TypeRef, TypeContainer>;
using TypeMeshStructuredSub = m0sh::StructuredSub<TypeVector, TypeRef, TypeContainer>;
using TypeMeshStructuredUniform = m0sh::Uniform<TypeVector, TypeRef, TypeContainer>;
// Data
const std::size_t order = 4; // interpolation order

double f(const double x, const double y) {
    return std::cos(x) + std::sin(y);
}

int main() {
    // Init
    v0l::FileData<float> data("../data/v.vtk", 0);
    // build length
    std::vector<double> lengths(data.meta.spacing.size());
    TypeVector origin;
    for(unsigned int i = 0; i < lengths.size(); i++) {
        lengths[i] = data.meta.spacing[i] * data.meta.dimensions[i];
        origin[i] = data.meta.origin[i];
    }
    // build create mesh
    std::shared_ptr<TypeMeshStructured> sMesh = std::make_shared<TypeMeshStructuredUniform>(data.meta.dimensions, lengths, origin, TypeContainer<bool>(DIM, true));
    // interpolate
    std::cout << "interpolated value: " << p0l::lagrangeMeshCell<TypeMeshStructured, v0l::FileData, float, TypeVector, TypeRef, TypeMeshStructuredSub>(sMesh, data, TypeVector::Random() * 0.5, order + 1) << std::endl;
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/range.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <deque>
#include <iostream>
#include <list>
#include <string>
#include <type_traits>
#include <vector>

// Couldn't find the actual example used in the text,
// but the one from chapter 1 will do just fine it seems :-).
// Mostly code serrouding the dfs visitor, which is a neat idea itself,
// and a large part of chapter 3 of the boost::graph book.

using graph_type = boost::adjacency_list<boost::listS, boost::vecS, boost::directedS>;

typedef boost::graph_traits<graph_type>::vertex_descriptor vertex_t;
typedef boost::graph_traits<graph_type>::edge_descriptor edge_t;
// white means undiscovered, gray means discovered but still searching descendants, and black
// means the vertex and all of its descendants have been discovered.
enum class color_type { white,
    gray,
    black };

bool has_cycle_dfs(
    const graph_type& g,
    vertex_t u,
    color_type* color)
{
    color[u] = color_type::gray;
    boost::graph_traits<graph_type>::adjacency_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = boost::adjacent_vertices(u, g); vi != vi_end; ++vi) {
        if (color[*vi] == color_type::white) {
            if (has_cycle_dfs(g, *vi, color)) {
                return true;
            } else if (color[*vi] == color_type::gray) {
                return true;
            }
        }
    }
    color[u] = color_type::black;
    return false;
}

bool has_cycle(const graph_type& g)
{
    std::vector<color_type> color(
        boost::num_vertices(g),
        color_type::white);

    boost::graph_traits<graph_type>::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
        if (color[*vi] == color_type::white) {
            if (has_cycle_dfs(g, *vi, &color[0])) {
                return true;
            }
        }
    }

    return false;
}

template <typename Visitor>
void dfs_v1(const graph_type& g, vertex_t u, color_type* color, Visitor vis)
{
    color[u] = color_type::gray;
    vis.discover_vertex(u, g);

    boost::graph_traits<graph_type>::out_edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ++ei) {
        // boost::target/ boost::source -> get start/end edge
        if (color[boost::target(*ei, g)] == color_type::white) {
            vis.tree_edge(*ei, g);
            dfs_v1(g, target(*ei, g), color, vis);
        } else if (color[target(*ei, g)] == color_type::gray) {
            vis.back_edge(*ei, g);
        } else {
            vis.forward_or_cross_edge(*ei, g);
        }
    }

    color[u] = color_type::black;
    vis.finish_vertex(u, g);
}

template <typename Visitor>
void generic_dfs_v1(const graph_type& g, Visitor vis)
{
    std::vector<color_type> color(num_vertices(g), color_type::white);
    boost::graph_traits<graph_type>::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
        if (color[*vi] == color_type::white) {
            dfs_v1(g, *vi, &color[0], vis);
        }
    }
}

struct default_dfs_visitor {
    template <typename V, typename G>
    void discover_vertex(V, const G&) { }

    template <typename E, typename G>
    void tree_edge(E, const G&) { }

    template <typename E, typename G>
    void back_edge(E, const G&) { }

    template <typename E, typename G>
    void forward_or_cross_edge(E, const G&) { }

    template <typename V, typename G>
    void finish_vertex(V, const G&) { }
};

struct topo_visitor : public default_dfs_visitor {
    topo_visitor(vertex_t*& order)
        : topo_order(order)
    {
    }

    void finish_vertex(vertex_t u, const graph_type&)
    {
        *--topo_order = u;
    }

    vertex_t*& topo_order;
};

void topo_sort(const graph_type& g, vertex_t* topo_order)
{
    topo_visitor vis(topo_order);
    generic_dfs_v1(g, vis);
}

struct cycle_detector : public default_dfs_visitor{
    cycle_detector(bool& cycle) : has_cycle(cycle) {}
    void back_edge (edge_t, const graph_type&)
    {
        has_cycle = true;
    }

    bool& has_cycle;
};

bool has_cycle_vis(const graph_type& g)
{
    bool has_cycle = true;
    cycle_detector vis(has_cycle);
    generic_dfs_v1(g, vis);
    return has_cycle;
}

int main()
{
    const char* tasks[] = {
        "pick up kids from school",
        "buy groceries (and snack)",
        "get cash at ATM",
        "drop off kids at soccer practice",
        "cook dinner",
        "pick up kids from soccer",
        "eat dinner"
    };

    const int n_tasks = sizeof(tasks) / sizeof(char*);

    graph_type g(n_tasks);
    add_edge(0, 3, g);
    add_edge(1, 3, g);
    add_edge(1, 4, g);
    add_edge(2, 1, g);
    add_edge(3, 5, g);
    add_edge(4, 6, g);
    add_edge(5, 6, g);

    std::cout << "cycle detection: " << (has_cycle(g) ? "true" : "false") << std::endl;
    //std::deque<int> topo_order;

    //topological_sort(g, std::front_inserter(topo_order),
    //        vertex_index_map(identity_property_map()));

    //for(const auto& [index, i] : topo_order
    //        | boost::adaptors::indexed())
    //{
    //    std::cout << tasks[i.head] << std::endl;
    //}

    return 0;
}


//  Copyright (c) 2014 Anton Bikineev
//  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)
//
//  Computes test data for the derivatives of the
//  various bessel functions. v and x parameters are taken
//  from bessel_*_data.ipp files. Results of derivatives
//  are generated by the relations between the derivatives
//  and Bessel functions, which actual implementation
//  doesn't use. Results are printed to ~ 50 digits.
//
#include <fstream>
#include <utility>
#include <map>
#include <iterator>
#include <algorithm>

#include <boost/multiprecision/mpfr.hpp>

#include <boost/math/special_functions/bessel.hpp>

template <class T>
T bessel_j_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_j(v, x) - boost::math::cyl_bessel_j(v+1, x);
}

template <class T>
T bessel_y_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_neumann(v, x) - boost::math::cyl_neumann(v+1, x);
}

template <class T>
T bessel_i_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_i(v, x) + boost::math::cyl_bessel_i(v+1, x);
}

template <class T>
T bessel_k_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_k(v, x) - boost::math::cyl_bessel_k(v+1, x);
}

template <class T>
T sph_bessel_j_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_bessel(1, x);
   return boost::math::sph_bessel(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_bessel(itrunc(v), x);
}

template <class T>
T sph_bessel_y_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_neumann(1, x);
   return boost::math::sph_neumann(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_neumann(itrunc(v), x);
}

using FloatType = boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<200u> >;

enum class BesselFamily: char
{
   J = 0,
   Y,
   I,
   K,
   j,
   y
};

namespace
{

const unsigned kSignificand = 50u;

const std::map<BesselFamily, std::vector<std::string> > kSourceFiles = {
   {BesselFamily::J, {"bessel_j_data.ipp", "bessel_j_int_data.ipp", "bessel_j_large_data.ipp"}},
   {BesselFamily::Y, {"bessel_y01_data.ipp", "bessel_yn_data.ipp", "bessel_yv_data.ipp"}},
   {BesselFamily::I, {"bessel_i_data.ipp", "bessel_i_int_data.ipp"}},
   {BesselFamily::K, {"bessel_k_data.ipp", "bessel_k_int_data.ipp"}},
   {BesselFamily::j, {"sph_bessel_data.ipp"}},
   {BesselFamily::y, {"sph_neumann_data.ipp"}}
};

FloatType (*fp)(FloatType, FloatType) = ::bessel_j_derivative_bare;

std::string parseValue(std::string::iterator& iter)
{
   using std::isdigit;

   auto value = std::string{};

   while (!isdigit(*iter) && *iter != '-')
      ++iter;
   while (isdigit(*iter) || *iter == '.' || *iter == 'e' || *iter == '-' || *iter == '+')
   {
      value.push_back(*iter);
      ++iter;
   }
   return value;
}

void replaceResultInLine(std::string& line)
{
   using std::isdigit;

   auto iter = line.begin();

   // parse v and x values from line and convert them to FloatType
   auto v = FloatType{::parseValue(iter)};
   auto x = FloatType{::parseValue(iter)};
   auto result = fp(v, x).str(kSignificand);

   while (!isdigit(*iter) && *iter != '-')
      ++iter;
   const auto where_to_write = iter;
   while (isdigit(*iter) || *iter == '.' || *iter == 'e' || *iter == '-' || *iter == '+')
      line.erase(iter);

   line.insert(where_to_write, result.begin(), result.end());
}

void generateResultFile(const std::string& i_file, const std::string& o_file)
{
   std::ifstream in{i_file.c_str()};
   std::ofstream out{o_file.c_str()};

   auto line = std::string{};
   while (!in.eof())
   {
      std::getline(in, line);
      if (__builtin_expect(line.find("SC_") != std::string::npos, 1))
         ::replaceResultInLine(line);
      out << line << std::endl;
   }
}

void processFiles(BesselFamily family)
{
   const auto& family_files = kSourceFiles.find(family)->second;

   std::for_each(std::begin(family_files), std::end(family_files),
      [&](const std::string& src){
         auto new_file = src;

         const auto int_pos = new_file.find("int");
         const auto large_pos = new_file.find("large");
         const auto data_pos = new_file.find("data");
         const auto derivative_pos = (int_pos == std::string::npos ?
            (large_pos == std::string::npos ? data_pos : large_pos) :
            int_pos);

         new_file.insert(derivative_pos, "derivative_");

         ::generateResultFile(src, new_file);
      });
}

} // namespace

int main(int argc, char*argv [])
{
   auto functype = BesselFamily::J;
   auto letter = std::string{"J"};

   if(argc == 2)
   {
      if(std::strcmp(argv[1], "--Y") == 0)
      {
         functype = BesselFamily::Y;
         fp = ::bessel_y_derivative_bare;
         letter = "Y";
      }
      else if(std::strcmp(argv[1], "--I") == 0)
      {
         functype = BesselFamily::I;
         fp = ::bessel_i_derivative_bare;
         letter = "I";
      }
      else if(std::strcmp(argv[1], "--K") == 0)
      {
         functype = BesselFamily::K;
         fp = ::bessel_k_derivative_bare;
         letter = "K";
      }
      else if(std::strcmp(argv[1], "--j") == 0)
      {
         functype = BesselFamily::j;
         fp = ::sph_bessel_j_derivative_bare;
         letter = "j";
      }
      else if(std::strcmp(argv[1], "--y") == 0)
      {
         functype = BesselFamily::y;
         fp = ::sph_bessel_y_derivative_bare;
         letter = "y";
      }
      else
         assert(0);
   }

   ::processFiles(functype);

   return 0;
}


///////////////////////////////////////////////////////////////////////////////
// accumulator::statistics::variance_of_mean_normalized.hpp                  //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_STATISTICS_VARIANCE_OF_MEAN_NORMALIZED_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_STATISTICS_VARIANCE_OF_MEAN_NORMALIZED_HPP_ER_2009
#include <boost/parameter/binding.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace accumulator{

namespace impl
{

    // Var(w/c) = Var(w) / c^2, where c = mean(w)
    template<typename T>
    class variance_of_mean_normalized
            : public boost::accumulators::accumulator_base{
        typedef boost::accumulators::dont_care dont_care_;

        typedef boost::accumulators::tag::variance tag_v_;
        typedef boost::accumulators::tag::mean tag_m_;
        typedef boost::accumulators::tag::accumulator tag_acc_;

        public:
        typedef T result_type;
        variance_of_mean_normalized(){}
        variance_of_mean_normalized(dont_care_){}
        void operator()(dont_care_)const{}

        template<typename Args>
        result_type result(const Args& args) const
        {
            typedef
                typename boost::parameter::binding<Args,tag_acc_>::type cref_;
            cref_ acc = args[boost::accumulators::accumulator];
            T v = accumulators::extract_result<tag_v_>(acc);
            T c = accumulators::extract_result<tag_m_>(acc);
            return v / (c*c);
        }
    };

}//impl

///////////////////////////////////////////////////////////////////////////////
// tag::variance_of_mean_normalized
namespace tag
{
    struct variance_of_mean_normalized
      : boost::accumulators::depends_on<
        boost::accumulators::tag::mean,
        boost::accumulators::tag::variance
    >
    {
      typedef statistics::detail::accumulator::
      	impl::variance_of_mean_normalized<boost::mpl::_1> impl;

    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::variance_of_mean_normalized
namespace extract
{

  template<typename AccSet>
  typename
    boost::mpl::apply<
        AccSet,
        tag::variance_of_mean_normalized
    >::type::result_type
    variance_of_mean_normalized(AccSet const& acc){
        typedef tag::variance_of_mean_normalized the_tag;
        return boost::accumulators::extract_result<the_tag>(acc);
    }

}

using extract::variance_of_mean_normalized;

}// accumulator
}// detail
}// statistics
}// boost

#endif


/*
 * point_kinematics.hpp
 *
 * Created on: Oct 24, 2018 23:05
 * Description: (reduced) point kinematics model, used for lattice generation
 *
 * Reference:
 *  [1] M. McNaughton and C. Urmson and J. M. Dolan and J. W. Lee. 2011.
 *    “Motion Planning for Autonomous Driving with a Conformal Spatiotemporal
 * Lattice.” In 2011 IEEE International Conference on Robotics and Automation,
 * 4889–95.
 *
 * Copyright (c) 2018 Ruixiang Du (rdu)
 */

#ifndef POINT_KINEMATICS_HPP
#define POINT_KINEMATICS_HPP

#include <iostream>

#include <eigen3/Eigen/Dense>

#include <boost/numeric/odeint.hpp>

#include "state_lattice/details/motion_state.hpp"

namespace robosw {
class PointKinematics {
 public:
  struct Param {
    Param(double _p0 = 0, double _p1 = 0, double _p2 = 0, double _p3 = 0,
          double _sf = 0)
        : p0(_p0), p1(_p1), p2(_p2), p3(_p3), sf(_sf) {}

    double p0;
    double p1;
    double p2;
    double p3;
    double sf;

    friend std::ostream &operator<<(std::ostream &os, const Param &p) {
      os << "(p0,p1,p2,p3,sf): " << p.p0 << " , " << p.p1 << " , " << p.p2
         << " , " << p.p3 << " , " << p.sf;
      return os;
    }
  };

  using state_type = std::vector<double>;

  /////////////////////////////////////////////////////

 public:
  PointKinematics() = default;
  PointKinematics(double a, double b, double c, double d)
      : a_(a), b_(b), c_(c), d_(d){};

  // calculate intermediate parameters
  void SetParameters(const Param &p);

  // propagate system model
  MotionState Propagate(const MotionState &init, const Param &p,
                        double ds = 0.1);
  StatePMatrix PropagateP(const StatePMatrix &init, const Param &p,
                          double ds = 0.1);

  // The following functions could be called externally ONLY when a_,b_,c_,d_
  // have been set properly by
  //  (1) construct the model with: PointKinematics(double a, double b, double
  //  c, double d) (2) default construct and then use SetParameters() to set
  //  parameters
  inline MotionState Propagate(const MotionState &init, double sf, double ds) {
    {
      double sf_squared = sf * sf;

      // theta_p and kappa_p could be calculated analytically
      double theta_p = a_ * sf + b_ * sf_squared / 2.0 +
                       c_ * sf_squared * sf / 3.0 +
                       d_ * sf_squared * sf_squared / 4.0;
      double kappa_p = a_ + b_ * sf + c_ * sf_squared + d_ * sf_squared * sf;

      // calculate x_p, y_p numerically
      double s = 0;
      state_type x = {init.x, init.y};
      while (s <= sf) {
        // integrator_(*this, x, s, ds);
        boost::numeric::odeint::integrate_const(
            boost::numeric::odeint::runge_kutta4<state_type>(), *this, x, s,
            s + ds, ds / 10.0);
      }

      return MotionState(x[0], x[1], theta_p, kappa_p);
    }
  }

  std::vector<MotionState> GenerateTrajectoryPoints(const MotionState &init,
                                                    double sf, double step,
                                                    double ds = 0.1);

  // Shall not be called by user, used for propagation by RK4 integrator class
  void operator()(const state_type &x, state_type &xd, const double s);

 private:
  double a_;
  double b_;
  double c_;
  double d_;

  inline void CalculateIntermediateParams(const Param &p);
};
}  // namespace robosw

#endif /* POINT_KINEMATICS_HPP */


#include <state_estimation/filters/ekf.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

void EKF::myPredict(const Eigen::VectorXd& u, double dt) {
    system_model_->update(filter_state_.x, u, dt);

    // Update the state and covariance
    filter_state_.x = system_model_->g();
    filter_state_.covariance =
        system_model_->G() * filter_state_.covariance * system_model_->G().transpose() +
        system_model_->P() * system_model_->Rp() * system_model_->P().transpose() +
        system_model_->V() * system_model_->Rc() * system_model_->V().transpose();

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "EKF predicition update:" << std::endl
              << "g=" << printMatrix(system_model_->g()) << std::endl
              << "G=" << std::endl
              << printMatrix(system_model_->G()) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void EKF::myCorrect(const Eigen::VectorXd& z, measurement_models::NonlinearMeasurementModel* model,
                    double dt) {
    // Update our measurement model
    model->update(filter_state_.x, dt);

    // Compute the Kalman gain
    const Eigen::MatrixXd cov_H_T = filter_state_.covariance * model->H().transpose();
    const Eigen::MatrixXd K = cov_H_T * (model->H() * cov_H_T + model->covariance()).inverse();

    // Update the state and covariance with the measurement
    const Eigen::MatrixXd I =
        Eigen::MatrixXd::Identity(filter_state_.x.rows(), filter_state_.x.rows());
    const Eigen::VectorXd dx = K * model->subtractVectors(z, model->h());
    filter_state_.x = system_model_->addVectors(filter_state_.x, dx);
    filter_state_.covariance = (I - K * model->H()) * filter_state_.covariance;

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "EKF measurement update:" << std::endl
              << "h=" << printMatrix(model->h()) << std::endl
              << "H=" << std::endl
              << printMatrix(model->H()) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "K=" << std::endl
              << printMatrix(K) << std::endl
              << "Innovation=" << printMatrix(dx) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation