#include <chrono>
#include <random>
#include <vector>

// Boost.Graph
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

// This library
// Note that Boost's adjacency list invalidates vertex and edge descriptors and indices on removals, so an apples-to-apples comparison that includes removals is more difficult.
#include <graph/Stable_adjacency_list.hpp>

// Number of vertices and edges to insert.
int M = 100, N = 1000;

// The functions `with_this_library` and `with_boost_graph` perform the same set of basic operations but the first is implemented with this graph library, while the second uses Boost.Graph.  I'll let you decide which is simpler.  Of course, Boost.Graph is much more complete and includes an impressive range of graph algorithms albeit with a now dated interface.  If you compile with NDEBUG defined, basic timing information will also be reported with this library performing comparably or slightly better than Boost.Graph.

template <class RNG>
void with_boost_graph(RNG& rng) {
	// Generate a random graph
	using namespace boost;
	adjacency_list<> g;
	for (int i = 0; i < M; ++i)
		add_vertex(g);
	for (int i = 0; i < N; ++i) {
		auto u = random_vertex(g, rng), v = random_vertex(g, rng);
		add_edge(u, v, g);
	}

	// Build a map of degrees by iterating over edges
	auto vertex_index_map = get(vertex_index, g);
	std::vector<int> out_degrees(num_vertices(g));
	for (auto [it, end] = edges(g); it != end; ++it)
		++out_degrees[get(vertex_index_map, source(*it, g))];
	// Make sure the map agrees with the graph by iterating over vertices
	for (auto [it, end] = vertices(g); it != end; ++it)
		if (out_degrees[get(vertex_index_map, *it)] != out_degree(*it, g))
			throw std::runtime_error("degrees did not match");

	// Run Dijkstra's algorithm
	using vertex_descriptor = typename graph_traits<decltype(g)>::vertex_descriptor;
	using edge_descriptor = typename graph_traits<decltype(g)>::edge_descriptor;
	auto weights = make_static_property_map<edge_descriptor>(1.0);
	std::vector<vertex_descriptor> tree(num_vertices(g));
	std::vector<double> distances(num_vertices(g));
	dijkstra_shortest_paths(g, random_vertex(g, rng), weight_map(weights).
		predecessor_map(make_iterator_property_map(tree.begin(), vertex_index_map)).
		distance_map(make_iterator_property_map(distances.begin(), vertex_index_map)));
}

template <class RNG>
void with_this_library(RNG& rng) {
	// Generate a random graph
	// Notable difference: uses methods rather than free functions
	graph::Stable_out_adjacency_list g;
	for (int i = 0; i < M; ++i)
		g.insert_vert();
	for (int i = 0; i < N; ++i) {
		auto u = g.random_vert(rng), v = g.random_vert(rng);
		g.insert_edge(u, v);
	}

	// Build a map of degrees by iterating over edges
	// Notable differences:
	// - graphs know how to make performant maps from vertices and edges
	// - range-based for loops work as expected
	// - maps supply operator[] rather than requring `get` and `put` functions
	auto out_degrees = g.vert_map(0);
	for (auto e : g.edges())
		++out_degrees[g.tail(e)];
	// Make sure the map agrees with the graph by iterating over vertices
	for (auto v : g.verts())
		if (out_degrees[v] != g.out_degree(v))
			throw std::runtime_error("degrees did not match");

	// Run Dijkstra's algorithm
	// Notable differences:
	// - This only required two simple lines of code
	// - The result includes a tree with semantics beyond a simple predecessor map
	auto weights = [](auto e) { return 1.0; };
	auto [tree, distances] = g.shortest_paths_from(g.random_vert(rng), weights);
}

int main(int argc, char *argv[]) {
	{
		std::mt19937_64 random;
		with_this_library(random);
		with_boost_graph(random);
	}
#ifdef NDEBUG
	using clock = std::chrono::high_resolution_clock;
	{
		std::mt19937_64 random;
		auto start = clock::now();
		for (int i = 0; i < 10000; ++i)
			with_this_library(random);
		std::cout << "elapsed time (This Library): " << (clock::now() - start).count() << std::endl;
	}
	{
		std::mt19937_64 random;
		auto start = clock::now();
		for (int i = 0; i < 10000; ++i)
			with_boost_graph(random);
		std::cout << "elapsed time (Boost.Graph): " << (clock::now() - start).count() << std::endl;
	}
#endif
}


// Copyright (c) 2005 - 2015 Marc de Kamps
// 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 copyright holder 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 OWNER 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.
//
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <TwoDLib.hpp>
#include <vector>
#include <iostream>
#include <fstream>

using namespace std;
using namespace TwoDLib;

BOOST_AUTO_TEST_CASE(TriangleGeneratorTest)
{

	Point p1(0.,0.);
	Point p2(1.,0.);
	Point p3(0.,1.);
	Triangle t(p1,p2,p3);

	Uniform uni(0);
	TriangleGenerator gen(t,uni);
	vector<Point> vec(10);

	gen.Generate(&vec);
}

BOOST_AUTO_TEST_CASE(QuadGeneratorTest)
{
	Point p1(0.,0.);
	Point p2(1.,0.);
	Point p3(1.,1.);
	Point p4(0.,1.);

	Quadrilateral q(p1,p2,p3,p4);

	Uniform uni(0);
	QuadGenerator gen(q,uni);
	vector<Point> vec(1000);

	gen.Generate(&vec);

	ofstream ofst("square.dat");
	for (auto it = vec.begin(); it != vec.end(); it++)
		ofst << (*it)[0] << " " << (*it)[1] << '\n';
	ofst.close();

	Point p5(0.,0.);
	Point p6(1,-0.5);
	Point p7(0.,1);
	Point p8(-0.5,-0.5);

	Quadrilateral kite(p5,p6,p7,p8);
	QuadGenerator genkite(kite,uni);
	genkite.Generate(&vec);

	ofstream ofst2("kite.dat");
	for (auto it = vec.begin(); it != vec.end(); it++)
		ofst2 << (*it)[0] << " " << (*it)[1] << '\n';
	ofst2.close();
}

BOOST_AUTO_TEST_CASE(PolyGeneratorTest)
{
	Point p1(-1.,-1.);
	Point p2( 1.,-1.);
	Point p3( 0.,0.);
	Point p4(-1.,1.);
	Point p5( 0.,1.5);
	Point p6( 1.,1.);

	vector<Point> vec_point { p1, p2, p3, p4, p5, p6 };
	Quadrilateral q(vec_point);

	Uniform uni(0);
	PolyGenerator gen(q,uni);
	vector<Point> vec(1000);
	gen.Generate(&vec);

	ofstream ofst("complex.dat");
	for (auto it = vec.begin(); it != vec.end(); it++)
		ofst << (*it)[0] << " " << (*it)[1] << '\n';
	ofst.close();
}


