//
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
//
// 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_GIL_IMAGE_PROCESSING_FILTER_HPP
#define BOOST_GIL_IMAGE_PROCESSING_FILTER_HPP

#include <cstddef>
#include <vector>

#include <boost/gil/image.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/extension/numeric/kernel.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>

namespace boost { namespace gil {

template <typename SrcView, typename DstView>
void box_filter(
    SrcView const& src_view,
    DstView const& dst_view,
    std::size_t kernel_size,
    long int anchor = -1,
    bool normalize=true,
    convolve_boundary_option option = convolve_option_extend_zero
)
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    gil_function_requires<MutableImageViewConcept<DstView>>();
    static_assert(color_spaces_are_compatible
    <
        typename color_space_type<SrcView>::type,
        typename color_space_type<DstView>::type
    >::value, "Source and destination views must have pixels with the same color space");

    std::vector<float> kernel_values;
    if (normalize) { kernel_values.resize(kernel_size, 1.0f / float(kernel_size)); }
    else { kernel_values.resize(kernel_size, 1.0f); }

    if (anchor == -1) anchor = static_cast<int>(kernel_size / 2);
    kernel_1d<float> kernel(kernel_values.begin(), kernel_size, anchor);

    convolve_1d<pixel<float, typename SrcView::value_type::layout_t>>(src_view, kernel, dst_view, option);
}

template <typename SrcView, typename DstView>
void blur(
    SrcView const& src_view,
    DstView const& dst_view,
    std::size_t kernel_size,
    long int anchor = -1,
    convolve_boundary_option option = convolve_option_extend_zero
)
{
    box_filter(src_view, dst_view, kernel_size, anchor, true, option);
}

}} //namespace boost::gil

#endif // !BOOST_GIL_IMAGE_PROCESSING_FILTER_HPP


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RWLIBS_ALGORTIHMS_KDTREEQ_HPP_
#define RWLIBS_ALGORTIHMS_KDTREEQ_HPP_

#include "KDTree.hpp"

#include <rw/common/InputArchive.hpp>
#include <rw/common/OutputArchive.hpp>
#include <rw/common/Serializable.hpp>
#include <rw/core/Ptr.hpp>
#include <rw/core/macros.hpp>
#include <rw/math/Math.hpp>
#include <rw/math/MetricUtil.hpp>
#include <rw/math/Q.hpp>

#include <algorithm>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/tuple/tuple.hpp>
#include <float.h>
#include <list>
#include <map>
#include <queue>
#include <vector>

namespace rwlibs { namespace algorithms {

    /** \addtogroup algorithms */
    /*@{*/

    /**
     * @brief a space partitioning structure for organizing points in k-dimensional space.
     * Used for searches involving multi.dimensional search keys, including nearest
     * neighbor and range search.
     *
     * This KDTree implementation takes any value type but the key is constrained to a
     * rw::math::Q
     */
    template< class VALUE_TYPE > class KDTreeQ : public rw::common::Serializable
    {
      private:
        struct TreeNode;

      public:
        typedef rw::core::Ptr< KDTreeQ< VALUE_TYPE > > Ptr;

        typedef rw::math::Q KEY;

        //! a struct for the node in the tree
        struct KDNode
        {
            KDNode (rw::math::Q k, VALUE_TYPE val) : key (k), value (val) {}
            rw::math::Q key;
            VALUE_TYPE value;

            // template<class T>
            // T valueAs() { return boost::any_cast<T>(value); }
            // template<class T>
            // T valueAs() const { return boost::any_cast<T>(value); }
        };

        struct KDResult
        {
            KDResult (KDNode* node, double d) : n (node), dist (d) {}
            KDNode* n;
            double dist;
        };

        typedef KDNode Node;
        typedef KDResult Result;
        typedef VALUE_TYPE Value;

        /**
         * @brief Constructor
         * @param dim [in] the dimension of the keys in the KDTreeQ
         */
        KDTreeQ (size_t dim) :
            _dim (dim), _nrOfNodes (0), _root (NULL), _nodes (new std::vector< TreeNode* > ())
        {}

        /**
         * @brief destructor
         */
        virtual ~KDTreeQ ()
        {
            delete _root;
            delete _nodes;
        };

        /**
         * @brief Builds a KDTreeQ from a list of key values and nodes. This method is more
         * efficient than creating an empty KDTreeQ and then inserting nodes
         * @param nodes [in] a list of KDNode's
         * @return if build succesfull then a pointer to a KD-tree is returned else NULL
         */
        static KDTreeQ* buildTree (std::vector< KDNode >& nodes);

        /**
         * @brief Builds a KDTreeQ from a list of key values and nodes. This method is more
         * efficient than creating an empty KDTreeQ and then inserting nodes
         * @param nodes [in] a list of KDNode's
         * @return if build succesfull then a pointer to a KD-tree is returned else NULL
         */
        static KDTreeQ* buildTree (const std::vector< KDNode* >& nodes);

        /**
         * @brief gets the number of dimensions that this KDTreeQ supports
         * @return the nr of dimensions of this KD-Tree
         */
        size_t getDimensions () const { return _dim; };

        /**
         * @brief adds a key value pair to the KDTreeQ.
         * @param key [in] must be the same length as the dimensionality of the KDTreeQ
         * @param val [in] value that is to be stored at the keys position
         */
        void addNode (const rw::math::Q& key, VALUE_TYPE val);

        /**
         * @brief remove the node with key nnkey
         * @param nnkey [in] the key of the node to remove
         * @return
         */
        void removeNode (const rw::math::Q& nnkey);

        /**
         * @brief finds the KDNode with key equal to nnkey
         * @param nnkey [in] the key that is to be found
         * @return KDNode with key equal to nnkey if existing, else NULL
         */
        KDNode* search (const rw::math::Q& nnkey);

        /**
         * @brief finds the KDNode with the key closest too nnkey
         * @param nnkey [in] the key to which the nearest neighbor is found
         * @return the nearest neighbor to nnkey
         */
        KDNode& nnSearch (const rw::math::Q& nnkey);

        /**
         * @brief finds all neighbors in the hyperelipse with radius radi and center in nnkey.
         * @param nnkey [in] the center of the hyperelipse
         * @param radi [in] the radius of the hyperelipse in euclidean 2-norm
         * @param nodes [out] a container for all nodes that is found within the hyperelipse
         */
        void nnSearchElipse (const rw::math::Q& nnkey, const rw::math::Q& radi,
                             std::list< const KDNode* >& nodes);

        /**
         * @brief finds all neighbors in the hyperelipse with radius radi and center in nnkey.
         * @param nnkey [in] the center of the hyperelipse
         * @param radi [in] the radius of the hyperelipse in euclidean 2-norm
         * @param nodes [out] a container for all nodes that is found within the hyperelipse
         */
        void nnSearchElipseRect (const rw::math::Q& nnkey, const rw::math::Q& radi,
                                 std::list< const KDNode* >& nodes);

        /**
         * @brief finds all neighbors in the hyperrectangle defined by the lower bound and the
         * upper bound
         */
        void nnSearchRect (const rw::math::Q& low, const rw::math::Q& upp,
                           std::list< const KDNode* >& nodes);

      private:
        size_t _dim;
        size_t _nrOfNodes;
        TreeNode* _root;
        std::vector< TreeNode* >* _nodes;

      public:
        void read (rw::common::InputArchive& iarchive, const std::string& id)
        {
            std::string name, data;
            int dim, nrNodes;
            boost::uint64_t rootId;
            iarchive.read (dim, "dim");
            iarchive.read (nrNodes, "nrNodes");
            rootId = iarchive.readUInt64 ("rootId");

            std::vector< TreeNode* >* nodes = new std::vector< TreeNode* > (nrNodes);
            std::vector< int > idToNodeIdx (nrNodes);
            std::map< boost::uint64_t, boost::tuple< TreeNode*, boost::uint64_t, boost::uint64_t > >
                toNode;
            for (int i = 0; i < nrNodes; i++) {
                (*nodes)[i]    = new TreeNode ();
                TreeNode& node = *(*nodes)[i];

                boost::uint64_t id = iarchive.readUInt64 ("id");
                node._axis         = iarchive.readInt ("axis");
                node._deleted      = iarchive.readBool ("del");

                boost::uint64_t leftId  = iarchive.readUInt64 ("left");
                boost::uint64_t rightId = iarchive.readUInt64 ("right");

                iarchive.read (node._kdnode->key, "Q");
                iarchive.read (node._kdnode->value, "value");

                toNode[id]     = boost::make_tuple (&node, leftId, rightId);
                idToNodeIdx[i] = static_cast< int > (id);
            }
            toNode[0] =
                boost::make_tuple ((TreeNode*) NULL, (boost::uint64_t) 0, (boost::uint64_t) 0);

            // finally travel through nodes and set the correct left/right values
            for (int i = 0; i < nrNodes; i++) {
                TreeNode& node = *(*nodes)[i];

                boost::tuple< TreeNode*, boost::uint64_t, boost::uint64_t > val =
                    toNode[idToNodeIdx[i]];
                const boost::uint64_t leftIdx = boost::get< 1 > (val);
                node._left                    = boost::get< 0 > (toNode[leftIdx]);
                node._right                   = boost::get< 0 > (toNode[boost::get< 2 > (val)]);
            }
            TreeNode* root = boost::get< 0 > (toNode[rootId]);
            _dim           = root->_kdnode->key.size ();
            _root          = root;
            _nodes         = nodes;
        }

        void write (rw::common::OutputArchive& oarchive, const std::string& id) const
        {
            oarchive.write (_dim, "dim");
            oarchive.write ((int) _nodes->size (), "nrNodes");
            oarchive.write ((boost::uint64_t) _root, "rootId");
            RW_ASSERT (_nrOfNodes == _nodes->size ());
            for (std::size_t i = 0; i < _nodes->size (); i++) {
                const TreeNode& node = *(*_nodes)[i];
                oarchive.write ((boost::uint64_t) &node, "id");
                oarchive.write ((int) node._axis, "axis");
                oarchive.write (node._deleted, "del");
                oarchive.write ((boost::uint64_t) node._left, "left");
                oarchive.write ((boost::uint64_t) node._right, "right");

                oarchive.write (node._kdnode->key, "Q");
                oarchive.write (node._kdnode->value, "value");
            }
        }

        /*
                static void save(RWOutputArchive& archive);
                static KDTreeQ* load(RWInputArchive& archive){

                }

                static void save(KDTreeQ* tree, ValueSerializer& serializer, const std::string&
           filename); static KDTreeQ* load(ValueSerializer& serializer, const std::string&
           filename);
                */

      private:
        //! default constructor
        KDTreeQ () :
            _dim (0), _nrOfNodes (0), _root (NULL), _nodes (new std::vector< TreeNode* > ())
        {}

        //! constructor
        KDTreeQ (TreeNode* root, std::vector< TreeNode* >* nodes) :
            _dim (root->_kdnode->key.size ()), _nrOfNodes (nodes->size ()), _root (root),
            _nodes (nodes)
        {}

        /**
         * @brief Internal representation of a KD Tree Node. To save processing time when deleting
         * TreeNodes, a boolean is kept that say if the node is deleted or not. If deleted all
         * rutines kan skip the node and forward the call to its children.
         */
        struct TreeNode
        {
          public:
            TreeNode () :
                _left (NULL), _right (NULL), _kdnode (NULL), _deleted (false), _axis (0){};

            TreeNode (KDNode* node) :
                _left (NULL), _right (NULL), _kdnode (node), _deleted (false), _axis (0){};

            TreeNode (TreeNode* left, TreeNode* right, KDNode* node) :
                _left (left), _right (right), _kdnode (node), _deleted (false), _axis (0){};

            static void swap (TreeNode& n1, TreeNode& n2)
            {
                std::swap (n1._left, n2._left);
                std::swap (n1._right, n2._right);
                std::swap (n1._kdnode, n2._kdnode);
            }

            TreeNode *_left, *_right;
            KDNode* _kdnode;
            bool _deleted;          //
            unsigned char _axis;    // the splitting axis
        };
        /*
                struct SimpleCompare {
                private:
                    size_t _dim;
                public:
                    SimpleCompare(size_t dim):_dim(dim){};

                    bool operator()(const TreeNode& e1, const TreeNode& e2) {
                        RW_ASSERT(e1._kdnode);
                        RW_ASSERT(e2._kdnode);
                        return e1._kdnode->key[_dim] < e2._kdnode->key[_dim] ;
                    }
                };
        */
        struct SimpleCompare2
        {
          private:
            size_t _dim;

          public:
            SimpleCompare2 (size_t dim) : _dim (dim){};

            bool operator() (const TreeNode* e1, const TreeNode* e2)
            {
                return e1->_kdnode->key[_dim] < e2->_kdnode->key[_dim];
            }
        };

        static TreeNode* buildBalancedRec (std::vector< TreeNode* >& tNodes, int startIdx,
                                           int endIdx, size_t depth, size_t nrOfDims)
        {
            if (endIdx <= startIdx)
                return NULL;

            // std::cout << "RecBuild(" << startIdx << "," << endIdx << ")" << std::endl;
            size_t len = endIdx - startIdx;
            size_t dim = depth % nrOfDims;

            // the compare func can
            std::sort (tNodes.begin () + startIdx, tNodes.begin () + endIdx, SimpleCompare2 (dim));
            // check sorting is okay
            for (int i = startIdx; i < endIdx - 1; i++)
                if (tNodes[i]->_kdnode->key[dim] > tNodes[i + 1]->_kdnode->key[dim])
                    RW_WARN (" sort not working!" << i);
            size_t medianIdx = startIdx + len / 2;
            TreeNode* mNode  = tNodes[medianIdx];
            mNode->_axis     = 0xFF & dim;
            mNode->_left =
                buildBalancedRec (tNodes, startIdx, (int) medianIdx, depth + 1, nrOfDims);
            mNode->_right =
                buildBalancedRec (tNodes, (int) (medianIdx + 1), endIdx, depth + 1, nrOfDims);
            return mNode;
        }

        void nnSearchRec (const rw::math::Q& nnkey, TreeNode* node, rw::math::Q& min,
                          rw::math::Q& max, KDResult& out)
        {
            using namespace rw::math;
            if (node == NULL)
                return;
            size_t axis = node->_axis;
            // std::cout << "nnSearchRec("<< axis << ")" << std::endl;

            Q& key = node->_kdnode->key;
            double distSqr (MetricUtil::dist2Sqr (nnkey, key));
            // std::cout << "le" << std::endl;

            // if this node is closer than any other then update out
            if (distSqr < out.dist && !node->_deleted) {
                out.dist = distSqr;
                out.n    = node->_kdnode;
            }
            // stop if the distance is very small
            if (distSqr < kdtree_epsilon)
                return;
            // std::cout << "1" << std::endl;
            // call nnSearch recursively with closerNode,
            // closestNode and closestDistSqr is updated
            bool isLeftClosest = nnkey (axis) < key (axis);
            if (isLeftClosest) {
                // std::cout << "left" << std::endl;
                // left is closest, backup split value and make the recursive call
                double maxTmp = max (axis);
                max (axis)    = key (axis);
                nnSearchRec (nnkey, node->_left, min, max, out);
                // undo the change of max
                max (axis) = maxTmp;
            }
            else {
                // std::cout << "right" << std::endl;
                // right is closest, backup split value and make the recursive call
                double minTmp = min (axis);
                min (axis)    = key (axis);
                nnSearchRec (nnkey, node->_right, min, max, out);
                // undo the change of max
                min (axis) = minTmp;
            }
            // std::cout << "2" << std::endl;

            // next check if fartherNode split plane lies closer than closestDistSqr
            if (Math::sqr (nnkey (axis) - key (axis)) >= out.dist)
                return;
            // std::cout << "3" << std::endl;

            bool isLeftFarthest = !isLeftClosest;
            // if closest point in hyperrect of farther node is closer than closest
            // then call nnSearch recursively with farther node
            if (isLeftFarthest) {
                double maxTmp       = max (axis);
                max (axis)          = key (axis);
                rw::math::Q closest = Math::clampQ (nnkey, min, max);
                if (MetricUtil::dist2Sqr (nnkey, closest) < out.dist)
                    nnSearchRec (nnkey, node->_left, min, max, out);
                // undo the change of max
                max (axis) = maxTmp;
            }
            else {
                double minTmp       = min (axis);
                min (axis)          = key (axis);
                rw::math::Q closest = Math::clampQ (nnkey, min, max);
                if (MetricUtil::dist2Sqr (nnkey, closest) < out.dist)
                    nnSearchRec (nnkey, node->_right, min, max, out);
                // undo the change of max
                min (axis) = minTmp;
            }
        };

        void nnSearchElipseRec (const rw::math::Q& nnkey, TreeNode* node, rw::math::Q& min,
                                rw::math::Q& max, double maxRadiSqr,
                                std::list< const KDNode* >& nodes)
        {
            using namespace rw::math;
            if (node == NULL)
                return;
            size_t axis = node->_axis;
            // std::cout << "nnSearchRec("<< axis << ")" << std::endl;

            Q& key = node->_kdnode->key;
            double distSqr (MetricUtil::dist2Sqr (nnkey, key));

            // if this node is closer than any other then update out
            if (distSqr < maxRadiSqr && !node->_deleted) {
                nodes.push_back (node->_kdnode);
            }
            // stop if the distance is very small
            if (distSqr < kdtree_epsilon)
                return;

            // call nnSearch recursively with closerNode,
            // closestNode and closestDistSqr is updated
            bool isLeftClosest = nnkey (axis) < key (axis);
            if (isLeftClosest) {
                // left is closest, backup split value and make the recursive call
                double maxTmp = max (axis);
                max (axis)    = key (axis);
                nnSearchElipseRec (nnkey, node->_left, min, max, maxRadiSqr, nodes);
                // undo the change of max
                max (axis) = maxTmp;
            }
            else {
                // right is closest, backup split value and make the recursive call
                double minTmp = min (axis);
                min (axis)    = key (axis);
                nnSearchElipseRec (nnkey, node->_right, min, max, maxRadiSqr, nodes);
                // undo the change of max
                min (axis) = minTmp;
            }

            // next check if fartherNode split plane lies closer than closestDistSqr
            if (Math::sqr (nnkey (axis) - key (axis)) >= maxRadiSqr)
                return;

            bool isLeftFarthest = !isLeftClosest;
            // if closest point in hyperrect of farther node is closer than closest
            // then call nnSearch recursively with farther node
            if (isLeftFarthest) {
                double maxTmp       = max (axis);
                max (axis)          = key (axis);
                rw::math::Q closest = Math::clampQ (nnkey, min, max);
                if (MetricUtil::dist2Sqr (nnkey, closest) < maxRadiSqr)
                    nnSearchElipseRec (nnkey, node->_left, min, max, maxRadiSqr, nodes);
                // undo the change of max
                max (axis) = maxTmp;
            }
            else {
                double minTmp       = min (axis);
                min (axis)          = key (axis);
                rw::math::Q closest = Math::clampQ (nnkey, min, max);
                if (MetricUtil::dist2Sqr (nnkey, closest) < maxRadiSqr)
                    nnSearchElipseRec (nnkey, node->_right, min, max, maxRadiSqr, nodes);
                // undo the change of max
                min (axis) = minTmp;
            }
        };
    };

