//          Copyright W.P. McNeill 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 <boost/concept/assert.hpp>
#include <boost/graph/adjacency_iterator.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/properties.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <utility>

/*
This file defines a simple example of a read-only implicit weighted graph
built using the Boost Graph Library. It can be used as a starting point for
developers creating their own implicit graphs.

The graph models the following concepts:
  Graph
  IncidenceGraph
  BidirectionalGraph
  AdjacencyGraph
  VertexListGraph
  EdgeListGraph
  AdjacencyMatrix
  ReadablePropertyGraph

The graph defined here is a ring graph, a graph whose vertices are arranged in
a ring so that each vertex has exactly two neighbors. For example, here is a
ring graph with five nodes.

                                    0
                                  /   \
                                4      1
                                |      |
                                3 ---- 2

The edges of this graph are undirected and each has a weight that is a
function of its position in the graph.

The vertices indexed are by integer and arranged sequentially so that each
vertex i is adjacent to i-1 for i>0 and i+1 for i<n-1.  Vertex 0 is also
adjacent to vertex n-1.  Edges are indexed by pairs of vertex indices.

Various aspects of the graph are modeled by the following classes:

  ring_graph
    The graph class instantiated by a client. This defines types for the
    concepts that this graph models and keeps track of the number of vertices
    in the graph.

  ring_incident_edge_iterator
    This is an iterator that ranges over edges incident on a given vertex. The
    behavior of this iterator defines the ring topology. Other iterators that
    make reference to the graph structure are defined in terms of this one.

  edge_weight_map
    This defines a property map between edges and weights. Here edges have a
    weight equal to the average of their endpoint vertex indices, i.e. edge
    (2,3) has weight 2.5, edge (0,4) has weight 2, etc.

  boost::property_map<graph, boost::edge_weight_t>
    This tells Boost to associate the edges of the ring graph with the edge
    weight map.

Along with these classes, the graph concepts are modeled by various valid
expression functions defined below.  This example also defines a
get(boost::vertex_index_t, const ring_graph&) function which isn't part of a
graph concept, but is used for Dijkstra search.

Apart from graph, client code should not instantiate the model classes
directly. Instead it should access them and their properties via
graph_traits<...> and property_traits<...> lookups. For convenience,
this example defines short names for all these properties that client code can
use.
*/

// Forward declarations
class ring_graph;
class ring_incident_edge_iterator;
class ring_adjacency_iterator;
class ring_edge_iterator;
struct edge_weight_map;

// ReadablePropertyGraph associated types
namespace boost
{
template <> struct property_map< ring_graph, edge_weight_t >
{
    typedef edge_weight_map type;
    typedef edge_weight_map const_type;
};

template <> struct property_map< const ring_graph, edge_weight_t >
{
    typedef edge_weight_map type;
    typedef edge_weight_map const_type;
};
}

// Tag values that specify the traversal type in graph::traversal_category.
struct ring_traversal_catetory : virtual public boost::bidirectional_graph_tag,
                                 virtual public boost::adjacency_graph_tag,
                                 virtual public boost::vertex_list_graph_tag,
                                 virtual public boost::edge_list_graph_tag
{
};

/*
Undirected graph of vertices arranged in a ring shape.

Vertices are indexed by integer, and edges connect vertices with consecutive
indices.  Vertex 0 is also adjacent to the vertex n-1.
*/
class ring_graph
{
public:
    // Graph associated types
    typedef std::size_t vertex_descriptor;
    typedef boost::undirected_tag directed_category;
    typedef boost::disallow_parallel_edge_tag edge_parallel_category;
    typedef ring_traversal_catetory traversal_category;

    // IncidenceGraph associated types
    typedef std::pair< vertex_descriptor, vertex_descriptor > edge_descriptor;
    typedef ring_incident_edge_iterator out_edge_iterator;
    typedef std::size_t degree_size_type;

    // BidirectionalGraph associated types
    // Note that undirected graphs make no distinction between in- and out-
    // edges.
    typedef ring_incident_edge_iterator in_edge_iterator;

    // AdjacencyGraph associated types
    typedef ring_adjacency_iterator adjacency_iterator;

    // VertexListGraph associated types
    typedef boost::counting_iterator< vertex_descriptor > vertex_iterator;
    typedef std::size_t vertices_size_type;

    // EdgeListGraph associated types
    typedef ring_edge_iterator edge_iterator;
    typedef std::size_t edges_size_type;

    // This type is not part of a graph concept, but is used to return the
    // default vertex index map used by the Dijkstra search algorithm.
    typedef vertex_descriptor vertex_property_type;

    ring_graph(std::size_t n) : m_n(n) {};
    std::size_t n() const { return m_n; }

private:
    // The number of vertices in the graph.
    std::size_t m_n;
};

// Use these graph_traits parameterizations to refer to the associated
// graph types.
typedef boost::graph_traits< ring_graph >::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits< ring_graph >::edge_descriptor edge_descriptor;
typedef boost::graph_traits< ring_graph >::out_edge_iterator out_edge_iterator;
typedef boost::graph_traits< ring_graph >::in_edge_iterator in_edge_iterator;
typedef boost::graph_traits< ring_graph >::adjacency_iterator
    adjacency_iterator;
typedef boost::graph_traits< ring_graph >::degree_size_type degree_size_type;
typedef boost::graph_traits< ring_graph >::vertex_iterator vertex_iterator;
typedef boost::graph_traits< ring_graph >::vertices_size_type
    vertices_size_type;
typedef boost::graph_traits< ring_graph >::edge_iterator edge_iterator;
typedef boost::graph_traits< ring_graph >::edges_size_type edges_size_type;

// Tag values passed to an iterator constructor to specify whether it should
// be created at the start or the end of its range.
struct iterator_position
{
};
struct iterator_start : virtual public iterator_position
{
};
struct iterator_end : virtual public iterator_position
{
};

/*
Iterator over edges incident on a vertex in a ring graph.

For vertex i, this returns edge (i, i+1) and then edge (i, i-1), wrapping
around the end of the ring as needed.

It is implemented with the boost::iterator_adaptor class, adapting an
offset into the dereference::ring_offset array.
*/
class ring_incident_edge_iterator
: public boost::iterator_adaptor< ring_incident_edge_iterator,
      boost::counting_iterator< std::size_t >, edge_descriptor,
      boost::use_default, edge_descriptor >
{
public:
    ring_incident_edge_iterator()
    : ring_incident_edge_iterator::iterator_adaptor_(0), m_n(0), m_u(0) {};
    explicit ring_incident_edge_iterator(
        const ring_graph& g, vertex_descriptor u, iterator_start)
    : ring_incident_edge_iterator::iterator_adaptor_(0), m_n(g.n()), m_u(u) {};
    explicit ring_incident_edge_iterator(
        const ring_graph& g, vertex_descriptor u, iterator_end)
    : // A graph with one vertex only has a single self-loop.  A graph with
      // two vertices has a single edge between them.  All other graphs have
      // two edges per vertex.
        ring_incident_edge_iterator::iterator_adaptor_(g.n() > 2 ? 2 : 1)
    , m_n(g.n())
    , m_u(u) {};

private:
    friend class boost::iterator_core_access;

    edge_descriptor dereference() const
    {
        static const int ring_offset[] = { 1, -1 };
        vertex_descriptor v;

        std::size_t p = *this->base_reference();
        if (m_u == 0 && p == 1)
            v = m_n - 1; // Vertex n-1 precedes vertex 0.
        else
            v = (m_u + ring_offset[p]) % m_n;
        return edge_descriptor(m_u, v);
    }

    std::size_t m_n; // Size of the graph
    vertex_descriptor m_u; // Vertex whose out edges are iterated
};

// IncidenceGraph valid expressions
vertex_descriptor source(edge_descriptor e, const ring_graph&)
{
    // The first vertex in the edge pair is the source.
    return e.first;
}

vertex_descriptor target(edge_descriptor e, const ring_graph&)
{
    // The second vertex in the edge pair is the target.
    return e.second;
}

std::pair< out_edge_iterator, out_edge_iterator > out_edges(
    vertex_descriptor u, const ring_graph& g)
{
    return std::pair< out_edge_iterator, out_edge_iterator >(
        out_edge_iterator(g, u, iterator_start()),
        out_edge_iterator(g, u, iterator_end()));
}

degree_size_type out_degree(vertex_descriptor, const ring_graph&)
{
    // All vertices in a ring graph have two neighbors.
    return 2;
}

// BidirectionalGraph valid expressions
std::pair< in_edge_iterator, in_edge_iterator > in_edges(
    vertex_descriptor u, const ring_graph& g)
{
    // The in-edges and out-edges are the same in an undirected graph.
    return out_edges(u, g);
}

degree_size_type in_degree(vertex_descriptor u, const ring_graph& g)
{
    // The in-degree and out-degree are both equal to the number of incident
    // edges in an undirected graph.
    return out_degree(u, g);
}

degree_size_type degree(vertex_descriptor u, const ring_graph& g)
{
    // The in-degree and out-degree are both equal to the number of incident
    // edges in an undirected graph.
    return out_degree(u, g);
}

/*
Iterator over vertices adjacent to a given vertex.

This iterates over the target vertices of all the incident edges.
*/
class ring_adjacency_iterator
: public boost::adjacency_iterator_generator< ring_graph, vertex_descriptor,
      out_edge_iterator >::type
{
    // The parent class is an iterator_adpator that turns an iterator over
    // out edges into an iterator over adjacent vertices.
    typedef boost::adjacency_iterator_generator< ring_graph, vertex_descriptor,
        out_edge_iterator >::type parent_class;

public:
    ring_adjacency_iterator() {};
    ring_adjacency_iterator(
        vertex_descriptor u, const ring_graph& g, iterator_start)
    : parent_class(out_edge_iterator(g, u, iterator_start()), &g) {};
    ring_adjacency_iterator(
        vertex_descriptor u, const ring_graph& g, iterator_end)
    : parent_class(out_edge_iterator(g, u, iterator_end()), &g) {};
};

// AdjacencyGraph valid expressions
std::pair< adjacency_iterator, adjacency_iterator > adjacent_vertices(
    vertex_descriptor u, const ring_graph& g)
{
    return std::pair< adjacency_iterator, adjacency_iterator >(
        adjacency_iterator(u, g, iterator_start()),
        adjacency_iterator(u, g, iterator_end()));
}

// VertexListGraph valid expressions
vertices_size_type num_vertices(const ring_graph& g) { return g.n(); };

std::pair< vertex_iterator, vertex_iterator > vertices(const ring_graph& g)
{
    return std::pair< vertex_iterator, vertex_iterator >(
        vertex_iterator(0), // The first iterator position
        vertex_iterator(num_vertices(g))); // The last iterator position
}

/*
Iterator over edges in a ring graph.

This object iterates over all the vertices in the graph, then for each
vertex returns its first outgoing edge.

It is implemented with the boost::iterator_adaptor class, because it is
essentially a vertex_iterator with a customized deference operation.
*/
class ring_edge_iterator
: public boost::iterator_adaptor< ring_edge_iterator, vertex_iterator,
      edge_descriptor, boost::use_default, edge_descriptor >
{
public:
    ring_edge_iterator()
    : ring_edge_iterator::iterator_adaptor_(0), m_g(NULL) {};
    explicit ring_edge_iterator(const ring_graph& g, iterator_start)
    : ring_edge_iterator::iterator_adaptor_(vertices(g).first), m_g(&g) {};
    explicit ring_edge_iterator(const ring_graph& g, iterator_end)
    : ring_edge_iterator::iterator_adaptor_(
        // Size 2 graphs have a single edge connecting the two vertices.
        g.n() == 2 ? ++(vertices(g).first) : vertices(g).second)
    , m_g(&g) {};

private:
    friend class boost::iterator_core_access;

    edge_descriptor dereference() const
    {
        // The first element in the incident edge list of the current vertex.
        return *(out_edges(*this->base_reference(), *m_g).first);
    }

    // The graph being iterated over
    const ring_graph* m_g;
};

// EdgeListGraph valid expressions
std::pair< edge_iterator, edge_iterator > edges(const ring_graph& g)
{
    return std::pair< edge_iterator, edge_iterator >(
        ring_edge_iterator(g, iterator_start()),
        ring_edge_iterator(g, iterator_end()));
}

edges_size_type num_edges(const ring_graph& g)
{
    // There are as many edges as there are vertices, except for size 2
    // graphs, which have a single edge connecting the two vertices.
    return g.n() == 2 ? 1 : g.n();
}

