#ifndef STAN_MATH_PRIM_MAT_PROB_BERNOULLI_LOGIT_GLM_LPMF_HPP
#define STAN_MATH_PRIM_MAT_PROB_BERNOULLI_LOGIT_GLM_LPMF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Returns the log PMF of the Generalized Linear Model (GLM)
 * with Bernoulli distribution and logit link function.
 * If containers are supplied, returns the log sum of the probabilities.
 * @tparam T_n type of binary vector of dependent variables (labels);
 * this can also be a single binary value;
 * @tparam T_x type of the matrix of independent variables (features); this
 * should be an Eigen::Matrix type whose number of rows should match the
 * length of n and whose number of columns should match the length of beta
 * @tparam T_beta type of the weight vector;
 * this can also be a single value;
 * @tparam T_alpha type of the intercept;
 * this has to be single value;
 * @param n binary vector parameter
 * @param x design matrix
 * @param beta weight vector
 * @param alpha intercept (in log odds)
 * @return log probability or log sum of probabilities
 * @throw std::domain_error if x, beta or alpha is infinite.
 * @throw std::domain_error if n is not binary.
 * @throw std::invalid_argument if container sizes mismatch.
 */
template <bool propto, typename T_n, typename T_x, typename T_beta,
          typename T_alpha>
typename return_type<T_x, T_beta, T_alpha>::type bernoulli_logit_glm_lpmf(
    const T_n &n, const T_x &x, const T_beta &beta, const T_alpha &alpha) {
  static const char *function = "bernoulli_logit_glm_lpmf";
  typedef typename stan::partials_return_type<T_n, T_x, T_beta, T_alpha>::type
      T_partials_return;

  using Eigen::Dynamic;
  using Eigen::Matrix;
  using std::exp;

  if (size_zero(n, x, beta))
    return 0.0;

  T_partials_return logp(0.0);

  check_bounded(function, "Vector of dependent variables", n, 0, 1);
  check_finite(function, "Matrix of independent variables", x);
  check_finite(function, "Weight vector", beta);
  check_finite(function, "Intercept", alpha);
  check_consistent_sizes(function, "Rows in matrix of independent variables",
                         x.col(0), "Vector of dependent variables", n);
  check_consistent_sizes(function, "Columns in matrix of independent variables",
                         x.row(0), "Weight vector", beta);

  if (!include_summand<propto, T_x, T_beta, T_alpha>::value)
    return 0.0;

  const size_t N = x.col(0).size();
  const size_t M = x.row(0).size();

  Matrix<T_partials_return, Dynamic, 1> signs(N, 1);
  {
    scalar_seq_view<T_n> n_vec(n);
    for (size_t n = 0; n < N; ++n) {
      signs[n] = 2 * n_vec[n] - 1;
    }
  }
  Matrix<T_partials_return, Dynamic, 1> beta_dbl(M, 1);
  {
    scalar_seq_view<T_beta> beta_vec(beta);
    for (size_t m = 0; m < M; ++m) {
      beta_dbl[m] = value_of(beta_vec[m]);
    }
  }
  Matrix<T_partials_return, Dynamic, Dynamic> x_dbl = value_of(x);

  Eigen::Array<T_partials_return, Dynamic, 1> ntheta
      = signs.array()
        * (x_dbl * beta_dbl
           + Matrix<double, Dynamic, 1>::Ones(N, 1) * value_of(alpha))
              .array();
  Eigen::Array<T_partials_return, Dynamic, 1> exp_m_ntheta = (-ntheta).exp();

  static const double cutoff = 20.0;
  for (size_t n = 0; n < N; ++n) {
    // Compute the log-density and handle extreme values gracefully
    // using Taylor approximations.
    if (ntheta[n] > cutoff)
      logp -= exp_m_ntheta[n];
    else if (ntheta[n] < -cutoff)
      logp += ntheta[n];
    else
      logp -= log1p(exp_m_ntheta[n]);
  }

  // Compute the necessary derivatives.
  operands_and_partials<T_x, T_beta, T_alpha> ops_partials(x, beta, alpha);
  if (!(is_constant_struct<T_x>::value && is_constant_struct<T_beta>::value
        && is_constant_struct<T_alpha>::value)) {
    Matrix<T_partials_return, Dynamic, 1> theta_derivative(N, 1);
    for (size_t n = 0; n < N; ++n) {
      if (ntheta[n] > cutoff)
        theta_derivative[n] = -exp_m_ntheta[n];
      else if (ntheta[n] < -cutoff)
        theta_derivative[n] = signs[n];
      else
        theta_derivative[n]
            = signs[n] * exp_m_ntheta[n] / (exp_m_ntheta[n] + 1);
    }
    if (!is_constant_struct<T_beta>::value) {
      ops_partials.edge2_.partials_ = x_dbl.transpose() * theta_derivative;
    }
    if (!is_constant_struct<T_x>::value) {
      ops_partials.edge1_.partials_ = theta_derivative * beta_dbl.transpose();
    }
    if (!is_constant_struct<T_alpha>::value) {
      ops_partials.edge3_.partials_[0] = theta_derivative.trace();
    }
  }

  return ops_partials.build(logp);
}

template <typename T_n, typename T_x, typename T_beta, typename T_alpha>
inline typename return_type<T_x, T_beta, T_alpha>::type
bernoulli_logit_glm_lpmf(const T_n &n, const T_x &x, const T_beta &beta,
                         const T_alpha &alpha) {
  return bernoulli_logit_glm_lpmf<false>(n, x, beta, alpha);
}
}  // namespace math
}  // namespace stan
#endif


/*
 * Author: Konstantin Schauwecker
 * Year:   2012
 */

// This is a minimalistic example on how to use the extended
// FAST feature detector and the sparse stereo matcher.

#include <opencv2/opencv.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <vector>
#include <iostream>
#include <sparsestereo/exception.h>
#include <sparsestereo/extendedfast.h>
#include <sparsestereo/stereorectification.h>
#include <sparsestereo/sparsestereo-inl.h>
#include <sparsestereo/census-inl.h>
#include <sparsestereo/imageconversion.h>
#include <sparsestereo/censuswindow.h>

using namespace std;
using namespace cv;
using namespace sparsestereo;
using namespace boost;
using namespace boost::posix_time;

int main(int argc, char** argv) {
	try {
		// Stereo matching parameters
		double uniqueness = 0.7;
		int maxDisp = 70;
		int leftRightStep = 2;

		// Feature detection parameters
		double adaptivity = 1.0;
		int minThreshold = 10;

		// Parse arguments
		if(argc != 3 && argc != 4) {
			cout << "Usage: " << argv[0] << " LEFT-IMG RIGHT-IMG [CALIBRARION-FILE]" << endl;
			return 1;
		}
		char* leftFile = argv[1];
		char* rightFile = argv[2];
		char* calibFile = argc == 4 ? argv[3] : NULL;

		// Read input images
		cv::Mat_<unsigned char> leftImg, rightImg;
		leftImg = imread(leftFile, CV_LOAD_IMAGE_GRAYSCALE);
		rightImg = imread(rightFile, CV_LOAD_IMAGE_GRAYSCALE);

		if(leftImg.data == NULL || rightImg.data == NULL)
			throw sparsestereo::Exception("Unable to open input images!");

		// Load rectification data
		StereoRectification* rectification = NULL;
		if(calibFile != NULL)
			rectification = new StereoRectification(CalibrationResult(calibFile));

		// The stereo matcher. SSE Optimized implementation is only available for a 5x5 window
		SparseStereo<CensusWindow<5>, short> stereo(maxDisp, 1, uniqueness,
			rectification, false, false, leftRightStep);

		// Feature detectors for left and right image
		FeatureDetector* leftFeatureDetector = new ExtendedFAST(true, minThreshold, adaptivity, false, 2);
		FeatureDetector* rightFeatureDetector = new ExtendedFAST(false, minThreshold, adaptivity, false, 2);

		ptime lastTime = microsec_clock::local_time();
		vector<SparseMatch> correspondences;

		// Objects for storing final and intermediate results
		cv::Mat_<char> charLeft(leftImg.rows, leftImg.cols),
			charRight(rightImg.rows, rightImg.cols);
		Mat_<unsigned int> censusLeft(leftImg.rows, leftImg.cols),
			censusRight(rightImg.rows, rightImg.cols);
		vector<KeyPoint> keypointsLeft, keypointsRight;

		// For performance evaluation we do the stereo matching 100 times
		for(int i=0; i< 100; i++) {
			// Featuredetection. This part can be parallelized with OMP
			#pragma omp parallel sections default(shared) num_threads(2)
			{
				#pragma omp section
				{
					ImageConversion::unsignedToSigned(leftImg, &charLeft);
					Census::transform5x5(charLeft, &censusLeft);
					keypointsLeft.clear();
					leftFeatureDetector->detect(leftImg, keypointsLeft);
				}
				#pragma omp section
				{
					ImageConversion::unsignedToSigned(rightImg, &charRight);
					Census::transform5x5(charRight, &censusRight);
					keypointsRight.clear();
					rightFeatureDetector->detect(rightImg, keypointsRight);
				}
			}

			// Stereo matching. Not parallelized (overhead too large)
			correspondences.clear();
			stereo.match(censusLeft, censusRight, keypointsLeft, keypointsRight, &correspondences);
		}

		// Print statistics
		time_duration elapsed = (microsec_clock::local_time() - lastTime);
		cout << "Time for 100x stereo matching: " << elapsed.total_microseconds()/1.0e6 << "s" << endl
			<< "Features detected in left image: " << keypointsLeft.size() << endl
			<< "Features detected in right image: " << keypointsRight.size() << endl
			<< "Percentage of matched features: " << (100.0 * correspondences.size() / keypointsLeft.size()) << "%" << endl;

		// Highlight matches as colored boxes
		Mat_<Vec3b> screen(leftImg.rows, leftImg.cols);
		cvtColor(leftImg, screen, CV_GRAY2BGR);

		for(int i=0; i<(int)correspondences.size(); i++) {
			double scaledDisp = (double)correspondences[i].disparity() / maxDisp;
			Vec3b color;
			if(scaledDisp > 0.5)
				color = Vec3b(0, (1 - scaledDisp)*512, 255);
			else color = Vec3b(0, 255, scaledDisp*512);

			rectangle(screen, correspondences[i].imgLeft->pt - Point2f(2,2),
				correspondences[i].imgLeft->pt + Point2f(2, 2),
				(Scalar) color, CV_FILLED);
		}

		// Display image and wait
		namedWindow("Stereo");
		imshow("Stereo", screen);
		waitKey();

		// Clean up
		delete leftFeatureDetector;
		delete rightFeatureDetector;
		if(rectification != NULL)
			delete rectification;

		return 0;
	}
	catch (const std::exception& e) {
		cerr << "Fatal exception: " << e.what();
		return 1;
	}
}


#ifndef YQVMC_NLOHMANN_JSON_EIGEN3_ADAPTOR_HPP
#define YQVMC_NLOHMANN_JSON_EIGEN3_ADAPTOR_HPP

#include <Eigen/Dense>

namespace nlohmann {
    //template <typename Derived>
    //struct adl_serializer<EigenBase<Derived>> {
    //};
    template <>
    struct adl_serializer<Eigen::ArrayXd> {
      static void to_json(json& j, const Eigen::ArrayXd& vec) {
        for (unsigned i = 0; i < vec.size(); i++) {
          j[i] = vec[i];
        }
      }

      static void from_json(const json& j, Eigen::ArrayXd& vec) {
        vec.resize(j.size());
        for (unsigned i = 0; i < j.size(); i++) {
          vec[i] = j[i];
        }
      }
    };

    template <>
    struct adl_serializer<Eigen::MatrixXd> {

      static void to_json(json& js, const Eigen::MatrixXd& mat) {
        for (unsigned i = 0; i < mat.rows(); i++) {
          js[i] = json();
          for (unsigned j = 0; j < mat.cols(); j++) {
            js[i][j] = mat(i, j);
          }
        }
      }

