/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * 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 project's author 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 THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS 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.
 *
 */

#include "LowRankGPModelBuilder.h"
#include "StatisticalModel.h"
#include "vtkPolyDataReader.h"
#include "vtkStandardMeshRepresenter.h"

#include "Kernels.h"
#include "KernelCombinators.h"

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

using namespace statismo;

/**
 * A scalar valued gaussian kernel.
 */
class GaussianKernel: public ScalarValuedKernel<vtkPoint> {
  public:

    GaussianKernel(double sigma) : m_sigma(sigma), m_sigma2(sigma * sigma) {
    }

    inline double operator()(const vtkPoint& x, const vtkPoint& y) const {
        VectorType r(3);
        r << x[0] - y[0], x[1] - y[1], x[2] - y[2];
        return exp(-r.dot(r) / m_sigma2);
    }

    std::string GetKernelInfo() const {
        std::ostringstream os;
        os << "GaussianKernel(" << m_sigma << ")";
        return os.str();
    }

  private:

    double m_sigma;
    double m_sigma2;
};

vtkPolyData* loadVTKPolyData(const std::string& filename) {
    vtkPolyDataReader* reader = vtkPolyDataReader::New();
    reader->SetFileName(filename.c_str());
    reader->Update();
    vtkPolyData* pd = vtkPolyData::New();
    pd->ShallowCopy(reader->GetOutput());
    return pd;
}

//
// This example illustrates, how the flexibility of a statistical (shape) model can be extended by combining its covariance function
// with a Gaussian Kernel function.
//
int main(int argc, char** argv) {

    if (argc < 5) {
        std::cout << "Usage " << argv[0] << " model gaussianKernelWidth numberOfComponents, outputmodelName" << std::endl;
        exit(-1);
    }
    std::string modelFilename(argv[1]);
    double gaussianKernelSigma = std::atof(argv[2]);
    int numberOfComponents = std::atoi(argv[3]);
    std::string outputModelFilename(argv[4]);

    // All the statismo classes have to be parameterized with the RepresenterType.

    typedef vtkStandardMeshRepresenter RepresenterType;
    typedef LowRankGPModelBuilder<vtkPolyData> ModelBuilderType;
    typedef StatisticalModel<vtkPolyData> StatisticalModelType;
    typedef GaussianKernel GaussianKernelType;
    typedef MatrixValuedKernel<vtkPoint> MatrixValuedKernelType;

    try {

        // we load an existing statistical model and create a StatisticalModelKernel from it. The statisticlModelKernel
        // takes the covariance (matrix) of the model and defines a kernel function from it.
        vtkStandardMeshRepresenter* representer = vtkStandardMeshRepresenter::Create();
        boost::scoped_ptr<StatisticalModelType> model(StatisticalModelType::Load(representer, modelFilename));
        const MatrixValuedKernelType& statModelKernel = StatisticalModelKernel<vtkPolyData>(model.get());

        // Create a (scalar valued) gaussian kernel. This kernel is then made matrix-valued. We use a UncorrelatedMatrixValuedKernel,
        // which assumes that each output component is independent.

        const GaussianKernel gk = GaussianKernel(gaussianKernelSigma);
        const MatrixValuedKernelType& mvGk = UncorrelatedMatrixValuedKernel<vtkPoint>(&gk, model->GetRepresenter()->GetDimensions());

        // We scale the kernel (and hence the resulting deformations) of the Gaussian kernel by  a factor of 100, in order
        // to achieve a visible effect.
        const MatrixValuedKernelType& scaledGk = ScaledKernel<vtkPoint>(&mvGk, 100.0);

        // The model kernel and the Gaussian kernel are combined to a new kernel.
        const MatrixValuedKernelType& combinedModelAndGaussKernel = SumKernel<vtkPoint>(&statModelKernel, &scaledGk);

        // We create a new model using the combined kernel. The new model will be more flexible than the original statistical model.
        boost::scoped_ptr<ModelBuilderType> modelBuilder(ModelBuilderType::Create(model->GetRepresenter()));
        boost::scoped_ptr<StatisticalModelType> combinedModel(modelBuilder->BuildNewModel(model->DrawMean(), combinedModelAndGaussKernel, numberOfComponents));

        // Once we have built the model, we can save it to disk.
        combinedModel->Save(outputModelFilename);
        std::cout << "Successfully saved shape model as " << outputModelFilename << std::endl;

    } catch (StatisticalModelException& e) {
        std::cout << "Exception occured while building the shape model" << std::endl;
        std::cout << e.what() << std::endl;
    }
}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#define BOOST_FILESYSTEM_NO_DEPRECATED

#include "boost/filesystem.hpp"
#include "boost/program_options.hpp"

#include "Molassembler/IO.h"
#include "Molassembler/Interpret.h"
#include "Molassembler/Graph.h"
#include "Molassembler/Molecule.h"

#include "Utils/Geometry/AtomCollection.h"
#include "Utils/Bonds/BondOrderCollection.h"
#include "Utils/IO/ChemicalFileFormats/ChemicalFileHandler.h"

#include <iostream>
#include <fstream>
#include <Eigen/Dense>

void readTriples(const std::string& file, Scine::Utils::BondOrderCollection& bondOrders) {
  std::ifstream in(file);
  std::string line;

  unsigned numBondOrders = 0;
  unsigned i;
  unsigned j;
  double value;
  if(in.is_open()) {
    while(true) {
      in >> i >> j >> value;
      if(in.eof() || in.fail()) {
        break;
      }

      bondOrders.setOrder(i, j, value);
      ++numBondOrders;
    }
  }

  std::cout << "Read " << numBondOrders << " bond orders from '" << file << "'\n";
}

Eigen::MatrixXd readCSV(const std::string& file, int rows, int cols) {
  std::ifstream in(file);

  std::string line;

  int row = 0;
  int col = 0;

  Eigen::MatrixXd res(rows, cols);

  if (in.is_open()) {
    while (std::getline(in, line)) {
      char *ptr = const_cast<char *>(line.c_str());
      int len = line.length();

      col = 0;

      char *start = ptr;
      for (int i = 0; i < len; i++) {

        if (ptr[i] == ',') {
          res(row, col++) = atof(start);
          start = ptr + i + 1;
        }
      }
      res(row, col) = atof(start);

      row++;
    }

    in.close();
  }

  return res;
}

int main(int argc, char* argv[]) {
  using namespace Scine;
  using namespace Molassembler;

  // Set up option parsing
  boost::program_options::options_description options_description("Recognized options");
  options_description.add_options()
    ("help", "Produce help message")
    ("m", boost::program_options::value<std::string>(), "Molecule data file (.xyz, .mol, etc)")
    ("bo-csv", boost::program_options::value<std::string>(), "Matrix CSV file to read as bond orders")
    ("bo-triples", boost::program_options::value<std::string>(), "Matrix triples list file to read as bond orders")
    ("threshold", boost::program_options::value<double>(), "Manual threshold for bond stereopermutator interpretation")
  ;

  // Parse
  boost::program_options::variables_map options_variables_map;
  boost::program_options::store(
    boost::program_options::parse_command_line(argc, argv, options_description),
    options_variables_map
  );
  boost::program_options::notify(options_variables_map);

  if(options_variables_map.count("m") > 0) {
    boost::filesystem::path filepath {
      options_variables_map["m"].as<std::string>()
    };

    if(!boost::filesystem::exists(filepath)) {
      std::cout << "The molecular file does not exist" << std::endl;
      return 1;
    }

    // This can throw in lots of cases
    auto readData = Utils::ChemicalFileHandler::read(filepath.string());
    const auto& atomCollection = readData.first;
    auto& bondOrders = readData.second;

    if(bondOrders.empty()) {
      if(options_variables_map.count("bo-csv") == 0 && options_variables_map.count("bo-triples") == 0) {
        std::cout << "The molecule data file did not contain bond order information and you did not supply a file to read as bond orders\n";
        return 1;
      }

      const int N = atomCollection.size();
      bondOrders.resize(N);

      if(options_variables_map.count("bo-csv") == 1) {
        auto fileMatrix = readCSV(
          options_variables_map["bo-csv"].as<std::string>(),
          N,
          N
        );
        for(Eigen::Index i = 0; i < N; ++i) {
          for(Eigen::Index j = i + 1; j < N; ++j) {
            if(fileMatrix(i, j) > 0) {
              bondOrders.setOrder(i, j, fileMatrix(i, j));
            }
          }
        }
      } else if(options_variables_map.count("bo-triples") == 1) {
        readTriples(
          options_variables_map["bo-triples"].as<std::string>(),
          bondOrders
        );
      }
    }

    double defaultThreshold = 1.4;
    if(options_variables_map.count("threshold") == 1) {
      defaultThreshold = options_variables_map["threshold"].as<double>();
    }

    auto interpretation = Interpret::molecules(atomCollection, bondOrders, Interpret::BondDiscretizationOption::RoundToNearest, defaultThreshold);
    auto positions = interpretation.componentMap.apply(atomCollection);

    if(interpretation.molecules.size() == 1) {
      std::ofstream graphFile("interpreted.dot");
      graphFile << interpretation.molecules.front().dumpGraphviz();
      graphFile.close();

      IO::write("interpreted.json", interpretation.molecules.front());
      IO::write("interpreted.mol", interpretation.molecules.front(), positions.front().getPositions());
    } else {
      for(unsigned i = 0; i < interpretation.molecules.size(); ++i) {
        const auto& mol = interpretation.molecules[i];
        std::string filebase = "interpreted-" + std::to_string(i);

        std::ofstream graphFile(filebase + ".dot");
        graphFile << mol.dumpGraphviz();
        graphFile.close();

        IO::write(filebase + ".json", mol);
        IO::write(filebase + ".mol", mol, positions.at(i).getPositions());
      }
    }

    for(const auto& mol : interpretation.molecules) {
      std::cout << "Interpreted molecule N=" << mol.graph().N() << ": "
        << mol << "\n\n";
    }

  } else {
    std::cout << options_description << std::endl;
  }

  return 0;
}


// Practice 4.3

#include <iostream>
#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/multiplies.hpp>
using namespace boost::mpl::placeholders;
#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>

namespace mpl = boost::mpl;

struct test
{
	template<class N>
	struct apply
	{
		typedef typename mpl::equal_to< N, mpl::int_<2> >::type type;
	};
};

// 1
template <typename N, typename Predicate >
struct next_if :
	mpl::eval_if<
	typename mpl::apply<Predicate,N>::type
	,mpl::next<N>
	,N>
{ };

// 2
template <typename N1, typename N2>
struct formula :
	mpl::eval_if<
		mpl::not_equal_to<N1, N2>
		, mpl::eval_if<
			mpl::greater<N1,N2>
			, mpl::minus<N1,N2>
			, N1
			>
		, mpl::plus< N1, mpl::multiplies<N1, mpl::int_<2> > >
	>
{ };