// AdjacencyMatrix valid expressions
std::pair< edge_descriptor, bool > edge(
    vertex_descriptor u, vertex_descriptor v, const ring_graph& g)
{
    if ((u == v + 1 || v == u + 1) && u > 0 && u < num_vertices(g) && v > 0
        && v < num_vertices(g))
        return std::pair< edge_descriptor, bool >(edge_descriptor(u, v), true);
    else
        return std::pair< edge_descriptor, bool >(edge_descriptor(), false);
}

/*
Map from edges to weight values
*/
struct edge_weight_map
{
    typedef double value_type;
    typedef value_type reference;
    typedef edge_descriptor key_type;
    typedef boost::readable_property_map_tag category;

    // Edges have a weight equal to the average of their endpoint indexes.
    reference operator[](key_type e) const
    {
        return (e.first + e.second) / 2.0;
    }
};

// Use these propety_map and property_traits parameterizations to refer to
// the associated property map types.
typedef boost::property_map< ring_graph, boost::edge_weight_t >::const_type
    const_edge_weight_map;
typedef boost::property_traits< const_edge_weight_map >::reference
    edge_weight_map_value_type;
typedef boost::property_traits< const_edge_weight_map >::key_type
    edge_weight_map_key;

// PropertyMap valid expressions
edge_weight_map_value_type get(
    const_edge_weight_map pmap, edge_weight_map_key e)
{
    return pmap[e];
}

// ReadablePropertyGraph valid expressions
const_edge_weight_map get(boost::edge_weight_t, const ring_graph&)
{
    return const_edge_weight_map();
}

edge_weight_map_value_type get(
    boost::edge_weight_t tag, const ring_graph& g, edge_weight_map_key e)
{
    return get(tag, g)[e];
}

// This expression is not part of a graph concept, but is used to return the
// default vertex index map used by the Dijkstra search algorithm.
boost::identity_property_map get(boost::vertex_index_t, const ring_graph&)
{
    // The vertex descriptors are already unsigned integer indices, so just
    // return an identity map.
    return boost::identity_property_map();
}

// Print edges as (x, y)
std::ostream& operator<<(std::ostream& output, const edge_descriptor& e)
{
    return output << "(" << e.first << ", " << e.second << ")";
}

int main(int argc, char const* argv[])
{
    using namespace boost;
    // Check the concepts that graph models.  This is included to demonstrate
    // how concept checking works, but is not required for a working program
    // since Boost algorithms do their own concept checking.
    BOOST_CONCEPT_ASSERT((BidirectionalGraphConcept< ring_graph >));
    BOOST_CONCEPT_ASSERT((AdjacencyGraphConcept< ring_graph >));
    BOOST_CONCEPT_ASSERT((VertexListGraphConcept< ring_graph >));
    BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< ring_graph >));
    BOOST_CONCEPT_ASSERT((AdjacencyMatrixConcept< ring_graph >));
    BOOST_CONCEPT_ASSERT(
        (ReadablePropertyMapConcept< const_edge_weight_map, edge_descriptor >));
    BOOST_CONCEPT_ASSERT((ReadablePropertyGraphConcept< ring_graph,
        edge_descriptor, edge_weight_t >));

    // Specify the size of the graph on the command line, or use a default size
    // of 5.
    std::size_t n = argc == 2 ? boost::lexical_cast< std::size_t >(argv[1]) : 5;

    // Create a small ring graph.
    ring_graph g(n);

    // Print the outgoing edges of all the vertices.  For n=5 this will print:
    //
    // Vertices, outgoing edges, and adjacent vertices
    // Vertex 0: (0, 1)  (0, 4)   Adjacent vertices 1 4
    // Vertex 1: (1, 2)  (1, 0)   Adjacent vertices 2 0
    // Vertex 2: (2, 3)  (2, 1)   Adjacent vertices 3 1
    // Vertex 3: (3, 4)  (3, 2)   Adjacent vertices 4 2
    // Vertex 4: (4, 0)  (4, 3)   Adjacent vertices 0 3
    // 5 vertices
    std::cout << "Vertices, outgoing edges, and adjacent vertices" << std::endl;
    vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; vi++)
    {
        vertex_descriptor u = *vi;
        std::cout << "Vertex " << u << ": ";
        // Adjacenct edges
        out_edge_iterator ei, ei_end;
        for (boost::tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ei++)
            std::cout << *ei << "  ";
        std::cout << " Adjacent vertices ";
        // Adjacent vertices
        // Here we want our adjacency_iterator and not
        // boost::adjacency_iterator.
        ::adjacency_iterator ai, ai_end;
        for (boost::tie(ai, ai_end) = adjacent_vertices(u, g); ai != ai_end;
             ai++)
        {
            std::cout << *ai << " ";
        }
        std::cout << std::endl;
    }
    std::cout << num_vertices(g) << " vertices" << std::endl << std::endl;

    // Print all the edges in the graph along with their weights.  For n=5 this
    // will print:
    //
    // Edges and weights
    // (0, 1) weight 0.5
    // (1, 2) weight 1.5
    // (2, 3) weight 2.5
    // (3, 4) weight 3.5
    // (4, 0) weight 2
    // 5 edges
    std::cout << "Edges and weights" << std::endl;
    edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ei++)
    {
        edge_descriptor e = *ei;
        std::cout << e << " weight " << get(edge_weight, g, e) << std::endl;
    }
    std::cout << num_edges(g) << " edges" << std::endl;

    if (n > 0)
    {
        std::cout << std::endl;
        // Do a Dijkstra search from vertex 0.  For n=5 this will print:
        //
        // Dijkstra search from vertex 0
        // Vertex 0: parent 0, distance 0
        // Vertex 1: parent 0, distance 0.5
        // Vertex 2: parent 1, distance 2
        // Vertex 3: parent 2, distance 4.5
        // Vertex 4: parent 0, distance 2
        vertex_descriptor source = 0;
        std::vector< vertex_descriptor > pred(num_vertices(g));
        std::vector< edge_weight_map_value_type > dist(num_vertices(g));
        iterator_property_map< std::vector< vertex_descriptor >::iterator,
            property_map< ring_graph, vertex_index_t >::const_type >
            pred_pm(pred.begin(), get(vertex_index, g));
        iterator_property_map<
            std::vector< edge_weight_map_value_type >::iterator,
            property_map< ring_graph, vertex_index_t >::const_type >
            dist_pm(dist.begin(), get(vertex_index, g));

        dijkstra_shortest_paths(
            g, source, predecessor_map(pred_pm).distance_map(dist_pm));

        std::cout << "Dijkstra search from vertex " << source << std::endl;
        for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        {
            vertex_descriptor u = *vi;
            std::cout << "Vertex " << u << ": "
                      << "parent " << pred[*vi] << ", "
                      << "distance " << dist[u] << std::endl;
        }
    }

    return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 1, 2> test_long_1_2(const Eigen::Matrix<boost::int64_t, 1, 2> & M)
{
	return M;
}
void export_long_1_2()
{
	boost::python::def("test_long_1_2",test_long_1_2);
}


/*
 * OrientationBox.hpp
 *
 *  Created on: 01.11.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_STATEBOXES_ORIENTATIONSPEEDBOX_HPP_
#define ESTIMATORS_STATEBOXES_ORIENTATIONSPEEDBOX_HPP_

#include "StateBox.hpp"
#include <eigen3/Eigen/Core>
#include <Eigen_Utils.hpp>
#include "OrientationBox.hpp"
namespace zavi
::estimator::state_boxes {
	/**
	 * Box to contain orientation and angular velocity
	 */
	class OrientationSpeedBox : public StateBox<12,6,3> {
		template<typename T>
		using ROT_MATRIX= Eigen::Matrix<T,3,3>;

	public:
		INNER_T<double> std;
		OrientationSpeedBox(std::shared_ptr<plugin::SensorPlugin> sensor):StateBox<12,6,3>(sensor) {
		}
		virtual ~OrientationSpeedBox() {};

		template<typename T>
		inline static OUTER_T<T> boxPlus(const OUTER_T<T> &state,const INNER_T<T> &delta) {
			assert_inputs(state,delta);
			OUTER_T<T> result;
			result <<OrientationBox::boxPlus<T>(state.template block<9,1>(0,0),delta.template block<3,1>(0,0)),(state.template block<3,1>(9,0)+delta.template block<3,1>(3,0));
			return result;
		}
		template<typename T>
		inline static INNER_T<T> boxPlusInnerSpace(const INNER_T<T> &delta1,const INNER_T<T> &delta2) {
			assert_inputs(delta1,delta2);
			INNER_T<T> result=INNER_T<T>::Zero();
			result.template block<3,1>(0,0)=OrientationBox::boxPlusInnerSpace<T>(delta1.template block<3,1>(0,0),delta2.template block<3,1>(0,0));
			result.template block<3,1>(3,0)=delta1.template block<3,1>(3,0)+delta2.template block<3,1>(3,0);
			return result;
		}

		template<typename T>
		inline static INNER_T<T> boxMinus(const OUTER_T<T> &a,const OUTER_T<T> &b) {
			assert_inputs(a,b);
			INNER_T<T>result;
			result <<OrientationBox::boxMinus<T>(a.template block<9,1>(0,0),b.template block<9,1>(0,0)),b.template block<3,1>(9,0)-a.template block<3,1>(9,0);
			return result;
		}
		template<typename T>
		inline static OUTER_T<T> stateTransition(const OUTER_T<T> & state,double time_diff ) {
			assert_inputs(state,time_diff);
			OUTER_T<T> result;
			result << OrientationBox::boxPlus<T>(state.template block<9,1>(0,0),state.template block<3,1>(9,0)*time_diff),state.template block<3,1>(9,0);
			return result;
		}
		inline INNER_T<double> getSTD(double time_diff) {
			return std*time_diff;
		}

		virtual OUTER_T<double> normalise(const OUTER_T<double> &state) const {
			assert_inputs(state);
			OUTER_T<double> result;
			result <<OrientationBox::static_normalise(state.block<9,1>(0,0)),state.block<3,1>(9,0);
			return result;
		}
	};
}

#endif /* ESTIMATORS_STATEBOXES_ORIENTATIONSPEEDBOX_HPP_ */


#include <gsa/search_parameters.hh>

#include <boost/test/unit_test.hpp>
#include <random>

BOOST_AUTO_TEST_SUITE( ts_gsa_searchparam )

BOOST_AUTO_TEST_CASE( basic )  {
  std::size_t               steps   = 100;
  std::size_t               popsize =  10;
  common::index_type        dim     =   3;
  double                    alpha   =  20.0;
  double                    G0      = 100.0;
  common::limits            poslimits(-10.0, 10.0);
  common::exponential_slide Gslide(steps, alpha, G0);
  common::linear_slide      Kslide(steps, 1, 0);

  gsa::search_parameters param(steps,
                               popsize,
                               dim,
                               poslimits,
                               Gslide,
                               Kslide);

  BOOST_CHECK_EQUAL(steps          , param.max_steps());
  BOOST_CHECK_EQUAL(popsize        , param.population_size());
  BOOST_CHECK_EQUAL(dim            , param.dimensions());
  BOOST_CHECK_CLOSE(poslimits.min(), param.limits_position().min(), 1e-10);
  BOOST_CHECK_CLOSE(poslimits.max(), param.limits_position().max(), 1e-10);

  {
    std::mt19937_64 rng1;

    auto rpos = param.random_position(rng1);

    BOOST_CHECK_EQUAL(dim, rpos.size());

    for(int i = 0; i < rpos.size(); ++i) {
      BOOST_CHECK(poslimits.contains(rpos(i)));
    }

    rpos(0) = poslimits.min() - 1.0;

    BOOST_CHECK(param.out_of_bounds(rpos));

    rpos = param.fixed_position(rpos, rng1);

    BOOST_CHECK(poslimits.contains(rpos(0)));
  }

  BOOST_CHECK_CLOSE(Gslide(10)          , param.G(10), 1e-10);
  BOOST_CHECK_CLOSE(Kslide(10) * popsize, param.K(10), 1e-10);
}

BOOST_AUTO_TEST_SUITE_END()


#include "Lenstra.h"
#include <iostream>
#include <boost/mpi.hpp>
#include <omp.h>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

namespace mpi = boost::mpi;

NTL::ZZ generated_counter{0};
int end = 0;

