/***********************************************************************************
 * Copyright (c) 2016, UT-Battelle
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the <organization> nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Contributors:
 *   Initial API and implementation - Alex McCaskey
 *
 **********************************************************************************/
#ifndef XACC_UTILS_IGRAPH_HPP_
#define XACC_UTILS_IGRAPH_HPP_

#include <boost/graph/adjacency_list.hpp>
#include <type_traits>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/eccentricity.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <numeric>
#include "QCIError.hpp"
#include <utility>

using namespace boost;

namespace qci {
namespace common {

/**
 * Utility structs to help determine if
 * we have been given valid Vertices.
 */
template<typename T, typename = void>
struct is_valid_vertex: std::false_type {
};
template<typename T>
struct is_valid_vertex<T, decltype(std::declval<T>().properties, void())> : std::true_type {
};

/**
 * The base class of all QCI Vertices for the
 * QCI Common Graph class. All Vertices must keep
 * track of a set of properties, stored as a tuple.
 */
template<typename ... Properties>
class QCIVertex {
public:
	std::tuple<Properties...> properties;
	std::vector<std::string> propertyNames;
	QCIVertex() : propertyNames(sizeof...(Properties)) {}
	std::string getPropertyName(const int index) {
		return propertyNames[index];
	}
};

/**
 * For now, we only allow Edges with weight property.
 */
struct DefaultEdge {
	double weight = 0.0;
};

enum GraphType {
	Undirected, Directed
};

// Overload stream operator for vector
template<class T>
std::ostream& operator << (std::ostream& os, const std::vector<T>& v) {
	os << "[";
	for (std::size_t i = 0; i < v.size(); i++) {
		os << v[i] << (i == v.size() - 1 ? "" : " ");
	}
	os << "]";
	return os;
}

template<std::size_t> struct int_{};

template <class Tuple, size_t Pos>
std::ostream& print_tuple(std::ostream& out, const Tuple& t, int_<Pos> ) {
  out << std::get< std::tuple_size<Tuple>::value-Pos >(t) << ',';
  return print_tuple(out, t, int_<Pos-1>());
}

template <class Tuple>
std::ostream& print_tuple(std::ostream& out, const Tuple& t, int_<1> ) {
  return out << std::get<std::tuple_size<Tuple>::value-1>(t);
}

template <class... Args>
std::ostream& operator<<(std::ostream& out, const std::tuple<Args...>& t) {
  out << std::string(" [label=\"");
  print_tuple(out, t, int_<sizeof...(Args)>());
  return out << "\"]";
}

/**
 * The Graph class provides a generic data structure modeling
 * mathematical graph structures. It is templated on the vertex
 * type, allowing for graphs with a wide variety of graph nodes
 * (for example, in quantum computing - graph of tensors, graph of
 * Ising parameters, etc.)
 *
 * All provided Vertex types must be a subclass of the
 * QCIVertex in order to properly provide a tuple of vertex
 * properties.
 * s
 */
template<typename Vertex, GraphType type = Undirected>
class Graph {

	// Make sure we've been given a valid Vertex
	static_assert(is_valid_vertex<Vertex>::value, "QCI Graph can only take Vertices that are derived from QCIVertex, or have a tuple properties member.");

	using graph_type = typename std::conditional<(type == GraphType::Undirected), undirectedS, directedS>::type;

	// Setup some easy to use aliases
	using adj_list = adjacency_list<vecS, vecS, graph_type, Vertex, DefaultEdge>;
	using BoostGraph = std::shared_ptr<adj_list>;
	using vertex_type = typename boost::graph_traits<adjacency_list<vecS, vecS, undirectedS, Vertex, DefaultEdge>>::vertex_descriptor;
	using edge_type = typename boost::graph_traits<adjacency_list<vecS, vecS, undirectedS, Vertex, DefaultEdge>>::edge_descriptor;

protected:

	/**
	 * This is a custom utility class for writing
	 * QCIVertices with user-defined properties.
	 */
	class QCIVertexPropertiesWriter {
	protected:
		adj_list graph;
	public:
		QCIVertexPropertiesWriter(adj_list& list) :
				graph(list) {
		}
		template<class BoostVertex>
		void operator()(std::ostream& out, const BoostVertex& v) const {
			auto node = vertex(v, graph);
			std::stringstream ss;
			ss << graph[node].properties;

			// Now we have a string of comma separated parameters
			std::string result;
			auto vertexString = ss.str();
			boost::trim(vertexString);
			std::vector<std::string> splitVec;
			boost::split(splitVec, vertexString, boost::is_any_of(","));

			int counter = 0;
			for (std::size_t i = 0; i < splitVec.size(); i++) {
				if (!graph[node].propertyNames[counter].empty()) {
					auto s = splitVec[i];
					if (i == 0) {
						s.insert(8, graph[node].propertyNames[counter] + "=");
					} else {
						s.insert(0, graph[node].propertyNames[counter] + "=");
					}
					counter++;
					result += s + ",";
				}
			}

			result = result.substr(0,result.size() - 1);
			if (result.substr(result.size() - 2, result.size()) != "\"]") {
				result += "\"]";
			}
			out << " " << result;
		}
	};

	/**
	 * The actual graph data structure we are
	 * delegating to.
	 */
	BoostGraph _graph;

public:

	/**
	 * The nullary constructor
	 */
	Graph() {
		_graph = std::make_shared<adj_list>();
	}

	/**
	 * The constructor, constructs a graph with
	 * specified number of vertices.
	 *
	 * @param numberOfVertices The number of vertices
	 */
	Graph(const int numberOfVertices) {
		_graph = std::make_shared<adj_list>(
				numberOfVertices);
	}

	/**
	 * Add an edge between the vertices with given provided
	 * indices and edge weight.
	 *
	 * @param srcIndex Index of the starting vertex
	 * @param tgtIndex Index of the ending vertex
	 * @param edgeWeight The edge weight
	 */
	void addEdge(const int srcIndex, const int tgtIndex, const double edgeWeight) {
		auto edgeBoolPair = add_edge(vertex(srcIndex, *_graph.get()),
				vertex(tgtIndex, *_graph.get()), *_graph.get());
		if (!edgeBoolPair.second) {

		}
		(*_graph.get())[edgeBoolPair.first].weight = edgeWeight;
	}

	/**
	 * Add an edge with default edge weight between the
	 * vertices at the provided indices.
	 *
	 * @param srcIndex Index of the starting vertex
	 * @param tgtIndex Index of the ending vertex
	 */
	void addEdge(const int srcIndex, const int tgtIndex) {
		add_edge(vertex(srcIndex, *_graph.get()),
				vertex(tgtIndex, *_graph.get()), *_graph.get());
	}

	/**
	 * Add a vertex to this Graph.
	 */
	void addVertex() {
		add_vertex(*_graph.get());
	}

	/**
	 * Add a vertex to this graph with the provided properties.
	 * s
	 * @param properties
	 */
	template<typename ... Properties>
	void addVertex(Properties ... properties) {
		auto v = add_vertex(*_graph.get());
		(*_graph.get())[v].properties = std::make_tuple(properties...);
	}

	void addVertex(Vertex& vertex) {
		auto v = add_vertex(*_graph.get());
		(*_graph.get())[v].properties = vertex.properties;
	}

	/**
	 * Set an existing vertices properties.
	 *
	 * @param index The index of the vertex
	 * @param properties The new properties for the vertex
	 */
	template<typename... Properties>
	void setVertexProperties(const int index, Properties... properties) {
		auto v = vertex(index, *_graph.get());
		(*_graph.get())[v].properties = std::make_tuple(properties...);
	}

	/**
	 * Set a specific vertex property for the vertex at given index.
	 *
	 * @param index The index of the vertex
	 * @param prop The property to set.
	 */
	template<const int PropertyIndex>
	void setVertexProperty(const int index,
			decltype(std::get<PropertyIndex>(std::declval<Vertex>().properties)) prop) {
		auto v = vertex(index, *_graph.get());
		std::get<PropertyIndex>((*_graph.get())[v].properties) = prop;
		return;
	}

	Vertex& getVertex(const int index) {
		auto v = vertex(index, *_graph.get());
		return (*_graph.get())[v];
	}

	auto getVertexProperties(const int index) -> decltype((*_graph.get())[index].properties) {
		return (*_graph.get())[index].properties;
	}

	/**
	 * Return the vertex property of the vertex
	 * at the given index and at the provided
	 * valid vertex property template index.
	 *
	 * @param index The index of the vertex
	 * @return property The property value.
	 */
	template<const int PropertyIndex>
	decltype(std::get<PropertyIndex>(std::declval<Vertex>().properties))& getVertexProperty(
			const int index) {
		auto v = vertex(index, *_graph.get());
		return std::get<PropertyIndex>((*_graph.get())[v].properties);
	}

	/**
	 * Set the weight on the edge between the vertices at the
	 * provided indices.
	 *
	 * @param srcIndex The starting vertex index
	 * @param tgtIndex The ending vertex index
	 * @param weight The weight to set.
	 */
	void setEdgeWeight(const int srcIndex, const int tgtIndex, const double weight) {
		auto e = edge(vertex(srcIndex, *_graph.get()), vertex(tgtIndex, *_graph.get()), *_graph.get());
		(*_graph.get())[e.first].weight = weight;
	}

	/**
	 * Return the edge weight at the edge between
	 * the provided vertices.
	 *
	 * @param srcIndex The starting vertex index
	 * @param tgtIndex The ending vertex index
	 * @return The edge weight
	 */
	double getEdgeWeight(const int srcIndex, const int tgtIndex) {
		auto e = edge(vertex(srcIndex, *_graph.get()), vertex(tgtIndex, *_graph.get()), *_graph.get());
		return (*_graph.get())[e.first].weight;
	}

	/**
	 * Return true if there is an edge between the
	 * two vertices at the given vertex indices.
	 *
	 * @param srcIndex The starting vertex index
	 * @param tgtIndex The ending vertex index
	 * @return exists Boolean indicating if edge exists or not
	 */
	bool edgeExists(const int srcIndex, const int tgtIndex) {
		return edge(vertex(srcIndex, *_graph.get()),
				vertex(tgtIndex, *_graph.get()), *_graph.get()).second;
	}

	/**
	 * Return the vertex degree at the given vertex index.
	 *
	 * @param index The index of the vertex
	 * @return degree The degree of the vertex
	 */
	int degree(const int index) {
		return boost::degree(vertex(index, *_graph.get()), *_graph.get());
	}

	/**
	 * Return the diameter of this Graph.
	 *
	 * @return diameter The graph diameter
	 */
	int diameter() {
		// Set some aliases we'll need
		using DistanceProperty = boost::exterior_vertex_property<adj_list, int>;
		using DistanceMatrix = typename DistanceProperty::matrix_type;
		using DistanceMatrixMap = typename DistanceProperty::matrix_map_type;
		using EccentricityProperty = boost::exterior_vertex_property<adj_list, int>;
		using EccentricityContainer = typename EccentricityProperty::container_type;
		using EccentricityMap = typename EccentricityProperty::map_type;

		// Construct the distance mapping
		DistanceMatrix distances(order());
		DistanceMatrixMap dm(distances, *_graph.get());
		constant_property_map<edge_type, int> wm(1);

		// Compute the shortest paths
		floyd_warshall_all_pairs_shortest_paths(*_graph.get(), dm, weight_map(wm));

		// Now get diameter information
		EccentricityContainer eccs(order());
		EccentricityMap em(eccs, *_graph.get());

		// Return the diameter
		return all_eccentricities(*_graph.get(), dm, em).second;
	}