    template< class T >
    KDTreeQ< T >* KDTreeQ< T >::buildTree (std::vector< typename KDTreeQ< T >::KDNode >& nodes)
    {
        if (nodes.size () == 0)
            return NULL;

        // create all tree nodes in a list
        std::vector< TreeNode* >* tNodes = new std::vector< TreeNode* > (nodes.size ());
        // copy the KDNodes into the tree nodes
        for (unsigned int i = 0; i < tNodes->size (); i++) {
            (*tNodes)[i]          = new TreeNode ();
            (*tNodes)[i]->_kdnode = new KDTreeQ< T >::KDNode (nodes[i]);
        }

        // create a simple median balanced tree
        size_t nrOfDims = nodes.front ().key.size ();
        TreeNode* root  = buildBalancedRec (*tNodes, 0, (int) tNodes->size (), 0, nrOfDims);
        return new KDTreeQ< T > (root, tNodes);
    }

    template< class T >
    KDTreeQ< T >*
    KDTreeQ< T >::buildTree (const std::vector< typename KDTreeQ< T >::KDNode* >& nodes)
    {
        if (nodes.size () == 0)
            return NULL;

        // create all tree nodes in a list
        std::vector< TreeNode* >* tNodes = new std::vector< TreeNode* > (nodes.size ());

        // copy the KDNodes into the tree nodes
        int i = 0;
        for (KDNode* n : nodes) {
            (*tNodes)[i]          = new TreeNode ();
            (*tNodes)[i]->_kdnode = n;
            i++;
        }

        // create a simple median balanced tree
        size_t nrOfDims      = nodes.front ()->key.size ();
        const int tNodesSize = boost::numeric_cast< int > (tNodes->size ());
        TreeNode* root       = buildBalancedRec (*tNodes, 0, tNodesSize, 0, nrOfDims);

        return new KDTreeQ< T > (root, tNodes);
    }

    template< class T >
    typename KDTreeQ< T >::KDNode* KDTreeQ< T >::search (const rw::math::Q& nnkey)
    {
        TreeNode* tmpNode = _root;
        for (size_t lev = 0; tmpNode != NULL; lev = (lev + 1) % _dim) {
            rw::math::Q& key = tmpNode->_kdnode->key;
            if (nnkey (lev) == key (lev) && !(tmpNode->_deleted) && (nnkey == key)) {
                return tmpNode->_kdnode;
            }
            else if (nnkey (lev) > key (lev)) {
                tmpNode = tmpNode->_right;
            }
            else {
                tmpNode = tmpNode->_left;
            }
        }
        return NULL;
    }