NTL::ZZ Lenstra::factorize() const {
    /// Sequential algorithm for computing factorization
    auto divisor = NTL::conv<NTL::ZZ>(1);
    auto sqrt_n = NTL::SqrRoot(*_options->composite_number);

    NTL::ZZ bound = sqrt_n;
    if (*_options->bound > 2)
        bound = (*_options->bound < sqrt_n ? *_options->bound : sqrt_n);
    auto divided = bound / 1000000;
    NTL::ZZ test_after{(divided < 100 ? (NTL::ZZ)100 : divided)};
    NTL::ZZ counter(1);
    while (true) {
        auto point = _model->generate_elliptic_curve();
        for (NTL::ZZ k{2}; k < bound; k++, counter++) {
            point = _model->mul_points(k, point);
            if (counter % test_after == 0) {
                divisor = _model->try_get_factor(point);
                if (divisor > 1 && divisor < *_options->composite_number) {
                    return divisor;
                }
                counter = 0;
            }
            if (_model->is_infinity_point(point)) {
                break;
            }
        }
        divisor = _model->try_get_factor(point);
        if (divisor > 1 && divisor < *_options->composite_number) {
            return divisor;
        }
    }
}

NTL::ZZ Lenstra::factorize_parallel(int argc, char **argv) {
    /// Master-slave algorithm
    /// First we initialize communication environment via mpi::environment and mpi::communicator
    mpi::environment env(argc, argv, mpi::threading::multiple);
    mpi::communicator world;

    /// Result of factorizing
    NTL::ZZ result{0};

    if (!world.rank()) {
        /// master part
        _generate_ecc(env, world);
        _point = _model->generate_elliptic_curve();
    } else {
        /// slave part
        _get_ecc(env, world);
    }

    mpi::timer timer;

    result = _factorize_parallel(env, world);

    double elapsed = timer.elapsed();

    if (result != 0) {
        std::cout << "proc " << world.rank() << ": factor = " << result << "\n";
        std::cout << "proc " << world.rank() << ": time = " << elapsed << " s\n";
        _stop_all(env, world);
        mpi::environment::abort(0);
    }

    world.barrier();

    if (world.rank() == 0) {
        std::cout << "generated ecc = " << generated_counter << "\n";
    }

    return result;
}

void Lenstra::_generate_ecc(const mpi::environment &environment, const mpi::communicator &communicator) {
    /// Generates elliptic curve for all working processes
    for (int i = 1; i < communicator.size(); i++) {
        _par_generate_ecc(environment, communicator);
    }
}

NTL::ZZ Lenstra::_factorize_parallel(const mpi::environment &environment, const mpi::communicator &communicator) {
    /// starts computing algorithm for process
    std::vector<ProjectivePoint> points;
    NTL::ZZ result{0};
    NTL::ZZ k{1};
    NTL::ZZ counter{1};
    auto divisor = NTL::conv<NTL::ZZ>(1);
    auto sqrt_n = NTL::SqrRoot(*_options->composite_number);

    NTL::ZZ bound = sqrt_n;
    if (*_options->bound > 2)
        bound = (*_options->bound < sqrt_n ? *_options->bound : sqrt_n);
    auto divided = bound / 1000000;
    NTL::ZZ test_after{(divided < 100 ? (NTL::ZZ)100 : divided)};

    while(!end) {
        k = 2;
        points.clear();
        #pragma omp parallel shared(points, result, end, k, environment, communicator, sqrt_n, test_after)
        {
            NTL::ZZ tmp;
            auto point = _point;
            bool tested = false;
            while (k < bound && !end) {
                #pragma omp critical
                {
                    tmp = k;
                    k++;
                    counter++;
                }

                point = _model->mul_points(tmp, point);
                if (_model->is_infinity_point(point)) {
                    break;
                }

                if (!tested && counter >= test_after) {
                    tested = true;
                    auto factor = _model->try_get_factor(point);
                    if (factor > 1 && factor < *_options->composite_number) {
                        result = factor;
                        #pragma omp critical (ending)
                        {
                            end = 1;
                        }
                        break;
                    }
                }

                #pragma omp critical (ending)
                {
                    if (!end && communicator.rank() == 0) {
                        end = !_par_generate_ecc(environment, communicator);
                    } else if (!end && communicator.rank() != 0) {
                        end = _check_end(environment, communicator) || end;
                    }
                }
            }

            #pragma omp critical (add_point)
            {
                points.push_back(point);
            }

            #pragma omp barrier

            if (counter % test_after == 0) {
                auto factor = _model->try_get_factor(point);
                if (factor > 1 && factor < *_options->composite_number) {
                    result = factor;
                    end = 1;
                }
            }

            #pragma omp single
            {
                #pragma omp critical (ending)
                {
                    if (communicator.rank() != 0)
                        end = _check_end(environment, communicator);
                    else
                        end = !_par_generate_ecc(environment, communicator);
                }
                for (const auto &vec_point : points) {
                    _point = _model->add_points(_point, vec_point);
                }

                if (!_model->is_infinity_point(_point)) {
                    auto factor = _model->try_get_factor(_point);
                    if (factor > 1 && factor < *_options->composite_number) {
                        result = factor;
                        end = 1;
                    }
                }
                if (!end) {
                    if (communicator.rank() == 0) {
                        generated_counter++;
                        _point = _model->generate_elliptic_curve();
                        end = !_par_generate_ecc(environment, communicator);
                    } else {
                        end = !_get_ecc(environment, communicator);
                    }
                }
            }
        }
    }
    return result;
}

void Lenstra::_generate_edwards(const boost::mpi::environment &environment, const boost::mpi::communicator &communicator,
                           int source) {
    /// Generates edwards curve  for working process
    auto model = dynamic_cast<EdwardsModel*>(_model.get());
    std::stringstream buffer;
    boost::archive::text_oarchive ar(buffer);
    auto point = model->generate_elliptic_curve();
    auto curve = model->get_elliptic_curve();

    ar << point << curve;
    generated_counter++;
    communicator.send(source, TAGS::NEW_ECC, buffer.str());
}

void Lenstra::_generate_weierstrass(const boost::mpi::environment &environment, const boost::mpi::communicator &communicator,
                               int source) {
    /// Generates weierstrass curve for working process
    auto model = dynamic_cast<WeierstrassModel*>(_model.get());
    std::stringstream buffer;
    boost::archive::text_oarchive ar(buffer);
    auto point = model->generate_elliptic_curve();
    auto curve = model->get_elliptic_curve();

    ar << point << curve;
    generated_counter++;
    communicator.send(source, TAGS::NEW_ECC, buffer.str());
}

bool Lenstra::_get_ecc(const mpi::environment &environment, const mpi::communicator &communicator) {
    /// Gets new elliptic curve from master process
    communicator.send(0, TAGS::NEW_ECC);
    auto status = communicator.probe();
    if (status.tag() == TAGS::NEW_ECC) {
        std::string msg;
        communicator.recv(status.source(), status.tag(), msg);
        std::istringstream buffer(msg);
        boost::archive::text_iarchive ar(buffer);

        ar >> _point;
        _point.z = 1;
        if (_options->weierstrass) {
            WeierstrassModel::EllipticCurve ecc;
            ar >> ecc;
            dynamic_cast<WeierstrassModel*>(_model.get())->set_elliptic_curve(ecc);

        } else {
            EdwardsModel::EllipticCurve ecc;
            ar >> ecc;
            dynamic_cast<EdwardsModel*>(_model.get())->set_elliptic_curve(ecc);
        }
        return true;
    }
    return false;
}

bool Lenstra::_check_end(const mpi::environment &environment, const mpi::communicator &communicator) {
    /// Check if we have pending message
    auto status = communicator.iprobe();
    if (status.has_value()) {
        communicator.recv(status.value().source(), status.value().tag());
    }
    return status.has_value() && status.value().tag() == TAGS::STOP;
}

void Lenstra::_stop_all(const mpi::environment &, const boost::mpi::communicator& communicator) {
    /// Send stop tag to all processses
    std::vector<mpi::request> requests;
    for (int i = 0; i < communicator.size(); i++) {
        if (i != communicator.rank()) {
            requests.emplace_back(communicator.isend(i, TAGS::STOP));
        }
    }
    mpi::wait_all(requests.begin(), requests.end());
}

bool Lenstra::_par_generate_ecc(const mpi::environment &environment, const mpi::communicator &communicator) {
    /// Master part with generating of new elliptic curve
    auto status = communicator.iprobe();
    if (status.has_value() && status.value().tag() == TAGS::STOP) {
        communicator.recv(status.value().source(), status.value().tag());
        return false;
    } else if (status.has_value() && status.value().tag() == TAGS::NEW_ECC) {
        communicator.recv(status.value().source(), status.value().tag());
        if (_options->weierstrass) {
            _generate_weierstrass(environment, communicator, status.value().source());
        } else {
            _generate_edwards(environment, communicator, status.value().source());
        }
    }
    return true;
}


/**
 * @author Irene Sanz
 */

/**
 * @addtogroup DFNs
 * @{
 */

#include "ThresholdForce.hpp"

#include <Eigen/Dense>

#include "Errors/Assert.hpp"
#include "Types/CPP/PointCloud.hpp"
#include "Validators/Number.hpp"

namespace CDFF
{
namespace DFN
{
namespace ForceMeshGenerator
{

ThresholdForce::ThresholdForce()
{
    parameters = DEFAULT_PARAMETERS;

    parametersHelper.AddParameter<double>("GeneralParameters", "Threshold", parameters.threshold, DEFAULT_PARAMETERS.threshold);
	configurationFilePath = "";
}

const ThresholdForce::ThresholdForceOptionsSet ThresholdForce::DEFAULT_PARAMETERS =
{
    /*.threshold =*/ 0.5
};

void ThresholdForce::configure()
{
	parametersHelper.ReadFile(configurationFilePath);
	ValidateParameters();
}

void ThresholdForce::process()
{
	Eigen::Vector3d contact_force(
		inArmEndEffectorWrench.force.arr[0],
		inArmEndEffectorWrench.force.arr[1],
		inArmEndEffectorWrench.force.arr[2]
		);

	if( contact_force.norm() > parameters.threshold )
	{
		Eigen::Vector3d effector_position(inArmEndEffectorPose.pos.arr[0], inArmEndEffectorPose.pos.arr[1], inArmEndEffectorPose.pos.arr[2]);
		Eigen::Quaterniond quaternion2(inArmBasePose.orient.arr[0], inArmBasePose.orient.arr[1], inArmBasePose.orient.arr[2], inArmBasePose.orient.arr[3]);
		quaternion2.normalize();
		Eigen::Matrix3d rotation = quaternion2.toRotationMatrix();
		Eigen::Vector3d position(inArmBasePose.pos.arr[0], inArmBasePose.pos.arr[1], inArmBasePose.pos.arr[2]);

		effector_position += position;
		effector_position = rotation * effector_position;

		PointCloudWrapper::AddPoint(outPointCloud, effector_position.x(), effector_position.y(), effector_position.z());
	}
}

void ThresholdForce::ValidateParameters()
{
    Validators::Number::IsPositive(parameters.threshold);
}

}
}
}

/** @} */


#include <math.h>
#include <uWS/uWS.h>
#include <iostream>
#include <string>
#include "json.hpp"
#include "PID.h"
#include <fstream>
#include <boost/range/numeric.hpp>

// for convenience
using nlohmann::json;
using std::string;

// using namespace std;

// For converting back and forth between radians and degrees.
constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
// else the empty string "" will be returned.
string hasData(string s) {
  auto found_null = s.find("null");
  auto b1 = s.find_first_of("[");
  auto b2 = s.find_last_of("]");
  if (found_null != string::npos) {
    return "";
  }
  else if (b1 != string::npos && b2 != string::npos) {
    return s.substr(b1, b2 - b1 + 1);
  }
  return "";
}

