/*

Copyright (c) 2015, Project OSRM contributors
All rights reserved.

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

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "coordinate_calculation.hpp"

#include "../util/mercator.hpp"
#include "../util/string_util.hpp"

#include <boost/assert.hpp>

#include <osrm/coordinate.hpp>

#include <cmath>

#include <limits>

namespace
{
constexpr static const float RAD = 0.017453292519943295769236907684886f;
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
constexpr static const float earth_radius = 6372797.560856f;
}

namespace coordinate_calculation
{

double great_circle_distance(const int lat1,
                                                     const int lon1,
                                                     const int lat2,
                                                     const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    const double lt1 = lat1 / COORDINATE_PRECISION;
    const double ln1 = lon1 / COORDINATE_PRECISION;
    const double lt2 = lat2 / COORDINATE_PRECISION;
    const double ln2 = lon2 / COORDINATE_PRECISION;
    const double dlat1 = lt1 * (RAD);

    const double dlong1 = ln1 * (RAD);
    const double dlat2 = lt2 * (RAD);
    const double dlong2 = ln2 * (RAD);

    const double dLong = dlong1 - dlong2;
    const double dLat = dlat1 - dlat2;

    const double aHarv = std::pow(std::sin(dLat / 2.0), 2.0) +
                         std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dLong / 2.), 2);
    const double cHarv = 2. * std::atan2(std::sqrt(aHarv), std::sqrt(1.0 - aHarv));
    return earth_radius * cHarv;
}

double great_circle_distance(const FixedPointCoordinate &coordinate_1,
                                                     const FixedPointCoordinate &coordinate_2)
{
    return great_circle_distance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                                 coordinate_2.lon);
}

float euclidean_distance(const FixedPointCoordinate &coordinate_1,
                                                 const FixedPointCoordinate &coordinate_2)
{
    return euclidean_distance(coordinate_1.lat, coordinate_1.lon, coordinate_2.lat,
                              coordinate_2.lon);
}

float euclidean_distance(const int lat1,
                                                 const int lon1,
                                                 const int lat2,
                                                 const int lon2)
{
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const float float_lat1 = (lat1 / COORDINATE_PRECISION) * RAD;
    const float float_lon1 = (lon1 / COORDINATE_PRECISION) * RAD;
    const float float_lat2 = (lat2 / COORDINATE_PRECISION) * RAD;
    const float float_lon2 = (lon2 / COORDINATE_PRECISION) * RAD;

    const float x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.f);
    const float y_value = float_lat2 - float_lat1;
    return std::hypot(x_value, y_value) * earth_radius;
}

float perpendicular_distance(const FixedPointCoordinate &source_coordinate,
                                                     const FixedPointCoordinate &target_coordinate,
                                                     const FixedPointCoordinate &query_location)
{
    float ratio;
    FixedPointCoordinate nearest_location;

    return perpendicular_distance(source_coordinate, target_coordinate, query_location,
                                  nearest_location, ratio);
}

float perpendicular_distance(const FixedPointCoordinate &segment_source,
                                                     const FixedPointCoordinate &segment_target,
                                                     const FixedPointCoordinate &query_location,
                                                     FixedPointCoordinate &nearest_location,
                                                     float &ratio)
{
    return perpendicular_distance_from_projected_coordinate(
        segment_source, segment_target, query_location,
        {mercator::lat2y(query_location.lat / COORDINATE_PRECISION),
         query_location.lon / COORDINATE_PRECISION},
        nearest_location, ratio);
}

float perpendicular_distance_from_projected_coordinate(
    const FixedPointCoordinate &source_coordinate,
    const FixedPointCoordinate &target_coordinate,
    const FixedPointCoordinate &query_location,
    const std::pair<double, double> &projected_coordinate)
{
    float ratio;
    FixedPointCoordinate nearest_location;

    return perpendicular_distance_from_projected_coordinate(source_coordinate, target_coordinate,
                                                            query_location, projected_coordinate,
                                                            nearest_location, ratio);
}

float perpendicular_distance_from_projected_coordinate(
    const FixedPointCoordinate &segment_source,
    const FixedPointCoordinate &segment_target,
    const FixedPointCoordinate &query_location,
    const std::pair<double, double> &projected_coordinate,
    FixedPointCoordinate &nearest_location,
    float &ratio)
{
    BOOST_ASSERT(query_location.is_valid());

    // initialize values
    const double x = projected_coordinate.first;
    const double y = projected_coordinate.second;
    const double a = mercator::lat2y(segment_source.lat / COORDINATE_PRECISION);
    const double b = segment_source.lon / COORDINATE_PRECISION;
    const double c = mercator::lat2y(segment_target.lat / COORDINATE_PRECISION);
    const double d = segment_target.lon / COORDINATE_PRECISION;
    double p, q /*,mX*/, nY;
    if (std::abs(a - c) > std::numeric_limits<double>::epsilon())
    {
        const double m = (d - b) / (c - a); // slope
        // Projection of (x,y) on line joining (a,b) and (c,d)
        p = ((x + (m * y)) + (m * m * a - m * b)) / (1.f + m * m);
        q = b + m * (p - a);
    }
    else
    {
        p = c;
        q = y;
    }
    nY = (d * p - c * q) / (a * d - b * c);

    // discretize the result to coordinate precision. it's a hack!
    if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
    {
        nY = 0.f;
    }

    // compute ratio
    ratio =
        static_cast<float>((p - nY * a) / c); // These values are actually n/m+n and m/m+n , we need
    // not calculate the explicit values of m an n as we
    // are just interested in the ratio
    if (std::isnan(ratio))
    {
        ratio = (segment_target == query_location ? 1.f : 0.f);
    }
    else if (std::abs(ratio) <= std::numeric_limits<float>::epsilon())
    {
        ratio = 0.f;
    }
    else if (std::abs(ratio - 1.f) <= std::numeric_limits<float>::epsilon())
    {
        ratio = 1.f;
    }

    // compute nearest location
    BOOST_ASSERT(!std::isnan(ratio));
    if (ratio <= 0.f)
    {
        nearest_location = segment_source;
    }
    else if (ratio >= 1.f)
    {
        nearest_location = segment_target;
    }
    else
    {
        // point lies in between
        nearest_location.lat = static_cast<int>(mercator::y2lat(p) * COORDINATE_PRECISION);
        nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
    }
    BOOST_ASSERT(nearest_location.is_valid());

    const float approximate_distance =
        euclidean_distance(query_location, nearest_location);
    BOOST_ASSERT(0.f <= approximate_distance);
    return approximate_distance;
}

void lat_or_lon_to_string(const int value, std::string &output)
{
    char buffer[12];
    buffer[11] = 0; // zero termination
    output = printInt<11, 6>(buffer, value);
}

float deg_to_rad(const float degree)
{
    return degree * (static_cast<float>(M_PI) / 180.f);
}

float rad_to_deg(const float radian)
{
    return radian * (180.f * static_cast<float>(M_1_PI));
}

float bearing(const FixedPointCoordinate &first_coordinate,
                                      const FixedPointCoordinate &second_coordinate)
{
    const float lon_diff =
        second_coordinate.lon / COORDINATE_PRECISION - first_coordinate.lon / COORDINATE_PRECISION;
    const float lon_delta = deg_to_rad(lon_diff);
    const float lat1 = deg_to_rad(first_coordinate.lat / COORDINATE_PRECISION);
    const float lat2 = deg_to_rad(second_coordinate.lat / COORDINATE_PRECISION);
    const float y = std::sin(lon_delta) * std::cos(lat2);
    const float x =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    float result = rad_to_deg(std::atan2(y, x));
    while (result < 0.f)
    {
        result += 360.f;
    }

    while (result >= 360.f)
    {
        result -= 360.f;
    }
    return result;
}

}


#ifndef BOOSTGRAPH
#define BOOSTGRAPH
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/edge_connectivity.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/clustering_coefficient.hpp>
#include <boost/graph/dominator_tree.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>
#include <boost/graph/king_ordering.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/properties.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/edge_list.hpp>

#include <map>
#include <iostream>
#include <utility>

typedef int v_index;
typedef long e_index;

// This struct is the output of the edge connectivity Boost algorithm.
typedef struct {
    v_index ec; // The edge connectivity
    std::vector<v_index> edges; // The edges in a minimum cut, stored as a list of
                       // nodes. For instance, if the minimum cut is
                       // {(1,2),(3,4)}, the output vector will be (1,2,3,4).
} result_ec;

// This struct is the output of the clustering coefficient Boost algorithm.
typedef struct {
    double average_clustering_coefficient; // The average clustering coefficient
    std::vector<double> clust_of_v;             // The clustering coefficient of each node.
} result_cc;

// This struct is the output of the edge connectivity Boost algorithm.
typedef struct {
    std::vector<double> distances; // An array with all distances from the starting vertex
    std::vector<v_index> predecessors; // For each vertex v, the first vertex in a shortest
                                  // path from the starting vertex to v.
} result_distances;

template <class OutEdgeListS, // How neighbors are stored
          class VertexListS,  // How vertices are stored
          class DirectedS,    // The kind of graph (undirectedS, directedS, or bidirectionalS)
          class EdgeListS,    // How the list of edges is stored
          class EdgeProperty> // Properties of edges (weight)