      //static void from_json(const json& j, Eigen::ArrayXXd& vec) {
      //}

    };
}

#endif


// rk.hpp
#ifndef RK_H_
#define RK_H_
/* ------------------------------------------------------------------------- */
// Includes
/* ------------------------------------------------------------------------- */
#include <vector>
#include <boost/python.hpp>
#include "tableau.hpp"
/* ------------------------------------------------------------------------- */

namespace py = boost::python;

/* ------------------------------------------------------------------------- */
// Type Definitions
/* ------------------------------------------------------------------------- */
typedef std::vector< double > vec1D;
typedef std::vector< std::vector< double > > vec2D;
/* ------------------------------------------------------------------------- */

class RKIntegrator {
	private:
		double t_;
		double tf_;
		double h_;
		double x_;
		int stages_;
		int steps_;
		bool finished_;
		bool using_vector_;
		bool explicit_t_dependence_;
		vec1D k_vec_;
		py::object func_;

		vec1D x_vec_;
		vec1D dx_vec_;

		vec2D xVec2D_;
		vec2D dxVec2D_;

		vec1D t_vec_;

	public:
		Tableau bt_;
		/* ----------------------------------------------------------------- */
		// Constructors
		/* ----------------------------------------------------------------- */
		RKIntegrator();
		RKIntegrator(
				py::object&, py::list&, double, double,
				double, double, bool);
		RKIntegrator(
				py::object&, py::list&, double, double,
				double, bool);
		RKIntegrator(const RKIntegrator& rkArg);
		~RKIntegrator();
		/* ----------------------------------------------------------------- */
		/*
		void RK(double (*f)(double, double), ButcherTableau&, double, double,
			double, double);
		*/

		void stepWrap();
		void stepAugWrap(double x);
		double step(double);
		//std::vector< double > vecStep(py::list&);
		py::list vecStep(py::list&);
		double run();
		bool isFinished();

		void setTimeStep(double);
		double getTimeStep();

		void initkVecTo0();

		//std::vector< double > get_xVec();
		//std::vector< double > get_dxVec();
		//std::vector< double > get_tVec();

		//std::vector< std::vector< double > > get_xVec2D();
		//std::vector< std::vector< double > > get_dxVec2D();

		py::list get_xVec();
		py::list get_dxVec();
		py::list get_tVec();
		py::list get_xVec2D();
		py::list get_dxVec2D();

		double get_last(int);

		py::list get_lastVec(int);
};

#endif


/*
 * FibonacciHeap.hpp
 *
 *  Created on: 9 sty 2016
 *      Author: tomasz
 */

#ifndef SRC_INCLUDE_HEAP_FIBONACCIHEAP_HPP_
#define SRC_INCLUDE_HEAP_FIBONACCIHEAP_HPP_

#include <boost/heap/fibonacci_heap.hpp>
#include <boost/heap/policies.hpp>
#include <map>

#include "../typedefs/primitive.hpp"
#include "VertexHeapIF.hpp"
#include "VertexHeapItem.hpp"

struct compare_node {
	bool operator()(VertexHeapItem * const & vertexU,
			VertexHeapItem * const & vertexV) const {
		return vertexU->getKey() > vertexV->getKey();
	}
};

class FibonacciHeap: public VertexHeapIF {
private:

	//************************************ PRIVATE CONSTANT FIELDS *************************************//

	//************************************** PRIVATE CLASS FIELDS **************************************//

	boost::heap::fibonacci_heap<VertexHeapItem *,
			boost::heap::compare<compare_node>> heap;
	std::map<VertexIdx,
			boost::heap::fibonacci_heap<VertexHeapItem *,
					boost::heap::compare<compare_node>>::handle_type> heapMap;

	//*************************************** PRIVATE FUNCTIONS ****************************************//

protected:

	//*********************************** PROTECTED CONSTANT FIELDS ************************************//

	//************************************ PROTECTED CLASS FIELDS **************************************//

	//************************************** PROTECTED FUNCTIONS ***************************************//

public:

	//************************************* PUBLIC CONSTANT FIELDS *************************************//

	//************************************** PUBLIC CLASS FIELDS ***************************************//

	//************************************ CONSTRUCTOR & DESTRUCTOR ************************************//

	FibonacciHeap();

	virtual ~FibonacciHeap();

	//*************************************** PUBLIC FUNCTIONS *****************************************//

	VertexCount size() const;

	void push(VertexHeapItem * const vertex);

	VertexIF * pop();

	VertexHeapItem * peek();

	void increaseKey(VertexHeapItem * heapItem, VertexKey newKey);

	void decreaseKey(VertexHeapItem * heapItem, VertexKey newKey);

	VertexHeapItem * getItem(VertexIdx const vertexId);

	bool isVertexInsideHeap(VertexIF * const vertex);

	bool isVertexInsideHeap(VertexIdx const vertexId);

	//*************************************** GETTERS & SETTERS ****************************************//

};

#endif /* SRC_INCLUDE_HEAP_FIBONACCIHEAP_HPP_ */


#include <vector>
#include <queue>
#include <numeric>
#include <functional>
#include <chrono>

#include <omp.h>

#include <boost/range.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/graph/graph_utility.hpp>

using namespace boost;

struct Vertex {
    std::string name;
};

struct Edge {
    double weight;
};

struct Path {
    std::vector<int> vertices;
    std::vector<double> costs;

    double totalCost;

    Path() : totalCost{0.0} {}
};

// Define comparison operators for Path
bool operator>(const Path &p1, const Path &p2) {
    return (p1.totalCost > p2.totalCost);
}

bool operator<(const Path &p1, const Path &p2) {
    return (p1.totalCost < p2.totalCost);
}

typedef adjacency_list<vecS, vecS, directedS, Vertex, Edge> graph_t;

graph_t getGraph(const std::string &filename);

bool getStartAndEndNodeIndexes(const graph_t &graph, const std::string &startNode, const std::string &endNode,
                               int &startNodeIndex, int &endNodeIndex);

void findShortestPaths(const graph_t &graph, int startVertexIndex, int endVertexIndex, int pathCount,
                       std::vector<Path> &shortestPaths);

void findShortestPathsLoopless(const graph_t &graph, int startVertexIndex, int endVertexIndex, int pathCount,
                               std::vector<Path> &shortestPaths);

void printShortestPaths(const std::vector<Path> &shortestPaths, const graph_t &graph);

void
printPathQueue(std::priority_queue<Path, std::vector<Path>, std::greater<>> q, const graph_t &graph, int pathCount);

void findShortestPathsParallel(const graph_t &graph, int startVertexIndex, int endVertexIndex, int pathCount,
                               std::vector<Path> &shortestPaths);

void findShortestPathsHighIQ(const graph_t &graph, int startVertexIndex, int endVertexIndex, int pathCount,
                             std::priority_queue<Path, std::vector<Path>, std::greater<>> &accumulatedShortestPaths);

template<class T>
void fastInsert(std::deque<T> &q, T &&item);

int main(int argc, char *argv[]) {
    std::string filename, startNode, endNode;
    int startNodeIndex{};
    int endNodeIndex{};
    int pathCount;

    if (argc != 6) {
        std::cerr << "./gal19 graph start_node end_node path_count parallel_threads" << std::endl
                  << "\tgraph - path to graph file in .dot format" << std::endl
                  << "\tstart_node - identifier of start node" << std::endl
                  << "\tend_node - identifier of end node" << std::endl
                  << "\tpath_count - maximum number of shortest paths to find" << std::endl
                  << "\tparallel_threads - number of parallel threads to run (0 will run the reference sequential version)"
                  << std::endl;
        exit(EXIT_FAILURE);
    }

    graph_t graph;

    filename = argv[1];
    startNode = argv[2];
    endNode = argv[3];
    pathCount = std::stoi(argv[4]);
    auto threadNum = std::stoi(argv[5]);

    graph = getGraph(filename);
    if (!getStartAndEndNodeIndexes(graph, startNode, endNode, startNodeIndex, endNodeIndex)) {
        std::cerr << "Start or end node is not in graph" << std::endl;
        exit(EXIT_FAILURE);
    }

    if (threadNum > 0) {
        omp_set_num_threads(threadNum);

        std::priority_queue<Path, std::vector<Path>, std::greater<>> accumulatedShortestPaths;
        auto start = std::chrono::high_resolution_clock::now();
        findShortestPathsHighIQ(graph, startNodeIndex, endNodeIndex, pathCount, accumulatedShortestPaths);
        auto stop = std::chrono::high_resolution_clock::now();
        printPathQueue(accumulatedShortestPaths, graph, pathCount);
        std::cout << std::endl << "Runtime: "
                  << std::chrono::duration_cast<std::chrono::microseconds>(stop - start).count() << " usec"
                  << std::endl;
    } else {
        std::vector<Path> shortestPaths;
        auto start = std::chrono::high_resolution_clock::now();
        findShortestPaths(graph, startNodeIndex, endNodeIndex, pathCount, shortestPaths);
        auto stop = std::chrono::high_resolution_clock::now();
        printShortestPaths(shortestPaths, graph);
        std::cout << std::endl << "Runtime: "
                  << std::chrono::duration_cast<std::chrono::microseconds>(stop - start).count() << " usec"
                  << std::endl;
    }
}

graph_t getGraph(const std::string &filename) {
    // Construct an empty graph and prepare the dynamic_property_maps.
    graph_t graph(0);

    dynamic_properties dp;
    dp.property("node_id", get(&Vertex::name, graph));
    dp.property("weight", get(&Edge::weight, graph));

    std::ifstream dot(filename);

    if (!read_graphviz(dot, graph, dp)) {
        std::cerr << "Can't read input graph" << std::endl;
        exit(EXIT_FAILURE);
    }

    write_graphviz_dp(std::cout, graph, dp);

    return graph;
}

bool getStartAndEndNodeIndexes(const graph_t &graph, const std::string &startNode, const std::string &endNode,
                               int &startNodeIndex, int &endNodeIndex) {
    int index = 0;
    bool startNodeFound, endNodeFound;
    startNodeFound = endNodeFound = false;
    for (const auto &vertex : graph.m_vertices) {
        if (vertex.m_property.name == startNode) {
            startNodeIndex = index;
            startNodeFound = true;
        } else if (vertex.m_property.name == endNode) {
            endNodeIndex = index;
            endNodeFound = true;
        }
        index++;
        if (startNodeFound && endNodeFound) {
            break;
        }
    }

    return (startNodeFound && endNodeFound);
}

void findShortestPaths(const graph_t &graph, int startVertexIndex, int endVertexIndex, int pathCount,
                       std::vector<Path> &shortestPaths) {
    // Allocate shortest path priority queue
    std::priority_queue<Path, std::vector<Path>, std::greater<>> pathQueue;
    //std::deque<Path> pathQueue;
    // Allocate per-node path count vector
    std::vector<int> pathCounts(graph.m_vertices.size(), 0);

    shortestPaths.clear();

    // Insert initial path
    auto initialPath = Path{};
    initialPath.vertices.push_back(startVertexIndex);
    initialPath.costs.push_back(0.0);
    pathQueue.push(initialPath);
    //pathQueue.push_back(initialPath);

    while (!pathQueue.empty() && pathCounts[endVertexIndex] < pathCount) {
        Path currentShortestPath{pathQueue.top()};
        pathQueue.pop();
        //Path currentShortestPath{pathQueue[0]};
        //pathQueue.erase(pathQueue.begin());

        auto pathEndVertex = currentShortestPath.vertices.back();
        pathCounts[pathEndVertex]++;

        if (pathEndVertex == endVertexIndex) {
            shortestPaths.push_back(currentShortestPath);
        }

        if (pathCounts[pathEndVertex] <= pathCount) {
            for (const auto &edge : graph.out_edge_list(pathEndVertex)) {
                auto targetIndex = edge.m_target;
                auto newPath = Path(currentShortestPath);
                newPath.vertices.push_back(targetIndex);

                auto edgeCost = edge.get_property().weight;
                newPath.costs.push_back(edgeCost);
                newPath.totalCost += edgeCost;

                pathQueue.push(newPath);
                //pathQueue.push_back(newPath);
                //fastInsert(pathQueue, std::move(newPath));
            }
            //std::sort(pathQueue.begin(), pathQueue.end());
        }
    }
}