	/**
	 * Return the number of edges.
	 * @return nEdges The number of edges.
	 */
	int size() {
		return num_edges(*_graph.get());
	}

	/**
	 * Return the number of vertices in this graph
	 *
	 * @return nVerts The number of vertices.
	 */
	int order() {
		return num_vertices(*_graph.get());
	}

	/**
	 * Write this graph in a graphviz dot format to the
	 * provided ostream.
	 *
	 * @param stream
	 */
	void write(std::ostream& stream) {
		std::stringstream ss;
		QCIVertexPropertiesWriter writer(*_graph.get());
		boost::write_graphviz(ss, *_graph.get(), writer);
		auto str = ss.str();
		// Modify the style...
		str = str.insert(9, "\n{\nnode [shape=box style=filled]");

		std::vector<std::string> splitVec;
		boost::split(splitVec, str, boost::is_any_of("\n"));
		splitVec.insert(splitVec.begin() + 3 + order(), "}");

		std::stringstream combine;
		std::for_each(splitVec.begin(), splitVec.end(), [&](const std::string& elem) { combine << elem << "\n"; });
		stream << combine.str().substr(0, combine.str().size() - 2);
	}

	/**
	 * Read in a graphviz dot graph from the given input
	 * stream. This is left for subclasses.
	 *
	 * @param stream
	 */
	virtual void read(std::istream& stream) {
		stream.fail();
		QCIError("Reading must be implemented by subclasses.");
	}
};

}
}

#endif


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_STATE_BASE_HPP_
#define CROCODDYL_CORE_STATE_BASE_HPP_

#include <Eigen/Core>
#include <vector>
#include <string>

namespace crocoddyl {

enum Jcomponent { both = 0, first = 1, second = 2 };

inline bool is_a_Jcomponent(Jcomponent firstsecond) {
  return (firstsecond == first || firstsecond == second || firstsecond == both);
}

class StateAbstract {
 public:
  StateAbstract(unsigned int const& nx, unsigned int const& ndx);
  virtual ~StateAbstract();

  virtual Eigen::VectorXd zero() = 0;
  virtual Eigen::VectorXd rand() = 0;
  virtual void diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
                    Eigen::Ref<Eigen::VectorXd> dxout) = 0;
  virtual void integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
                         Eigen::Ref<Eigen::VectorXd> xout) = 0;
  virtual void Jdiff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
                     Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
                     Jcomponent firstsecond = both) = 0;
  virtual void Jintegrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
                          Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
                          Jcomponent firstsecond = both) = 0;

  const unsigned int& get_nx() const;
  const unsigned int& get_ndx() const;
  const unsigned int& get_nq() const;
  const unsigned int& get_nv() const;

 protected:
  unsigned int nx_;
  unsigned int ndx_;
  unsigned int nq_;
  unsigned int nv_;

#ifdef PYTHON_BINDINGS

 public:
  Eigen::VectorXd diff_wrap(const Eigen::VectorXd& x0, const Eigen::VectorXd& x1) {
    Eigen::VectorXd dxout = Eigen::VectorXd::Zero(ndx_);
    diff(x0, x1, dxout);
    return dxout;
  }
  Eigen::VectorXd integrate_wrap(const Eigen::VectorXd& x, const Eigen::VectorXd& dx) {
    Eigen::VectorXd xout = Eigen::VectorXd::Zero(nx_);
    integrate(x, dx, xout);
    return xout;
  }
  std::vector<Eigen::MatrixXd> Jdiff_wrap(const Eigen::VectorXd& x0, const Eigen::VectorXd& x1,
                                          std::string firstsecond = "both") {
    Eigen::MatrixXd Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
    std::vector<Eigen::MatrixXd> Jacs;
    if (firstsecond == "both") {
      Jdiff(x0, x1, Jfirst, Jsecond, both);
      Jacs.push_back(Jfirst);
      Jacs.push_back(Jsecond);
    } else if (firstsecond == "first") {
      Jdiff(x0, x1, Jfirst, Jsecond, first);
      Jacs.push_back(Jfirst);
    } else if (firstsecond == "second") {
      Jdiff(x0, x1, Jfirst, Jsecond, second);
      Jacs.push_back(Jsecond);
    } else {
      Jdiff(x0, x1, Jfirst, Jsecond, both);
      Jacs.push_back(Jfirst);
      Jacs.push_back(Jsecond);
    }
    return Jacs;
  }
  std::vector<Eigen::MatrixXd> Jintegrate_wrap(const Eigen::VectorXd& x, const Eigen::VectorXd& dx,
                                               std::string firstsecond = "both") {
    Eigen::MatrixXd Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
    std::vector<Eigen::MatrixXd> Jacs;
    if (firstsecond == "both") {
      Jintegrate(x, dx, Jfirst, Jsecond, both);
      Jacs.push_back(Jfirst);
      Jacs.push_back(Jsecond);
    } else if (firstsecond == "first") {
      Jintegrate(x, dx, Jfirst, Jsecond, first);
      Jacs.push_back(Jfirst);
    } else if (firstsecond == "second") {
      Jintegrate(x, dx, Jfirst, Jsecond, second);
      Jacs.push_back(Jsecond);
    } else {
      Jintegrate(x, dx, Jfirst, Jsecond, both);
      Jacs.push_back(Jfirst);
      Jacs.push_back(Jsecond);
    }
    return Jacs;
  }
#endif
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_STATE_BASE_HPP_


// Boost.Convert test and usage example
// Copyright (c) 2009-2014 Vladimir Batov.
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. See http://www.boost.org/LICENSE_1_0.txt.

#include "./test.hpp"

#ifdef BOOST_CONVERT_INTEL_SFINAE_BROKEN
int main(int, char const* []) { return 0; }
#else

#include <boost/convert.hpp>
#include <boost/convert/printf.hpp>
#include <boost/detail/lightweight_test.hpp>

using std::string;
using boost::convert;

namespace arg = boost::cnv::parameter;

int
main(int argc, char const* argv[])
{
    boost::cnv::printf cnv;

    string const not_int_str = "not an int";
    string const     std_str = "-11";
    char const* const  c_str = "-12";

    BOOST_TEST( -1 == convert<int>(not_int_str, cnv).value_or(-1));
    BOOST_TEST(-11 == convert<int>(std_str,     cnv).value_or(-1));
    BOOST_TEST(-12 == convert<int>(c_str,       cnv).value_or(-1));

    BOOST_TEST("255" == convert<std::string>(255, cnv(arg::base = boost::cnv::base::dec)).value());
    BOOST_TEST( "ff" == convert<std::string>(255, cnv(arg::base = boost::cnv::base::hex)).value());
    BOOST_TEST("377" == convert<std::string>(255, cnv(arg::base = boost::cnv::base::oct)).value());

    string s01 = convert<string>(12.3456, cnv(arg::precision = 6)).value();
    string s02 = convert<string>(12.3456, cnv(arg::precision = 3)).value();

    BOOST_TEST(s01 == "12.345600");
    BOOST_TEST(s02 == "12.346");

    return boost::report_errors();
}

#endif


#ifndef SWIFT_VIO_TEST_SIMULATED_MOTION_FOR_PARALLAX_ANGLE_TEST_HPP_
#define SWIFT_VIO_TEST_SIMULATED_MOTION_FOR_PARALLAX_ANGLE_TEST_HPP_

#include <Eigen/Core>
#include <swift_vio/DirectionFromParallaxAngleJacobian.hpp>
#include <swift_vio/VectorOperations.hpp>
#include <swift_vio/VectorNormalizationJacobian.hpp>

namespace simul {
enum MotionType {
  Sideways = 0, // R_WC = I3
  Sideways_R_WC = 1, // R_WC != I3
  Forward,
  Backward,
  Mixed,
  AssociateObserver,
  Rotation,
  AntiRotation,
  ForwardWithCenterPoint,
};

std::ostream& operator<<(std::ostream& out, const MotionType value);

/**
 * @brief RotY If we rotate a coordinate frame A about its y axis for theta
 *    to obtain a coordinate frame B, then the coordinates of a point in B p_B
 *    and its coordinates in A are related by p_A = RotY * p_B.
 * @param theta
 * @return
 */
Eigen::Matrix3d RotY(double theta);

typedef std::vector<okvis::kinematics::Transformation,
Eigen::aligned_allocator<okvis::kinematics::Transformation>> TransformationList;

class SimulatedMotionForParallaxAngleTest {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  enum ProjectionListStatus {
    Healthy = 0,
    MainAnchorFailed = 1,
    AssociateAnchorFailed = 2,
  };

  SimulatedMotionForParallaxAngleTest(MotionType motion,
                                      bool addOutlierObservation);

  void dN_dp_WCmi(Eigen::Matrix3d* j) const;

  void dN_dtheta_WCmi(Eigen::Matrix3d* j) const;

  void dN_dp_WCai(Eigen::Matrix3d* j) const;

  void dN_dp_WCtij(Eigen::Matrix3d* j) const;

  void dN_dni(Eigen::Matrix<double, 3, 2>* j) const;

  void dN_dthetai(Eigen::Vector3d* j) const;

  const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>&
  observations() const {
    return observationsxy1_;
  }

  const std::vector<okvis::kinematics::Transformation,
                    Eigen::aligned_allocator<okvis::kinematics::Transformation>>&
  T_WC_list() const {
    return T_WC_list_;
  }

  std::vector<size_t> anchorIndices() const {
    return anchorObservationIndices_;
  }

  const swift_vio::ParallaxAnglePoint& pap()
      const {
    return pap_;
  }

  swift_vio::DirectionFromParallaxAngleJacobian dfpaj_;
  ProjectionListStatus observationListStatus_;

 private:
  double computeCosParallaxAngle() const {
    return computeCosParallaxAngle(pW_, T_WCm_.r(), T_WCa_.r());
  }

  double computeCosParallaxAngle(const Eigen::Vector3d& pW,
                                 const Eigen::Vector3d& t_WCm,
                                 const Eigen::Vector3d& t_WCa) const;

  bool computeObservationXY1(const Eigen::Vector3d pW,
                             const okvis::kinematics::Transformation& T_WC,
                             Eigen::Vector3d* xy1, bool addNoise);

  void simulateSidewaysMotion(double a, bool addOutlier);

  okvis::kinematics::Transformation T_WCm_;
  okvis::kinematics::Transformation T_WCa_;
  okvis::kinematics::Transformation T_WCj_;
  Eigen::Vector3d pW_;
  double cosParallaxAngle_;
  swift_vio::ParallaxAnglePoint pap_;
  Eigen::Vector3d Nij_;
  const MotionType motionType_;

  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
      observationsxy1_;
  TransformationList T_WC_list_;
  std::vector<size_t> anchorObservationIndices_;
};
}
#endif // SWIFT_VIO_TEST_SIMULATED_MOTION_FOR_PARALLAX_ANGLE_TEST_HPP_


//----------------------------------------------------------------------------
/// @file benchmark_numbers.cpp
/// @brief Benchmark of several sort methods with integer objects
///
/// @author Copyright (c) 2017 Francisco José Tapia (fjtapia@gmail.com )\n
///         Distributed under the Boost Software License, Version 1.0.\n
///         ( See accompanying file LICENSE_1_0.txt or copy at
///           http://www.boost.org/LICENSE_1_0.txt )
///
///         This program use for comparison purposes, the Threading Building
///         Blocks which license is the GNU General Public License, version 2
///         as  published  by  the  Free Software Foundation.
///
/// @version 0.1
///
/// @remarks
//-----------------------------------------------------------------------------
#include <algorithm>