    template< class T >
    typename KDTreeQ< T >::KDNode& KDTreeQ< T >::nnSearch (const rw::math::Q& nnkey)
    {
        // std::cout << "nnSearch " << _dim << std::endl;

        RW_ASSERT (nnkey.size () == _dim);
        if (_root == NULL)
            RW_THROW ("KDTreeQ has no data!");

        rw::math::Q min (_dim), max (_dim);
        KDResult result (NULL, DBL_MAX);
        for (size_t i = 0; i < _dim; i++) {
            min (i) = -DBL_MAX;
            max (i) = DBL_MAX;
        }
        // std::cout << "nnSearchRec" << std::endl;
        nnSearchRec (nnkey, _root, min, max, result);
        if (result.n == NULL)
            RW_THROW ("KDTreeQ has no data!");
        return *result.n;
    }

    template< class T > void KDTreeQ< T >::removeNode (const rw::math::Q& nnkey)
    {
        TreeNode* tmpNode = _root;
        for (size_t lev = 0; tmpNode != NULL; lev = (lev + 1) % _dim) {
            rw::math::Q& key = tmpNode->_kdnode->key;
            if (nnkey (lev) == key (lev) && !(tmpNode->_deleted) && (nnkey == key)) {
                tmpNode->_deleted = true;
                return;
            }
            else if (nnkey (lev) > key (lev)) {
                tmpNode = tmpNode->_right;
            }
            else {
                tmpNode = tmpNode->_left;
            }
        }
    }

    template< class T >
    void KDTreeQ< T >::nnSearchElipse (const rw::math::Q& nnkey, const rw::math::Q& radi,
                                       std::list< const KDNode* >& nodes)
    {
        // typedef std::pair<TreeNode*,size_t> QElem;
        using namespace rw::math;
        std::queue< TreeNode* > unhandled;
        unhandled.push (_root);
        double distSqr  = MetricUtil::norm2Sqr (radi);
        double distRadi = sqrt (distSqr);
        Q low (_dim), upp (_dim);
        for (size_t i = 0; i < _dim; i++) {
            low (i) = nnkey (i) - distRadi;
            upp (i) = nnkey (i) + distRadi;
        }

        nnSearchElipseRec (nnkey, _root, low, upp, distSqr, nodes);
    }

    template< class T >
    void KDTreeQ< T >::nnSearchElipseRect (const rw::math::Q& nnkey, const rw::math::Q& radi,
                                           std::list< const KDNode* >& nodes)
    {
        // typedef std::pair<TreeNode*,size_t> QElem;
        using namespace rw::math;
        std::queue< TreeNode* > unhandled;
        unhandled.push (_root);
        double distSqr  = MetricUtil::norm2Sqr (radi);
        double distRadi = sqrt (distSqr);
        Q low (_dim), upp (_dim);
        for (size_t i = 0; i < _dim; i++) {
            low (i) = nnkey (i) - distRadi;
            upp (i) = nnkey (i) + distRadi;
        }

        while (!unhandled.empty ()) {
            // std::cout << "unhandled size: " << unhandled.size() << std::endl;
            TreeNode* n = unhandled.front ();
            unhandled.pop ();

            unsigned char axis = n->_axis;
            rw::math::Q& key   = n->_kdnode->key;

            // std::cout << "Axis: " << axis << std::endl;

            // if the key is in range then add it to the result
            size_t j;
            for (j = 0; j < _dim && low[j] <= key[j] && upp[j] >= key[j]; j++)
                ;
            // std::cout << j << "==" << _dim << " k:" << key << std::endl;
            if (j == _dim) {    // this is in range if
                double dist = MetricUtil::dist2Sqr (nnkey, key);
                // std::cout << "Dist: " << dist << " < " << distSqr << std::endl;
                if (dist < distSqr)
                    nodes.push_back (n->_kdnode);
            }

            // add the children to the unhandled queue if the current dimension
            if ((low (axis) <= key (axis)) && (n->_left != NULL))
                unhandled.push (n->_left);
            if ((upp (axis) > key (axis)) && (n->_right != NULL))
                unhandled.push (n->_right);
        }
    }

    template< class T >
    void KDTreeQ< T >::nnSearchRect (const rw::math::Q& low, const rw::math::Q& upp,
                                     std::list< const KDNode* >& nodes)
    {
        // typedef std::pair<TreeNode*,size_t> QElem;
        std::queue< TreeNode* > unhandled;
        unhandled.push (_root);

        // std::cout << "nnSearchRect: "<< std::endl;
        // std::cout << "- low bound: "<< low << std::endl;
        // std::cout << "- upp bound: "<< upp << std::endl;

        while (!unhandled.empty ()) {
            // std::cout << "unhandled size: " << unhandled.size() << std::endl;
            TreeNode* n = unhandled.front ();
            unhandled.pop ();

            unsigned char axis = n->_axis;
            rw::math::Q& key   = n->_kdnode->key;

            // std::cout << "Axis: " << axis << std::endl;

            // if the key   is in range then add it to the result
            size_t j;
            for (j = 0; j < _dim && low[j] <= key[j] && key[j] <= upp[j]; j++)
                ;

            // std::cout << j << "==" << _dim << " k:" << key << std::endl;
            if (j == _dim && n->_deleted == false)    // this is in range
                nodes.push_back (n->_kdnode);

            // add the children to the unhandled queue if the current dimension
            if ((low (axis) <= key (axis)) && (n->_left != NULL))
                unhandled.push (n->_left);

            if ((upp (axis) > key (axis)) && (n->_right != NULL))
                unhandled.push (n->_right);
        }
    }

    template< class T > void KDTreeQ< T >::addNode (const rw::math::Q& nnkey, T val)
    {
        // first test if root is empty. if it is add value as root
        if (_root == NULL) {
            _nodes->push_back (new TreeNode (new KDNode (nnkey, val)));
            _root = _nodes->back ();
            return;
        }

        // else find the parent in which this is to be inserted
        TreeNode* tmpNode = _root;
        // find the leaf in which to insert the new value
        for (size_t lev = 0; tmpNode != NULL; lev = (lev + 1) % _dim) {
            KEY& key = tmpNode->_kdnode->key;

            if (nnkey (lev) > key (lev)) {
                if (tmpNode->_right == NULL) {
                    TreeNode* node = new TreeNode (new KDNode (nnkey, val));
                    node->_axis    = static_cast< unsigned char > ((lev + 1) % _dim);
                    _nodes->push_back (node);
                    tmpNode->_right = _nodes->back ();
                    return;
                }
                tmpNode = tmpNode->_right;
            }
            else {
                if (tmpNode->_left == NULL) {
                    TreeNode* node = new TreeNode (new KDNode (nnkey, val));
                    node->_axis    = static_cast< unsigned char > ((lev + 1) % _dim);
                    _nodes->push_back (node);
                    tmpNode->_left = _nodes->back ();
                    return;
                }
                tmpNode = tmpNode->_left;
            }
        }
    }

    /**@}*/

}}    // namespace rwlibs::algorithms

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2015 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestInvoke
#include <boost/test/unit_test.hpp>

#include <boost/compute/function.hpp>
#include <boost/compute/lambda.hpp>
#include <boost/compute/functional/integer.hpp>
#include <boost/compute/functional/math.hpp>
#include <boost/compute/utility/invoke.hpp>

#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(invoke_builtin)
{
    BOOST_CHECK_EQUAL(compute::invoke(compute::abs<int>(), queue, -3), 3);
    BOOST_CHECK_CLOSE(compute::invoke(compute::pow<float>(), queue, 2.f, 8.f), 256.f, 1e-4);
}

BOOST_AUTO_TEST_CASE(invoke_function)
{
    BOOST_COMPUTE_FUNCTION(int, plus_two, (int x),
    {
        return x + 2;
    });

    BOOST_CHECK_EQUAL(compute::invoke(plus_two, queue, 2), 4);

    BOOST_COMPUTE_FUNCTION(float, get_max, (float x, float y),
    {
      if(x > y)
          return x;
      else
          return y;
    });

    BOOST_CHECK_EQUAL(compute::invoke(get_max, queue, 10.f, 20.f), 20.f);
}