int main() {
  uWS::Hub h;

  PID pid;
  /**
   * TODO: Initialize the pid variable.o
   */

  double TARGET_SPEED = 40;
  pid.tune = false;

  if (TARGET_SPEED == 20) {
    pid.Init(0.795131, 0.000209, 3.089950);
  } else
  if (TARGET_SPEED == 30) {
    pid.Init(0.187722,	0.001840,	1.005840);
  } else
  if (TARGET_SPEED == 40) {
    pid.Init(0.096164, 0.005433, 0.938557);
  } else
  if (TARGET_SPEED == 60) {
    pid.Init(0.032581, 0.006453,	0.625201);
  } else {
      TARGET_SPEED = 40;
       pid.Init(0.096164, 0.005433, 0.938557);
    }
  // true if tuning pid
  std::cout << "Initial PID params: " << pid.Kp << "," << pid.Ki << "," << pid.Kd << "\n";
  pid.tune_iter = -1;
  pid.twiddle_iter_p = 0;
  pid.tune_tolerance = 0.1;
  int step = 0;
  double throttle = 0.75;
  pid.target_speed = TARGET_SPEED;
  std::ofstream logfile;
  std::ofstream summary;

  if (pid.tune == true) {
    logfile.open("../pid_log.csv", std::ios::trunc); // overwrite existing file
    // write header
    logfile << "iteration" << "," << "step" << ","
            << "cte" << "," << "speed" << ","
            << "Kp" << "," << "Ki" << "," << "Kd" << "\n";

    summary.open("../pid_summary.csv", std::ios::trunc); // overwrite existing file
    // write header
    summary << "iteration" << "," << "cum_err" << "," << "best_err" << ","
            << "sum(dp)" << ","<< "target speed" << ","
            << "Kp" << "," << "Ki" << "," << "Kd" << "\n";
  }

  h.onMessage([&pid, &step, &throttle, &logfile, &summary](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length,
                     uWS::OpCode opCode) {
    // "42" at the start of the message means there's a websocket message event.
    // The 4 signifies a websocket message
    // The 2 signifies a websocket event
    if (length && length > 2 && data[0] == '4' && data[1] == '2') {
      auto s = hasData(string(data).substr(0, length));

      if (s != "") {
        auto j = json::parse(s);

        string event = j[0].get<string>();

        if (event == "telemetry") {
          // j[1] is the data JSON object
          double cte = std::stod(j[1]["cte"].get<string>());
          double speed = std::stod(j[1]["speed"].get<string>());
          double angle = std::stod(j[1]["steering_angle"].get<string>());
          double steer_value;
          const int TUNE_STEPS = 1000;
          const int SETTLE_STEPS = 100;

          pid.UpdateError(cte);
          steer_value = pid.SteeringAngle(1);

          // Log and twiddle below, if tuning:
          if (pid.tune == true) {
            pid.log_tune(logfile, cte, speed, step);
            if (step > SETTLE_STEPS) pid.cum_err += std::pow(cte, 2);

            if (step > TUNE_STEPS) {
              std::cout << "================================" << std::endl;
              std::cout << "PID params: " << pid.Kp << "," << pid.Ki << "," << pid.Kd << "\n";
              step = 0;
              int tpi = pid.twiddle_iter_p;
              pid.twiddle(summary);
              std::cout << "iteration: " << pid.tune_iter << " | cumulative error: " << pid.cum_err << " | best error: " << pid.best_err << std::endl;
              std::cout << "sum(dp): " << boost::accumulate(pid.twiddle_dp, 0.) << " | par i: " << tpi << std::endl;
              pid.cum_err = 0;
              std::string msg("42[\"reset\", {}]");
              ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
              if (boost::accumulate(pid.twiddle_dp, 0.) <= pid.tune_tolerance) pid.tune = false;

              logfile.flush();
              summary.flush();
            }
            step++;
          }

          // hold speed const for steer pid tune
          if ( speed > pid.target_speed) {
            throttle = 0.1;
          } else {
            throttle = 1; //0.9+ for 40+ mph
          }

          // =============================================================================
          // DEBUG
          // std::cout << "CTE: " << cte << " Steering Value: " << steer_value
          //           << std::endl;

          json msgJson;
          msgJson["steering_angle"] = steer_value;
          msgJson["throttle"] = throttle;
          auto msg = "42[\"steer\"," + msgJson.dump() + "]";
          // std::cout << msg << std::endl;
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
        }  // end "telemetry" if
      } else {
        // Manual driving
        string msg = "42[\"manual\",{}]";
        ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
      }
    }  // end websocket message if
  }); // end h.onMessage

  h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
    std::cout << "Connected!!!" << std::endl;
  });

  h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
                         char *message, size_t length) {
    ws.close();
    std::cout << "Disconnected" << std::endl;
  });

  int port = 4567;
  if (h.listen(port)) {
    std::cout << "Listening to port " << port << std::endl;
  } else {
    std::cerr << "Failed to listen to port" << std::endl;
    return -1;
  }

  h.run();
if (pid.tune == true) {
  logfile.close();
  summary.close();
}
}


/**
 * This file cannot be built without CombBLAS and Elemental both being present.
 * The accompanying CMakeLists.txt checks for both libraries to be available.
 */
#include <iostream>
#include <functional>
#include <cstring>
#include <vector>
#include <utility>
#include <ext/hash_map>

#include <elemental.hpp>
#include <CombBLAS.h>
#include <boost/mpi.hpp>
#include <skylark.hpp>

#include "utilities.hpp"
#include "parser.hpp"

/*******************************************/
namespace bmpi =  boost::mpi;
namespace skys =  skylark::sketch;
namespace skyb =  skylark::base;
namespace skyalg = skylark::algorithms;
namespace skynla = skylark::nla;
namespace skyutil = skylark::utility;
/*******************************************/

/* These were declared as extern in utilities.hpp --- defining it here */
int int_params [NUM_INT_PARAMETERS];
char* chr_params[NUM_CHR_PARAMETERS];

/** Typedef DistMatrix and Matrix */
typedef std::vector<int> IntContainer;
typedef std::vector<double> DblContainer;
typedef elem::Matrix<double> DenseMatrixType;
typedef elem::DistMatrix<double> DenseDistMatrixType;

typedef SpDCCols<int, double> SparseMatrixType;
typedef SpParMat<int, double, SparseMatrixType> SparseDistMatrixType;
typedef FullyDistVec<int, double> SparseVectorType;
typedef FullyDistMultiVec<int, double> SparseMultiVectorType;

typedef skyutil::uniform_matrix_t<DenseDistMatrixType> uni_dense_dist_mat_t;
typedef skyutil::uniform_matrix_t<SparseDistMatrixType> uni_sparse_dist_mat_t;
typedef skyutil::uniform_matrix_t<SparseMultiVectorType>
                                                  uni_sparse_dist_multi_vec_t;

typedef skyutil::empty_matrix_t<DenseDistMatrixType> empty_dense_dist_mat_t;
typedef skyutil::empty_matrix_t<SparseDistMatrixType> empty_sparse_dist_mat_t;
typedef skyutil::empty_matrix_t<SparseMultiVectorType>
                                                  empty_sparse_dist_multi_vec_t;

typedef skyutil::print_t<SparseDistMatrixType> sparse_dist_mat_printer_t;
typedef skyutil::print_t<SparseMultiVectorType> sparse_dist_multi_vec_printer_t;
typedef skyutil::print_t<DenseDistMatrixType> dense_dist_mat_printer_t;

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

  /* Initilize MPI manually to make sure that threading is supported */
  int provided;
  MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);

  /* Create a global communicator */
  bmpi::communicator world;

  /* MPI sends argc and argv everywhere --- parse everywhere */
  parse_parameters (argc,argv);

  /* Initialize skylark */
  skyb::context_t context (int_params[RAND_SEED_INDEX]);

  /** Only left sketch is supported for now */
  if (SKETCH_LEFT != int_params[SKETCH_DIRECTION_INDEX]) {
    std::cout << "We don't support reading --- yet --" << std::endl;
    goto END;
  }

  /** Only randomization is supported for now */
  if (0==int_params[USE_RANDOM_INDEX]) {
    /** TODO: Read the entries! */
    std::cout << "We don't support reading --- yet --" << std::endl;
    goto END;
  }

  if (0==strcmp("elem",chr_params[MATRIX_TYPE_INDEX])) {
    /** Run the test for elemental matrices */

    /* Initialize elemental */
    elem::Initialize (argc, argv);
    MPI_Comm mpi_world(world);
    elem::Grid grid (mpi_world);

    DenseDistMatrixType A=uni_dense_dist_mat_t::generate
            (int_params[M_INDEX], int_params[N_INDEX], grid, context);
    DenseDistMatrixType B=uni_dense_dist_mat_t::generate
            (int_params[M_INDEX], int_params[N_RHS_INDEX], grid, context);
    DenseDistMatrixType X(int_params[N_INDEX], int_params[N_RHS_INDEX], grid);

    /** Depending on which sketch is requested, do the sketching */
    DenseMatrixType sketch_A(int_params[S_INDEX], int_params[N_INDEX]);
    DenseMatrixType sketch_B(int_params[S_INDEX], int_params[N_RHS_INDEX]);
    DenseMatrixType sketch_X(int_params[N_INDEX], int_params[N_RHS_INDEX]);

    /***********************************************************************/
    /* Hack to make sure that we can sketch matrices with MC,MR distributn */
    /***********************************************************************/
    typedef elem::DistMatrix<double, elem::VC, elem::STAR> CheatDistMatrixType;
    CheatDistMatrixType A_cheat(A), B_cheat(B);
    /***********************************************************************/
    if (0==strcmp("JLT", chr_params[TRANSFORM_INDEX]) ) {
      skys::JLT_t<CheatDistMatrixType, DenseMatrixType>
        JLT (int_params[M_INDEX], int_params[S_INDEX], context);
      JLT.apply (A_cheat, sketch_A, skys::columnwise_tag());
      JLT.apply (B_cheat, sketch_B, skys::columnwise_tag());
    } else if (0==strcmp("FJLT", chr_params[TRANSFORM_INDEX]) ) {
      skys::FJLT_t<CheatDistMatrixType, DenseMatrixType>
        FJLT (int_params[M_INDEX], int_params[S_INDEX], context);
        FJLT.apply (A_cheat, sketch_A, skys::columnwise_tag());
        FJLT.apply (B_cheat, sketch_B, skys::columnwise_tag());
    } else if (0==strcmp("CWT", chr_params[TRANSFORM_INDEX]) ) {
      skys::CWT_t<CheatDistMatrixType, DenseMatrixType>
        Sparse (int_params[M_INDEX], int_params[S_INDEX], context);
      Sparse.apply (A_cheat, sketch_A, skys::columnwise_tag());
      Sparse.apply (B_cheat, sketch_B, skys::columnwise_tag());
    } else {
      std::cout << "We only have JLT/FJLT/CWT sketching" << std::endl;
    }
    A_cheat.Empty();
    B_cheat.Empty();

    /** Set up the iterative solver parameters */
    skynla::iter_params_t params(1e-14,  /* tolerance */
                                 world.rank()==0, /* only root prints */
                                 100, /* number of iterations */
                                 int_params[DEBUG_INDEX]-1); /* debugging */

    /** Solve the exact problem */
    DblContainer exact_norms(int_params[N_RHS_INDEX], 0.0);
    skyalg::regression_problem_t<skyalg::l2_tag, DenseDistMatrixType>
            exact_problem(int_params[M_INDEX], int_params[N_INDEX], A);
    skyalg::exact_regressor_t
        <skyalg::l2_tag,
         DenseDistMatrixType,
         DenseDistMatrixType,
         skyalg::iterative_l2_solver_tag<skyalg::lsqr_tag> >
         exact_regr(exact_problem);
    exact_regr.solve(B, X, params);
    skynla::iter_solver_op_t<DenseDistMatrixType, DenseDistMatrixType>::
                      residual_norms(A, B, X, exact_norms);

    if (1<int_params[DEBUG_INDEX] &&
        (int_params[M_INDEX]*int_params[N_INDEX])<100 &&
        world.rank() == 0) {
      elem::Display(A, "A");
      elem::Display(B, "B");
      elem::Display(X, "X");
    }

    /** Solve the sketched problem---change to using the sketched regressor */
    DblContainer sketched_norms(int_params[N_RHS_INDEX], 0.0);
    skyalg::regression_problem_t<skyalg::l2_tag, DenseMatrixType>
        sketched_problem(int_params[S_INDEX], int_params[N_INDEX], sketch_A);
    skyalg::exact_regressor_t
        <skyalg::l2_tag,
         DenseMatrixType,
         DenseMatrixType,
         skyalg::iterative_l2_solver_tag<skyalg::lsqr_tag> >
         sketched_regr(sketched_problem);
    sketched_regr.solve(sketch_B, sketch_X, params);
    DenseDistMatrixType sketch_X_global(X);
    skynla::iter_solver_op_t<DenseDistMatrixType, DenseDistMatrixType>::
                    make_dist_from_local (sketch_X, sketch_X_global);
    skynla::iter_solver_op_t<DenseDistMatrixType, DenseDistMatrixType>::
                      residual_norms(A, B, sketch_X_global, sketched_norms);

    if (1<int_params[DEBUG_INDEX] &&
        (int_params[M_INDEX]*int_params[N_INDEX])<100 &&
        world.rank() == 0) {
      dense_dist_mat_printer_t::apply (A, "A", true, 2);
      dense_dist_mat_printer_t::apply (B, "B", true, 2);
      dense_dist_mat_printer_t::apply (X, "X", true, 2);
    }

    /** Check the quality of the solution */
    if (0==world.rank()) {
      for (int i=0; i<int_params[N_RHS_INDEX]; ++i)
        printf ("For RHS (%d), exact norm = %lf, sketched norm=%lf\n",
                              i, exact_norms[i], sketched_norms[i]);
    }

    /** Finalize Elemental */
    elem::Finalize();

  } else {
    /** Run the test for CombBLAS matrices */

    /* Create matrices A and B */
    const int NNZ = int_params[M_INDEX]*int_params[N_INDEX]*0.5;
    SparseDistMatrixType A=uni_sparse_dist_mat_t::generate
            (int_params[M_INDEX], int_params[N_INDEX], NNZ, context);
    SparseMultiVectorType B=uni_sparse_dist_multi_vec_t::generate
            (int_params[M_INDEX], int_params[N_RHS_INDEX], context);
    SparseMultiVectorType X (int_params[N_INDEX], int_params[N_RHS_INDEX]);

    /** Depending on which sketch is requested, do the sketching */
    SparseDistMatrixType sketch_A=empty_sparse_dist_mat_t::generate
            (int_params[S_INDEX], int_params[N_INDEX]);
    SparseMultiVectorType sketch_B=empty_sparse_dist_multi_vec_t::generate
            (int_params[S_INDEX], int_params[N_RHS_INDEX]);
    SparseMultiVectorType sketch_X=empty_sparse_dist_multi_vec_t::generate
            (int_params[N_INDEX], int_params[N_RHS_INDEX]);
    if (0==strcmp("CWT", chr_params[TRANSFORM_INDEX]) ) {
      skys::CWT_t<SparseDistMatrixType, SparseDistMatrixType>
        CWT_mat (int_params[M_INDEX], int_params[S_INDEX], context);
      CWT_mat.apply (A, sketch_A, skys::columnwise_tag());
      skys::CWT_t<SparseMultiVectorType, SparseMultiVectorType>
        CWT_vec (CWT_mat);
      CWT_vec.apply (B, sketch_B, skys::columnwise_tag());
    } else {
      std::cout << "We only have CWT for sparse sketching" << std::endl;
    }

    /** Set up the iterative solver parameters */
    skynla::iter_params_t params(1e-14,  /* tolerance */
                                 world.rank()==0, /* only root prints */
                                 100, /* number of iterations */
                                 int_params[DEBUG_INDEX]-1); /* debugging */

    /** Solve the exact problem */
    DblContainer exact_norms(int_params[N_RHS_INDEX], 0.0);
    skyalg::regression_problem_t<skyalg::l2_tag, SparseDistMatrixType>
            exact_problem(int_params[M_INDEX], int_params[N_INDEX], A);
    skyalg::exact_regressor_t
        <skyalg::l2_tag,
         SparseDistMatrixType,
         SparseMultiVectorType,
         skyalg::iterative_l2_solver_tag<skyalg::lsqr_tag> >
         exact_regr(exact_problem);
    exact_regr.solve(B, X, params);
    skynla::iter_solver_op_t<SparseDistMatrixType, SparseMultiVectorType>::
                      residual_norms(A, B, X, exact_norms);

    /** Solve the sketched problem */
    DblContainer sketched_norms(int_params[N_RHS_INDEX], 0.0);
    skyalg::regression_problem_t<skyalg::l2_tag, SparseDistMatrixType>
         sketched_problem(int_params[S_INDEX], int_params[N_INDEX], sketch_A);
    skyalg::exact_regressor_t
        <skyalg::l2_tag,
         SparseDistMatrixType,
         SparseMultiVectorType,
         skyalg::iterative_l2_solver_tag<skyalg::lsqr_tag> >
         sketched_regr(sketched_problem);
    sketched_regr.solve(sketch_B, sketch_X, params);
    skynla::iter_solver_op_t<SparseDistMatrixType, SparseMultiVectorType>::
              residual_norms(A, B, sketch_X, sketched_norms);

    if (1<int_params[DEBUG_INDEX] &&
        (int_params[M_INDEX]*int_params[N_INDEX])<100 &&
        world.rank() == 0) {
      sparse_dist_mat_printer_t::apply (A, "A", true, 2);
      sparse_dist_multi_vec_printer_t::apply (B, "B", true, 2);
      sparse_dist_multi_vec_printer_t::apply (X, "X", true, 2);
    }

    /** Check the quality of the solution */
    if (0==world.rank()) {
      for (int i=0; i<int_params[N_RHS_INDEX]; ++i)
        printf ("For RHS (%d), exact norm = %lf, sketched norm=%lf\n",
                              i, exact_norms[i], sketched_norms[i]);
    }
  }

