//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_BLAS_LEVEL2_GBMV_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_LEVEL2_GBMV_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/bandwidth.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/blas/detail/default_order.hpp>
#include <boost/numeric/bindings/has_band_array.hpp>
#include <boost/numeric/bindings/has_linear_array.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/trans_tag.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>

//
// The BLAS-backend is selected by defining a pre-processor variable,
//  which can be one of
// * for CBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
// * for CUBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
// * netlib-compatible BLAS is the default
//
#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
#include <boost/numeric/bindings/blas/detail/cblas.h>
#include <boost/numeric/bindings/blas/detail/cblas_option.hpp>
#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
#include <boost/numeric/bindings/blas/detail/cublas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#else
#include <boost/numeric/bindings/blas/detail/blas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#endif

namespace boost {
namespace numeric {
namespace bindings {
namespace blas {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end BLAS-routine.
//
namespace detail {

#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * float value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const int m, const int n,
        const int kl, const int ku, const float alpha, const float* a,
        const int lda, const float* x, const int incx, const float beta,
        float* y, const int incy ) {
    cblas_sgbmv( cblas_option< Order >::value, cblas_option< Trans >::value,
            m, n, kl, ku, alpha, a, lda, x, incx, beta, y, incy );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * double value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const int m, const int n,
        const int kl, const int ku, const double alpha, const double* a,
        const int lda, const double* x, const int incx, const double beta,
        double* y, const int incy ) {
    cblas_dgbmv( cblas_option< Order >::value, cblas_option< Trans >::value,
            m, n, kl, ku, alpha, a, lda, x, incx, beta, y, incy );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<float> value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const int m, const int n,
        const int kl, const int ku, const std::complex<float> alpha,
        const std::complex<float>* a, const int lda,
        const std::complex<float>* x, const int incx,
        const std::complex<float> beta, std::complex<float>* y,
        const int incy ) {
    cblas_cgbmv( cblas_option< Order >::value, cblas_option< Trans >::value,
            m, n, kl, ku, &alpha, a, lda, x, incx, &beta, y, incy );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<double> value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const int m, const int n,
        const int kl, const int ku, const std::complex<double> alpha,
        const std::complex<double>* a, const int lda,
        const std::complex<double>* x, const int incx,
        const std::complex<double> beta, std::complex<double>* y,
        const int incy ) {
    cblas_zgbmv( cblas_option< Order >::value, cblas_option< Trans >::value,
            m, n, kl, ku, &alpha, a, lda, x, incx, &beta, y, incy );
}

#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * float value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const int m, const int n,
        const int kl, const int ku, const float alpha, const float* a,
        const int lda, const float* x, const int incx, const float beta,
        float* y, const int incy ) {
    BOOST_STATIC_ASSERT( (is_same<Order, tag::column_major>::value) );
    cublasSgbmv( blas_option< Trans >::value, m, n, kl, ku, alpha, a, lda, x,
            incx, beta, y, incy );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * double value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const int m, const int n,
        const int kl, const int ku, const double alpha, const double* a,
        const int lda, const double* x, const int incx, const double beta,
        double* y, const int incy ) {
    BOOST_STATIC_ASSERT( (is_same<Order, tag::column_major>::value) );
    cublasDgbmv( blas_option< Trans >::value, m, n, kl, ku, alpha, a, lda, x,
            incx, beta, y, incy );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<float> value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const int m, const int n,
        const int kl, const int ku, const std::complex<float> alpha,
        const std::complex<float>* a, const int lda,
        const std::complex<float>* x, const int incx,
        const std::complex<float> beta, std::complex<float>* y,
        const int incy ) {
    BOOST_STATIC_ASSERT( (is_same<Order, tag::column_major>::value) );
    cublasCgbmv( blas_option< Trans >::value, m, n, kl, ku, alpha, a, lda, x,
            incx, beta, y, incy );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<double> value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const int m, const int n,
        const int kl, const int ku, const std::complex<double> alpha,
        const std::complex<double>* a, const int lda,
        const std::complex<double>* x, const int incx,
        const std::complex<double> beta, std::complex<double>* y,
        const int incy ) {
    BOOST_STATIC_ASSERT( (is_same<Order, tag::column_major>::value) );
    cublasZgbmv( blas_option< Trans >::value, m, n, kl, ku, alpha, a, lda, x,
            incx, beta, y, incy );
}

#else
//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * float value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const fortran_int_t m,
        const fortran_int_t n, const fortran_int_t kl, const fortran_int_t ku,
        const float alpha, const float* a, const fortran_int_t lda,
        const float* x, const fortran_int_t incx, const float beta, float* y,
        const fortran_int_t incy ) {
    BOOST_STATIC_ASSERT( (is_same<Order, tag::column_major>::value) );
    BLAS_SGBMV( &blas_option< Trans >::value, &m, &n, &kl, &ku, &alpha, a,
            &lda, x, &incx, &beta, y, &incy );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * double value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const fortran_int_t m,
        const fortran_int_t n, const fortran_int_t kl, const fortran_int_t ku,
        const double alpha, const double* a, const fortran_int_t lda,
        const double* x, const fortran_int_t incx, const double beta,
        double* y, const fortran_int_t incy ) {
    BOOST_STATIC_ASSERT( (is_same<Order, tag::column_major>::value) );
    BLAS_DGBMV( &blas_option< Trans >::value, &m, &n, &kl, &ku, &alpha, a,
            &lda, x, &incx, &beta, y, &incy );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<float> value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const fortran_int_t m,
        const fortran_int_t n, const fortran_int_t kl, const fortran_int_t ku,
        const std::complex<float> alpha, const std::complex<float>* a,
        const fortran_int_t lda, const std::complex<float>* x,
        const fortran_int_t incx, const std::complex<float> beta,
        std::complex<float>* y, const fortran_int_t incy ) {
    BOOST_STATIC_ASSERT( (is_same<Order, tag::column_major>::value) );
    BLAS_CGBMV( &blas_option< Trans >::value, &m, &n, &kl, &ku, &alpha, a,
            &lda, x, &incx, &beta, y, &incy );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<double> value-type.
//
template< typename Order, typename Trans >
inline void gbmv( const Order, const Trans, const fortran_int_t m,
        const fortran_int_t n, const fortran_int_t kl, const fortran_int_t ku,
        const std::complex<double> alpha, const std::complex<double>* a,
        const fortran_int_t lda, const std::complex<double>* x,
        const fortran_int_t incx, const std::complex<double> beta,
        std::complex<double>* y, const fortran_int_t incy ) {
    BOOST_STATIC_ASSERT( (is_same<Order, tag::column_major>::value) );
    BLAS_ZGBMV( &blas_option< Trans >::value, &m, &n, &kl, &ku, &alpha, a,
            &lda, x, &incx, &beta, y, &incy );
}

#endif

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to gbmv.
//
template< typename Value >
struct gbmv_impl {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;
    typedef void result_type;

    //
    // Static member function that
    // * Deduces the required arguments for dispatching to BLAS, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename VectorX, typename VectorY >
    static result_type invoke( const value_type alpha, const MatrixA& a,
            const VectorX& x, const value_type beta, VectorY& y ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef typename detail::default_order< MatrixA >::type order;
        typedef typename result_of::trans_tag< MatrixA, order >::type trans;
        BOOST_STATIC_ASSERT( (is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorX >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorY >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::has_band_array< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::has_linear_array< VectorX >::value) );
        BOOST_STATIC_ASSERT( (bindings::has_linear_array< VectorY >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorY >::value) );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        detail::gbmv( order(), trans(), bindings::size_row_op(a,
                trans()), bindings::size_column_op(a, trans()),
                bindings::bandwidth_lower_op(a, trans()),
                bindings::bandwidth_upper_op(a, trans()), alpha,
                bindings::begin_value(a), bindings::stride_major(a),
                bindings::begin_value(x), bindings::stride(x), beta,
                bindings::begin_value(y), bindings::stride(y) );
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. Calls
// to these functions are passed to the gbmv_impl classes. In the
// documentation, the const-overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for gbmv. Its overload differs for
//
template< typename MatrixA, typename VectorX, typename VectorY >
inline typename gbmv_impl< typename bindings::value_type<
        MatrixA >::type >::result_type
gbmv( const typename bindings::value_type< MatrixA >::type alpha,
        const MatrixA& a, const VectorX& x,
        const typename bindings::value_type< MatrixA >::type beta,
        VectorY& y ) {
    gbmv_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( alpha, a, x, beta, y );
}

} // namespace blas
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#include "RouteFinder.h"

#include <boost/math/constants/constants.hpp>

namespace carto { namespace osrm {
    Result RouteFinder::find(const Query& query) const {
        std::array<std::vector<Graph::NearestNode>, 2> nearestNodes;
        std::array<std::priority_queue<SearchNode>, 2> heaps;
        std::unordered_map<Graph::NodeId, PathNode, Graph::NodeId::Hash> pathSuffixMap;
        float minWeight = 0.0f;
        for (int i = 0; i < 2; i++) {
            nearestNodes[i] = _graph->findNearestNode(query.getPos(i));
            if (nearestNodes[i].empty()) {
                return Result();
            }

            for (const Graph::NearestNode& nearestNode : nearestNodes[i]) {
                Graph::NodePtr node = _graph->getNode(nearestNode.nodeId);

                // Calculate end-point weights
                float weight = (i == 0 ? -nearestNode.geometryRelPos : nearestNode.geometryRelPos) * node->nodeData.weight;
                minWeight = std::min(minWeight, weight);

                // Special case: we have already added same node but the node is inaccessible along the current direction
                if (i == 1 && nearestNodes[0].size() == 1 && nearestNodes[1].size() == 1) {
                    const Graph::NearestNode& otherNearestNode = nearestNodes[1 - i][0];
                    if (nearestNode.nodeId == otherNearestNode.nodeId && nearestNode.geometryRelPos < otherNearestNode.geometryRelPos) {
                        // Add all backward edges "leading" to current node
                        for (auto edge = node->firstEdge; edge != node->lastEdge; edge++) {
                            if (edge->backward) {
                                heaps[i].emplace(edge->targetNodeId, Graph::NodeId(), weight + edge->edgeData.weight);
                                pathSuffixMap[edge->targetNodeId] = PathNode(edge->targetNodeId, *edge, nearestNode.nodeId);
                            }
                        }

                        // Here comes the tricky part: we must perform another spatial query to find INCOMING edges pointing to current edge
                        std::vector<WGSPos> geometry = _graph->getNodeGeometry(*node);
                        std::vector<Graph::NearestNode> nearestNodes2 = _graph->findNearestNode(geometry.front());
                        for (const Graph::NearestNode& nearestNode2 : nearestNodes2) {
                            Graph::NodePtr node2 = _graph->getNode(nearestNode2.nodeId);
                            for (auto edge2 = node2->firstEdge; edge2 != node2->lastEdge; edge2++) {
                                if (edge2->forward && edge2->targetNodeId == nearestNode.nodeId) {
                                    heaps[i].emplace(nearestNode2.nodeId, Graph::NodeId(), weight + edge2->edgeData.weight);
                                    pathSuffixMap[nearestNode2.nodeId] = PathNode(nearestNode2.nodeId, *edge2, nearestNode.nodeId);
                                }
                            }
                        }

                        continue;
                    }
                }

                // Add the node to heap, if other nodes were not already added
                heaps[i].emplace(nearestNode.nodeId, Graph::NodeId(), weight);
            }
        }

        // Apply bidirectional Dijkstra
        Graph::NodeId bestNodeId;
        float bestWeight = std::numeric_limits<float>::infinity();
        std::array<std::unordered_map<Graph::NodeId, SearchNode, Graph::NodeId::Hash>, 2> settledNodes;
        for (int i = 0; !(heaps[0].empty() && heaps[1].empty()); i = 1 - i) {
            if (heaps[i].empty()) {
                continue;
            }
            SearchNode searchNode = heaps[i].top();
            heaps[i].pop();

            // Skip all invalid nodes
            if (searchNode.nodeId.blockId.packageId == -1) {
                continue;
            }

            // Already shorter path found? In that case we can stop searching in the given direction
            if (searchNode.weight + minWeight > bestWeight) {
                while (!heaps[i].empty()) {
                    heaps[i].pop();
                }
                continue;
            }

            // Best path to give node?
            auto it0 = settledNodes[i].find(searchNode.nodeId);
            if (it0 != settledNodes[i].end()) {
                if (searchNode.weight >= it0->second.weight) { // not sure if the check is required
                    continue;
                }
            }

            // Settle the node
            settledNodes[i][searchNode.nodeId] = searchNode;

            // Stalling optimization. This implementation is not optimal, we should also look at non-settled heap nodes
            Graph::NodePtr node = _graph->getNode(searchNode.nodeId);
            bool stall = false;
            for (auto edge = node->firstEdge; edge != node->lastEdge; edge++) {
                if ((i == 0 && edge->backward) || (i != 0 && edge->forward)) {
                    auto it = settledNodes[i].find(edge->targetNodeId);
                    if (it != settledNodes[i].end()) {
                        if (it->second.weight + edge->edgeData.weight < searchNode.weight) {
                            stall = true;
                            break;
                        }
                    }
                }
            }
            if (stall) {
                continue;
            }

            // Recalculate shortest path and middle node
            auto it1 = settledNodes[1 - i].find(searchNode.nodeId);
            if (it1 != settledNodes[1 - i].end()) {
                float totalWeight = searchNode.weight + it1->second.weight;
                if (totalWeight >= 0 && totalWeight < bestWeight) {
                    bestWeight = totalWeight;
                    bestNodeId = searchNode.nodeId;
                }
            }

            // Add target nodes to heap
            for (auto edge = node->firstEdge; edge != node->lastEdge; edge++) {
                if ((i == 0 && edge->forward) || (i != 0 && edge->backward)) {
                    heaps[i].emplace(edge->targetNodeId, searchNode.nodeId, searchNode.weight + edge->edgeData.weight);
                }
            }
        }

        // Check that path was found
        if (bestNodeId.blockId.packageId == -1) {
            return Result();
        }

        // Unpack path
        std::array<std::vector<PathNode>, 2> paths;
        for (int i = 0; i < 2; i++) {
            std::stack<std::pair<Graph::NodeId, Graph::NodeId>> stack;
            Graph::NodeId nodeId = bestNodeId;
            while (true) {
                auto it = settledNodes[i].find(nodeId);
                assert(it != settledNodes[i].end());
                Graph::NodeId prevNodeId = it->second.prevNodeId;
                if (prevNodeId.blockId.packageId == -1) {
                    break;
                }
                stack.emplace(prevNodeId, nodeId);
                nodeId = prevNodeId;
            }

            while (!stack.empty()) {
                std::pair<Graph::NodeId, Graph::NodeId> nodeIds = stack.top();
                stack.pop();

                Graph::NodePtr matchedNode;
                const Graph::Edge* matchedEdge = nullptr;

                // Find the edge between prevNodeId and nodeId. Do matching based on node ids.
                Graph::NodeId prevNodeId = nodeIds.first;
                Graph::NodeId nodeId = nodeIds.second;
                for (int j = 0; j < 2 && !matchedEdge; j++) {
                    Graph::NodePtr prevNode = _graph->getNode(prevNodeId);
                    for (auto edge = prevNode->firstEdge; edge != prevNode->lastEdge; edge++) {
                        if (edge->targetNodeId == nodeId && ((j == i && edge->forward) || (j != i && edge->backward))) {
                            matchedNode = prevNode;
                            matchedEdge = edge;
                            break;
                        }
                    }
                    std::swap(nodeId, prevNodeId);
                }

                // If the edge was not found, then we have a link between packages with different node encodings. Do slow matching, based on geometry, not node ids
                for (int j = 0; j < 2 && !matchedEdge; j++) {
                    Graph::NodePtr prevNode = _graph->getNode(prevNodeId);
                    Graph::NodePtr node = _graph->getNode(nodeId);
                    std::vector<WGSPos> nodeGeometry = _graph->getNodeGeometry(*node);
                    for (auto edge = prevNode->firstEdge; edge != prevNode->lastEdge; edge++) {
                        if (edge->targetNodeId.blockId.packageId == -1) {
                            continue;
                        }
                        Graph::NodePtr targetNode = _graph->getNode(edge->targetNodeId);
                        std::vector<WGSPos> targetNodeGeometry = _graph->getNodeGeometry(*targetNode);
                        if (nodeGeometry == targetNodeGeometry && ((j == i && edge->forward) || (j != i && edge->backward))) {
                            matchedNode = prevNode;
                            matchedEdge = edge;
                            break;
                        }
                    }
                    std::swap(nodeId, prevNodeId);
                }

                // Unpack the matched edge
                if (matchedEdge) {
                    if (matchedEdge->contracted) {
                        if (matchedEdge->contractedNodeId.blockId.packageId == -1) {
                            return Result(); // Contracted node is not available, packing failed
                        }
                        stack.emplace(matchedEdge->contractedNodeId, nodeIds.second);
                        stack.emplace(nodeIds.first, matchedEdge->contractedNodeId);
                    }
                    else {
                        paths[i].emplace_back(nodeIds.first, *matchedEdge, nodeIds.second);
                    }
                } else {
                    return Result(); // NOTE: this should not happen, unless the graph is broken
                }
            }
        }

        // Build joined path. Add pseudo-node at the beginning to simplify processing and add final node, if rerouting in case of one-way street
        std::vector<PathNode> path;
        path = paths[0];
        for (auto it = paths[1].rbegin(); it != paths[1].rend(); it++) {
            path.emplace_back(it->nextNodeId, it->edge, it->prevNodeId);
        }
        if (path.empty()) {
            path.emplace(path.begin(), bestNodeId, Graph::Edge(), bestNodeId);
        }
        else {
            Graph::NodeId firstNodeId = path.front().prevNodeId;
            path.emplace(path.begin(), firstNodeId, Graph::Edge(), firstNodeId);
        }

        auto finalNodeIt = pathSuffixMap.find(path.back().nextNodeId);
        if (finalNodeIt != pathSuffixMap.end()) {
            path.push_back(finalNodeIt->second);
        }

        // Construct query result
        std::vector<Instruction> instructions;
        std::vector<WGSPos> routeVertices;
        for (std::size_t j = 0; j < path.size(); j++) {
            Graph::NodeId nodeId = path[j].nextNodeId;
            Graph::NodePtr node = _graph->getNode(nodeId);

            std::vector<WGSPos> geometry = _graph->getNodeGeometry(*node);
            std::pair<std::size_t, std::size_t> geometryIndex(0, geometry.size());
            std::pair<float, float> geometryRelPos(0.0f, 1.0f);

            std::size_t firstNNIndex = std::numeric_limits<std::size_t>::max();
            if (j == 0) {
                for (std::size_t k = 0; k < nearestNodes[0].size(); k++) {
                    if (nearestNodes[0][k].nodeId == nodeId) {
                        geometryIndex.first = nearestNodes[0][k].geometrySegmentIndex;
                        geometryRelPos.first = nearestNodes[0][k].geometryRelPos;
                        firstNNIndex = k;
                        break;
                    }
                }
            }

            std::size_t lastNNIndex = std::numeric_limits<std::size_t>::max();
            if (j == path.size() - 1) {
                for (std::size_t k = 0; k < nearestNodes[1].size(); k++) {
                    if (nearestNodes[1][k].nodeId == nodeId) {
                        geometryIndex.second = nearestNodes[1][k].geometrySegmentIndex;
                        geometryRelPos.second = nearestNodes[1][k].geometryRelPos;
                        lastNNIndex = k;
                        break;
                    }
                }
            }

            double dist = calculateGeometryLength(geometry, geometryRelPos.first, geometryRelPos.second);
            double time = (j > 0 ? path[j].edge.edgeData.weight : node->nodeData.weight) * (geometryRelPos.second - geometryRelPos.first) / 10.0;
            std::string streetName = _graph->getNodeName(*node);

            // Initial route instruction/vertex
            if (firstNNIndex != std::numeric_limits<std::size_t>::max()) {
                instructions.emplace_back(Instruction::Type::HEAD_ON, Instruction::TravelMode::DEFAULT, streetName, dist, time, routeVertices.size());
                routeVertices.push_back(nearestNodes[0][firstNNIndex].nodePos);
            }

            // Middle instructions/vertices
            if (!routeVertices.empty() && geometryIndex.first < geometryIndex.second) {
                if (routeVertices.back() == geometry[geometryIndex.first]) {
                    routeVertices.pop_back();
                }
            }
            std::size_t vertexIndex = routeVertices.size();
            routeVertices.insert(routeVertices.end(), geometry.begin() + geometryIndex.first, geometry.begin() + geometryIndex.second);
            if (j > 0) {
                Instruction::Type type = static_cast<Instruction::Type>(path[j].edge.edgeData.turnInstruction);
                Instruction::TravelMode travelMode = static_cast<Instruction::TravelMode>(node->nodeData.travelMode);
                instructions.emplace_back(type, travelMode, streetName, dist, time, vertexIndex);
            }

            // Final instruction/vertex
            if (lastNNIndex != std::numeric_limits<std::size_t>::max()) {
                instructions.emplace_back(Instruction::Type::REACHED_YOUR_DESTINATION, Instruction::TravelMode::DEFAULT, "", 0, 0, routeVertices.size());
                routeVertices.push_back(nearestNodes[1][lastNNIndex].nodePos);
            }
        }

        return Result(std::move(instructions), std::move(routeVertices));
    }

    double RouteFinder::calculateGeometryLength(const std::vector<WGSPos>& geometry, double t0, double t1) {
        double totalLen = 0;
        for (unsigned int j = 1; j < geometry.size(); j++) {
            totalLen += calculateGreatCircleDistance(geometry[j - 1], geometry[j]);
        }
        if (t0 == 0 && t1 == 1) {
            return totalLen;
        }

        double pos = 0;
        double len = 0;
        for (unsigned int j = 1; j < geometry.size(); j++) {
            double segmentLen = calculateGreatCircleDistance(geometry[j - 1], geometry[j]);
            double segmentPos0 = std::max(pos, t0 * totalLen);
            double segmentPos1 = std::min(pos + segmentLen, t1 * totalLen);
            len += std::max(0.0, segmentPos1 - segmentPos0);
            pos += segmentLen;
        }
        return len;
    }

    double RouteFinder::calculateGreatCircleDistance(const WGSPos& p0, const WGSPos& p1) {
        const double degToRad = boost::math::constants::pi<double>() / 180.0;

        double lat1 = p0(0) * degToRad;
        double lng1 = p0(1) * degToRad;
        double lat2 = p1(0) * degToRad;
        double lng2 = p1(1) * degToRad;

        double dLng = lng1 - lng2;
        double dLat = lat1 - lat2;

        double aHarv = std::pow(std::sin(dLat / 2.0), 2) + std::cos(lat1) * std::cos(lat2) * std::pow(std::sin(dLng / 2.0), 2);
        double cHarv = 2.0 * std::atan2(std::sqrt(aHarv), std::sqrt(1.0 - aHarv));
        return EARTH_RADIUS * cHarv;
    }
} }


//=======================================================================
// Copyright (c) 2018 Yi Ji
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
//=======================================================================

#include <boost/graph/max_cardinality_matching.hpp>
#include <boost/graph/maximum_weighted_matching.hpp>

#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/test/minimal.hpp>

using namespace boost;

typedef property<edge_weight_t, float, property<edge_index_t, int> > EdgeProperty;

typedef adjacency_list<vecS, vecS, undirectedS, property<vertex_index_t,int>, EdgeProperty> undirected_graph;
typedef adjacency_list<listS, listS, undirectedS, property<vertex_index_t,int>, EdgeProperty> undirected_list_graph;
typedef adjacency_matrix<undirectedS, property<vertex_index_t,int>, EdgeProperty> undirected_adjacency_matrix_graph;

template <typename Graph>
struct vertex_index_installer
{
    static void install(Graph&) {}
};

template <>
struct vertex_index_installer<undirected_list_graph>
{
    static void install(undirected_list_graph& g)
    {
        typedef graph_traits<undirected_list_graph>::vertex_iterator vertex_iterator_t;
        typedef graph_traits<undirected_list_graph>::vertices_size_type v_size_t;

        vertex_iterator_t vi, vi_end;
        v_size_t i = 0;
        for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi, ++i)
            put(vertex_index, g, *vi, i);
    }
};

template <typename Graph>
void print_graph(const Graph& g)
{
    typedef typename graph_traits<Graph>::edge_iterator edge_iterator_t;
    edge_iterator_t ei, ei_end;
    std::cout << std::endl << "The graph is: " << std::endl;
    for (boost::tie(ei,ei_end) = edges(g); ei != ei_end; ++ei)
        std::cout << "add_edge(" << source(*ei, g) << ", " << target(*ei, g) << ", EdgeProperty(" << get(edge_weight, g, *ei) << "), );" << std::endl;
}

template <typename Graph>
void weighted_matching_test(const Graph& g,
                            typename property_traits<typename property_map<Graph, edge_weight_t>::type>::value_type answer)
{
    typedef typename property_map<Graph,vertex_index_t>::type vertex_index_map_t;
    typedef vector_property_map< typename graph_traits<Graph>::vertex_descriptor, vertex_index_map_t > mate_t;
    mate_t mate(num_vertices(g));
    maximum_weighted_matching(g, mate);
    bool same_result = (matching_weight_sum(g, mate) == answer);
    BOOST_CHECK(same_result);
    if (!same_result)
    {
        mate_t max_mate(num_vertices(g));
        brute_force_maximum_weighted_matching(g, max_mate);

        std::cout << std::endl << "Found a weighted matching of weight sum " << matching_weight_sum(g, mate) << std::endl
        << "While brute-force search found a weighted matching of weight sum " << matching_weight_sum(g, max_mate) << std::endl;

        typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
        vertex_iterator_t vi, vi_end;

        print_graph(g);

        std::cout << std::endl << "The algorithmic matching is:" << std::endl;
        for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
            if (mate[*vi] != graph_traits<Graph>::null_vertex() && *vi < mate[*vi])
                std::cout << "{" << *vi << ", " << mate[*vi] << "}" << std::endl;

        std::cout << std::endl << "The brute-force matching is:" << std::endl;
        for (boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
            if (max_mate[*vi] != graph_traits<Graph>::null_vertex() && *vi < max_mate[*vi])
                std::cout << "{" << *vi << ", " << max_mate[*vi] << "}" << std::endl;

        std::cout << std::endl;
    }
}

template <typename Graph>
Graph make_graph(typename graph_traits<Graph>::vertices_size_type num_v,
                 typename graph_traits<Graph>::edges_size_type num_e,
                 std::deque<std::size_t> input_edges)
{
    Graph g(num_v);
    vertex_index_installer<Graph>::install(g);
    for (std::size_t i = 0; i < num_e; ++i)
    {
        std::size_t src_v, tgt_v, edge_weight;
        src_v = input_edges.front();
        input_edges.pop_front();
        tgt_v = input_edges.front();
        input_edges.pop_front();
        edge_weight = input_edges.front();
        input_edges.pop_front();
        add_edge(vertex(src_v, g), vertex(tgt_v, g), EdgeProperty(edge_weight), g);
    }
    return g;
}

int test_main(int, char*[])
{
    std::ifstream in_file("weighted_matching.dat");
    std::string line;
    while (std::getline(in_file, line))
    {
        std::istringstream in_graph(line);
        std::size_t answer, num_v, num_e;
        in_graph >> answer >> num_v >> num_e;

        std::deque<std::size_t> input_edges;
        std::size_t i;
        while (in_graph >> i)
            input_edges.push_back(i);

        weighted_matching_test(make_graph<undirected_graph>(num_v, num_e, input_edges), answer);
        weighted_matching_test(make_graph<undirected_list_graph>(num_v, num_e, input_edges), answer);
        weighted_matching_test(make_graph<undirected_adjacency_matrix_graph>(num_v, num_e, input_edges), answer);
    }
    return 0;
}


//  Copyright Nick Thompson 2018.
//  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)
//
// Basic sanity check that header <boost/math/interpolators/catmull_rom.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/interpolators/catmull_rom.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

void compile_and_link_test()
{
    std::vector<double> p0{0.1, 0.2, 0.3};
    std::vector<double> p1{0.2, 0.3, 0.4};
    std::vector<double> p2{0.3, 0.4, 0.5};
    std::vector<double> p3{0.4, 0.5, 0.6};
    std::vector<double> p4{0.5, 0.6, 0.7};
    std::vector<double> p5{0.6, 0.7, 0.8};
    std::vector<std::vector<double>> v{p0, p1, p2, p3, p4, p5};
    boost::math::catmull_rom<std::vector<double>> cat(std::move(v));
    check_result<std::vector<double>>(cat(0.0));
    check_result<std::vector<double>>(cat.prime(0.0));
}


#include <boost/math/cstdfloat/cstdfloat_complex.hpp>


#ifndef LBP_SCSLBP_HPP
#define LBP_SCSLBP_HPP

#include <lbp/defs.hpp>
#include <lbp/utils.hpp>
#include <lbp/detail/neighborhoods.hpp>
#include <lbp/detail/sampling.hpp>

#include <opencv2/core.hpp>

#include <boost/hana/fold.hpp>
#include <boost/hana/size.hpp>
#include <boost/hana/tuple.hpp>

#include <boost/integer.hpp>

//
// @inproceedings{xue2010dynamic,
//   title={Dynamic background subtraction based on spatial extended center-symmetric local binary pattern},
//   author={Xue, Gengjian and Sun, Jun and Song, Li},
//   booktitle={Multimedia and Expo (ICME), 2010 IEEE International Conference on},
//   pages={1050--1054},
//   year={2010},
//   organization={IEEE}
// }
//

namespace lbp {
namespace scslbp_detail {

const auto scslbp = [](auto N, auto S) {
    const auto n = hana::size (N);

    return [=](const cv::Mat& src, const cv::Mat& m, const cv::Mat& s,
               size_t i, size_t j) {
        using namespace hana::literals;

        const auto f = abs (
            S (src, i, j) - S (m, i, j)) >= 2.5 * S (s, i, j);

        return boost::hana::fold_left (
            N, 0, [&, shift = 0](auto accum, auto x) mutable {
                const auto a = S (src, i + x [0_c], j + x [1_c]);
                const auto b = S (src, i - x [0_c], j - x [1_c]);
                return accum + ((a >= b) << shift++);
            }) + (int (f) << n);
    };
};

} // namespace scslbp_detail

template< typename T, size_t R, size_t P >
auto scslbp = [](double alpha = 0.05) {
    //
    // Learning rate for both mean and variance:
    //
    LBP_ASSERT (0. <= alpha && alpha <= 1.);

    //
    // Mean, variance, standard deviation and previous frame:
    //
    cv::Mat m, v, s, I;

    //
    // Elementary operator:
    //
    auto op = scslbp_detail::scslbp (
        detail::semicircular_neighborhood< R, P >,
        detail::nearest_sampler< float >);

    return [=, init = 0](const cv::Mat& arg) mutable {
        using value_type = typename boost::uint_t< P/2 + 1 >::least;

        //
        // Only works on single-channel, gray images:
        //
        LBP_ASSERT (1 == arg.channels ());

        cv::Mat src = lbp::equalize (lbp::convert (arg, CV_32F));

        if (0 == init && 1 == ++init) {
            I = src.clone ();

            m = I.clone ();
            v = I.clone ();
            s = I.clone ();

            return I;
        }

        cv::Mat dst (src.size (), opencv_type< value_type >, cv::Scalar (0));

        //
        // Exponential moving average for estimating the mean:
        //
        cv::Mat m_ = (1 - alpha) * m + alpha * src;

        //
        // Rolling variance with a twist from [2009Gil-Jiménez] (see chapter 2):
        //
        cv::Mat v_ = (1 - alpha) * v + (alpha / 2) * lbp::pow (src - I, 2);
        cv::Mat s_ = lbp::sqrt (v_);

#pragma omp parallel for
        for (size_t i = R; i < src.rows - R - 1; ++i) {
            for (size_t j = R; j < src.cols - R - 1; ++j) {
                dst.at< value_type > (i, j) = op (src, m, s, i, j);
            }
        }

        m = m_;
        v = v_;
        s = s_;

        I = src.clone ();

        return dst;
    };
};

} // namespace lbp

//
// References:
//
// [2009Gil-Jiménez] Gil-Jiménez, Pedro, et al. "Continuous variance estimation
// in video surveillance sequences with high illumination changes." Signal
// Processing 89.7 (2009): 1412-1416.
//
//

#endif // LBP_SCSLBP_HPP


#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_srvs/Empty.h>

#include <boost/uuid/uuid_io.hpp>

#include <slam3d/core/Mapper.hpp>
#include <slam3d/graph/boost/BoostGraph.hpp>
#include <slam3d/sensor/pcl/PointCloudSensor.hpp>
#include <slam3d/sensor/gdal/GpsSensor.hpp>
#include <slam3d/solver/g2o/G2oSolver.hpp>

#include <iostream>
#include <thread>

#include "GraphPublisher.hpp"
#include "GpsPublisher.hpp"
#include "ros_common.hpp"
#include "ros_tf.hpp"
#include "helper_functions.hpp"

#define ROBOT_NAME "kitti"
#define SENSOR_NAME "velodyne"

#define MAP_FRAME "map"
#define SENSOR_FRAME "velodyne_laser"

tf::TransformBroadcaster* gTransformBroadcaster;
ros::Publisher* gMapPublisher;
ros::Publisher* gSignalPublisher;
GraphPublisher* gGraphPublisher;

slam3d::BoostGraph* gGraph;
slam3d::Mapper* gMapper;
slam3d::PointCloudSensor* gPclSensor;
slam3d::G2oSolver* gSolver;
slam3d::G2oSolver* gPatchSolver;

double gScanResolution;
bool gMultiThreaded;

void publishTransforms(const ros::Time& t)
{
	slam3d::Transform current = gMapper->getCurrentPose();
	if(current.matrix().determinant() == 0)
	{
		ROS_ERROR("Current pose from mapper has 0 determinant!");
	}

	tf::StampedTransform robot_in_map(eigen2tf(current), t, MAP_FRAME, SENSOR_FRAME);
	gTransformBroadcaster->sendTransform(robot_in_map);
}

void publishGraph(const ros::Time& t)
{
	gGraphPublisher->publishNodes(t, MAP_FRAME);
	gGraphPublisher->publishEdges(SENSOR_NAME, t, MAP_FRAME);
}

void receivePointCloud(const slam3d::PointCloud::ConstPtr& pcl)
{
	ROS_DEBUG("Received scan");
	gSignalPublisher->publish(std_msgs::Empty());

	ros::Time t;
	t.fromNSec(pcl->header.stamp * 1000);

	slam3d::PointCloud::Ptr cloud;
	if(gScanResolution > 0)
	{
		cloud = gPclSensor->downsample(pcl, gScanResolution);
	}else
	{
		cloud = slam3d::PointCloud::Ptr(new slam3d::PointCloud(*pcl));
	}

	slam3d::PointCloudMeasurement::Ptr m(
		new slam3d::PointCloudMeasurement(cloud, ROBOT_NAME, SENSOR_NAME, slam3d::Transform::Identity()));

	if(gPclSensor->addMeasurement(m))
	{
		publishTransforms(t);
		publishGraph(t);
		gPclSensor->linkLastToNeighbors(gMultiThreaded);
	}
}

bool optimize(std_srvs::Empty::Request  &req, std_srvs::Empty::Response &res)
{
	bool success = gGraph->optimize();
	return true;
}

void build_map(VertexObjectList vertices)
{
	PointCloud::Ptr downsampled = gPclSensor->buildMap(vertices);
	downsampled->header.frame_id = MAP_FRAME;
	gMapPublisher->publish(downsampled);
}

bool show_map(std_srvs::Empty::Request  &req, std_srvs::Empty::Response &res)
{
	// Get the accumulated point cloud
	VertexObjectList vertices = gGraph->getVerticesFromSensor(gPclSensor->getName());
	std::thread t(build_map, vertices);
	t.detach();
	return true;
}

int main(int argc, char **argv)
{
	// Create the ROS node
	ros::init(argc, argv, "mapper");
	ros::NodeHandle n;
	ros::NodeHandle pn("~/");
	pn.param("scan_resolution", gScanResolution, 0.5);
	pn.param("multi_threaded", gMultiThreaded, true);
	gTransformBroadcaster = new tf::TransformBroadcaster;

	// Clock and Logger to be used by all slam3d components
	slam3d::Clock* clock = new RosClock();
	slam3d::Logger* logger = new RosLogger();

	// Create the Graph, that will hold all mapping information
	gGraph = new slam3d::BoostGraph(logger);

	// We have no absolute positioning (e.g. GPS), so we fix the
	// first node in the graph
	gGraph->fixNext();

	// Create the Mapper object
	gMapper = new slam3d::Mapper(gGraph, logger);

	// Create the Backend for global graph optimization
	gSolver = new slam3d::G2oSolver(logger);
	gGraph->setSolver(gSolver, pn.param("optimization_rate", 10));

	// Create the PointCloudSensor for the velodyne laser
	gPclSensor = new slam3d::PointCloudSensor(SENSOR_NAME, logger);
	readPointcloudSensorParameters(pn, gPclSensor);
	gMapper->registerSensor(gPclSensor);

	// Create a solver to optimize local scan patches
	gPatchSolver = new slam3d::G2oSolver(logger);
	gPclSensor->setPatchSolver(gPatchSolver);

	// Subscribe to topics
	ros::Publisher pclPub = n.advertise<slam3d::PointCloud>("map", 1);
	ros::Subscriber pclSub = n.subscribe<slam3d::PointCloud>("pointcloud", 10, &receivePointCloud);
	ros::ServiceServer optSrv = n.advertiseService("optimize", &optimize);
	ros::ServiceServer showSrv = n.advertiseService("show_map", &show_map);
	ros::Publisher signalPub = n.advertise<std_msgs::Empty>("trigger", 1, true);

	gMapPublisher = &pclPub;
	gSignalPublisher = &signalPub;

	// Create the visualizer for the nodes and edges
	gGraphPublisher = new GraphPublisher(gGraph);
	gGraphPublisher->addNodeSensor(SENSOR_NAME, 0,1,0);
	gGraphPublisher->addEdgeSensor(SENSOR_NAME);

	ros::spin();

	delete gGraph;
	delete gMapper;
	delete gGraphPublisher;
	delete gPclSensor;
	delete gSolver;
	delete gPatchSolver;
	delete logger;
	delete clock;
	delete gTransformBroadcaster;
	return 0;
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ContinuousReduce
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
// codelets
#include "../../lib/popops/reduction/ReductionVertex.hpp"
#include "poplar/Target.hpp"
#include "poplibs_test/Check.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/codelets.hpp"
#include "poputil/VertexTemplates.hpp"
#include <boost/program_options.hpp>
#include <poplibs_test/Reduce.hpp>
#include <popops/Reduce.hpp>
#include <stdexcept>
#include <string.h>

#include <boost/program_options.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace popops;
using namespace poputil;
using namespace poplibs_test::util;
using namespace poplibs_test::reduce;
using namespace poplibs_support;

static bool doTest(const DeviceType &deviceType, const Type &partialsType,
                   const Type &outType, const unsigned outerDim,
                   const unsigned innerDim, const popops::Operation op,
                   const float scale, bool isUpdate) {
  auto device = createTestDevice(deviceType);
  auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  const float initialValue = 3.0;

  // Claim enough space for floats
  std::vector<char> data(innerDim * outerDim * 4);
  MultiArray<float> nums{outerDim, innerDim};
  std::vector<int> int_data(innerDim * outerDim);
  for (unsigned i = 0; i < outerDim; ++i) {
    for (unsigned j = 0; j < innerDim; ++j) {
      nums[i][j] = i + j;
      int_data[(i * innerDim) + j] = i + j;
    }
  }

  copy(target, nums.data(), innerDim * outerDim, partialsType, data.data());
  std::vector<float> answers(outerDim, initialValue);
  std::vector<char> ans_data((outerDim)*4);
  copy(target, answers.data(), outerDim, outType, ans_data.data());

  Sequence prog;

  auto cs = graph.addComputeSet("cs");

  Tensor partials;
  partials = graph.addVariable(partialsType, {outerDim, innerDim});
  Tensor out;
  out = graph.addVariable(outType, {outerDim});

  const auto vertexClass =
      templateVertex(scale == 1.0f ? "popops::ContinuousReduce"
                                   : "popops::ScaledContinuousReduce",
                     "popops::" + getReductionVertexOpName(op), partialsType,
                     outType, isUpdate);

  auto v1 = graph.addVertex(cs, vertexClass);

  graph.connect(v1["partials"], partials.flatten());
  graph.connect(v1["out"], out);

  graph.setInitialValue(v1["numOutputsM1"], outerDim - 1);
  graph.setInitialValue(v1["numPartials"], innerDim);

  auto scaleTensor = graph.addVariable(FLOAT, {});
  graph.setTileMapping(scaleTensor, 0);
  graph.setInitialValue(scaleTensor, scale);
  if (scale != 1.0f) {
    graph.connect(v1["k"], scaleTensor.reshape({1}));
  }

  graph.setTileMapping(v1, 0);
  graph.setTileMapping(partials, 0);
  graph.setTileMapping(out, 0);

  graph.createHostWrite("partials", partials);
  graph.createHostWrite("outw", out);
  graph.createHostRead("out", out);

  prog.add(Execute(cs));

  Engine e(graph, prog);
  auto outSize = out.numElements() * target.getTypeSize(outType);

  device.bind([&](const Device &d) {
    e.load(d);
    if (outType == FLOAT || outType == HALF) {
      e.writeTensor("partials", data.data(),
                    data.data() + partials.numElements() *
                                      target.getTypeSize(partialsType));
      e.writeTensor("outw", ans_data.data(), ans_data.data() + outSize);
    } else if (outType == INT) {
      e.writeTensor("partials", int_data.data(),
                    int_data.data() + partials.numElements() *
                                          target.getTypeSize(partialsType));
      e.writeTensor("outw", ans_data.data(), ans_data.data() + outSize);
    }
    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);

    e.run();

    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);
  });

  copy(target, outType, ans_data.data(), answers.data(), outerDim);
  copy(target, outType, ans_data.data(), int_data.data(), outerDim);

  bool success = true;

  auto result = reduce<float>(nums, {1}, op);

  std::vector<float> correct_answer(outerDim, initialValue);
  for (unsigned i = 0; i < outerDim; i++) {
    if (isUpdate) {
      correct_answer[i] += result[i] * scale;
    } else {
      correct_answer[i] = result[i] * scale;
    }
  }
  if (outType == FLOAT || outType == HALF) {
    CHECK_ELEMWISE_EQ(success, correct_answer, answers, outerDim);
    for (unsigned i = 0; i < outerDim; ++i) {
      answers[i] = 0; // zero for next iteration
    }
  } else if (outType == INT) {
    CHECK_ELEMWISE_EQ(success, correct_answer, int_data, outerDim);
  } else {
    success = false;
  }
  if (!success) {
    std::cerr << "nums = " << nums << '\n';
    std::cerr << "scale = " << scale << '\n';
    if (isUpdate) {
      std::cerr << "result = " << result << '\n';
      std::cerr << "initialValue = " << initialValue << '\n';
    }
  }

  return success;
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  popops::Operation op = popops::Operation::ADD;
  DeviceType deviceType;
  Type partialsType;
  Type outType;
  float scale = 2.0f;
  bool isUpdate = true;
  unsigned outerDim, innerDim;
  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("partials-type",
     po::value<Type>(&partialsType)->required(),
     "Partials Type")
    ("out-type",
     po::value<Type>(&outType)->required(),
     "Output Type")
    ("operation",
     po::value(&op),
     "operation:ADD SQUARE_ADD MAX MIN MUL LOGICAL_OR or LOGICAL_AND")
    ("update",
     po::value<bool>(&isUpdate)->default_value(isUpdate),
     "reduce with update")
    ("scale",
     po::value<float>(&scale)->default_value(scale),
     "scale")
    ("outer-dim",
     po::value<unsigned>(&outerDim)->required(),
     "Outer dimension")
    ("inner-dim",
     po::value<unsigned>(&innerDim)->required(),
     "Inner dimension");
  // clang-format on
  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  if (!doTest(deviceType, partialsType, outType, outerDim, innerDim, op, scale,
              isUpdate))
    return 1;
  return 0;
}


#include <boost/python.hpp>
#include <rstbx/simage/image_simple.hpp>

namespace rstbx { namespace simage { namespace ext {

  struct image_simple_wrappers
  {
    typedef image_simple wt;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      typedef return_internal_reference<> rir;
      class_<wt>("image_simple", no_init)
        .def(init<bool, bool, bool, bool, bool, bool, bool>((
          arg("apply_proximity_filter")=true,
          arg("apply_detector_clipping")=true,
          arg("apply_proximity_factor")=true,
          arg("store_miller_index_i_seqs")=false,
          arg("store_spots")=false,
          arg("store_signals")=false,
          arg("set_pixels")=false)))
        .def("compute", &wt::compute, (
          arg("unit_cell"),
          arg("miller_indices"),
          arg("spot_intensity_factors"),
          arg("crystal_rotation_matrix"),
          arg("ewald_radius"),
          arg("ewald_proximity"),
          arg("signal_max"),
          arg("detector_distance"),
          arg("detector_size"),
          arg("detector_pixels"),
          arg("point_spread"),
          arg("gaussian_falloff_scale")), rir())
        .add_property("miller_index_i_seqs",
          make_getter(&wt::miller_index_i_seqs, rbv()))
        .add_property("spots", make_getter(&wt::spots, rbv()))
        .add_property("signals", make_getter(&wt::signals, rbv()))
        .def_readonly("pixels", &wt::pixels)
      ;
    }
  };

  std::string
  combine_rgb_images(
    boost::python::list const& rgb_images)
  {
    namespace bp = boost::python;
    TBXX_ASSERT(bp::len(rgb_images) > 0);
    std::size_t img_size = static_cast<std::size_t>(bp::len(rgb_images[0]));
    std::string result(img_size, '\0');
    std::size_t n_imgs = static_cast<std::size_t>(bp::len(rgb_images));
    boost::scoped_array<const char*> img_ptrs(new const char*[n_imgs]);
    for(std::size_t j=0;j!=n_imgs;j++) {
      TBXX_ASSERT(bp::len(rgb_images[j]) == img_size);
      img_ptrs[j] = bp::extract<const char*>(rgb_images[j])();
    }
    for(std::size_t i=0;i!=img_size;i++) {
      unsigned sum = 0;
      for(std::size_t j=0;j!=n_imgs;j++) {
        sum += static_cast<unsigned>(img_ptrs[j][i]);
      }
      unsigned c = static_cast<unsigned>(
        static_cast<double>(sum) / n_imgs + 0.5);
      if (c > 255) c = 255;
      result[i] = static_cast<char>(c);
    }
    return result;
  }

  void init_module()
  {
    using namespace boost::python;
    image_simple_wrappers::wrap();
    def("combine_rgb_images", combine_rgb_images);
  }

}}} // namespace rstbx::simage::ext

BOOST_PYTHON_MODULE(rstbx_simage_ext)
{
  rstbx::simage::ext::init_module();
}


#include "parse_opening_hours.hpp"
#include "opening_hours_parsers.hpp"

#include <boost/spirit/include/phoenix_bind.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>  // operator,

namespace osmoh
{
  namespace parsing
  {
    weekday_selector_parser::weekday_selector_parser() : weekday_selector_parser::base_type(main)
    {
      using qi::_a;
      using qi::_1;
      using qi::_2;
      using qi::_val;
      using qi::lit;
      using qi::ushort_;
      using osmoh::NthWeekdayOfTheMonthEntry;
      using osmoh::Holiday;
      using osmoh::WeekdayRange;
      using osmoh::Weekdays;

      nth = ushort_(1)[_val = NthWeekdayOfTheMonthEntry::NthDayOfTheMonth::First]
      | ushort_(2) [_val = NthWeekdayOfTheMonthEntry::NthDayOfTheMonth::Second]
      | ushort_(3) [_val = NthWeekdayOfTheMonthEntry::NthDayOfTheMonth::Third]
      | ushort_(4) [_val = NthWeekdayOfTheMonthEntry::NthDayOfTheMonth::Fourth]
      | ushort_(5) [_val = NthWeekdayOfTheMonthEntry::NthDayOfTheMonth::Fifth];

      nth_entry = (nth >> dash >> nth) [bind(&NthWeekdayOfTheMonthEntry::SetStart, _val, _1),
                                        bind(&NthWeekdayOfTheMonthEntry::SetEnd, _val, _2)]
      | (lit('-') >> nth)          [bind(&NthWeekdayOfTheMonthEntry::SetEnd, _val, _1)]
      | nth [bind(&NthWeekdayOfTheMonthEntry::SetStart, _val, _1)]
      ;

      day_offset =
      ( (lit('+')[_a = 1] | lit('-') [_a = -1]) >>
       ushort_  [_val = _1 * _a] >>
       charset::no_case[(lit("days") | lit("day"))] )
      ;

      holiday = (charset::no_case[lit("SH")] [bind(&Holiday::SetPlural, _val, false)]
                 >> -day_offset              [bind(&Holiday::SetOffset, _val, _1)])
      | charset::no_case[lit("PH")]      [bind(&Holiday::SetPlural, _val, true)]
      ;

      holiday_sequence %= (holiday % ',');

      weekday_range =
      ( charset::no_case[wdays]  [bind(&WeekdayRange::SetStart, _val, _1)] >>
       '[' >> (nth_entry        [bind(&WeekdayRange::AddNth, _val, _1)]) % ',') >> ']' >>
      -(day_offset             [bind(&WeekdayRange::SetOffset, _val, _1)])
      | charset::no_case[(wdays >> dash >> wdays)]  [bind(&WeekdayRange::SetStart, _val, _1),
                                                     bind(&WeekdayRange::SetEnd, _val, _2)]
      | charset::no_case[wdays]  [bind(&WeekdayRange::SetStart, _val, _1)]
      ;

      weekday_sequence %= (weekday_range % ',') >> !qi::no_skip[charset::alpha]
      ;

      main = (holiday_sequence >> -lit(',') >> weekday_sequence)
      [bind(&Weekdays::SetHolidays, _val, _1),
       bind(&Weekdays::SetWeekdayRanges, _val, _2)]
      | holiday_sequence [bind(&Weekdays::SetHolidays, _val, _1)]
      | weekday_sequence [bind(&Weekdays::SetWeekdayRanges, _val, _1)]
      ;

      BOOST_SPIRIT_DEBUG_NODE(main);
      BOOST_SPIRIT_DEBUG_NODE(weekday_sequence);
      BOOST_SPIRIT_DEBUG_NODE(weekday_range);
      BOOST_SPIRIT_DEBUG_NODE(holiday_sequence);
    }
  }