/*
Copyright 2018 Dennis Rohde

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#pragma once

#include <iostream>
#include <limits>
#include <sstream>
#include <cmath>

#include <boost/python/numpy.hpp>

namespace np = boost::python::numpy;
namespace p = boost::python;

template <typename T = double>
class Tuple : public std::vector<T> {
    typedef unsigned long index_type;

public:
    typedef T value_type;
    typedef std::vector<value_type> tuple_type;

    Tuple() : tuple_type{} {}
    Tuple(const index_type d) : tuple_type(d) {}
    Tuple(const T &in, const index_type d) : tuple_type(d, in) {}
    Tuple(const tuple_type &in, const index_type d) : tuple_type{} {
        if (in.size() < d) {
            throw std::invalid_argument("Could not build d-tuple from vector of dimension "+std::to_string(in.size()));
        }
        this->assign(in.begin(), in.begin() + d);
    }

    Tuple(const np::ndarray &in) : tuple_type{} {
        auto dimensions = in.get_nd();
        if (dimensions != 1 or in.get_dtype() != np::dtype::get_builtin<T>()) {
            int status;
            const std::type_info &ti = typeid(T);
            std::cerr << "Need 1-dimensional numpy array of type " << abi::__cxa_demangle(ti.name(), 0, 0, &status) << " !" << std::endl;
            return;
        }
        auto size = in.get_shape();
        auto strides = in.get_strides();
        auto data = in.get_data();
        for (index_type i = 0; i < size[0]; ++i) {
            (*this)[i] = *reinterpret_cast<const T*>(data + i);
        }
    }

    inline auto get(const index_type i) const {
        if (i >= this->size()) return std::numeric_limits<T>::signaling_NaN();
        return this->operator[](i);
    }

    inline auto dimension() const {
        return this->size();
    }

    inline auto operator-(const Tuple<T> &o) const {
        Tuple<T> result{*this, this->size()};
        if (this->size() != o.size()) {
            result = Tuple<T>(std::numeric_limits<T>::signaling_NaN(), this->size());
            return result;
        }
        for (index_type i = 0; i < this->size(); ++i) {
            result[i] -= o.get(i);
        }
        return result;
    }

    inline auto operator*(const Tuple<T> &o) const {
        if (this->size() != o.size()) return std::numeric_limits<T>::signaling_NaN();
        T sum = 0;
        for (index_type i = 0; i < this->size(); ++i) {
            sum += get(i) * o.get(i);
        }
        return sum;
    }

    inline auto operator==(const Tuple<T> &o) const {
        if (this->size() != o.size()) {
            std::cerr << "Can not compare tuples of different dimension!" << std::endl;
            return false;
        }
        for (index_type i = 0; i < this->size(); ++i) {
            if (std::fabs(get(i) - o.get(i)) < std::numeric_limits<T>::epsilon()) continue;
            else return false;
        }
        return true;
    }

    inline bool operator<(const Tuple<T> &o) const {
        if (this->size() != o.size()) {
            std::cerr << "Can not compare tuples of different dimension!" << std::endl;
            return false;
        }
        for (index_type i = 0; i < this->size(); ++i) {
            if (not (std::fabs(get(i) - o.get(i)) < std::numeric_limits<T>::epsilon())) {
                if (o.get(i) - get(i) >= std::numeric_limits<T>::epsilon()) {
                    return true;
                } else {
                    return false;
                }
            }
        }
        return false;
    }

    inline auto as_str() const {
        std::stringstream ss;
        ss << *this;
        return ss.str();
    }

    inline auto repr() const {
        std::stringstream ss;
        ss << "Tuple of Dimension " << dimension() << ":" << std::endl;
        ss << as_str();
        return ss.str();
    }

    inline auto as_ndarray() const {
        np::dtype dt = np::dtype::get_builtin<T>();
        p::list l;
        np::ndarray result = np::array(l, dt);
        for (const auto &elem: *this) {
            l.append(elem);
        }
        result = np::array(l, dt);
        return result;
    }

    inline auto pbegin() const {
        return this->begin();
    }

    inline auto pend() const {
        return this->end();
    }
};

template <typename T>
std::ostream& operator<<(std::ostream &out, const Tuple<T> &t) {
    const auto d = t.size();
    out << "(";
    for (auto i = d-d; i < d-1; ++i) {
        out << t.get(i);
        out << ", ";
    }
    out << t.get(d-1) << ")";
    return out;
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       David Salinas
 *
 *    Copyright (C) 2014 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <fstream>
#include <sstream>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "skeleton_blocker_geometric_complex"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <gudhi/Skeleton_blocker.h>

struct Geometry_trait {
  typedef std::vector<double> Point;
};

typedef Geometry_trait::Point Point;
typedef Gudhi::skeleton_blocker::Skeleton_blocker_simple_geometric_traits<Geometry_trait> Complex_geometric_traits;
typedef Gudhi::skeleton_blocker::Skeleton_blocker_geometric_complex< Complex_geometric_traits > Complex;
typedef Complex::Vertex_handle Vertex_handle;
typedef Complex_geometric_traits::Root_vertex_handle Root_vertex_handle;

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_off_reader_writer) {
  Complex complex;
  Gudhi::skeleton_blocker::Skeleton_blocker_off_reader<Complex> off_reader("test2.off", complex);
  BOOST_CHECK(off_reader.is_valid());

  std::clog << "complex has " <<
      complex.num_vertices() << " vertices, " <<
      complex.num_blockers() << " blockers, " <<
      complex.num_edges() << " edges and " <<
      complex.num_triangles() << " triangles.";

  BOOST_CHECK(complex.num_vertices() == 7);
  BOOST_CHECK(complex.num_edges() == 12);
  BOOST_CHECK(complex.num_triangles() == 6);

  Gudhi::skeleton_blocker::Skeleton_blocker_off_writer<Complex> off_writer("tmp.off", complex);
  Complex same;
  Gudhi::skeleton_blocker::Skeleton_blocker_off_reader<Complex> off_reader2("tmp.off", same);

  std::clog << "\ncomplex:" << complex.to_string() << std::endl;
  std::clog << "\nsame:" << same.to_string() << std::endl;

  BOOST_CHECK(complex == same);
}

BOOST_AUTO_TEST_CASE(test_skeleton_blocker_abstract_link) {
  Complex complex;
  Gudhi::skeleton_blocker::Skeleton_blocker_off_reader<Complex> off_reader("test2.off", complex);
  BOOST_CHECK(off_reader.is_valid());

  std::clog << "complex has " <<
      complex.num_vertices() << " vertices, " <<
      complex.num_blockers() << " blockers, " <<
      complex.num_edges() << " edges and " <<
      complex.num_triangles() << " triangles.";

  BOOST_CHECK(complex.num_vertices() == 7);
  BOOST_CHECK(complex.num_edges() == 12);
  BOOST_CHECK(complex.num_triangles() == 6);

  auto link_0 = complex.abstract_link(Vertex_handle(0));

  std::clog << "\n link(0):" << link_0.to_string() << std::endl;

  BOOST_CHECK(link_0.num_vertices() == 2);
  BOOST_CHECK(link_0.num_edges() == 1);
  BOOST_CHECK(link_0.num_blockers() == 0);

  // Check the 2 link vertices
  auto vertex_handle = link_0.vertex_range().begin();
  BOOST_CHECK(link_0[*vertex_handle].get_id() == Root_vertex_handle(1));
  vertex_handle++;
  BOOST_CHECK(link_0[*(vertex_handle)].get_id() == Root_vertex_handle(4));

  // Check the lonely link edge
  auto edge_handle = link_0.edge_range().begin();
  BOOST_CHECK(link_0[*edge_handle].first() == Root_vertex_handle(1));
  BOOST_CHECK(link_0[*(edge_handle)].second() == Root_vertex_handle(4));

  auto link_geometric_0 = complex.link(Vertex_handle(0));
  std::clog << "\n link_geometric(0):" << link_geometric_0.to_string() << std::endl;

  BOOST_CHECK(link_0 == link_geometric_0);

  auto print_point = [&](Vertex_handle v) {
    for (auto x : link_geometric_0.point(v)) std::clog << x << " ";
    std::clog << std::endl;
  };

  std::for_each(link_geometric_0.vertex_range().begin(), link_geometric_0.vertex_range().end(), print_point);

  // Check the 2 link vertices
  vertex_handle = link_geometric_0.vertex_range().begin();
  std::vector<double> point_1 = {0,2,0};
  std::vector<double> point_4 = {-1,1,0};
  BOOST_CHECK(link_geometric_0.point(*vertex_handle) == point_1);
  vertex_handle++;
  BOOST_CHECK(link_geometric_0.point(*vertex_handle) == point_4);

}


/**********************************************************************
*  Copyright (c) 2008-2014, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include <utilities/data/Vector.hpp>

#include <algorithm>

// this should all be moved to a utilities/core/Random.h
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

using namespace std;
using namespace boost;

namespace openstudio{

  Vector createVector(const std::vector<double>& values) {
    unsigned n = values.size();
    Vector result(n);
    for (unsigned i = 0; i < n; ++i) {
      result[i] = values[i];
    }
    return result;
  }

  Vector createVector(const std::vector<long>& values) {
    unsigned n = values.size();
    Vector result(n);
    for (unsigned i = 0; i < n; ++i) {
      result[i] = (double)(values[i]);
    }
    return result;
  }

  std::vector<double> toStandardVector(const Vector& values) {
    unsigned n = values.size();
    std::vector<double> result(n);
    for (unsigned i = 0; i < n; ++i) {
      result[i] = values[i];
    }
    return result;
  }

  bool operator==(const Vector& lhs, const Vector& rhs)
  {
    bool result = false;
    if (lhs.size() == rhs.size()){
      result = (norm_1(lhs-rhs) == 0);
    }
    return result;
  }

  bool operator!=(const Vector& lhs, const Vector& rhs)
  {
    return !(lhs==rhs);
  }

  /// linear interpolation of the function y = f(x) at point xi
  /// assumes that x is strictly increasing
  InterpInfo interpInfo(const Vector& x, double xi)
  {
    unsigned N = x.size();

    InterpInfo result;

    if (x(0) == xi){
      result.ia = 0; result.ib = 0; result.wa = 1.0; result.wb = 0.0; result.extrapolated = false;
    }else if (xi < x(0)){
      result.ia = 0; result.ib = 0; result.wa = 1.0; result.wb = 0.0; result.extrapolated = true;
    }else if (x(N-1) == xi){
      result.ia = N-1; result.ib = N-1; result.wa = 0.0; result.wb = 1.0; result.extrapolated = false;
    }else if (xi > x(N-1)){
      result.ia = N-1; result.ib = N-1; result.wa = 0.0; result.wb = 1.0; result.extrapolated = true;
    }else{

      // ((xi > x(0)) && (xi < x(N-1)))
      result.extrapolated = false;
      Vector::const_iterator begin = x.begin();
      Vector::const_iterator it = std::lower_bound(begin, x.end(), xi);

      result.ia = (unsigned)(it-begin-1);
      result.ib = (unsigned)(it-begin);
      result.wa = (x(result.ib)-xi)/(x(result.ib)-x(result.ia));
      result.wb = (xi-x(result.ia))/(x(result.ib)-x(result.ia));
    }

    return result;
  }

  /// linear interpolation of the function y = f(x) at point xi
  /// assumes that x is strictly increasing
  double interp(const Vector& x, const Vector& y, double xi, InterpMethod interpMethod, ExtrapMethod extrapMethod){

    unsigned N = x.size();

    double result = 0.0;

    if (y.size() != N){
      return result;
    }

    InterpInfo info = interpInfo(x, xi);

    if (info.extrapolated){
      switch(extrapMethod){
        case NoneExtrap:
          // set to zero
          result = 0.0;
          break;
        case NearestExtrap:
          // pick closest point
          result = (info.wa > info.wb ? y(info.ia) : y(info.ib));
          break;
      }
    }else{
      switch(interpMethod){
        case LinearInterp:
          // linear interpolation
          result = info.wa*y(info.ia) + info.wb*y(info.ib);
          break;
        case NearestInterp:
          // pick closest point
          result = (info.wa > info.wb ? y(info.ia) : y(info.ib));
          break;
        case HoldLastInterp:
          // set to previous value
          result = y(info.ia);
          break;
        case HoldNextInterp:
          // set to next value
          result = y(info.ib);
          break;
      }
    }

    return result;
  }

  /// linear interpolation of the function y = f(x) at points xi
  /// assumes that x is strictly increasing
  Vector interp(const Vector& x, const Vector& y, const Vector& xi, InterpMethod interpMethod, ExtrapMethod extrapMethod){

    unsigned N = x.size();

    Vector result(N);

    if (y.size() != N){
      return result;
    }

    for (unsigned i = 0; i < N; ++i){
      result(i) = interp(x, y, xi(i), interpMethod, extrapMethod);
    }

    return result;
  }

  /// generates a Vector of N points randomly drawn between and including a and b.
  Vector randVector(double a, double b, unsigned N)
  {
    // seed random number generator
    static minstd_rand generator(42u);

    // define distribution
    uniform_real<> dist(a,b);

    // create a generator
    variate_generator<minstd_rand&, boost::uniform_real<> > uniformGenerator(generator, dist);

    Vector result(N);
    for (unsigned n = 0; n < N; ++n){
      result(n) = uniformGenerator(); // draw from generator
    }

    return result;
  }

  /// generates a Vector of N points linearly spaced between and including a and b.
  Vector linspace(double a, double b, unsigned N)
  {
    double delta = (b-a)/(N-1);

    Vector result(N);
    for (unsigned n = 0; n < N; ++n){
      result(n) = a + n*delta;
    }

    return result;
  }

  /// generates a Vector linearly spaced points starting at a and ending before or at b with interval delta.
  Vector deltaSpace(double a, double b, double delta)
  {
    unsigned N = (unsigned) floor((b-a)/delta) + 1;

    Vector result(N);
    for (unsigned n = 0; n < N; ++n){
      result(n) = a + n*delta;
    }

    return result;
  }

  /// generates a Vector of N points logorithmically spaced between and including base^a and base^b.
  Vector logspace(double a, double b, unsigned N, double base)
  {
    Vector powers = linspace(a,b,N);

    Vector result(N);
    for (unsigned n = 0; n < N; ++n){
      result(n) = std::pow(base, powers(n));
    }

    return result;
  }

  /// take the natural logarithm of elements of a Vector
  Vector log(const Vector& x)
  {
    unsigned N = x.size();
    Vector result(N);
    for (unsigned i = 0; i < N; ++i){
      result(i) = std::log(x(i));
    }
    return result;
  }

  /// take the logarithm of elements of a Vector with certain base
  Vector log(const Vector& x, double base)
  {
    double logBase = std::log(base);
    unsigned N = x.size();
    Vector result(N);
    for (unsigned i = 0; i < N; ++i){
      result(i) = std::log(x(i)) / logBase;
    }
    return result;
  }

  /// compute the cumulative sum of a Vector
  Vector cumsum(const Vector& x, double runningSum)
  {
    unsigned N = x.size();
    Vector result(N);
    for (unsigned i = 0; i < N; ++i){
      runningSum += x(i);
      result(i) = runningSum;
    }
    return result;
  }

  /// dot product
  double dot(const Vector& lhs, const Vector& rhs)
  {
    return boost::numeric::ublas::inner_prod(lhs, rhs);
  }

  /// sum
  double sum(const Vector& vector)
  {
    return boost::numeric::ublas::sum(vector);
  }

  /// maximum
  double maximum(const Vector& vector)
  {
    double max = 0;
    if (vector.size() > 0){
      max = *std::max_element(vector.begin(), vector.end());
    }
    return max;
  }

  /// minimum
  double minimum(const Vector& vector)
  {
    double min = 0;
    if (vector.size() > 0){
      min = *std::min_element(vector.begin(), vector.end());
    }
    return min;
  }

  /// mean
  double mean(const Vector& vector)
  {
    double avg = 0;
    unsigned N = vector.size();
    if (N > 0){
      avg = sum(vector) / N;
    }
    return avg;
  }

  /// variance
  double variance(const Vector& vector)
  {
    double result = 0;
    unsigned N = vector.size();
    if (N > 0)
    {
      double sumSquares = pow(boost::numeric::ublas::norm_2(vector), 2.0);
      result = sumSquares/N - pow(mean(vector), 2);
    }
    return result;
  }

  /// std dev
  double stdDev(const Vector& vector)
  {
    return sqrt(variance(vector));
  }

  boost::function<double (const Vector&)> sumVectorFunctor() {
    return boost::function<double (const Vector&)> (&sum);
  }

  boost::function<double (const Vector&)> maximumVectorFunctor() {
    return boost::function<double (const Vector&)> (&maximum);
  }

  boost::function<double (const Vector&)> minimumVectorFunctor() {
    return boost::function<double (const Vector&)> (&minimum);
  }

  boost::function<double (const Vector&)> meanVectorFunctor() {
    return boost::function<double (const Vector&)> (&mean);
  }

  boost::function<double (const Vector&)> varianceVectorFunctor() {
    return boost::function<double (const Vector&)> (&variance);
  }

  boost::function<double (const Vector&)> stdDevVectorFunctor() {
    return boost::function<double (const Vector&)> (&stdDev);
  }

  double evaluateDoubleFromVectorFunctor(const boost::function<double (const Vector&)>& functor,
                                         const Vector& vector)
  {
    return functor(vector);
  }
} // openstudio


//=======================================================================
// Copyright 2001 University of Notre Dame.
// Author: Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

/*
   Sample output:

   filtered edge set: (A,B) (C,D) (D,B)
   filtered out-edges:
   A --> B
   B -->
   C --> D
   D --> B
   E -->
 */