END:
  return 0;
}


#pragma once

#ifdef POLYSOLVE_WITH_AMGCL
// #define POLYSOLVE_AMGCL_V2
// #define POLYSOLVE_AMGCL_SIMPLE
#define POLYSOLVE_AMGCL_DUMMY_PRECOND

////////////////////////////////////////////////////////////////////////////////
#include <polysolve/LinearSolver.hpp>

#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/adapter/crs_tuple.hpp>

#include <amgcl/solver/cg.hpp>
#include <amgcl/solver/bicgstab.hpp>
#include <amgcl/solver/fgmres.hpp>

#include <amgcl/coarsening/smoothed_aggregation.hpp>
#include <amgcl/coarsening/aggregation.hpp>

#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/relaxation/gauss_seidel.hpp>
#include <amgcl/relaxation/ilu0.hpp>
#include <amgcl/relaxation/iluk.hpp>
#include <amgcl/relaxation/ilut.hpp>
#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/relaxation/as_preconditioner.hpp>

#include <amgcl/preconditioner/dummy.hpp>
#include <amgcl/preconditioner/schur_pressure_correction.hpp>
#include <amgcl/relaxation/as_preconditioner.hpp>

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

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

namespace polysolve
{

    class LinearSolverAMGCL : public LinearSolver
    {

    public:
        LinearSolverAMGCL();
        ~LinearSolverAMGCL();

    private:
        POLYSOLVE_DELETE_MOVE_COPY(LinearSolverAMGCL)

    public:
        //////////////////////
        // Public interface //
        //////////////////////

        // Set solver parameters
        virtual void setParameters(const json &params) override;

        // Retrieve memory information from Pardiso
        virtual void getInfo(json &params) const override;

        // Analyze sparsity pattern
        virtual void analyzePattern(const StiffnessMatrix &A, const int precond_num) override { precond_num_ = precond_num; }

        // Factorize system matrix
        virtual void factorize(const StiffnessMatrix &A) override;

        // Solve the linear system Ax = b
        virtual void solve(const Ref<const VectorXd> b, Ref<VectorXd> x) override;

        // Name of the solver type (for debugging purposes)
        virtual std::string name() const override { return "AMGCL"; }

    private:
        // typedef amgcl::backend::eigen<double> Backend;
        typedef amgcl::backend::builtin<double> Backend;

        // typedef amgcl::make_solver<
        // 	amgcl::amg<
        // 		Backend,
        // 		amgcl::coarsening::aggregation,
        // 		amgcl::relaxation::gauss_seidel>,
        // 	amgcl::solver::cg<Backend>> Solver;

        // typedef amgcl::make_solver<
        // 	// Use AMG as preconditioner:
        // 	amgcl::amg<
        // 		Backend,
        // 		amgcl::coarsening::smoothed_aggregation,
        // 		amgcl::relaxation::spai0>,
        // 	// And BiCGStab as iterative solver:
        // 	amgcl::solver::bicgstab<Backend>>
        // 	Solver;

#ifdef POLYSOLVE_AMGCL_SIMPLE
        // Use AMG as preconditioner:
        typedef amgcl::make_solver<
            // Use AMG as preconditioner:
            amgcl::amg<
                Backend,
                amgcl::coarsening::smoothed_aggregation,
                amgcl::relaxation::gauss_seidel>,
            // And CG as iterative solver:
            amgcl::solver::cg<Backend>>
            Solver;
#endif

#ifdef POLYSOLVE_AMGCL_DUMMY_PRECOND
        typedef amgcl::make_solver<
            amgcl::preconditioner::schur_pressure_correction<
                amgcl::make_solver<
                    amgcl::amg<
                        Backend,
                        amgcl::coarsening::smoothed_aggregation,
                        amgcl::relaxation::ilu0>,
                    amgcl::solver::bicgstab<Backend>>,
                amgcl::make_solver<
                    amgcl::preconditioner::dummy<Backend>,
                    amgcl::solver::bicgstab<Backend>>>,
            amgcl::solver::fgmres<Backend>>
            Solver;
#endif
#ifdef POLYSOLVE_AMGCL_V1
        typedef amgcl::make_solver<
            amgcl::relaxation::as_preconditioner<Backend,
                                                 amgcl::relaxation::iluk>,
            amgcl::solver::cg<Backend>>
            Solver;
#endif

#ifdef POLYSOLVE_AMGCL_V2
        typedef amgcl::make_solver<
            amgcl::preconditioner::schur_pressure_correction<amgcl::make_solver< // Solver for (8b)
                                                                 amgcl::amg<
                                                                     Backend,
                                                                     amgcl::coarsening::aggregation,
                                                                     amgcl::relaxation::ilut>,
                                                                 amgcl::solver::cg<Backend>>,
                                                             amgcl::make_solver< // Solver for (8a)
                                                                 amgcl::relaxation::as_preconditioner<Backend,
                                                                                amgcl::relaxation::spai0>,
                                                                 amgcl::solver::cg<Backend>>>,
            amgcl::solver::fgmres<Backend>>
            Solver;
#endif

        Solver *solver_ = nullptr;
        Solver::params params_;
        // StiffnessMatrix mat;

        std::vector<int> ia_, ja_;
        std::vector<double> a_;

        size_t iterations_;
        double residual_error_;
        int precond_num_;
    };

} // namespace polysolve

#endif


#include "OnlineRrtStar.hpp"
#include <stdio.h>
#include <cmath>
#include <cstdlib>
#include "../planning-utils/geom/Coord.hpp"
#include "../planning-utils/geom/utils.hpp"

//#include <boost/geometry.hpp>
//#include <boost/geometry/geometries/geometries.hpp>
//#include <boost/geometry/index/rtree.hpp>

using namespace std;

OnlineRrtStar::OnlineRrtStar(shared_ptr<vector<vector<bool>>> obstacleHash, shared_ptr<vector<shared_ptr<Rect>>> obstacleRects, double maxSegment, int width, int height,
                             bool usePseudoRandom, shared_ptr<Coord> start, double pruneRadius, double percentCoverage)
    : SamplingPlanner(obstacleHash, obstacleRects, maxSegment, width, height, usePseudoRandom, start, pruneRadius, percentCoverage) {
	this->name = "orrtstar";
	this->root->status = Status::Closed;
	this->endNode->status = Status::Closed;

	this->numNodes = 2;
}

bool OnlineRrtStar::isDoneBuilding() { return this->numNodes > this->nodeAddThreshold; }

void OnlineRrtStar::sampleAndAdd() {
	auto p = this->randomOpenAreaPoint();
	auto nn = this->getNearestNeighbor(p);
	auto dist = euclideanDistance(p, nn->coord);

	if (dist > this->maxSegment) {
		auto angle = angleBetweenCoords(nn->coord, p);
		auto x = this->maxSegment * cos(angle) + nn->coord.x;
		auto y = this->maxSegment * sin(angle) + nn->coord.y;

		p.change(x, y);
	}

	if (!(*this->obstacleHash)[(int)p.y][(int)p.x]) {
		vector<shared_ptr<RrtNode>> neighbors;
		vector<double> neighborCosts;
		shared_ptr<RrtNode> bestNeighbor;
		double bestCost;
		this->findBestNeighbor(p, bestNeighbor, bestCost, neighbors, neighborCosts);
		if (bestNeighbor && bestNeighbor->status == Status::Closed && !this->lineIntersectsObstacle(p, bestNeighbor->coord)) {
			auto node = make_shared<RrtNode>(p, nullptr, numeric_limits<double>::max());
			node->status = Status::Closed;
			bestNeighbor->addChild(node, bestCost);
			this->rtree.insert(RtreeValue(p.getBoostPoint(), node));
			this->numNodes++;

			for (unsigned long i = 0; i < neighbors.size(); i++) {
				if (neighbors[i]->status == Status::Closed && neighbors[i] != bestNeighbor) {
					// printf("nc: %.2f\n", neighbors[i]->cumulativeCost);
					// printf("pot: %.2f, curr: %.2f\n", min(node->cumulativeCost + neighborCosts[i], 10000.0),
					//       min(neighbors[i]->cumulativeCost, 10000.0));
					// auto cost = this->getCost(bestNeighbor, neighbors[i]);

					if (node->cumulativeCost + neighborCosts[i] < neighbors[i]->cumulativeCost) {
						if (!this->lineIntersectsObstacle(neighbors[i]->coord, node->coord)) {
							neighbors[i]->rewire(node, neighborCosts[i]);
						}
					}
				}
			}
		}
	}
}