class BoostGraph
/*
 * This generic class wraps a Boost graph, in order to make it Cython-friendly.
 *
 * In particular, it allows to "keep together" the Boost graph and the vector
 * *vertices: these two variables are generic, and Cython is not able to deal
 * with them properly, since it does not support generic classes.
 *
 * Vertices are numbers from 0 to n-1, where n is the total number of vertices:
 * this class takes care of the relation between number i and the corresponding
 * Boost vertex descriptor (which might be any object, depending on the value of
 * VertexListS). In particular, (*vertices)[i] contains the Boost vertex
 * corresponding to number i, while to transform a Boost vertex v into a number
 * we use vertex properties, and the syntax is (*graph)[v].
*/
{
private:
    typedef typename boost::adjacency_list<
        OutEdgeListS, VertexListS, DirectedS,
        boost::property<boost::vertex_index_t, v_index>,
        EdgeProperty, boost::no_property, EdgeListS> adjacency_list;
    typedef typename boost::graph_traits<adjacency_list>::vertex_descriptor vertex_descriptor;
    typedef typename boost::graph_traits<adjacency_list>::edge_descriptor edge_descriptor;
    typedef typename std::vector<edge_descriptor> edge_container;
    typedef typename boost::property_map<adjacency_list, boost::vertex_index_t>::type vertex_to_int_map;

    // This struct is used for biconnected_components function
    struct order_edges {
        bool operator()(const edge_descriptor& x, const edge_descriptor& y) const { return x.get_property() < y.get_property(); }
    };
    // This map is a parameter/output for biconnected_components function
    typedef typename std::map<edge_descriptor, int, order_edges> edge_map;

public:
    adjacency_list graph;
    std::vector<vertex_descriptor> vertices;
    vertex_to_int_map index;

    BoostGraph() {
    }

    v_index num_verts() {
        return num_vertices(graph);
    }

    e_index num_edges() {
        return boost::num_edges(graph);
    }

    void add_vertex() {
        vertices.push_back(boost::add_vertex(vertices.size(), graph));
    }

    void add_edge(v_index u, v_index v) {
        boost::add_edge(vertices[u], vertices[v], graph);
    }

    void add_edge(v_index u, v_index v, double weight) {
        boost::add_edge(vertices[u], vertices[v], weight, graph);
    }

    std::vector<std::pair<v_index, std::pair<v_index, double>>> edge_list() {
        std::vector<std::pair<v_index, std::pair<v_index, double>>> to_return;
        typename boost::graph_traits<adjacency_list>::edge_iterator ei, ei_end;
        for (boost::tie(ei, ei_end) = boost::edges(graph); ei != ei_end; ++ei) {
            to_return.push_back({index[boost::source(*ei, graph)],
                                 {index[boost::target(*ei, graph)],
                                  get(boost::edge_weight, graph, *ei)}});
        }
        return to_return;
    }

    result_ec edge_connectivity() {
        result_ec to_return;
        edge_container disconnecting_set;
        std::back_insert_iterator<edge_container> inserter(disconnecting_set);
        to_return.ec = boost::edge_connectivity(graph, inserter);

        for (size_t i = 0; i < disconnecting_set.size(); i++) {
            edge_descriptor edge = disconnecting_set[i];
            to_return.edges.push_back(index[boost::source(edge, graph)]);
            to_return.edges.push_back(index[boost::target(edge, graph)]);
        }
        return to_return;
    }

    double clustering_coeff(v_index v) {
        return clustering_coefficient(graph, vertices[v]);
    }

    result_cc clustering_coeff_all() {
        result_cc to_return;
        to_return.clust_of_v.resize(num_verts());
        to_return.average_clustering_coefficient = all_clustering_coefficients(graph,
            boost::make_iterator_property_map(to_return.clust_of_v.begin(), index));
        return to_return;
    }

    std::vector<v_index> dominator_tree(v_index v) {
        std::vector<v_index> fathers(num_verts());
        std::vector<vertex_descriptor> fathers_descr(num_verts(),
                    boost::graph_traits<adjacency_list>::null_vertex());

        lengauer_tarjan_dominator_tree(graph, vertices[v],
                                       boost::make_iterator_property_map(
                                           fathers_descr.begin(), index));

        for (v_index i = 0; i < num_verts(); i++) {
            vertex_descriptor v = fathers_descr[i];
            if (v == boost::graph_traits<adjacency_list>::null_vertex()) {
                fathers[i] = -1;
            } else {
                fathers[i] = index[v];
            }
        }
        return fathers;
    }

    // Works only in undirected graphs!
    std::vector<v_index> bandwidth_ordering(bool cuthill) {
        std::vector<v_index> to_return;
        std::vector<vertex_descriptor> inv_perm(num_vertices(graph));

        if (cuthill) {
            boost::cuthill_mckee_ordering(graph, inv_perm.rbegin());
        } else {
            boost::king_ordering(graph, inv_perm.rbegin());
        }

        for (size_t i = 0; i < inv_perm.size(); i++) {
            to_return.push_back(index[inv_perm[i]]);
        }
        return to_return;
    }

    // This function works only on undirected graphs.
    std::vector<v_index> kruskal_min_spanning_tree() {
        std::vector<v_index> to_return;
        std::vector<edge_descriptor> spanning_tree;
        kruskal_minimum_spanning_tree(graph, std::back_inserter(spanning_tree));

        for (unsigned int i = 0; i < spanning_tree.size(); i++) {
            to_return.push_back(index[source(spanning_tree[i], graph)]);
            to_return.push_back(index[target(spanning_tree[i], graph)]);
        }
        return to_return;
    }

    // This function works only on undirected graphs with no parallel edge.
    std::vector<v_index> prim_min_spanning_tree() {
        std::vector<v_index> to_return;
        std::vector<vertex_descriptor> predecessors(num_verts());
        prim_minimum_spanning_tree(graph, boost::make_iterator_property_map(predecessors.begin(), index));

        for (unsigned int i = 0; i < predecessors.size(); i++) {
            if (index[predecessors[i]] != i) {
                to_return.push_back(i);
                to_return.push_back(index[predecessors[i]]);
            }
        }
        return to_return;
    }

    // This function returns the biconnected components of the graph.
    std::vector<std::vector<v_index>> blocks_and_cut_vertices() {
        edge_map bicmp_map;
        boost::associative_property_map<edge_map> bimap(bicmp_map);
        std::size_t num_comps = biconnected_components(graph, bimap);

        // We iterate over every edge and add the vertices of block i into to_return[i].
        // to_return[i] could contain repetitions.
        std::vector<std::vector<v_index>> to_return(num_comps, std::vector<v_index>(0));
        typename boost::graph_traits<adjacency_list>::edge_iterator ei, ei_end;
        for (boost::tie(ei, ei_end) = edges(graph); ei != ei_end; ++ei) {
            to_return[bimap[*ei]].push_back(index[source(*ei, graph)]);
            to_return[bimap[*ei]].push_back(index[target(*ei, graph)]);
        }

        return to_return;
    }

    result_distances dijkstra_shortest_paths(v_index s) {
         v_index N = num_verts();
         result_distances to_return;
         std::vector<double> distances(N, (std::numeric_limits<double>::max)());
         std::vector<vertex_descriptor> predecessors(N);
         try {
             boost::dijkstra_shortest_paths(graph, vertices[s], distance_map(boost::make_iterator_property_map(distances.begin(), index))
                                            .predecessor_map(boost::make_iterator_property_map(predecessors.begin(), index)));
         } catch (boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::negative_edge> > e) {
             return to_return;
         }

         to_return.distances = distances;

         for (int i = 0; i < N; i++) {
             to_return.predecessors.push_back(index[predecessors[i]]);
         }

         return to_return;
     }

     result_distances bellman_ford_shortest_paths(v_index s) {
         v_index N = num_verts();

         std::vector<double> distance(N, (std::numeric_limits<double>::max)());
         std::vector<vertex_descriptor> predecessors(N);
         result_distances to_return;
         typename boost::property_map<adjacency_list, boost::edge_weight_t>::type weight = get(boost::edge_weight, (graph));

         for (v_index i = 0; i < N; ++i)
             predecessors[i] = vertices[i];

         distance[s] = 0;
         bool r = boost::bellman_ford_shortest_paths
             (graph, N, boost::weight_map(weight).distance_map(boost::make_iterator_property_map(distance.begin(), index)).predecessor_map(boost::make_iterator_property_map(predecessors.begin(), index)));

         if (!r) {
             return to_return;
         }

         to_return.distances = distance;
         for (int i = 0; i < N; i++) {
             to_return.predecessors.push_back(index[predecessors[i]]);
         }
         return to_return;
     }
     std::vector<std::vector<double> > floyd_warshall_shortest_paths() {
         v_index N = num_verts();

         std::vector<std::vector<double> > D(N, std::vector<double>(N));
         if (floyd_warshall_all_pairs_shortest_paths(graph, D)) {
             return D;
         } else {
             return std::vector<std::vector<double> >();
         }
     }
     std::vector<std::vector<double> > johnson_shortest_paths() {
         v_index N = num_verts();

         std::vector<std::vector<double> > D(N, std::vector<double>(N));
         if (johnson_all_pairs_shortest_paths(graph, D)) {
             return D;
         } else {
             return std::vector<std::vector<double> >();
         }
     }
};

#endif // BOOSTGRAPH


/**
 * \file boost/numeric/ublasx/operation/log2.hpp
 *
 * \brief Apply the \c std::log2 function to a vector or matrix expression.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_LOG2_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LOG2_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>
#include <complex>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_log2_functor_traits
{
	typedef VectorExprT input_expression_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef vector_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_log2_functor_traits
{
	typedef MatrixExprT input_expression_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef matrix_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

namespace /*<unnamed>*/ {

/// Auxiliary function used to replace ::std::log2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
T log2(T x)
{
  // C++0x and higher has std::log2 but it
  // doesn't work with complex numbers.
	return ::std::log(x)/::std::log(2);
}

} // Namespace <unnamed>

} // Namespace detail