void main()
{
	std::cout << "aa" << std::endl;

	// 1
	bool r1 = mpl::equal_to<
		typename next_if< mpl::int_<2>, test >::type
		, mpl::int_<3>
	>::value;

	// 2
	bool r2 = mpl::equal_to<
		typename formula< mpl::int_<3>, mpl::int_<2> >
		, mpl::int_<1>
	>::value;

}


#ifndef HEADER_SHAPEMODEL
#define HEADER_SHAPEMODEL

#include <string>
#include <iostream>
#include <armadillo>
#include <set>
#include <map>
#include <limits>

#include <ShapeUQLib/FrameGraph.hpp>

class Element;

/**
Declaration of the ShapeModel class. Base class for
the implementation of shape model
*/
template <class PointType>
class ShapeModel {

public:

	/**
	Constructor
	*/
	ShapeModel();

	/**
	Constructor
	@param frame_graph Pointer to the graph storing
	reference frame relationships
	@param frame_graph Pointer to the reference frame graph
	*/
	ShapeModel(std::string ref_frame_name,
		FrameGraph * frame_graph);

	/**
	Returns the dimensions of the bounding box
	@param Bounding box dimension to be computed (xmin,ymin,zmin,xmax,ymax,zmax)
	*/
	void get_bounding_box(double * bounding_box,arma::mat M = arma::eye<arma::mat>(3,3)) const;

	/**
	Translates the shape model by x
	@param x translation vector applied to the coordinates of each control point
	*/
	void translate(const arma::vec::fixed<3> &);

	/**
	Rotates the shape model by
	@param M rotation matrix
	*/
	void rotate(const arma::mat::fixed<3,3> & M);

	/**
	Shifts the coordinates of the shape model
	so as to have (0,0,0) aligned with its barycenter
	The resulting barycenter coordinates are (0,0,0)
	*/
	void shift_to_barycenter();

	/**
	Applies a rotation that aligns the body
	with its principal axes.
	This assumes that the body has been shifted so
	that (0,0,0) lies at its barycenter
	The resulting inertia tensor is diagonal
	Undefined behavior if
	the inertia tensor has not been computed beforehand
	*/
	void align_with_principal_axes();

	/**
	Returns the volume of the provided shape model
	@return volume (U^2 where U is the unit of the shape coordinates)
	*/
	double get_volume() const;

	/**
	Returns the principal axes and principal moments of the shape model
	@param axes M as in X = MX' where X' is a position expressed in the principal frame
	@param moments dimensionless inertia moments in ascending order
	*/
	void get_principal_inertias(arma::mat & axes,arma::vec & moments) const;

	/**
	Defines the reference frame attached to the shape model
	@param ref_frame Pointer to the reference frame attached
	to the shape model
	*/
	void set_ref_frame_name(std::string ref_frame_name);

	/**
	Returns the name of the reference frame attached to this
	ref frame
	@return name of reference frame
	*/
	std::string get_ref_frame_name() const;

	PointType & get_point(unsigned int i) ;
	const arma::vec::fixed<3> & get_point_coordinates(unsigned int i) const;

	virtual arma::vec::fixed<3> get_point_normal_coordinates(unsigned int i) const = 0;

	/**
	Pointer to the shape model's control points
	@return vertices pointer to the control points
	*/
	const std::vector<PointType> & get_points() const;

	unsigned int get_point_index(std::shared_ptr<PointType> point) const;

	/**
	Returns the geometrical center of the shape
	@return geometrical center
	*/
	arma::vec::fixed<3> get_center() const;

	/**
	Augment the internal container storing vertices with a new (and not already inserted)
	one
	@param control_point pointer to the new control point to be inserted
	*/
	void add_control_point(PointType & control_point);

	virtual void clear() = 0;

	/**
	Returns number of elements
	@return number of elements
	*/
	virtual unsigned int get_NElements() const  = 0;

	/**
	Returns number of control points
	@return number of control points
	*/
	unsigned int get_NControlPoints() const ;

	/**
	Computes the surface area of the shape model
	*/
	virtual void compute_surface_area() = 0;
	/**
	Computes the volume of the shape model
	*/
	virtual void compute_volume() = 0;

	/**
	Computes the center of mass of the shape model
	*/
	virtual void compute_center_of_mass() = 0;
	/**
	Computes the inertia tensor of the shape model
	*/
	virtual void compute_inertia() = 0;

	/**
	Constructs a connectivity table associated a control point pointer to its index
	in this shape model control points vector
	*/
	void initialize_index_table();

	/**
	Returns the non-dimensional inertia tensor of the body in the body-fixed
	principal axes. (rho == 1, l = (volume)^(1/3))
	@return principal inertia tensor
	*/
	const arma::mat::fixed<3,3> & get_inertia() const;

	/**
	Updates shape geometric and mass properties
	*/
	virtual void update_mass_properties() = 0;

	/**
	Returns the surface area of the shape model
	@return surface area (U^2 where U is the unit of the shape coordinates)
	*/
	double get_surface_area() const;

	/**
	Returns the location of the center of mass
	@return pointer to center of mass
	*/
	const arma::vec::fixed<3> & get_center_of_mass() const;

	/**
	Builds the covariance of the provided control points
	@param P covariance to set
	@param Ci pointer to first point
	@param Cj pointer to second point
	@param Ck pointer to third point
	@param Cl pointer to fourth point
	@param Cm pointer to fifth point
	@param Cp pointer to sixth point
	*/
	static void assemble_covariance(arma::mat & P,
		const PointType & Ci,
		const PointType & Cj,
		const PointType & Ck,
		const PointType & Cl,
		const PointType & Cm,
		const PointType & Cp);

	/**
	Builds the covariance of the provided control points
	@param P covariance to set
	@param Ci pointer to first point
	@param Cj pointer to second point
	@param Ck pointer to third point
	@param Cl pointer to fourth point
	@param Cm pointer to fifth point
	@param Cp pointer to sixth point
	@param Cq pointer to seventh point
	*/
	static void assemble_covariance(arma::mat & P,
		const PointType & Ci,
		const PointType & Cj,
		const PointType & Ck,
		const PointType & Cl,
		const PointType & Cm,
		const PointType & Cp,
		const PointType & Cq);

	/**
	Builds the covariance of the provided control points
	@param P covariance to set
	@param Ci pointer to first point
	@param Cj pointer to second point
	@param Ck pointer to third point
	@param Cl pointer to fourth point
	@param Cm pointer to fifth point
	@param Cp pointer to sixth point
	@param Cq pointer to seventh point
	@param Cq pointer to eigth point
	*/
	static void assemble_covariance(arma::mat & P,
		const PointType & Ci,
		const PointType & Cj,
		const PointType & Ck,
		const PointType & Cl,
		const PointType & Cm,
		const PointType & Cp,
		const PointType & Cq,
		const PointType & Cr);

	/**
	Returns radius of circumscribing sphere, measured from the shape's center of mass
	@return radius of circumscribing sphere
	*/
	double get_circumscribing_radius() const;

	arma::vec get_inertia_param() const;

	virtual const std::vector<int> & get_element_control_points(int e) const = 0;

protected:

	std::vector<std::set<int> > edges;
	std::vector<PointType> control_points;

	std::map<std::shared_ptr<PointType> ,unsigned int> pointer_to_global_index;

	FrameGraph * frame_graph;
	std::string ref_frame_name;

	arma::mat::fixed<3,3> inertia;
	arma::vec::fixed<3> cm;
	double volume;
	double surface_area;

	/**
	Radius of sphere of equivalent volume
	*/
	double r_avg;

};

#endif


#include <ros/ros.h>
#include <iostream>
#include <vector>
#include <std_msgs/Float32.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Point.h>

#include <darknet_ros_msgs/BoundingBox.h>
#include <darknet_ros_msgs/BoundingBoxes.h>
#include <master_layer/ChangeOdom.h>
#include <Eigen/Dense>

double z, y, x;
double z_avg, y_avg, x_avg;
double y_ml, z_ml, x_ml;

nav_msgs::Odometry odom_data;

std::vector<double> z_coord;
std::vector<double> y_coord;
std::vector<double> x_coord;

int z_count = 0;
int y_count = 0;
int x_count = 0;

bool odom_init = false;

Eigen::Matrix3f quaternion_matrix;

std::string odom_source = "dvl";

// I have not yet added anything to retrieve the current task
std::string current_task = "vampire";

void ml_callback(const darknet_ros_msgs::BoundingBoxes &msg)
{
    double x_len, y_len;
    int number_of_objects = sizeof(msg.bounding_boxes)/sizeof(msg.bounding_boxes[0]);
    for(int i = 0; i < number_of_objects; i++)
    {
        if(msg.bounding_boxes[i].Class == current_task)
        {
            ROS_INFO("Inside ML Callback");
            y_ml = (msg.bounding_boxes[i].xmin + msg.bounding_boxes[i].xmax)/2 - 320;
            z_ml = 320 - (msg.bounding_boxes[i].ymin + msg.bounding_boxes[i].ymax)/2;
            y_len = (msg.bounding_boxes[i].ymax - msg.bounding_boxes[i].ymin);
            x_len = (msg.bounding_boxes[i].ymax - msg.bounding_boxes[i].ymin);
            x_ml = pow(y_len*y_len + x_len*x_len, 0.5);

        }
    }
}

bool changeOdom (master_layer::ChangeOdom::Request &req,
                master_layer::ChangeOdom::Response &res) {
    odom_source = req.odom;
    res.success = true;
    return true;
}

double avg (std::vector<double> array) {
    double sum = 0;
    for (int i = 0; i < array.size(); i++) {
        sum += array[i];
    }
    double size = array.size();
    double avg_ = sum/size;

    return avg_;
}

bool inRange (double x, double avg, double thres) {
    if (avg + thres >= x || avg - thres <= x) {
        return true;
    }
    return false;
}

void zCallback (const std_msgs::Float32 msg) {
    z = -msg.data;
    if (z_count < 10) {
        z_coord[z_count] = z;
        z_avg = avg (z_coord);
        z_count++;
    }
    else {
        z_avg = avg (z_coord);
        if (inRange(z, z_avg, 15)) {
            std::rotate (z_coord.begin(), z_coord.begin() + 1, z_coord.end());
            z_coord[9] = z;
        }
    }
}

void yCallback (const std_msgs::Float32 msg) {
    y = msg.data;
    if (y_count < 10) {
        y_coord[y_count] = y;
        y_avg = avg (y_coord);
        y_count++;
    }
    else {
        y_avg = avg (y_coord);
        if (inRange(y, y_avg, 15)) {
            std::rotate (y_coord.begin(), y_coord.begin() + 1, y_coord.end());
            y_coord[9] = y;
        }
    }
}