#ifndef INCLUDED_scheme_nest_maps_OriTransMap_HH
#define INCLUDED_scheme_nest_maps_OriTransMap_HH

#include "scheme/nest/pmap/ScaleMap.hh"
#include "scheme/nest/pmap/TetracontoctachoronMap.hh"

#include "scheme/util/SimpleArray.hh"

#include <Eigen/Dense>

#include <boost/static_assert.hpp>
#include <iostream>
#include <vector>

namespace scheme { namespace nest { namespace pmap {

	template<
		int DIM=6,
		class Value=Eigen::Transform<double,3,Eigen::AffineCompact>,
		class Index=uint64_t,
		class Float=typename Value::Scalar
	>
	struct OriTransMap {
		BOOST_STATIC_ASSERT_MSG(DIM==6,"RotTransMap DIM must be == 6");
		BOOST_STATIC_ASSERT_MSG(sizeof(Value)==48||sizeof(Value)==96,"Value must be AffineCompact repr");

		static int const DIMENSION = DIM;
		typedef Value ValueType ;
		typedef Float FloatType ;
		typedef Index IndexType ;
		typedef util::SimpleArray<DIM,Index> Indices;
		typedef util::SimpleArray<DIM,Float> Params;
		typedef util::SimpleArray<3,Float> P3;
		typedef Eigen::Matrix<Float,3,1> V;
		typedef Eigen::Matrix<Float,3,3> M;

		typedef scheme::nest::pmap::TetracontoctachoronMap<3,M,Index,Float> OriMap;
		typedef scheme::nest::pmap::              ScaleMap<3,V,Index,Float> TransMap;

		OriMap ori_map_;
		TransMap trans_map_;

		OriTransMap(){}

		template< class P, class I >
		OriTransMap(
			Float rot_resl_deg,
			P const & lb,
			P const & ub,
			I const & bs
		){
			init( rot_resl_deg, lb, ub, bs );
		}

		template< class P, class I >
		void init(
			Float rot_resl_deg,
			P const & lb,
			P const & ub,
			I const & bs
		){
			int const ori_nside = OriMap::get_nside_for_rot_resl_deg( rot_resl_deg );
			ori_map_.init( ori_nside );
			trans_map_.init( lb, ub, bs );
			// cout << "OriMap: TetracontoctachoronMap, "
			//      << rot_resl_deg << " nside: " << ori_map_.nside_ << ", covrad: "
		 //    	 << ori_map_.bin_circumradius(0)*180.0/M_PI << ", size: " << ori_map_.num_cells() << endl;
		}

		///@brief sets value to parameters without change
		///@return false iff invalid parameters
		bool params_to_value(
			Params const & params,
			Index cell_index,
			Index resl,
			Value & value
		) const {
			Index const ncori  = ori_map_.num_cells();
			Index const cori   = cell_index % ncori;
			Index const ctrans = cell_index / ncori;
			P3 pori  ( params, 0 ); // offset 0
			P3 ptrans( params, 3 ); // offset 3
			M m;
			V v;
			bool valid = ori_map_.params_to_value( pori  , cori  , resl, m );
			valid   &= trans_map_.params_to_value( ptrans, ctrans, resl, v );
			if( !valid ) return false;
			value = Value( m );
			value.translation()[0] = v[0];
			value.translation()[1] = v[1];
			value.translation()[2] = v[2];
			return true;
		}

		///@brief sets params/cell_index from value
		///@note necessary for value lookup and neighbor lookup
		bool value_to_params(
			Value const & value,
			Index resl,
			Params & params,
			Index & cell_index
		) const {
			V v;
			M m;
			for( int i = 0; i < 3; ++i ){
				v[i] = value.translation()[i];
				for( int j = 0; j < 3; ++j ) m(i,j) = value.data()[i+3*j]; // awful....
					// this is because v.rotation() is slow for eigen AffineCompact
			}
			P3 o_params, t_params;
			Index o_ci, t_ci;
			bool valid = ori_map_.value_to_params( m, resl, o_params, o_ci );
			if( !valid ) return false;
			// cout << "o_ci " << o_ci << endl;
			trans_map_.value_to_params( v, resl, t_params, t_ci );
			for( int i = 0; i < 3; ++i ){
				params[i  ] = o_params[i];
				params[i+3] = t_params[i];
			}
			cell_index = t_ci * ori_map_.num_cells() + o_ci;

			return true;
		}

		///@brief cell size
		Index num_cells() const { return ori_map_.num_cells() * trans_map_.num_cells(); }

		static std::string pmap_name() { return "OriTransMap< "+OriMap::pmap_name()+", "+TransMap::pmap_name()+" >"; }

	};

template< int D, class V, class I, class F >
std::ostream & operator << ( std::ostream & out, OriTransMap<D,V,I,F> const & otm ){
	out << "OriMap: " << otm.ori_map_ << std::endl;
	out << "TransMap: " << otm.trans_map_ ;
	return out;
}

}}}

#endif


//
// Copyright Antoine Leblanc 2010 - 2015
// Distributed under the MIT license.
//
// http://nauths.fr
// http://github.com/nicuveo
// mailto://antoine.jp.leblanc@gmail.com
//

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Includes

#include <cstdlib>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "nauths/mml/mml.hh"

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Local code

namespace
{

  int next()
  {
    return sqrt(rand()) / 5;
  }

  typedef mml::Point<mml::Num> Point;

}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Implementation

BOOST_AUTO_TEST_SUITE(point)

BOOST_AUTO_TEST_CASE(t0)
{
  srand(time(0));

  int x = next() - next();
  int y = next() - next();

  Point p0;
  Point p1(x, y);
  Point p2(p1);

  BOOST_REQUIRE_EQUAL(p0, Point());
  BOOST_REQUIRE_EQUAL(p0, Point(0, 0));
  BOOST_REQUIRE_EQUAL(p0.x(), 0);
  BOOST_REQUIRE_EQUAL(p0.y(), 0);
  BOOST_REQUIRE_EQUAL(p0.rx(), 0);
  BOOST_REQUIRE_EQUAL(p0.ry(), 0);
  BOOST_REQUIRE_EQUAL(p0[0], 0);
  BOOST_REQUIRE_EQUAL(p0[1], 0);

  BOOST_REQUIRE_EQUAL(p1, Point(x, y));
  BOOST_REQUIRE_EQUAL(p1.x(), x);
  BOOST_REQUIRE_EQUAL(p1.y(), y);
  BOOST_REQUIRE_EQUAL(p1.rx(), x);
  BOOST_REQUIRE_EQUAL(p1.ry(), y);
  BOOST_REQUIRE_EQUAL(p1[0], x);
  BOOST_REQUIRE_EQUAL(p1[1], y);

  BOOST_REQUIRE_EQUAL(p2, p1);
  BOOST_REQUIRE_EQUAL(p2.x(), p1.x());
  BOOST_REQUIRE_EQUAL(p2.y(), p1.y());
  BOOST_REQUIRE_EQUAL(p2.rx(), p1.x());
  BOOST_REQUIRE_EQUAL(p2.ry(), p1.y());
  BOOST_REQUIRE_EQUAL(p2[0], p1.x());
  BOOST_REQUIRE_EQUAL(p2[1], p1.y());
}

BOOST_AUTO_TEST_CASE(t1)
{
  srand(time(0));

  int x = next() - next();
  int y = next() - next();

  Point p0;
  Point p1(x, y);
  Point p2(p1);

  BOOST_REQUIRE_EQUAL(p0.length(), 0);
  BOOST_REQUIRE_EQUAL(p0.sqr_length(), 0);
  BOOST_REQUIRE_EQUAL(p1.length(), sqrt(x * x + y * y));
  BOOST_REQUIRE_EQUAL(p1.sqr_length(), x * x + y * y);
  BOOST_REQUIRE_EQUAL(p2.length(), p1.length());
  BOOST_REQUIRE_EQUAL(p2.sqr_length(), p1.sqr_length());
}

BOOST_AUTO_TEST_CASE(t2)
{
  srand(time(0));

  int x = next() - next();
  int y = next() - next();

  Point p0;
  Point p1(x, y);
  Point p2(p1);

  BOOST_REQUIRE_EQUAL(p0 * 4, p0);
  BOOST_REQUIRE_EQUAL(p0 / 4, p0);
  BOOST_REQUIRE_EQUAL(p0 % 4, p0);
  BOOST_REQUIRE_EQUAL(p0 + p0, p0);
  BOOST_REQUIRE_EQUAL(p0 - p0, p0);
  BOOST_REQUIRE_EQUAL(p0 * p0, 0);
  BOOST_REQUIRE_EQUAL(p0.normal(), p0);

  BOOST_REQUIRE_EQUAL(p1 * 4, Point(x * 4, y * 4));
  BOOST_REQUIRE_EQUAL(p1 / 4, Point(x / 4, y / 4));
  BOOST_REQUIRE_EQUAL(p1 % 4, Point(x % 4, y % 4));
  BOOST_REQUIRE_EQUAL(p1 + p0, p1);
  BOOST_REQUIRE_EQUAL(p1 - p0, p1);
  BOOST_REQUIRE_EQUAL(p1 * p0, 0);
  BOOST_REQUIRE_EQUAL(p1 * p1, x * x + y * y);
  BOOST_REQUIRE_EQUAL(p1.normal(), Point(-y, x));
}

BOOST_AUTO_TEST_CASE(t3)
{
  srand(time(0));

  int x = next() - next();
  int y = next() - next();

  Point p(x, y);

#define C_TEST_TYPE(TYPE, MIN, MAX)             \
  {                                             \
    typedef TYPE T;                             \
    for (T i = T(MIN); i < T(MAX); i += T(1))   \
      BOOST_REQUIRE_EQUAL(p * i, i * p);        \
  }                                             \

  C_TEST_TYPE(signed char,     -10, 10);
  C_TEST_TYPE(unsigned char,     0, 10);
  C_TEST_TYPE(signed short,    -10, 10);
  C_TEST_TYPE(unsigned short,    0, 10);
  C_TEST_TYPE(signed int,      -10, 10);
  C_TEST_TYPE(unsigned int,      0, 10);
  C_TEST_TYPE(signed long,     -10, 10);
  C_TEST_TYPE(unsigned long,     0, 10);
  C_TEST_TYPE(float,           -10, 10);
  C_TEST_TYPE(double,          -10, 10);
}

BOOST_AUTO_TEST_SUITE_END()


#include <fenv.h>
#include <iomanip>
#include <iostream>

#include <boost/numeric/interval.hpp>

// clang-format off
typedef boost::numeric::interval<
    double,
    boost::numeric::interval_lib::policies<
        boost::numeric::interval_lib::save_state<
            boost::numeric::interval_lib::rounded_transc_std<double> >,
        boost::numeric::interval_lib::checking_base<double> > >
    Interval;
// clang-format on

void show_fe_current_rounding_method(void)
{
    switch (fegetround()) {
    case FE_TONEAREST:
        std::cout << "FE_TONEAREST" << std::endl;
        break;
    case FE_DOWNWARD:
        std::cout << "FE_DOWNWARD" << std::endl;
        break;
    case FE_UPWARD:
        std::cout << "FE_UPWARD" << std::endl;
        break;
    case FE_TOWARDZERO:
        std::cout << "FE_TOWARDZERO" << std::endl;
        break;
    default:
        std::cout << "unknown" << std::endl;
    };
}