BOOST_AUTO_TEST_CASE(invoke_lambda)
{
    using boost::compute::lambda::_1;
    using boost::compute::lambda::_2;

    BOOST_CHECK_EQUAL(compute::invoke(_1 / 2, queue, 4), 2);
    BOOST_CHECK_EQUAL(compute::invoke(_1 * _2 + 1, queue, 3, 3), 10);
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_TEST_INTERSECTION_HPP
#define BOOST_GEOMETRY_TEST_INTERSECTION_HPP

#include <fstream>
#include <iomanip>

#include <boost/foreach.hpp>
#include <boost/variant/variant.hpp>

#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/is_valid.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/algorithms/num_points.hpp>

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

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

#include <boost/geometry/io/wkt/wkt.hpp>

#if defined(TEST_WITH_SVG)
#  include <boost/geometry/io/svg/svg_mapper.hpp>
#endif

#include <geometry_test_common.hpp>

struct ut_settings
{
    double percentage;
    bool test_validity;
    bool debug;

    ut_settings(double p = 0.0001)
        : percentage(p)
        , test_validity(false)
        , debug(false)
    {}

};

template <typename G1, typename G2, typename OutputType>
typename bg::default_area_result<G1>::type
check_result(
    std::vector<OutputType> const& intersection_output,
    std::string const& caseid,
    std::size_t expected_count, int expected_point_count,
    double expected_length_or_area,
    ut_settings const& settings)
{
    bool const is_line = bg::geometry_id<OutputType>::type::value == 2;

    typename bg::default_area_result<G1>::type length_or_area = 0;
    int n = 0;
    for (typename std::vector<OutputType>::const_iterator it = intersection_output.begin();
            it != intersection_output.end();
            ++it)
    {
        if (expected_point_count > 0)
        {
            // here n should rather be of type std::size_t, but expected_point_count
            // is set to -1 in some test cases so type int was left for now
            n += static_cast<int>(bg::num_points(*it, true));
        }

        // instead of specialization we check it run-time here
        length_or_area += is_line
            ? bg::length(*it)
            : bg::area(*it);

        if (settings.debug)
        {
            std::cout << std::setprecision(20) << bg::wkt(*it) << std::endl;
        }

        if (settings.test_validity)
        {
            std::string message;
            bool const valid = bg::is_valid(*it, message);
            BOOST_CHECK_MESSAGE(valid,
                "intersection: " << caseid << " not valid " << message);
        }
    }

#if ! defined(BOOST_GEOMETRY_NO_BOOST_TEST)
#if ! defined(BOOST_GEOMETRY_NO_ROBUSTNESS)
    if (expected_point_count > 0)
    {
        BOOST_CHECK_MESSAGE(bg::math::abs(n - expected_point_count) < 3,
                "intersection: " << caseid
                << " #points expected: " << expected_point_count
                << " detected: " << n
                << " type: " << (type_for_assert_message<G1, G2>())
                );
    }
#endif

    if (expected_count > 0)
    {
        BOOST_CHECK_MESSAGE(intersection_output.size() == expected_count,
                "intersection: " << caseid
                << " #outputs expected: " << expected_count
                << " detected: " << intersection_output.size()
                << " type: " << (type_for_assert_message<G1, G2>())
                );
    }

    double const detected_length_or_area = boost::numeric_cast<double>(length_or_area);
    if (settings.percentage > 0.0)
    {
        BOOST_CHECK_CLOSE(detected_length_or_area, expected_length_or_area, settings.percentage);
    }
    else
    {
        // In some cases (geos_2) the intersection is either 0, or a tiny rectangle,
        // depending on compiler/settings. That cannot be tested by CLOSE
        BOOST_CHECK_LE(detected_length_or_area, expected_length_or_area);
    }
#endif

    return length_or_area;
}

template <typename OutputType, typename CalculationType, typename G1, typename G2>
typename bg::default_area_result<G1>::type test_intersection(std::string const& caseid,
        G1 const& g1, G2 const& g2,
        std::size_t expected_count = 0, int expected_point_count = 0,
        double expected_length_or_area = 0,
        ut_settings const& settings = ut_settings())
{
    if (settings.debug)
    {
        std::cout << std::endl << "case " << caseid << std::endl;
    }

    typedef typename bg::point_type<G1>::type point_type;

    if (! settings.debug)
    {
        // Check _inserter behaviour with stratey
        typedef bg::strategy_intersection
            <
                typename bg::cs_tag<point_type>::type,
                G1,
                G2,
                point_type,
                typename bg::rescale_policy_type<point_type>::type,
                CalculationType
            > strategy;
        std::vector<OutputType> clip;
        bg::detail::intersection::intersection_insert<OutputType>(g1, g2, std::back_inserter(clip), strategy());
    }

    typename bg::default_area_result<G1>::type length_or_area = 0;

    // Check normal behaviour
    std::vector<OutputType> intersection_output;
    bg::intersection(g1, g2, intersection_output);

    check_result<G1, G2>(intersection_output, caseid, expected_count, expected_point_count,
        expected_length_or_area, settings);

    // Check variant behaviour
    intersection_output.clear();
    bg::intersection(boost::variant<G1>(g1), g2, intersection_output);

    check_result<G1, G2>(intersection_output, caseid, expected_count, expected_point_count,
        expected_length_or_area, settings);

    intersection_output.clear();
    bg::intersection(g1, boost::variant<G2>(g2), intersection_output);

    check_result<G1, G2>(intersection_output, caseid, expected_count, expected_point_count,
        expected_length_or_area, settings);

    intersection_output.clear();
    bg::intersection(boost::variant<G1>(g1), boost::variant<G2>(g2), intersection_output);

    check_result<G1, G2>(intersection_output, caseid, expected_count, expected_point_count,
        expected_length_or_area, settings);

#if defined(TEST_WITH_SVG)
    {
        bool const is_line = bg::geometry_id<OutputType>::type::value == 2;
        typedef typename bg::coordinate_type<G1>::type coordinate_type;

        bool const ccw =
            bg::point_order<G1>::value == bg::counterclockwise
            || bg::point_order<G2>::value == bg::counterclockwise;
        bool const open =
            bg::closure<G1>::value == bg::open
            || bg::closure<G2>::value == bg::open;

        std::ostringstream filename;
        filename << "intersection_"
            << caseid << "_"
            << string_from_type<coordinate_type>::name()
            << string_from_type<CalculationType>::name()
            << (ccw ? "_ccw" : "")
            << (open ? "_open" : "")
#if defined(BOOST_GEOMETRY_NO_ROBUSTNESS)
            << "_no_rob"
#endif
            << ".svg";

        std::ofstream svg(filename.str().c_str());

        bg::svg_mapper<point_type> mapper(svg, 500, 500);

        mapper.add(g1);
        mapper.add(g2);

        mapper.map(g1, is_line
            ? "opacity:0.6;stroke:rgb(0,255,0);stroke-width:5"
            : "fill-opacity:0.5;fill:rgb(153,204,0);"
                    "stroke:rgb(153,204,0);stroke-width:3");
        mapper.map(g2, "fill-opacity:0.3;fill:rgb(51,51,153);"
                    "stroke:rgb(51,51,153);stroke-width:3");

        for (typename std::vector<OutputType>::const_iterator it = intersection_output.begin();
                it != intersection_output.end(); ++it)
        {
            mapper.map(*it, "fill-opacity:0.2;stroke-opacity:0.4;fill:rgb(255,0,0);"
                        "stroke:rgb(255,0,255);stroke-width:8");
        }
    }
#endif

    if (settings.debug)
    {
        std::cout << "end case " << caseid << std::endl;
    }

    return length_or_area;
}

template <typename OutputType, typename G1, typename G2>
typename bg::default_area_result<G1>::type test_one(std::string const& caseid,
        std::string const& wkt1, std::string const& wkt2,
        std::size_t expected_count = 0, int expected_point_count = 0,
        double expected_length_or_area = 0,
        ut_settings const& settings = ut_settings())
{
    G1 g1;
    bg::read_wkt(wkt1, g1);

    G2 g2;
    bg::read_wkt(wkt2, g2);

    // Reverse if necessary
    bg::correct(g1);
    bg::correct(g2);

    return test_intersection<OutputType, void>(caseid, g1, g2,
        expected_count, expected_point_count,
        expected_length_or_area, settings);
}

template <typename OutputType, typename Areal, typename Linear>
void test_one_lp(std::string const& caseid,
        std::string const& wkt_areal, std::string const& wkt_linear,
        std::size_t expected_count = 0, int expected_point_count = 0,
        double expected_length = 0,
        ut_settings const& settings = ut_settings())
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << caseid << " -- start" << std::endl;
#endif
    Areal areal;
    bg::read_wkt(wkt_areal, areal);
    bg::correct(areal);

    Linear linear;
    bg::read_wkt(wkt_linear, linear);

    test_intersection<OutputType, void>(caseid, areal, linear,
        expected_count, expected_point_count,
        expected_length, settings);

    // A linestring reversed should deliver exactly the same.
    bg::reverse(linear);

    test_intersection<OutputType, void>(caseid + "_rev", areal, linear,
        expected_count, expected_point_count,
        expected_length, settings);
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << caseid << " -- end" << std::endl;
#endif
}

template <typename Geometry1, typename Geometry2>
void test_point_output(std::string const& wkt1, std::string const& wkt2, unsigned int expected_count)
{
    Geometry1 g1;
    bg::read_wkt(wkt1, g1);
    bg::correct(g1);

    Geometry2 g2;
    bg::read_wkt(wkt2, g2);
    bg::correct(g2);

    std::vector<typename bg::point_type<Geometry1>::type> points;
    bg::intersection(g1, g2, points);
    BOOST_CHECK_EQUAL(points.size(), expected_count);
}

#endif


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with parallel SGD. We first create factors and then a data matrix
 * from these factors. This process ensures that we know the best factorization of the input.
 * These matrices are distributed across a cluster. We then try to reconstruct the factors
 * using PSGD.
 *
 * Run with: psgd
 * (make sure to use a production build, otherwise it will be slow)
 */
#include <iostream>
#include <sstream>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>

#include <mpi2/mpi2.h>
#include <mf/mf.h>

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

using namespace std;
using namespace mf;
using namespace mpi2;
using namespace rg;
using namespace boost::numeric::ublas;

// type of SGD
typedef UpdateTruncate<UpdateNzslL2> Update;
typedef UpdateLock<Update> UpdateL;
typedef RegularizeNone Regularize;
typedef SumLoss<NzslLoss, L2Loss> Loss;
typedef NzslLoss TestLoss;

MPI2_TYPE_TRAITS(UpdateL);

int main(int argc, char* argv[]) {
	// initialize mf library and mpi2
	boost::mpi::communicator& world = mfInit(argc, argv);

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 1000000;
	double sigma = sqrt(10); // standard deviation
	double lambda = 1/sigma/sigma;
	mf_size_type r = 10;

	// parameters for distribution
	int tasks = 4;

	// parameters for SGD
	double eps0 = 0.01;
	mf_size_type epochs = 20;
	SgdOrder order = SGD_ORDER_WOR;
	PsgdShuffle shuffle = PSGD_SHUFFLE_PARALLEL;
	Update update = Update(UpdateNzslL2(lambda), -10*sigma, 10*sigma); // truncate for numerical stability
	UpdateL updateLock = UpdateL(update, size1, size2); // with locking
	Regularize regularize;
	Loss loss((NzslLoss()), L2Loss(lambda));
	TestLoss testLoss;
	mf_size_type testNnz = nnz/100;
	BalanceType balanceType = BALANCE_NONE;
	BalanceMethod balanceMethod = BALANCE_OPTIMAL;

	// start mf library
	mfStart();

	if (world.rank() == 0)
	{
#ifndef NDEBUG
		LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif
		LOG4CXX_INFO(logger, "Using " << tasks << " parallel tasks");

		// TODO: distribute matrix generation
		// generate original factors by sampling from a normal(0,sigma) distribution
		Random32 random; // note: this takes a default seed (not randomized!)
		DenseMatrix wIn(size1, r);
		DenseMatrixCM hIn(r, size2);
		generateRandom(wIn, random, boost::normal_distribution<>(0, sigma));
		generateRandom(hIn, random, boost::normal_distribution<>(0, sigma));

		// generate a sparse matrix by selecting random entries from the generated factors
		// and add small Gaussian noise
		SparseMatrix v;
		generateRandom(v, nnz, wIn, hIn, random);
		addRandom(v, random, boost::normal_distribution<>(0, 0.1));
		LOG4CXX_INFO(logger, "Data matrix: "
			<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
		LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));

		// create a test matrix (without noise)
		SparseMatrix vTest;
		generateRandom(vTest, testNnz, wIn, hIn, random);
		LOG4CXX_INFO(logger, "Test matrix: "
			<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

		// take a small sample and remove empty rows/columns
		ProjectedSparseMatrix Vsample;
		projectRandomSubmatrix(random, v, Vsample, v.size1()/5, v.size2()/5);
		projectFrequent(Vsample, 0);
		LOG4CXX_INFO(logger, "Sample matrix: "
			<< Vsample.data.size1() << " x " << Vsample.data.size2()
			<< ", " << Vsample.data.nnz() << " nonzeros");

		// generate initial factors by sampling from a uniform[-0.5,0.5] distribution
		DenseMatrix w(size1, r);
		DenseMatrixCM h(r, size2);
		generateRandom(w, random, boost::uniform_real<>(-0.5, 0.5));
		generateRandom(h, random, boost::uniform_real<>(-0.5, 0.5));

		// initialize
		Timer t;
		PsgdRunner psgdRunner(random);
		PsgdJob<Update,Regularize> psgdJob(v, w, h, update, regularize, order, tasks, shuffle);
		PsgdJob<UpdateL,Regularize> psgdJobLock(v, w, h, updateLock, regularize, order, tasks, shuffle);
		ParallelDecayAuto<Update,Regularize,Loss> decay(psgdJob, loss, Vsample, eps0, tasks);
		Trace trace;

		// print the test loss
		FactorizationData<> testData(vTest, w, h);
		LOG4CXX_INFO(logger, "Initial test loss: " << testLoss(testData));

		// run PSGD to try to reconstruct the original factors
		t.start();
//		psgdRunner.run(psgdJob, loss, epochs, decay, trace, balanceType, balanceMethod, &testData, &testLoss);
		psgdRunner.run(psgdJobLock, loss, epochs, decay, trace, balanceType, balanceMethod, &testData, &testLoss);

		t.stop();
		LOG4CXX_INFO(logger, "Total time: " << t);

		// print the test loss
		LOG4CXX_INFO(logger, "Final test loss: " << testLoss(testData));

		// write trace to an R file
		LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/psgd-trace.R");
		trace.toRfile("/tmp/psgd-trace.R", "psgd");
	}

	mfStop();
	mfFinalize();

	return 0;
}