#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_utility.hpp>

template <typename EdgeWeightMap>
struct positive_edge_weight {
  positive_edge_weight() { }
  positive_edge_weight(EdgeWeightMap weight) : m_weight(weight) { }
  template <typename Edge>
  bool operator()(const Edge& e) const {
    return 0 < boost::get(m_weight, e);
  }
  EdgeWeightMap m_weight;
};

int main()
{
  using namespace boost;

  typedef adjacency_list<vecS, vecS, directedS,
    no_property, property<edge_weight_t, int> > Graph;
  typedef property_map<Graph, edge_weight_t>::type EdgeWeightMap;

  enum { A, B, C, D, E, N };
  const char* name = "ABCDE";
  Graph g(N);
  add_edge(A, B, 2, g);
  add_edge(A, C, 0, g);
  add_edge(C, D, 1, g);
  add_edge(C, E, 0, g);
  add_edge(D, B, 3, g);
  add_edge(E, C, 0, g);

  positive_edge_weight<EdgeWeightMap> filter(get(edge_weight, g));
  filtered_graph<Graph, positive_edge_weight<EdgeWeightMap> >
    fg(g, filter);

  std::cout << "filtered edge set: ";
  print_edges(fg, name);

  std::cout << "filtered out-edges:" << std::endl;
  print_graph(fg, name);

  return 0;
}


// Copyright (C) 2007 Trustees of Indiana University

// Authors: Douglas Gregor
//          Andrew Lumsdaine

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

// A test of the communicator that passes data around a ring and
// verifies that the same data makes it all the way. Should test all
// of the various kinds of data that can be sent (primitive types, POD
// types, serializable objects, etc.)
#include <boost/mpi/graph_communicator.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/environment.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/test/minimal.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/isomorphism.hpp>
#include <algorithm> // for random_shuffle
#include <boost/serialization/vector.hpp>
#include <boost/mpi/collectives/broadcast.hpp>

using boost::mpi::communicator;
using boost::mpi::graph_communicator;
using namespace boost;