  bool Parse(std::string const & str, Weekdays & context)
  {
    return osmoh::ParseImpl<parsing::weekday_selector_parser>(str, context);
  }
} // namespace osmoh


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "default_direct_solver.hpp"

#include "../assembly/abstract_boundary_operator.hpp"
#include "../assembly/blocked_boundary_operator.hpp"
#include "../assembly/boundary_operator.hpp"
#include "../assembly/discrete_boundary_operator.hpp"
#include "../fiber/explicit_instantiation.hpp"

#include <boost/variant.hpp>

namespace Bempp {

/** \cond HIDDEN_INTERNAL */

template <typename BasisFunctionType, typename ResultType>
struct DefaultDirectSolver<BasisFunctionType, ResultType>::Impl {
  Impl(const BoundaryOperator<BasisFunctionType, ResultType> &op_) : op(op_) {}

  Impl(const BlockedBoundaryOperator<BasisFunctionType, ResultType> &op_)
      : op(op_) {}

  boost::variant<BoundaryOperator<BasisFunctionType, ResultType>,
                 BlockedBoundaryOperator<BasisFunctionType, ResultType>> op;
};

/** \endcond */

template <typename BasisFunctionType, typename ResultType>
DefaultDirectSolver<BasisFunctionType, ResultType>::DefaultDirectSolver(
    const BoundaryOperator<BasisFunctionType, ResultType> &boundaryOp)
    : m_impl(new Impl(boundaryOp)) {}

template <typename BasisFunctionType, typename ResultType>
DefaultDirectSolver<BasisFunctionType, ResultType>::DefaultDirectSolver(
    const BlockedBoundaryOperator<BasisFunctionType, ResultType> &boundaryOp)
    : m_impl(new Impl(boundaryOp)) {}

template <typename BasisFunctionType, typename ResultType>
DefaultDirectSolver<BasisFunctionType, ResultType>::~DefaultDirectSolver() {}

template <typename BasisFunctionType, typename ResultType>
Solution<BasisFunctionType, ResultType>
DefaultDirectSolver<BasisFunctionType, ResultType>::solveImplNonblocked(
    const GridFunction<BasisFunctionType, ResultType> &rhs) const {
  typedef BoundaryOperator<BasisFunctionType, ResultType> BoundaryOp;

  const BoundaryOp *boundaryOp = boost::get<BoundaryOp>(&m_impl->op);
  if (!boundaryOp)
    throw std::logic_error(
        "DefaultDirectSolver::solve(): for solvers constructed "
        "from a BlockedBoundaryOperator the other solve() overload "
        "must be used");
  Solver<BasisFunctionType, ResultType>::checkConsistency(
      *boundaryOp, rhs, ConvergenceTestMode::TEST_CONVERGENCE_IN_DUAL_TO_RANGE);

  arma::Col<ResultType> armaSolution =
      arma::solve(boundaryOp->weakForm()->asMatrix(),
                  rhs.projections(boundaryOp->dualToRange()));

  return Solution<BasisFunctionType, ResultType>(
      GridFunction<BasisFunctionType, ResultType>(
          boundaryOp->context(), boundaryOp->domain(), armaSolution),
      SolutionStatus::CONVERGED,
      SolutionBase<BasisFunctionType, ResultType>::unknownTolerance(),
      "Solver finished");
}

template <typename BasisFunctionType, typename ResultType>
BlockedSolution<BasisFunctionType, ResultType>
DefaultDirectSolver<BasisFunctionType, ResultType>::solveImplBlocked(
    const std::vector<GridFunction<BasisFunctionType, ResultType>> &rhs) const {
  typedef BlockedBoundaryOperator<BasisFunctionType, ResultType> BoundaryOp;

  const BoundaryOp *boundaryOp = boost::get<BoundaryOp>(&m_impl->op);
  if (!boundaryOp)
    throw std::logic_error(
        "DefaultDirectSolver::solve(): for solvers constructed "
        "from a (non-blocked) BoundaryOperator the other solve() overload "
        "must be used");
  std::vector<GridFunction<BasisFunctionType, ResultType>> canonicalRhs =
      Solver<BasisFunctionType, ResultType>::canonicalizeBlockedRhs(
          *boundaryOp, rhs,
          ConvergenceTestMode::TEST_CONVERGENCE_IN_DUAL_TO_RANGE);
  // Shouldn't be needed, but better safe than sorry...
  Solver<BasisFunctionType, ResultType>::checkConsistency(
      *boundaryOp, canonicalRhs,
      ConvergenceTestMode::TEST_CONVERGENCE_IN_DUAL_TO_RANGE);

  // Construct the right-hand size vector
  arma::Col<ResultType> armaRhs(
      boundaryOp->totalGlobalDofCountInDualsToRanges());
  for (size_t i = 0, start = 0; i < canonicalRhs.size(); ++i) {
    const arma::Col<ResultType> &chunkProjections =
        canonicalRhs[i].projections(boundaryOp->dualToRange(i));
    size_t chunkSize = chunkProjections.n_rows;
    armaRhs.rows(start, start + chunkSize - 1) = chunkProjections;
    start += chunkSize;
  }

  // Solve
  arma::Col<ResultType> armaSolution =
      arma::solve(boundaryOp->weakForm()->asMatrix(), armaRhs);

  // Convert chunks of the solution vector into grid functions
  std::vector<GridFunction<BasisFunctionType, ResultType>> solutionFunctions;
  Solver<BasisFunctionType, ResultType>::constructBlockedGridFunction(
      armaSolution, *boundaryOp, solutionFunctions);

  // Return solution
  return BlockedSolution<BasisFunctionType, ResultType>(
      solutionFunctions, SolutionStatus::CONVERGED,
      SolutionBase<BasisFunctionType, ResultType>::unknownTolerance(),
      "Solver finished");
}

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS_AND_RESULT(DefaultDirectSolver);

} // namespace Bempp


#include <cctbx/boost_python/flex_fwd.h>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/list.hpp>
#include <boost/python/dict.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/versa.h>
#include <scitbx/array_family/accessors/c_grid.h>
#include <cctbx/miller.h>
#include <dials/array_family/reflection_table.h>
#include <algorithm>

typedef cctbx::miller::index<> miller_index_t;

namespace sx_merging {
  void foo2(){
    std::cout <<"HELLO merging foo2"<< std::endl;
  }