void xCallback (const geometry_msgs::Point msg) {
    x = msg.z;
    if (x_count < 10) {
        x_coord[x_count] = x;
        x_avg = avg (x_coord);
        x_count++;
    }
    else {
        x_avg = avg (x_coord);
        if (inRange(x, x_avg, 1)) {
            std::rotate (x_coord.begin(), x_coord.begin() + 1, x_coord.end());
            x_coord[9] = x;
        }
    }
}

void dvlCallback (const nav_msgs::Odometry msg) {
    odom_data.pose.pose.position.x = msg.pose.pose.position.x;
    odom_data.pose.pose.position.y = msg.pose.pose.position.y;
    odom_data.pose.pose.position.z = msg.pose.pose.position.z;
    odom_data.pose.pose.orientation.x = msg.pose.pose.orientation.x;
    odom_data.pose.pose.orientation.y = msg.pose.pose.orientation.y;
    odom_data.pose.pose.orientation.z = msg.pose.pose.orientation.z;
    odom_data.pose.pose.orientation.w = msg.pose.pose.orientation.w;
    odom_data.twist.twist.linear.x = msg.twist.twist.linear.x;
    odom_data.twist.twist.linear.y = msg.twist.twist.linear.y;
    odom_data.twist.twist.linear.z = msg.twist.twist.linear.z;
    odom_data.twist.twist.angular.x = msg.twist.twist.angular.x;
    odom_data.twist.twist.angular.y = msg.twist.twist.angular.y;
    odom_data.twist.twist.angular.z = msg.twist.twist.angular.z;
    odom_data.header.frame_id = msg.header.frame_id;
    odom_data.child_frame_id = msg.child_frame_id;
    odom_data.header.stamp = odom_data.header.stamp;
    odom_init = true;
}

void qmatrixUpdate () {
    geometry_msgs::Quaternion q = odom_data.pose.pose.orientation;
    quaternion_matrix(0, 0) = 1 - 2*q.y*q.y - 2*q.z*q.z;
    quaternion_matrix(0, 1) = 2*q.x*q.y - 2*q.z*q.w;
    quaternion_matrix(0, 2) = 2*q.x*q.z + 2*q.y*q.w;
    quaternion_matrix(1, 0) = 2*q.x*q.y + 2*q.z*q.w;
    quaternion_matrix(1, 1) = 1 - 2*q.x*q.x - 2*q.z*q.z;
    quaternion_matrix(1, 2) = 2*q.y*q.z - 2*q.x*q.w;
    quaternion_matrix(2, 0) = 2*q.x*q.z - 2*q.y*q.w;
    quaternion_matrix(2, 1) = 2*q.y*q.z + 2*q.x*q.w;
    quaternion_matrix(2, 2) = 1 - 2*q.x*q.x - 2*q.y*q.y;
}

void transform (geometry_msgs::Point& p) {
    Eigen::Vector3f p_world;
    p_world(0) = p.x;
    p_world(1) = p.y;
    p_world(2) = p.z;
    qmatrixUpdate ();
    Eigen::Vector3f p_body = quaternion_matrix.transpose()*p_world;
    p.x = p_body(0);
    p.y = p_body(1);
    p.z = p_body(2);
}

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

    ros::init (argc, argv, "local_vision_odom");
    ros::NodeHandle nh;

    ros::Subscriber z_sub = nh.subscribe("/anahita/front_camera/z_coordinate", 100, &zCallback);
    ros::Subscriber y_sub = nh.subscribe("/anahita/front_camera/y_coordinate", 100, &yCallback);
    ros::Subscriber odom_sub = nh.subscribe("/anahita/pose_gt/relay", 100, &dvlCallback);
    ros::Subscriber mean_coord_sub = nh.subscribe("/anahita/mean_coord", 100, &xCallback);

    ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("/anahita/pose_gt", 100);
    ros::Subscriber ml_sub = nh.subscribe("/anahita/bounding_boxes", 1, &ml_callback);

    ros::ServiceServer service = nh.advertiseService("odom_source", changeOdom);
    ros::Rate loop_rate(20);

    nav_msgs::Odometry odom_msg;

    z_coord.resize(10);
    y_coord.resize(10);
    x_coord.resize(10);

    while (ros::ok()) {

        if (odom_source == "dvl") {
            odom_msg = odom_data;
        }
        else if (odom_source == "vision") {
            odom_msg = odom_data;
            odom_msg.pose.pose.position.y = y_avg/200.0;
            odom_msg.pose.pose.position.z = z_avg/200.0;
            if (odom_init) transform (odom_msg.pose.pose.position);
        }
        else if (odom_source == "vision_ml"){
            odom_msg = odom_data;
            odom_msg.pose.pose.position.x = -x_ml;
            odom_msg.pose.pose.position.y = y_ml;
            odom_msg.pose.pose.position.z = z_ml;
        }
        else if (odom_source == "stereo_vision"){
            odom_msg = odom_data;
            odom_msg.pose.pose.position.y = y_avg/200.0;
            odom_msg.pose.pose.position.z = z_avg/200.0;
            if (odom_init) transform (odom_msg.pose.pose.position);
            odom_msg.pose.pose.position.x = -x_avg;
        }
        else {
            ROS_INFO("Invalid odom source");
            return 1;
        }

        odom_pub.publish(odom_msg);

        loop_rate.sleep();
        ros::spinOnce();
    }

}