/*
 * FLSPriorErrorTerm.hpp
 *
 *  Created on: Apr 26, 2013
 *      Author: mbuerki
 */

#ifndef MARGINALIZATIONPRIORERRORTERM_HPP_
#define MARGINALIZATIONPRIORERRORTERM_HPP_

#include <aslam/backend/ErrorTermDs.hpp>
#include <aslam/backend/DesignVariable.hpp>

#include <Eigen/Core>

#include <boost/shared_ptr.hpp>

#include <list>
#include <vector>

namespace aslam {
namespace backend {

class MarginalizationPriorErrorTerm : public aslam::backend::ErrorTermDs {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	typedef boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm> Ptr;

  // creates the marginalization error term of the form e(x) = d - R*x
  // designVariables: 	the design variables of this marginalization prior error term
  MarginalizationPriorErrorTerm(const std::vector<DesignVariable*>& designVariables,
                                const Eigen::VectorXd& d,
                                const Eigen::MatrixXd& R);

  ~MarginalizationPriorErrorTerm() override;

  int numDesignVariables() { return _designVariables.size(); }
  aslam::backend::DesignVariable* getDesignVariable(int i);

private:
  MarginalizationPriorErrorTerm();

  double evaluateErrorImplementation() override;
  void evaluateJacobiansImplementation(JacobianContainer & outJ) override;

  // computes the minimal difference of all design variables between the linearization point at marginalization and the current guess (i.e. log(x_bar - x))
  Eigen::VectorXd getDifferenceSinceMarginalization();

  std::vector<DesignVariable*> _designVariables;
  Eigen::VectorXd _d;
  Eigen::MatrixXd _R; // R from the QR decomposition!!!
  int _dimensionDesignVariables;
  // store values of design variables at time of marginalization
  std::vector<Eigen::MatrixXd> _designVariableValuesAtMarginalization;
};

} /* namespace backend */
} /* namespace aslam */
#endif /* MARGINALIZATIONPRIORERRORTERM_HPP_ */


#pragma once
#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <Eigen/Dense>
#include <TrtNet.h>
#include <image_transport/image_transport.h>
#include <opencv2/opencv.hpp>
#include <nav_msgs/OccupancyGrid.h>
#include "bonnet.hpp"
#include "box3d.hpp"

using namespace std;

namespace perception
{

// Core perception module that runs following pipeline:
//
// 1. Receives image from topic
// 2. Yolo v3 obect detection
// 3. 3DBox estimation (only for cars now).
// 4. Drivable area segmentation (used in local planner and mapping).
// 5. Publishes bboxes and occupancy grid (+ vizualization image).
//
class EPerception
{
public:
  EPerception(ros::NodeHandle nh, ros::NodeHandle private_nh);

private:
  // ROS parameters
  string config_file_;

  // params
  string cam_frame_id_;
  int cam_width_;
  int cam_height_;
  float cam_height_from_ground_;
  int top_width_;
  int top_height_;
  float top_res_;
  float top2_res_;
  bool do_viz_;
  cv::Mat H_;
  cv::Mat K_;
  float fx_, fy_, u0_, v0_;
  Eigen::Matrix<float,3,4> P_;
  int top2_width_;
  int top2_height_;
  float cam_origin_x_;
  float cam_origin_y_;
  float bonnet_scale_;

  unique_ptr<Tn::trtNet> yolo_;
  unique_ptr<bonnet::Bonnet> bonnet_net_;
  unique_ptr<box3d::Box3D> box3d_net_;
  cv::Mat frame_viz_;
  cv::Mat bonnet_output_;
  cv_bridge::CvImageConstPtr bridge_;
  nav_msgs::OccupancyGrid map_msg_;

  // ROS services
  ros::Publisher image_pub_;
  ros::Publisher map_pub_;
  ros::Publisher bbox_pub_;
  ros::Publisher viz_pub_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;

  // ROS callbacks
  void imageCallback(const sensor_msgs::ImageConstPtr& cmd);
};

} // namespace perception


//  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_ENABLE_ASSERT_HANDLER
#define BOOST_MATH_MAX_SERIES_ITERATION_POLICY INT_MAX
// for consistent behaviour across compilers/platforms:
#define BOOST_MATH_PROMOTE_DOUBLE_POLICY false
// overflow to infinity is OK, we treat these as zero error as long as the sign is correct!
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#include <iostream>
#include <ctime>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/special_functions/hypergeometric_1F1.hpp>
#include <boost/math/special_functions/hypergeometric_pFq.hpp>
#include <boost/math/special_functions/relative_difference.hpp>

#include <boost/random.hpp>
#include <set>
#include <fstream>
#include <boost/iostreams/tee.hpp>
#include <boost/iostreams/stream.hpp>

using boost::multiprecision::mpfr_float;

namespace boost {
   //
   // We convert assertions into exceptions, so we can log them and continue:
   //
   void assertion_failed(char const * expr, char const *, char const * file, long line)
   {
      std::ostringstream oss;
      oss << file << ":" << line << " Assertion failed: " << expr;
      throw std::runtime_error(oss.str());
   }

}

typedef boost::multiprecision::cpp_bin_float_quad test_type;

int main()
{
   using std::floor;
   using std::ceil;
   try {
      test_type a_start, a_end;
      test_type b_start, b_end;
      test_type a_mult, b_mult;

      std::cout << "Enter range for parameter a: ";
      std::cin >> a_start >> a_end;
      std::cout << "Enter range for parameter b: ";
      std::cin >> b_start >> b_end;
      std::cout << "Enter multiplier for a parameter: ";
      std::cin >> a_mult;
      std::cout << "Enter multiplier for b parameter: ";
      std::cin >> b_mult;

      double error_limit = 200;
      double time_limit = 10.0;

      for (test_type a = a_start; a < a_end; a_start < 0 ? a /= a_mult : a *= a_mult)
      {
         for (test_type b = b_start; b < b_end; b_start < 0 ? b /= b_mult : b *= b_mult)
         {
            test_type z_mult = 2;
            test_type last_good = 0;
            test_type bad = 0;
            try {
               for (test_type z = 1; z < 1e10; z *= z_mult, z_mult *= 2)
               {
                  // std::cout << "z = " << z << std::endl;
                  std::uintmax_t max_iter = 1000;
                  test_type calc = boost::math::tools::function_ratio_from_forwards_recurrence(boost::math::detail::hypergeometric_1F1_recurrence_a_and_b_coefficients<test_type>(a, b, z), std::numeric_limits<test_type>::epsilon() * 2, max_iter);
                  test_type reference = (test_type)(boost::math::hypergeometric_pFq_precision({ mpfr_float(a) }, { mpfr_float(b) }, mpfr_float(z), 50, time_limit) / boost::math::hypergeometric_pFq_precision({ mpfr_float(a + 1) }, { mpfr_float(b + 1) }, mpfr_float(z), std::numeric_limits<test_type>::digits10 * 2, time_limit));
                  double err = (double)boost::math::epsilon_difference(reference, calc);

                  if (err < error_limit)
                  {
                     last_good = z;
                     break;
                  }
                  else
                  {
                     bad = z;
                  }
               }
            }
            catch (const std::exception& e)
            {
               std::cout << "Unexpected exception: " << e.what() << std::endl;
               std::cout << "For a = " << a << " b = " << b << " z = " << bad * z_mult / 2 << std::endl;
            }
            test_type z_limit;
            if (0 == bad)
               z_limit = 1;  // Any z is large enough
            else if (0 == last_good)
               z_limit = std::numeric_limits<test_type > ::infinity();
            else
            {
               //
               // At this stage last_good and bad should bracket the edge of the domain, bisect to narrow things down:
               //
               z_limit = last_good == 0 ? 0 : boost::math::tools::bisect([&a, b, error_limit, time_limit](test_type z)
               {
                  std::uintmax_t max_iter = 1000;
                  test_type calc = boost::math::tools::function_ratio_from_forwards_recurrence(boost::math::detail::hypergeometric_1F1_recurrence_a_and_b_coefficients<test_type>(a, b, z), std::numeric_limits<test_type>::epsilon() * 2, max_iter);
                  test_type reference = (test_type)(boost::math::hypergeometric_pFq_precision({ mpfr_float(a) }, { mpfr_float(b) }, mpfr_float(z), 50, time_limit + 20) / boost::math::hypergeometric_pFq_precision({ mpfr_float(a + 1) }, { mpfr_float(b + 1) }, mpfr_float(z), std::numeric_limits<test_type>::digits10 * 2, time_limit + 20));
                  test_type err = boost::math::epsilon_difference(reference, calc);
                  return err < error_limit ? 1 : -1;
               }, bad, last_good, boost::math::tools::equal_floor()).first;
               z_limit = floor(z_limit + 2);  // Give ourselves some headroom!
            }
            // std::cout << "z_limit = " << z_limit << std::endl;
            //
            // Now over again for backwards recurrence domain at the same points:
            //
            bad = z_limit > 1e10 ? 1e10 : z_limit;
            last_good = 0;
            z_mult = 1.1;
            for (test_type z = bad; z > 1; z /= z_mult, z_mult *= 2)
            {
               // std::cout << "z = " << z << std::endl;
               try {
                  std::uintmax_t max_iter = 1000;
                  test_type calc = boost::math::tools::function_ratio_from_backwards_recurrence(boost::math::detail::hypergeometric_1F1_recurrence_a_and_b_coefficients<test_type>(a, b, z), std::numeric_limits<test_type>::epsilon() * 2, max_iter);
                  test_type reference = (test_type)(boost::math::hypergeometric_pFq_precision({ mpfr_float(a) }, { mpfr_float(b) }, mpfr_float(z), 50, time_limit) / boost::math::hypergeometric_pFq_precision({ mpfr_float(a - 1) }, { mpfr_float(b - 1) }, mpfr_float(z), std::numeric_limits<test_type>::digits10 * 2, time_limit));
                  test_type err = boost::math::epsilon_difference(reference, calc);

                  if (err < error_limit)
                  {
                     last_good = z;
                     break;
                  }
                  else
                  {
                     bad = z;
                  }
               }
               catch (const std::exception& e)
               {
                  bad = z;
                  std::cout << "Unexpected exception: " << e.what() << std::endl;
                  std::cout << "For a = " << a << " b = " << b << " z = " << z << std::endl;
               }
            }
            test_type lower_z_limit;
            if (last_good < 1)
               lower_z_limit = 0;
            else if (last_good >= bad)
            {
               std::uintmax_t max_iter = 1000;
               test_type z = bad;
               test_type calc = boost::math::tools::function_ratio_from_forwards_recurrence(boost::math::detail::hypergeometric_1F1_recurrence_a_and_b_coefficients<test_type>(a, b, z), std::numeric_limits<test_type>::epsilon() * 2, max_iter);
               test_type reference = (test_type)(boost::math::hypergeometric_pFq_precision({ mpfr_float(a) }, { mpfr_float(b) }, mpfr_float(z), 50, time_limit) / boost::math::hypergeometric_pFq_precision({ mpfr_float(a + 1) }, { mpfr_float(b + 1) }, mpfr_float(z), std::numeric_limits<test_type>::digits10 * 2, time_limit));
               test_type err = boost::math::epsilon_difference(reference, calc);
               if (err < error_limit)
               {
                  lower_z_limit = bad;   //  Both forwards and backwards iteration work!!!
               }
               else
                  throw std::runtime_error("Internal logic failed!");
            }
            else
            {
               //
               // At this stage last_good and bad should bracket the edge of the domain, bisect to narrow things down:
               //
               lower_z_limit = last_good == 0 ? 0 : boost::math::tools::bisect([&a, b, error_limit, time_limit](test_type z)
               {
                  std::uintmax_t max_iter = 1000;
                  test_type calc = boost::math::tools::function_ratio_from_backwards_recurrence(boost::math::detail::hypergeometric_1F1_recurrence_a_and_b_coefficients<test_type>(a, b, z), std::numeric_limits<test_type>::epsilon() * 2, max_iter);
                  test_type reference = (test_type)(boost::math::hypergeometric_pFq_precision({ mpfr_float(a) }, { mpfr_float(b) }, mpfr_float(z), 50, time_limit + 20) / boost::math::hypergeometric_pFq_precision({ mpfr_float(a - 1) }, { mpfr_float(b - 1) }, mpfr_float(z), std::numeric_limits<test_type>::digits10 * 2, time_limit + 20));
                  test_type err = boost::math::epsilon_difference(reference, calc);
                  return err < error_limit ? 1 : -1;
               }, last_good, bad, boost::math::tools::equal_floor()).first;
               z_limit = ceil(z_limit - 2);  // Give ourselves some headroom!
            }

            std::cout << std::setprecision(std::numeric_limits<test_type>::max_digits10) << "{ " << a << ", " << b << ", " << lower_z_limit << ", " << z_limit << "}," << std::endl;
         }
      }
   }
   catch (const std::exception& e)
   {
      std::cout << "Unexpected exception: " << e.what() << std::endl;
   }
   return 0;
}