  void get_hkl_chunks_cpp(dials::af::reflection_table reflections,
                          const scitbx::af::shared<miller_index_t>& hkl_list,
                          const scitbx::af::shared<int>& chunk_id_list,
                          boost::python::list hkl_chunks_cpp){
    // set up a map hkl:chunk_id
    std::map<miller_index_t, size_t> chunk_lookup;
    for(size_t i = 0UL; i < hkl_list.size(); ++i){
      chunk_lookup[hkl_list[i]] = (size_t)chunk_id_list[i];
    }

    SCITBX_ASSERT(reflections.contains("miller_index_asymmetric"));
    SCITBX_ASSERT(reflections.contains("intensity.sum.value"));
    SCITBX_ASSERT(reflections.contains("intensity.sum.variance"));
    SCITBX_ASSERT(reflections.contains("exp_id"));

    scitbx::af::ref<miller_index_t> miller_index    = reflections["miller_index_asymmetric"];
    scitbx::af::ref<double> intensity               = reflections["intensity.sum.value"];
    scitbx::af::ref<double> variance                = reflections["intensity.sum.variance"];
    scitbx::af::ref<std::string> experiment_id      = reflections["exp_id"];

    miller_index_t* mi_ptr          = miller_index.begin();
    double* intensity_ptr           = intensity.begin();
    double* variance_ptr            = variance.begin();
    std::string* experiment_id_ptr  = experiment_id.begin();

    int n_chunks = boost::python::len(hkl_chunks_cpp);
    std::vector<dials::af::reflection_table> tables;
    for(size_t i=0UL; i < n_chunks; ++i){
      tables.push_back(boost::python::extract<dials::af::reflection_table>(hkl_chunks_cpp[i]));
      SCITBX_ASSERT(tables.back().contains("miller_index_asymmetric"));
    }

    // cache all columns for all hkl chunks
    std::vector<scitbx::af::shared<miller_index_t> >  mi_cols;
    std::vector<scitbx::af::shared<double> >          intensity_cols;
    std::vector<scitbx::af::shared<double> >          variance_cols;
    std::vector<scitbx::af::shared<std::string> >     experiment_id_cols;

    for(size_t i=0UL; i < n_chunks; ++i){
      mi_cols.push_back(tables[i]["miller_index_asymmetric"]);
      intensity_cols.push_back(tables[i]["intensity.sum.value"]);
      variance_cols.push_back(tables[i]["intensity.sum.variance"]);
      experiment_id_cols.push_back(tables[i]["exp_id"]);
    }

    // distribute reflections over chunks
    for(size_t i=0UL; i < reflections.size(); ++i){
      miller_index_t  hkl       = *mi_ptr++;
      double          intensity = *intensity_ptr++;
      double          variance  = *variance_ptr++;
      std::string     experiment_id = *experiment_id_ptr++;

      if( 0 != chunk_lookup.count(hkl) )
      {
          size_t chunk_id  = chunk_lookup[hkl];
          mi_cols[chunk_id].push_back(hkl);
          intensity_cols[chunk_id].push_back(intensity);
          variance_cols[chunk_id].push_back(variance);
          experiment_id_cols[chunk_id].push_back(experiment_id);
       }
    }
  }