/*
@copyright Louis Dionne 2015
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.hpp>

#include <test/identity.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/functional/compose.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/applicative.hpp>
#include <laws/base.hpp>
#include <laws/functor.hpp>
#include <laws/monad.hpp>
#include <test/cnumeric.hpp>
using namespace boost::hana;

int main() {
    using test::ct_eq;
    using test::cnumeric;
    test::_injection<0> f{};

    // Functor
    {
        auto functor = test::identity;
        // adjust_if
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                adjust_if(functor(ct_eq<0>{}), always(cnumeric<bool, true>), f),
                functor(f(ct_eq<0>{}))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                adjust_if(functor(ct_eq<0>{}), always(cnumeric<bool, false>), f),
                functor(ct_eq<0>{})
            ));
        }

        // fill
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                fill(functor(ct_eq<0>{}), ct_eq<1>{}),
                functor(ct_eq<1>{})
            ));
        }

        // transform
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                transform(functor(ct_eq<0>{}), f),
                functor(f(ct_eq<0>{}))
            ));
        }

        // replace_if
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                replace_if(functor(ct_eq<0>{}), always(cnumeric<bool, true>), ct_eq<1>{}),
                functor(ct_eq<1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                replace_if(functor(ct_eq<0>{}), always(cnumeric<bool, false>), ct_eq<1>{}),
                functor(ct_eq<0>{})
            ));
        }
    }

    // Applicative
    {
        auto a = test::identity;
        using A = test::Identity;

        // ap
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(a(f), a(ct_eq<0>{})),
                a(f(ct_eq<0>{}))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(a(f), a(ct_eq<0>{}), a(ct_eq<1>{})),
                a(f(ct_eq<0>{}, ct_eq<1>{}))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(a(f), a(ct_eq<0>{}), a(ct_eq<1>{}), a(ct_eq<2>{})),
                a(f(ct_eq<0>{}, ct_eq<1>{}, ct_eq<2>{}))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(a(f), a(ct_eq<0>{}), a(ct_eq<1>{}), a(ct_eq<2>{}), a(ct_eq<3>{})),
                a(f(ct_eq<0>{}, ct_eq<1>{}, ct_eq<2>{}, ct_eq<3>{}))
            ));
        }

        // lift
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                lift<A>(ct_eq<0>{}),
                a(ct_eq<0>{})
            ));
        }
    }

    // Monad
    {
        auto monad = test::identity;
        using M = test::Identity;
        auto f = compose(monad, test::_injection<0>{});

        // chain
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                chain(monad(ct_eq<1>{}), f),
                f(ct_eq<1>{})
            ));
        }

        // tap
        {
            bool executed = false;
            auto exec = [&](auto) { executed = true; };
            BOOST_HANA_CONSTANT_CHECK(equal(
                chain(monad(ct_eq<0>{}), tap<M>(exec)),
                monad(ct_eq<0>{})
            ));
            BOOST_HANA_RUNTIME_CHECK(executed);
        }

        // then
        {
            struct invalid { };
            BOOST_HANA_CONSTANT_CHECK(equal(
                then(monad(invalid{}), monad(ct_eq<0>{})),
                monad(ct_eq<0>{})
            ));
        }
    }
}


#include <boost/msm/front/euml/euml.hpp>
#include <boost/msm/front/euml/state_grammar.hpp>
#include <boost/msm/back/state_machine.hpp>
#include <iostream>

namespace msm = boost::msm;
using namespace boost::msm::front::euml;

BOOST_MSM_EUML_DECLARE_ATTRIBUTE(int, switched_on)

void write_message()
{
  std::cout << "Switched on\n";
}

BOOST_MSM_EUML_FUNCTION(WriteMessage_, write_message, write_message_,
  void, void)

BOOST_MSM_EUML_STATE((), Off)
BOOST_MSM_EUML_STATE((), On)

BOOST_MSM_EUML_EVENT(press)

BOOST_MSM_EUML_TRANSITION_TABLE((
  Off + press [fsm_(switched_on) < Int_<2>()] / (++fsm_(switched_on),
    write_message_()) == On,
  On + press == Off
), light_transition_table)

BOOST_MSM_EUML_DECLARE_STATE_MACHINE(
(light_transition_table, init_ << Off, no_action, no_action,
attributes_ << switched_on), light_state_machine)

int main()
{
  msm::back::state_machine<light_state_machine> light;
  light.process_event(press);
  light.process_event(press);
  light.process_event(press);
  light.process_event(press);
  light.process_event(press);
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

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

#include <utility>
using std::pair;

#include <armadillo>
using arma::Row;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenRowVecElemIndGenDouble: public Expected {
    public:
      ExpectedInPlaceGenRowVecElemIndGenDouble() {
        cout << "Compute ExpectedInPlaceGenRowVecElemIndGenDouble(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters( {
            InputClass::GenRowVec,
            InputClass::ElemInd,
            InputClass::GenDouble
          });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genRowVec = *static_cast<Row<double>*>(value.second);
                break;
              case 1:
                _fileSuffix += "," + value.first;
                _elemInd = *static_cast<int*>(value.second);
                break;
              case 2:
                _fileSuffix += "," + value.first;
                _genDouble = *static_cast<double*>(value.second);
                break;
            }
            ++n;
          }

          cout << "Using input: " << _fileSuffix << endl;

          _copyOfGenRowVec = _genRowVec;
          _copyOfElemInd = _elemInd;
          _copyOfGenDouble = _genDouble;

          expectedRowVecAtEqual();

          _genRowVec = _copyOfGenRowVec;
          _elemInd = _copyOfElemInd;
          _genDouble = _copyOfGenDouble;

          expectedRowVecAtPlus();

          _genRowVec = _copyOfGenRowVec;
          _elemInd = _copyOfElemInd;
          _genDouble = _copyOfGenDouble;

          expectedRowVecAtMinus();

          _genRowVec = _copyOfGenRowVec;
          _elemInd = _copyOfElemInd;
          _genDouble = _copyOfGenDouble;

          expectedRowVecAtTimes();

          _genRowVec = _copyOfGenRowVec;
          _elemInd = _copyOfElemInd;
          _genDouble = _copyOfGenDouble;

          expectedRowVecAtDivide();

          _genRowVec = _copyOfGenRowVec;
          _elemInd = _copyOfElemInd;
          _genDouble = _copyOfGenDouble;

          expectedRowVecColPlus();

          _genRowVec = _copyOfGenRowVec;
          _elemInd = _copyOfElemInd;
          _genDouble = _copyOfGenDouble;

          expectedRowVecColMinus();

          _genRowVec = _copyOfGenRowVec;
          _elemInd = _copyOfElemInd;
          _genDouble = _copyOfGenDouble;

          expectedRowVecColTimes();

          _genRowVec = _copyOfGenRowVec;
          _elemInd = _copyOfElemInd;
          _genDouble = _copyOfGenDouble;

          expectedRowVecColDivide();
        }

        cout << "done." << endl;
      }

    protected:
      Row<double> _genRowVec;
      Row<double> _copyOfGenRowVec;

      int _elemInd;
      int _copyOfElemInd;

      double _genDouble;
      double _copyOfGenDouble;

      void expectedRowVecAtEqual() {
        if (_elemInd >= _genRowVec.n_elem) {
          return;
        }

        _genRowVec.at(_elemInd) = _genDouble;

        cout << "- Compute expectedRowVecAtEqual() ... ";
        save<double>("Row.atEqual", _genRowVec);
        cout << "done." << endl;
      }

      void expectedRowVecAtPlus() {
        if (_elemInd >= _genRowVec.n_elem) {
          return;
        }

        _genRowVec.at(_elemInd) += _genDouble;

        cout << "- Compute expectedRowVecAtPlus() ... ";
        save<double>("Row.atPlus", _genRowVec);
        cout << "done." << endl;
      }

      void expectedRowVecAtMinus() {
        if (_elemInd >= _genRowVec.n_elem) {
          return;
        }

        _genRowVec.at(_elemInd) -= _genDouble;

        cout << "- Compute expectedRowVecAtMinus() ... ";
        save<double>("Row.atMinus", _genRowVec);
        cout << "done." << endl;
      }

      void expectedRowVecAtTimes() {
        if (_elemInd >= _genRowVec.n_elem) {
          return;
        }

        _genRowVec.at(_elemInd) *= _genDouble;

        cout << "- Compute expectedRowVecAtTimes() ... ";
        save<double>("Row.atTimes", _genRowVec);
        cout << "done." << endl;
      }

      void expectedRowVecAtDivide() {
        if (_elemInd >= _genRowVec.n_elem) {
          return;
        }

        _genRowVec.at(_elemInd) /= _genDouble;

        cout << "- Compute expectedRowVecAtDivide() ... ";
        save<double>("Row.atDivide", _genRowVec);
        cout << "done." << endl;
      }

      void expectedRowVecColPlus() {
        if (_elemInd >= _genRowVec.n_elem) {
          return;
        }

        _genRowVec.col(_elemInd) += _genDouble;

        cout << "- Compute expectedRowVecRowPlus() ... ";
        save<double>("Row.colPlus", _genRowVec);
        cout << "done." << endl;
      }

      void expectedRowVecColMinus() {
        if (_elemInd >= _genRowVec.n_elem) {
          return;
        }

        _genRowVec.col(_elemInd) -= _genDouble;

        cout << "- Compute expectedRowVecRowMinus() ... ";
        save<double>("Row.colMinus", _genRowVec);
        cout << "done." << endl;
      }

      void expectedRowVecColTimes() {
        if (_elemInd >= _genRowVec.n_elem) {
          return;
        }

        _genRowVec.col(_elemInd) *= _genDouble;

        cout << "- Compute expectedRowVecRowTimes() ... ";
        save<double>("Row.colTimes", _genRowVec);
        cout << "done." << endl;
      }

      void expectedRowVecColDivide() {
        if (_elemInd >= _genRowVec.n_elem) {
          return;
        }

        _genRowVec.col(_elemInd) /= _genDouble;

        cout << "- Compute expectedRowVecRowDivide() ... ";
        save<double>("Row.colDivide", _genRowVec);
        cout << "done." << endl;
      }

  };
}


/*
 *  testTrackedPolyhedronSensitivity.cpp
 *
 *  Created by Paul C Hansen on 6/27/11.
 *  Copyright 2011 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE testFillFactors

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <cstdlib>
#include <ctime>
#include "Voxelize.h"
#include "SimpleMesh/Search.h"

using namespace std;

BOOST_AUTO_TEST_CASE( StandardCube )
{
    Rect3d rect1 = Rect3d(-1,-1,-1,1,1,1);
    std::vector<SimpleMesh::ControlVertex> controlVertices1, controlVertices2;
    std::vector<SimpleMesh::Triangle> meshTris1, meshTris2;
    SimpleMesh::Test::oneBoxMesh(controlVertices1, meshTris1, rect1);

    Rect3d bounds(-3,-2,-1,3,2,1);
    Rect3i voxels(-1,-1,-1,1,1,1);
    Voxelize vox(bounds, voxels);

    RLE::DynamicRLE3<double> ff = vox.fillFactors(meshTris1);

    Vector3d voxSize = vox.voxelSize();
    double cellVolume = voxSize[0]*voxSize[1]*voxSize[2];

    for (int xx = -1; xx <= 1; xx++)
    for (int yy = -1; yy <= 1; yy++)
    for (int zz = -1; zz <= 1; zz++)
    {
        double volume = cellVolume;

        if (xx == -1 || xx == 1)
            volume *= 0;

        if (yy == -1 || yy == 1)
            volume *= 0.25;

        if (volume != 0.0)
            BOOST_CHECK_CLOSE(ff.at(xx,yy,zz), volume, 1e-10);
        else
            BOOST_CHECK_SMALL(ff.at(xx,yy,zz), 1e-10);
    }
}

BOOST_AUTO_TEST_CASE( Cube2x2x2 )
{
    Rect3d rect1 = Rect3d(-1,-1,-1,1,1,1);
    std::vector<SimpleMesh::ControlVertex> controlVertices1, controlVertices2;
    std::vector<SimpleMesh::Triangle> meshTris1, meshTris2;
    SimpleMesh::Test::oneBoxMesh(controlVertices1, meshTris1, rect1);

    Rect3d bounds(-2,-2,-2,2,2,2);
    Rect3i voxels(0,0,0,3,3,3);
    Voxelize vox(bounds, voxels);

    RLE::DynamicRLE3<double> ff = vox.fillFactors(meshTris1);

    for (int xx = 0; xx < 4; xx++)
    for (int yy = 0; yy < 4; yy++)
    for (int zz = 0; zz < 4; zz++)
    if (xx >= 1 && xx <= 2 && yy >= 1 && yy <= 2 && zz >= 1 && zz <= 2)
    {
        BOOST_CHECK_CLOSE(ff.at(xx,yy,zz), 1.0, 1e-10);
    }
    else
    {
        BOOST_CHECK_SMALL(ff.at(xx,yy,zz), 1e-10);
    }
}

BOOST_AUTO_TEST_CASE( Cube1x1x1 )
{
    Rect3d rect1 = 0.5*Rect3d(-1,-1,-1,1,1,1);
    std::vector<SimpleMesh::ControlVertex> controlVertices1, controlVertices2;
    std::vector<SimpleMesh::Triangle> meshTris1, meshTris2;
    SimpleMesh::Test::oneBoxMesh(controlVertices1, meshTris1, rect1);

    Rect3d bounds(-2,-2,-2,2,2,2);
    Rect3i voxels(0,0,0,3,3,3);
    Voxelize vox(bounds, voxels);

    RLE::DynamicRLE3<double> ff = vox.fillFactors(meshTris1);

    for (int xx = 0; xx < 4; xx++)
    for (int yy = 0; yy < 4; yy++)
    for (int zz = 0; zz < 4; zz++)
    if (xx >= 1 && xx <= 2 && yy >= 1 && yy <= 2 && zz >= 1 && zz <= 2)
    {
        BOOST_CHECK_CLOSE(ff.at(xx,yy,zz), 0.125, 1e-10);
    }
    else
    {
        BOOST_CHECK_SMALL(ff.at(xx,yy,zz), 1e-10);
    }
}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center.

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#include "mitkTractClusteringFilter.h"

#define _USE_MATH_DEFINES
#include <math.h>
#include <boost/progress.hpp>
#include <vnl/vnl_sparse_matrix.h>

namespace mitk{

TractClusteringFilter::TractClusteringFilter()
  : m_NumPoints(12)
  , m_InCentroids(nullptr)
  , m_MinClusterSize(1)
  , m_MaxClusters(0)
  , m_MergeDuplicateThreshold(-1)
  , m_DoResampling(true)
  , m_FilterMask(nullptr)
  , m_OverlapThreshold(0.0)
{

}

TractClusteringFilter::~TractClusteringFilter()
{
  for (auto m : m_Metrics)
    delete m;
}

std::vector<std::vector<unsigned int> > TractClusteringFilter::GetOutFiberIndices() const
{
  return m_OutFiberIndices;
}

void TractClusteringFilter::SetMetrics(const std::vector<mitk::ClusteringMetric *> &Metrics)
{
  m_Metrics = Metrics;
}

std::vector<TractClusteringFilter::Cluster> TractClusteringFilter::GetOutClusters() const
{
  return m_OutClusters;
}

std::vector<mitk::FiberBundle::Pointer> TractClusteringFilter::GetOutCentroids() const
{
  return m_OutCentroids;
}

std::vector<mitk::FiberBundle::Pointer> TractClusteringFilter::GetOutTractograms() const
{
  return m_OutTractograms;
}

void TractClusteringFilter::SetDistances(const std::vector<float> &Distances)
{
  m_Distances = Distances;
}

float TractClusteringFilter::CalcOverlap(vnl_matrix<float>& t)
{
  float overlap = 0;
  if (m_FilterMask.IsNotNull())
  {

    for (unsigned int i=0; i<m_NumPoints; ++i)
    {
      vnl_vector<float> p = t.get_column(i);
      itk::Point<float,3> point;
      point[0] = p[0];
      point[1] = p[1];
      point[2] = p[2];
      itk::Index<3> idx;
      m_FilterMask->TransformPhysicalPointToIndex(point, idx);
      if (m_FilterMask->GetLargestPossibleRegion().IsInside(idx) && m_FilterMask->GetPixel(idx)>0)
        overlap += 1;
    }
    overlap /= m_NumPoints;
  }
  else
    return 1.0;

  return overlap;
}

std::vector<vnl_matrix<float> > TractClusteringFilter::ResampleFibers(mitk::FiberBundle::Pointer tractogram)
{
  mitk::FiberBundle::Pointer temp_fib = tractogram->GetDeepCopy();
  if (m_DoResampling)
    temp_fib->ResampleToNumPoints(m_NumPoints);

  std::vector< vnl_matrix<float> > out_fib;

  for (int i=0; i<temp_fib->GetFiberPolyData()->GetNumberOfCells(); i++)
  {
    vtkCell* cell = temp_fib->GetFiberPolyData()->GetCell(i);
    int numPoints = cell->GetNumberOfPoints();
    vtkPoints* points = cell->GetPoints();

    vnl_matrix<float> streamline;
    streamline.set_size(3, m_NumPoints);
    streamline.fill(0.0);

    for (int j=0; j<numPoints; j++)
    {
      double cand[3];
      points->GetPoint(j, cand);

      vnl_vector_fixed< float, 3 > candV;
      candV[0]=cand[0]; candV[1]=cand[1]; candV[2]=cand[2];
      streamline.set_column(j, candV);
    }

    out_fib.push_back(streamline);
  }

  return out_fib;
}

std::vector< TractClusteringFilter::Cluster > TractClusteringFilter::ClusterStep(std::vector< unsigned int > f_indices, std::vector<float> distances)
{
  float dist_thres = distances.back();
  distances.pop_back();
  std::vector< Cluster > C;

  int N = f_indices.size();

  Cluster c1;
  c1.I.push_back(f_indices.at(0));
  c1.h = T[f_indices.at(0)];
  c1.n = 1;
  C.push_back(c1);
  if (f_indices.size()==1)
    return C;

  for (int i=1; i<N; ++i)
  {
    vnl_matrix<float> t = T.at(f_indices.at(i));

    int min_cluster_index = -1;
    float min_cluster_distance = 99999;
    bool flip = false;

    for (unsigned int k=0; k<C.size(); ++k)
    {
      vnl_matrix<float> v = C.at(k).h / C.at(k).n;
      bool f = false;
      float d = 0;
      for (auto m : m_Metrics)
        d += m->CalculateDistance(t, v, f);
      d /= m_Metrics.size();

      if (d<min_cluster_distance)
      {
        min_cluster_distance = d;
        min_cluster_index = k;
        flip = f;
      }
    }

    if (min_cluster_index>=0 && min_cluster_distance<dist_thres)
    {
      C[min_cluster_index].I.push_back(f_indices.at(i));
      if (!flip)
        C[min_cluster_index].h += t;
      else
        C[min_cluster_index].h += t.fliplr();
      C[min_cluster_index].n += 1;
    }
    else
    {
      Cluster c;
      c.I.push_back(f_indices.at(i));
      c.h = t;
      c.n = 1;
      C.push_back(c);
    }
  }

  if (!distances.empty())
  {
    std::vector< Cluster > outC;
#pragma omp parallel for
    for (int c=0; c<(int)C.size(); c++)
    {

      std::vector< Cluster > tempC = ClusterStep(C.at(c).I, distances);
#pragma omp critical
      AppendCluster(outC, tempC);
    }
    return outC;
  }
  else
    return C;
}

void TractClusteringFilter::AppendCluster(std::vector< Cluster >& a, std::vector< Cluster >&b)
{
  for (auto c : b)
    a.push_back(c);
}

unsigned int TractClusteringFilter::GetDiscardedClusters() const
{
  return m_DiscardedClusters;
}

void TractClusteringFilter::SetDoResampling(bool DoResampling)
{
  m_DoResampling = DoResampling;
}

void TractClusteringFilter::SetOverlapThreshold(float OverlapThreshold)
{
  m_OverlapThreshold = OverlapThreshold;
}

void TractClusteringFilter::SetFilterMask(const UcharImageType::Pointer &FilterMask)
{
  m_FilterMask = FilterMask;
}

void TractClusteringFilter::SetMinClusterSize(unsigned int MinClusterSize)
{
  m_MinClusterSize = MinClusterSize;
}

void TractClusteringFilter::SetMaxClusters(unsigned int MaxClusters)
{
  m_MaxClusters = MaxClusters;
}

void TractClusteringFilter::SetNumPoints(unsigned int NumPoints)
{
  m_NumPoints = NumPoints;
}

void TractClusteringFilter::SetMergeDuplicateThreshold(float MergeDuplicateThreshold)
{
  m_MergeDuplicateThreshold = MergeDuplicateThreshold;
}

void TractClusteringFilter::SetInCentroids(const mitk::FiberBundle::Pointer &InCentroids)
{
  m_InCentroids = InCentroids;
}

void TractClusteringFilter::SetTractogram(const mitk::FiberBundle::Pointer &Tractogram)
{
  m_Tractogram = Tractogram;
}

std::vector< TractClusteringFilter::Cluster > TractClusteringFilter::MergeDuplicateClusters2(std::vector< TractClusteringFilter::Cluster >& clusters)
{
  if (m_MergeDuplicateThreshold<0)
    m_MergeDuplicateThreshold = m_Distances.at(0)/2;
  else if (m_MergeDuplicateThreshold==0)
    return clusters;

  MITK_INFO << "Merging duplicate clusters with distance threshold " << m_MergeDuplicateThreshold;

  std::vector< TractClusteringFilter::Cluster > new_clusters;
  for (Cluster c1 : clusters)
  {
    vnl_matrix<float> t = c1.h / c1.n;

    int min_idx = -1;
    float min_d = 99999;
    bool flip = false;

#pragma omp parallel for
    for (int k2=0; k2<(int)new_clusters.size(); ++k2)
    {
      Cluster c2 = new_clusters.at(k2);
      vnl_matrix<float> v = c2.h / c2.n;

      bool f = false;
      float d = 0;
      for (auto m : m_Metrics)
        d += m->CalculateDistance(t, v, f);
      d /= m_Metrics.size();

#pragma omp critical
      if (d<min_d && d<m_MergeDuplicateThreshold)
      {
        min_d = d;
        min_idx = k2;
        flip = f;
      }
    }

    if (min_idx<0)
      new_clusters.push_back(c1);
    else
    {
      for (int i=0; i<c1.n; ++i)
      {
        new_clusters[min_idx].I.push_back(c1.I.at(i));
        new_clusters[min_idx].n += 1;
      }
      if (!flip)
        new_clusters[min_idx].h += c1.h;
      else
        new_clusters[min_idx].h += c1.h.fliplr();
    }
  }

  MITK_INFO << "\nNumber of clusters after merging duplicates: " << new_clusters.size();
  return new_clusters;
}

void TractClusteringFilter::MergeDuplicateClusters(std::vector< TractClusteringFilter::Cluster >& clusters)
{
  if (m_MergeDuplicateThreshold<0)
    m_MergeDuplicateThreshold = m_Distances.at(0)/2;
  bool found = true;

  MITK_INFO << "Merging duplicate clusters with distance threshold " << m_MergeDuplicateThreshold;
  int start = 0;
  while (found && m_MergeDuplicateThreshold>mitk::eps)
  {
    std::cout << "                                                                                \r";
    std::cout << "Number of clusters: " << clusters.size() << '\r';
    cout.flush();

    found = false;
    for (int k1=start; k1<(int)clusters.size(); ++k1)
    {
      Cluster c1 = clusters.at(k1);
      vnl_matrix<float> t = c1.h / c1.n;

      std::vector< int > merge_indices;
      std::vector< bool > flip_indices;

#pragma omp parallel for
      for (int k2=start+1; k2<(int)clusters.size(); ++k2)
      {
        if (k1!=k2)
        {
          Cluster c2 = clusters.at(k2);
          vnl_matrix<float> v = c2.h / c2.n;
          bool f = false;

          float d = 0;
          for (auto m : m_Metrics)
            d += m->CalculateDistance(t, v, f);
          d /= m_Metrics.size();

#pragma omp critical
          if (d<m_MergeDuplicateThreshold)
          {
            merge_indices.push_back(k2);
            flip_indices.push_back(f);
          }
        }
      }

      for (unsigned int i=0; i<merge_indices.size(); ++i)
      {
        Cluster c2 = clusters.at(merge_indices.at(i));
        for (int i=0; i<c2.n; ++i)
        {
          clusters[k1].I.push_back(c2.I.at(i));
          clusters[k1].n += 1;
        }
        if (!flip_indices.at(i))
          clusters[k1].h += c2.h;
        else
          clusters[k1].h += c2.h.fliplr();
      }

      std::sort(merge_indices.begin(), merge_indices.end());
      for (unsigned int i=0; i<merge_indices.size(); ++i)
      {
        clusters.erase (clusters.begin()+merge_indices.at(i)-i);
        found = true;
      }
      if (found)
        break;
    }
    ++start;
  }
  MITK_INFO << "\nNumber of clusters after merging duplicates: " << clusters.size();
}

std::vector<TractClusteringFilter::Cluster> TractClusteringFilter::AddToKnownClusters(std::vector< unsigned int > f_indices, std::vector<vnl_matrix<float> >& centroids)
{
  float dist_thres = m_Distances.at(0);
  int N = f_indices.size();

  std::vector< Cluster > C;
  vnl_matrix<float> zero_h; zero_h.set_size(T.at(0).rows(), T.at(0).cols()); zero_h.fill(0.0);
  Cluster no_fit;
  no_fit.h = zero_h;

  for (unsigned int i=0; i<centroids.size(); ++i)
  {
    Cluster c;
    c.h.set_size(T.at(0).rows(), T.at(0).cols()); c.h.fill(0.0);
    c.f_id = i;
    C.push_back(c);
  }

#pragma omp parallel for
  for (int i=0; i<N; ++i)
  {
    vnl_matrix<float> t = T.at(f_indices.at(i));

    int min_cluster_index = -1;
    float min_cluster_distance = 99999;
    bool flip = false;

    if (CalcOverlap(t)>=m_OverlapThreshold)
    {
      int c_idx = 0;
      for (vnl_matrix<float> centroid : centroids)
      {
        bool f = false;
        float d = 0;
        for (auto m : m_Metrics)
          d += m->CalculateDistance(t, centroid, f);
        d /= m_Metrics.size();

        if (d<min_cluster_distance)
        {
          min_cluster_distance = d;
          min_cluster_index = c_idx;
          flip = f;
        }
        ++c_idx;
      }
    }

    if (min_cluster_index>=0 && min_cluster_distance<dist_thres)
    {
#pragma omp critical
      {
        C[min_cluster_index].I.push_back(f_indices.at(i));
        if (!flip)
          C[min_cluster_index].h += t;
        else
          C[min_cluster_index].h += t.fliplr();
        C[min_cluster_index].n += 1;
      }
    }
    else
    {
#pragma omp critical
      {
        no_fit.I.push_back(f_indices.at(i));
        no_fit.n++;
      }
    }
  }
  C.push_back(no_fit);
  return C;
}

void TractClusteringFilter::GenerateData()
{
  m_OutTractograms.clear();
  m_OutCentroids.clear();
  m_OutClusters.clear();

  if (m_Metrics.empty())
  {
    mitkThrow() << "No metric selected!";
    return;
  }

  T = ResampleFibers(m_Tractogram);
  if (T.empty())
  {
    MITK_INFO << "No fibers in tractogram!";
    return;
  }

  std::vector< unsigned int > f_indices;
  for (unsigned int i=0; i<T.size(); ++i)
    f_indices.push_back(i);
  //  std::random_shuffle(f_indices.begin(), f_indices.end());

  Cluster no_match;
  std::vector< Cluster > clusters;
  if (m_InCentroids.IsNull())
  {
    MITK_INFO << "Clustering fibers";
    clusters = ClusterStep(f_indices, m_Distances);
    MITK_INFO << "Number of clusters: " << clusters.size();
    clusters = MergeDuplicateClusters2(clusters);
    std::sort(clusters.begin(),clusters.end());
  }
  else
  {
    std::vector<vnl_matrix<float> > centroids = ResampleFibers(m_InCentroids);
    if (centroids.empty())
    {
      MITK_INFO << "No fibers in centroid tractogram!";
      return;
    }
    MITK_INFO << "Clustering with input centroids";
    clusters = AddToKnownClusters(f_indices, centroids);
    no_match = clusters.back();
    clusters.pop_back();
    MITK_INFO << "Number of clusters: " << clusters.size();
    clusters = MergeDuplicateClusters2(clusters);
  }

  MITK_INFO << "Clustering finished";
  int max = clusters.size()-1;
  if (m_MaxClusters>0 && clusters.size()-1>m_MaxClusters)
    max = m_MaxClusters;
  m_DiscardedClusters = 0;
  for (int i=clusters.size()-1; i>=0; --i)
  {
    Cluster c = clusters.at(i);
    if ( c.n>=(int)m_MinClusterSize && !(m_MaxClusters>0 && clusters.size()-i>m_MaxClusters) )
    {
      m_OutClusters.push_back(c);

      vtkSmartPointer<vtkFloatArray> weights = vtkSmartPointer<vtkFloatArray>::New();
      vtkSmartPointer<vtkPolyData> pTmp = m_Tractogram->GeneratePolyDataByIds(c.I, weights);
      mitk::FiberBundle::Pointer fib = mitk::FiberBundle::New(pTmp);
      if (max>0)
        fib->SetFiberWeights((float)i/max);
      m_OutTractograms.push_back(fib);
      m_OutFiberIndices.push_back(c.I);

      // create centroid
      vtkSmartPointer<vtkPoints> vtkNewPoints = vtkSmartPointer<vtkPoints>::New();
      vtkSmartPointer<vtkCellArray> vtkNewCells = vtkSmartPointer<vtkCellArray>::New();
      vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
      vtkSmartPointer<vtkPolyLine> container = vtkSmartPointer<vtkPolyLine>::New();
      vnl_matrix<float> centroid_points = c.h / c.n;
      for (unsigned int j=0; j<centroid_points.cols(); j++)
      {
        double p[3];
        p[0] = centroid_points.get(0,j);
        p[1] = centroid_points.get(1,j);
        p[2] = centroid_points.get(2,j);

        vtkIdType id = vtkNewPoints->InsertNextPoint(p);
        container->GetPointIds()->InsertNextId(id);
      }
      vtkNewCells->InsertNextCell(container);
      polyData->SetPoints(vtkNewPoints);
      polyData->SetLines(vtkNewCells);
      mitk::FiberBundle::Pointer centroid = mitk::FiberBundle::New(polyData);
      centroid->SetFiberColors(255, 255, 255);
      m_OutCentroids.push_back(centroid);
    }
    else
    {
      m_DiscardedClusters++;
    }
  }
  MITK_INFO << "Final number of clusters: " << m_OutTractograms.size() << " (discarded " << m_DiscardedClusters << " clusters)";

  int w = 0;
  for (auto fib : m_OutTractograms)
  {
    if (m_OutTractograms.size()>1)
    {
      fib->SetFiberWeights((float)w/(m_OutTractograms.size()-1));
      m_OutCentroids.at(w)->SetFiberWeights((float)w/(m_OutTractograms.size()-1));
    }
    else
    {
      fib->SetFiberWeights(1);
      m_OutCentroids.at(w)->SetFiberWeights(1);
    }
    fib->ColorFibersByFiberWeights(false, false);
    ++w;
  }

  if (no_match.n>0)
  {
    vtkSmartPointer<vtkFloatArray> weights = vtkSmartPointer<vtkFloatArray>::New();
    vtkSmartPointer<vtkPolyData> pTmp = m_Tractogram->GeneratePolyDataByIds(no_match.I, weights);
    mitk::FiberBundle::Pointer fib = mitk::FiberBundle::New(pTmp);
    fib->SetFiberColors(0, 0, 0);
    m_OutFiberIndices.push_back(no_match.I);
    m_OutTractograms.push_back(fib);
  }
}

}


/*!
@file
Defines `boost::hana::Iterable::mcd`.

@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_ITERABLE_MCD_HPP
#define BOOST_HANA_ITERABLE_MCD_HPP

#include <boost/hana/iterable/iterable.hpp>

#include <boost/hana/comparable/comparable.hpp>
#include <boost/hana/core/is_a.hpp>
#include <boost/hana/foldable/mcd.hpp>
#include <boost/hana/group/group.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/logical/logical.hpp>

namespace boost { namespace hana {
    //! Minimal complete definition: `head`, `tail` and `is_empty`
    struct Iterable::mcd {
        template <typename Index, typename Iterable_>
        static constexpr auto at_impl(Index n, Iterable_ iterable) {
            return eval_if(equal(n, size_t<0>),
                [=](auto _) { return head(_(iterable)); },
                [=](auto _) { return at_impl(_(minus)(n, size_t<1>), tail(_(iterable))); }
            );
        }

        template <typename Iterable_>
        static constexpr auto last_impl(Iterable_ iterable) {
            return eval_if(is_empty(tail(iterable)),
                [=](auto _) { return head(_(iterable)); },
                [=](auto _) { return last_impl(tail(_(iterable))); }
            );
        }

        template <typename N, typename Iterable_>
        static constexpr auto drop_impl(N n, Iterable_ iterable) {
            return eval_if(or_(equal(n, size_t<0>), is_empty(iterable)),
                [=](auto) { return iterable; },
                [=](auto _) { return drop_impl(_(minus)(n, size_t<1>), tail(_(iterable))); }
            );
        }

        template <typename Pred, typename Iterable_>
        static constexpr auto drop_while_impl(Pred pred, Iterable_ iterable) {
            return eval_if(is_empty(iterable),
                [=](auto) { return iterable; },
                [=](auto _) {
                    return eval_if(pred(_(head)(iterable)),
                        [=](auto _) { return drop_while_impl(pred, _(tail)(iterable)); },
                        [=](auto) { return iterable; }
                    );
                }
            );
        }

        template <typename Pred, typename Iterable_>
        static constexpr auto drop_until_impl(Pred pred, Iterable_ iterable) {
            return drop_while([=](auto x) { return not_(pred(x)); }, iterable);
        }

        template <typename It, typename F>
        static constexpr auto for_each_impl(It it, F f) {
            return eval_if(is_empty(it),
                [](auto) { },
                [=](auto _) {
                    f(_(head)(it));
                    for_each_impl(_(tail)(it), f);
                }
            );
        }
    };

    namespace detail {
        struct FoldableFromIterable : Foldable::mcd {
            template <typename Xs, typename State, typename F>
            static constexpr auto foldl_impl(Xs xs, State s, F f) {
                return eval_if(is_empty(xs),
                    [=](auto) { return s; },
                    [=](auto _) {
                        return foldl_impl(tail(_(xs)), f(s, head(_(xs))), f);
                    }
                );
            }

            template <typename Xs, typename F>
            static constexpr auto foldl1_impl(Xs xs, F f)
            { return foldl(tail(xs), head(xs), f); }

            template <typename Xs, typename F>
            static constexpr auto foldr1_impl(Xs xs, F f) {
                return eval_if(is_empty(tail(xs)),
                    [=](auto) { return head(xs); },
                    [=](auto _) {
                        return f(head(xs), foldr1_impl(tail(_(xs)), f));
                    }
                );
            }

            template <typename Xs, typename State, typename F>
            static constexpr auto foldr_impl(Xs xs, State s, F f) {
                return eval_if(is_empty(xs),
                    [=](auto) { return s; },
                    [=](auto _) {
                        return f(head(_(xs)), foldr_impl(tail(_(xs)), s, f));
                    }
                );
            }
        };
    }

    //! Every `Iterable` instance gives rise to an instance of `Foldable`,
    //! which is valid for all finite such `Iterable`s.
    //!
    //! @details
    //! Let `xs` be an `Iterable` and let `xi` denote its `i`-th element. In
    //! other words, `xs` can be folded into a list containing `[x1, ..., xN]`,
    //! where `N` is the number of elements. Right-folding `xs` with a binary
    //! operation `*` (in infix notation for legibility) is equivalent to
    //! @code
    //!     x1 * (x2 * ( ... * (xN-1 * xN)))
    //! @endcode
    //!
    //! Similarly, left-folding `xs` is equivalent to
    //! @code
    //!     (((x1 * x2) * x3) * ...) * xN
    //! @endcode
    //!
    //! In both cases, notice the side of the parentheses. Left-folding
    //! applies `*` in a left-associative manner, whereas right-folding
    //! applies it in a right-associative manner. For associative operations,
    //! i.e. operations such that for all `a`, `b` and `c`,
    //! @code
    //!     (a * b) * c = a * (b * c)
    //! @endcode
    //! this makes no difference. Also note that lazy folds and folds with an
    //! initial state are implemented in an analogous way.
    //!
    //! ### Example 1
    //! @snippet example/list/foldable/foldl.cpp main
    //!
    //! ### Example 2
    //! @snippet example/integer_list/foldable/foldr.cpp main
    template <typename T>
    struct Foldable::instance<T, when<is_an<Iterable, T>()>>
        : detail::FoldableFromIterable
    { };
}} // end namespace boost::hana

#endif // !BOOST_HANA_ITERABLE_MCD_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_GE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_GE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing is_ge capabilities

    Returns @ref True or @ref False according x is greater or equal to y or not.

    Infix notation can be used with operator '>='.

    This is a convenient alias of @ref is_greater_equal
  **/
  const boost::dispatch::functor<tag::is_ge_> is_ge = {};
} }
#endif