/**
 * \brief Applies the \c std::log2 function to a given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::log2 to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_log2_functor_traits<VectorExprT>::result_type log2(vector_expression<VectorExprT> const& ve)
{
	typedef typename detail::vector_log2_functor_traits<VectorExprT>::expression_type expression_type;
	typedef typename detail::vector_log2_functor_traits<VectorExprT>::signature_result_type signature_result_type;

	return expression_type(ve(), detail::log2<signature_result_type>);
}

/**
 * \brief Applies the \c std::log2 function to a given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::log2 to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_log2_functor_traits<MatrixExprT>::result_type log2(matrix_expression<MatrixExprT> const& me)
{
	typedef typename detail::matrix_log2_functor_traits<MatrixExprT>::expression_type expression_type;
	typedef typename detail::matrix_log2_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

	return expression_type(me(), detail::log2<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LOG2_HPP


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

#ifndef BOOST_MATH_STATISTICS_RUNS_TEST_HPP
#define BOOST_MATH_STATISTICS_RUNS_TEST_HPP

#include <cmath>
#include <algorithm>
#include <utility>
#include <boost/math/statistics/univariate_statistics.hpp>
#include <boost/math/distributions/normal.hpp>

namespace boost::math::statistics {

template<class RandomAccessContainer>
auto runs_above_and_below_threshold(RandomAccessContainer const & v,
                          typename RandomAccessContainer::value_type threshold)
{
    using Real = typename RandomAccessContainer::value_type;
    using std::sqrt;
    using std::abs;
    if (v.size() <= 1)
    {
        throw std::domain_error("At least 2 samples are required to get number of runs.");
    }
    typedef boost::math::policies::policy<
          boost::math::policies::promote_float<false>,
          boost::math::policies::promote_double<false> >
          no_promote_policy;

    decltype(v.size()) nabove = 0;
    decltype(v.size()) nbelow = 0;

    decltype(v.size()) imin = 0;

    // Take care of the case that v[0] == threshold:
    while (imin < v.size() && v[imin] == threshold) {
        ++imin;
    }

    // Take care of the constant vector case:
    if (imin == v.size()) {
        return std::make_pair(std::numeric_limits<Real>::quiet_NaN(), Real(0));
    }

    bool run_up = (v[imin] > threshold);
    if (run_up) {
        ++nabove;
    } else {
        ++nbelow;
    }
    decltype(v.size()) runs = 1;
    for (decltype(v.size()) i = imin + 1; i < v.size(); ++i) {
      if (v[i] == threshold) {
        // skip values precisely equal to threshold (following R's randtests package)
        continue;
      }
      bool above = (v[i] > threshold);
      if (above) {
          ++nabove;
      } else {
          ++nbelow;
      }
      if (run_up == above) {
        continue;
      }
      else {
        run_up = above;
        runs++;
      }
    }

    // If you make n an int, the subtraction is gonna be bad in the variance:
    Real n = nabove + nbelow;

    Real expected_runs = Real(1) + Real(2*nabove*nbelow)/Real(n);
    Real variance = 2*nabove*nbelow*(2*nabove*nbelow-n)/Real(n*n*(n-1));

    // Bizarre, pathological limits:
    if (variance == 0)
    {
        if (runs == expected_runs)
        {
            Real statistic = 0;
            Real pvalue = 1;
            return std::make_pair(statistic, pvalue);
        }
        else
        {
            return std::make_pair(std::numeric_limits<Real>::quiet_NaN(), Real(0));
        }
    }

    Real sd = sqrt(variance);
    Real statistic = (runs - expected_runs)/sd;

    auto normal = boost::math::normal_distribution<Real, no_promote_policy>(0,1);
    Real pvalue = 2*boost::math::cdf(normal, -abs(statistic));
    return std::make_pair(statistic, pvalue);
}

template<class RandomAccessContainer>
auto runs_above_and_below_median(RandomAccessContainer const & v)
{
    using Real = typename RandomAccessContainer::value_type;
    using std::log;
    using std::sqrt;

    // We have to memcpy v because the median does a partial sort,
    // and that would be catastrophic for the runs test.
    auto w = v;
    Real median = boost::math::statistics::median(w);
    return runs_above_and_below_threshold(v, median);
}

}
#endif


// Copyright (c) Jeremy Siek 2001, Marc Wintermantel 2002
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appears in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  Silicon Graphics makes no
// representations about the suitability of this software for any
// purpose.  It is provided "as is" without express or implied warranty.

#ifndef BOOST_GRAPH_BANDWIDTH_HPP
#define BOOST_GRAPH_BANDWIDTH_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/detail/numeric_traits.hpp>

namespace boost {

  template <typename Graph, typename VertexIndexMap>
  typename graph_traits<Graph>::vertices_size_type
  ith_bandwidth(typename graph_traits<Graph>::vertex_descriptor i,
                const Graph& g,
                VertexIndexMap index)
  {
    typedef typename graph_traits<Graph>::vertices_size_type size_type;
    size_type b = 0;
    typename graph_traits<Graph>::out_edge_iterator e, end;
    for (tie(e, end) = out_edges(i, g); e != end; ++e) {
      int f_i = get(index, i);
      int f_j = get(index, target(*e, g));
      using namespace std; // to call abs() unqualified
      if(f_i > f_j)
      b = std::max(b, size_type(f_i - f_j));
    }
    return b;
  }

  template <typename Graph>
  typename graph_traits<Graph>::vertices_size_type
  ith_bandwidth(typename graph_traits<Graph>::vertex_descriptor i,
                const Graph& g)
  {
    return ith_bandwidth(i, g, get(vertex_index, g));
  }

  template <typename Graph, typename VertexIndexMap>
  typename graph_traits<Graph>::vertices_size_type
  bandwidth(const Graph& g, VertexIndexMap index)
  {
    typename graph_traits<Graph>::vertices_size_type b = 0;
    typename graph_traits<Graph>::vertex_iterator i, end;
    for (tie(i, end) = vertices(g); i != end; ++i)
        b = std::max(b, ith_bandwidth(*i, g, index));
    return b;
  }

  template <typename Graph>
  typename graph_traits<Graph>::vertices_size_type
  bandwidth(const Graph& g)
  {
    return bandwidth(g, get(vertex_index, g));
  }

  template <typename Graph, typename VertexIndexMap>
  typename graph_traits<Graph>::vertices_size_type
  edgesum(const Graph& g, VertexIndexMap index_map)
  {
    typedef typename graph_traits<Graph>::vertices_size_type size_type;
    typedef typename detail::numeric_traits<size_type>::difference_type diff_t;
    size_type sum = 0;
    typename graph_traits<Graph>::edge_iterator i, end;
    for (tie(i, end) = edges(g); i != end; ++i) {
      diff_t f_u = get(index_map, source(*i, g));
      diff_t f_v = get(index_map, target(*i, g));
      using namespace std; // to call abs() unqualified
      sum += abs(f_u - f_v);
    }
    return sum;
  }

} // namespace boost

#endif // BOOST_GRAPH_BANDWIDTH_HPP


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

#ifndef BOOST_MATH_STATISTICS_LJUNG_BOX_HPP
#define BOOST_MATH_STATISTICS_LJUNG_BOX_HPP

#include <cmath>
#include <iterator>
#include <utility>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>

namespace boost::math::statistics {

template<class RandomAccessIterator>
auto ljung_box(RandomAccessIterator begin, RandomAccessIterator end, int64_t lags = -1, int64_t fit_dof = 0) {
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;
    int64_t n = std::distance(begin, end);
    if (lags >= n) {
      throw std::domain_error("Number of lags must be < number of elements in array.");
    }

    if (lags == -1) {
      // This is the same default as Mathematica; it seems sensible enough . . .
      lags = static_cast<int64_t>(std::ceil(std::log(Real(n))));
    }

    if (lags <= 0) {
      throw std::domain_error("Must have at least one lag.");
    }

    auto mu = boost::math::statistics::mean(begin, end);

    std::vector<Real> r(lags + 1, Real(0));
    for (size_t i = 0; i < r.size(); ++i) {
      for (auto it = begin + i; it != end; ++it) {
        Real ak = *(it) - mu;
        Real akml = *(it-i) - mu;
        r[i] += ak*akml;
      }
    }

    Real Q = 0;

    for (size_t k = 1; k < r.size(); ++k) {
      Q += r[k]*r[k]/(r[0]*r[0]*(n-k));
    }
    Q *= n*(n+2);

    typedef boost::math::policies::policy<
          boost::math::policies::promote_float<false>,
          boost::math::policies::promote_double<false> >
          no_promote_policy;

    auto chi = boost::math::chi_squared_distribution<Real, no_promote_policy>(Real(lags - fit_dof));

    Real pvalue = 1 - boost::math::cdf(chi, Q);
    return std::make_pair(Q, pvalue);
}

template<class RandomAccessContainer>
auto ljung_box(RandomAccessContainer const & v, int64_t lags = -1, int64_t fit_dof = 0) {
    return ljung_box(v.begin(), v.end(), lags, fit_dof);
}

}
#endif


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/conj.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef matrix::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    matrix A(n, n), B(n, n), C(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
	A(i, j)=rand_normal<complex>::get();
	B(i, j)=rand_normal<complex>::get();
      }
    for (size_type j=0; j<n; ++j) {
      for (size_type i=0; i<j; ++i) {
	C(i, j)=rand_normal<complex>::get();
	C(j, i)=std::conj(C(i, j));
      }
      C(j, j)=rand_normal<complex>::get().real();
    }
    complex alpha(rand_normal<complex>::get());
    double beta(rand_normal<complex>::get().real());
    {
      matrix C1(alpha*ublas::prod(A, ublas::trans(ublas::conj(B)))+
		std::conj(alpha)*ublas::prod(B, ublas::trans(ublas::conj(A)))+
		beta*C);
      for (size_type j=0; j<n; ++j)
	for (size_type i=j+1; i<n; ++i)
	  C1(i, j)=C(i, j);
      matrix C2(C);
      blas::her2k(alpha, A, B, beta, blas::upper(C2));
      std::cout << "testing boost::ublas containers\n"
		<< "using ublas (A right htrans, C upper):\n" << print_mat(C1) << '\n'
		<< "using blas (A right htrans, C upper):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
    {
      matrix C1(alpha*ublas::prod(A, ublas::trans(ublas::conj(B)))+
		std::conj(alpha)*ublas::prod(B, ublas::trans(ublas::conj(A)))+
		beta*C);
      for (size_type j=0; j<n; ++j)
    	for (size_type i=0; i<j; ++i)
    	  C1(i, j)=C(i, j);
      matrix C2(C);
      blas::her2k(alpha, A, B, beta, blas::lower(C2));
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (A right htrans, C lower):\n" << print_mat(C1) << '\n'
    		<< "using blas (A right htrans, C lower):\n" << print_mat(C2) << '\n'
    		<< '\n';
    }
    {
      matrix C1(alpha*ublas::prod(ublas::trans(ublas::conj(A)), B)+
		std::conj(alpha)*ublas::prod(ublas::trans(ublas::conj(B)), A)+
		beta*C);
      for (size_type j=0; j<n; ++j)
    	for (size_type i=j+1; i<n; ++i)
    	  C1(i, j)=C(i, j);
      matrix C2(C);
      blas::her2k(alpha, blas::conj(A), B, beta, blas::upper(C2));
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (A left htrans, C upper):\n" << print_mat(C1) << '\n'
    		<< "using blas (A left htrans, C upper):\n" << print_mat(C2) << '\n'
    		<< '\n';
    }
    {
      matrix C1(alpha*ublas::prod(ublas::trans(ublas::conj(A)), B)+
		std::conj(alpha)*ublas::prod(ublas::trans(ublas::conj(B)), A)+
		beta*C);
      for (size_type j=0; j<n; ++j)
    	for (size_type i=0; i<j; ++i)
    	  C1(i, j)=C(i, j);
      matrix C2(C);
      blas::her2k(alpha, blas::conj(A), B, beta, blas::lower(C2));
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (A left htrans, C lower):\n" << print_mat(C1) << '\n'
    		<< "using blas (A left htrans, C lower):\n" << print_mat(C2) << '\n'
    		<< '\n';
    }
  }
  return EXIT_SUCCESS;
}


#ifndef _NCTX_PY_PTHS_
#define _NCTX_PY_PTHS_
//~ #include <boost/graph/property_maps/constant_property_map.hpp>
//~ #include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>

namespace nctx { namespace python {
  template<bool d>
  class SpDummy{};

  template <bool directed>
  inline py::list build_path( typename GraphContainer<directed>::vertex start,
                          typename GraphContainer<directed>::vertex goal,
                          PropertyMapValueHolder<size_t,directed>& predecessors,
                          size_t pred_undef){
    using GraphC = GraphContainer<directed>;
    using Vertex = typename GraphC::vertex;
    py::list path;
    // extract path
    Vertex current = goal;
    //~ std::cout << "extract path: " << start << " -> " << goal << std::endl;
    do {
        auto const pred = predecessors.get_elem(current);

        //~ std::cout << "extract path: " << current << " <- " << pred << " path length now " << py::len(path) << std::endl;

        if(pred == pred_undef || current == pred)
            break;

        current = pred;

        path.append(current);
    } while(current != start);

    if(py::len(path) > 1){
      path.reverse();
      path.append(goal);
    }
    return path;
  }

  template <bool directed>
  inline void wrap_pathalgs(){
    using PMap = PropertyMapValueHolder<size_t,directed>;
    using PMatrix = PropertyMapVectorHolder<size_t, directed>;
    using GraphC = GraphContainer<directed>;
    using Vertex = typename GraphC::vertex;
    using Edge = typename GraphC::edge;

    std::string apsp_doc = std::string("Obtain all-pairs-shortest-paths using Dijkstra's algorithm taking into account contextual constraints.\n\nThis is basically a wrapper around :py:func:`dijkstra_ctx <nctx.") + (directed ? std::string(""):std::string("un")) + std::string("directed.AlgPaths.dijkstra_ctx>`. Please see the mentioned function for more information regarding the contextual constraints.\n\nArgs:\n    g (Graph): The Graph object\n    decision_fct (function): The decision function enforcing contextual constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``\n     distances (PropertyMapVecULong): matrix in which distances will be written\n\nExample:\n    >>> distmap = PropertyMapVecULong(g, 'distances_ctx')\n    >>> AlgPaths.dijkstra_apsp_ctx(g,lambda _start,_current,_next: (True), distmap)\n\n\n");
    std::string apsp_ret_doc = std::string("Obtain all-pairs-shortest-paths using Dijkstra's algorithm taking into account contextual constraints.\n\nThis is basically a wrapper around :py:func:`dijkstra_ctx <nctx.") + (directed ? std::string(""):std::string("un")) + std::string("directed.AlgPaths.dijkstra_ctx>`. Please see the mentioned function for more information regarding the contextual constraints.\n\nArgs:\n    g (Graph): The Graph object\n    decision_fct (function): The decision function enforcing contextual constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``\n\nReturns:\n    PropertyMapVecULong: matrix of distances named apsp_constraint\n\nExample:\n    >>> distmap = AlgPaths.dijkstra_apsp_ctx(g,lambda _start,_current,_next: (True))\n\n\n");

    std::string fpath_doc = std::string("Obtain a shortest paths from a start vertex to a target vertex using Dijkstra's algorithm taking into account contextual constraints.\n\nThis is basically a wrapper around :py:func:`dijkstra_ctx <nctx.") + (directed ? std::string(""):std::string("un")) + std::string("directed.AlgPaths.dijkstra_ctx>`. Please see the mentioned function for more information regarding the contextual constraints.\n\nArgs:\n    g (Graph): The Graph object\n    decision_fct (function): The decision function enforcing contextual constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``\n     start (int): Start vertex\n    target (int): Target vertex\n\nReturns:\n    list: A list of vertex indices describing the shortest path from start to target.\n\nExample:\n    >>> start = 3\n    >>> target = 10\n    >>> path = AlgPaths.find_path_ctx(g,start,target,lambda _start,_current,_next: (True))\n\n\n");

    py::class_<SpDummy<directed>>("AlgPaths", py::no_init)
      .def("dijkstra_ctx", +[](GraphC& gc, Vertex start, py::object fct_decision, PMap& distances) {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        auto dummy_pred = b::dummy_property_map();
        dijkstra_ctx_dispatch(g, start,
                              b::weight_map(weight_map)
                              .distance_map(distances.get_map()),
                              distances.get_map(),
                              weight_map,
                              dummy_pred,
                              0,
                              lmbd_decision,
                              dist_inf);
      },(py::arg("g"), py::arg("s"), py::arg("decision_fct"), py::arg("distances")), "Obtain shortest paths with dynamic contextual constraints.\n\n\n\nSingle-source shortest paths using Dijkstra's algorithm taking into account contextual constraints. Enforcement of constraints is the task of the given user-defined function.\n\nThe function enforcing contextual constraints is evaluated at each node during shortest path discovery. The function needs to evaluate to ``true`` or ``false`` allowing an edge to be visited or not. As parameters, the function needs to accept the starting node of path traversal, the current node, and the descending node in question. If the function returns False, the descending node is not being visited. Passing the start vertex is unnecessary here. However, the signature is the same as for the other functions for usability reasons.\n\nThe three nodes are passed as indices allowing for access of (external) attribute and other associated information.\n\nNote that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.\n\nIf the decision function simply returns True all the time, the set of shortest paths is the unaltered set of shortest paths expected by the classical Dijkstra-implementation.\n\n\n\nArgs:\n    g The graph object\n    s The source vertex\n    decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> bool``, i.e. the function expects three vertex IDs for the start, the current, and the next vertex. It must evaluate to ``bool``. Passing the start vertex is unnecessary here. However, the signature is the same as for the other functions for usability reasons.\n    distances (PropertyMapULong): map in which distances will be written\n\nExample:\n    >>> start = 3\n    >>> distmap = PropertyMapULong(g, 'distances_ctx')\n    >>> AlgPaths.dijkstra_ctx(g, start, lambda _start,_current,_next: (True), distmap)\n\n\n")

      .def("dijkstra_ctx", +[](GraphC& gc, Vertex start, py::object fct_decision) -> PMap {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        auto g = gc.get_graph();
        PMap distances(gc, "dijkstra_constraint");
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        auto dummy_pred = b::dummy_property_map();
        dijkstra_ctx_dispatch(g, start,
                              b::weight_map(weight_map)
                              .distance_map(distances.get_map()),
                              distances.get_map(),
                              weight_map,
                              dummy_pred,
                              0,
                              lmbd_decision,
                              dist_inf);
        return distances;
      },(py::arg("g"), py::arg("s"), py::arg("decision_fct")), "Obtain shortest paths with dynamic contextual constraints.\n\n\n\nSingle-source shortest paths using Dijkstra's algorithm taking into account contextual constraints. Enforcement of constraints is the task of the given user-defined function.\n\nThe function enforcing contextual constraints is evaluated at each node during shortest path discovery. The function needs to evaluate to ``true`` or ``false`` allowing an edge to be visited or not. As parameters, the function needs to accept the starting node of path traversal, the current node, and the descending node in question. If the function returns False, the descending node is not being visited. Passing the start vertex is unnecessary here. However, the signature is the same as for the other functions for usability reasons.\n\nThe three nodes are passed as indices allowing for access of (external) attribute and other associated information.\n\nNote that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.\n\nIf the decision function simply returns True all the time, the set of shortest paths is the unaltered set of shortest paths expected by the classical Dijkstra-implementation.\n\n\n\nArgs:\n    g The graph object\n    s The source vertex\n    decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> bool``, i.e. the function expects three vertex IDs for the start, the current, and the next vertex. It must evaluate to ``bool``. Passing the start vertex is unnecessary here. However, the signature is the same as for the other functions for usability reasons.\n\n\nReturns:\n    PropertyMapULong: map containing distances named dijkstra_constraint\n\nExample:\n    >>> start = 3\n    >>> distmap = AlgPaths.dijkstra_ctx(g, start, lambda _start,_current,_next: (True))\n\n\n")

      .def("dijkstra_apsp_ctx", +[](GraphC& gc, py::object fct_decision, PMatrix& dmap) {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);

        for(auto start : make_iterator_range(vertices(g))){
          std::vector<size_t> distances(b::num_vertices(g), 0);
          auto distance_map = make_iterator_property_map(distances.begin(), b::get(b::vertex_index, g));
          auto dummy_pred = b::dummy_property_map();
          dijkstra_ctx_dispatch(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distance_map),
                                distance_map,
                                weight_map,
                                dummy_pred,
                                0,
                                lmbd_decision,
                                dist_inf);

          for(auto u : make_iterator_range(b::vertices(g)))
            dmap.set_elem(start,u,distances[u]);
        }
      },(py::arg("g"), py::arg("decision_fct"), py::arg("distances")), apsp_doc.c_str())

      .def("dijkstra_apsp_ctx", +[](GraphC& gc, py::object fct_decision) -> PMatrix {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        PMatrix dmap(gc, "apsp_constraint");
        auto dummy_pred = b::dummy_property_map();

        for(auto start : make_iterator_range(vertices(g))){
          std::vector<size_t> distances(b::num_vertices(g), 0);
          auto distance_map = make_iterator_property_map(distances.begin(), b::get(b::vertex_index, g));
          dijkstra_ctx_dispatch(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distance_map),
                                distance_map,
                                weight_map ,
                                dummy_pred,
                                0,
                                lmbd_decision,
                                dist_inf);

          for(auto u : make_iterator_range(b::vertices(g)))
            dmap.set_elem(start,u,distances[u]);
        }
        return dmap;
      },(py::arg("g"), py::arg("decision_fct")), apsp_ret_doc.c_str())

      .def("find_path_ctx", +[](GraphC& gc, Vertex start, Vertex goal, py::object fct_decision) -> py::list {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        const int pred_init = (std::numeric_limits<int>::min)();
        auto g = gc.get_graph();
        PMap distances(gc, "dijkstra_constraint");
        PMap predecessors(gc, "predecessors_constraint", pred_init);
        auto weight_map = boost::make_constant_property<Edge>(1.0);

        dijkstra_ctx_dispatch(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distances.get_map())
                                .predecessor_map(predecessors.get_map()),
                                distances.get_map(),
                                weight_map,
                                predecessors.get_map(),
                                pred_init,
                                lmbd_decision,
                                dist_inf);

        // extract path
        return build_path(start, goal, predecessors, pred_init);
      },(py::arg("g"), py::arg("start"), py::arg("target"), py::arg("decision_fct")), fpath_doc.c_str())

      .def("dijkstra", +[](GraphC& gc, Vertex start, PMap& distances) {
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        boost::dijkstra_shortest_paths_no_color_map(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distances.get_map()));
      },(py::arg("g"), py::arg("start"), py::arg("distances")), "Conventional single-source shortest paths discovery using Dijkstra's algorithm.\n\nArgs:\n    g (Graph): The Graph object\n    start (int): Source vertex\n    distances (PropertyMapULong): PropertyMap in which distances will be written.\n")

      .def("dijkstra", +[](GraphC& gc, Vertex start) -> PMap {
        auto g = gc.get_graph();
        PMap distances(gc, "dijkstra");
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        boost::dijkstra_shortest_paths_no_color_map(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distances.get_map()));
        return distances;
      },(py::arg("g"), py::arg("start")), "Conventional single-source shortest paths discovery using Dijkstra's algorithm.\n\nArgs:\n    g (Graph): The Graph object\n    start (int): Source vertex\n\nReturns:\n    PropertyMapULong: PropertyMap with distances.\n")

      .def("find_path", +[](GraphC& gc, Vertex start, Vertex goal) -> py::list {
        auto g = gc.get_graph();
        const int pred_init = (std::numeric_limits<int>::min)();
        PMap distances(gc, "dijkstra");
        PMap predecessors(gc, "predecessors", pred_init);
        auto weight_map = boost::make_constant_property<Edge>(1.0);

        boost::dijkstra_shortest_paths_no_color_map(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distances.get_map())
                                .predecessor_map(predecessors.get_map()));

        return build_path(start, goal, predecessors, pred_init);
      }, (py::arg("g"), py::arg("start"), py::arg("target")), "Conventional shortest path discovery using Dijkstra's algorithm.\n\nArgs:\n    g (Graph): The Graph object\n    start (int): Source vertex\n    target (int): Target vertex\n\nReturns:\n    list: A list of vertex indices describing the shortest path from start to target.\n");
  }

}} //nproc::python

#endif


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#include <vector>

#include "../base.hpp"

#include "../util/matrix_3_x_3.hpp"
#include "../util/vec.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/LU"
    #include "../externals/Eigen/Geometry"
#else
    #include <Eigen/LU>
    #include <Eigen/Geometry>
#endif

#endif

#ifndef PIC_EIGEN_UTIL
#define PIC_EIGEN_UTIL

#ifndef PIC_DISABLE_EIGEN
namespace Eigen {
    typedef Matrix<float,  3, 4> Matrix34f;
    typedef Matrix<double, 3, 4> Matrix34d;
}
#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief readMatrix34dFromFile
 * @param nameFile
 * @return
 */