int test_main(int argc, char* argv[])
{
  boost::function_requires< IncidenceGraphConcept<graph_communicator> >();
  boost::function_requires< AdjacencyGraphConcept<graph_communicator> >();
  boost::function_requires< VertexListGraphConcept<graph_communicator> >();
  boost::function_requires< EdgeListGraphConcept<graph_communicator> >();

  double prob = 0.1;

  boost::mpi::environment env(argc, argv);

  communicator world;

  // Random number generator
  minstd_rand gen;

  // Build a random graph with as many vertices as there are processes
  typedef adjacency_list<listS, vecS, bidirectionalS> Graph;
  sorted_erdos_renyi_iterator<minstd_rand, Graph>
    first(gen, world.size(), prob), last;
  Graph graph(first, last, world.size());

  // Display the original graph
  if (world.rank() == 0) {
    std::cout << "Original, random graph:\n";
    BGL_FORALL_VERTICES(v, graph, Graph) {
      BGL_FORALL_OUTEDGES(v, e, graph, Graph) {
        std::cout << source(e, graph) << " -> " << target(e, graph)
                  << std::endl;
      }
    }
  }

  // Create an arbitrary mapping from vertices to integers
  typedef property_map<Graph, vertex_index_t>::type GraphVertexIndexMap;
  std::vector<int> graph_alt_index_vec(num_vertices(graph));
  iterator_property_map<int*, GraphVertexIndexMap>
    graph_alt_index(&graph_alt_index_vec[0], get(vertex_index, graph));

  // Rank 0 will populate the alternative index vector
  if (world.rank() == 0) {
    int index = 0;
    BGL_FORALL_VERTICES(v, graph, Graph)
      put(graph_alt_index, v, index++);

    std::random_shuffle(graph_alt_index_vec.begin(), graph_alt_index_vec.end());
  }
  broadcast(world, graph_alt_index_vec, 0);

  // Display the original graph with the remapping
  if (world.rank() == 0) {
    std::cout << "Original, random graph with remapped vertex numbers:\n";
    BGL_FORALL_VERTICES(v, graph, Graph) {
      BGL_FORALL_OUTEDGES(v, e, graph, Graph) {
        std::cout << get(graph_alt_index, source(e, graph)) << " -> "
                  << get(graph_alt_index, target(e, graph)) << std::endl;
      }
    }
  }

  // Create a communicator with a topology equivalent to the graph
  graph_communicator graph_comm(world, graph, graph_alt_index, false);

  // The communicator's topology should have the same number of
  // vertices and edges and the original graph
  BOOST_CHECK((int)num_vertices(graph) == num_vertices(graph_comm));
  BOOST_CHECK((int)num_edges(graph) == num_edges(graph_comm));

  // Display the communicator graph
  if (graph_comm.rank() == 0) {
    std::cout << "Communicator graph:\n";
    BGL_FORALL_VERTICES(v, graph_comm, graph_communicator) {
      BGL_FORALL_OUTEDGES(v, e, graph_comm, graph_communicator) {
        std::cout << source(e, graph_comm) << " -> " << target(e, graph_comm)
                  << std::endl;
      }
    }

    std::cout << "Communicator graph via edges():\n";
    BGL_FORALL_EDGES(e, graph_comm, graph_communicator)
      std::cout << source(e, graph_comm) << " -> " << target(e, graph_comm)
                << std::endl;
  }
  (graph_comm.barrier)();

  // Verify the isomorphism
  if (graph_comm.rank() == 0)
    std::cout << "Verifying isomorphism..." << std::endl;
  BOOST_CHECK(verify_isomorphism(graph, graph_comm, graph_alt_index));

  return 0;
}


/**
 * @file    timeSchurFactors.cpp
 * @brief   Time various Schur-complement Jacobian factors
 * @author  Frank Dellaert
 * @date    Oct 27, 2013
 */

#include "DummyFactor.h"
#include <gtsam/base/timing.h>

#include <gtsam/slam/JacobianFactorQ.h>
#include "gtsam/slam/JacobianFactorQR.h"
#include <gtsam/slam/RegularImplicitSchurFactor.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholePose.h>

#include <boost/assign/list_of.hpp>
#include <boost/assign/std/vector.hpp>
#include <fstream>

using namespace std;
using namespace boost::assign;
using namespace gtsam;

#define SLOW
#define RAW
#define HESSIAN
#define NUM_ITERATIONS 1000

// Create CSV file for results
ofstream os("timeSchurFactors.csv");

/*************************************************************************************/
template<typename CAMERA>
void timeAll(size_t m, size_t N) {

  cout << m << endl;

  // create F
  static const int D = CAMERA::dimension;
  typedef Eigen::Matrix<double, 2, D> Matrix2D;
  FastVector<Key> keys;
  vector <Matrix2D> Fblocks;
  for (size_t i = 0; i < m; i++) {
    keys.push_back(i);
    Fblocks.push_back((i + 1) * Matrix::Ones(2, D));
  }

  // create E
  Matrix E(2 * m, 3);
  for (size_t i = 0; i < m; i++)
    E.block < 2, 3 > (2 * i, 0) = Matrix::Ones(2, 3);

  // Calculate point covariance
  Matrix P = (E.transpose() * E).inverse();

  // RHS and sigmas
  const Vector b = Vector::Constant(2*m,1);
  const SharedDiagonal model;

  // parameters for multiplyHessianAdd
  double alpha = 0.5;
  VectorValues xvalues, yvalues;
  for (size_t i = 0; i < m; i++)
    xvalues.insert(i, Vector::Constant(D,2));

  // Implicit
  RegularImplicitSchurFactor<CAMERA> implicitFactor(keys, Fblocks, E, P, b);
  // JacobianFactor with same error
  JacobianFactorQ<D, 2> jf(keys, Fblocks, E, P, b, model);
  // JacobianFactorQR with same error
  JacobianFactorQR<D, 2> jqr(keys, Fblocks, E, P, b, model);
  // Hessian
  HessianFactor hessianFactor(jqr);

#define TIME(label,factor,xx,yy) {\
     for (size_t t = 0; t < N; t++) \
     factor.multiplyHessianAdd(alpha, xx, yy);\
     gttic_(label);\
     for (size_t t = 0; t < N; t++) {\
       factor.multiplyHessianAdd(alpha, xx, yy);\
     }\
     gttoc_(label);\
     tictoc_getNode(timer, label)\
     os << timer->secs()/NUM_ITERATIONS << ", ";\
   }

#ifdef SLOW
  TIME(Implicit, implicitFactor, xvalues, yvalues)
  TIME(Jacobian, jf, xvalues, yvalues)
  TIME(JacobianQR, jqr, xvalues, yvalues)
#endif

#ifdef HESSIAN
  TIME(Hessian, hessianFactor, xvalues, yvalues)
#endif

#ifdef OVERHEAD
  DummyFactor<D> dummy(Fblocks, E, P, b);
  TIME(Overhead,dummy,xvalues,yvalues)
#endif

#ifdef RAW
  { // Raw memory Version
    FastVector < Key > keys;
    for (size_t i = 0; i < m; i++)
      keys += i;
    Vector x = xvalues.vector(keys);
    double* xdata = x.data();

    // create a y
    Vector y = Vector::Zero(m * D);
    TIME(RawImplicit, implicitFactor, xdata, y.data())
    TIME(RawJacobianQ, jf, xdata, y.data())
    TIME(RawJacobianQR, jqr, xdata, y.data())
  }
#endif

  os << m << endl;

} // timeAll

/*************************************************************************************/
int main(void) {
#ifdef SLOW
  os << "Implicit,";
  os << "JacobianQ,";
  os << "JacobianQR,";
#endif
#ifdef HESSIAN
  os << "Hessian,";
#endif
#ifdef OVERHEAD
  os << "Overhead,";
#endif
#ifdef RAW
  os << "RawImplicit,";
  os << "RawJacobianQ,";
  os << "RawJacobianQR,";
#endif
  os << "m" << endl;
  // define images
  vector < size_t > ms;
  //  ms += 2;
  //  ms += 3, 4, 5, 6, 7, 8, 9, 10;
  // ms += 11,12,13,14,15,16,17,18,19;
  //  ms += 20, 30, 40, 50;
  // ms += 20,30,40,50,60,70,80,90,100;
  for (size_t m = 2; m <= 50; m += 2)
    ms += m;
  //for (size_t m=10;m<=100;m+=10) ms += m;
  // loop over number of images
  for(size_t m: ms)
    timeAll<PinholePose<Cal3Bundler> >(m, NUM_ITERATIONS);
}

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


#ifndef __SKIPCTS_HPP__
#define __SKIPCTS_HPP__

/******************************
      Author: Joel Veness
        Date: 2013
******************************/

#include "common.hpp"

#include <set>
#include <string>
#include <map>

// boost includes
#include <boost/utility.hpp>

typedef uint64_t zobhash_t;
typedef std::vector<std::string> skip_contexts_t;
typedef std::vector<size_t> indices_t;
typedef std::vector<indices_t> indices_list_t;

struct aux_info_t {
    int skips_left;
    int last_idx;
    /** The total number of context variables split on */
    int num_splits;
};
typedef std::vector<aux_info_t> aux_info_list_t;

// skipping context tree node
class SkipNode {

    friend class SkipCTS;
    friend std::ostream &operator<<(std::ostream &o, const SkipNode &);

    public:

        SkipNode();
        ~SkipNode();

        // update the KT estimates
        void updateKT(bit_t b, double log_est_mul);

        // update weighted estimates at leaf nodes
        void updateWeightedLeaf();

        // log weighted blocked probability
        weight_t logProbWeighted() const { return m_log_prob_weighted; }

        // log KT estimated probability
        weight_t logProbEstimated() const { return m_log_prob_est; }