#include <boost/simd/function/scalar/is_greater_equal.hpp>
#include <boost/simd/function/simd/is_ge.hpp>

#endif


#include <armadillo>
#include <iostream>
#include "gtest/gtest.h"

#include "svm_helpers.h"
#include "svm_client.h"
#include "utils/logger.h"

namespace {

int log_level = LogLevel::DEBUG_LEVEL;

// The fixture
class SVMClientTest: public ::testing::Test {

protected:

    SVMClientTest() {
        std::cout << "Creating SVMConfiguration..." << std::endl << std::flush;
        svm_config = SVMConfiguration();
        std::cout << "Setting logger..." << std::endl << std::flush;
        svm_config.log.verbosity = log_level;

        // Exemplary sparse matrix
        sparse_matrix_csc_01_row_indices =
            helper_sparse_matrix_csc_01_row_indices();
        sparse_matrix_csc_01_column_pointers =
            helper_sparse_matrix_csc_01_column_pointers();
        sparse_matrix_csc_01_values =
            helper_sparse_matrix_csc_01_values();
        sparse_matrix_csc_01_nrow =
            helper_sparse_matrix_csc_01_nrow();
        sparse_matrix_csc_01_ncol =
            helper_sparse_matrix_csc_01_ncol();
        sparse_matrix_csc_01_sp_mat =
            helper_sparse_matrix_csc_01_sp_mat();
        sparse_matrix_csc_01_learning_target =
            helper_sparse_matrix_csc_01_learning_target();

        std::cout << "Starting test..." << std::endl << std::flush;
    }