#include <iostream>
#include <iomanip>
#include <random>
#include <stdlib.h>
#include <vector>

#include <boost/sort/common/time_measure.hpp>
#include <boost/sort/common/file_vector.hpp>
#include <boost/sort/common/int_array.hpp>

#include <boost/sort/sort.hpp>

#include "BlockQuicksort/blocked_double_pivot_check_mosqrt.h++"

extern "C" {
  #include "introsort.h"
}

#define NELEM 10000000
#define NITER 10

using namespace std;
namespace bsort = boost::sort;
namespace bsc = boost::sort::common;

using bsc::time_point;
using bsc::now;
using bsc::subtract_time;
using bsc::fill_vector_uint64;
using bsc::write_file_uint64;

using bsort::spinsort;
using bsort::flat_stable_sort;
using bsort::spreadsort::spreadsort;
using bsort::pdqsort;
using bsort::pdqsort_branchless;

template<typename A> double time_op(A op) {
  auto start = now ();
  op();
  auto finish = now ();
  return subtract_time (finish, start);
}

template<typename A> void time_op(std::string name, A op) {
  auto start = now ();
  op();
  auto finish = now ();
  auto duration = subtract_time (finish, start);

  std::cout<<"  "<<name<<"."<<"time: "<<duration<<std::endl;
}

namespace annot {

  template<class Iter, class Compare>
  inline void pdqsort(Iter first, Iter last, Compare comp) {
      if (first == last) return;
      boost::sort::pdqsort_detail::pdqsort_loop<Iter, Compare,
          false
          >(
          first, last, comp, boost::sort::pdqsort_detail::log2(last - first));
  }

  double insort_duration=0;

  void ll_introsort(uint64_t *a, int64_t l, int64_t h) {
    introsort_aux(a,l,h,std::__lg(h-l) * 2+100);
    insort_duration+=time_op([&]{insertion_sort(a,l,h);});
  }

  uint64_t cnt_dlimit=0;
  uint64_t cnt_intro_loops=0;
  uint64_t cnt_partitionings=0;
  uint64_t cnt_partitions=0;

  void print_stats() {
    std::cout<<"**** STL ***"<<std::endl;
    std::cout<<"hit depth limit: "<<cnt_dlimit<<std::endl;
    std::cout<<"intro loops: "<<cnt_intro_loops<<std::endl;
    std::cout<<"partitionings: "<<cnt_partitionings<<std::endl;
    std::cout<<"partitions: "<<cnt_partitions<<std::endl;
  }

  /// This is a helper function for the sort routine.
  template<typename _RandomAccessIterator, typename _Compare>
    void
    __guarded_linear_insert(_RandomAccessIterator __first, _RandomAccessIterator __last,
			      _Compare __comp)
    {
      typename iterator_traits<_RandomAccessIterator>::value_type
	__val = _GLIBCXX_MOVE(*__last);
      _RandomAccessIterator __i = __last;
      while (__i != __first && __comp(__val, __i-1))
	{
	  *__i = _GLIBCXX_MOVE(*(__i-1));
	  --__i;
	}
      *__i = _GLIBCXX_MOVE(__val);
    }

  /// This is a helper function for the sort routine.
  template<typename _RandomAccessIterator, typename _Compare>
    void
    __insertion_sort(_RandomAccessIterator __first,
		     _RandomAccessIterator __last, _Compare __comp)
    {
      if (__first == __last) return;

      for (_RandomAccessIterator __i = __first + 1; __i != __last; ++__i)
	{
// 	  if (__comp(__i, __first))
// 	    {
// 	      typename iterator_traits<_RandomAccessIterator>::value_type
// 		__val = _GLIBCXX_MOVE(*__i);
// 	      _GLIBCXX_MOVE_BACKWARD3(__first, __i, __i + 1);
// 	      *__first = _GLIBCXX_MOVE(__val);
// 	    }
// 	  else
	    annot::__guarded_linear_insert(__first, __i,
				__gnu_cxx::__ops::__val_comp_iter(__comp));
	}
    }

  template<typename _RandomAccessIterator, typename _Compare>
    inline void
    insort(_RandomAccessIterator __first, _RandomAccessIterator __last,
	   _Compare __comp)
    {
      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
	    _RandomAccessIterator>)
      __glibcxx_requires_valid_range(__first, __last);
      __glibcxx_requires_irreflexive_pred(__first, __last, __comp);

      typedef __decltype(__comp) _Cmp;
      __gnu_cxx::__ops::_Iter_comp_iter<_Cmp> __cmp(_GLIBCXX_MOVE(__comp));

      if (__first != __last)
	{
	  annot::__insertion_sort(__first, __last, __cmp);
	}
    }

  /// This is a helper function for the sort routine.
  template<typename _RandomAccessIterator, typename _Size, typename _Compare>
    void
    __introsort_loop(_RandomAccessIterator __first,
		     _RandomAccessIterator __last,
		     _Size __depth_limit, _Compare __comp)
    {
      while (__last - __first > int(std::_S_threshold))
	{
          ++cnt_intro_loops;
	  if (__depth_limit == 0)
	    {
              ++cnt_dlimit;
	      std::__partial_sort(__first, __last, __last, __comp);
	      return;
	    }
	  --__depth_limit;
//           print_range(__first,__last);
	  _RandomAccessIterator __cut =
	    std::__unguarded_partition_pivot(__first, __last, __comp);
//           print_range(__first,__last);
          ++cnt_partitionings;
	  annot::__introsort_loop(__cut, __last, __depth_limit, __comp);
	  __last = __cut;
	}

      ++cnt_partitions;
    }

  // sort

  template<typename _RandomAccessIterator, typename _Compare>
    inline void
    __sort(_RandomAccessIterator __first, _RandomAccessIterator __last,
	   _Compare __comp)
    {
      if (__first != __last)
	{
	  std::__introsort_loop(__first, __last,
				std::__lg(__last - __first) * 2,
				__comp);
	  insort_duration+=time_op([&]{std::__final_insertion_sort(__first, __last, __comp);});
	}
    }

  template<typename _RandomAccessIterator, typename _Compare>
    inline void
    __sort2(_RandomAccessIterator __first, _RandomAccessIterator __last,
	   _Compare __comp)
    {
      if (__first != __last)
	{
	  std::__introsort_loop(__first, __last,
				std::__lg(__last - __first) * 2,
				__comp);
// 	  insort_duration+=time_op([&]{std::__final_insertion_sort(__first, __last, __comp);});
	  insort_duration+=time_op([&]{annot::__insertion_sort(__first, __last, __comp);});
	}
    }

  template<typename _RandomAccessIterator>
    inline void
    sort(_RandomAccessIterator __first, _RandomAccessIterator __last)
    {
      // concept requirements
      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
	    _RandomAccessIterator>)
      __glibcxx_function_requires(_LessThanComparableConcept<
	    typename iterator_traits<_RandomAccessIterator>::value_type>)
      __glibcxx_requires_valid_range(__first, __last);
      __glibcxx_requires_irreflexive(__first, __last);

      annot::__sort(__first, __last, __gnu_cxx::__ops::__iter_less_iter());
    }

  template<typename _RandomAccessIterator, typename _Compare>
    inline void
    sort(_RandomAccessIterator __first, _RandomAccessIterator __last,
	 _Compare __comp)
    {
      // concept requirements
      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
	    _RandomAccessIterator>)
      __glibcxx_function_requires(_BinaryPredicateConcept<_Compare,
	    typename iterator_traits<_RandomAccessIterator>::value_type,
	    typename iterator_traits<_RandomAccessIterator>::value_type>)
      __glibcxx_requires_valid_range(__first, __last);
      __glibcxx_requires_irreflexive_pred(__first, __last, __comp);

      annot::__sort(__first, __last, __gnu_cxx::__ops::__iter_comp_iter(__comp));
    }

  template<typename _RandomAccessIterator, typename _Compare>
    inline void
    sort2(_RandomAccessIterator __first, _RandomAccessIterator __last,
	 _Compare __comp)
    {
      // concept requirements
      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
	    _RandomAccessIterator>)
      __glibcxx_function_requires(_BinaryPredicateConcept<_Compare,
	    typename iterator_traits<_RandomAccessIterator>::value_type,
	    typename iterator_traits<_RandomAccessIterator>::value_type>)
      __glibcxx_requires_valid_range(__first, __last);
      __glibcxx_requires_irreflexive_pred(__first, __last, __comp);

      annot::__sort2(__first, __last, __gnu_cxx::__ops::__iter_comp_iter(__comp));
    }

}

template<typename RandomAccessIterator, typename Compare>
  inline void
  std_heapsort(RandomAccessIterator first, RandomAccessIterator last,
        Compare comp)
  {
    std::make_heap(first,last,comp);
    std::sort_heap(first,last,comp);

  }

namespace testbp {

  vector<uint64_t>::iterator l0;

  /// This is a helper function...
  template<typename _RandomAccessIterator, typename _Compare>
    _RandomAccessIterator
    __unguarded_partition2(_RandomAccessIterator __first,
			  _RandomAccessIterator __last,
			  _RandomAccessIterator __pivot, _Compare __comp)
    {
      while (__comp(__first, __pivot))
        ++__first;
      --__last;
      while (__comp(__pivot, __last))
        --__last;

      while ((__first < __last))
	{
	  std::iter_swap(__first, __last);
	  ++__first;

	  while (__comp(__first, __pivot))
	    ++__first;
	  --__last;
	  while (__comp(__pivot, __last))
	    --__last;
	}

      return __first;
    }

  template<typename _RandomAccessIterator, typename _Compare>
    _RandomAccessIterator
    __unguarded_partition(_RandomAccessIterator __first,
			  _RandomAccessIterator __last,
			  _RandomAccessIterator __pivot, _Compare __comp)
    {
      while (true)
	{
	  while (__comp(__first, __pivot))
	    ++__first;
	  --__last;
	  while (__comp(__pivot, __last))
	    --__last;
	  if (!(__first < __last))
	    return __first;
	  std::iter_swap(__first, __last);
	  ++__first;
	}
    }

  template<typename _RandomAccessIterator, typename _Compare>
    _RandomAccessIterator
    __unguarded_partition3(_RandomAccessIterator __first,
			  _RandomAccessIterator __last,
			  _RandomAccessIterator __pivot, _Compare __comp)
    {
      bool brk=false;
      while (!brk)
	{
	  while (__comp(__first, __pivot))
	    ++__first;
	  --__last;
	  while (__comp(__pivot, __last))
	    --__last;
	  if (!(__first < __last))
	    brk=true;
          else {
            std::iter_swap(__first, __last);
            ++__first;
          }
	}

      return __first;
    }

  /// This is a helper function...
  template<typename _RandomAccessIterator, typename _Compare>
    inline _RandomAccessIterator
    __unguarded_partition_pivot(_RandomAccessIterator __first,
				_RandomAccessIterator __last, _Compare __comp)
    {
      _RandomAccessIterator __mid = __first + (__last - __first) / 2;
      std::__move_median_to_first(__first, __first + 1, __mid, __last - 1,
				  __comp);
       return testbp::__unguarded_partition3(__first + 1, __last, __first, __comp);
    }