        // log Split probability
        weight_t logProbSplit() const { return m_log_prob_split; }

    private:

        // compute the KT-estimator update multiplier
        double logKTMul(bit_t b) const;
        double ktMul(bit_t b) const;

        weight_t m_log_prob_est;
        weight_t m_log_prob_split;
        weight_t m_log_prob_weighted;
        weight_t m_buf;

        weight_t *m_log_skip_lik;

        // one slot for each binary value
        count_t m_count[2];

        short m_depth;
        short m_submodels;
};

// skip context tree switching
class SkipCTS : public Compressor, boost::noncopyable {

    static const int MaxLogDepth = 9;
    static const int MaxDepth = (1 << MaxLogDepth);
    static const int LogTblSize = MaxDepth * 2;

    public:

        SkipCTS(history_t &history, int depth, int max_skips=1, size_t log2_slots=24);
        ~SkipCTS();

        // file extension
        virtual const char *fileExtension() const { return "skipcts"; }

        // the logarithm of the probability of all processed experience
        virtual double logBlockProbability() const;

        // the probability of seeing a particular symbol next
        virtual double prob(bit_t b);

        // process a new piece of sensory experience
        virtual void update(bit_t b);

    private:

        // update the switching posterior weights
        void updatePosteriors(SkipNode &n, int n_submodels, double alpha,
            double log_alpha, double log_stop_mul, double log_split_mul);

        // compute the switching rate for a given time t
        double switchRate(size_t t) const;

        // gets the node's index into the hash table
        SkipNode &getNode(zobhash_t hash, int depth, int submodels) const;

        // Returns how many submodels a node at a given context position, with a number of
        // skips left, has.
        int numSubmodels(int position, int skips) const;

        // precomputes the zobrist hash keys
        static void initZobrist();

        // update the weighted estimates
        void updateWeighted(SkipNode &n, zobhash_t hash, int end_idx, int depth);

        // performs the update operation to maintain a switching posterior
        void posteriorUpdate(SkipNode &n, double log_scale, double log_alpha,
            double log_K, double log_mul, double &log_post) const;

        // lazy allocation of skipping posterior weights
        void lazyAllocate(SkipNode &n, int n_submodels, int skips_left) const;

        // compute the information needed to update the current context stats
        void getContextInfo(zobhash_t &hash, const indices_t &idxs) const;

        // functions for precomputing the context indices
        void initContexts();
        void genBoundedSkipContexts(std::string buf, int depth, int skips);
        void printContexts(skip_contexts_t &l) const;

        // miscellaneous precomputations
        void initLogTbl();
        void initHashDeltas();
        void initAuxInfo();
        void initIndices();

        // compute the current binary context
        void getContext();

        // static members
        static zobhash_t s_zobtbl[MaxDepth][2];
        static weight_t s_log_tbl[LogTblSize];

        std::vector<aux_info_list_t> m_auxinfo;
        history_t &m_history;
        int m_depth;
        int m_skips;
        skip_contexts_t m_skip_contexts;
        std::vector<indices_list_t> m_indices;
        std::vector<size_t> m_context;
        std::vector<weight_t> m_log_skip_preds;
        SkipNode *m_nodes;
        zobhash_t m_mask;
};

#endif // __SKIPCTS_HPP__


#ifndef CRADLE_IMAGING_HISTOGRAM_EQUALIZE_HPP
#define CRADLE_IMAGING_HISTOGRAM_EQUALIZE_HPP

#include <cradle/imaging/histogram.hpp>
#include <cradle/imaging/apply_palette.hpp>
#include <cradle/imaging/view_transforms.hpp>
#include <vector>
#include <limits>
#include <boost/type_traits/remove_cv.hpp>

namespace cradle {

// Histogram equalize the source image and place the result in the destination
// image. Only use this on 8- or 16-bit images.
template<unsigned N, class DstT, class DstSP, class SrcT, class SrcSP>
void histogram_equalize(image<N,DstT,DstSP> const& dst,
    image<N,SrcT,SrcSP> const& src)
{
    typedef typename unsigned_channel_type<SrcT>::type unsigned_t;
    std::vector<DstT> palette(int(std::numeric_limits<unsigned_t>::max()) + 1);
    create_histogram_equalization_palette(src, &palette[0]);
    apply_palette(dst, src, &palette[0]);
}

// Fill in the given palette such that it will histogram equalize the given
// image if applied to it.
// The supplied palette should be capable of holding 2^n entries, where n is
// the number of bits in the source image's channel type.
template<unsigned N, class SrcT, class SrcSP, class PaletteT>
void create_histogram_equalization_palette(image<N,SrcT,SrcSP> const& img,
    PaletteT* palette)
{
    // Compute a histogram of the image.  Using a subsampled view speeds up
    // the process significantly, and unless the image contains a very strong,
    // regular, high-frequency pattern, it shouldn't affect the result much.
    std::vector<unsigned> hist;
    if (product(img.size) > 10000)
    {
        compute_raw_histogram(&hist,
            subsampled_view(img, make_vector<unsigned>(4, 4)));
    }
    else
        compute_raw_histogram(&hist, img);

    typedef typename unsigned_channel_type<SrcT>::type unsigned_t;
    typedef typename boost::remove_cv<SrcT>::type source_t;

    int const min_channel_v = int(std::numeric_limits<source_t>::min());
    int const max_channel_v = int(std::numeric_limits<source_t>::max());

    // Do a cumulative integral of the histogram.
    int sum = 0, highest = 0;
    for (int i = min_channel_v; i <= max_channel_v; ++i)
    {
        unsigned_t index = unsigned_t(SrcT(i));
        int v = hist[index];
        if (v != 0)
            highest = sum;
        hist[index] = sum;
        sum += v;
    }

    // Create the palette by scaling the integrated histogram to fill all
    // possible values in the palette's channel type.
    float scale = float(std::numeric_limits<PaletteT>::max()) / highest;
    for (int i = min_channel_v; i <= max_channel_v; ++i)
    {
        unsigned_t index = unsigned_t(SrcT(i));
        int tmp = int(scale * hist[index]);
        palette[index] = PaletteT(std::min(tmp,
            int(std::numeric_limits<PaletteT>::max())));
    }
}

}

#endif


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2010.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/return_.hpp>
#include <mpllibs/metamonad/bind.hpp>
#include <mpllibs/metamonad/tmp_value.hpp>
#include <mpllibs/metamonad/lambda_c.hpp>
#include <mpllibs/metamonad/name.hpp>
#include <mpllibs/metamonad/either.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/tag.hpp>
#include <boost/mpl/assert.hpp>

#include <boost/static_assert.hpp>

#include <boost/test/unit_test.hpp>

#include "common.hpp"

#include <iostream>

using boost::mpl::minus;

using mpllibs::metamonad::tmp_value;
using mpllibs::metamonad::lambda_c;
using mpllibs::metamonad::right;

namespace
{
  typedef lambda_c<a, right<minus<a, int2> > > minus_2;
}

BOOST_AUTO_TEST_CASE(test_monad)
{
  using boost::mpl::equal_to;
  using boost::mpl::tag;

  using mpllibs::metamonad::return_;
  using mpllibs::metamonad::bind;
  using mpllibs::metamonad::left;
  using mpllibs::metamonad::either_tag;

  typedef tag<int13>::type int_tag;
  typedef either_tag<int_tag, int_tag> either;

  // test_return
  BOOST_MPL_ASSERT((equal_to<right<int13>, return_<either, int13>::type>));

  // test_bind_right
  BOOST_MPL_ASSERT((
    equal_to<right<int11>, bind<either, right<int13>, minus_2>::type>
  ));

  // test_bind_left
  BOOST_MPL_ASSERT((
    equal_to<left<int13>, bind<either, left<int13>, minus_2>::type>
  ));

  // test_multi_step_chain
  BOOST_MPL_ASSERT((
    equal_to<
      right<int9>,
      bind<either, bind<either, return_<either, int13>, minus_2>, minus_2>
    >
  ));
}


/*
May 2015

          Authors:
Ankit Dhall     Yash Chandak

TO DO:
-set capture frame size in VideoCapture object
-take lines only closer to the region of the vanishing points
    - plug the prevRes in the eqns for new frame and consider lines only with error < |epsilon|
-cluster more if more than 2 vanishing points are present (advanced)
-refactor the CODE
-error calculation, remove so many divisions!

*/
#include <iostream>
#include <armadillo>
#include <string>
#include "opencv2/core/core.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <vector>
#include <fstream>
#include <math.h>
#include <ctime>

//add OpenCV and Armadillo namespaces
using namespace cv;
using namespace std;
using namespace arma;

class vanishingPt
{
    public:
    cv::Mat image, img, gray;
    cv::Mat frame;
    vector< vector<int> > points;
    mat A,b, prevRes;
    mat Atemp, btemp, res, aug, error, soln;
    //ofstream out1, out2;
    float epsilon;

    //store slope (m) and y-intercept (c) of each lines
    float m,c;

    //store minimum length for lines to be considered while estimating vanishing point
    int minlength;

    //temporary vector for intermediate storage
    vector<int> temp;

    //store (x1, y1) and (x2, y2) endpoints for each line segment
    vector<cv::Vec4i> lines_std;