int main(int argc, char* argv[])
{
    {
        Interval theta = Interval(0.79358805865013693);
        Interval output = cos(theta);
        std::cout << std::setprecision(
                         std::numeric_limits<double>::digits10 + 1)
                  << "[" << output.lower() << ", " << output.upper() << "]"
                  << std::endl;
        std::cout << "is empty: "
                  << (output.lower() > output.upper() ? "true" : "false")
                  << std::endl;
    }

    {
        double theta = 0.79358805865013693;
        show_fe_current_rounding_method();
        volatile double output1 = cos(theta);
        std::cout << std::setprecision(
                         std::numeric_limits<double>::digits10 + 1)
                  << output1 << std::endl;
        fesetround(FE_DOWNWARD);
        show_fe_current_rounding_method();
        volatile double output2 = cos(theta);
        std::cout << std::setprecision(
                         std::numeric_limits<double>::digits10 + 1)
                  << output2 << std::endl;
        fesetround(FE_UPWARD);
        show_fe_current_rounding_method();
        volatile double output3 = cos(theta);
        std::cout << std::setprecision(
                         std::numeric_limits<double>::digits10 + 1)
                  << output3 << std::endl;
        fesetround(FE_TOWARDZERO);
        show_fe_current_rounding_method();
        volatile double output4 = cos(theta);
        std::cout << std::setprecision(
                         std::numeric_limits<double>::digits10 + 1)
                  << output4 << std::endl;
    }
}


#include <mex.h>
#include "segment_list.h"
#include "box_up.h"
#include <Eigen/Core>
#include <iostream>
#include <igl/C_STR.h>
#include <igl/matlab/mexErrMsgTxt.h>
#undef assert
#define assert( isOK ) ( (isOK) ? (void)0 : (void) ::mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )

#include <igl/PI.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Bbox_3.h>
#include <CGAL/box_intersection_d.h>

#include <igl/barycenter.h>
#include <algorithm>
namespace igl
{
// FORM_FACTOR
//
// [F,V,BC,N] = form_factor(P,E)
//
// Inputs:
//   P  #P by 2 list of 2D vertices
//   E  #E by 2 list of edge indices into P
// Outputs:
//   F  #E by #E matrix of integrated form factor values
//   V  #E by #E matrix of visibility values
//   BC  #E by 2 list of edge barycenters
//   N  #E by 2 list of length-weighted normals
template <
  typename DerivedP,
  typename DerivedE,
  typename DerivedF,
  typename DerivedV,
  typename DerivedBC,
  typename DerivedN>
inline void form_factor(
  const Eigen::MatrixBase<DerivedP> & P,
  const Eigen::MatrixBase<DerivedE> & E,
  Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedV> & V,
  Eigen::PlainObjectBase<DerivedBC> & BC,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  assert(P.cols() == 2 && "P should be 2D");
  assert(E.cols() == 2 && "E should be segments");
  const int m = E.rows();
  F.resize(m,m);
  V.resize(m,m);
  igl::barycenter(P,E,BC);
  DerivedN U;
  N.resize(m,2);
  for(int e = 0;e<m;e++)
  {
    N(e,0) = + (P(E(e,1),1)-P(E(e,0),1));
    N(e,1) = - (P(E(e,1),0)-P(E(e,0),0));
  }
  // unit normals
  U = N;
  U.rowwise().normalize();

  typedef int Index;
  typedef CGAL::Epick Kernel;
  // 2D Primitives
  typedef CGAL::Point_2<Kernel>    Point_2;
  typedef CGAL::Segment_2<Kernel>  Segment_2;
  typedef std::vector<Segment_2> Segments;
  typedef typename std::vector<CGAL::Segment_2<Kernel> > Segments;
  typedef typename Segments::iterator SegmentsIterator;
  typedef typename Segments::const_iterator SegmentsConstIterator;
  typedef
    CGAL::Box_intersection_d::Box_with_handle_d<double,2,SegmentsIterator>
    Box;
  typedef
    std::map<Index,std::vector<std::pair<Index,CGAL::Object> > >
    OffendingMap;
  typedef std::map<std::pair<Index,Index>,std::vector<Index> >  EdgeMap;
  typedef std::pair<Index,Index> EMK;

  Segments scene_segments;
  std::vector<Box> scene_boxes;
  igl::copyleft::cgal::segment_list(P,E,scene_segments);

  igl::copyleft::cgal::box_up(scene_segments,scene_boxes);

  // rays as segments
  Segments ray_segments;
  ray_segments.reserve(m*(m-1)/2);
  std::vector<std::pair<int,int> > ray_subs;
  for(int i = 0;i<m;i++)
  {
    Point_2 pi(BC(i,0),BC(i,1));
    for(int j = i+1;j<m;j++)
    {
      Point_2 pj(BC(j,0),BC(j,1));
      ray_segments.emplace_back(pj,pi);
      ray_subs.push_back({i,j});
    }
  }
  assert(ray_segments.size() == (m*(m-1)/2));
  std::vector<Box> ray_boxes;
  igl::copyleft::cgal::box_up(ray_segments,ray_boxes);
  V.setConstant(m,m,true);
  const auto cb = [&](const Box &scene_box, const Box &ray_box) -> void
  {
    using namespace std;
    // index in F and T
    int scene_ind = scene_box.handle()-scene_segments.begin();
    int ray_ind = ray_box.handle()-ray_segments.begin();
    int source_ind = ray_subs[ray_ind].first;
    int dest_ind = ray_subs[ray_ind].second;
    if(source_ind == scene_ind || dest_ind == scene_ind)
    {
      return;
    }
    const Segment_2 & scene_seg = *scene_box.handle();
    const Segment_2 & ray_seg = *ray_box.handle();
    if(CGAL::do_intersect(scene_seg,ray_seg))
    {
      V(source_ind,dest_ind) = false;
      // also fill in transpose
      V(dest_ind,source_ind) = V(source_ind,dest_ind);
    }
  };
  CGAL::box_intersection_d(
    scene_boxes.begin(), scene_boxes.end(),
    ray_boxes.begin(), ray_boxes.end(),
    cb);
  typedef Eigen::Matrix<typename DerivedF::Scalar,1,2> RowVector2S;
  typedef typename DerivedF::Scalar Scalar;
  F.setConstant(m,m,0);
  for(int i = 0;i<m;i++)
  {
    const RowVector2S bci = BC.row(i);
    for(int j = i+1;j<m;j++)
    {
      const RowVector2S bcj = BC.row(j);
      const RowVector2S rij = bcj-bci;
      const Scalar rij_ui = +rij.dot(U.row(i));
      const Scalar rij_uj = -rij.dot(U.row(j));
      if( rij_ui > 0 && rij_uj > 0 && V(i,j))
      {
        const RowVector2S urij = rij.normalized();
        F(i,j) = (rij_ui*rij_uj)/(igl::PI*std::pow(rij.squaredNorm(),1.5));
        // also fill in transpose
        F(j,i) = F(i,j);
      }
    }
  }
}

}

void mexFunction(
  int          nlhs,
  mxArray      *plhs[],
  int          nrhs,
  const mxArray *prhs[]
)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  MatrixXd P;
  MatrixXi E;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  mexErrMsgTxt(nrhs>=2,"nrhs should be >= 2");
  parse_rhs_double(prhs,P);
  parse_rhs_index(prhs+1,E);
  mexErrMsgTxt(P.cols()==2,"P must be #P by 2");
  mexErrMsgTxt(E.cols()==2,"E must be #E by 2");

  MatrixXd FF,V,BC,N;
  form_factor(P,E,FF,V,BC,N);

  switch(nlhs)
  {
    case 4:
      prepare_lhs_double(N,plhs+3);
    case 3:
      prepare_lhs_double(BC,plhs+2);
    case 2:
      prepare_lhs_double(V,plhs+1);
    case 1:
      prepare_lhs_double(FF,plhs+0);
    default:break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
  return;
}


/****
   Copyright 2005-2007, Moshe Looks

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
****/

#include "scoring.h"
#include "dtree_modeling.h"

#include <vector>
#include <map>
#include <boost/iterator/counting_iterator.hpp>

namespace alignment {
  static const score_t EPSILON=0.001;

  score_t Scorer::score(const match_pair& p) const {
    score_map::const_iterator it=_map.find(p);
    return (it==_map.end() ? _map.insert
	    (make_pair(p,compute_score(p.first,p.second)-
		       _noise*randfloat())).first :
	    it)->second;
  }
  score_t Scorer::score(const range_pair& p) const {
    if (p.first.first==p.first.second || p.second.first==p.second.second)
      return 0;
    range_score_map::const_iterator it=_rangeMap.find(p);
    return (it==_rangeMap.end() ? _rangeMap.insert
	    (make_pair(p,ordered_match(p)-_noise*randfloat())).first :
	    it)->second;
  }

  score_t Scorer::compute_score(sib_it l,sib_it r) const {
    static const score_t descent_penalty=1;

    //is_empty case taken out for now...
    score_t s=exact_match(l,r)+descent_penalty;
    if (go_down(*l))
      for (sib_it lc=l.begin();lc!=l.end();++lc)
	s=max(s,score(lc,r));
    if (go_down(*r))
      for (sib_it rc=r.begin();rc!=r.end();++rc)
	s=max(s,score(l,rc));
    return s-descent_penalty;
  }

  score_t Scorer::exact_match(sib_it l,sib_it r) const {
    return (matches(*l,*r) ?
	    ((l.begin()==l.end() && r.begin()==r.end()) ?
	     1 :
	     (is_commutative(*l) ?
	      unordered_match(l.begin(),l.end(),r.begin(),r.end()) :
	      (is_associative(*l) ?
	       score(l.begin(),l.end(),r.begin(),r.end()) :
	       exact_match(l.begin(),l.end(),r.begin(),r.end())))) :
	    0);
  }

  score_t Scorer::exact_match(sib_it l1,sib_it l2,
			      sib_it r1,sib_it r2) const {
    score_t res=0;
    for (;l1!=l2;++l1,++r1)
      res+=score(l1,r1);
    return res;
  }

  score_t Scorer::ordered_match(range_pair p) const {
    score_t s=score(p.first.first++,p.second.first++);
    s+=score(p);
    --p.first.first;
    s=max(s,score(p));
    ++p.first.first;
    --p.second.first;
    return max(s,score(p));
  }

  score_t Scorer::unordered_match(sib_it l1,sib_it l2,
				  sib_it r1,sib_it r2) const {
    std::vector<sib_it>
      lmatched(make_counting_iterator(l1),
	       make_counting_iterator(l2)),
      rmatched(make_counting_iterator(r1),
	       make_counting_iterator(r2));
    typedef std::multimap<score_t,pair<int,int>,std::greater<score_t> >
      ScoreMap;
    ScoreMap scores;
    for (int i=0;l1!=l2;++l1,++i) {
      sib_it r=r1;
      for (int j=0;r!=r2;++r,++j)
	scores.insert(make_pair(score(l1,r),make_pair(i,j)));
    }

    score_t s=0;
    int n=0,m=min(lmatched.size(),rmatched.size());
    for (ScoreMap::const_iterator it=scores.begin();
	 it!=scores.end();++it) {
      if (lmatched[it->second.first].node!=NULL &&
	  rmatched[it->second.second].node!=NULL) { //a valid match
	lmatched[it->second.first].node=NULL;
	rmatched[it->second.second].node=NULL;
	s+=it->first;
	if (++n==m)
	  return s;
      }
    }
    return s;
  }