void findShortestPathsHighIQ(const graph_t &graph, int startVertexIndex, int endVertexIndex, int pathCount,
                             std::priority_queue<Path, std::vector<Path>, std::greater<>> &accumulatedShortestPaths) {
    auto threadCount = omp_get_max_threads();

    // Allocate shortest path priority queue
    std::priority_queue<Path, std::vector<Path>, std::greater<>> pathQueue;
    // Allocate per-node path count vector
    std::vector<int> pathCounts(graph.m_vertices.size(), 0);

    // Insert initial path
    auto initialPath = Path{};
    initialPath.vertices.push_back(startVertexIndex);
    initialPath.costs.push_back(0.0);
    pathQueue.push(initialPath);

    while (!pathQueue.empty() && pathCounts[endVertexIndex] < pathCount && pathQueue.size() < threadCount) {
        Path currentShortestPath{pathQueue.top()};
        pathQueue.pop();

        auto pathEndVertex = currentShortestPath.vertices.back();
        pathCounts[pathEndVertex]++;

        if (pathEndVertex == endVertexIndex) {
            accumulatedShortestPaths.push(currentShortestPath);
        }

        if (pathCounts[pathEndVertex] <= pathCount) {
            for (const auto &edge : graph.out_edge_list(pathEndVertex)) {
                auto targetIndex = edge.m_target;
                auto newPath = Path(currentShortestPath);
                newPath.vertices.push_back(targetIndex);

                auto edgeCost = edge.get_property().weight;
                newPath.costs.push_back(edgeCost);
                newPath.totalCost += edgeCost;

                pathQueue.push(newPath);
            }
        }
    }

    std::vector<double> threadLastPathCosts(threadCount);
    double sharedMaxCost = 0.0;
    bool stop = false;
#pragma omp parallel default(none) firstprivate(pathQueue, pathCounts) shared(stop, threadCount, sharedMaxCost, threadLastPathCosts, graph, startVertexIndex, endVertexIndex, pathCount, accumulatedShortestPaths)
    {
        auto threadId = omp_get_thread_num();
        auto itemCount = pathQueue.size() / threadCount;
        auto itemModulo = pathQueue.size() % threadCount;

        auto removeCount = itemCount * threadId + std::min<int>(threadId, itemModulo);

        for (int i = 0; i < removeCount; i++)
            pathQueue.pop();

        std::priority_queue<Path, std::vector<Path>, std::greater<>> localQueue;
        itemCount = threadId < itemModulo ? itemCount + 1 : itemCount;
        for (int i = 0; i < itemCount; i++) {
            localQueue.push(pathQueue.top());
            pathQueue.pop();
        }

        while (!localQueue.empty() && pathCounts[endVertexIndex] < pathCount && !stop) {
            Path currentShortestPath{localQueue.top()};
            localQueue.pop();

            auto pathEndVertex = currentShortestPath.vertices.back();
            pathCounts[pathEndVertex]++;

            if (pathEndVertex == endVertexIndex) {
#pragma omp critical
                {
                    threadLastPathCosts[threadId] = currentShortestPath.totalCost;
                    if (currentShortestPath.totalCost > sharedMaxCost)
                        sharedMaxCost = currentShortestPath.totalCost;
                    accumulatedShortestPaths.push(currentShortestPath);
                };
            }

            if (pathCounts[pathEndVertex] <= pathCount) {
                for (const auto &edge : graph.out_edge_list(pathEndVertex)) {
                    auto targetIndex = edge.m_target;
                    auto newPath = Path(currentShortestPath);
                    newPath.vertices.push_back(targetIndex);

                    auto edgeCost = edge.get_property().weight;
                    newPath.costs.push_back(edgeCost);
                    newPath.totalCost += edgeCost;

                    localQueue.push(newPath);
                }
            }

#pragma omp master
            {
                bool allCostsGreater = true;
                for (const auto &cost : threadLastPathCosts) {
                    if (cost < sharedMaxCost) {
                        allCostsGreater = false;
                        break;
                    }
                }

                if (allCostsGreater && accumulatedShortestPaths.size() >= pathCount)
                    stop = true;
            };
        }
    };
}

void printShortestPaths(const std::vector<Path> &shortestPaths, const graph_t &graph) {
    for (const auto &path : shortestPaths) {
        std::cout << "Cost: " << path.totalCost << "\tPath: ";
        int loopIndex = 0; // To check for last vertex in path (cannot check by value in case of loopy paths)
        for (const auto &vertexIndex : path.vertices) {
            std::cout << graph[vertexIndex].name;
            if (loopIndex != path.vertices.size() - 1)
                std::cout << " -> ";

            loopIndex++;
        }
        std::cout << std::endl;
    }
}

void
printPathQueue(std::priority_queue<Path, std::vector<Path>, std::greater<>> q, const graph_t &graph, int pathCount) {
    auto end = std::min<int>(q.size(), pathCount);
    for (int i = 0; i < end; i++) {
        auto path = q.top();
        q.pop();
        std::cout << "Cost: " << path.totalCost << "\tPath: ";
        int loopIndex = 0; // To check for last vertex in path (cannot check by value in case of loopy paths)
        for (const auto &vertexIndex : path.vertices) {
            std::cout << graph[vertexIndex].name;
            if (loopIndex != path.vertices.size() - 1)
                std::cout << " -> ";

            loopIndex++;
        }
        std::cout << std::endl;
    }
}

template<class T>
void fastInsert(std::deque<T> &q, T &&item) {
    for (auto it = q.rbegin(); it != q.rend(); it++) {
        if (item > *it) {
            q.insert(it.base(), item);
            return;
        }
    }

    q.push_back(item);
}


#include <NTL/lzz_p.h>

#include <NTL/new.h>

NTL_START_IMPL

SmartPtr<zz_pInfoT> Build_zz_pInfo(FFTPrimeInfo *info)
{
   return MakeSmart<zz_pInfoT>(INIT_FFT, info);
}

zz_pInfoT::zz_pInfoT(long NewP, long maxroot)
{
   if (maxroot < 0) LogicError("zz_pContext: maxroot may not be negative");

   if (NewP <= 1) LogicError("zz_pContext: p must be > 1");
   if (NumBits(NewP) > NTL_SP_NBITS) ResourceError("zz_pContext: modulus too big");

   ZZ P, B, M, M1, MinusM;
   long n, i;
   long q, t;

   p = NewP;

   pinv = 1/double(p);

   p_info = 0;

   conv(P, p);

   sqr(B, P);
   LeftShift(B, B, maxroot+NTL_FFTFudge);

   set(M);
   n = 0;
   while (M <= B) {
      UseFFTPrime(n);
      q = GetFFTPrime(n);
      n++;
      mul(M, M, q);
   }

   if (n > 4) LogicError("zz_pInit: too many primes");

   NumPrimes = n;
   PrimeCnt = n;
   MaxRoot = CalcMaxRoot(q);

   if (maxroot < MaxRoot)
      MaxRoot = maxroot;

   negate(MinusM, M);
   MinusMModP = rem(MinusM, p);

   CoeffModP.SetLength(n);
   x.SetLength(n);
   u.SetLength(n);

   for (i = 0; i < n; i++) {
      q = GetFFTPrime(i);

      div(M1, M, q);
      t = rem(M1, q);
      t = InvMod(t, q);
      if (NTL_zz_p_QUICK_CRT) mul(M1, M1, t);
      CoeffModP[i] = rem(M1, p);
      x[i] = ((double) t)/((double) q);
      u[i] = t;
   }
}

zz_pInfoT::zz_pInfoT(INIT_FFT_TYPE, FFTPrimeInfo *info)
{
   p = info->q;
   pinv = info->qinv;

   p_info = info;

   NumPrimes = 1;
   PrimeCnt = 0;

   MaxRoot = CalcMaxRoot(p);
}

// FIXME: we could make bigtab an optional argument

zz_pInfoT::zz_pInfoT(INIT_USER_FFT_TYPE, long q)
{
   long w;
   if (!IsFFTPrime(q, w)) LogicError("invalid user supplied prime");

   p = q;
   pinv = 1/((double) q);

   p_info_owner.make();
   p_info = p_info_owner.get();

   bool bigtab = false;
#ifdef NTL_FFT_BIGTAB
   bigtab = true;
#endif
   InitFFTPrimeInfo(*p_info, q, w, bigtab);

   NumPrimes = 1;
   PrimeCnt = 0;

   MaxRoot = CalcMaxRoot(p);
}

NTL_THREAD_LOCAL SmartPtr<zz_pInfoT> zz_pInfo = 0;

void zz_p::init(long p, long maxroot)
{
   zz_pContext c(p, maxroot);
   c.restore();

}

void zz_p::FFTInit(long index)
{
   zz_pContext c(INIT_FFT, index);
   c.restore();
}

void zz_p::UserFFTInit(long q)
{
   zz_pContext c(INIT_USER_FFT, q);
   c.restore();
}

zz_pContext::zz_pContext(long p, long maxroot) :
   ptr(MakeSmart<zz_pInfoT>(p, maxroot))
{ }

zz_pContext::zz_pContext(INIT_FFT_TYPE, long index)
{
   if (index < 0)
      LogicError("bad FFT prime index");

   UseFFTPrime(index);

   ptr =  FFTTables[index]->zz_p_context;
}

zz_pContext::zz_pContext(INIT_USER_FFT_TYPE, long q) :
   ptr(MakeSmart<zz_pInfoT>(INIT_USER_FFT, q))
{ }

void zz_pContext::save()
{
   ptr = zz_pInfo;
}

void zz_pContext::restore() const
{
   zz_pInfo = ptr;
}

zz_pBak::~zz_pBak()
{
   if (MustRestore) c.restore();
}

void zz_pBak::save()
{
   c.save();
   MustRestore = true;
}

void zz_pBak::restore()
{
   c.restore();
   MustRestore = false;
}

static inline
long reduce(long a, long p)
{
   if (a >= 0 && a < p)
      return a;
   else {
      a = a % p;
      if (a < 0) a += p;
      return a;
   }
}

zz_p to_zz_p(long a)
{
   return zz_p(reduce(a, zz_p::modulus()), INIT_LOOP_HOLE);
}

void conv(zz_p& x, long a)
{
   x._zz_p__rep = reduce(a, zz_p::modulus());
}

zz_p to_zz_p(const ZZ& a)
{
   return zz_p(rem(a, zz_p::modulus()), INIT_LOOP_HOLE);
}

void conv(zz_p& x, const ZZ& a)
{
   x._zz_p__rep = rem(a, zz_p::modulus());
}

istream& operator>>(istream& s, zz_p& x)
{
   NTL_ZZRegister(y);
   NTL_INPUT_CHECK_RET(s, s >> y);
   conv(x, y);

   return s;
}

ostream& operator<<(ostream& s, zz_p a)
{
   NTL_ZZRegister(y);
   y = rep(a);
   s << y;

   return s;
}

NTL_END_IMPL


// test file for special functions.

//  (C) Copyright Hubert Holin 2003.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#include <iomanip>

#include <boost/mpl/list.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_log.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/tools/test.hpp>

template<typename T>
struct string_type_name;