    //video capture object from OpenCV
    cv::VideoCapture cap;

    //to store intermediate errors
    double temperr;

    //constructor to set video/webcam and find vanishing point
    vanishingPt()
    {
        cv::namedWindow("win", 2);
        cv::namedWindow("Lines", 2);

        // to calculate fps
        clock_t begin, end;

        //read from video file on disk
        //to read from webcam initialize as: cap = VideoCapture(int device_id);
        cap = VideoCapture(0);
        //cap = VideoCapture("road.m4v");

        if( cap.isOpened() )//check if camera/ video stream is available
        {
            //get first frame to intialize the values
            cap.read(frame);
            image= cv::Mat(cv::Size(frame.rows,frame.cols), CV_8UC1, 0.0);
        }

        // define minimum length requirement for any line
        minlength = image.cols * image.cols * 0.001 ;
        minlength /= 10. ; //added for debugging ...must be removed later
        int flag=0;
        while( cap.isOpened() )//check if camera/ video stream is available
        {
            if ( ! cap.grab() )
                continue;

            if(!cap.retrieve(img))
                continue;

            //it's advised not to modify image stored in the buffer structure of the opencv.
            frame  = img.clone();

            //to calculate fps
            begin = clock();

            cv::cvtColor(frame,image , cv::COLOR_BGR2GRAY);

            //resize frame to 480x320
            cv::resize(image, image, cv::Size(480,320));

            //equalize histogram
            cv::equalizeHist(image, image);

            //initialize the line segment matrix in format y = m*x + c
            init(image, prevRes);

            //draw lines on image and display
            makeLines(flag);

            //approximate vanishing point
            eval();

            //to calculate fps
            end = clock();

            //hit 'esc' to exit program
            int k = cv::waitKey(1);
            cout<<"fps: "<<1/(double(end-begin)/CLOCKS_PER_SEC)<<";k="<<k<<endl;

            if ( k==27 )
                break;
        }

    }
    void init(cv::Mat image, mat prevRes)
    {
        //create OpenCV object for line segment detection
        cv::Ptr<cv::LineSegmentDetector> ls = cv::createLineSegmentDetector(cv::LSD_REFINE_STD);

        //initialize
        lines_std.clear();

        //detect lines in image and store in linse_std
        //store (x1, y1) and (x2, y2) endpoints for each line segment
        ls->detect(image, lines_std);

        // Show found lines
        cv::Mat drawnLines (image);

        for(int i=0; i<(int)lines_std.size(); i++)
        {

            //ignore if almost vertical
            if ( abs(lines_std[i][0]-lines_std[i][2]) < 10 || abs(lines_std[i][1]-lines_std[i][3]) < 10) //check if almost vertical
                continue;
            //ignore shorter lines (x1-x2)^2 + (y2-y1)^2 < minlength
            if( ((lines_std[i][0]-lines_std[i][2])*(lines_std[i][0]-lines_std[i][2]) +(lines_std[i][1]-lines_std[i][3])*(lines_std[i][1]-lines_std[i][3])) < minlength)
                continue;

            //store valid lines' endpoints for calculations
            for(int j=0; j<4; j++)
            {
                temp.push_back(lines_std[i][j]);
            }

            points.push_back(temp);
            temp.clear();
        }
        ls->drawSegments(drawnLines, lines_std);
        cv::imshow("Lines", drawnLines);
        cout<<"Detected:"<<lines_std.size()<<endl;
        cout<<"Filtered:"<<points.size()<<endl;
    }
    void makeLines(int flag)
    {
        // to solve Ax = b for x
        int points_size = (int) points.size();
        A = zeros<mat>(points_size, 2);
        b = zeros<mat>(points_size, 1);

        //convert given end-points of line segment into a*x + b*y = c format for calculations
        //do for each line segment detected
        for(int i=0; i<points_size; i++)
        {

            A(i,0)=-(points[i][3]-points[i][1]);            //-(y2-y1)
            A(i,1)=(points[i][2]-points[i][0]);             //x2-x1
            b(i,0)=A(i,0)*points[i][0]+A(i,1)*points[i][1]; //-(y2-y1)*x1 + (x2-x1)*y1
        }
    }

    //estimate the vanishing point
    void eval()
    {
        //stores the estimated co-ordinates of the vanishing point with respect to the image
        soln= zeros<mat>(2,1);

        //initialize error
        double err = 9999999999;

        //calculate point of intersection of every pair of lines and
        //find the sum of distance from all other lines
        //select the point which has the minimum sum of distance
        int points_size = (int) points.size();
        for(int i=0; i<points_size; i++)
        {
            for(int j=0; j<points_size; j++)
            {
                if(i >= j)
                continue;

                //armadillo vector
                uvec indices;

                //store indices of lines to be used for calculation
                indices << i << j;

                //extract the rows with indices specified in uvec indices
                //stores the ith and jth row of matrices A and b into Atemp and btemp respectively
                //hence creating a 2x2 matrix for calculating point of intersection
                Atemp = A.rows(indices);
                btemp = b.rows(indices);

                //if lines are parallel then skip
                if(arma::rank(Atemp) != 2)
                    continue;

                //solves for 'x' in A*x = b
                res = calc(Atemp, btemp);

                if(res.n_rows == 0 || res.n_cols == 0)
                    continue;

                // calculate error assuming perfect intersection is
                error = A*res - b;

                //reduce size of error
                error = error/1000;

                // to store intermediate error values
                temperr = 0;
                //summation of errors
                for(int i=0; i<(int)error.n_rows ; i++)
                    temperr+=(error(i,0)*error(i,0))/1000;

                //scale errors to prevent any overflows
                temperr/=1000000;

                //if current error is smaller than previous min error then update the solution (point)
                if(err > temperr)
                {
                    soln = res;
                    err = temperr;
                }
            }
        }

        //cout<<"\n\nResult:\n"<<soln(0,0)<<","<<soln(1,0)<<"\nError:"<<err<<"\n\n";

        // draw a circle to visualize the approximate vanishing point
        if(soln(0,0) > 0 && soln(0,0) < image.cols && soln(1,0) > 0 && soln(1,0) < image.rows)
            cv::circle(image, Point(soln(0,0), soln(1,0)), 25, cv::Scalar(0,0,255), 10);
        cv::imshow("win", image);

        //flush the vector
        points.clear();

        //toDo: use previous frame's result to reduce calculations and stabilize the region of vanishing point
        prevRes = soln;
    }

    //function to calculate and return the intersection point
    mat calc(mat A, mat b)
    {
        mat x = zeros<mat>(2,1);
        solve(x,A,b);
        return x;
    }
};