  void merge_trees::unordered_merge(sib_it l1,sib_it l2,sib_it r1,sib_it r2) {
    std::vector<sib_it>
      lmatched(make_counting_iterator(l1),
	       make_counting_iterator(l2)),
      rmatched(make_counting_iterator(r1),
	       make_counting_iterator(r2));
    typedef std::multimap<score_t,pair<int,int>,std::greater<score_t> >
      ScoreMap;
    ScoreMap scores;
    sib_it l=l1;
    for (int i=0;l!=l2;++l,++i) {
      sib_it r=r1;
      for (int j=0;r!=r2;++r,++j)
	scores.insert(make_pair(_sc.score(l,r),make_pair(i,j)));
    }

    int n=0,m=min(lmatched.size(),rmatched.size());
    for (ScoreMap::const_iterator it=scores.begin();
	 it!=scores.end();++it) {
      if (it->first<=0)
	break;
      if (lmatched[it->second.first].node!=NULL &&
	  rmatched[it->second.second].node!=NULL) { //a valid match

	merge(lmatched[it->second.first],rmatched[it->second.second]);
	lmatched[it->second.first].node=NULL;
	rmatched[it->second.second].node=NULL;

	if (++n==m)
	  break;
      }
    }

    for (std::vector<sib_it>::const_iterator lsib=lmatched.begin();
	 lsib!=lmatched.end();++lsib)
      if (lsib->node!=NULL) //unmatched - add to end of right
	_dst.insert_subtree(r1,*lsib);
  }
  void merge_trees::ordered_merge(sib_it l,sib_it r) {
      score_t best=_sc.score(l,r)-1; //-1 for the alignment of *l with *r;

      sib_it lat=l.begin(),lnxt=++l.begin();
      sib_it rat=r.begin(),rnxt=++r.begin();
      while (lat!=l.end() && rat!=r.end()) {
	score_t s_im=_sc.score(lat,rat);
	score_t s0=s_im+_sc.score(lnxt,l.end(),rnxt,r.end());
	score_t s1=_sc.score(lat,l.end(),rnxt,r.end());
	score_t s2=_sc.score(lnxt,l.end(),rat,r.end());
	if (s_im>0 && s0>=s1 && s0>=s2) {
	  assert(s0>=best-EPSILON);
	  best-=s_im;
	  merge(lat,rat);
	  rat=rnxt++;
	  lat=lnxt++;
	} else if (s1>=s2) {
	  assert(ceil(s1)>=ceil(best)-EPSILON);
	  rat=rnxt++;
	} else {
	  assert(ceil(s2)>=ceil(best)-EPSILON);
	  _dst.move_before(rat,lat);
	  lat=lnxt++;
	}
      }
      _dst.append_children(r,lat,l.end());
  }

  void merge_trees::merge(sib_it l,sib_it r) {
    r->vlocs.splice(r->vlocs.begin(),l->vlocs);
    if (is_turn(*l)) {
      assert(is_turn(*r));
      r->v.insert(l->v.begin(),l->v.end()); //jumps too once we have jumps
      /*} else if (is_if_food(*l) || is_identity(*l) || is_near(*l) ||
	       is_pickup(*l) || is_holding(*l) || is_walktowards(*l)) {
      merge(l.begin(),r.begin());
      merge(++l.begin(),++r.begin());*/
    } else if (is_not(*l)) {
      assert(is_not(*r));
      merge(l.begin(),r.begin());
    } else if (is_or(*l) || is_and(*l)) {
      assert(*l==*r);
      unordered_merge(l.begin(),l.end(),r.begin(),r.end());
    } else if (is_progn(*l)) {
      assert(is_progn(*r));
      ordered_merge(l,r);
    } else {
      //if (*l!=*r
      //cout << *l << " " << *r << endl;
      if (*l!=*r)
	r->v.insert(l->v.begin(),l->v.end());
      //assert(*l==*r);
      cout << *l <<" " << *r << endl;
      assert(l.number_of_children()==r.number_of_children());
      for (sib_it sib1=l.begin(),sib2=r.begin();sib1!=l.end();++sib1,++sib2)
	merge(sib1,sib2);
    }
  }

} //~namespace alignment


//
// Created by david on 2018-05-04.
//
//#define EIGEN_USE_BLAS
//#define EIGEN_USE_LAPACK
#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>
#include <mps_routines/class_custom_contraction.h>
using Scalar = std::complex<double>;

template<class T>
void class_custom_contraction<T>::MultMv(T* theta_in_, T* theta_out_) {
    Eigen::TensorMap<Textra::Tensor<const T, 4>> theta_in (theta_in_, shape4);
    Eigen::TensorMap<Textra::Tensor<T, 1>>       theta_out(theta_out_, shape1);
    //Best yet! The sparcity of the effective hamiltonian (Lblock HA Rblock) is about 58% nonzeros.

    theta_out = Lblock
            .contract(theta_in, Textra::idx({0},{1}))
            .contract(HA ,     Textra::idx({1,2},{0,2}))//  idx({1,2,3},{0,4,5}))
            .contract(HB ,     Textra::idx({3,1},{0,2}))//  idx({1,2,3},{0,4,5}))
            .contract(Rblock,  Textra::idx({1,3},{0,2}))
            .shuffle(Textra::array4{1,0,2,3})
            .reshape(shape1);
    counter++;

//    Eigen::Map<Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>> mymat(mytens.data(), shape_theta1[0],shape_theta1[0]);
//    Eigen::TensorMap<Textra::Tensor<const T, 1>> theta_in(theta_in_, shape_theta1[0]);
//    Eigen::TensorMap<Textra::Tensor<T, 1>>       theta_out(theta_out_, shape_theta1[0]);
//    theta_out = mytens.contract(theta_in, Textra::idx({1},{0}));

//
//    Eigen::Map<Eigen::Matrix<T,Eigen::Dynamic,1>> theta_in(theta_in_, shape_theta1[0]);
//    Eigen::Map<Eigen::Matrix<T,Eigen::Dynamic,1>> theta_out(theta_out_, shape_theta1[0]);
//    theta_out = mymat * theta_in;
//

//    Eigen::TensorMap<Textra::Tensor<const T, 4>> theta_in (theta_in_, shape_theta4);
//    Eigen::TensorMap<Textra::Tensor<T, 1>>       theta_out(theta_out_, shape_theta1);
//    //Best yet! The sparcity of the effective hamiltonian (Lblock HA Rblock) is about 58% nonzeros.
//
//    theta_out = theta_in
//            .contract(Lblock, Textra::idx({1},{0}))
//            .contract(HA ,     Textra::idx({4,0},{0,2}))//  idx({1,2,3},{0,4,5}))
//            .contract(HB ,     Textra::idx({3,0},{0,2}))//  idx({1,2,3},{0,4,5}))
//            .contract(Rblock,  Textra::idx({0,3},{0,2}))
//            .shuffle(Textra::array4{1,0,2,3})
//            .reshape(shape_theta1);
//
//

//
//    Eigen::TensorMap<Textra::Tensor<const T, 4>> theta_in (theta_in_, shape_theta4);
//    Eigen::TensorMap<Textra::Tensor<T, 1>>       theta_out(theta_out_, shape_theta1);
//    //Best yet! The sparcity of the effective hamiltonian (Lblock HA Rblock) is about 58% nonzeros.
//
//    theta_out = Lblock
//            .contract(theta_in,    Textra::idx({0},{1}))
//            .contract(Rblock ,     Textra::idx({4},{0}))//  idx({1,2,3},{0,4,5}))
//            .contract(HA ,         Textra::idx({1,2},{0,2}))//  idx({1,2,3},{0,4,5}))
//            .contract(HB,          Textra::idx({4,3,1},{0,1,2}))
//            .shuffle(Textra::array4{2,0,3,1})
//            .reshape(shape_theta1);
//

}

//template class class_custom_contraction<double>;
template class class_custom_contraction<Scalar>;


/*
Copyright (c) 2012-2018, Visillect Service LLC. All rights reserved.
Developed for Kharkevich Institute for Information Transmission Problems of the
              Russian Academy of Sciences (IITP RAS).

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

   1. Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.

   2. 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.

THIS SOFTWARE IS PROVIDED BY COPYRIGHT HOLDERS "AS IS" AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL COPYRIGHT HOLDERS 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.

The views and conclusions contained in the software and documentation are those
of the authors and should not be interpreted as representing official policies,
either expressed or implied, of copyright holders.
*/

#include <roadseg/road_distance_func.h>

#include <remseg/distance_func.h>

#include <cassert>
#include <cmath>
#include <algorithm>

#include <iostream>

THIRDPARTY_INCLUDES_BEGIN
#include <Eigen/Geometry>
THIRDPARTY_INCLUDES_END

namespace roadseg {

double dist_point_to_line(Eigen::Vector3d const& p,
                          Eigen::Vector3d const& v,
                          Eigen::Vector3d const& mean)
{
  Eigen::Vector3d r = mean - p;
  Eigen::Vector3d res = r - v.dot(r) * v;
  return res.norm();
}

double dist_point_to_segment(const Eigen::Vector3d& p,
                             const Eigen::Vector3d& a,
                             const Eigen::Vector3d& b)
{
  if ((b - a).dot(p - a) < 0)
    return (p - a).norm();
  if ((a - b).dot(p - b) < 0)
    return (p - b).norm();
  return dist_point_to_line(p, (b - a).normalized(), a);
}

EdgeValue error(const RoadVertex *v) {
  RoadVertex::HelperStats const & hs = v->getHelperStats();
  double err = hs.eigenvalues()[0];
  return err * v->area;
}

bool isUnionLowVariance(const RoadVertex *v1, const RoadVertex *v2)
{
  RoadVertex v1cp(v1);
  RoadVertex v2cp(v2);
  v1cp.absorb(&v2cp);
  double firstEigenVal = error(&v1cp);
  return firstEigenVal < RoadVertex::getMaxFirstEigenVal();
}

EdgeValue criteria(const RoadVertex *v1, const RoadVertex *v2)
{
  if (!isUnionLowVariance(v1, v2))
    return std::numeric_limits<double>::infinity();

  RoadVertex v(v1);
  RoadVertex tmp(v2);
  v.absorb(&tmp);
  auto err_diff = error(&v) - error(v1) - error(v2);
  if (err_diff < 0) { // possible if points fit plane exact precisely
    return 0;
  }
  return std::sqrt(err_diff);
}

} // ns roadseg


/*    Copyright (c) 2010-2018, 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.
 *
 */

#include <boost/make_shared.hpp>

#include <Eigen/Geometry>

#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"
#include "Tudat/Astrodynamics/MissionSegments/lambertTargeterGooding.h"

//! Tudat library namespace.
namespace tudat
{
namespace mission_segments
{

using namespace root_finders;

//! Constructor with immediate definition of parameters and execution of the algorithm.
LambertTargeterGooding::LambertTargeterGooding(
        const Eigen::Vector3d& aCartesianPositionAtDeparture,
        const Eigen::Vector3d& aCartesianPositionAtArrival,
        const double aTimeOfFlight,
        const double aGravitationalParameter,
        RootFinderPointer aRootFinder )
    : LambertTargeter( aCartesianPositionAtDeparture, aCartesianPositionAtArrival,
                       aTimeOfFlight, aGravitationalParameter ),
      rootFinder( aRootFinder )
{
    // Required because the make_shared in the function definition gives problems for MSVC.
    if ( !rootFinder.get( ) )
    {
        rootFinder = std::make_shared< NewtonRaphson >( 1.0e-12, 1000 );
    }

    // Execute algorithm.
    execute( );
}

//! Execute Lambert targeting solver.
void LambertTargeterGooding::execute( )
{
    // Call Gooding's Lambert targeting routine.
    solveLambertProblemGooding( cartesianPositionAtDeparture, cartesianPositionAtArrival,
                                timeOfFlight, gravitationalParameter,
                                cartesianVelocityAtDeparture, cartesianVelocityAtArrival,
                                rootFinder );
}

//! Get radial velocity at departure.
double LambertTargeterGooding::getRadialVelocityAtDeparture( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );

    // Compute radial velocity at departure.
    return cartesianVelocityAtDeparture.dot( radialUnitVectorAtDeparture );
}

//! Get radial velocity at arrival.
double LambertTargeterGooding::getRadialVelocityAtArrival( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute radial velocity at arrival.
    return cartesianVelocityAtArrival.dot( radialUnitVectorAtArrival );
}

//! Get transverse velocity at departure.
double LambertTargeterGooding::getTransverseVelocityAtDeparture( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtDeparture.cross( cartesianVelocityAtDeparture );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtDeparture =
                angularMomentumUnitVector.cross( radialUnitVectorAtDeparture );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtDeparture.dot( tangentialUnitVectorAtDeparture );
}

//! Get transverse velocity at arrival.
double LambertTargeterGooding::getTransverseVelocityAtArrival( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtArrival.cross( cartesianVelocityAtArrival );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtArrival =
                angularMomentumUnitVector.cross( radialUnitVectorAtArrival );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtArrival.dot( tangentialUnitVectorAtArrival );
}

//! Get semi-major axis.
double LambertTargeterGooding::getSemiMajorAxis( )
{
    // Compute specific orbital energy: eps = v^2/ - mu/r.
    const double specificOrbitalEnergy = cartesianVelocityAtDeparture.squaredNorm( ) / 2.0
            - gravitationalParameter / cartesianPositionAtDeparture.norm( );

    // Compute semi-major axis: a = -mu / 2*eps.
    return -gravitationalParameter / ( 2.0 * specificOrbitalEnergy );
}

} // namespace mission_segments
} // namespace tudat