#define DEFINE_TYPE_NAME(Type)              \
template<> struct string_type_name<Type>    \
{                                           \
    static char const * _()                 \
    {                                       \
        return #Type;                       \
    }                                       \
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
DEFINE_TYPE_NAME(float);
DEFINE_TYPE_NAME(double);
DEFINE_TYPE_NAME(long double);

typedef boost::mpl::list<float,double,long double>  test_types;
#else
DEFINE_TYPE_NAME(float);
DEFINE_TYPE_NAME(double);

typedef boost::mpl::list<float,double>  test_types;
#endif

// Apple GCC 4.0 uses the "double double" format for its long double,
// which means that epsilon is VERY small but useless for
// comparisons. So, don't do those comparisons.
#if (defined(__APPLE_CC__) && defined(__GNUC__) && __GNUC__ == 4) || defined(BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS)
typedef boost::mpl::list<float,double>  near_eps_test_types;
#else
typedef boost::mpl::list<float,double,long double>  near_eps_test_types;
#endif

#include "sinc_test.hpp"
#include "sinhc_test.hpp"
#include "atanh_test.hpp"
#include "asinh_test.hpp"
#include "acosh_test.hpp"

boost::unit_test::test_suite *    init_unit_test_suite(int, char *[])
{
    ::boost::unit_test::unit_test_log.
        set_threshold_level(::boost::unit_test::log_messages);

    boost::unit_test::test_suite *    test =
        BOOST_TEST_SUITE("special_functions_test");

    BOOST_TEST_MESSAGE("Results of special functions test.");
    BOOST_TEST_MESSAGE(" ");
    BOOST_TEST_MESSAGE("(C) Copyright Hubert Holin 2003-2005.");
    BOOST_TEST_MESSAGE("Distributed under the Boost Software License, Version 1.0.");
    BOOST_TEST_MESSAGE("(See accompanying file LICENSE_1_0.txt or copy at");
    BOOST_TEST_MESSAGE("http://www.boost.org/LICENSE_1_0.txt)");
    BOOST_TEST_MESSAGE(" ");

#define BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(fct)   \
    test->add(BOOST_TEST_CASE_TEMPLATE(fct##_test, test_types));

#define BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR_NEAR_EPS(fct)   \
    test->add(BOOST_TEST_CASE_TEMPLATE(fct##_test, near_eps_test_types));

#define BOOST_SPECIAL_FUNCTIONS_COMMON_TEST             \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(atanh)     \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(asinh)     \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(acosh)     \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(sinc_pi)   \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR(sinhc_pi)

#define BOOST_SPECIAL_FUNCTIONS_TEMPLATE_TEMPLATE_TEST          \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR_NEAR_EPS(sinc_pi_complex)   \
    BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR_NEAR_EPS(sinhc_pi_complex)

#ifdef  BOOST_NO_TEMPLATE_TEMPLATES

#define BOOST_SPECIAL_FUNCTIONS_TEST    \
    BOOST_SPECIAL_FUNCTIONS_COMMON_TEST \
    BOOST_TEST_MESSAGE("Warning: no template templates; curtailed functionality.");

#else   /* BOOST_NO_TEMPLATE_TEMPLATES */

#define BOOST_SPECIAL_FUNCTIONS_TEST                \
    BOOST_SPECIAL_FUNCTIONS_COMMON_TEST             \
    BOOST_SPECIAL_FUNCTIONS_TEMPLATE_TEMPLATE_TEST

#endif  /* BOOST_NO_TEMPLATE_TEMPLATES */

    BOOST_SPECIAL_FUNCTIONS_TEST

#undef  BOOST_SPECIAL_FUNCTIONS_TEST

#undef  BOOST_SPECIAL_FUNCTIONS_TEMPLATE_TEMPLATE_TEST

#undef  BOOST_SPECIAL_FUNCTIONS_COMMON_TEST

#undef  BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR

#undef  BOOST_SPECIAL_FUNCTIONS_COMMON_GENERATOR_NEAR_EPS

#ifdef    BOOST_SPECIAL_FUNCTIONS_TEST_VERBOSE

    using    ::std::numeric_limits;

    BOOST_TEST_MESSAGE("epsilon");

    BOOST_TEST_MESSAGE( ::std::setw(15) << numeric_limits<float>::epsilon()
                << ::std::setw(15) << numeric_limits<double>::epsilon()
                << ::std::setw(15) << numeric_limits<long double>::epsilon());

    BOOST_TEST_MESSAGE(" ");

    test->add(BOOST_TEST_CASE(atanh_manual_check));
    test->add(BOOST_TEST_CASE(asinh_manual_check));
    test->add(BOOST_TEST_CASE(acosh_manual_check));
    test->add(BOOST_TEST_CASE(sinc_pi_manual_check));
    test->add(BOOST_TEST_CASE(sinhc_pi_manual_check));

#endif    /* BOOST_SPECIAL_FUNCTIONS_TEST_VERBOSE */

    return test;
}

#undef DEFINE_TYPE_NAME


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/bool.hpp>
#include <boost/hana/ext/std/integer_sequence.hpp>
#include <boost/hana/find_if.hpp>
#include <boost/hana/integral_constant.hpp>

#include <utility>
namespace hana = boost::hana;

struct is_last {
    template <typename N>
    constexpr auto operator()(N) const {
        return hana::bool_c<N::value == <%= input_size %>>;
    }
};

int main() {
    auto sequence = std::integer_sequence<
        <%= (["int"] + (1..input_size).to_a).join(', ') %>
    >{};
    auto result = hana::find_if(sequence, is_last{});
    (void)result;
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file replace_test.cpp
/// \brief Add your file description here.

#include <boost/test/minimal.hpp>
#include <iostream>
#include <iterator>
#include <list>
#include <map>
#include <vector>

#include "mcrl2/data/data_expression.h"
#include "mcrl2/data/detail/data_functional.h"
#include "mcrl2/data/parse.h"
#include "mcrl2/data/replace.h"
#include "mcrl2/data/standard_utility.h"
#include "mcrl2/data/substitutions/assignment_sequence_substitution.h"
#include "mcrl2/data/substitutions/mutable_map_substitution.h"
#include "mcrl2/data/substitutions/sequence_sequence_substitution.h"
#include "mcrl2/data/variable.h"
#include "mcrl2/utilities/text_utility.h"

using namespace mcrl2;
using namespace mcrl2::data;

void test_assignment_list()
{
  using namespace mcrl2::data::sort_bool;

  std::cerr << "assignment_list replace" << std::endl;

  variable d1("d1", basic_sort("D"));
  variable d2("d2", basic_sort("D"));
  variable d3("d3", basic_sort("D"));
  variable e1("e1", basic_sort("D"));
  variable e2("e2", basic_sort("D"));
  variable e3("e3", basic_sort("D"));

  assignment_vector l;
  l.push_back(assignment(d1, e1));
  l.push_back(assignment(e1, e2));
  l.push_back(assignment(e2, e3));

  data_expression t  = and_(equal_to(d1, e1), not_equal_to(e2, d3));
  data_expression t0 = and_(equal_to(e1, e2), not_equal_to(e3, d3));
  data_expression t2 = data::replace_variables(t, assignment_sequence_substitution(assignment_list(l.begin(), l.end())));
  std::cerr << "t  == " << data::pp(t) << std::endl;
  std::cerr << "t2 == " << data::pp(t2) << std::endl;
  BOOST_CHECK(t0 == t2);
}

void test_variable_replace()
{
  using namespace mcrl2::data::sort_bool;

  std::cerr << "variable replace" << std::endl;

  variable d1("d1", basic_sort("D"));
  variable d2("d2", basic_sort("D"));
  variable d3("d3", basic_sort("D"));
  variable_vector variables;
  variables.push_back(d1);
  variables.push_back(d2);
  variables.push_back(d3);

  variable x("x", basic_sort("D"));
  variable y("y", basic_sort("D"));
  variable z("z", basic_sort("D"));
  const data_expression& e1 = x;
  const data_expression& e2 = z;
  const data_expression& e3 = y;
  data_expression_vector replacements;
  replacements.push_back(e1);
  replacements.push_back(e2);
  replacements.push_back(e3);

  std::vector<variable> v;
  v.push_back(d1);
  v.push_back(d2);
  v.push_back(d3);
  std::list<data_expression> l;
  l.push_back(e1);
  l.push_back(e2);
  l.push_back(e3);

  data_expression t  = and_(equal_to(d1, d2), not_equal_to(d2, d3));
  data_expression t1 = data::replace_variables(t, make_sequence_sequence_substitution(variables, replacements));
  data_expression t2 = data::replace_variables(t, make_sequence_sequence_substitution(v, l));
  std::cerr << "t  == " << data::pp(t) << std::endl;
  std::cerr << "t1 == " << data::pp(t1) << std::endl;
  std::cerr << "t2 == " << data::pp(t2) << std::endl;
  BOOST_CHECK(t1 == t2);

  t = and_(equal_to(d1, d2), not_equal_to(d2, d3));
  BOOST_CHECK(t1 == replace_variables(t, make_sequence_sequence_substitution(variables, replacements)));
  BOOST_CHECK(t1 == replace_variables(t, make_sequence_sequence_substitution(variables, replacements)));
  BOOST_CHECK(t1 == replace_variables(t, make_sequence_sequence_substitution(v, l)));
  BOOST_CHECK(t1 == replace_variables(t, make_mutable_map_substitution(variables, replacements)));
}

void test_replace_with_binders()
{
  std::cerr << "replace with binders" << std::endl;
  mutable_map_substitution< > sigma;
  data_expression input1(variable("c", sort_bool::bool_()));
  data_expression input2(parse_data_expression("exists b: Bool, c: Bool. if(b, c, b)"));

  sigma[variable("c", sort_bool::bool_())] = sort_bool::false_();

  BOOST_CHECK(replace_free_variables(input1, sigma) == sort_bool::false_());

  // variable c is bound and should not be replaced
  BOOST_CHECK(replace_free_variables(input2, sigma) == input2);
}

inline
variable make_bool(const std::string& s)
{
  return variable(s, sort_bool::bool_());
}

void test_variables()
{
  variable d1 = make_bool("d1");
  variable d2 = make_bool("d2");
  variable d3 = make_bool("d3");
  variable d4 = make_bool("d4");
  data_expression e1 = data::sort_bool::not_(d1);
  data_expression e2 = data::sort_bool::not_(d2);
  data_expression e3 = data::sort_bool::not_(d3);

  mutable_map_substitution<> sigma;
  sigma[d1] = e1;
  sigma[d2] = e2;
  sigma[d3] = e3;

  // the variable in an assignment is not replaced by replace_free_variables
  assignment a(d1, d4);
  assignment b = replace_free_variables(a, sigma);
  BOOST_CHECK(b == a);

  // the variable in an assignment is not replaced by replace_variables
// TODO: this does no longer work, so it should be fixed
//  assignment c = replace_variables(a, sigma);
//  BOOST_CHECK(c == a);

  // the variable d1 in the right hand side is replaced by replace_free_variables, since
  // we do not consider the left hand side a binding variable
  a = assignment(d1, sort_bool::and_(d1, d2));
  b = replace_free_variables(a, sigma);
  BOOST_CHECK(b == assignment(d1, sort_bool::and_(e1, e2)));

  // the variable d1 in the right hand side is replaced by replace_free_variables
// TODO: this does no longer work, so it should be fixed
//  c = replace_variables(a, sigma);
//  BOOST_CHECK(c == assignment(d1, sort_bool::and_(e1, e2)));

  // this will lead to an assertion failure, because an attempt will be made to store
  // a data expression in a variable
  sigma[d1] = sort_bool::and_(d1, d2);
  // data_expression d = replace_variables(d1, sigma);

  // therefore one should first convert d1 to a data expression:
  data_expression d = replace_variables(data_expression(d1), sigma);
  BOOST_CHECK(d == sort_bool::and_(d1, d2));
}

void check_result(const std::string& expression, const std::string& result, const std::string& expected_result, const std::string& title)
{
  if (result != expected_result)
  {
    std::cout << "--- failure in " << title << " ---" << std::endl;
    std::cout << "expression      = " << expression << std::endl;
    std::cout << "result          = " << result << std::endl;
    std::cout << "expected result = " << expected_result << std::endl;
    BOOST_CHECK(result == expected_result);
  }
}

inline
std::vector<data::variable> variable_context()
{
  std::vector<data::variable> result;
  result.push_back(make_bool("k"));
  result.push_back(make_bool("m"));
  result.push_back(make_bool("n"));
  result.push_back(make_bool("v"));
  result.push_back(make_bool("w"));
  result.push_back(make_bool("x"));
  result.push_back(make_bool("y"));
  result.push_back(make_bool("z"));
  result.push_back(make_bool("k1"));
  result.push_back(make_bool("m1"));
  result.push_back(make_bool("n1"));
  result.push_back(make_bool("v1"));
  result.push_back(make_bool("w1"));
  result.push_back(make_bool("x1"));
  result.push_back(make_bool("y1"));
  result.push_back(make_bool("z1"));
  result.push_back(make_bool("k2"));
  result.push_back(make_bool("m2"));
  result.push_back(make_bool("n2"));
  result.push_back(make_bool("v2"));
  result.push_back(make_bool("w2"));
  result.push_back(make_bool("x2"));
  result.push_back(make_bool("y2"));
  result.push_back(make_bool("z2"));
  return result;
}

inline
data::data_expression parse_expression(const std::string& text, const std::vector<data::variable>& variables = variable_context())
{
  return data::parse_data_expression(text, variables);
}

/// \brief Parses a string of the form "b: Bool := v, c: Bool := !w", and adds
inline
data::mutable_map_substitution<> parse_substitution(const std::string& text, const std::vector<data::variable>& variables = variable_context())
{
  data::mutable_map_substitution<> sigma;
  std::vector<std::string> substitutions = utilities::split(text, ";");
  for (const std::string& substitution: substitutions)
  {
    std::vector<std::string> words = utilities::regex_split(substitution, ":=");
    if (words.size() != 2)
    {
      continue;
    }
    data::variable v = data::parse_variable(words[0]);
    data::data_expression e = data::parse_data_expression(words[1], variables);
    sigma[v] = e;
  }
  return sigma;
}

// Returns the free variables in the right hand side of sigma.
std::set<data::variable> sigma_variables(const data::mutable_map_substitution<>& sigma)
{
  std::set<data::variable> result;
  for (const auto& i: sigma)
  {
    std::set<data::variable> V = data::find_free_variables(i.second);
    V.erase(i.first);
    result.insert(V.begin(), V.end());
  }
  return result;
}

void test_replace_variables_capture_avoiding(const std::string& x_text, const std::string& sigma_text, const std::string& expected_result)
{
  data::data_expression x = parse_expression(x_text);
  data::mutable_map_substitution<> sigma = parse_substitution(sigma_text);
  std::set<data::variable> sv = sigma_variables(sigma);
  std::string result = data::pp(data::replace_variables_capture_avoiding(x, sigma, sv));
  check_result(x_text + " sigma = " + sigma_text, result, expected_result, "replace_variables_capture_avoiding");
}

void test_replace_variables_capture_avoiding()
{
  test_replace_variables_capture_avoiding("v", "v: Bool := w", "w");
  test_replace_variables_capture_avoiding("forall x: Bool . x => y", "x: Bool := z", "forall x1: Bool. x1 => y");
  test_replace_variables_capture_avoiding("forall x: Bool . x => y", "y: Bool := z", "forall x: Bool. x => z");
  test_replace_variables_capture_avoiding("forall x: Bool . x => y", "y: Bool := x", "forall x1: Bool. x1 => x");
  test_replace_variables_capture_avoiding("forall x: Bool . x => x1 => y", "y: Bool := x", "forall x2: Bool. x2 => x1 => x");
  test_replace_variables_capture_avoiding("x => x1 => y whr x = y end", "y: Bool := x", "x2 => x1 => x whr x2 = x end");
  test_replace_variables_capture_avoiding("forall n: Bool. n => forall k: Bool. k => m", "m: Bool := n", "forall n1: Bool. n1 => (forall k: Bool. k => n)");
  test_replace_variables_capture_avoiding("forall n: Bool. n => forall n: Bool. n => m", "m: Bool := n", "forall n1: Bool. n1 => (forall n2: Bool. n2 => n)");
  test_replace_variables_capture_avoiding("forall n: Bool. n => forall k: Bool. k => m", "m: Bool := n", "forall n1: Bool. n1 => (forall k: Bool. k => n)");
  test_replace_variables_capture_avoiding("forall n: Bool. n => forall n: Bool. n => m", "m: Bool := n", "forall n1: Bool. n1 => (forall n2: Bool. n2 => n)");
}

void test_replace_free_variables()
{
  data::mutable_map_substitution<> sigma;
  data::variable x("x", data::sort_bool::bool_());
  data::data_expression y = data::sort_bool::not_(x);
  sigma[x] = y;
  data::assignment a(x, x);
  data::assignment b(x, y);
  data::assignment c = data::replace_free_variables(a, sigma);
  BOOST_CHECK(b == c);

  data::assignment_list va;
  va.push_front(a);
  data::assignment_list vb;
  vb.push_front(b);
  data::assignment_list vc = data::replace_free_variables(va, sigma);
  BOOST_CHECK(vb == vc);
}

void test_ticket_1209()
{
  std::string text = "n whr n = m, m = 3 end whr m = 255 end";
  std::string expected_result = "n whr n = m2, m3 = 3 end whr m2 = 255 end";
  data::data_expression x = data::parse_data_expression(text);
  data::mutable_map_substitution<> sigma;
  data::variable m("m", data::sort_pos::pos());
  data::variable m1("m1", data::sort_pos::pos());
  sigma[m] = m1;
  std::set<data::variable> v;
  v.insert(m1);
  data::data_expression x1 = data::replace_variables_capture_avoiding(x, sigma, v);
  std::string result = data::pp(x1);
  BOOST_CHECK(result == expected_result);
}

int test_main(int argc, char** argv)
{
  test_assignment_list();
  test_variable_replace();
  test_replace_with_binders();
  test_variables();
  test_replace_variables_capture_avoiding();
  test_replace_free_variables();
  test_ticket_1209();

  return 0;
}


//=============================================================================
//
//  CLASS SuperSparseMatrixT
//
//=============================================================================

#ifndef COMISO_SUPERSPARSEMATRIXT_HH
#define COMISO_SUPERSPARSEMATRIXT_HH

//== INCLUDES =================================================================

#include <iostream>
#include <map>
#include <math.h>
#include <Eigen/Dense>

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class SuperSparseMatrixT SuperSparseMatrixT.hh <COMISO/.../SuperSparseMatrixT.hh>

    Brief Description.

    A more elaborate description follows.
*/

template <class VType>
class SuperSparseMatrixT
{
public:

  // value type
  typedef VType                                  VT;
  typedef std::pair<unsigned int, unsigned int> PII;

  // iterate over elements
  class iterator
  {
  public:

    // default constructor
    iterator() {}
    // copy constructor
    iterator(const iterator& _it) { m_it_ = _it.m_it_; }
    // map iterator constructor
    iterator(const typename std::map<PII,VType>::iterator& _m_it) { m_it_ = _m_it; }

    // get reference to data
    VT& operator*() { return m_it_->second; }

    // get row and col of value
    unsigned int row() {return m_it_->first.first; }
    unsigned int col() {return m_it_->first.second; }

    // post-increment
    iterator& operator++()    { ++m_it_; return(*this);}
    iterator  operator++(int) { return iterator(++m_it_); }

    bool operator== (const iterator& _it) { return (m_it_ == _it.m_it_);}
    bool operator!= (const iterator& _it) { return (m_it_ != _it.m_it_);}

    // get raw iterator of map
    typename std::map<PII,VType>::iterator& map_iterator() {return m_it_;}

  private:
    typename std::map<PII,VType>::iterator m_it_;
  };

  /// Constructor
  SuperSparseMatrixT(const int _n_rows = 0, const int _n_cols = 0)
  : n_rows_(_n_rows), n_cols_(_n_cols)
  {}

  /// Destructor
  ~SuperSparseMatrixT() {}

  // iterate over non-zeros
  iterator begin() { return iterator(data_.begin()); }
  iterator end()   { return iterator(data_.end());   }
  // erase element
  void     erase( iterator _it) { data_.erase(_it.map_iterator()); }

  // element access
  VT& operator()(const unsigned int _i, const unsigned int _j)
  {  return data_[PII(_i,_j)]; }

  // const element access
  const VT& operator()(const unsigned int _i, const unsigned int _j) const
  {  return data_[PII(_i,_j)]; }

  // get number of stored elements
  unsigned int nonZeros() { return data_.size(); }

  // get dimensions
  unsigned int rows() const { return n_rows_;}
  unsigned int cols() const { return n_cols_;}

  // resize matrix -> delete invalid elements
  void resize(const unsigned int _n_rows, const unsigned int _n_cols)
  {
    n_rows_ = _n_rows;
    n_cols_ = _n_cols;

    // delete out of range elements
    typename std::map<PII, VT>::iterator m_it = data_.begin();
    for(; m_it != data_.end();)
    {
      if(m_it->first.first >=n_rows_ || m_it->first.second >= n_cols_)
        data_.erase(m_it++);
      else
        ++m_it;
    }
  }

  // clear data
  void clear()
  { data_.clear(); }

  // removes all values whose absolut value is smaller than _eps
  void prune(const VT _eps)
  {
    typename std::map<PII, VT>::iterator m_it = data_.begin();
    for(; m_it != data_.end();)
    {
      if(fabs(m_it->second) < _eps)
        data_.erase(m_it++);
      else
        ++m_it;
    }
  }

  // scale matrix by scalar
  void scale(const VT _s)
  {
    typename std::map<PII, VT>::iterator m_it = data_.begin();
    for(; m_it != data_.end(); ++m_it)
      m_it->second *=_s;
  }

  void print()
  {
    iterator it  = begin();
    iterator ite = end();

    std::cerr << "######## SuperSparseMatrix ########" << std::endl;
    std::cerr << "dimension : " << rows()   << " x " << cols() << std::endl;
    std::cerr << "#non-zeros: " << nonZeros() << std::endl;
    for(; it!=ite; ++it)
      std::cerr << "(" << it.row() << "," << it.col() << ") -> " << *it << std::endl;
  }

  void print_eigenvalues()
  {
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n_rows_, n_cols_);

    iterator it = begin();
    for(; it != end(); ++it)
      A(it.row(),it.col()) = *it;

    Eigen::EigenSolver<Eigen::MatrixXd> eigensolver(A);
    if (eigensolver.info() != Eigen::Success) abort();
    std::cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << std::endl;
//    std::cout << "Here's a matrix whose columns are eigenvectors of A \n"
//    << "corresponding to these eigenvalues:\n"
//    << eigensolver.eigenvectors() << std::endl;
  }

private:

  // dimension of matrix
  unsigned int n_rows_;
  unsigned int n_cols_;

  typename std::map<PII, VT> data_;
};

//=============================================================================
} // namespace COMISO
//=============================================================================
#if defined(INCLUDE_TEMPLATES) && !defined(COMISO_SUPERSPARSEMATRIXT_C)
#define COMISO_SUPERSPARSEMATRIXT_TEMPLATES
#include "SuperSparseMatrixT.cc"
#endif
//=============================================================================
#endif // COMISO_SUPERSPARSEMATRIXT_HH defined
//=============================================================================


#ifndef material_hpp
#define material_hpp

#include <three-dim-util/opengl2/gl.hpp>
#include <array>
#include <Eigen/Core>

namespace threedimutil
{
    struct Material
    {
        constexpr Material() {}

        constexpr Material(double r, double g, double b) :
        diffuse({{ static_cast<GLfloat>(r), static_cast<GLfloat>(g), static_cast<GLfloat>(b), 1.0 }})
        {
        }

        Material(const Eigen::Vector3d& diffuse) :
        diffuse({{ static_cast<GLfloat>(diffuse(0)), static_cast<GLfloat>(diffuse(1)), static_cast<GLfloat>(diffuse(2)), 1.0 }})
        {
        }

        std::array<GLfloat, 4> ambient   = {{ 0.1, 0.1, 0.1, 1.0 }};
        std::array<GLfloat, 4> diffuse   = {{ 0.5, 0.5, 0.5, 1.0 }};
        std::array<GLfloat, 4> specular  = {{ 0.8, 0.8, 0.8, 1.0 }};
        GLfloat                shininess = 100.0;

        void set() const
        {
            gl()->glMaterialfv(GL_FRONT, GL_AMBIENT,   ambient  .data());
            gl()->glMaterialfv(GL_FRONT, GL_DIFFUSE,   diffuse  .data());
            gl()->glMaterialfv(GL_FRONT, GL_SPECULAR,  specular .data());
            gl()->glMaterialf (GL_FRONT, GL_SHININESS, shininess       );
        }
    };
}

#endif /* material_hpp */


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

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

  vertex_descriptor router_six;
  property_map < GraphvizDigraph, vertex_attribute_t >::type
    vertex_attr_map = get(vertex_attribute, g_dot);
  graph_traits < GraphvizDigraph >::vertex_iterator vi, vi_end;
  for (tie(vi, vi_end) = vertices(g_dot); vi != vi_end; ++vi)
    if ("RT6" == vertex_attr_map[*vi]["label"]) {
      router_six = *vi;
      break;
    }

  std::vector < vertex_descriptor > parent(num_vertices(g));
  // All vertices start out as there own parent
  typedef graph_traits < Graph >::vertices_size_type size_type;
  for (size_type p = 0; p < num_vertices(g); ++p)
    parent[p] = p;

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  std::vector<int> distance(num_vertices(g));
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
  property_map<Graph, vertex_index_t>::type indexmap = get(vertex_index, g);
  dijkstra_shortest_paths
    (g, router_six, &parent[0], &distance[0], weightmap,
     indexmap, std::less<int>(), closed_plus<int>(),
     std::numeric_limits<int>::max(), 0, default_dijkstra_visitor());
#else
  dijkstra_shortest_paths(g, router_six, predecessor_map(&parent[0]));
#endif

  graph_traits < GraphvizDigraph >::edge_descriptor e;
  for (size_type i = 0; i < num_vertices(g); ++i)
    if (parent[i] != i) {
      e = edge(parent[i], i, g_dot).first;
      edge_attr_map[e]["color"] = "black";
    }

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ can't handle write_graphviz :(
  {
    std::ofstream out("figs/ospf-sptree.dot");
    out << "digraph loops {\n"
        << "size=\"3,3\"\n"
        << "ratio=\"fill\"\n"
        << "shape=\"box\"\n";
    graph_traits<Graph>::vertex_iterator vi, vi_end;
    for (tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
      out << *vi << "[";
      for (std::map<std::string,std::string>::iterator ai = vattr_map[*vi].begin();
           ai != vattr_map[*vi].end(); ++ai) {
        out << ai->first << "=" << ai->second;
        if (next(ai) != vattr_map[*vi].end())
          out << ", ";
      }
      out<< "]";
    }

    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
      out << source(*ei, g) << " -> " << target(*ei, g) << "[";
      std::map<std::string,std::string>& attr_map = eattr_map[*ei];
      for (std::map<std::string,std::string>::iterator eai = attr_map.begin();
           eai != attr_map.end(); ++eai) {
        out << eai->first << "=" << eai->second;
        if (next(eai) != attr_map.end())
          out << ", ";
      }
      out<< "]";
    }
    out << "}\n";
  }
#else
  graph_property < GraphvizDigraph, graph_edge_attribute_t >::type &
    graph_edge_attr_map = get_property(g_dot, graph_edge_attribute);
  graph_edge_attr_map["color"] = "grey";
  write_graphviz("figs/ospf-sptree.dot", g_dot);
#endif

  std::ofstream rtable("routing-table.dat");
  rtable << "Dest    Next Hop    Total Cost" << std::endl;
  for (tie(vi, vi_end) = vertices(g_dot); vi != vi_end; ++vi)
    if (parent[*vi] != *vi) {
      rtable << vertex_attr_map[*vi]["label"] << "    ";
      vertex_descriptor v = *vi, child;
      int path_cost = 0;
      property_map < Graph, edge_weight_t >::type
        weight_map = get(edge_weight, g);
      do {
        path_cost += get(weight_map, edge(parent[v], v, g).first);
        child = v;
        v = parent[v];
      } while (v != parent[v]);
      rtable << vertex_attr_map[child]["label"] << "     ";
      rtable << path_cost << std::endl;

    }

  return EXIT_SUCCESS;
}


//=======================================================================
// Copyright (c) 2015
//
// 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)
//=======================================================================
/**
 * @file frequent_directions_test.cpp
 * @brief
 * @author Tomasz Strozak
 * @version 1.0
 * @date 2015-04-17
 */

#include "test_utils/sketch_accuracy_check.hpp"
#include "test_utils/serialization.hpp"

#include "paal/sketch/frequent_directions.hpp"
#include "paal/utils/irange.hpp"

#include <boost/algorithm/cxx11/all_of.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/equal.hpp>
#include <boost/test/unit_test.hpp>

#include <vector>

using coordinate_t = double;
using matrix_t = boost::numeric::ublas::matrix<coordinate_t>;
using matrix_row_t = boost::numeric::ublas::matrix_row<matrix_t>;
using data_t = std::vector<std::vector<coordinate_t>>;

namespace {
    std::size_t const rows_count = 5;
    std::size_t const columns_count = 3;
    std::size_t const sketch_size = 4;
    auto compress_sizes = {1, 2, 3};
    /* Data is chosen so that the sketch matrix is non-degenerate before compress
     * stage. Right-singular vectors are unique up to sign, so the result matrix
     * rows are unique up to sign. */
    data_t const data = {{24, 12, 24},
                           {-40, 40, 20},
                           {0, 0 ,0},
                           {-37, -74, 74},
                           {10, 20, 30}};
    data_t const result = {{-35, -70, 70},
                             {-32, 32, 16},
                             {10, 20, 30},
                             {0, 0, 0}};
    coordinate_t EPS = 1e-9;
}

auto generate_data_matrix() {
    matrix_t matrix(rows_count, columns_count);
    for (auto i : paal::irange(data.size())) {
        matrix_row_t row(matrix, i);
        boost::copy(data[i], row.begin());
    }
    return matrix;
}

BOOST_AUTO_TEST_SUITE(frequent_directions)

BOOST_AUTO_TEST_CASE(update) {
    auto fd_sketch = paal::make_frequent_directions<coordinate_t>(sketch_size, columns_count);
    auto data_matrix = generate_data_matrix();

    fd_sketch.update(data_matrix);
    fd_sketch.compress();

    auto sketch = fd_sketch.get_sketch().first;
    check_frequent_directions(std::move(data_matrix), std::move(sketch), EPS);
}

BOOST_AUTO_TEST_CASE(update_row) {
    auto fd_sketch = paal::make_frequent_directions<coordinate_t>(sketch_size, columns_count);
    auto data_matrix = generate_data_matrix();

    for (auto i : paal::irange(data_matrix.size1()))
    {
        matrix_row_t row(data_matrix, i);
        fd_sketch.update_row(row);
    }
    fd_sketch.compress();

    auto sketch = fd_sketch.get_sketch().first;
    check_frequent_directions(std::move(data_matrix), std::move(sketch), EPS);
}

BOOST_AUTO_TEST_CASE(zero_rows) {
    auto fd_sketch = paal::make_frequent_directions<coordinate_t>(sketch_size, columns_count);
    auto data_matrix = generate_data_matrix();

    fd_sketch.update(std::move(data_matrix));
    fd_sketch.compress();

    auto actual_size = fd_sketch.get_sketch().second;
    BOOST_CHECK(actual_size <= sketch_size/2);

    auto sketch = fd_sketch.get_sketch().first;
    for (auto i : paal::irange(actual_size, sketch_size)) {
        matrix_row_t row(sketch, i);
        BOOST_CHECK(boost::algorithm::all_of_equal(row, 0.0));
    }
}

BOOST_AUTO_TEST_CASE(result_matrix) {
    auto fd_sketch = paal::make_frequent_directions<coordinate_t>(sketch_size, columns_count);
    auto data_matrix = generate_data_matrix();

    fd_sketch.update(std::move(data_matrix));

    auto sketch = fd_sketch.get_sketch().first;

    auto check_close_doubles = [](double l, double r) {
        return std::abs(l - r) < EPS;
    };

    for (auto i : paal::irange(sketch_size)) {
        matrix_row_t row (sketch, i);
        BOOST_CHECK(boost::equal(result[i], row, check_close_doubles) ||
                    boost::equal(result[i], -row, check_close_doubles));
    };
}

BOOST_AUTO_TEST_CASE(sparse) {
    matrix_t matrix(sketch_size, columns_count, 0);
    auto fd_sketch = paal::make_frequent_directions(std::move(matrix));
    boost::numeric::ublas::mapped_matrix<coordinate_t> data_matrix(rows_count, columns_count);
    for (auto i : paal::irange(rows_count)) {
        data_matrix(i, i % columns_count) = i + 1;
    }

    fd_sketch.update(data_matrix);
    fd_sketch.compress();

    auto sketch = fd_sketch.get_sketch().first;
    check_frequent_directions(std::move(data_matrix), std::move(sketch), EPS);
}

BOOST_AUTO_TEST_CASE(zero_matrix) {
    auto fd_sketch = paal::make_frequent_directions<coordinate_t>(sketch_size, columns_count);
    matrix_t data_matrix(rows_count, columns_count, 0);

    fd_sketch.update(data_matrix);
    fd_sketch.compress();

    auto sketch = fd_sketch.get_sketch().first;
    check_frequent_directions(std::move(data_matrix), std::move(sketch), EPS);
}

BOOST_AUTO_TEST_CASE(compress_size) {
    for(auto compress_size : compress_sizes) {
        auto fd_sketch = paal::make_frequent_directions<coordinate_t>(sketch_size, columns_count, compress_size);
        auto data_matrix = generate_data_matrix();

        fd_sketch.update(data_matrix);
        fd_sketch.compress();

        auto sketch = fd_sketch.get_sketch().first;
        check_frequent_directions(std::move(data_matrix), std::move(sketch),
                EPS, coordinate_t(compress_size));
    }

    for(auto compress_size : compress_sizes) {
        matrix_t matrix(sketch_size, columns_count);
        auto fd_sketch = paal::make_frequent_directions(std::move(matrix), compress_size);
        auto data_matrix = generate_data_matrix();

        fd_sketch.update(data_matrix);
        fd_sketch.compress();

        auto sketch = fd_sketch.get_sketch().first;
        check_frequent_directions(std::move(data_matrix), std::move(sketch),
                EPS, coordinate_t(compress_size));
    }
}

BOOST_AUTO_TEST_CASE(serialization) {
    auto fd_sketch = paal::make_frequent_directions<coordinate_t>(sketch_size, columns_count);
    auto data_matrix = generate_data_matrix();

    fd_sketch.update(std::move(data_matrix));
    fd_sketch.compress();

    serialize_test(fd_sketch, "tmp.bin");
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_APPROXIMATE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_APPROXIMATE_HPP

#include <cstddef>
#include <cmath>
#include <array>

#include <boost/mp11/integral.hpp>
#include <boost/mp11/list.hpp>
#include <boost/mp11/algorithm.hpp>

#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/expression_tree.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace generic_robust_predicates
{

template
<
    typename Node,
    std::size_t N,
    typename Real,
    typename InputArr
>
struct get_nth_real_impl
{
    static inline Real apply(const InputArr& input)
    {
        return input[N - 1];
    }
};

template
<
    typename Node,
    typename Real,
    typename InputArr
>
struct get_nth_real_impl<Node, 0, Real, InputArr>
{
    static inline Real apply(const InputArr&)
    {
        return Node::value;
    }
};

template
<
    typename Node,
    std::size_t N,
    typename Real,
    typename InputArr
>
inline Real get_nth_real(const InputArr& input)
{
    return get_nth_real_impl<Node, Node::argn, Real, InputArr>::apply(input);
}

template
<
    typename All,
    typename Node,
    typename Real,
    typename Arr,
    bool is_leaf,
    typename InputArr
>
struct get_approx_impl
{
    static inline Real apply(Arr& interim_results, const InputArr& input)
    {
        return interim_results[boost::mp11::mp_find<All, Node>::value];
    }
};

template
<
    typename All,
    typename Node,
    typename Real,
    typename Arr,
    typename InputArr
>
struct get_approx_impl<All, Node, Real, Arr, true, InputArr>
{
    static inline Real apply(Arr& interim_results, const InputArr& input)
    {
        return get_nth_real<Node, Node::argn, Real, InputArr>(input);
    }
};

template
<
    typename All,
    typename Node,
    typename Real,
    typename Arr,
    typename InputArr
>
inline Real get_approx(Arr& interim_results, const InputArr& input)
{
    return get_approx_impl
        <
            All,
            Node,
            Real,
            Arr,
            is_leaf<Node>::value,
            InputArr
        >::apply(interim_results, input);
}

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    operator_types Op,
    typename InputArr
>
struct approximate_interim_impl {};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    bool Empty,
    typename InputArr
>
struct approximate_remainder_impl
{
    static inline void apply(Arr& interim_results, const InputArr& input)
    {
        using node = boost::mp11::mp_front<Remaining>;
        approximate_interim_impl
            <
                All,
                Remaining,
                Real,
                Arr,
                node::operator_type,
                InputArr
            >::apply(interim_results, input);
    }
};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
struct approximate_remainder_impl
    <
        All,
        Remaining,
        Real,
        Arr,
        true,
        InputArr
    >
{
    static inline void apply(Arr& interim_results, const InputArr& input) {}
};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
inline void approximate_remainder(Arr& interim_results, const InputArr& input)
{
    approximate_remainder_impl
        <
            All,
            Remaining,
            Real,
            Arr,
            boost::mp11::mp_empty<Remaining>::value,
            InputArr
        >::apply(interim_results, input);
}

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
struct approximate_interim_impl
    <
        All,
        Remaining,
        Real,
        Arr,
        operator_types::product,
        InputArr
    >
{
    static inline void apply(Arr& interim_results, const InputArr& input)
    {
        using node = boost::mp11::mp_front<Remaining>;
        interim_results[boost::mp11::mp_find<All, node>::value] =
                  get_approx<All, typename node::left, Real>(interim_results,
                                                             input)
                * get_approx<All, typename node::right, Real>(interim_results,
                                                              input);
        approximate_remainder
            <
                All,
                boost::mp11::mp_pop_front<Remaining>,
                Real
            >(interim_results, input);
    }
};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
struct approximate_interim_impl
    <
        All,
        Remaining,
        Real,
        Arr,
        operator_types::max,
        InputArr
    >
{
    static inline void apply(Arr& interim_results, const InputArr& input)
    {
        using node = boost::mp11::mp_front<Remaining>;
        interim_results[boost::mp11::mp_find<All, node>::value] = std::max(
                  get_approx<All, typename node::left, Real>(interim_results,
                                                             input),
                  get_approx<All, typename node::right, Real>(interim_results,
                                                              input));
        approximate_remainder
            <
                All,
                boost::mp11::mp_pop_front<Remaining>,
                Real
            >(interim_results, input);
    }
};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
struct approximate_interim_impl
    <
        All,
        Remaining,
        Real,
        Arr,
        operator_types::min,
        InputArr
    >
{
    static inline void apply(Arr& interim_results, const InputArr& input)
    {
        using node = boost::mp11::mp_front<Remaining>;
        interim_results[boost::mp11::mp_find<All, node>::value] = std::min(
                  get_approx<All, typename node::left, Real>(interim_results,
                                                             input),
                  get_approx<All, typename node::right, Real>(interim_results,
                                                              input));
        approximate_remainder
            <
                All,
                boost::mp11::mp_pop_front<Remaining>,
                Real
            >(interim_results, input);
    }
};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
struct approximate_interim_impl
    <
        All,
        Remaining,
        Real,
        Arr,
        operator_types::sum,
        InputArr
    >
{
    static inline void apply(Arr& interim_results, const InputArr& input)
    {
        using node = boost::mp11::mp_front<Remaining>;
        interim_results[boost::mp11::mp_find<All, node>::value] =
                  get_approx<All, typename node::left, Real>(interim_results,
                                                             input)
                + get_approx<All, typename node::right, Real>(interim_results,
                                                              input);
        approximate_remainder
            <
                All,
                boost::mp11::mp_pop_front<Remaining>,
                Real
            >(interim_results, input);
    }
};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
struct approximate_interim_impl
    <
        All,
        Remaining,
        Real,
        Arr,
        operator_types::difference,
        InputArr
    >
{
    static inline void apply(Arr& interim_results, const InputArr& input)
    {
        using node = boost::mp11::mp_front<Remaining>;
        interim_results[boost::mp11::mp_find<All, node>::value] =
                  get_approx<All, typename node::left, Real>(interim_results,
                                                             input)
                - get_approx<All, typename node::right, Real>(interim_results,
                                                              input);
        approximate_remainder
            <
                All,
                boost::mp11::mp_pop_front<Remaining>,
                Real
            >(interim_results, input);
    }
};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
struct approximate_interim_impl
    <
        All,
        Remaining,
        Real,
        Arr,
        operator_types::abs,
        InputArr
    >
{
    static inline void apply(Arr& interim_results, const InputArr& input)
    {
        using node = boost::mp11::mp_front<Remaining>;
        interim_results[boost::mp11::mp_find<All, node>::value] =
            std::abs(get_approx
                <
                    All,
                    typename node::child,
                    Real
                >(interim_results, input));
        approximate_remainder
            <
                All,
                boost::mp11::mp_pop_front<Remaining>,
                Real
            >(interim_results, input);
    }
};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
struct approximate_interim_impl
    <
        All,
        Remaining,
        Real,
        Arr,
        operator_types::no_op,
        InputArr
    >
{
    static inline void apply(Arr& interim_results, const InputArr& input)
    {
        approximate_remainder
            <
                All,
                boost::mp11::mp_pop_front<Remaining>,
                Real
            >(interim_results, input);
    }
};

template
<
    typename All,
    typename Remaining,
    typename Real,
    typename Arr,
    typename InputArr
>
inline void approximate_interim(Arr& interim_results, const InputArr& input)
{
    approximate_remainder
        <
            All,
            Remaining,
            Real
        >(interim_results, input);
}

template<typename Expression, typename Real, typename InputArr>
inline Real approximate_value(const InputArr& input)
{
    using stack = typename boost::mp11::mp_unique<post_order<Expression>>;
    using evals = typename boost::mp11::mp_remove_if<stack, is_leaf>;
    std::array<Real, boost::mp11::mp_size<evals>::value> results;
        approximate_interim<evals, evals, Real>(results, input);
    return results.back();
}

}} // namespace detail::generic_robust_predicates

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_APPROXIMATE_HPP


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/termstructures/pricecurve.hpp
    \brief Interpolated price curve
*/

#ifndef quantext_price_curve_hpp
#define quantext_price_curve_hpp

#include <boost/algorithm/cxx11/is_sorted.hpp>

#include <qle/termstructures/pricetermstructure.hpp>

#include <ql/math/comparison.hpp>
#include <ql/patterns/lazyobject.hpp>
#include <ql/quote.hpp>
#include <ql/termstructures/interpolatedcurve.hpp>
#include <ql/time/calendars/nullcalendar.hpp>

namespace QuantExt {

//! Interpolated price curve
/*! Class representing a curve of projected prices in the future.

    \warning for consistency, if curve is constructed by inferring times from dates
             using a given day counter, pass the same day counter to the constructor

    \ingroup termstructures
*/
template <class Interpolator>
class InterpolatedPriceCurve : public PriceTermStructure,
                               public QuantLib::LazyObject,
                               protected QuantLib::InterpolatedCurve<Interpolator> {
public:
    //! \name Constructors
    //@{
    //! Curve constructed from times and prices
    InterpolatedPriceCurve(const std::vector<QuantLib::Time>& times, const std::vector<QuantLib::Real>& prices,
                           const QuantLib::DayCounter& dc, const Interpolator& interpolator = Interpolator());

    //! Curve constructed from times and quotes
    InterpolatedPriceCurve(const std::vector<QuantLib::Time>& times,
                           const std::vector<QuantLib::Handle<QuantLib::Quote> >& quotes,
                           const QuantLib::DayCounter& dc, const Interpolator& interpolator = Interpolator());

    //! Curve constructed from dates and prices
    InterpolatedPriceCurve(const std::vector<QuantLib::Date>& dates, const std::vector<QuantLib::Real>& prices,
                           const QuantLib::DayCounter& dc, const Interpolator& interpolator = Interpolator());

    //! Curve constructed from dates and quotes
    InterpolatedPriceCurve(const std::vector<QuantLib::Date>& dates,
                           const std::vector<QuantLib::Handle<QuantLib::Quote> >& quotes,
                           const QuantLib::DayCounter& dc, const Interpolator& interpolator = Interpolator());
    //@}

    //! \name Observer interface
    //@{
    void update();
    //@}

    //! \name LazyObject interface
    //@{
    void performCalculations() const;
    //@}

    //! \name TermStructure interface
    //@{
    //! This is not used by this class and returns the maximum date
    QuantLib::Date maxDate() const { return QuantLib::Date::maxDate(); }
    QuantLib::Time maxTime() const;
    //@}

    //! \name PriceTermStructure interface
    //@{
    QuantLib::Time minTime() const;
    //@}

    //! \name Inspectors
    //@{
    const std::vector<QuantLib::Time>& times() const { return this->times_; }
    const std::vector<QuantLib::Real>& prices() const { return this->data_; }
    //@}

protected:
    //! \name PriceTermStructure implementation
    //@{
    QuantLib::Real priceImpl(QuantLib::Time t) const;
    //@}

private:
    std::vector<QuantLib::Handle<QuantLib::Quote> > quotes_;
    std::vector<QuantLib::Date> dates_;

    void initialise();
    void convertDatesToTimes();
    void getPricesFromQuotes() const;
};

template <class Interpolator>
InterpolatedPriceCurve<Interpolator>::InterpolatedPriceCurve(const std::vector<QuantLib::Time>& times,
                                                             const std::vector<QuantLib::Real>& prices,
                                                             const QuantLib::DayCounter& dc,
                                                             const Interpolator& interpolator)
    : PriceTermStructure(0, QuantLib::NullCalendar(), dc), QuantLib::InterpolatedCurve<Interpolator>(times, prices,
                                                                                interpolator) {

    initialise();
}

template <class Interpolator>
InterpolatedPriceCurve<Interpolator>::InterpolatedPriceCurve(
    const std::vector<QuantLib::Time>& times, const std::vector<QuantLib::Handle<QuantLib::Quote> >& quotes,
    const QuantLib::DayCounter& dc, const Interpolator& interpolator)
    : PriceTermStructure(0, QuantLib::NullCalendar(), dc), QuantLib::InterpolatedCurve<Interpolator>(
                                                               times, std::vector<QuantLib::Real>(quotes.size()),
                                                               interpolator),
      quotes_(quotes) {

    initialise();

    // Observe the quotes
    for (QuantLib::Size i = 0; i < quotes_.size(); ++i) {
        registerWith(quotes[i]);
    }
}

template <class Interpolator>
InterpolatedPriceCurve<Interpolator>::InterpolatedPriceCurve(const std::vector<QuantLib::Date>& dates,
                                                             const std::vector<QuantLib::Real>& prices,
                                                             const QuantLib::DayCounter& dc,
                                                             const Interpolator& interpolator)
    : PriceTermStructure(dates.at(0), QuantLib::NullCalendar(), dc), QuantLib::InterpolatedCurve<Interpolator>(
                                                                         std::vector<QuantLib::Time>(dates.size()),
                                                                         prices, interpolator),
      dates_(dates) {

    initialise();
}

template <class Interpolator>
InterpolatedPriceCurve<Interpolator>::InterpolatedPriceCurve(
    const std::vector<QuantLib::Date>& dates, const std::vector<QuantLib::Handle<QuantLib::Quote> >& quotes,
    const QuantLib::DayCounter& dc, const Interpolator& interpolator)
    : PriceTermStructure(dates.at(0), QuantLib::NullCalendar(), dc), QuantLib::InterpolatedCurve<Interpolator>(
                                                                         std::vector<QuantLib::Time>(dates.size()),
                                                                         std::vector<QuantLib::Real>(quotes.size()),
                                                                         interpolator),
      quotes_(quotes), dates_(dates) {

    initialise();

    // Observe the quotes
    for (QuantLib::Size i = 0; i < quotes_.size(); ++i) {
        registerWith(quotes[i]);
    }
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::update() {

    QuantLib::LazyObject::update();

    // TermStructure::update() update part
    if (moving_) {
        updated_ = false;
    }
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::performCalculations() const {
    // Calculations only need to be performed if the curve depends on quotes
    if (!quotes_.empty()) {
        getPricesFromQuotes();
        this->interpolation_.update();
    }
}

template <class Interpolator> QuantLib::Time InterpolatedPriceCurve<Interpolator>::maxTime() const {
    return this->times_.back();
}

template <class Interpolator> QuantLib::Time InterpolatedPriceCurve<Interpolator>::minTime() const {
    return this->times_.front();
}

template <class Interpolator> QuantLib::Real InterpolatedPriceCurve<Interpolator>::priceImpl(QuantLib::Time t) const {

    // Make sure interpolation is up to date
    QuantLib::LazyObject::calculate();

    // Return interpolated/extrapolated price
    return this->interpolation_(t, true);
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::initialise() {

    QL_REQUIRE(this->data_.size() >= Interpolator::requiredPoints, "not enough times for the interpolation method");

    // If we are dates based, populate times_ from dates_
    if (!this->dates_.empty()) {
        convertDatesToTimes();
    }

    // If we are quotes based, get prices from quotes
    if (!quotes_.empty()) {
        getPricesFromQuotes();
    }

    QL_REQUIRE(this->data_.size() == this->times_.size(), "Number of times must equal number of prices");
    QL_REQUIRE(boost::algorithm::is_sorted(this->times_.begin(), this->times_.end()), "Times must be sorted");
    QL_REQUIRE(*std::min_element(this->data_.begin(), this->data_.end()) >= 0.0, "Prices must be positive");

    QuantLib::InterpolatedCurve<Interpolator>::setupInterpolation();
    this->interpolation_.update();
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::convertDatesToTimes() {

    this->times_[0] = 0.0;
    for (QuantLib::Size i = 1; i < this->dates_.size(); ++i) {
        QL_REQUIRE(this->dates_[i] > this->dates_[i - 1],
                   "invalid date (" << this->dates_[i] << ", vs " << this->dates_[i - 1] << ")");
        this->times_[i] = dayCounter().yearFraction(this->dates_[0], this->dates_[i]);
        QL_REQUIRE(!QuantLib::close(this->times_[i], this->times_[i - 1]), "two dates correspond to the same time "
                                                                           "under this curve's day count convention");
    }
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::getPricesFromQuotes() const {

    for (QuantLib::Size i = 0; i < quotes_.size(); ++i) {
        QL_REQUIRE(!this->quotes_[i].empty(), "price quote at index " << i << " is empty");
        this->data_[i] = quotes_[i]->value();
    }
}

} // namespace QuantExt

#endif


#include <lwr_controllers/dynamic_sliding_mode_controller.h>
#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <math.h>
#include <Eigen/LU>
#include <utils/pseudo_inversion.h>

namespace lwr_controllers
{
	DynamicSlidingModeController::DynamicSlidingModeController() {}
	DynamicSlidingModeController::~DynamicSlidingModeController() {}

	bool DynamicSlidingModeController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
	{
        KinematicChainControllerBase<hardware_interface::EffortJointInterface>::init(robot, n);

		jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
		id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
		fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));

		joint_ref_.resize(kdl_chain_.getNrOfJoints());
		tau_.resize(kdl_chain_.getNrOfJoints());
		J_.resize(kdl_chain_.getNrOfJoints());
// 		PIDs_.resize(kdl_chain_.getNrOfJoints());
		M_.resize(kdl_chain_.getNrOfJoints());
		C_.resize(kdl_chain_.getNrOfJoints());
		G_.resize(kdl_chain_.getNrOfJoints());

		S_.resize(kdl_chain_.getNrOfJoints());
		S0_.resize(kdl_chain_.getNrOfJoints());
		Sd_.resize(kdl_chain_.getNrOfJoints());
		Sq_.resize(kdl_chain_.getNrOfJoints());
		Sr_.resize(kdl_chain_.getNrOfJoints());
		sigma_.resize(kdl_chain_.getNrOfJoints());
		sigma_dot_.resize(kdl_chain_.getNrOfJoints());

		Kd_.resize(kdl_chain_.getNrOfJoints());
		gamma_.resize(kdl_chain_.getNrOfJoints());
		alpha_.resize(kdl_chain_.getNrOfJoints());
		lambda_.resize(kdl_chain_.getNrOfJoints());
		k_.resize(kdl_chain_.getNrOfJoints());

		sub_command_ = nh_.subscribe("command", 1, &DynamicSlidingModeController::command, this);
// 		sub_gains_ = nh_.subscribe("set_gains", 1, &DynamicSlidingModeController::set_gains, this);

		pub_error_ = nh_.advertise<std_msgs::Float64MultiArray>("error", 1000);
		pub_pose_ = nh_.advertise<std_msgs::Float64MultiArray>("pose", 1000);
		pub_traj_ = nh_.advertise<std_msgs::Float64MultiArray>("traj", 1000);
		pub_marker_ = nh_.advertise<visualization_msgs::Marker>("marker",1000);

		return true;
	}

	void DynamicSlidingModeController::starting(const ros::Time& time)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    		joint_des_states_.q(i) = joint_msr_states_.q(i);
  			sigma_(i) = 0;

  			// coefficients > 0
  			Kd_(i) = 25;
  			gamma_(i) = 1.5;
  			alpha_(i) = 15;
  			//lambda_(i) = 5;
  			k_(i) = 1;
    	}

    	SetToZero(joint_des_states_);

    	Kp = 200;
    	Ki = 1;
    	Kd = 5;

//     	for (int i = 0; i < PIDs_.size(); i++)
//     		PIDs_[i].initPid(Kp,Ki,Kd,0.1,-0.1);
    	//ROS_INFO("PIDs gains are: Kp = %f, Ki = %f, Kd = %f",Kp,Ki,Kd);

    	cmd_flag_ = 0;
    	step_ = 0;
	}

	void DynamicSlidingModeController::update(const ros::Time& time, const ros::Duration& period)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++)
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	double freq_ = 1.0;

    	joint_des_states_.q(3) = sin(freq_/2*step_*period.toSec());
    	joint_des_states_.qdot(3) = freq_/2*cos(freq_/2*step_*period.toSec());
    	joint_des_states_.q(1) = sin(freq_*step_*period.toSec());
    	joint_des_states_.qdot(1) = freq_*cos(freq_*step_*period.toSec());

    	msg_pose_.data.clear();
    	msg_traj_.data.clear();

    	// updating messages for plot visualization
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		msg_pose_.data.push_back(joint_msr_states_.q(i));
    		msg_traj_.data.push_back(joint_des_states_.q(i));
    	}

    	// computing S
    	S_.data = (joint_msr_states_.qdot.data - joint_des_states_.qdot.data) + alpha_.data.cwiseProduct(joint_msr_states_.q.data - joint_des_states_.q.data);

		//for (int i = 0; i < joint_handles_.size(); i++)
			//S_(i) = (joint_msr_states_.qdot(i) - joint_des_states_.qdot(i)) + alpha_(i)*tanh(lambda_(i)*(joint_msr_states_.q(i) - joint_des_states_.q(i)));

    	// saving S0 on the first step
    	if (step_ == 0)
    		S0_ = S_;

    	// computing Sd
    	for (int i = 0; i < joint_handles_.size(); i++)
    		Sd_(i) = S0_(i)*exp(-k_(i)*(step_*period.toSec()));

    	Sq_.data = S_.data - Sd_.data;

    	// computing sigma_dot as sgn(Sq)
    	for (int i = 0; i < joint_handles_.size(); i++)
    		sigma_dot_(i) = -(Sq_(i) < 0) + (Sq_(i) > 0);

    	// integrating sigma_dot
    	sigma_.data += period.toSec()*sigma_dot_.data;

    	// computing Sr
    	Sr_.data = Sq_.data + gamma_.data.cwiseProduct(sigma_.data);

    	// computing tau
    	tau_.data = -Kd_.data.cwiseProduct(Sr_.data);

    	step_++;

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
	    	joint_handles_[i].setCommand(tau_(i));
    	}

    	// publishing markers for visualization in rviz
    	pub_marker_.publish(msg_marker_);
    	msg_id_++;

	    // publishing error for all tasks as an array of ntasks*6
	    pub_error_.publish(msg_err_);
	    // publishing actual and desired trajectory for each joint specified
	    pub_pose_.publish(msg_pose_);
	    pub_traj_.publish(msg_traj_);
	    ros::spinOnce();

	}

	void DynamicSlidingModeController::command(const std_msgs::Float64MultiArray::ConstPtr &msg)
	{

	}