int main()
{
    // make object
    vanishingPt obj;
    cv::destroyAllWindows();
    return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/bind/bind.hpp>
using namespace boost::placeholders;

#include <boost/make_shared.hpp>
#include <memory>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "tudat/basics/testMacros.h"
#include "tudat/astro/basic_astro/orbitalElementConversions.h"
#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/astro/basic_astro/unitConversions.h"
#include "tudat/math/integrators/rungeKutta4Integrator.h"
#include "tudat/astro/basic_astro/stateVectorIndices.h"
#include "tudat/basics/basicTypedefs.h"
#include "tudat/io/basicInputOutput.h"
#include "tudat/simulation/propagation_setup/dynamicsSimulator.h"
#include "tudat/interface/spice/spiceInterface.h"
#include "tudat/simulation/environment_setup/body.h"
#include "tudat/simulation/environment_setup/createBodies.h"
#include "tudat/simulation/estimation_setup/createNumericalSimulator.h"

namespace tudat
{
namespace unit_tests
{

using namespace mathematical_constants;

BOOST_AUTO_TEST_SUITE( test_hybrid_state_derivative_model )

std::map< double, Eigen::VectorXd > propagateKeplerOrbitAndMassState(
        const int simulationCase )
{
    using namespace simulation_setup;
    using namespace propagators;
    using namespace numerical_integrators;
    using namespace orbital_element_conversions;
    using namespace basic_mathematics;
    using namespace gravitation;
    using namespace numerical_integrators;
    using namespace unit_conversions;

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation end epoch.
    const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY;

    // Set numerical integration fixed step size.
    const double fixedStepSize = 60.0;

    // Define body settings for simulation.
    BodyListSettings bodySettings = BodyListSettings( "SSB", "J2000" );
    bodySettings.addSettings( "Earth" );
    bodySettings.at( "Earth" )->ephemerisSettings = std::make_shared< ConstantEphemerisSettings >(
                Eigen::Vector6d::Zero( ), "SSB", "J2000" );
    bodySettings.at( "Earth" )->gravityFieldSettings = std::make_shared< GravityFieldSettings >( central_spice );

    // Create Earth object
    SystemOfBodies bodies = createSystemOfBodies( bodySettings );

    // Create spacecraft object.
    bodies.createEmptyBody( "Asterix" );

    // Define propagator settings variables.
    SelectedAccelerationMap accelerationMap;
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;

    // Define propagation settings.
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfAsterix;
    accelerationsOfAsterix[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::point_mass_gravity ) );
    accelerationMap[ "Asterix" ] = accelerationsOfAsterix;
    bodiesToPropagate.push_back( "Asterix" );
    centralBodies.push_back( "Earth" );

    // Create acceleration models and propagation settings.
    basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodies, accelerationMap, bodiesToPropagate, centralBodies );

    // Set Keplerian elements for Asterix.
    Eigen::Vector6d asterixInitialStateInKeplerianElements;
    asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3;
    asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
    asterixInitialStateInKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 85.3 );
    asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
            = convertDegreesToRadians( 235.7 );
    asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
            = convertDegreesToRadians( 23.4 );
    asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 );

    // Convert Asterix state from Keplerian elements to Cartesian elements.
    double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
    Eigen::VectorXd systemInitialState = convertKeplerianToCartesianElements(
                asterixInitialStateInKeplerianElements,
                earthGravitationalParameter );

    std::shared_ptr< SingleArcPropagatorSettings< double > > translationalPropagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState,
              std::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) );

    // Create mass rate model and mass propagation settings
    std::map< std::string, std::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels;
    massRateModels[ "Vehicle" ] = std::make_shared< basic_astrodynamics::CustomMassRateModel >(
                [ ]( const double ){ return -0.01; } );
    Eigen::VectorXd initialMass = Eigen::VectorXd( 1 );
    initialMass( 0 ) = 500.0;
    std::shared_ptr< SingleArcPropagatorSettings< double > > massPropagatorSettings =
            std::make_shared< MassPropagatorSettings< double > >(
                std::vector< std::string >{ "Asterix" }, massRateModels, initialMass,
                std::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) );

    // Create total propagator settings, depending on current case.
    std::shared_ptr< SingleArcPropagatorSettings< double > > propagatorSettings;
    if( ( simulationCase  % 3 ) == 0 )
    {
        propagatorSettings = translationalPropagatorSettings;
    }
    else if( ( simulationCase  % 3 ) == 1 )
    {
        propagatorSettings = massPropagatorSettings;
    }
    else if( ( simulationCase  % 3 ) == 2 )
    {
        std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > >  propagatorSettingsList;
        propagatorSettingsList.push_back( translationalPropagatorSettings );
        propagatorSettingsList.push_back( massPropagatorSettings );

        propagatorSettings = std::make_shared< MultiTypePropagatorSettings< double > >(
                    propagatorSettingsList,
                    std::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) );
    }

    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, 0.0, fixedStepSize );

    // Create simulation object and propagate dynamics.
    SingleArcDynamicsSimulator< > dynamicsSimulator(
                bodies, integratorSettings, propagatorSettings, true, false, true );

    // Return propagated dynamics (if simulationCase < 3) or interpolated dynamics (else)
    if( simulationCase < 3 )
    {
        return dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
    }
    else
    {
        std::map< double, Eigen::VectorXd > returnMap;
        double currentTime = 4.0 * fixedStepSize;

        double interpolationTimeStep = 32.1;

        // Interpolate dynamics for range of times.
        while( currentTime < simulationEndEpoch - 4.0 * fixedStepSize )
        {

            // Interpolate propagated dynamics type
            if( simulationCase == 3 )
            {
                returnMap[ currentTime ] = bodies.at( "Asterix" )->getEphemeris( )->getCartesianState( currentTime );
            }
            else if( simulationCase == 4 )
            {
                returnMap[ currentTime ] = Eigen::VectorXd::Zero( 1 );
                returnMap[ currentTime ]( 0 ) = bodies.at( "Asterix" )->getBodyMassFunction( )( currentTime );
            }
            else if( simulationCase == 5 )
            {
                returnMap[ currentTime ] = Eigen::VectorXd::Zero( 7 );
                returnMap[ currentTime ].segment( 0, 6 ) = bodies.at( "Asterix" )->getEphemeris( )->getCartesianState( currentTime );
                returnMap[ currentTime ]( 6 ) = bodies.at( "Asterix" )->getBodyMassFunction( )( currentTime );

            }

            // Increment time (non-resonant with integration time step).
            currentTime += interpolationTimeStep;
        }

        return returnMap;
    }

}
//! Test if conversion from Keplerian elements to Cartesian elements is working correctly.
BOOST_AUTO_TEST_CASE( testHybridStateDerivativeModel )
{
    // Compare separate and multitype (independent) propagation directly from propagation (useCase = 0) and
    // interpolated from reset dynamics (useCase = 1)
    for( int useCase = 0; useCase < 2; useCase++ )
    {
        int simulationCaseToAdd = 3;
        if( useCase == 0 )
        {
             simulationCaseToAdd = 0;
        }

        // Propagate dynamics for translational, mass and combined state.
        std::map< double, Eigen::VectorXd >  translationalState = propagateKeplerOrbitAndMassState( 0 + simulationCaseToAdd );
        std::map< double, Eigen::VectorXd >  massState = propagateKeplerOrbitAndMassState( 1 + simulationCaseToAdd );
        std::map< double, Eigen::VectorXd >  combinedState = propagateKeplerOrbitAndMassState( 2  + simulationCaseToAdd );

        std::map< double, Eigen::VectorXd >::const_iterator stateIterator = translationalState.begin( );
        std::map< double, Eigen::VectorXd >::const_iterator massIterator = massState.begin( );
        std::map< double, Eigen::VectorXd >::const_iterator combinedIterator = combinedState.begin( );

        // Compare separate and multitype dynamics of each type.
        for( unsigned int i = 0; i < translationalState.size( ); i++ )
        {
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION( stateIterator->second, combinedIterator->second.segment( 0, 6 ),
                                               std::numeric_limits< double >::epsilon( ) );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION( massIterator->second, combinedIterator->second.segment( 6, 1 ),
                                               std::numeric_limits< double >::epsilon( ) );
            stateIterator++;
            massIterator++;
            combinedIterator++;
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#define BOOST_TEST_MODULE "test_bond_angle_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/BondAngleInteraction.hpp>
#include <mjolnir/forcefield/local/HarmonicPotential.hpp>
#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/make_unique.hpp>

#include <random>

BOOST_AUTO_TEST_CASE(BondAngleInteraction_force)
{
    using traits_type     = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type       = traits_type::real_type;
    using coord_type      = traits_type::coordinate_type;
    using boundary_type   = traits_type::boundary_type;
    using system_type     = mjolnir::System<traits_type>;
    using harmonic_type   = mjolnir::HarmonicPotential<real_type>;
    using bond_angle_type = mjolnir::BondAngleInteraction<traits_type, harmonic_type>;

    constexpr real_type tol = 1e-7;

    auto normalize = [](const coord_type& v){return v / mjolnir::math::length(v);};

    const real_type k(1e0);
    const real_type native(mjolnir::math::constants<real_type>::pi() * 2.0 / 3.0); // 120 degree

    harmonic_type potential{k, native};
    bond_angle_type interaction("none", {{ {{0,1,2}}, potential}});

    const coord_type pos1(1., 0., 0.);
    const coord_type pos2(0., 0., 0.);
    system_type sys(3, boundary_type{});

    sys.at(0).mass = 1.0;
    sys.at(1).mass = 1.0;
    sys.at(2).mass = 1.0;
    sys.at(0).rmass = 1.0;
    sys.at(1).rmass = 1.0;
    sys.at(2).rmass = 1.0;

    sys.at(0).position = pos1;
    sys.at(1).position = pos2;
    sys.at(2).position = coord_type(0,0,0);
    sys.at(0).velocity = coord_type(0,0,0);
    sys.at(1).velocity = coord_type(0,0,0);
    sys.at(2).velocity = coord_type(0,0,0);
    sys.at(0).force    = coord_type(0,0,0);
    sys.at(1).force    = coord_type(0,0,0);
    sys.at(2).force    = coord_type(0,0,0);

    sys.at(0).name  = "X";
    sys.at(1).name  = "X";
    sys.at(2).name  = "X";
    sys.at(0).group = "NONE";
    sys.at(1).group = "NONE";
    sys.at(2).group = "NONE";

    const int N = 1800;
    const real_type dtheta = mjolnir::math::constants<real_type>::pi()  / N;
    for(int i = 1; i < N; ++i)
    {
        BOOST_TEST(mjolnir::math::length(sys[0].position - pos1) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys[1].position - pos2) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys[0].velocity)        == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys[1].velocity)        == 0.0, boost::test_tools::tolerance(tol));
        sys[0].force = coord_type(0,0,0);
        sys[1].force = coord_type(0,0,0);
        sys[2].force = coord_type(0,0,0);

        const real_type theta = i * dtheta;
        const coord_type pos3(std::cos(theta), std::sin(theta), 0e0);
        sys[2].position = pos3;

        const real_type deriv = potential.derivative(theta);
        const real_type coef = std::abs(deriv);

        interaction.calc_force(sys);

        // magnitude
        // if radius == 1e0, then force strength is equal to dV/dtheta.
        BOOST_TEST(mjolnir::math::length(sys[0].position - sys[1].position) == 1.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys[2].position - sys[1].position) == 1.0, boost::test_tools::tolerance(tol));

        const real_type force_strength1 = mjolnir::math::length(sys[0].force);
        const real_type force_strength3 = mjolnir::math::length(sys[2].force);
        if(i == 1200) // most stable point
        {
            BOOST_TEST(coef == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(coef == 0.0, boost::test_tools::tolerance(tol));
        }
        else
        {
            BOOST_TEST(coef == force_strength1, boost::test_tools::tolerance(tol));
            BOOST_TEST(coef == force_strength3, boost::test_tools::tolerance(tol));
        }

        // force applied to center particle is equal to sum of others
        const coord_type sum = sys[0].force + sys[1].force + sys[2].force;
        BOOST_TEST(mjolnir::math::length(sum) == 0.0, boost::test_tools::tolerance(tol));

        // direction
        if(i == 1200) // most stable point
        {
            BOOST_TEST(std::abs(force_strength1) == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(std::abs(force_strength3) == 0.0, boost::test_tools::tolerance(tol));
        }
        else if(i < 1200) // narrow
        {
            // perpendicular to radius vector
            const real_type dot1 = mjolnir::math::dot_product(sys[0].force, sys[0].position - sys[1].position);
            const real_type dot2 = mjolnir::math::dot_product(sys[2].force, sys[2].position - sys[1].position);
            BOOST_TEST(dot1 == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(dot2 == 0.0, boost::test_tools::tolerance(tol));

            const coord_type f1(0., -1., 0.);
            BOOST_TEST(mjolnir::math::length(normalize(sys[0].force) - f1) == 0.0, boost::test_tools::tolerance(tol));

            const coord_type f3(
                    cos(theta + mjolnir::math::constants<real_type>::pi() / 2.),
                    sin(theta + mjolnir::math::constants<real_type>::pi() / 2.),
                    0.);
            BOOST_TEST(mjolnir::math::length(normalize(sys[2].force) - f3) == 0.0, boost::test_tools::tolerance(tol));
        }
        else if(i > 1200) // extensive
        {
            const real_type dot1 = mjolnir::math::dot_product(sys[0].force, sys[0].position - sys[1].position);
            const real_type dot2 = mjolnir::math::dot_product(sys[2].force, sys[2].position - sys[1].position);
            BOOST_TEST(dot1 == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(dot2 == 0.0, boost::test_tools::tolerance(tol));

            const coord_type f1(0., 1., 0.);
            BOOST_TEST(mjolnir::math::length(normalize(sys[0].force) - f1) == 0.0, boost::test_tools::tolerance(tol));

            const coord_type f3(
                    cos(theta - mjolnir::math::constants<real_type>::pi() / 2.),
                    sin(theta - mjolnir::math::constants<real_type>::pi() / 2.),
                    0.);
            BOOST_TEST(mjolnir::math::length(normalize(sys[2].force) - f3) == 0.0, boost::test_tools::tolerance(tol));
        }

        // perpendicular to z axis
        BOOST_TEST(sys[0].force[2] == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys[1].force[2] == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys[2].force[2] == 0.0, boost::test_tools::tolerance(tol));
    }
}

BOOST_AUTO_TEST_CASE(BondAngleInteraction_numerical_diff)
{
    using traits_type     = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type       = traits_type::real_type;
    using coord_type      = traits_type::coordinate_type;
    using boundary_type   = traits_type::boundary_type;
    using system_type     = mjolnir::System<traits_type>;
    using harmonic_type   = mjolnir::HarmonicPotential<real_type>;
    using bond_angle_type = mjolnir::BondAngleInteraction<traits_type, harmonic_type>;

    const real_type k(10.0);
    const real_type native(mjolnir::math::constants<real_type>::pi() / 3.0);

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    harmonic_type potential{k, native};
    bond_angle_type interaction("none", {{ {{0,1,2}}, potential}});

    for(int i = 0; i < 1000; ++i)
    {
        system_type sys(3, boundary_type{});

        sys.mass (0)= 1.0;
        sys.mass (1)= 1.0;
        sys.mass (2)= 1.0;
        sys.rmass(0) = 1.0;
        sys.rmass(1) = 1.0;
        sys.rmass(2) = 1.0;

        sys.position(0) = coord_type(1.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt));
        sys.position(1) = coord_type(0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt));
        sys.position(2) = coord_type(1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt));
        sys.velocity(0) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(1) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(2) = coord_type(0.0, 0.0, 0.0);
        sys.force   (0) = coord_type(0.0, 0.0, 0.0);
        sys.force   (1) = coord_type(0.0, 0.0, 0.0);
        sys.force   (2) = coord_type(0.0, 0.0, 0.0);

        sys.name (0) = "X";
        sys.name (1) = "X";
        sys.name (2) = "X";
        sys.group(0) = "NONE";
        sys.group(1) = "NONE";
        sys.group(2) = "NONE";

        const auto init = sys;

        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;
        for(std::size_t idx=0; idx<3; ++idx)
        {
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::X(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::Y(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::Z(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(BondAngleInteraction_calc_force_and_energy)
{
    using traits_type     = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type       = traits_type::real_type;
    using coord_type      = traits_type::coordinate_type;
    using boundary_type   = traits_type::boundary_type;
    using system_type     = mjolnir::System<traits_type>;
    using harmonic_type   = mjolnir::HarmonicPotential<real_type>;
    using bond_angle_type = mjolnir::BondAngleInteraction<traits_type, harmonic_type>;

    const real_type k(10.0);
    const real_type native(mjolnir::math::constants<real_type>::pi() / 3.0);

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    harmonic_type potential{k, native};
    bond_angle_type interaction("none", {{ {{0,1,2}}, potential}});

    for(int i = 0; i < 1000; ++i)
    {
        system_type sys(3, boundary_type{});

        sys.mass (0)= 1.0;
        sys.mass (1)= 1.0;
        sys.mass (2)= 1.0;
        sys.rmass(0) = 1.0;
        sys.rmass(1) = 1.0;
        sys.rmass(2) = 1.0;

        sys.position(0) = coord_type(1.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt));
        sys.position(1) = coord_type(0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt));
        sys.position(2) = coord_type(1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt));
        sys.velocity(0) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(1) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(2) = coord_type(0.0, 0.0, 0.0);
        sys.force   (0) = coord_type(0.0, 0.0, 0.0);
        sys.force   (1) = coord_type(0.0, 0.0, 0.0);
        sys.force   (2) = coord_type(0.0, 0.0, 0.0);

        sys.name (0) = "X";
        sys.name (1) = "X";
        sys.name (2) = "X";
        sys.group(0) = "NONE";
        sys.group(1) = "NONE";
        sys.group(2) = "NONE";

        constexpr real_type tol = 1e-4;
        auto ref_sys = sys;

        const auto energy = interaction.calc_force_and_energy(sys);
        const auto ref_energy = interaction.calc_energy(ref_sys);
        interaction.calc_force(ref_sys);
        BOOST_TEST(ref_energy == energy, boost::test_tools::tolerance(tol));

        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            BOOST_TEST(mjolnir::math::X(sys.force(idx)) == mjolnir::math::X(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Y(sys.force(idx)) == mjolnir::math::Y(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Z(sys.force(idx)) == mjolnir::math::Z(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
        }
    }
}


#ifndef RIGIDTRANSFORMER_HPP
#define RIGIDTRANSFORMER_HPP

#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <stdio.h>
#include <iostream>
#include "../global.hpp"

typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic> MatDynFloat; //matrix MxN of type float
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::Vector3f Vec3Float;
typedef Eigen::Vector4f Vec4Float;
typedef Eigen::Matrix3f Mat3Float;
typedef Eigen::Matrix4f Mat4Float;
typedef Eigen::SelfAdjointEigenSolver<Mat4Float> EigenVectorDecomposer;

namespace registration{

class RigidTransformer
{
    /*
    # GOAL
    This class computes the rigid transformation between a set a features and
    a set of corresponding features. Each correspondence can be weighed between
    0.0 and 1.0.

    # INPUTS
    -ioFeatures
    -inCorrespondingFeatures
    -inWeights

    # PARAMETERS
    -scaling:
    Whether or not to allow scaling.

    # OUTPUTS
    -ioFeatures
    */
    public:

        void set_input(const FeatureMat * const inCorrespondingFeatures, const VecDynFloat * const inWeights);
        void set_output(FeatureMat * const ioFeatures);
        void set_parameters(bool scaling);
        Mat4Float get_transformation() const {return _transformationMatrix;}
        void update();

    protected:

    private:
        //# Inputs
        FeatureMat * _ioFeatures = NULL;
        const FeatureMat * _inCorrespondingFeatures = NULL;
        const VecDynFloat * _inWeights = NULL;

        //# Outputs
        //_ioFeatures is used as both an input (to compute the transformation) and output

        //# User Parameters
        bool _scaling = false;

        //# Internal Data structures
        Mat4Float _transformationMatrix = Mat4Float::Identity();

        //# Internal Parameters
        size_t _numElements = 0;
        size_t _numFeatures = 0;

        //# Internal functions
        //## Function to update the transformation matrix and apply it to the floating positions
        void _update_transformation();
};

}//namespace registration

#endif // RIGIDTRANSFORMER_HPP