    virtual ~SVMClientTest() {}

    /// Called immediately after the constructor (righ before each test)
    virtual void SetUp() {}

    /// Called immediately after each test (right before the destructor)
    virtual void TearDown() {}

    /* Objects declared here can be used by all tests in the test case */

    SVMConfiguration svm_config;

	arma::uvec sparse_matrix_csc_01_row_indices;
    arma::uvec sparse_matrix_csc_01_column_pointers;
    arma::vec sparse_matrix_csc_01_values;
    size_t sparse_matrix_csc_01_nrow;
    size_t sparse_matrix_csc_01_ncol;
    arma::sp_mat sparse_matrix_csc_01_sp_mat;
    arma::vec sparse_matrix_csc_01_learning_target;
};

} // end namespace

/* Fixture tests */

TEST_F(SVMClientTest, DISABLED_sparse_data_test) {
    std::cout << "Testing learning..." << std::endl << std::flush;
    svm_config.setSparseData(
        sparse_matrix_csc_01_row_indices,
        sparse_matrix_csc_01_column_pointers,
        sparse_matrix_csc_01_values,
        sparse_matrix_csc_01_nrow,
        sparse_matrix_csc_01_ncol,
        true
    );
    //svm_config.sparse_data = sparse_matrix_csc_01_sp_mat;
    svm_config.target = sparse_matrix_csc_01_learning_target;
    svm_config.setLibrary("libsvm");
    SVMClient *svm_client = new SVMClient(&svm_config);
    // XXX: Why access violation here?
    svm_client->train();

    std::cout << "Testing prediction..." << std::endl << std::flush;
    svm_client->sparse_predict(
        sparse_matrix_csc_01_row_indices,
        sparse_matrix_csc_01_column_pointers,
        sparse_matrix_csc_01_values,
        sparse_matrix_csc_01_nrow,
        sparse_matrix_csc_01_ncol
    );
    SVMConfiguration client_config = svm_client->getConfiguration();

    for (int i = 0; i < sparse_matrix_csc_01_learning_target.n_rows; ++i) {
        ASSERT_DOUBLE_EQ(
            client_config.result[i], sparse_matrix_csc_01_learning_target[i]);
    }
}