// 	void DynamicSlidingModeController::set_gains(const std_msgs::Float64MultiArray::ConstPtr &msg)
// 	{
// 		if(msg->data.size() == 3)
// 		{
// 			for(int i = 0; i < PIDs_.size(); i++)
// 				PIDs_[i].setGains(msg->data[0],msg->data[1],msg->data[2],0.3,-0.3);
// 			ROS_INFO("New gains set: Kp = %f, Ki = %f, Kd = %f",msg->data[0],msg->data[1],msg->data[2]);
// 		}
// 		else
// 			ROS_INFO("PIDs gains needed are 3 (Kp, Ki and Kd)");
// 	}

	void DynamicSlidingModeController::set_marker(KDL::Frame x, int id)
	{
				msg_marker_.header.frame_id = "world";
				msg_marker_.header.stamp = ros::Time();
				msg_marker_.ns = "end_effector";
				msg_marker_.id = id;
				msg_marker_.type = visualization_msgs::Marker::SPHERE;
				msg_marker_.action = visualization_msgs::Marker::ADD;
				msg_marker_.pose.position.x = x.p(0);
				msg_marker_.pose.position.y = x.p(1);
				msg_marker_.pose.position.z = x.p(2);
				msg_marker_.pose.orientation.x = 0.0;
				msg_marker_.pose.orientation.y = 0.0;
				msg_marker_.pose.orientation.z = 0.0;
				msg_marker_.pose.orientation.w = 1.0;
				msg_marker_.scale.x = 0.005;
				msg_marker_.scale.y = 0.005;
				msg_marker_.scale.z = 0.005;
				msg_marker_.color.a = 1.0;
				msg_marker_.color.r = 0.0;
				msg_marker_.color.g = 1.0;
				msg_marker_.color.b = 0.0;
	}
}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::DynamicSlidingModeController, controller_interface::ControllerBase)