  dials::af::reflection_table isigi_dict_to_reflection_table(
                          dials::af::shared<cctbx::miller::index<> > miller_indices,
                          boost::python::dict ISIGI) {
    dials::af::reflection_table table;
    boost::python::list keys = ISIGI.keys();
    dials::af::shared<cctbx::miller::index<> > miller_index          = table.get<cctbx::miller::index<> >("miller_index");
    dials::af::shared<cctbx::miller::index<> > miller_index_original = table.get<cctbx::miller::index<> >("miller_index_original");
    dials::af::shared<double> scaled_intensity = table.get<double>("scaled_intensity");
    dials::af::shared<double> isigi            = table.get<double>("isigi");
    dials::af::shared<double> slope            = table.get<double>("slope");
    dials::af::shared<double> iobs             = table.get<double>("iobs");
    dials::af::shared<size_t> miller_id           = table.get<size_t>("miller_id");
    dials::af::shared<size_t> crystal_id          = table.get<size_t>("crystal_id");
    for (size_t i = 0; i < miller_indices.size(); i ++) {
      if (!ISIGI.has_key<cctbx::miller::index<> >(miller_indices[i])) continue;
      boost::python::list values = boost::python::extract<boost::python::list>(ISIGI[miller_indices[i]]);
      for (size_t j = 0; j < boost::python::len(values); j++) {
        miller_index.push_back(miller_indices[i]);
        boost::python::tuple item = boost::python::extract<boost::python::tuple>(values[j]);
        scaled_intensity.push_back(boost::python::extract<double>(item[0]));
        isigi.push_back(boost::python::extract<double>(item[1]));
        slope.push_back(boost::python::extract<double>(item[2]));
        miller_id.push_back(i);
        crystal_id.push_back(0);
        iobs.push_back(0);
        miller_index_original.push_back(cctbx::miller::index<int>(0,0,0));
      }
    }
    return table;
  }
}

using namespace boost::python;
namespace sx_merging{
namespace boost_python { namespace {