#include "../src/lelantus_prover.h"
#include "../src/lelantus_verifier.h"

#include "../bitcoin/streams.h"

#include "lelantus_test_fixture.h"

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

static const int PROTOCOL_VERSION = 90030;

BOOST_FIXTURE_TEST_SUITE(lelantus_serialize_tests, lelantus::LelantusTestingSetup)

BOOST_AUTO_TEST_CASE(serialize)
{
    auto params = lelantus::Params::get_default();
    std::map<uint32_t, std::vector<lelantus::PublicCoin>> anonymity_sets;
    int N = 100;

    std::vector<std::pair<lelantus::PrivateCoin, uint32_t>> Cin;
    lelantus::PrivateCoin input_coin1(params, 5);
    Cin.emplace_back(std::make_pair(input_coin1, 0));
    std::vector <size_t> indexes;
    indexes.push_back(0);

    std::vector <lelantus::PublicCoin> anonymity_set;
    anonymity_set.reserve(N);
    anonymity_set.push_back(Cin[0].first.getPublicCoin());
    for(int i = 0; i < N; ++i){
          secp_primitives::GroupElement coin;
          coin.randomize();
           anonymity_set.emplace_back(lelantus::PublicCoin(coin));
     }

    anonymity_sets[0] = anonymity_set;

    secp_primitives::Scalar Vin(uint64_t(5));
    secp_primitives::Scalar Vout(uint64_t(6));
    std::vector <lelantus::PrivateCoin> Cout;
    Cout.push_back(lelantus::PrivateCoin(params, 2));
    Cout.push_back(lelantus::PrivateCoin(params, 1));
    secp_primitives::Scalar f(uint64_t(1));

    lelantus::LelantusProof initial_proof;
    lelantus::SchnorrProof qkSchnorrProof;

    lelantus::LelantusProver prover(params, LELANTUS_TX_VERSION_4_5);
    prover.proof(anonymity_sets, {}, Vin, Cin, indexes, {}, Vout, Cout, f,  initial_proof, qkSchnorrProof);

    CDataStream serialized(SER_NETWORK, PROTOCOL_VERSION);
    serialized << initial_proof;

    lelantus::LelantusProof resulted_proof;
    serialized >> resulted_proof;

    for(size_t i = 0; i <  initial_proof.sigma_proofs.size(); ++i){
        BOOST_CHECK(initial_proof.sigma_proofs[i].B_ == resulted_proof.sigma_proofs[i].B_);
        BOOST_CHECK(initial_proof.sigma_proofs[i].A_ == resulted_proof.sigma_proofs[i].A_);
        BOOST_CHECK(initial_proof.sigma_proofs[i].C_ == resulted_proof.sigma_proofs[i].C_);
        BOOST_CHECK(initial_proof.sigma_proofs[i].D_ == resulted_proof.sigma_proofs[i].D_);
        BOOST_CHECK(initial_proof.sigma_proofs[i].f_ == resulted_proof.sigma_proofs[i].f_);
        BOOST_CHECK(initial_proof.sigma_proofs[i].ZA_ == resulted_proof.sigma_proofs[i].ZA_);
        BOOST_CHECK(initial_proof.sigma_proofs[i].ZC_ == resulted_proof.sigma_proofs[i].ZC_);
        BOOST_CHECK(initial_proof.sigma_proofs[i].Gk_ == resulted_proof.sigma_proofs[i].Gk_);
        BOOST_CHECK(initial_proof.sigma_proofs[i].zV_ == resulted_proof.sigma_proofs[i].zV_);
        BOOST_CHECK(initial_proof.sigma_proofs[i].zR_ == resulted_proof.sigma_proofs[i].zR_);
    }

    BOOST_CHECK(initial_proof.bulletproofs.A == resulted_proof.bulletproofs.A);
    BOOST_CHECK(initial_proof.bulletproofs.S == resulted_proof.bulletproofs.S);
    BOOST_CHECK(initial_proof.bulletproofs.T1 == resulted_proof.bulletproofs.T1);
    BOOST_CHECK(initial_proof.bulletproofs.T2 == resulted_proof.bulletproofs.T2);
    BOOST_CHECK(initial_proof.bulletproofs.T_x1 == resulted_proof.bulletproofs.T_x1);
    BOOST_CHECK(initial_proof.bulletproofs.T_x2 == resulted_proof.bulletproofs.T_x2);
    BOOST_CHECK(initial_proof.bulletproofs.u == resulted_proof.bulletproofs.u);
    BOOST_CHECK(initial_proof.bulletproofs.innerProductProof.a_ == resulted_proof.bulletproofs.innerProductProof.a_);
    BOOST_CHECK(initial_proof.bulletproofs.innerProductProof.b_ == resulted_proof.bulletproofs.innerProductProof.b_);
    BOOST_CHECK(initial_proof.bulletproofs.innerProductProof.c_ == resulted_proof.bulletproofs.innerProductProof.c_);
    BOOST_CHECK(initial_proof.bulletproofs.innerProductProof.L_ == resulted_proof.bulletproofs.innerProductProof.L_);
    BOOST_CHECK(initial_proof.bulletproofs.innerProductProof.R_ == resulted_proof.bulletproofs.innerProductProof.R_);

        BOOST_CHECK(initial_proof.schnorrProof.u == resulted_proof.schnorrProof.u);
        BOOST_CHECK(initial_proof.schnorrProof.P1 == resulted_proof.schnorrProof.P1);
        BOOST_CHECK(initial_proof.schnorrProof.T1 == resulted_proof.schnorrProof.T1);
}

BOOST_AUTO_TEST_SUITE_END()


/*!
 * @file resample_test.cpp
 *
 * @author Andrzej Ciarkowski <mailto:andrzej.ciarkowski@gmail.com>
 */

#include <boost/test/auto_unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <dsp++/resample.h>
#include <dsp++/snd/reader.h>
#include <dsp++/snd/writer.h>
#include <dsp++/snd/format.h>
#include <dsp++/float.h>

BOOST_AUTO_TEST_SUITE(resample)