  /// This is a helper function for the sort routine.
  template<typename _RandomAccessIterator, typename _Size, typename _Compare>
    void
    __introsort_loop(_RandomAccessIterator __first,
		     _RandomAccessIterator __last,
		     _Size __depth_limit, _Compare __comp)
    {
      while (__last - __first > int(_S_threshold))
	{
	  if (__depth_limit == 0)
	    {
	      std::__partial_sort(__first, __last, __last, __comp);
	      return;
	    }
	  --__depth_limit;
	  _RandomAccessIterator __cut =
	  testbp::__unguarded_partition_pivot(__first, __last, __comp);
	  testbp::__introsort_loop(__cut, __last, __depth_limit, __comp);
	  __last = __cut;
	}
    }

  template<typename I, typename S, typename C> void introsort_alt(I l, I h, S d, C comp) {
    if (h-l > (int)(_S_threshold)) {
      if (d==0) {
        std::__partial_sort(l,h,h,comp);
      } else {
        --d;

        I m = testbp::__unguarded_partition_pivot(l,h,comp);

//         std::iter_swap(l,m-1);

        introsort_alt(l,m,d,comp);
        introsort_alt(m,h,d,comp);
      }
    }
  }

  // sort

  template<typename _RandomAccessIterator, typename _Compare>
    void
    __unguarded_linear_insert(_RandomAccessIterator __last,
			      _Compare __comp)
    {
      typename iterator_traits<_RandomAccessIterator>::value_type
	__val = _GLIBCXX_MOVE(*__last);
      _RandomAccessIterator __next = __last;
      --__next;
      while (__comp(__val, __next))
	{
	  *__last = _GLIBCXX_MOVE(*__next);
	  __last = __next;
	  --__next;
	}
      *__last = _GLIBCXX_MOVE(__val);
    }

  template<typename I, typename C> void lin_insert(I l, I h, C comp) {
    auto tmp=*h;
    while (l<h && comp(tmp,h-1)) {
      *h=*(h-1);
      --h;
    }
    *h=tmp;
  }

  /// This is a helper function for the sort routine.
  template<typename _RandomAccessIterator, typename _Compare>
    void
    __insertion_sort(_RandomAccessIterator __first,
		     _RandomAccessIterator __last, _Compare __comp)
    {
      if (__first == __last) return;

      for (_RandomAccessIterator __i = __first + 1; __i != __last; ++__i)
	{

          testbp::lin_insert(__first,__i,__gnu_cxx::__ops::__val_comp_iter(__comp));

/*
	  if (__comp(__i, __first))
	    {
	      typename iterator_traits<_RandomAccessIterator>::value_type
		__val = _GLIBCXX_MOVE(*__i);
	      _GLIBCXX_MOVE_BACKWARD3(__first, __i, __i + 1);
	      *__first = _GLIBCXX_MOVE(__val);
	    }
	  else
	    testbp::__unguarded_linear_insert(__i,
				__gnu_cxx::__ops::__val_comp_iter(__comp));*/

	}
    }

  template<typename _RandomAccessIterator, typename _Compare>
    inline void
    __sort(_RandomAccessIterator __first, _RandomAccessIterator __last,
	   _Compare __comp)
    {
      if (__first != __last)
	{
// 	  testbp::__introsort_loop(__first, __last,
// 				std::__lg(__last - __first) * 2,
// 				__comp);
	  testbp::introsort_alt(__first, __last,
				std::__lg(__last - __first) * 2,
				__comp);

	  testbp::__insertion_sort(__first, __last, __comp);
// 	  testbp::__insertion_sort(__first, __last, __comp);
// 	  testbp::__insertion_sort(__first, __last, __comp);
// 	  testbp::__insertion_sort(__first, __last, __comp);
// 	  testbp::__insertion_sort(__first, __last, __comp);
// 	  testbp::__insertion_sort(__first, __last, __comp);
// 	  testbp::__insertion_sort(__first, __last, __comp);
// 	  testbp::__insertion_sort(__first, __last, __comp);
	}
    }

  template<typename _RandomAccessIterator>
    inline void
    sort(_RandomAccessIterator __first, _RandomAccessIterator __last)
    {
      // concept requirements
      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
	    _RandomAccessIterator>)
      __glibcxx_function_requires(_LessThanComparableConcept<
	    typename iterator_traits<_RandomAccessIterator>::value_type>)
      __glibcxx_requires_valid_range(__first, __last);
      __glibcxx_requires_irreflexive(__first, __last);

      testbp::__sort(__first, __last, __gnu_cxx::__ops::__iter_less_iter());
    }

}

namespace test_no_final_sort {

  template<typename _RandomAccessIterator, typename _Compare>
    inline void
    __sort(_RandomAccessIterator __first, _RandomAccessIterator __last,
	   _Compare __comp)
    {
      if (__first != __last)
	{
	  std::__introsort_loop(__first, __last,
				std::__lg(__last - __first) * 2,
				__comp);
// 	  std::__final_insertion_sort(__first, __last, __comp);
	}
    }

  template<typename _RandomAccessIterator>
    inline void
    sort(_RandomAccessIterator __first, _RandomAccessIterator __last)
    {
      // concept requirements
      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
	    _RandomAccessIterator>)
      __glibcxx_function_requires(_LessThanComparableConcept<
	    typename iterator_traits<_RandomAccessIterator>::value_type>)
      __glibcxx_requires_valid_range(__first, __last);
      __glibcxx_requires_irreflexive(__first, __last);

      test_no_final_sort::__sort(__first, __last, __gnu_cxx::__ops::__iter_less_iter());
    }

  void ll_introsort(uint64_t *a, int64_t l, int64_t h) {
    introsort_aux(a,l,h,std::__lg(h-l) * 2+100);
//     insertion_sort(a,l,h);
  }

}

void Generator_random (void);
void Generator_random_dup (void);
void Generator_sorted (void);
void Generator_almost_sorted (size_t nperm);
void Generator_alleq ();
void Generator_sorted_end (size_t n_last);
void Generator_sorted_middle (size_t n_last);
void Generator_reverse_sorted (void);
void Generator_reverse_sorted_end (size_t n_last);
void Generator_reverse_sorted_middle (size_t n_last);

template<class IA, class compare>
int Test (std::vector<IA> &B, compare comp = compare ());

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

    Generator_random ();
    Generator_random_dup ();

    Generator_alleq ();
    Generator_almost_sorted (NELEM / 1000);
    Generator_almost_sorted (NELEM / 100);
    Generator_almost_sorted (NELEM / 10);
    Generator_almost_sorted (NELEM);

    Generator_sorted ();
    Generator_sorted_end (NELEM / 1000);
    Generator_sorted_end (NELEM / 100);
    Generator_sorted_end (NELEM / 10);
    Generator_sorted_middle (NELEM / 1000);
    Generator_sorted_middle (NELEM / 100);
    Generator_sorted_middle (NELEM / 10);
    Generator_reverse_sorted ();
    Generator_reverse_sorted_end (NELEM / 1000);
    Generator_reverse_sorted_end (NELEM / 100);
    Generator_reverse_sorted_end (NELEM / 10);
    Generator_reverse_sorted_middle (NELEM / 1000);
    Generator_reverse_sorted_middle (NELEM / 100);
    Generator_reverse_sorted_middle (NELEM / 10);

//     annot::print_stats();

    return 0;
}
void
Generator_random (void)
{
    std::cout<<std::endl<<std::endl<<"random"<<std::endl;
    vector<uint64_t> A;
    A.reserve (NELEM);
    A.clear ();
    if (fill_vector_uint64 ("input.bin", A, NELEM) != 0)
    {
        std::cout << "Error in the input file\n";
        return;
    };
    Test<uint64_t, std::less<uint64_t>> (A);
}
;

void
Generator_random_dup (void)
{
    std::cout<<std::endl<<std::endl<<"random with lots of dups"<<std::endl;
    vector<uint64_t> A;
    A.reserve (NELEM);
    A.clear ();
    if (fill_vector_uint64 ("input.bin", A, NELEM) != 0)
    {
        std::cout << "Error in the input file\n";
        return;
    };

    for (size_t i=0;i<A.size();++i) A[i]%=(2048);

    Test<uint64_t, std::less<uint64_t>> (A);
}
;

void
Generator_sorted (void)
{
    std::cout<<std::endl<<std::endl<<"sorted"<<std::endl;
    vector<uint64_t> A;

    A.reserve (NELEM);
    A.clear ();
    for (size_t i = 0; i < NELEM; ++i)
        A.push_back (i);
    Test<uint64_t, std::less<uint64_t>> (A);

}

void
Generator_almost_sorted (size_t nperm)
{
    std::cout<<std::endl<<std::endl<<"almost sorted ("<< (float)nperm/NELEM * 100<<"perm)"<<std::endl;
    vector<uint64_t> A;

    A.reserve (NELEM);
    A.clear ();
    for (size_t i = 0; i < NELEM; ++i)
        A.push_back (i);

    vector<size_t> P;

    if (fill_vector_uint64 ("input.bin", P, nperm) != 0)
    {
        std::cout << "Error in the input file\n";
        return;
    };

    for (size_t i=0;i<P.size();++i) P[i]%=NELEM;

    for (size_t i=0;i+1<P.size();i+=2) {
      std::swap(A[P[i]],A[P[i+1]]);
    }

    Test<uint64_t, std::less<uint64_t>> (A);

}

void
Generator_alleq ()
{
    std::cout<<std::endl<<std::endl<<"all equal"<<std::endl;
    vector<uint64_t> A(NELEM,42);

    Test<uint64_t, std::less<uint64_t>> (A);

}

void Generator_sorted_end (size_t n_last)
{
    std::cout<<std::endl<<std::endl<<"sorted + "<<(float)n_last/NELEM * 100<<"% end"<<std::endl;

    vector<uint64_t> A;
    A.reserve (NELEM);
    A.clear ();
    if (fill_vector_uint64 ("input.bin", A, NELEM + n_last) != 0)
    {
        std::cout << "Error in the input file\n";
        return;
    };
    std::sort (A.begin (), A.begin () + NELEM);

    Test<uint64_t, std::less<uint64_t>> (A);

}
;
void Generator_sorted_middle (size_t n_last)
{
    std::cout<<std::endl<<std::endl<<"sorted + "<<(float)n_last/NELEM * 100<<"% mid"<<std::endl;

    vector<uint64_t> A, B, C;
    A.reserve (NELEM);
    A.clear ();
    if (fill_vector_uint64 ("input.bin", A, NELEM + n_last) != 0)
    {
        std::cout << "Error in the input file\n";
        return;
    };
    for (size_t i = NELEM; i < A.size (); ++i)
        B.push_back (std::move (A[i]));
    A.resize ( NELEM);
    for (size_t i = 0; i < (NELEM >> 1); ++i)
        std::swap (A[i], A[NELEM - 1 - i]);

    std::sort (A.begin (), A.end ());
    size_t step = NELEM / n_last + 1;
    size_t pos = 0;

    for (size_t i = 0; i < B.size (); ++i, pos += step)
    {
        C.push_back (B[i]);
        for (size_t k = 0; k < step; ++k)
            C.push_back (A[pos + k]);
    };
    while (pos < A.size ())
        C.push_back (A[pos++]);
    A = C;
    Test<uint64_t, std::less<uint64_t>> (A);
}
;
void Generator_reverse_sorted (void)
{
    std::cout<<std::endl<<std::endl<<"rev sorted"<<std::endl;
    vector<uint64_t> A;

    A.reserve (NELEM);
    A.clear ();
    for (size_t i = NELEM; i > 0; --i)
        A.push_back (i);
    Test<uint64_t, std::less<uint64_t>> (A);
}
void Generator_reverse_sorted_end (size_t n_last)
{
    std::cout<<std::endl<<std::endl<<"rev sorted + "<<(float)n_last/NELEM * 100<<"% end"<<std::endl;

    vector<uint64_t> A;
    A.reserve (NELEM);
    A.clear ();
    if (fill_vector_uint64 ("input.bin", A, NELEM + n_last) != 0)
    {
        std::cout << "Error in the input file\n";
        return;
    };
    std::sort (A.begin (), A.begin () + NELEM);
    for (size_t i = 0; i < (NELEM >> 1); ++i)
        std::swap (A[i], A[NELEM - 1 - i]);

    Test<uint64_t, std::less<uint64_t>> (A);
}
void Generator_reverse_sorted_middle (size_t n_last)
{
    std::cout<<std::endl<<std::endl<<"rev sorted + "<<(float)n_last/NELEM * 100<<"% mid"<<std::endl;

    vector<uint64_t> A, B, C;
    A.reserve (NELEM);
    A.clear ();
    if (fill_vector_uint64 ("input.bin", A, NELEM + n_last) != 0)
    {
        std::cout << "Error in the input file\n";
        return;
    };
    for (size_t i = NELEM; i < A.size (); ++i)
        B.push_back (std::move (A[i]));
    A.resize ( NELEM);
    for (size_t i = 0; i < (NELEM >> 1); ++i)
        std::swap (A[i], A[NELEM - 1 - i]);

    std::sort (A.begin (), A.end ());
    size_t step = NELEM / n_last + 1;
    size_t pos = 0;

    for (size_t i = 0; i < B.size (); ++i, pos += step)
    {
        C.push_back (B[i]);
        for (size_t k = 0; k < step; ++k)
            C.push_back (A[pos + k]);
    };
    while (pos < A.size ())
        C.push_back (A[pos++]);
    A = C;
    Test<uint64_t, std::less<uint64_t>> (A);
};