PIC_INLINE Eigen::Matrix34d readMatrix34dFromFile(std::string nameFile)
{
    Eigen::Matrix34d mat;

    FILE *file = fopen(nameFile.c_str(), "r");

    if(file == NULL) {
        return mat;
    }

    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 4; j++) {
            float val;
            fscanf(file, "%f", &val);
            mat(i, j) = double(val);
        }
    }

    fclose(file);

    return mat;
}

/**
 * @brief writeMatrix34dToFile
 * @param nameFile
 * @param mat
 * @return
 */
PIC_INLINE bool writeMatrix34dToFile(std::string nameFile, Eigen::Matrix34d &mat)
{
    FILE *file = fopen(nameFile.c_str(), "w");

    if(file == NULL) {
        return false;
    }

    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 4; j++) {
            fprintf(file, "%f ", mat(i, j));
        }

        if(i < 2) {
            fprintf(file, "\n");
        }
    }

    fclose(file);

    return true;
}

/**
 * @brief DiagonalMatrix creates a diagonal matrix.
 * @param D a vector of size 3.
 * @return It returns a diagonal matrix.
 */
PIC_INLINE Eigen::Matrix3d DiagonalMatrix(Eigen::Vector3d D)
{
    Eigen::Matrix3d ret;

    ret.setZero();
    ret(0, 0) = D[0];
    ret(1, 1) = D[1];
    ret(2, 2) = D[2];

    return ret;
}