BOOST_AUTO_TEST_CASE(interpolator)
{
	using namespace dsp::snd;

	const size_t factor = 4;
	dsp::snd::file_format ff;
	reader r;
	r.open("data/coil_mono.wav", &ff);

	ff.set_sample_rate(ff.sample_rate() * factor);

	writer w;
	w.open("data/coil_mono_up4.wav", &ff);

	const size_t len = 1024;
	dsp::block_interpolator<float> interp(len, factor, 47, .2);

	while (true) {
		size_t read = r.read_frames(interp.x.begin(), len);
		std::fill(interp.x.begin() + read, interp.x.end(), 0.f);
		interp();
		w.write_frames(interp.y.begin(), read * factor);
		if (read != len)
			break;
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <smtbx/refinement/constraints/occupancy.h>

#include <boost/lambda/lambda.hpp>
#include <scitbx/array_family/tiny_algebra.h>

namespace smtbx { namespace refinement { namespace constraints {

  void
  dependent_occupancy
  ::linearise(uctbx::unit_cell const &unit_cell,
              sparse_matrix_type *jacobian_transpose)
  {
    scalar_parameter *o = reference();
    double k = multiplier/original_multiplier;
    if( as_one )
      value = k*o->value;
    else
      value = multiplier - k*o->value;

    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    if( as_one )
      jt.col(index()) = k*jt.col(o->index());
    else
      jt.col(index()) = (-k)*jt.col(o->index());
  }

}}}


//==================================================================================================
/*!
  @file

  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_LOGARITHM_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_LOGARITHM_HPP_INCLUDED

#include <boost/simd/arch/common/detail/simd/f_log.hpp>
#include <boost/simd/arch/common/detail/simd/d_log.hpp>

#endif


#ifndef GREATER_THAN_UTILS_HPP
#define GREATER_THAN_UTILS_HPP
#include "algebra/Vector.hpp"
#include <NTL/ZZX.h>
#include <vector>

/// @param range [1, range]
/// @param slot_nr
/// @return several Vector<long> each of Vector<long> contains slot_nr integers.
std::vector<MDL::Vector<long>> permutated_range(long range, long slot_nr);

/// make random noise
/// @param range [1, range]
/// @param slot_nr
/// @param return serveral polynomails that each slot contains a random noise.
std::vector<MDL::Vector<long>> random_noise(long domain, long slot_nr, long noise_domain);
#endif // GREATER_THAN_UTILS_HPP


/** @file ML/QEPopDtc.cpp
 *  @brief Class used to output the average population in a quantum emitter state
 *
 *  Class that collects the quantum emitter density matrix for all processes, averages them
 *  and outputs it to a text file.
 *
 *  @author Thomas A. Purcell (tpurcell90)
 *  @bug No known bugs.
 */

#ifndef ML_PARALLEL_POP_DTC
#define ML_PARALLEL_POP_DTC

#include <boost/serialization/complex.hpp>
#include <boost/serialization/vector.hpp>
#include <iomanip>
#include <MPI/mpiInterface.hpp>
/**
 * @brief Detector class for printing out field information to a text file
 * @details Copies the field magnitude to a text file
 */

class QEPopDtc
{
protected:
    std::shared_ptr<mpiInterface> gridComm_; //!< The communicator for the processes that are storing the grid
    int level_; //!< level to output
    int nlevel_; //!< number of levels in the QE object
    int npoints_; //!< number of points in QE object
    int t_step_;  //!< current time step
    int timeInt_; //!< time Interval for inputting the populations

    double tcur_; //!< current time
    double dt_; //!< time step of the simulation

    cplx curPop_; //!< current value of population

    std::vector<double> eWeights_; //!< weights for each level

    std::vector<cplx> allPop_; //!< sum of population of the level at all QE points

    std::string outFile_; //!< output file name

public:
    /**
     * @brief      Constructor for QE population detector
     *
     * @param[in]  gridComm  The MPIInterface for the grids
     * @param[in]  level     The level to be outputted
     * @param[in]  nlevel    The nlevel total number of levels in the system
     * @param[in]  npoints   The npoints total number of points of the QE
     * @param[in]  eWeights  The e weights the weight of the energy level
     * @param[in]  outFile   The out file
     * @param[in]  timeInt   The number of time steps before outputting population data
     * @param[in]  nt        number of time steps to be outputting
     * @param[in]  dt        the size of time step
     */
    QEPopDtc(std::shared_ptr<mpiInterface> gridComm, int level, int nlevel, int npoints, std::vector<double> eWeights, std::string outFile, int timeInt, int nt, double dt);

    /**
     * @brief      input the population of the QE at that level
     *
     * @param      denMat  The density matrix starting value
     */
    inline void inPop(cplx* denMat) { if(t_step_ % timeInt_ == 0) curPop_ +=  *(denMat + level_) ; return;}

    /**
     * @brief      accumlate the population into the storage vector and reset inPop to 0
     */
    void accumPop();

    /**
     * @brief      output accumulated population into a vector
     */
    void toFile();

    /**
     * @brief      accessor function to the level to be outputted
     *
     * @return     level_
     */
    inline int level() { return level_; }

    /**
     * @brief      Accessor to npoints_
     *
     * @return     npoints_
     */
    inline int npoints() { return npoints_; }

    /**
     * @brief      Accessor to output file name
     *
     * @return     outFile_
     */
    inline std::string fname() { return outFile_; }

    inline int timeInt() {return timeInt_;}

};

#endif


#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <ipc/collision_constraint.hpp>
// NOTE: Include this so the user can just include ipc.hpp
#include <ipc/friction/friction.hpp>
#include <ipc/broad_phase/broad_phase.hpp>

/// Incremental Potential Contact functions
namespace ipc {

/// @brief Construct a set of constraints used to compute the barrier potential.
///
/// All vertices in V will be considered for collisions, so V should be only the
/// surface vertices. The edges and face should be only for the surface
/// elements.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] dhat The activation distance of the barrier.
/// @param[out] constraint_set
///     The constructed set of constraints (any existing constraints will be
///     cleared).
/// @param[in] F2E Map from F edges to rows of E.
/// @param[in] dmin Minimum distance.
/// @param[in] method Broad-phase method to use.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in V) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
void construct_constraint_set(
    const Eigen::MatrixXd& V_rest,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    Constraints& constraint_set,
    const Eigen::MatrixXi& F2E = Eigen::MatrixXi(),
    const double dmin = 0,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Construct a set of constraints used to compute the barrier potential.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] codim_V
///     Vertex indices of codimensional points. Pass `Eigen::VectorXi()` to
///     ignore all codimensional vertices (vertices not connected to an edge).
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] dhat The activation distance of the barrier.
/// @param[out] constraint_set
///     The constructed set of constraints (any existing constraints will be
///     cleared).
/// @param[in] F2E Map from F edges to rows of E.
/// @param[in] dmin Minimum distance.
/// @param[in] method Broad-phase method to use.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in V) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
void construct_constraint_set(
    const Eigen::MatrixXd& V_rest,
    const Eigen::MatrixXd& V,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    Constraints& constraint_set,
    const Eigen::MatrixXi& F2E = Eigen::MatrixXi(),
    const double dmin = 0,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Construct a set of constraints used to compute the barrier potential.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @param[in]  candidates  Distance candidates from which the constraint set is
///                         built.
/// @param[in]  V  Vertex positions as rows of a matrix.
/// @param[in]  E  Edges as rows of indicies into V.
/// @param[in]  F  Triangular faces as rows of indicies into V.
/// @param[in]  dhat  The activation distance of the barrier.
/// @param[out] constraint_set
///     The constructed set of constraints (any existing constraints will be
///     cleared).
/// @param[in]  F2E  Map from F edges to rows of E.
/// @param[in]  dmin  Minimum distance.
void construct_constraint_set(
    const Candidates& candidates,
    const Eigen::MatrixXd& V_rest,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    Constraints& constraint_set,
    const Eigen::MatrixXi& F2E = Eigen::MatrixXi(),
    const double dmin = 0);

/// @brief Compute the barrier potential for a given constraint set.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] constraint_set The set of constraints.
/// @param[in] dhat The activation distance of the barrier.
/// @returns The sum of all barrier potentials (not scaled by the barrier
/// stiffness).
double compute_barrier_potential(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set,
    const double dhat);

/// @brief Compute the gradient of the barrier potential.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] constraint_set The set of constraints.
/// @param[in] dhat The activation distance of the barrier.
/// @returns The gradient of all barrier potentials (not scaled by the barrier
/// stiffness).
Eigen::VectorXd compute_barrier_potential_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set,
    const double dhat);

/// @brief Compute the hessian of the barrier potential.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] constraint_set The set of constraints.
/// @param[in] dhat The activation distance of the barrier.
/// @param[in] project_hessian_to_psd Make sure the hessian is positive
///            semi-definite.
/// @returns The hessian of all barrier potentials (not scaled by the barrier
/// stiffness).
Eigen::SparseMatrix<double> compute_barrier_potential_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set,
    const double dhat,
    const bool project_hessian_to_psd = true);

///////////////////////////////////////////////////////////////////////////////
// Collision detection

/// @brief Determine if the step is collision free.
///
/// All vertices in V0 and V1 will be considered for collisions, so V0 and
/// V1 should be only the surface vertices. The edges and face should be only
/// for the surface elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other primiti
/// @returns True if any collisions occur.
bool is_step_collision_free(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Determine if the step is collision free.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] codim_V Vertex indices of codimensional points. Pass
///     Eigen::VectorXi() to ignore all codimensional vertices (vertices not
///     connected to an edge).
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other primiti
/// @returns True if any collisions occur.
bool is_step_collision_free(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Determine if the step is collision free from a set of candidates.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] candidates Set of candidates to check for collisions.
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
///
/// @returns True if any collisions occur.
bool is_step_collision_free(
    const Candidates& candidates,
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7);

/// @brief Computes a maximal step size that is collision free.
///
/// All vertices in V0 and V1 will be considered for collisions, so V0 and
/// V1 should be only the surface vertices. The edges and face should be only
/// for the surface elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0
///     Vertex positions at start as rows of a matrix. Assumes V0 is
///     intersection free.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
/// @returns A step-size \f$\in [0, 1]\f$ that is collision free. A value of 1.0
/// if a full step and 0.0 is no step.
double compute_collision_free_stepsize(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Computes a maximal step size that is collision free.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0
///     Vertex positions at start as rows of a matrix. Assumes V0 is
///     intersection free.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] codim_V Vertex indices of codimensional points. Pass
///     Eigen::VectorXi() to ignore all codimensional vertices (vertices not
///     connected to an edge).
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
/// @returns A step-size \f$\in [0, 1]\f$ that is collision free. A value of 1.0
/// if a full step and 0.0 is no step.
double compute_collision_free_stepsize(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Computes a maximal step size that is collision free using a set of
/// collision candidates.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] candidates Set of candidates to check for collisions.
/// @param[in] V0
///     Vertex positions at start as rows of a matrix. Assumes V0 is
///     intersection free.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
////
/// @returns A step-size \f$\in [0, 1]\f$ that is collision free. A value of 1.0
/// if a full step and 0.0 is no step.
double compute_collision_free_stepsize(
    const Candidates& candidates,
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7);

///////////////////////////////////////////////////////////////////////////////
// Utilities

/// @brief Computes the minimum distance between any non-adjacent elements.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @returns The minimum distance between any non-adjacent elements.
double compute_minimum_distance(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set);

/// @brief Determine if the mesh has self intersections.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
bool has_intersections(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

} // namespace ipc