/**
 * \file
 * \author Thomas Fischer
 * \date   2010-03-17
 * \brief  Implementation of analytical geometry functions.
 *
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "AnalyticalGeometry.h"

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <limits>

#include "BaseLib/StringTools.h"
#include "MathLib/GeometricBasics.h"
#include "PointVec.h"
#include "Polyline.h"

extern double orient2d(double*, double*, double*);
extern double orient2dfast(double*, double*, double*);

namespace ExactPredicates
{
double getOrientation2d(MathLib::Point3d const& a, MathLib::Point3d const& b,
                        MathLib::Point3d const& c)
{
    return orient2d(const_cast<double*>(a.getCoords()),
                    const_cast<double*>(b.getCoords()),
                    const_cast<double*>(c.getCoords()));
}

double getOrientation2dFast(MathLib::Point3d const& a,
                            MathLib::Point3d const& b,
                            MathLib::Point3d const& c)
{
    return orient2dfast(const_cast<double*>(a.getCoords()),
                        const_cast<double*>(b.getCoords()),
                        const_cast<double*>(c.getCoords()));
}
}  // namespace ExactPredicates

namespace GeoLib
{
Orientation getOrientation(MathLib::Point3d const& p0,
                           MathLib::Point3d const& p1,
                           MathLib::Point3d const& p2)
{
    double const orientation = ExactPredicates::getOrientation2d(p0, p1, p2);
    if (orientation > 0)
    {
        return CCW;
    }
    if (orientation < 0)
    {
        return CW;
    }
    return COLLINEAR;
}

Orientation getOrientationFast(MathLib::Point3d const& p0,
                               MathLib::Point3d const& p1,
                               MathLib::Point3d const& p2)
{
    double const orientation =
        ExactPredicates::getOrientation2dFast(p0, p1, p2);
    if (orientation > 0)
    {
        return CCW;
    }
    if (orientation < 0)
    {
        return CW;
    }
    return COLLINEAR;
}

bool parallel(Eigen::Vector3d v, Eigen::Vector3d w)
{
    const double eps(std::numeric_limits<double>::epsilon());
    double const eps_squared = eps * eps;

    // check degenerated cases
    if (v.squaredNorm() < eps_squared)
    {
        return false;
    }

    if (w.squaredNorm() < eps_squared)
    {
        return false;
    }

    v.normalize();
    w.normalize();

    bool parallel(true);
    if (std::abs(v[0] - w[0]) > eps)
    {
        parallel = false;
    }
    if (std::abs(v[1] - w[1]) > eps)
    {
        parallel = false;
    }
    if (std::abs(v[2] - w[2]) > eps)
    {
        parallel = false;
    }

    if (!parallel)
    {
        parallel = true;
        // change sense of direction of v_normalised
        v *= -1.0;
        // check again
        if (std::abs(v[0] - w[0]) > eps)
        {
            parallel = false;
        }
        if (std::abs(v[1] - w[1]) > eps)
        {
            parallel = false;
        }
        if (std::abs(v[2] - w[2]) > eps)
        {
            parallel = false;
        }
    }

    return parallel;
}

bool lineSegmentIntersect(GeoLib::LineSegment const& s0,
                          GeoLib::LineSegment const& s1,
                          GeoLib::Point& s)
{
    GeoLib::Point const& pa{s0.getBeginPoint()};
    GeoLib::Point const& pb{s0.getEndPoint()};
    GeoLib::Point const& pc{s1.getBeginPoint()};
    GeoLib::Point const& pd{s1.getEndPoint()};

    if (!isCoplanar(pa, pb, pc, pd))
    {
        return false;
    }

    auto const a =
        Eigen::Map<Eigen::Vector3d const>(s0.getBeginPoint().getCoords());
    auto const b =
        Eigen::Map<Eigen::Vector3d const>(s0.getEndPoint().getCoords());
    auto const c =
        Eigen::Map<Eigen::Vector3d const>(s1.getBeginPoint().getCoords());
    auto const d =
        Eigen::Map<Eigen::Vector3d const>(s1.getEndPoint().getCoords());

    Eigen::Vector3d const v = b - a;
    Eigen::Vector3d const w = d - c;
    Eigen::Vector3d const qp = c - a;
    Eigen::Vector3d const pq = a - c;

    double const eps = std::numeric_limits<double>::epsilon();
    double const squared_eps = eps * eps;
    // handle special cases here to avoid computing intersection numerical
    if (qp.squaredNorm() < squared_eps || (d - a).squaredNorm() < squared_eps)
    {
        s = pa;
        return true;
    }
    if ((c - b).squaredNorm() < squared_eps ||
        (d - b).squaredNorm() < squared_eps)
    {
        s = pb;
        return true;
    }

    auto isLineSegmentIntersectingAB =
        [&v](Eigen::Vector3d const& ap, std::size_t i)
    {
        // check if p is located at v=(a,b): (ap = t*v, t in [0,1])
        return 0.0 <= ap[i] / v[i] && ap[i] / v[i] <= 1.0;
    };

    if (parallel(v, w))
    {  // original line segments (a,b) and (c,d) are parallel
        if (parallel(pq, v))
        {  // line segment (a,b) and (a,c) are also parallel
            // Here it is already checked that the line segments (a,b) and (c,d)
            // are parallel. At this point it is also known that the line
            // segment (a,c) is also parallel to (a,b). In that case it is
            // possible to express c as c(t) = a + t * (b-a) (analog for the
            // point d). Since the evaluation of all three coordinate equations
            // (x,y,z) have to lead to the same solution for the parameter t it
            // is sufficient to evaluate t only once.

            // Search id of coordinate with largest absolute value which is will
            // be used in the subsequent computations. This prevents division by
            // zero in case the line segments are parallel to one of the
            // coordinate axis.
            std::size_t i_max(std::abs(v[0]) <= std::abs(v[1]) ? 1 : 0);
            i_max = std::abs(v[i_max]) <= std::abs(v[2]) ? 2 : i_max;
            if (isLineSegmentIntersectingAB(qp, i_max))
            {
                s = pc;
                return true;
            }
            Eigen::Vector3d const ad = d - a;
            if (isLineSegmentIntersectingAB(ad, i_max))
            {
                s = pd;
                return true;
            }
            return false;
        }
        return false;
    }

    // general case
    const double sqr_len_v(v.squaredNorm());
    const double sqr_len_w(w.squaredNorm());

    Eigen::Matrix2d mat;
    mat(0, 0) = sqr_len_v;
    mat(0, 1) = -v.dot(w);
    mat(1, 1) = sqr_len_w;
    mat(1, 0) = mat(0, 1);

    Eigen::Vector2d rhs{v.dot(qp), w.dot(pq)};

    rhs = mat.partialPivLu().solve(rhs);

    // no theory for the following tolerances, determined by testing
    // lower tolerance: little bit smaller than zero
    const double l(-1.0 * std::numeric_limits<float>::epsilon());
    // upper tolerance a little bit greater than one
    const double u(1.0 + std::numeric_limits<float>::epsilon());
    if (rhs[0] < l || u < rhs[0] || rhs[1] < l || u < rhs[1])
    {
        return false;
    }

    // compute points along line segments with minimal distance
    GeoLib::Point const p0(a[0] + rhs[0] * v[0], a[1] + rhs[0] * v[1],
                           a[2] + rhs[0] * v[2]);
    GeoLib::Point const p1(c[0] + rhs[1] * w[0], c[1] + rhs[1] * w[1],
                           c[2] + rhs[1] * w[2]);

    double const min_dist(std::sqrt(MathLib::sqrDist(p0, p1)));
    double const min_seg_len(
        std::min(std::sqrt(sqr_len_v), std::sqrt(sqr_len_w)));
    if (min_dist < min_seg_len * 1e-6)
    {
        s[0] = 0.5 * (p0[0] + p1[0]);
        s[1] = 0.5 * (p0[1] + p1[1]);
        s[2] = 0.5 * (p0[2] + p1[2]);
        return true;
    }

    return false;
}

bool lineSegmentsIntersect(const GeoLib::Polyline* ply,
                           GeoLib::Polyline::SegmentIterator& seg_it0,
                           GeoLib::Polyline::SegmentIterator& seg_it1,
                           GeoLib::Point& intersection_pnt)
{
    std::size_t const n_segs(ply->getNumberOfSegments());
    // Neighbouring segments always intersects at a common vertex. The algorithm
    // checks for intersections of non-neighbouring segments.
    for (seg_it0 = ply->begin(); seg_it0 != ply->end() - 2; ++seg_it0)
    {
        seg_it1 = seg_it0 + 2;
        std::size_t const seg_num_0 = seg_it0.getSegmentNumber();
        for (; seg_it1 != ply->end(); ++seg_it1)
        {
            // Do not check first and last segment, because they are
            // neighboured.
            if (!(seg_num_0 == 0 && seg_it1.getSegmentNumber() == n_segs - 1))
            {
                if (lineSegmentIntersect(*seg_it0, *seg_it1, intersection_pnt))
                {
                    return true;
                }
            }
        }
    }
    return false;
}

void rotatePoints(Eigen::Matrix3d const& rot_mat,
                  std::vector<GeoLib::Point*>& pnts)
{
    rotatePoints(rot_mat, pnts.begin(), pnts.end());
}

Eigen::Matrix3d computeRotationMatrixToXY(Eigen::Vector3d const& n)
{
    Eigen::Matrix3d rot_mat = Eigen::Matrix3d::Zero();
    // check if normal points already in the right direction
    if (n[0] == 0 && n[1] == 0)
    {
        rot_mat(1, 1) = 1.0;

        if (n[2] > 0)
        {
            // identity matrix
            rot_mat(0, 0) = 1.0;
            rot_mat(2, 2) = 1.0;
        }
        else
        {
            // rotate by pi about the y-axis
            rot_mat(0, 0) = -1.0;
            rot_mat(2, 2) = -1.0;
        }

        return rot_mat;
    }

    // sqrt (n_1^2 + n_3^2)
    double const h0(std::sqrt(n[0] * n[0] + n[2] * n[2]));

    // In case the x and z components of the normal are both zero the rotation
    // to the x-z-plane is not required, i.e. only the rotation in the z-axis is
    // required. The angle is either pi/2 or 3/2*pi. Thus the components of
    // rot_mat are as follows.
    if (h0 < std::numeric_limits<double>::epsilon())
    {
        rot_mat(0, 0) = 1.0;
        if (n[1] > 0)
        {
            rot_mat(1, 2) = -1.0;
            rot_mat(2, 1) = 1.0;
        }
        else
        {
            rot_mat(1, 2) = 1.0;
            rot_mat(2, 1) = -1.0;
        }
        return rot_mat;
    }

    double const h1(1 / n.norm());

    // general case: calculate entries of rotation matrix
    rot_mat(0, 0) = n[2] / h0;
    rot_mat(0, 1) = 0;
    rot_mat(0, 2) = -n[0] / h0;
    rot_mat(1, 0) = -n[1] * n[0] / h0 * h1;
    rot_mat(1, 1) = h0 * h1;
    rot_mat(1, 2) = -n[1] * n[2] / h0 * h1;
    rot_mat(2, 0) = n[0] * h1;
    rot_mat(2, 1) = n[1] * h1;
    rot_mat(2, 2) = n[2] * h1;

    return rot_mat;
}

Eigen::Matrix3d rotatePointsToXY(std::vector<GeoLib::Point*>& pnts)
{
    return rotatePointsToXY(pnts.begin(), pnts.end(), pnts.begin(), pnts.end());
}

std::unique_ptr<GeoLib::Point> triangleLineIntersection(
    MathLib::Point3d const& a, MathLib::Point3d const& b,
    MathLib::Point3d const& c, MathLib::Point3d const& p,
    MathLib::Point3d const& q)
{
    auto const va = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const vb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const vc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    auto const vp = Eigen::Map<Eigen::Vector3d const>(p.getCoords());
    auto const vq = Eigen::Map<Eigen::Vector3d const>(q.getCoords());

    Eigen::Vector3d const pq = vq - vp;
    Eigen::Vector3d const pa = va - vp;
    Eigen::Vector3d const pb = vb - vp;
    Eigen::Vector3d const pc = vc - vp;

    double u = pq.cross(pc).dot(pb);
    if (u < 0)
    {
        return nullptr;
    }
    double v = pq.cross(pa).dot(pc);
    if (v < 0)
    {
        return nullptr;
    }
    double w = pq.cross(pb).dot(pa);
    if (w < 0)
    {
        return nullptr;
    }

    const double denom(1.0 / (u + v + w));
    u *= denom;
    v *= denom;
    w *= denom;
    return std::make_unique<GeoLib::Point>(u * a[0] + v * b[0] + w * c[0],
                                           u * a[1] + v * b[1] + w * c[1],
                                           u * a[2] + v * b[2] + w * c[2]);
}

void computeAndInsertAllIntersectionPoints(GeoLib::PointVec& pnt_vec,
                                           std::vector<GeoLib::Polyline*>& plys)
{
    auto computeSegmentIntersections =
        [&pnt_vec](GeoLib::Polyline& poly0, GeoLib::Polyline& poly1)
    {
        for (auto seg0_it(poly0.begin()); seg0_it != poly0.end(); ++seg0_it)
        {
            for (auto seg1_it(poly1.begin()); seg1_it != poly1.end(); ++seg1_it)
            {
                GeoLib::Point s(0.0, 0.0, 0.0, pnt_vec.size());
                if (lineSegmentIntersect(*seg0_it, *seg1_it, s))
                {
                    std::size_t const id(
                        pnt_vec.push_back(new GeoLib::Point(s)));
                    poly0.insertPoint(seg0_it.getSegmentNumber() + 1, id);
                    poly1.insertPoint(seg1_it.getSegmentNumber() + 1, id);
                }
            }
        }
    };

    for (auto it0(plys.begin()); it0 != plys.end(); ++it0)
    {
        auto it1(it0);
        ++it1;
        for (; it1 != plys.end(); ++it1)
        {
            computeSegmentIntersections(*(*it0), *(*it1));
        }
    }
}

std::tuple<std::vector<GeoLib::Point*>, Eigen::Vector3d>
rotatePolygonPointsToXY(GeoLib::Polygon const& polygon_in)
{
    // 1 copy all points
    std::vector<GeoLib::Point*> polygon_points;
    polygon_points.reserve(polygon_in.getNumberOfPoints());
    for (std::size_t k(0); k < polygon_in.getNumberOfPoints(); k++)
    {
        polygon_points.push_back(new GeoLib::Point(*(polygon_in.getPoint(k))));
    }

    // 2 rotate points
    auto [plane_normal, d_polygon] = GeoLib::getNewellPlane(polygon_points);
    Eigen::Matrix3d const rot_mat =
        GeoLib::computeRotationMatrixToXY(plane_normal);
    GeoLib::rotatePoints(rot_mat, polygon_points);

    // 3 set z coord to zero
    std::for_each(polygon_points.begin(), polygon_points.end(),
                  [](GeoLib::Point* p) { (*p)[2] = 0.0; });

    return {polygon_points, plane_normal};
}

std::vector<MathLib::Point3d> lineSegmentIntersect2d(
    GeoLib::LineSegment const& ab, GeoLib::LineSegment const& cd)
{
    GeoLib::Point const& a{ab.getBeginPoint()};
    GeoLib::Point const& b{ab.getEndPoint()};
    GeoLib::Point const& c{cd.getBeginPoint()};
    GeoLib::Point const& d{cd.getEndPoint()};

    double const orient_abc(getOrientation(a, b, c));
    double const orient_abd(getOrientation(a, b, d));

    // check if the segment (cd) lies on the left or on the right of (ab)
    if ((orient_abc > 0 && orient_abd > 0) ||
        (orient_abc < 0 && orient_abd < 0))
    {
        return std::vector<MathLib::Point3d>();
    }

    // check: (cd) and (ab) are on the same line
    if (orient_abc == 0.0 && orient_abd == 0.0)
    {
        double const eps(std::numeric_limits<double>::epsilon());
        if (MathLib::sqrDist2d(a, c) < eps && MathLib::sqrDist2d(b, d) < eps)
        {
            return {{a, b}};
        }
        if (MathLib::sqrDist2d(a, d) < eps && MathLib::sqrDist2d(b, c) < eps)
        {
            return {{a, b}};
        }

        // Since orient_ab and orient_abd vanish, a, b, c, d are on the same
        // line and for this reason it is enough to check the x-component.
        auto isPointOnSegment = [](double q, double p0, double p1)
        {
            double const t((q - p0) / (p1 - p0));
            return 0 <= t && t <= 1;
        };

        // check if c in (ab)
        if (isPointOnSegment(c[0], a[0], b[0]))
        {
            // check if a in (cd)
            if (isPointOnSegment(a[0], c[0], d[0]))
            {
                return {{a, c}};
            }
            // check b == c
            if (MathLib::sqrDist2d(b, c) < eps)
            {
                return {{b}};
            }
            // check if b in (cd)
            if (isPointOnSegment(b[0], c[0], d[0]))
            {
                return {{b, c}};
            }
            // check d in (ab)
            if (isPointOnSegment(d[0], a[0], b[0]))
            {
                return {{c, d}};
            }
            std::stringstream err;
            err.precision(std::numeric_limits<double>::digits10);
            err << ab << " x " << cd;
            OGS_FATAL(
                "The case of parallel line segments ({:s}) is not handled yet. "
                "Aborting.",
                err.str());
        }

        // check if d in (ab)
        if (isPointOnSegment(d[0], a[0], b[0]))
        {
            // check if a in (cd)
            if (isPointOnSegment(a[0], c[0], d[0]))
            {
                return {{a, d}};
            }
            // check if b==d
            if (MathLib::sqrDist2d(b, d) < eps)
            {
                return {{b}};
            }
            // check if b in (cd)
            if (isPointOnSegment(b[0], c[0], d[0]))
            {
                return {{b, d}};
            }
            // d in (ab), b not in (cd): check c in (ab)
            if (isPointOnSegment(c[0], a[0], b[0]))
            {
                return {{c, d}};
            }

            std::stringstream err;
            err.precision(std::numeric_limits<double>::digits10);
            err << ab << " x " << cd;
            OGS_FATAL(
                "The case of parallel line segments ({:s}) is not handled yet. "
                "Aborting.",
                err.str());
        }
        return std::vector<MathLib::Point3d>();
    }

    // precondition: points a, b, c are collinear
    // the function checks if the point c is onto the line segment (a,b)
    auto isCollinearPointOntoLineSegment = [](MathLib::Point3d const& a,
                                              MathLib::Point3d const& b,
                                              MathLib::Point3d const& c)
    {
        if (b[0] - a[0] != 0)
        {
            double const t = (c[0] - a[0]) / (b[0] - a[0]);
            return 0.0 <= t && t <= 1.0;
        }
        if (b[1] - a[1] != 0)
        {
            double const t = (c[1] - a[1]) / (b[1] - a[1]);
            return 0.0 <= t && t <= 1.0;
        }
        if (b[2] - a[2] != 0)
        {
            double const t = (c[2] - a[2]) / (b[2] - a[2]);
            return 0.0 <= t && t <= 1.0;
        }
        return false;
    };

    if (orient_abc == 0.0)
    {
        if (isCollinearPointOntoLineSegment(a, b, c))
        {
            return {{c}};
        }
        return std::vector<MathLib::Point3d>();
    }

    if (orient_abd == 0.0)
    {
        if (isCollinearPointOntoLineSegment(a, b, d))
        {
            return {{d}};
        }
        return std::vector<MathLib::Point3d>();
    }

    // check if the segment (ab) lies on the left or on the right of (cd)
    double const orient_cda(getOrientation(c, d, a));
    double const orient_cdb(getOrientation(c, d, b));
    if ((orient_cda > 0 && orient_cdb > 0) ||
        (orient_cda < 0 && orient_cdb < 0))
    {
        return std::vector<MathLib::Point3d>();
    }

    // at this point it is sure that there is an intersection and the system of
    // linear equations will be invertible
    // solve the two linear equations (b-a, c-d) (t, s)^T = (c-a) simultaneously
    Eigen::Matrix2d mat;
    mat(0, 0) = b[0] - a[0];
    mat(0, 1) = c[0] - d[0];
    mat(1, 0) = b[1] - a[1];
    mat(1, 1) = c[1] - d[1];
    Eigen::Vector2d rhs{c[0] - a[0], c[1] - a[1]};

    rhs = mat.partialPivLu().solve(rhs);
    if (0 <= rhs[1] && rhs[1] <= 1.0)
    {
        return {MathLib::Point3d{std::array<double, 3>{
            {c[0] + rhs[1] * (d[0] - c[0]), c[1] + rhs[1] * (d[1] - c[1]),
             c[2] + rhs[1] * (d[2] - c[2])}}}};
    }
    return std::vector<MathLib::Point3d>();  // parameter s not in the valid
                                             // range
}

void sortSegments(MathLib::Point3d const& seg_beg_pnt,
                  std::vector<GeoLib::LineSegment>& sub_segments)
{
    double const eps(std::numeric_limits<double>::epsilon());

    auto findNextSegment =
        [&eps](MathLib::Point3d const& seg_beg_pnt,
               std::vector<GeoLib::LineSegment>& sub_segments,
               std::vector<GeoLib::LineSegment>::iterator& sub_seg_it)
    {
        if (sub_seg_it == sub_segments.end())
        {
            return;
        }
        // find appropriate segment for the given segment begin point
        auto act_beg_seg_it = std::find_if(
            sub_seg_it, sub_segments.end(),
            [&seg_beg_pnt, &eps](GeoLib::LineSegment const& seg)
            {
                return MathLib::sqrDist(seg_beg_pnt, seg.getBeginPoint()) <
                           eps ||
                       MathLib::sqrDist(seg_beg_pnt, seg.getEndPoint()) < eps;
            });
        if (act_beg_seg_it == sub_segments.end())
        {
            return;
        }
        // if necessary correct orientation of segment, i.e. swap beg and
        // end
        if (MathLib::sqrDist(seg_beg_pnt, act_beg_seg_it->getEndPoint()) <
            MathLib::sqrDist(seg_beg_pnt, act_beg_seg_it->getBeginPoint()))
        {
            std::swap(act_beg_seg_it->getBeginPoint(),
                      act_beg_seg_it->getEndPoint());
        }
        assert(sub_seg_it != sub_segments.end());
        // exchange segments within the container
        if (sub_seg_it != act_beg_seg_it)
        {
            std::swap(*sub_seg_it, *act_beg_seg_it);
        }
    };

    // find start segment
    auto seg_it = sub_segments.begin();
    findNextSegment(seg_beg_pnt, sub_segments, seg_it);

    while (seg_it != sub_segments.end())
    {
        MathLib::Point3d& new_seg_beg_pnt(seg_it->getEndPoint());
        seg_it++;
        if (seg_it != sub_segments.end())
        {
            findNextSegment(new_seg_beg_pnt, sub_segments, seg_it);
        }
    }
}

Eigen::Matrix3d compute2DRotationMatrixToX(Eigen::Vector3d const& v)
{
    Eigen::Matrix3d rot_mat = Eigen::Matrix3d::Zero();
    const double cos_theta = v[0];
    const double sin_theta = v[1];
    rot_mat(0, 0) = rot_mat(1, 1) = cos_theta;
    rot_mat(0, 1) = sin_theta;
    rot_mat(1, 0) = -sin_theta;
    rot_mat(2, 2) = 1.0;
    return rot_mat;
}

Eigen::Matrix3d compute3DRotationMatrixToX(Eigen::Vector3d const& v)
{
    // a vector on the plane
    Eigen::Vector3d yy = Eigen::Vector3d::Zero();
    auto const eps = std::numeric_limits<double>::epsilon();
    if (std::abs(v[0]) > 0.0 && std::abs(v[1]) + std::abs(v[2]) < eps)
    {
        yy[2] = 1.0;
    }
    else if (std::abs(v[1]) > 0.0 && std::abs(v[0]) + std::abs(v[2]) < eps)
    {
        yy[0] = 1.0;
    }
    else if (std::abs(v[2]) > 0.0 && std::abs(v[0]) + std::abs(v[1]) < eps)
    {
        yy[1] = 1.0;
    }
    else
    {
        for (unsigned i = 0; i < 3; i++)
        {
            if (std::abs(v[i]) > 0.0)
            {
                yy[i] = -v[i];
                break;
            }
        }
    }
    // z"_vec
    Eigen::Vector3d const zz = v.cross(yy).normalized();
    // y"_vec
    yy = zz.cross(v).normalized();

    Eigen::Matrix3d rot_mat;
    rot_mat.row(0) = v;
    rot_mat.row(1) = yy;
    rot_mat.row(2) = zz;
    return rot_mat;
}

}  // end namespace GeoLib


/*
 [auto_generated]
 boost/numeric/odeint/external/mkl/mkl_operations.hpp

 [begin_description]
 Wrapper classes for intel math kernel library types.
 Get a free, non-commercial download of MKL at
 http://software.intel.com/en-us/articles/non-commercial-software-download/
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 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)
 */

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED

#include <iostream>

#include <mkl_cblas.h>
#include <boost/numeric/odeint/algebra/default_operations.hpp>

/* exemplary example for writing bindings to the Intel MKL library
 * see test/mkl for how to use mkl with odeint
 * this is a quick and dirty implementation showing the general possibility.
 * It works only with containers based on double and sequential memory allocation.
 */

namespace boost {
namespace numeric {
namespace odeint {

/* only defined for doubles */
struct mkl_operations
{
    //template< class Fac1 , class Fac2 > struct scale_sum2;

    template< class F1 = double , class F2 = F1 >
    struct scale_sum2
    {
        typedef double Fac1;
        typedef double Fac2;
        const Fac1 m_alpha1;
        const Fac2 m_alpha2;

        scale_sum2( const Fac1 alpha1 , const Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { }

        template< class T1 , class T2 , class T3 >
        void operator()( T1 &t1 , const T2 &t2 , const T3 &t3) const
        {   // t1 = m_alpha1 * t2 + m_alpha2 * t3;
            // we get Containers that have size() and [i]-access

            const int n = t1.size();
            //boost::numeric::odeint::copy( t1 , t3 );
            if( &(t2[0]) != &(t1[0]) )
            {
                cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 );
            }
            cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 );
            cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 );
            //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one );
        }
    };