/**
 * @brief getDiagonalFromMatrix
 * @param mat
 * @return
 */
PIC_INLINE Eigen::Vector3d getDiagonalFromMatrix(Eigen::Matrix3d &mat)
{
    Eigen::Vector3d D;

    D[0] = mat(0, 0);
    D[1] = mat(1, 1);
    D[2] = mat(2, 2);

    return D;
}

/**
 * @brief getSquareMatrix
 * @param mat
 * @return
 */
PIC_INLINE Eigen::Matrix3d getSquareMatrix(Eigen::Matrix34d &mat)
{
    Eigen::Matrix3d ret;
    ret(0, 0) = mat(0, 0);
    ret(0, 1) = mat(0, 1);
    ret(0, 2) = mat(0, 2);

    ret(1, 0) = mat(1, 0);
    ret(1, 1) = mat(1, 1);
    ret(1, 2) = mat(1, 2);

    ret(2, 0) = mat(2, 0);
    ret(2, 1) = mat(2, 1);
    ret(2, 2) = mat(2, 2);

    return ret;
}

/**
 * @brief getLastColumn
 * @param mat
 * @return
 */
PIC_INLINE Eigen::Vector3d getLastColumn(Eigen::Matrix34d &mat)
{
    Eigen::Vector3d ret;

    ret[0] = mat(0, 3);
    ret[1] = mat(1, 3);
    ret[2] = mat(2, 3);

    return ret;
}

/**
 * @brief addOne
 * @param x
 * @return
 */
PIC_INLINE Eigen::Vector3f addOne(Eigen::Vector2f &x)
{
    return Eigen::Vector3f(x[0], x[1], 1.0f);
}

/**
 * @brief addOne
 * @param x
 * @return
 */
PIC_INLINE Eigen::Vector3d addOne(Eigen::Vector2d &x)
{
    return Eigen::Vector3d(x[0], x[1], 1.0);
}

/**
 * @brief addOne
 * @param x
 * @return
 */
PIC_INLINE Eigen::Vector4d addOne(Eigen::Vector3d &x)
{
    return Eigen::Vector4d(x[0], x[1], x[2], 1.0);
}

/**
 * @brief printfVet3d
 * @param x
 */
PIC_INLINE void printfVet3d(Eigen::Vector3d &x)
{
    printf("%f %f %f\n", x[0], x[1], x[2]);
}

/**
 * @brief printf
 * @param mat
 */
PIC_INLINE void printfMat(Eigen::MatrixXd mat)
{
    for(int i = 0; i < mat.rows(); i++){
        for(int j = 0; j < mat.cols(); j++){
            printf("%3.3f ", mat(i, j));
        }
        printf("\n");
    }
}

/**
 * @brief printf
 * @param mat
 */
PIC_INLINE void printfMat(Eigen::Matrix3f &mat)
{
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            printf("%f ", mat(i, j));
        }
        printf("\n");
    }
}

/**
  * @brief printf
  * @param mat
  */
PIC_INLINE void printfMat34d(Eigen::Matrix34d &mat)
{
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 4; j++){
            printf("%.9f ", mat(i, j));
        }
        printf("\n");
    }
}

/**
 * @brief fprintf
 * @param mat
 */
PIC_INLINE void fprintfMat(Eigen::MatrixXd &mat, std::string name)
{
    FILE *file = fopen(name.c_str(), "w");
    for(int i = 0; i < mat.rows(); i++){
        for(int j = 0; j < mat.cols(); j++){
            fprintf(file, "%.9f ", mat(i, j));
        }
        fprintf(file, "\n");
    }
    fclose(file);
}

/**
 * @brief printf
 * @param mat
 */
PIC_INLINE void printfMat(Eigen::Matrix3d &mat)
{
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            printf("%f ", mat(i, j));
        }
        printf("\n");
    }
}

/**
 * @brief getShiftScaleMatrix computes a shifting and scaling matrix
 * @param info is an array with the center (0 and 1) a scaling factor (3)
 * @return It returns a scaling and shifting matrix.
 */
PIC_INLINE Eigen::Matrix3d getShiftScaleMatrix(Eigen::Vector3f &info)
{
    Eigen::Matrix3d ret;

    double cX = info[0];
    double cY = info[1];
    double s  = 1.0 / info[2];

    ret(0,0) = s;   ret(0,1) = 0.0; ret(0,2) = -cX / info[2];
    ret(1,0) = 0.0; ret(1,1) = s;   ret(1,2) = -cY / info[2];
    ret(2,0) = 0.0; ret(2,1) = 0.0; ret(2,2) = 1.0;

    return ret;
}

/**
 * @brief CrossProduct computes a cross product matrix from a vector.
 * @param t a translation vector
 * @return It returns a cross product matrix.
 */
PIC_INLINE Eigen::Matrix3d CrossProduct(Eigen::Vector3d &t)
{
    Eigen::Matrix3d ret;
    ret(0, 0) =  0.0;  ret(0, 1) = -t[2]; ret(0, 2) =  t[1];
    ret(1, 0) =  t[2]; ret(1, 1) =  0.0;  ret(1, 2) = -t[0];
    ret(2, 0) = -t[1]; ret(2, 1) =  t[0]; ret(2, 2) =  0.0;
    return ret;
}

/**
 * @brief rigidTransform computes a rigidi transformation in 3D.
 * @param point is the point to be transformed.
 * @param R is a rotation matrix 3x3.
 * @param t is a translation vector.
 * @return
 */
PIC_INLINE Eigen::Vector3d rigidTransform(Eigen::Vector3d &point, Eigen::Matrix3d &R, Eigen::Vector3d &t)
{
    return R * point + t;
}

/**
 * @brief RotationMatrixRefinement
 * @param R
 * @return
 */
PIC_INLINE Eigen::Matrix3d RotationMatrixRefinement(Eigen::Matrix3d &R)
{
    Eigen::Quaternion<double> reg(R);

    return reg.toRotationMatrix();
}

/**
 * @brief MatrixConvert converts a matrix from a Eigen::Matrix3f representation
 * into a Matrix3x3 representation.
 * @param mat is an Eigen 3x3 matrix.
 * @return It returns a Matrix3x3 with values from mat.
 */
PIC_INLINE Matrix3x3 MatrixConvert(Eigen::Matrix3f &mat)
{
    Matrix3x3 mtx;
    mtx.data[0] = mat(0, 0);
    mtx.data[1] = mat(0, 1);
    mtx.data[2] = mat(0, 2);

    mtx.data[3] = mat(1, 0);
    mtx.data[4] = mat(1, 1);
    mtx.data[5] = mat(1, 2);

    mtx.data[6] = mat(2, 0);
    mtx.data[7] = mat(2, 1);
    mtx.data[8] = mat(2, 2);

    return mtx;
}

/**
 * @brief MatrixConvert converts a matrix from a Eigen::Matrix3f representation
 * into a Matrix3x3 representation.
 * @param mat is an Eigen 3x3 matrix.
 * @return It returns a Matrix3x3 with values from mat.
 */
PIC_INLINE Matrix3x3 MatrixConvert(Eigen::Matrix3d &mat)
{
    Matrix3x3 mtx;
    mtx.data[0] = float(mat(0, 0));
    mtx.data[1] = float(mat(0, 1));
    mtx.data[2] = float(mat(0, 2));

    mtx.data[3] = float(mat(1, 0));
    mtx.data[4] = float(mat(1, 1));
    mtx.data[5] = float(mat(1, 2));

    mtx.data[6] = float(mat(2, 0));
    mtx.data[7] = float(mat(2, 1));
    mtx.data[8] = float(mat(2, 2));

    return mtx;
}

/**
 * @brief getLinearArray
 * @param mat
 * @return
 */
PIC_INLINE float *getLinearArrayFromMatrix(Eigen::Matrix3d &mat)
{
    int n = int(mat.cols() * mat.rows());

    float *ret = new float[n];
    int c = 0;
    for(int i = 0; i < mat.rows(); i++) {
        for(int j = 0; j < mat.cols(); j++) {
            ret[c] = float(mat(i, j));
            c++;
        }
    }

    return ret;
}