  void
  sx_merging_init_module() {
    using namespace boost::python;
    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;

    def("foo2",&sx_merging::foo2);
    def("get_hkl_chunks_cpp",&sx_merging::get_hkl_chunks_cpp);
    def("isigi_dict_to_reflection_table",&sx_merging::isigi_dict_to_reflection_table);
  }

}
}} // namespace sx_merging::boost_python::<anonymous>

BOOST_PYTHON_MODULE(sx_merging_ext)
{
  sx_merging::boost_python::sx_merging_init_module();
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_LENGTH_CARTESIAN_HPP
#define BOOST_GEOMETRY_STRATEGIES_LENGTH_CARTESIAN_HPP

#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>

#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/distance/detail.hpp>
#include <boost/geometry/strategies/length/services.hpp>

#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace length
{

template <typename CalculationType = void>
struct cartesian
    : public strategies::detail::cartesian_base
{
    template <typename Geometry1, typename Geometry2>
    static auto distance(Geometry1 const&, Geometry2 const&,
                         distance::detail::enable_if_pp_t<Geometry1, Geometry2> * = nullptr)
    {
        return strategy::distance::pythagoras<CalculationType>();
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, cartesian_tag>
{
    using type = strategies::length::cartesian<>;
};

template <typename CT>
struct strategy_converter<strategy::distance::pythagoras<CT> >
{
    static auto get(strategy::distance::pythagoras<CT> const&)
    {
        return strategies::length::cartesian<CT>();
    }
};

} // namespace services

}} // namespace strategies::length

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_LENGTH_CARTESIAN_HPP


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_MAXGAMMALN_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_MAXGAMMALN_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Maxgammaln generic tag

     Represents the Maxgammaln constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Maxgammaln, double, 1
                                , 0x7bc3f8eaLL, 0x7f574c5dd06d2516ULL
                                );
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Maxgammaln, Site> dispatching_Maxgammaln(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Maxgammaln, Site>();
   }
   template<class... Args>
   struct impl_Maxgammaln;
  }
  /*!
    Generates value \f$\log\pi\f$

    @par Semantic:

    @code
    T r = Maxgammaln<T>();
    @endcode

    is similar to:

    @code
    if T is float
      T r = 2.035093e36f
    else if T is double
      T r = 2.556348e305
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Maxgammaln, Maxgammaln)
} }

#include <boost/simd/constant/common.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_NEGATION_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_NEGATION_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/negation.hpp>
#include <boost/simd/include/functions/simd/is_ltz.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::negation_, tag::cpu_,
                           (A0)(X),
                           ((simd_<arithmetic_<A0>,X>))
                           ((simd_<arithmetic_<A0>,X>))
                          )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return  if_else(is_ltz(a1),-a0,if_else_zero(is_nez(a1), a0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::negation_, tag::cpu_,
                           (A0)(X),
                           ((simd_<unsigned_<A0>,X>))
                           ((simd_<unsigned_<A0>,X>))
                          )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
        return  is_nez(a1)&a0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::negation_, tag::cpu_,
                           (A0)(X),
                           ((simd_<floating_<A0>,X>))
                           ((simd_<floating_<A0>,X>))
                          )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 tmp = if_else(is_nez(a1), a0, a1);
      A0 ma0 =  unary_minus(a0);
      tmp = if_else(is_ltz(a1), ma0, tmp);
      tmp = select(is_nan(a1), a1, tmp);
      return tmp;
    }
  };
} } }
#endif