template<class IA, class compare>
void test_sorted(string name, std::vector<IA> &A, compare comp)
{
  if (!std::is_sorted(A.begin (), A.end (), comp)) {

    std::cout<<"*** NOT SORTED *** "<<name<<std::endl;
//     for (auto i=B.begin(); i<B.end();++i) std::cout<<*i<<" ";
//     std::cout<<std::endl;
//     for (auto i=A.begin(); i<A.end();++i) std::cout<<*i<<" ";
//     std::cout<<std::endl;

  }
}

template<class IA, class compare, class Op>
void sort_test(std::vector<IA> &A, std::vector<IA> &B, compare comp, string name, Op op, bool verify=true) {
//   std::cout<<name<<std::endl;

  vector<double> durations;
  annot::insort_duration=0;

  for (int i=0;i<NITER;++i) {
    A=B;

    durations.push_back(time_op(op) * 1000.0);
    if (verify) test_sorted(name,A,comp);
  }

  std::sort(durations.begin(),durations.end());

  auto N=durations.size();
  auto l=durations.begin()+N/10;
  auto h=durations.end()-N/10;

  N=h-l;

  double duration=std::accumulate(l,h,0.0) / (double)N;
  double min=*std::min_element(l,h);
  double max=*std::max_element(l,h);
//   double dev=max-min;

  std::cout<<"  "<<name<<"."<<"time x "<<N<<": "<<std::setw(2)<<duration<<" ["<<min<<" .. "<<max<<"]";

  if (annot::insort_duration != 0) {
    std::cout<<" ("<<std::setw(2)<<annot::insort_duration<<")";
  }

  std::cout<<std::endl;

}

extern "C" {
int cmpfunc (const void * ap, const void * bp) {
  uint64_t a=*(uint64_t*)ap;
  uint64_t b=*(uint64_t*)bp;
  if (a<b) return -1;
  else if (a==b) return 0;
  else return 1;
}
}

void qsort(std::vector<uint64_t> &A) {
  std::qsort(A.data(),A.size(),sizeof(uint64_t),cmpfunc);
}

template<class IA, class compare>
int Test (std::vector<IA> &B,  compare comp)
{
  std::vector<IA> A (B);

   sort_test(A,B,comp,"isa::introsort",[&]{ introsort(A.data (), 0, A.size()); });
//    sort_test(A,B,comp,"isa::introsortUGI",[&]{ introsortUGI(A.data (), 0, A.size()); });
//    sort_test(A,B,comp,"nfs-isa::introsort",[&]{ test_no_final_sort::ll_introsort(A.data (), 0, A.size()); }, false);
//   sort_test(A,B,comp,"isa::introsort*",[&]{ annot::ll_introsort(A.data (), 0, A.size()); });
//    sort_test(A,B,comp,"testbp::sort",[&]{ testbp::sort(A.begin (), A.end ()); });
   sort_test(A,B,comp,"std::sort",[&]{ std::sort(A.begin (), A.end (), comp);});
//   sort_test(A,B,comp,"no_final::sort",[&]{ test_no_final_sort::sort(A.begin (), A.end ()); }, false);

//   sort_test(A,B,comp,"std::qsort",[&]{ qsort(A); });
//  sort_test(A,B,comp,"bdpcmosqrt::sort",[&]{ blocked_double_pivot_check_mosqrt::sort(A.begin (), A.end (), comp); });

//   sort_test(A,B,comp,"pdqsort_bl",[&]{ pdqsort_branchless(A.begin (), A.end (), comp); });
//   sort_test(A,B,comp,"pdqsort",[&]{ annot::pdqsort(A.begin (), A.end (), comp); });
//   sort_test(A,B,comp,"std::sort*",[&]{ annot::sort(A.begin (), A.end (), comp); });
//   sort_test(A,B,comp,"std::sort2",[&]{ annot::sort2(A.begin (), A.end (), comp); });

//   std::vector<IA> A (B);
//
//   A = B;
//   time_op("std::sort",[&]{ annot::sort(A.begin (), A.end (), comp); });
//   test_sorted("std::sort",A,comp);
//
//   A = B;
//   time_op("isa::introsort*",[&]{ annot::ll_introsort(A.data (), 0, A.size()); });
//   test_sorted("isa::introsort*",A,comp);
//
//   A = B;
//   time_op("isa::introsort",[&]{ introsort(A.data (), 0, A.size()); });
//   test_sorted("isa::introsort",A,comp);
//

// //     A = B;
// //     start = now ();
// //     insertion_sort (A.data() , 0, A.size());

//     //---------------------------- begin --------------------------------
//     double duration;
//     time_point start, finish;
//     std::vector<IA> A (B);
//     std::vector<double> V;
//
//     //--------------------------------------------------------------------
//     A = B;
//     start = now ();
// //    std::sort (A.begin (), A.end (), comp);
//     annot::sort (A.begin (), A.end ()/*, comp*/);
//     finish = now ();
//     duration = subtract_time (finish, start);
//     V.push_back (duration);
//     test_sorted("std::sort",A,comp);
//
// //     A = B;
// //     start = now ();
// //     annot::insort (A.begin (), A.end (), comp);
// // //     annot::sort (A.begin (), A.end ()/*, comp*/);
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("std::insort",A,B,comp);
// //
// //     A = B;
// //     start = now ();
// //     insertion_sort (A.data() , 0, A.size());
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("isabelle-llvm::insort",A,B,comp);
// //
// //
// //     A = B;
// //     start = now ();
// //     introsort (A.data() , 0, A.size());
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("isabelle-llvm::introsort",A,B,comp);
// //
//
// //     A = B;
// //     start = now ();
// //     std_heapsort (A.begin (), A.end (), comp);
// // //     annot::sort (A.begin (), A.end ()/*, comp*/);
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("heapsort",A,B,comp);
// //
// //
// //     A = B;
// //     start = now ();
// //     heapsort (A.data() , 0, A.size());
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("isabelle-llvm::heapsort",A,B,comp);
//
//
// //     A = B;
// //     start = now ();
// //     pdqsort (A.begin (), A.end (), comp);
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("pdqsort",A,B,comp);
// //
// //     A = B;
// //     start = now ();
// //     std::stable_sort (A.begin (), A.end (), comp);
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("std::stable_sort",A,B,comp);
// //
// //     A = B;
// //     start = now ();
// //     spinsort(A.begin (), A.end (), comp);
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("spinsort",A,B,comp);
// //
// //     A = B;
// //     start = now ();
// //     flat_stable_sort (A.begin (), A.end (), comp);
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("flat_stable_sort",A,B,comp);
// //
// //
// //     A = B;
// //     start = now ();
// //     spreadsort (A.begin (), A.end ());
// //     finish = now ();
// //     duration = subtract_time (finish, start);
// //     V.push_back (duration);
// //     test_sorted("spreadsort",A,B,comp);
//
//     //-----------------------------------------------------------------------
//     // printing the vector
//     //-----------------------------------------------------------------------
//     std::cout<<std::setprecision(2)<<std::fixed;
//     for ( uint32_t i =0 ; i < V.size() ; ++i)
//     {   std::cout<<std::right<<std::setw(5)<<V[i]<<" |";
//     };
//     std::cout<<std::endl;
    return 0;
};


// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2018 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Felix Schindler (2015 - 2017)
//   Rene Milk       (2016 - 2018)
//   Tobias Leibner  (2018)

#ifndef DUNE_GDT_LOCAL_OPERATORS_INTEGRALS_HH
#define DUNE_GDT_LOCAL_OPERATORS_INTEGRALS_HH

#include <type_traits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/densematrix.hh>

#include <dune/geometry/quadraturerules.hh>

#include <dune/xt/common/matrix.hh>

#include <dune/xt/functions/interfaces.hh>

#include <dune/xt/la/container/common/matrix/dense.hh>

#include <dune/gdt/local/integrands/interfaces.hh>
#include <dune/gdt/type_traits.hh>

#include "interfaces.hh"

namespace Dune {
namespace GDT {

template <class BinaryEvaluationType,
          class TestBase,
          class AnsatzBase = TestBase,
          class Field = typename TestBase::RangeFieldType>
class LocalVolumeIntegralOperator : public LocalVolumeTwoFormInterface<TestBase, AnsatzBase, Field>
{
  static_assert(is_binary_volume_integrand<BinaryEvaluationType>::value, "");
  static_assert(std::is_same<typename TestBase::EntityType, typename AnsatzBase::EntityType>::value, "");
  static_assert(std::is_same<typename TestBase::DomainFieldType, typename AnsatzBase::DomainFieldType>::value, "");
  static_assert(TestBase::dimDomain == AnsatzBase::dimDomain, "");

  typedef LocalVolumeIntegralOperator<BinaryEvaluationType, TestBase, AnsatzBase, Field> ThisType;
  typedef LocalVolumeTwoFormInterface<TestBase, AnsatzBase, Field> BaseType;

  typedef typename TestBase::DomainFieldType D;
  static const size_t d = TestBase::dimDomain;

public:
  using typename BaseType::TestBaseType;
  using typename BaseType::AnsatzBaseType;
  using typename BaseType::FieldType;