/**
 * @brief getLinearArray
 * @param mat
 * @return
 */
PIC_INLINE float *getLinearArrayFromMatrix(Eigen::Matrix3f &mat)
{
    int n = int(mat.cols() * mat.rows());

    float *ret = new float[n];
    int c = 0;
    for(int i = 0; i < mat.rows(); i++) {
        for(int j = 0; j < mat.cols(); j++) {
            ret[c] = mat(i, j);
            c++;
        }
    }

    return ret;
}

/**
 * @brief getMatrixFromLinearArray
 * @param array
 * @param rows
 * @param cols
 * @return
 */
PIC_INLINE Eigen::MatrixXf getMatrixfFromLinearArray(float *array, int rows, int cols)
{
    Eigen::MatrixXf ret = Eigen::MatrixXf(rows, cols);

    int c = 0;
    for(int i = 0; i < rows; i++) {
        for(int j = 0; j < cols; j++) {
            ret(i, j) = array[c];
            c++;
        }
    }
    return ret;
}

/**
 * @brief getMatrixFromLinearArray
 * @param array
 * @param rows
 * @param cols
 * @return
 */
PIC_INLINE Eigen::MatrixXd getMatrixdFromLinearArray(float *array, int rows, int cols)
{
    Eigen::MatrixXd ret = Eigen::MatrixXd(rows, cols);

    int c = 0;
    for(int i = 0; i < rows; i++) {
        for(int j = 0; j < cols; j++) {
            ret(i, j) = array[c];
            c++;
        }
    }
    return ret;
}

/**
 * @brief getMatrix3dFromLinearArray
 * @param array
 * @return
 */
PIC_INLINE Eigen::Matrix3d getMatrix3dFromLinearArray(float *array)
{
    Eigen::Matrix3d ret;

    int c = 0;
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++) {
            ret(i, j) = array[c];
            c++;
        }
    }
    return ret;
}

/**
 * @brief MatrixConvert
 * @param mat
 * @return
 */
PIC_INLINE Eigen::Matrix3f MatrixConvert(Matrix3x3 &mat)
{
    Eigen::Matrix3f mtx;
    mtx(0, 0) = mat.data[0];
    mtx(0, 1) = mat.data[1];
    mtx(0, 2) = mat.data[2];

    mtx(1, 0) = mat.data[3];
    mtx(1, 1) = mat.data[4];
    mtx(1, 2) = mat.data[5];

    mtx(2, 0) = mat.data[6];
    mtx(2, 1) = mat.data[7];
    mtx(2, 2) = mat.data[8];

    return mtx;
}

/**
 * @brief ComputeNormalizationTransform
 * @param points
 * @return
 */
PIC_INLINE Eigen::Vector3f ComputeNormalizationTransform(std::vector< Eigen::Vector2f > &points)
{
    Eigen::Vector3f ret;

    if(points.size() < 2) {
        return ret;
    }

    ret[0] = 0.0f;
    ret[1] = 0.0f;

    for(unsigned int i = 0; i < points.size(); i++) {
        ret[0] += points[i][0];
        ret[1] += points[i][1];
    }

    float n = float(points.size());
    ret[0] /= n;
    ret[1] /= n;

    ret[2] = 0.0;
    for(unsigned int i = 0; i < points.size(); i++) {

        float dx = points[i][0] - ret[0];
        float dy = points[i][1] - ret[1];

        ret[2] += sqrtf(dx * dx + dy * dy);
    }

    ret[2] = ret[2] / n / sqrtf(2.0f);

    return ret;
}

/**
 * @brief convertFromEigenToVec
 * @param x
 */
PIC_INLINE Vec2i convertFromEigenToVec(Eigen::Vector2i &x)
{
    return Vec2i(x[0], x[1]);
}

#endif

}

#endif // PIC_EIGEN_UTIL


/// My typedefs
#include <inttypes.h>
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>

typedef int32_t    node_t;
typedef int32_t    edge_t;
typedef int64_t    cost_t;

/// From STL library
#include <fstream>

#include <vector>
using std::vector;

#include <string>

using std::pair;
using std::make_pair;

/// Lemon Graph Library
#include <lemon/smart_graph.h>
using lemon::SmartDigraph;

#include <lemon/adaptors.h>
#include <lemon/concepts/maps.h>
#include <lemon/dijkstra.h>
#include <lemon/path.h>
typedef SmartDigraph::Arc  Arc;
typedef SmartDigraph::Node Node;
typedef SmartDigraph::ArcMap<cost_t>   LengthMap;

#include <lemon/fib_heap.h>
typedef SmartDigraph::NodeMap<int>  NodeMap;
typedef lemon::FibHeap<cost_t, NodeMap> FibonacciHeap;

/// Boost Timer
#include <boost/progress.hpp>
using boost::timer;

using namespace boost;

/// Read input data, build graph, and run Dijkstra
cost_t runDijkstra( char* argv[] ) {
   /// Read instance from the OR-lib
   std::ifstream infile(argv[1]);
   if (!infile)
      exit ( EXIT_FAILURE );

   int n;     /// Number of variables
   int m;     /// Number of constraints

   // reads file of the form
   // #nodes #edges
   // e_1 = v_i v_j cost[e_m]
   // ..
   // e_m = v_i v_j cost[e_m]

   /// Read the first line
   infile >> n >> m;
   fprintf(stdout,"n %d, m %d\t", n, m);
   /// Build the graph
   SmartDigraph G;
   G.reserveNode(n);
   G.reserveArc(m);
   vector<Node> vs;
   vs.reserve(n);
   for ( int i = 0; i < n; ++i )
      vs.push_back( G.addNode() );

   int v, w;
   cost_t c;
   cost_t T_dist;
   LengthMap    C(G);
   for ( int i = 0; i < m; i++ ) {
      infile >> v >> w >> c;
      Arc a;
      a = G.addArc(vs[v-1], vs[w-1]);
      C[a] = c;
   }

   timer TIMER;
   for ( int i = 0; i < 50; ++i ) {
      double t0 = TIMER.elapsed();
      Node S = vs[i];
      Node T = vs[n-1-i];
      //NodeMap heap_cross_ref(G);
      //FibonacciHeap heap(heap_cross_ref);
      //lemon::Dijkstra<SmartDigraph, LengthMap>::SetHeap<FibonacciHeap, NodeMap>::Create spp(G, C);
      //spp.heap( heap, heap_cross_ref );
      lemon::Dijkstra<SmartDigraph, LengthMap> spp(G, C);
      spp.run(S,T);
      T_dist = spp.dist(T);
      fprintf(stdout,"Time %.4f Cost %"PRId64"\n", TIMER.elapsed()-t0, T_dist);
   }
   fprintf(stdout,"Tot %.4f\n", TIMER.elapsed());

   return T_dist;
}

/// Main function
int
main (int argc, char **argv)
{
   if ( argc != 2 ) {
      fprintf(stdout, "usage: ./dijkstra <filename>\n");
      exit ( EXIT_FAILURE );
   }
   /// Measure overall time
   timer TIMER;
   /// Invoke the different Dijkstra algorithm implementations
   cost_t T_dist = runDijkstra(argv);
   /// Print basic figures
   fprintf(stdout,"Cost %"PRId64" - Time %.3f\n", T_dist, TIMER.elapsed());

   return 0;
}


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

#define BOOST_MATH_MAX_SERIES_ITERATION_POLICY 10000000

#include "mp_t.hpp"
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <map>
#include <boost/math/tools/test_data.hpp>
#include <boost/random.hpp>

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;

struct hypergeometric_0f2_gen
{
   mp_t operator()(mp_t b1, mp_t b2, mp_t z)
   {
      mp_t result = 0;
      mp_t abs_result = 0;
      mp_t term = 1;
      mp_t k = 0;

      do
      {
         result += term;
         abs_result += fabs(term);
         if (fabs(result) * boost::math::tools::epsilon<mp_t>() > fabs(term))
            break;
         ++k;
         term /= b1++;
         term /= b2++;
         term /= k;
         term *= z;
      } while (true);
      //
      // check precision:
      //
      if (abs_result * boost::math::tools::epsilon<mp_t>() / fabs(result) > 1e-40)
         throw std::domain_error("Unable to calculate result");

      std::cout << b1 << " " << b2 << " " << z << " " << result << std::endl;
      return result;
   }
};

int main(int, char* [])
{
   parameter_info<mp_t> arg1, arg2, arg3, arg4;
   test_data<mp_t> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for 2F0:\n";

   std::string line;
   bool cont = true;

   while (cont)
   {
      float range;
      std::cout << "Enter the range to calculate over for b1 and b2 (single value, range will be -x to x): ";
      std::cin >> range;

      float z_range;
      std::cout << "Enter the range to calculate over for z (single value, range will be -x to x): ";
      std::cin >> z_range;

      int num_spots;
      std::cout << "Enter how many test points to calculate: ";
      std::cin >> num_spots;

      std::vector<mp_t> v;
      random_ns::mt19937 rnd;
      random_ns::uniform_real_distribution<float> ur_a(-range, range);
      random_ns::uniform_real_distribution<float> ur_z(-z_range, z_range);

      do
      {
         mp_t b1 = ur_a(rnd);
         mp_t b2 = ur_a(rnd);
         mp_t z = ur_z(rnd);

         arg1 = make_single_param(b1);
         arg2 = make_single_param(b2);
         arg3 = make_single_param(z);
         data.insert(hypergeometric_0f2_gen(), arg1, arg2, arg3);
      } while (num_spots--);

      std::cout << "Any more data?";
      std::cin >> cont;

   }

   std::cout << "Enter name of test data file [default=hypergeometric_0f2.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "hypergeometric_0f2.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, line.c_str());

   return 0;
}


//  Copyright (c) 2006 Xiaogang Zhang, 2015 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to handle
//  types longer than 80-bit reals.
//  Updated 2015 to use Carlson's latest methods.
//
#ifndef BOOST_MATH_ELLINT_RF_HPP
#define BOOST_MATH_ELLINT_RF_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/ellint_rc.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>