    template< class F1 = double , class F2 = F1 , class F3 = F2 >
    struct scale_sum3
    {
        typedef double Fac1;
        typedef double Fac2;
        typedef double Fac3;
        const Fac1 m_alpha1;
        const Fac2 m_alpha2;
        const Fac3 m_alpha3;

        scale_sum3( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 )
            : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { }

        template< class T1 , class T2 , class T3 , class T4 >
        void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 ) const
        {   // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4;
            // we get Containers that have size() and [i]-access

            const int n = t1.size();
            //boost::numeric::odeint::copy( t1 , t3 );
            if( &(t2[0]) != &(t1[0]) )
            {
                cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 );
            }
            cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 );
            cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 );
            //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one );
            cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 );
        }
    };

    template< class F1 = double , class F2 = F1 , class F3 = F2 , class F4 = F3 >
    struct scale_sum4
    {
        typedef double Fac1;
        typedef double Fac2;
        typedef double Fac3;
        typedef double Fac4;
        const Fac1 m_alpha1;
        const Fac2 m_alpha2;
        const Fac3 m_alpha3;
        const Fac4 m_alpha4;

        scale_sum4( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 )
            : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { }

        template< class T1 , class T2 , class T3 , class T4 , class T5 >
        void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 ) const
        {   // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5;
            // we get Containers that have size() and [i]-access

            const int n = t1.size();
            //boost::numeric::odeint::copy( t1 , t3 );
            if( &(t2[0]) != &(t1[0]) )
            {
                cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 );
            }

            cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 );
            cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 );
            //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one );
            cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 );
            cblas_daxpy( n , m_alpha4 , &(t5[0]) , 1 , &(t1[0]) , 1 );
        }
    };

    template< class F1 = double , class F2 = F1 , class F3 = F2 , class F4 = F3 , class F5 = F4 >
    struct scale_sum5
    {
        typedef double Fac1;
        typedef double Fac2;
        typedef double Fac3;
        typedef double Fac4;
        typedef double Fac5;
        const Fac1 m_alpha1;
        const Fac2 m_alpha2;
        const Fac3 m_alpha3;
        const Fac4 m_alpha4;
        const Fac5 m_alpha5;

        scale_sum5( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 , const Fac5 alpha5 )
            : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { }

        template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6   >
        void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 ) const
        {   // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6;
            // we get Containers that have size() and [i]-access

            const int n = t1.size();
            //boost::numeric::odeint::copy( t1 , t3 );
            if( &(t2[0]) != &(t1[0]) )
            {
                cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 );
            }

            cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 );
            cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 );
            //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one );
            cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 );
            cblas_daxpy( n , m_alpha4 , &(t5[0]) , 1 , &(t1[0]) , 1 );
            cblas_daxpy( n , m_alpha5 , &(t6[0]) , 1 , &(t1[0]) , 1 );
        }
    };

};

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED


#define BOOST_TEST_MODULE TestEigenSolver
#include <boost/test/unit_test.hpp>

#include <Eigen/SparseLU>
#include <amgcl/solver/eigen.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/profiler.hpp>
#include "sample_problem.hpp"

namespace amgcl {
    profiler<> prof;
}

BOOST_AUTO_TEST_SUITE( test_eigen_solver )

BOOST_AUTO_TEST_CASE(eigen_solver)
{
    std::vector<int>    ptr;
    std::vector<int>    col;
    std::vector<double> val;
    std::vector<double> rhs;

    size_t n = sample_problem(16, val, col, ptr, rhs);
    amgcl::backend::crs<double> A(std::tie(n, ptr, col, val));

    typedef
        amgcl::solver::EigenSolver<Eigen::SparseLU<Eigen::SparseMatrix<double, Eigen::ColMajor, int> > >
        Solver;

    Solver solve(A);

    std::vector<double> x(n);
    std::vector<double> r(n);

    solve(rhs, x);

    amgcl::backend::residual(rhs, A, x, r);

    BOOST_CHECK_SMALL(sqrt(amgcl::backend::inner_product(r, r)), 1e-8);
}

BOOST_AUTO_TEST_SUITE_END()