  template <class... Args>
  explicit LocalVolumeIntegralOperator(Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(0)
  {
  }

  template <class... Args>
  explicit LocalVolumeIntegralOperator(const int over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(boost::numeric_cast<size_t>(over_integrate))
  {
  }

  template <class... Args>
  explicit LocalVolumeIntegralOperator(const size_t over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(over_integrate)
  {
  }

  LocalVolumeIntegralOperator(const ThisType& other) = default;
  LocalVolumeIntegralOperator(ThisType&& source) = default;

  using BaseType::apply2;

  // copied from apply2 below
  // TODO: fix properly (use CommonDenseMatrix instead of DynamicMatrix everywhere in dune-gdt?)
  void apply2(const TestBaseType& test_base,
              const AnsatzBaseType& ansatz_base,
              XT::LA::CommonDenseMatrix<FieldType>& ret) const
  {
    const auto& entity = ansatz_base.entity();
    const auto local_functions = integrand_.localFunctions(entity);
    // create quadrature
    const size_t integrand_order = integrand_.order(local_functions, test_base, ansatz_base) + over_integrate_;
    const auto& quadrature = QuadratureRules<D, d>::rule(entity.type(), boost::numeric_cast<int>(integrand_order));
    // prepare storage
    const size_t rows = test_base.size();
    const size_t cols = ansatz_base.size();
    ret *= 0.0;
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    DynamicMatrix<FieldType> integrand_eval(rows, cols, 0.); // \todo: make mutable member, after SMP refactor
    // loop over all quadrature points
    for (const auto& quadrature_point : quadrature) {
      const auto xx = quadrature_point.position();
      // integration factors
      const auto integration_factor = entity.geometry().integrationElement(xx);
      const auto quadrature_weight = quadrature_point.weight();
      // evaluate the integrand
      integrand_.evaluate(local_functions, test_base, ansatz_base, xx, integrand_eval);
      // compute integral
      ret.axpy(integration_factor * quadrature_weight, integrand_eval);
    } // loop over all quadrature points
  } // ... apply2(...)

  void
  apply2(const TestBaseType& test_base, const AnsatzBaseType& ansatz_base, DynamicMatrix<FieldType>& ret) const override
  {
    const auto& entity = ansatz_base.entity();
    const auto local_functions = integrand_.localFunctions(entity);
    // create quadrature
    const size_t integrand_order = integrand_.order(local_functions, test_base, ansatz_base) + over_integrate_;
    const auto& quadrature = QuadratureRules<D, d>::rule(entity.type(), boost::numeric_cast<int>(integrand_order));
    // prepare storage
    const size_t rows = test_base.size();
    const size_t cols = ansatz_base.size();
    ret *= 0.0;
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    DynamicMatrix<FieldType> integrand_eval(rows, cols, 0.); // \todo: make mutable member, after SMP refactor
    // loop over all quadrature points
    for (const auto& quadrature_point : quadrature) {
      const auto xx = quadrature_point.position();
      // integration factors
      const auto integration_factor = entity.geometry().integrationElement(xx);
      const auto quadrature_weight = quadrature_point.weight();
      // evaluate the integrand
      integrand_.evaluate(local_functions, test_base, ansatz_base, xx, integrand_eval);
      // compute integral
      for (size_t ii = 0; ii < rows; ++ii) {
        const auto& integrand_eval_row = integrand_eval[ii];
        auto& ret_row = ret[ii];
        for (size_t jj = 0; jj < cols; ++jj)
          ret_row[jj] += integrand_eval_row[jj] * integration_factor * quadrature_weight;
      } // compute integral
    } // loop over all quadrature points
  } // ... apply2(...)

private:
  const BinaryEvaluationType integrand_;
  const size_t over_integrate_;
}; // class LocalVolumeIntegralOperator

template <class QuaternaryFaceIntegrandType,
          class TestBaseEntity,
          class Intersection,
          class AnsatzBaseEntity = TestBaseEntity,
          class TestBaseNeighbor = TestBaseEntity,
          class AnsatzBaseNeighbor = AnsatzBaseEntity,
          class Field = typename TestBaseEntity::RangeFieldType>
class LocalCouplingIntegralOperator : public LocalCouplingTwoFormInterface<TestBaseEntity,
                                                                           Intersection,
                                                                           AnsatzBaseEntity,
                                                                           TestBaseNeighbor,
                                                                           AnsatzBaseNeighbor,
                                                                           Field>
{
  static_assert(is_quaternary_face_integrand<QuaternaryFaceIntegrandType>::value, "");
  static_assert(std::is_same<typename TestBaseEntity::EntityType, typename AnsatzBaseEntity::EntityType>::value, "");
  static_assert(std::is_same<typename TestBaseEntity::EntityType, typename TestBaseNeighbor::EntityType>::value, "");
  static_assert(std::is_same<typename TestBaseEntity::EntityType, typename AnsatzBaseNeighbor::EntityType>::value, "");
  static_assert(
      std::is_same<typename TestBaseEntity::DomainFieldType, typename AnsatzBaseEntity::DomainFieldType>::value, "");
  static_assert(
      std::is_same<typename TestBaseEntity::DomainFieldType, typename TestBaseNeighbor::DomainFieldType>::value, "");
  static_assert(
      std::is_same<typename TestBaseEntity::DomainFieldType, typename AnsatzBaseNeighbor::DomainFieldType>::value, "");
  static_assert(TestBaseEntity::dimDomain == AnsatzBaseEntity::dimDomain, "");
  static_assert(TestBaseEntity::dimDomain == TestBaseNeighbor::dimDomain, "");
  static_assert(TestBaseEntity::dimDomain == AnsatzBaseNeighbor::dimDomain, "");

  typedef LocalCouplingIntegralOperator<QuaternaryFaceIntegrandType,
                                        TestBaseEntity,
                                        Intersection,
                                        AnsatzBaseEntity,
                                        TestBaseNeighbor,
                                        AnsatzBaseNeighbor,
                                        Field>
      ThisType;
  typedef LocalCouplingTwoFormInterface<TestBaseEntity,
                                        Intersection,
                                        AnsatzBaseEntity,
                                        TestBaseNeighbor,
                                        AnsatzBaseNeighbor,
                                        Field>
      BaseType;
  typedef typename TestBaseEntity::DomainFieldType D;
  static const size_t d = TestBaseEntity::dimDomain;

public:
  using typename BaseType::TestBaseEntityType;
  using typename BaseType::AnsatzBaseEntityType;
  using typename BaseType::TestBaseNeighborType;
  using typename BaseType::AnsatzBaseNeighborType;
  using typename BaseType::IntersectionType;
  using typename BaseType::FieldType;

  template <class... Args>
  explicit LocalCouplingIntegralOperator(Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(0)
  {
  }

  template <class... Args>
  explicit LocalCouplingIntegralOperator(const int over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(boost::numeric_cast<size_t>(over_integrate))
  {
  }

  template <class... Args>
  explicit LocalCouplingIntegralOperator(const size_t over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(over_integrate)
  {
  }

  LocalCouplingIntegralOperator(const ThisType& other) = default;
  LocalCouplingIntegralOperator(ThisType&& source) = default;

  void apply2(const TestBaseEntityType& test_base_en,
              const AnsatzBaseEntityType& ansatz_base_en,
              const TestBaseNeighborType& test_base_ne,
              const AnsatzBaseNeighborType& ansatz_base_ne,
              const IntersectionType& intersection,
              DynamicMatrix<FieldType>& ret_en_en,
              DynamicMatrix<FieldType>& ret_ne_ne,
              DynamicMatrix<FieldType>& ret_en_ne,
              DynamicMatrix<FieldType>& ret_ne_en) const override
  {
    // local inducing function
    const auto& entity = test_base_en.entity();
    const auto& neighbor = test_base_ne.entity();
    const auto local_functions_en = integrand_.localFunctions(entity);
    const auto local_functions_ne = integrand_.localFunctions(neighbor);
    // quadrature
    const size_t integrand_order =
        integrand_.order(
            local_functions_en, local_functions_ne, test_base_en, ansatz_base_en, test_base_ne, ansatz_base_ne)
        + over_integrate_;
    const auto& quadrature =
        QuadratureRules<D, d - 1>::rule(intersection.type(), boost::numeric_cast<int>(integrand_order));
    // check matrices
    ret_en_en *= 0.0;
    ret_ne_ne *= 0.0;
    ret_en_ne *= 0.0;
    ret_ne_en *= 0.0;
    const size_t rows_en = test_base_en.size();
    const size_t cols_en = ansatz_base_en.size();
    const size_t rows_ne = test_base_ne.size();
    const size_t cols_ne = ansatz_base_ne.size();
    assert(ret_en_en.rows() >= rows_en);
    assert(ret_en_en.cols() >= cols_en);
    assert(ret_ne_ne.rows() >= rows_ne);
    assert(ret_ne_ne.cols() >= cols_ne);
    assert(ret_en_ne.rows() >= rows_en);
    assert(ret_en_ne.cols() >= cols_ne);
    assert(ret_ne_en.rows() >= rows_en);
    assert(ret_ne_en.cols() >= cols_en);
    // \todo: make mutable member, after SMP refactor
    DynamicMatrix<FieldType> integrand_eval_en_en(rows_en, cols_en, 0.);
    DynamicMatrix<FieldType> integrand_eval_ne_ne(rows_ne, cols_ne, 0.);
    DynamicMatrix<FieldType> integrand_eval_en_ne(rows_en, cols_ne, 0.);
    DynamicMatrix<FieldType> integrand_eval_ne_en(rows_ne, cols_en, 0.);
    // loop over all quadrature points
    for (const auto& quadrature_point : quadrature) {
      const auto xx = quadrature_point.position();
      const auto integration_factor = intersection.geometry().integrationElement(xx);
      const auto quadrature_weight = quadrature_point.weight();
      // evaluate local
      integrand_.evaluate(local_functions_en,
                          local_functions_ne,
                          test_base_en,
                          ansatz_base_en,
                          test_base_ne,
                          ansatz_base_ne,
                          intersection,
                          xx,
                          integrand_eval_en_en,
                          integrand_eval_ne_ne,
                          integrand_eval_en_ne,
                          integrand_eval_ne_en);
      // compute integrals
      // loop over all entity test basis functions
      for (size_t ii = 0; ii < rows_en; ++ii) {
        auto& ret_en_en_row = ret_en_en[ii];
        auto& ret_en_ne_row = ret_en_ne[ii];
        const auto& integrand_eval_en_en_row = integrand_eval_en_en[ii];
        const auto& integrand_eval_en_ne_row = integrand_eval_en_ne[ii];
        // loop over all entity ansatz basis functions
        for (size_t jj = 0; jj < cols_en; ++jj) {
          ret_en_en_row[jj] += integrand_eval_en_en_row[jj] * integration_factor * quadrature_weight;
        }
        // loop over all neighbor ansatz basis functions
        for (size_t jj = 0; jj < cols_ne; ++jj) {
          ret_en_ne_row[jj] += integrand_eval_en_ne_row[jj] * integration_factor * quadrature_weight;
        }
      }
      // loop over all neighbor test basis functions
      for (size_t ii = 0; ii < rows_ne; ++ii) {
        auto& ret_ne_ne_row = ret_ne_ne[ii];
        auto& ret_ne_en_row = ret_ne_en[ii];
        const auto& integrand_eval_ne_ne_row = integrand_eval_ne_ne[ii];
        const auto& integrand_eval_ne_en_row = integrand_eval_ne_en[ii];
        // loop over all neighbor ansatz basis functions
        for (size_t jj = 0; jj < cols_ne; ++jj) {
          ret_ne_ne_row[jj] += integrand_eval_ne_ne_row[jj] * integration_factor * quadrature_weight;
        }
        // loop over all entity ansatz basis functions
        for (size_t jj = 0; jj < cols_en; ++jj) {
          ret_ne_en_row[jj] += integrand_eval_ne_en_row[jj] * integration_factor * quadrature_weight;
        }
      }
    } // loop over all quadrature points
  } // void apply(...) const

private:
  const QuaternaryFaceIntegrandType integrand_;
  const size_t over_integrate_;
}; // class LocalCouplingIntegralOperator

template <class BinaryFaceIntegrandType,
          class TestBase,
          class Intersection,
          class AnsatzBase = TestBase,
          class Field = typename TestBase::RangeFieldType>
class LocalBoundaryIntegralOperator : public LocalBoundaryTwoFormInterface<TestBase, Intersection, AnsatzBase, Field>
{
  static_assert(is_binary_face_integrand<BinaryFaceIntegrandType>::value, "");
  static_assert(std::is_same<typename TestBase::EntityType, typename AnsatzBase::EntityType>::value, "");
  static_assert(std::is_same<typename TestBase::DomainFieldType, typename AnsatzBase::DomainFieldType>::value, "");
  static_assert(TestBase::dimDomain == AnsatzBase::dimDomain, "");

  typedef LocalBoundaryIntegralOperator<BinaryFaceIntegrandType, TestBase, Intersection, AnsatzBase, Field> ThisType;
  typedef LocalBoundaryTwoFormInterface<TestBase, Intersection, AnsatzBase, Field> BaseType;
  typedef typename TestBase::DomainFieldType D;
  static const size_t d = TestBase::dimDomain;

public:
  using typename BaseType::TestBaseType;
  using typename BaseType::AnsatzBaseType;
  using typename BaseType::IntersectionType;
  using typename BaseType::FieldType;

  template <class... Args>
  LocalBoundaryIntegralOperator(Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(0)
  {
  }

  template <class... Args>
  LocalBoundaryIntegralOperator(const int over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(boost::numeric_cast<size_t>(over_integrate))
  {
  }

  template <class... Args>
  LocalBoundaryIntegralOperator(const size_t over_integrate, Args&&... args)
    : integrand_(std::forward<Args>(args)...)
    , over_integrate_(over_integrate)
  {
  }

  void apply2(const TestBaseType& test_base,
              const AnsatzBaseType& ansatz_base,
              const IntersectionType& intersection,
              DynamicMatrix<FieldType>& ret) const override
  {
    // local inducing function
    const auto& entity = test_base.entity();
    const auto local_functions = integrand_.localFunctions(entity);
    // create quadrature
    const auto integrand_order = integrand_.order(local_functions, test_base, ansatz_base) + over_integrate_;
    const auto& quadrature =
        QuadratureRules<D, d - 1>::rule(intersection.type(), boost::numeric_cast<int>(integrand_order));
    // prepare storage
    ret *= 0.0;
    const size_t rows = test_base.size();
    const size_t cols = ansatz_base.size();
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    DynamicMatrix<FieldType> integrand_eval(rows, cols, 0.); // \todo: make mutable member, after SMP refactor
    // loop over all quadrature points
    for (const auto& quadrature_point : quadrature) {
      const auto xx = quadrature_point.position();
      const auto integration_factor = intersection.geometry().integrationElement(xx);
      const auto quadrature_weight = quadrature_point.weight();
      // evaluate local
      integrand_.evaluate(local_functions, test_base, ansatz_base, intersection, xx, integrand_eval);
      // compute integral
      assert(integrand_eval.rows() >= rows);
      assert(integrand_eval.cols() >= cols);
      // loop over all test basis functions
      for (size_t ii = 0; ii < rows; ++ii) {
        auto& ret_row = ret[ii];
        const auto& integrand_eval_row = integrand_eval[ii];
        // loop over all ansatz basis functions
        for (size_t jj = 0; jj < cols; ++jj) {
          ret_row[jj] += integrand_eval_row[jj] * integration_factor * quadrature_weight;
        }
      }
    }
  } // ... apply(...)

private:
  const BinaryFaceIntegrandType integrand_;
  const size_t over_integrate_;
}; // class LocalBoundaryIntegralOperator

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_LOCAL_OPERATORS_INTEGRALS_HH


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

#include <iostream>
#include <cmath>
#include <complex>
#include <vector>

#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/mtl/mtl.hpp>

template <typename Vector, typename Ashape>
void test(const char* name, Vector& v, Ashape)
{
    v= 3, 4, 6;
    std::cout << name << ": v is " << v << ", typeid is " << typeid(v).name() << "\n";
    std::cout << "trans(v) is " << trans(v) << ", typeid is " << typeid(trans(v)).name() << "\n";
    BOOST_STATIC_ASSERT((boost::is_same<typename mtl::ashape::ashape<Vector>::type, Ashape>::value));
}

template <typename Vector, typename Ashape>
void test2(const char* name, const Vector& v, Ashape)
{
    std::cout << name << ": v is " << v << ", typeid is " << typeid(v).name() << "\n";
    std::cout << "trans(v) is " << trans(v) << ", typeid is " << typeid(trans(v)).name() << "\n";
    // trans(v)[0]= 2.3; // must not compile
    BOOST_STATIC_ASSERT((boost::is_same<typename mtl::ashape::ashape<Vector>::type, Ashape>::value));
}

int main(int, char**)
{
    using namespace mtl;

    typedef mtl::vec::fixed::dimension<3> fsize;
    mtl::dense_vector<float, mtl::vec::parameters<mtl::row_major, fsize, true> >     rf;
    mtl::dense_vector<float, mtl::vec::parameters<mtl::col_major, fsize, true> >     cf;

    mtl::dense_vector<float, mtl::vec::parameters<mtl::row_major> >                  rd(3);
    mtl::dense_vector<float>                                                         cd(3);

    mtl::dense_vector<std::complex<double> >                                         dc(3);

    mtl::dense2D<float>  A(3, 3);
    A= 2, 3, 4,
       3, 4, 6,
       7, 6, 9;

    test("Row vector fixed size", rf, ashape::rvec<ashape::scal>());
    test("Column vector fixed size", cf, ashape::cvec<ashape::scal>());
    test("Row vector", rd, ashape::rvec<ashape::scal>());
    test("Column vector", cd, ashape::cvec<ashape::scal>());
    test("Column vector complex", dc, ashape::cvec<ashape::scal>());
    test2("Matrix column", A[mtl::iall][1], ashape::cvec<ashape::scal>());
    test2("Matrix row", A[1][mtl::iall], ashape::rvec<ashape::scal>());

    return 0;
}


/******************************************************************************************************
*** This method performs the segmentation of iris to the images's database CASIA with the datas     ***
***    of the circles (cordinate "x" to the two circles, cordinate "y" to the two circles and radio ***
***    also to the two circles)                                                                     ***
******************************************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <dirent.h>
#include <fstream>
#include <iostream>
#include <sys/stat.h>
#include <opencv2/opencv.hpp>
#include <string>
#include <cmath>
#include <boost/algorithm/string/split.hpp>
#include <vector>

using namespace cv;
using namespace std;

struct datasIris{
  int xCircleInt;
  int yCircleInt;
  int rCircleInt;
  int xCircleExt;
  int yCircleExt;
  int rCircleExt;
};

struct datasIris split(string str, string sep){
    char* cstr = const_cast<char*>(str.c_str());
    char* current;
    vector<string> arr;
    struct datasIris datas;

    current = strtok(cstr, sep.c_str());
    while(current != NULL){
        arr.push_back(current);
        current = strtok(NULL,sep.c_str());
    }

    datas.xCircleInt = atoi(arr[0].c_str());
    datas.yCircleInt = atoi(arr[1].c_str());
    datas.rCircleInt = atoi(arr[2].c_str());
    datas.xCircleExt = atoi(arr[3].c_str());
    datas.yCircleExt = atoi(arr[4].c_str());
    datas.rCircleExt = atoi(arr[5].c_str());

    return datas;
}

/** @function main */
int main( int argc, char** argv )
{

	Mat src;
  DIR* directorio;
  struct dirent* elemento;
  ifstream infile;
  string nameFile, type, onlyName, pathFile, line;
  int pos = 0;
  struct datasIris datas;
  Mat image;

  directorio=opendir(argv[1]);

  if (directorio == NULL)
  {
    //printf ("Error de Lectura en el Directorio \n");
    cout << "Error de Lectura en el Directorio" << endl;
    return 0;

  }

  elemento = readdir(directorio); // getting the first element from directory
  while (elemento != NULL)
  {
    string nameFile = elemento->d_name; // getting the name's file with extension
    pos = nameFile.find(".");
    type = nameFile.substr(pos); // getting the extension's file only
    onlyName = nameFile.substr(0, pos); // getting the name's file only
    pathFile = (string)argv[1] + nameFile; // // getting the path's file

    if (type == ".txt") // if the file is a "txt" then we read it
    {
      infile.open(pathFile);
      getline(infile, line); // Saves the line in STRING.
      datas = split(line, " "); // Saves the line in structure "datasIris"
      infile.close();

      if(image.data)
      {
        Rect roi(datas.xCircleExt-datas.rCircleExt, datas.yCircleExt-datas.rCircleExt, datas.rCircleExt*2,datas.rCircleExt*2); // Making the region to the segmentation of iris

        if (0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= image.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= image.rows)
        {
          Mat segIris(image, roi); // Making a matrix with the size region "roi"

          vector<int> compression_params;
          compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
          compression_params.push_back(9);

          string nameImageSegmented = "../../../outputs/images_Segmented/seg-" + onlyName + ".jpg";
          imwrite(nameImageSegmented, segIris, compression_params); // Saving the new matrix in an image jpg
        }

      }
      else
        cout << "Image not loaded";
    }
    else if (type == ".jpg") // if the file is a "jpg" then we load it
    {
      image = imread(pathFile); // Loading the image in a matrix Mat
      if (!image.data)
      {
        cout << "Image not loaded";
        return -1;
      }
    }

    elemento = readdir (directorio); // Reading the next element from directory

  }

  closedir (directorio);

  return 0;
}


#ifndef GREG_DURATION_TYPES_HPP___
#define GREG_DURATION_TYPES_HPP___

/* Copyright (c) 2004 CrystalClear Software, Inc.
 * Subject to Boost Software License, Version 1.0. (See accompanying
 * file LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)
 * Author: Jeff Garland, Bart Garst
 * $Date$
 */

#include <boost/date_time/adjust_functors.hpp>
#include <boost/date_time/compiler_config.hpp>
#include <boost/date_time/date_duration_types.hpp>
#include <boost/date_time/gregorian/greg_date.hpp>
#include <boost/date_time/gregorian/greg_duration.hpp>
#include <boost/date_time/int_adapter.hpp>

namespace boost
{
namespace gregorian
{

//! config struct for additional duration types (ie months_duration<> &
//! years_duration<>)
struct BOOST_SYMBOL_VISIBLE greg_durations_config
{
    typedef date date_type;
    typedef date_time::int_adapter<int> int_rep;
    typedef date_time::month_functor<date_type> month_adjustor_type;
};

typedef date_time::months_duration<greg_durations_config> months;
typedef date_time::years_duration<greg_durations_config> years;

class BOOST_SYMBOL_VISIBLE weeks_duration : public date_duration
{
  public:
    weeks_duration(duration_rep w) : date_duration(w * 7)
    {
    }
    weeks_duration(date_time::special_values sv) : date_duration(sv)
    {
    }
};

typedef weeks_duration weeks;

} // namespace gregorian
} // namespace boost

#endif // GREG_DURATION_TYPES_HPP___


/*
 *           Copyright 2009-2020 The VOTCA Development Team
 *                      (http://www.votca.org)
 *
 *     Licensed under the Apache License,Version 2.0 (the "License")
 *
 *You may not use this file except in compliance with the License.
 *You may obtain a copy of the License at
 *
 *             http://www.apache.org/licenses/LICENSE-2.0
 *
 *Unless required by applicable law or agreed to in writing,software
 *distributed under the License is distributed on an "AS IS" BASIS,
 *WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,either express or implied.
 *See the License for the specific language governing permissions and
 *limitations under the License.
 *
 */

// Standard includes
#include <fstream>
#include <string>

// Third party includes
#include <boost/format.hpp>

// VOTCA includes
#include <votca/tools/constants.h>

// Local VOTCA includes
#include <votca/xtp/staticsite.h>

namespace votca {
namespace xtp {

Eigen::Matrix3d StaticSite::CalculateCartesianMultipole() const {
  // We are transforming here just quadrupoles
  // const  Eigen::VectorXd& MP = _multipole;
  const Vector9d& MP = _Q;
  Eigen::Matrix3d theta = Eigen::Matrix3d::Zero();
  if (_rank > 1) {
    double sqr3 = std::sqrt(3);
    theta(0, 0) = 0.5 * (-MP(4) + sqr3 * MP(7));     // theta_xx
    theta(1, 1) = 0.5 * (-MP(4) - sqr3 * MP(7));     // theta_yy
    theta(2, 2) = MP(4);                             // theta_zz
    theta(0, 1) = theta(1, 0) = 0.5 * sqr3 * MP(8);  // theta_xy = theta_yx
    theta(0, 2) = theta(2, 0) = 0.5 * sqr3 * MP(5);  // theta_xz = theta_zx
    theta(1, 2) = theta(2, 1) = 0.5 * sqr3 * MP(6);  // theta_yz = theta_zy
  }
  return theta;
}

Eigen::VectorXd StaticSite::CalculateSphericalMultipole(
    const Eigen::Matrix3d& quad_cart) {
  Eigen::VectorXd quadrupole_polar = Eigen::VectorXd::Zero(5);
  const double sqr3 = std::sqrt(3);
  quadrupole_polar(0) = quad_cart(2, 2);
  quadrupole_polar(1) = (2. / sqr3) * quad_cart(0, 2);
  quadrupole_polar(2) = (2. / sqr3) * quad_cart(1, 2);
  quadrupole_polar(3) = (1. / sqr3) * (quad_cart(0, 0) - quad_cart(1, 1));
  quadrupole_polar(4) = (2. / sqr3) * quad_cart(0, 1);
  return quadrupole_polar;
}

void StaticSite::Rotate(const Eigen::Matrix3d& R,
                        const Eigen::Vector3d& refPos) {
  Eigen::Vector3d dir = _pos - refPos;
  dir = R * dir;
  _pos = refPos + dir;  // Rotated Position
  if (_rank > 0) {
    const Eigen::Vector3d temp = R * _Q.segment<3>(1);
    _Q.segment<3>(1) = temp;
  }
  if (_rank > 1) {
    Eigen::Matrix3d cartesianquad = CalculateCartesianMultipole();
    Eigen::Matrix3d rotated = R * cartesianquad * R.transpose();
    _Q.segment<5>(4) = CalculateSphericalMultipole(rotated);
  }
  return;
}

void StaticSite::Translate(const Eigen::VectorXd& shift) {
  _pos += shift;
  return;
}

std::string StaticSite::writepolarization() const {
  tools::Elements e;
  double default_pol = 1;  // default is alway 1A^3
  try {
    default_pol =
        e.getPolarizability(_element) * std::pow(tools::conv::nm2ang, 3);
  } catch (const std::runtime_error&) {
    ;
  }
  return (boost::format("     P %1$+1.7f\n") % default_pol).str();
}

std::string StaticSite::WriteMpsLine(std::string unit) const {
  double conv_pos = 1.;
  if (unit == "angstrom") {
    conv_pos = tools::conv::bohr2ang;
  } else if (unit == "bohr") {
    conv_pos = 1.;
  } else {
    throw std::runtime_error(
        " StaticSite::WriteMpsLine: Unit conversion not known");
  }
  std::string output = "";
  output += (boost::format(" %1$2s %2$+1.7f %3$+1.7f %4$+1.7f Rank %5$d\n") %
             _element % (_pos(0) * conv_pos) % (_pos(1) * conv_pos) %
             (_pos(2) * conv_pos) % _rank)
                .str();
  output += (boost::format("    %1$+1.7f\n") % getCharge()).str();
  if (_rank > 0) {
    // Dipole z x y
    output += (boost::format("    %1$+1.7f %2$+1.7f %3$+1.7f\n") % _Q(3) %
               _Q(1) % _Q(2))
                  .str();
    if (_rank > 1) {
      // Quadrupole 20 21c 21s 22c 22s
      output +=
          (boost::format("    %1$+1.7f %2$+1.7f %3$+1.7f %4$+1.7f %5$+1.7f\n") %
           _Q(4) % _Q(5) % _Q(6) % _Q(7) % _Q(8))
              .str();
    }
  }
  // Polarizability
  output += writepolarization();
  return output;
}

void StaticSite::SetupCptTable(CptTable& table) const {
  table.addCol(_id, "index", HOFFSET(data, id));
  table.addCol(_element, "type", HOFFSET(data, element));

  table.addCol(_pos[0], "posX", HOFFSET(data, posX));
  table.addCol(_pos[1], "posY", HOFFSET(data, posY));
  table.addCol(_pos[2], "posZ", HOFFSET(data, posZ));

  table.addCol(_rank, "rank", HOFFSET(data, rank));

  table.addCol(_Q[0], "Q00", HOFFSET(data, Q00));
  table.addCol(_Q[1], "Q11c", HOFFSET(data, Q11c));
  table.addCol(_Q[2], "Q11s", HOFFSET(data, Q11s));
  table.addCol(_Q[3], "Q10", HOFFSET(data, Q10));
  table.addCol(_Q[4], "Q20", HOFFSET(data, Q20));
  table.addCol(_Q[5], "Q21c", HOFFSET(data, Q21c));
  table.addCol(_Q[6], "Q21s", HOFFSET(data, Q21s));
  table.addCol(_Q[7], "Q22c", HOFFSET(data, Q22c));
  table.addCol(_Q[8], "Q22s", HOFFSET(data, Q22s));
}

void StaticSite::WriteData(data& d) const {
  d.id = _id;
  d.element = const_cast<char*>(_element.c_str());
  d.posX = _pos[0];
  d.posY = _pos[1];
  d.posZ = _pos[2];

  d.rank = _rank;

  d.Q00 = _Q[0];
  d.Q11c = _Q[1];
  d.Q11s = _Q[2];
  d.Q10 = _Q[3];
  d.Q20 = _Q[4];
  d.Q21c = _Q[5];
  d.Q21s = _Q[6];
  d.Q22c = _Q[7];
  d.Q22s = _Q[8];
}

void StaticSite::ReadData(const data& d) {
  _id = d.id;
  _element = std::string(d.element);
  free(d.element);
  _pos[0] = d.posX;
  _pos[1] = d.posY;
  _pos[2] = d.posZ;

  _rank = d.rank;

  _Q[0] = d.Q00;
  _Q[1] = d.Q11c;
  _Q[2] = d.Q11s;
  _Q[3] = d.Q10;
  _Q[4] = d.Q20;
  _Q[5] = d.Q21c;
  _Q[6] = d.Q21s;
  _Q[7] = d.Q22c;
  _Q[8] = d.Q22s;
}

}  // namespace xtp
}  // namespace votca


#include "testsuite.h"
#include <blitz/bzconfig.h>
#include <blitz/array.h>
#include <random/uniform.h>

BZ_USING_NAMESPACE(blitz)

#ifdef BZ_HAVE_BOOST_SERIALIZATION
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <sstream>
#endif

int main()
{
#ifdef BZ_HAVE_BOOST_SERIALIZATION

  stringstream s;

  TinyVector<double, 4> v(1.1,2.2,3.3,4.4);
  TinyMatrix<double, 2, 4> m;
  m=1.1,2.2,3.3,4.4;
  Array<double,1> a(5),b(a);

  {
    a=2.2, -3.3, 4.4, -5.5, 6.6;
    boost::archive::text_oarchive oa(s);

    // write class instance to archive
    oa << v << m;
    oa << a << b;
  }

  cout << s.str() << endl;

  {
    Array<double,1> aa,bb;
    boost::archive::text_iarchive ia(s);
    TinyVector<double,4> vv;
    TinyMatrix<double, 2, 4> mm;
    ia >> vv >> mm;
    ia >> aa >> bb;

    BZTEST(all(v==vv));
    BZTEST(all(m==mm));
    BZTEST(all(a==aa));
    BZTEST(all(b==bb));
    // make sure aa and bb still refer to the same block
    bb(3)=4711;
    BZTEST(aa(3)==bb(3));

   }
  s.clear();

  // test serialization of random states
  {
    ranlib::Uniform<float> r;
    r.seed(42);

    // save state
    boost::archive::text_oarchive oa(s);
    {
      ranlib::MersenneTwister::T_state state(r.getState());
      oa << state;
    }

    const float n1 = r.random();
    cout << n1 << ".... ";
    const float crap = r.random();

    // restore state
    boost::archive::text_iarchive ia(s);
    {
      ranlib::MersenneTwister::T_state state(r.getState());
      ia >> state;
      r.setState(state);
    }

    const float n2 = r.random();
    cout << n2 << endl;

    BZTEST(n1 == n2);
  }

#else
  cout << "No serialization support enabled, can't test\n";
#endif

  return 0;
}


#include <pybind11/eigen.h>
#include <pybind11/stl_bind.h>
#include <pybind11/pybind11.h>
#include <pybind11/operators.h>
#include "gtsam/nonlinear/utilities.h"  // for RedirectCout.

#include "gtsam/geometry/Cal3Bundler.h"

#include "wrap/serialization.h"
#include <boost/serialization/export.hpp>

using namespace std;

namespace py = pybind11;

PYBIND11_MODULE(special_cases_py, m_) {
    m_.doc() = "pybind11 wrapper of special_cases_py";

    pybind11::module m_gtsam = m_.def_submodule("gtsam", "gtsam submodule");

    py::class_<gtsam::NonlinearFactorGraph, std::shared_ptr<gtsam::NonlinearFactorGraph>>(m_gtsam, "NonlinearFactorGraph")
        .def("addPriorPinholeCameraCal3Bundler",[](gtsam::NonlinearFactorGraph* self, size_t key, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& prior, const std::shared_ptr<gtsam::noiseModel::Base> noiseModel){ self->addPrior<gtsam::PinholeCamera<gtsam::Cal3Bundler>>(key, prior, noiseModel);}, py::arg("key"), py::arg("prior"), py::arg("noiseModel"));

    py::class_<gtsam::SfmTrack, std::shared_ptr<gtsam::SfmTrack>>(m_gtsam, "SfmTrack")
        .def_readwrite("measurements", &gtsam::SfmTrack::measurements);

    py::class_<gtsam::PinholeCamera<gtsam::Cal3Bundler>, std::shared_ptr<gtsam::PinholeCamera<gtsam::Cal3Bundler>>>(m_gtsam, "PinholeCameraCal3Bundler");

    py::class_<gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3>, std::shared_ptr<gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3>>>(m_gtsam, "GeneralSFMFactorCal3Bundler");

#include "python/specializations.h"

}