// Carlson's elliptic integral of the first kind
// R_F(x, y, z) = 0.5 * \int_{0}^{\infty} [(t+x)(t+y)(t+z)]^{-1/2} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost
{
namespace math
{
namespace detail
{

template <typename T, typename Policy>
T ellint_rf_imp(T x, T y, T z, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math;
    using std::swap;

    static const char* function = "boost::math::ellint_rf<%1%>(%1%,%1%,%1%)";

    if (x < 0 || y < 0 || z < 0)
    {
        return policies::raise_domain_error<T>(
            function,
            "domain error, all arguments must be non-negative, "
            "only sensible result is %1%.",
            std::numeric_limits<T>::quiet_NaN(), pol);
    }
    if (x + y == 0 || y + z == 0 || z + x == 0)
    {
        return policies::raise_domain_error<T>(
            function,
            "domain error, at most one argument can be zero, "
            "only sensible result is %1%.",
            std::numeric_limits<T>::quiet_NaN(), pol);
    }
    //
    // Special cases from http://dlmf.nist.gov/19.20#i
    //
    if (x == y)
    {
        if (x == z)
        {
            // x, y, z equal:
            return 1 / sqrt(x);
        }
        else
        {
            // 2 equal, x and y:
            if (z == 0)
                return constants::pi<T>() / (2 * sqrt(x));
            else
                return ellint_rc_imp(z, x, pol);
        }
    }
    if (x == z)
    {
        if (y == 0)
            return constants::pi<T>() / (2 * sqrt(x));
        else
            return ellint_rc_imp(y, x, pol);
    }
    if (y == z)
    {
        if (x == 0)
            return constants::pi<T>() / (2 * sqrt(y));
        else
            return ellint_rc_imp(x, y, pol);
    }
    if (x == 0)
        swap(x, z);
    else if (y == 0)
        swap(y, z);
    if (z == 0)
    {
        //
        // Special case for one value zero:
        //
        T xn = sqrt(x);
        T yn = sqrt(y);

        while (fabs(xn - yn) >= 2.7 * tools::root_epsilon<T>() * fabs(xn))
        {
            T t = sqrt(xn * yn);
            xn = (xn + yn) / 2;
            yn = t;
        }
        return constants::pi<T>() / (xn + yn);
    }

    T xn = x;
    T yn = y;
    T zn = z;
    T An = (x + y + z) / 3;
    T A0 = An;
    T Q = pow(3 * boost::math::tools::epsilon<T>(), T(-1) / 8) *
          (std::max)((std::max)(fabs(An - xn), fabs(An - yn)), fabs(An - zn));
    T fn = 1;

    // duplication
    unsigned k = 1;
    for (; k < boost::math::policies::get_max_series_iterations<Policy>(); ++k)
    {
        T root_x = sqrt(xn);
        T root_y = sqrt(yn);
        T root_z = sqrt(zn);
        T lambda = root_x * root_y + root_x * root_z + root_y * root_z;
        An = (An + lambda) / 4;
        xn = (xn + lambda) / 4;
        yn = (yn + lambda) / 4;
        zn = (zn + lambda) / 4;
        Q /= 4;
        fn *= 4;
        if (Q < fabs(An))
            break;
    }
    // Check to see if we gave up too soon:
    policies::check_series_iterations<T>(function, k, pol);
    BOOST_MATH_INSTRUMENT_VARIABLE(k);

    T X = (A0 - x) / (An * fn);
    T Y = (A0 - y) / (An * fn);
    T Z = -X - Y;

    // Taylor series expansion to the 7th order
    T E2 = X * Y - Z * Z;
    T E3 = X * Y * Z;
    return (1 + E3 * (T(1) / 14 + 3 * E3 / 104) +
            E2 * (T(-1) / 10 + E2 / 24 - (3 * E3) / 44 - 5 * E2 * E2 / 208 +
                  E2 * E3 / 16)) /
           sqrt(An);
}

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type
    ellint_rf(T1 x, T2 y, T3 z, const Policy& pol)
{
    typedef typename tools::promote_args<T1, T2, T3>::type result_type;
    typedef typename policies::evaluation<result_type, Policy>::type value_type;
    return policies::checked_narrowing_cast<result_type, Policy>(
        detail::ellint_rf_imp(static_cast<value_type>(x),
                              static_cast<value_type>(y),
                              static_cast<value_type>(z), pol),
        "boost::math::ellint_rf<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type ellint_rf(T1 x, T2 y,
                                                                T3 z)
{
    return ellint_rf(x, y, z, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_ELLINT_RF_HPP


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2007, 2010.

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

#ifdef _MSC_VER
# pragma warning (disable : 4305) // 'initializing' : truncation from 'long double' to 'const eval_type'
# pragma warning (disable : 4244) //  conversion from 'long double' to 'const eval_type'
#endif

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

//[policy_eg_3

#include <boost/math/distributions/binomial.hpp>
using boost::math::binomial_distribution;

// Begin by defining a policy type, that gives the behaviour we want:

//using namespace boost::math::policies; or explicitly
using boost::math::policies::policy;

using boost::math::policies::promote_float;
using boost::math::policies::discrete_quantile;
using boost::math::policies::integer_round_nearest;

typedef policy<
   promote_float<false>, // Do not promote to double.
   discrete_quantile<integer_round_nearest> // Round result to nearest integer.
> mypolicy;
//
// Then define a new distribution that uses it:
typedef boost::math::binomial_distribution<float, mypolicy> mybinom;

//  And now use it to get the quantile:

int main()
{
   cout << "quantile(mybinom(200, 0.25), 0.05) is: " <<
      quantile(mybinom(200, 0.25), 0.05) << endl;
}

//]

/*

Output:

  quantile(mybinom(200, 0.25), 0.05) is: 40

*/


/**
 * @file Didson.cpp
 * @brief Projection functions for DIDSON
 * @author: Michael Kaess
 * @date: Aug 2014
 */

#include <vector>
#include <utility>

#include <opencv2/opencv.hpp>

#include <Eigen/Dense>

#include "Didson.h"

using namespace boost;
using namespace std;
using namespace isam;
using namespace Eigen;

namespace sonar {

const float degtorad = M_PI / 180.;
const float radtodeg = 180. / M_PI;

class DidsonConstants {
public:
  double bearingFov;
  double elevationFov;
  int numBearings;
  int numRanges;
  double startBase;
  std::vector<double> lengths;
  DidsonConstants() {
    bearingFov = 28.8 * degtorad; // 28.8 or 29? (both are given in the specs)
    elevationFov = 28. * degtorad; // 28 degrees with spreader lens (default without lens is 14 degrees)
    numBearings = 96;
    numRanges = 512;
#if 1 // todo: unknown if DIDSON in CW or XW mode
    // classic window mode (CW)
    startBase = 0.375;
    lengths.resize (4);
    lengths[0] = 1.125; lengths[1] = 2.25; lengths[2] = 4.5; lengths[3] = 9.;
#else
    // extended window mode (XW)
    startBase = 0.42;
    lengths = {1.25, 2.5, 5., 10.};
#endif
  }
};

const DidsonConstants consts;

pair<int, int> DidsonCartesian::bearingRange2Cartesian(int bearing,
    int range) {
  int pos = _invMap[range * consts.numBearings + bearing];
  int col = pos % image.cols;
  int row = (pos - col) / image.cols;
  // HACK: don't just return col, it needs to be flipped
  return make_pair(row, (image.cols - 1) - col);
}

// conversion of Matlab DIDSON code from Soundmetrics ---
int Didson::lensDistortion(int nbeams, double theta) {
  double* a = NULL;
  double factor = 0.0;
  double a48[4] = { 0.0015, -0.0036, 1.3351, 24.0976 };
  double a189[4] = { 0.0015, -0.0036, 1.3351, 24.0978 };
  double a96[4] = { 0.0030, -0.0055, 2.6829, 48.04 };
  double a381[4] = { 0.0030, -0.0055, 2.6829, 48.04 };

  switch (nbeams) {
  case 48:
    factor = 1.0;
    a = a48;
    break;
  case 189:
    factor = 4.026;
    a = a189;
    break;
  case 96:
    factor = 1.012;
    a = a96;
    break;
  case 381:
    factor = 4.05;
    a = a381;
    break;
  }
  return (int) round(
      factor
          * (a[0] * theta * theta * theta + a[1] * theta * theta + a[2] * theta
              + a[3] + 1));
}

// conversion of Matlab DIDSON code from Soundmetrics ---
/**
 * Calculates a map from cartesian to polar coordinates
 *
 * ixsize  - number of pixels in horizontal direction in image space
 * rmax    - maximum range in meters
 * rmin    - minimum range in meters
 * halffov - one-half of sector field of view in radians
 * nbins   - number of range bins in sample space
 */
pair<vector<int>, int> Didson::createMapping(int ixsize, double rmax,
    double rmin, double halffov, int nbeams, int nbins) {
  //double d2 = rmax*cos(halffov); // distance from point scan touches image boundary to origin
  double d3 = rmin * cos(halffov); // bottom of image frame to r,theta origin in meters
  double c1 = (consts.numRanges - 1) / (rmax - rmin); // precalculation of constants used in do loop below
  //double c2 = (nbeams-1)/(2*halffov);
  double gamma = ixsize / (2 * rmax * sin(halffov)); // Ratio of pixel number to position in meters

  int iysize = (int) (gamma * (rmax - d3) + 0.5); // number of pixels in image in vertical direction

  vector<int> map(ixsize * iysize); // Stores the index map

  // ix,iy   - coordinates of a pixel in image space
  for (int iy = 1; iy <= iysize; iy++) {
    for (int ix = 1; ix <= ixsize; ix++) {
      double x = ((ix - 1) - ixsize / 2) / gamma; // Convert from pixels to meters

      double z = 0.0;
      double y = rmax - (iy - 1) / gamma; // Convert from pixels to meters

      double r = sqrt(y * y + x * x + z * z); // Convert to polar coordinates
      double theta = radtodeg * atan2(x, y); // Theta is in degrees
      int binnum = (int) ((r - rmin) * c1 + 1.5); // the rangebin number
      int beamnum = lensDistortion(nbeams, theta); // Remove the lens distortation using empirical formula
      int pos = 0; // invalid == -1, note pos-1 below
      if ((beamnum > 0) && (beamnum <= nbeams) && (binnum > 0)
          && (binnum <= nbins)) {
        pos = (binnum - 1) * nbeams + beamnum;
      }
      map[(iy - 1) * ixsize + ix - 1] = pos - 1;
    }
  }
  return make_pair(map, iysize);
}

// create a Cartesian image suitable for texture mapping from the raw
// bearing/range measurements; also return a mask of valid image regions
shared_ptr<DidsonCartesian> Didson::getCartesian(int width, int widthTmp) const {
  // generate map for Cartesian image
  vector<int> map;
  int height;
  pair<vector<int>, int> tmp1 = createMapping(width, maxRange(), minRange(),
      consts.bearingFov * 0.5, numBearings(), numRanges());
  map = tmp1.first;
  height = tmp1.second;

  // avoid having to write out the inverse mapping function by creating
  // a map with sufficiently high resolution as a lookup table for the inverse map
  // not ideal, but works...
  vector<int> invMap(consts.numRanges * consts.numBearings);
  vector<int> mapTmp;
  int heightTmp;
  pair<vector<int>, int> tmp2 = createMapping(widthTmp, maxRange(), minRange(),
      consts.bearingFov * 0.5, numBearings(), consts.numRanges);
  mapTmp = tmp2.first;
  heightTmp = tmp2.second;

  int c = 0;
  for (int y = 0; y < heightTmp; y++) {
    for (int x = 0; x < widthTmp; x++) {
      int idx = mapTmp[c];
      if (idx != -1) {
        int icol = x * ((double) width / (double) widthTmp);
        int irow = y * ((double) height / (double) heightTmp);
        int i = irow * width + icol;
        invMap[idx] = i;
      }
      c++;
    }
  }

  shared_ptr<DidsonCartesian> cartesian(new DidsonCartesian(map, invMap));
  cartesian->image = cv::Mat(height, width, CV_8UC1);
  cartesian->mask = cv::Mat(height, width, CV_8UC1);
  for (int i = 0; i < width * height; i++) {
    if (map[i] == -1) {
      cartesian->image.data[i] = 0;
      cartesian->mask.data[i] = 0;
    } else {
      cartesian->image.data[i] = _image.data[map[i]];
      cartesian->mask.data[i] = 255;
    }
  }

  return cartesian;
}

// constructor taking a DIDSON frame
Didson::Didson(int windowStart, int windowLength,
               const isam::Pose3d& vehiclePose, double tiltRad, double rollRad,
               const unsigned char* data, bool transformFrame) :
    Sonar(consts.startBase * windowStart,
        consts.startBase * windowStart + consts.lengths[windowLength],
        consts.bearingFov, consts.elevationFov, consts.numBearings,
        consts.numRanges), _tiltRad(tiltRad), _rollRad(
        rollRad), _vehiclePose(vehiclePose) {
  if (windowStart < 1 || windowStart > 31) {
    cout << "ERROR: windowStart out of range" << endl;
    exit(1);
  }
  if (windowLength < 0 || windowLength > 3) {
    cout << "ERROR: windowLength out of range" << endl;
    exit(1);
  }

  _image = cv::Mat(consts.numRanges, consts.numBearings, CV_8UC1);
  if (data)
      memcpy(_image.data, data, consts.numRanges * consts.numBearings);

  // we assume DIDSON facing to the right (DVL in hull lock mode)
  // i.e. 90 degrees yaw and -90 degrees roll configuration + tilt and roll actuators
  if (transformFrame) {
      Pose3d didsonOffset(0, 0.2, 0, 0, 0, 0); // DIDSON is to the right of the DVL along y  // todo: check DIDSON offset
      Pose3d didsonActuators(0, 0, 0, M_PI * 0.5, tiltRad, rollRad - M_PI * 0.5);
      _didsonPose = vehiclePose.oplus(didsonOffset).oplus(didsonActuators);
  } else {
      _didsonPose = vehiclePose;
  }
}

Pose3d Didson::didsonHuls3Pose() const {
  return _didsonPose;
}

vector<Point3d> Didson::getFrustum() const {
  return Sonar::getFrustum(didsonHuls3Pose());
}

bool Didson::project(const isam::Point3d& point,
    isam::Point2d& projection) const {
  return Sonar::project(didsonHuls3Pose(), point, projection);
}

} /* namespace sonar */


/*
 * SPDX-FileCopyrightText: © 2018 Ambrosys GmbH
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <Core/Common/Geometry/Helper.h>

#include <boost/geometry/strategies/spherical/distance_haversine.hpp>

namespace Core::Common::Geometry {

std::pair<Point, double> project(Point const & p, Segment const & segment)
{
    using boost::geometry::add_point;
    using boost::geometry::dot_product;
    using boost::geometry::multiply_value;
    using boost::geometry::subtract_point;

    auto const & p1 = segment.first;
    auto const & p2 = segment.second;
    auto p1p = p;
    subtract_point(p1p, p1);

    auto p1p2 = p2;
    subtract_point(p1p2, p1);

    const double segmentLength_sqred = dot_product(p1p2, p1p2);
    const double normLengthProjection = dot_product(p1p, p1p2) / segmentLength_sqred;

    auto projected = p1p2;
    multiply_value(projected, normLengthProjection);
    add_point(projected, p1);

    return {projected, normLengthProjection};
}

std::pair<Point, double> snap(Point const & p, Segment const segment)
{
    auto const result = project(p, segment);
    auto const & projected = result.first;
    auto const normLengthProjection = result.second;

    if (normLengthProjection <= 0.0)
    {
        return {segment.first, normLengthProjection};
    }
    else if (normLengthProjection >= 1.0)
    {
        return {segment.second, normLengthProjection};
    }
    else
    {
        return {projected, normLengthProjection};
    }
}

Point reverseHaversine(Point const & p, double bearing, double distanceMeter)
{
    using std::atan2;
    using std::cos;
    using std::sin;

    auto lon1 = rad(p.lon());
    auto lat1 = rad(p.lat());
    auto angdist = distanceMeter / equatorRadiusMeter;
    auto theta = rad(bearing);
    auto lat2 = asin(sin(lat1) * cos(angdist) + cos(lat1) * sin(angdist) * cos(theta));
    auto lon2 = lon1 + atan2(sin(theta) * sin(angdist) * cos(lat1), cos(angdist) - sin(lat1) * sin(lat2));
    return Point{}.setLon(degree(lon2)).setLat(degree(lat2));
}

Box buffer(Box const & box, double distanceMeter)
{
    constexpr auto sqrtOf2 = 1.414213562373095049;
    auto min_corner = reverseHaversine(box.min_corner(), 225.0, sqrtOf2 * distanceMeter);
    auto max_corner = reverseHaversine(box.max_corner(), 45.0, sqrtOf2 * distanceMeter);
    return {min_corner, max_corner};
}

ValueType geoDistance(Point const & g1, Point const & g2)
{
    auto strategy = boost::geometry::strategy::distance::haversine<double>(equatorRadiusMeter);
    return boost::geometry::distance(g1, g2, strategy);
}

std::tuple<ValueType, Point, double> geoDistance(Point const & g1, Segment const & g2)
{
    auto const clipped = snap(g1, g2);
    return {geoDistance(g1, clipped.first), clipped.first, clipped.second};
}

double heading(Point const & p1, Point const & p2)
{
    auto deltaLon = rad(p2.lon()) - rad(p1.lon());
    auto cos_p2lat = cos(rad(p2.lat()));
    auto angle = atan2(sin(deltaLon) * cos_p2lat, cos(rad(p1.lat())) * sin(rad(p2.lat())) - sin(rad(p1.lat())) * cos_p2lat * cos(deltaLon));
    return normalizeAngle(degree(angle));
}

double heading(Segment const & segment)
{
    return heading(segment.first, segment.second);
}

double normalizeAngle(double angle)
{
    angle = std::fmod(angle, 360.0);
    return angle < 0.0 ? angle + 360.0 : angle;
}

double headingDiff(double a, double b)
{
    auto res = normalizeAngle(a - b);
    return res > 180.0 ? res - 360.0 : res;
}

double absHeadingDiff(double a, double b)
{
    return std::abs(headingDiff(a, b));
}

double reversedHeading(double heading)
{
    if (heading >= 180.0)
        return heading - 180.0;
    else
        return heading + 180.0;
}

LineStringProjectionResult projectOntoLineString(Point const & point, LineString const & lineString)
{
    assert(lineString.size() > 1);

    auto minSnapped = snap(point, Segment{lineString[0], lineString[1]});
    auto minDistance = geoDistance(point, minSnapped.first);
    size_t idx = 1;
    for (size_t i = 2; i < lineString.size(); ++i)
    {
        auto snapped = snap(point, Segment{lineString[i - 1u], lineString[i]});
        auto distance = geoDistance(point, snapped.first);

        if (distance < minDistance)
        {
            minSnapped = snapped;
            minDistance = distance;
            idx = i;
        }
    }

    auto result = LineStringProjectionResult{};
    result.projectionPoint = minSnapped.first;
    result.distanceToSegment = minDistance;
    result.segmentHeading = heading(Segment{lineString[idx - 1u], lineString[idx]});

    result.distanceAlongLineString = 0.0;
    for (size_t i = 1; i < idx; ++i)
    {
        result.distanceAlongLineString += geoDistance(lineString[i - 1u], lineString[i]);
    }
    result.distanceAlongLineString += geoDistance(lineString[idx - 1u], minSnapped.first);

    if (idx == 1 and minSnapped.second < 0.0)
    {
        result.offLineString = true;
    }
    else if (idx == lineString.size() - 1u and minSnapped.second > 1.0)
    {
        result.offLineString = true;
    }
    else
    {
        result.offLineString = false;
    }
    return result;
}

double relativeDistanceAlongLineString(LineString const & lineString, Point const & point)
{
    auto result = projectOntoLineString(point, lineString);
    return result.distanceAlongLineString / geoLength(lineString);
}

/**
 * Returns the angle between segment1 and segment2 in range [180°, -180°].
 */
double angleBetweenSegments(Segment const & segment1, Segment const & segment2)
{
    auto headingSeg1 = heading(segment1);
    auto headingSeg2 = heading(segment2);
    return headingDiff(headingSeg1, headingSeg2);
}

std::vector<Core::Common::Geometry::Point> trimmed(std::vector<Core::Common::Geometry::Point> const & points, double const trimLeft, double const trimRight)
{
    std::vector<Core::Common::Geometry::Point> newPoints;
    std::shared_ptr<Core::Common::Geometry::Point> newStart;
    std::shared_ptr<Core::Common::Geometry::Point> newEnd;
    size_t oldBegin = 0;
    size_t oldEnd = points.size();

    double a = 0.0;
    for (size_t i = 0; i < points.size() - 1; ++i)
    {
        a += Core::Common::Geometry::geoDistance(points[i], points[i + 1]);
        if (a >= trimLeft)
        {
            oldBegin = i + 1;
            if (a > trimLeft)
                newStart = std::make_shared<Core::Common::Geometry::Point>(
                    Core::Common::Geometry::reverseHaversine(points[i + 1], Core::Common::Geometry::heading(points[i + 1], points[i]), a - trimLeft));
            break;
        }
    }

    a = 0.0;
    for (size_t i = points.size() - 1; i > 0; --i)
    {
        a += Core::Common::Geometry::geoDistance(points[i], points[i - 1]);
        if (a >= trimRight)
        {
            oldEnd = i;
            if (a > trimRight)
            {
                newEnd = std::make_shared<Core::Common::Geometry::Point>(
                    Core::Common::Geometry::reverseHaversine(points[i - 1], Core::Common::Geometry::heading(points[i - 1], points[i]), a - trimRight));
            }
            break;
        }
    }

    if (newStart)
        newPoints.push_back(*newStart);
    newPoints.insert(newPoints.end(), points.begin() + oldBegin, points.begin() + oldEnd);
    if (newEnd)
        newPoints.push_back(*newEnd);

    return newPoints;
}

}  // namespace Core::Common::Geometry