/********************************************************************************
 * 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 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;
}


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


/* Copyright (C) 2017 IBM Corp.
 *  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.
 */
/*****************************************************************************
 * TDMatrix.cpp - Matrixes with trapdoors,
 *   implementing the Efficient algorithm SampleD - Algorithm 3 from MP12.
 *****************************************************************************/
#include <stdexcept>
#include <ctime>
#include <NTL/mat_ZZ.h>
#include <NTL/mat_lzz_p.h>
#include <iostream>
#include <fstream>

NTL_CLIENT
#include "utils/timing.h"
#include "TDMatrix.h"
#include "mat_l.h"

//#define DEBUGPRINT
//#define DEBUG
//#define PRINTDOT //show how program progresses

// A static variable for debugging purposes
std::atomic<int> TDMatrix::maxSample(0); // keep the largest sample ever drawn

/***** Some local functions *****/

// Compute the covariance matrix sigmaP = sigmaX*I - sigmaG*[R/I]*(R^t|I)
static void
computeCovarianceMatrix(mat_l& covMat,const mat_l& R,long sigmaG,long sigmaX);

// Multiply a vector by the G matrix modulo the current NTL modulus, y=G*x.
// G is and n-by-m matrix with m=n*e*numOfFactors, and is defind by means
// of the vector of factors and the exponent e, each row of G is mostly zero,
// except for a progression
//    ( 1, f1,...,f1^e,  (f1^e f2), (f1^e f2^2), ..., (f1^e...fk^{e-1}) )
template<class T>
void multByG(vec_zz_p &y, const Vec<T> &x,
             long n, const vec_l& factors, long e)
{
    FHE_TIMER_START;
    long k = factors.length();
    long m = n*k*e;

    assert(x.length()==m);
    y.SetLength(n);

    for (long row=0; row<n; row++)   // Multiply x by next row of G
    {
        long index = row*k*e;      // first non-zero entry in this row
        zz_p val = to_zz_p(1L);  // value in current entry of G
        y[row] = to_zz_p(0L);    // accumulated sum
        for (long f=0; f<k; f++)     // go over all the factors
        {
            zz_p fact = to_zz_p(factors[f]); // convert to zz_p
            for (long i=0; i<e; i++)    // use each factor e times
            {
                zz_p addedTerm = val;
                addedTerm *= x[index++];  // update sum, advance index
                y[row] += addedTerm;
                val *= fact;              // compute next entry in this row of G
            }
        } // end of current factor
    } // end of current row
}
// Explicit instantiations for Vec<long> and Vec<zz_p>
template void multByG<zz_p>(vec_zz_p &y, const Vec<zz_p> &x,
                            long n, const vec_l& factors, long e);
template void multByG<long>(vec_zz_p &y, const Vec<long> &x,
                            long n, const vec_l& factors, long e);

// Choose a (pseudo)random A with a trapdoor R s.t. A*[R/I]=G
// A=(aBar| aPrime ) with aPrime=G-aBar*R isn't computed explicitly here
void TDMatrix::initTDmatrix(TDMatrixParams& prms, const CRTmatrix* abar)
{
    FHE_TIMER_START;
    params = &prms;

    if (abar)   // aBar is specified by the caller
        aBarCRT = *abar;
    else        // Choose aBar at random
        randomFill(aBarCRT, prms.n, prms.mBar, prms);

    mat_l sigmaP;
    do
    {
        // Choose R as a random small matrix
        setSmall(trapDoorMatR, params->mBar,params->wLen, params->r);

        // Compute the covariance matrix sigmaP = sigmaX*I - sigmaG*[R/I]*(R^t|I)

        computeCovarianceMatrix(sigmaP, trapDoorMatR,
                                /*sigmaG=*/prms.r *prms.maxFactor, prms.sigmaX);

#ifdef DEBUGPRINT
        long minVal =0;
        long maxVal=0;
        for (long i = 0; i < sigmaP.NumCols(); i++)
            for (long j = 0; j < sigmaP.NumRows(); j++)
            {
                if (sigmaP[i][j] < minVal)
                    minVal = sigmaP[i][j];
                if (sigmaP[i][j] > maxVal)
                    maxVal = sigmaP[i][j];
             }

        ZZ q = prms.getQ();
        cout << "minVal=" << minVal << endl;
        cout << "maxVal=" << maxVal << endl;
        cout << "q=" << q << endl;
        cout << "after computeCovarianceMatrix"  << endl;
#endif // DEBUGPRINT
    }
    while (!(gaussSamp.InitSampler(sigmaP)));  // error, try again

#ifdef DEBUGPRINT
        cout << "after gaussSamp.InitSampler"  << endl;
#endif // DEBUGPRINT
}

// Compute the covariance matrix sigmaP = sigmaX*I - sigmaG*[R/I]*(R^t|I)
// Note that [R/I]*(R^t|I) is a block matrix with this form:
//    RR = ( R^2 | R^t )
//         (  R  |  I  )
void computeCovarianceMatrix(mat_l& covMat, const mat_l& R, long sigmaG,long sigmaX)
{

    FHE_TIMER_START;

    long mBar = R.NumRows();
    long wLen = R.NumCols();
    long m = mBar + wLen;

    covMat.SetDims(m,m); // allocate space

    mat_l tmp(INIT_SIZE, mBar, mBar);
    square(tmp, R);   // Set the top-left mBar-by-mBar as R*R^t

    // Reset the top-left to sigmaX*I - sigmaG*R*R^t
    for (long i=0; i<mBar; i++) for (long j=0; j<mBar; j++)
        {
            if (i==j)
                covMat[i][j] = sigmaX - sigmaG*tmp[i][j];
            else
                covMat[i][j] = -sigmaG*tmp[i][j];
        }

    // Set the top-right ro -sigmaG*R^t and the bootom-left to -sigmaG*R
    for (long i=0; i<mBar; i++) for (long j=0; j<wLen; j++)
        {
            covMat[i][j+mBar] = covMat[j+mBar][i] = -sigmaG * R[i][j];
        }

    // Finally set the bottom-right to (sigmaX-sigmaG)*I;
    for (long i=mBar; i<m; i++) for (long j=mBar; j<m; j++)
        {
            if (i==j) covMat[i][j] = sigmaX -sigmaG;
            else      covMat[i][j] = 0;
        }
}

// Compute A explicitly, A = ( aBar | G - aBar*R )
void TDMatrix::getA(CRTmatrix& A) const
{
    FHE_TIMER_START;
    A.params = params;

    long n = params->n;
    long m = params->m;
    long kFactors = params->kFactors;
    long mBar = params->mBar;

    // Compute A modulo each factor separately

    zz_pPush ppush; // backup NTL's current modulus
    A.SetLength(kFactors);
    for (long f=0; f<kFactors; f++)
    {
        params->zzp_context[f].restore();
        mat_zz_p& aMod = A[f];
        const mat_zz_p& aBarMod = aBarCRT[f];

        aMod.SetDims(n,m); // allocate space

        // Copy aBar
        for (long i=0; i<n; i++) for (long j=0; j<mBar; j++)
            {
                aMod[i][j] = aBarMod[i][j];
            }

        // Compute aBar * R
        mat_zz_p tmp;
        mat_zz_p rMod;
        conv(rMod, trapDoorMatR); // conver to zz_p format
        mul(tmp, aBarMod, rMod); // tmp = aBar * R (mod P_f)

        // Copy -aBar*R to left part of A
        for (long i=0; i<n; i++) for (long j=mBar; j<m; j++)
            {
                aMod[i][j] = -tmp[i][j-mBar];
            }

        // Add G to left part of A
        for (long i=0; i<n; i++)    // one row at a time
        {
            // Add (1, f1, f1^2, ..., f1^e...fk^{e-1}) to each row

            long index = (i * kFactors * params->e) + mBar; // 1st index to add to

            zz_p val = to_zz_p(1L);      // The value to add to the next entry
            for (long j=0; j < kFactors; j++)
                for (long ei=0; ei<params->e; ei++, index++)
                {
                    aMod[i][index] += val;
                    val *= params->factors[j];// Change value to add before next entry
                }
        }
    }

}

/** sampleWithTrapdoor - Implementation of Efficient algorithm SampleD -
 * Algorithm 3 from MP12. Choose p at random and z to match the needed
 * solution. This debugging function also returns p and z.
 **/
int TDMatrix::sampleWithTrapdoor(vec_l &xOut, vec_l& p, vec_l& z,
                                 Vec<vec_zz_p>& U) const
{
    FHE_TIMER_START;
    long mBar = params->mBar;
    long m = params->m;
    long n = params->n;
    long e = params->e;
    long kFactors = params->kFactors;

    //choose perturbation P from (0,sigma)

    Vec<double> zeroVector(INIT_SIZE, m);
    clear(zeroVector);

    gaussSamp.SampleDiscreteGaussian(p, zeroVector);
#ifdef DEBUGPRINT
    cout << "\np = " << p << endl;
#endif
    // Split the pertubation vector in two
    vec_l p1(INIT_SIZE, mBar);
    for (long i = 0; i < mBar; i++)
        p1[i] = p[i];

    vec_l p2(INIT_SIZE, params->wLen);
    for (long i = mBar; i < m; i++)
        p2[i - mBar] = p[i];

    //compute wBar and w

    /* We need to compute v = u-A*p = u-wBar-w (in CRT representation), where
     *
     *    A*p = (Abar| Aprime ) * [p1/p2] = Abar*p1 + Aprime*p2
     *        = Abar*p1 + (G-Abar*R)*p2   = Abar*(p1-R*p2) +  G*p2
     *                                      \___wBar____/    \_w_/
     */

    // p1-R*p2 is computed over the integers, stored in p1
    vec_l tmp(INIT_SIZE, mBar);
    mul(tmp, trapDoorMatR, p2); // R*p2
    p1 -= tmp;                  // p1-R*p2

    // All other vectors are computed modulo each of the factors

    // FIXME: There's room for parallelization here, but in our case we have
    // thousands of calls to sampleWithTrapdoor that can be run in parallel,
    // so there is no reason to parallelize also at this lower level.

    zz_pPush push; // backup NTL's current modulus

    Vec<vec_zz_p> vVec = U; // allocate space and initialize v=u
    vec_zz_p wMod, wBarMod;
    for (long iE = 0; iE < kFactors; iE++)
    {
        params->zzp_context[iE].restore();

        // Compute Abar*(p1-R*p2) modulo the current factor
        const mat_zz_p& aBarMod = aBarCRT[iE];
        mul(wBarMod, aBarMod, conv<vec_zz_p>(p1));  // wBar = Abar*(p1-R*p2)
        // convert p1-R*p2 to vec_zz_p and multiply by aBar modulo each factor

        vVec[iE] -= wBarMod; // u - wBar

        multByG(wMod, p2, n, params->factors, e); // w = G*p2
        vVec[iE] -= wMod;    // u - wBar - w
    }

    // Now that we computed v modulo all the primes, sample z s.t. G*z=v

    sampleG(z, vVec, params);
#ifdef DEBUGPRINT
    cout << "z = " << z << endl;
#endif
    // finally, return x = p + [R/I]*z = p + [R*z / z]

    xOut = p;
    mul(tmp, trapDoorMatR, z); // add R*z to top mBar entries
#ifdef DEBUGPRINT
    cout << "R*z = " << tmp << endl;
#endif
    for (long i=0; i<mBar; i++)
    {
      xOut[i] += tmp[i];
      long absX = abs(xOut[i]);
      if (absX > TDMatrix::maxSample) TDMatrix::maxSample.store(absX);
         // not quite thread-safe, the stored value may not be the maximum
    }

    // add z to bottom m-mBar entries
    for (long i = mBar; i< params->m; i++)
    {
      xOut[i] += z[i - mBar];
      long absX = abs(xOut[i]);
      if (absX > TDMatrix::maxSample) TDMatrix::maxSample.store(absX);
         // not quite thread-safe, the stored value may not be the maximum
    }
    return 0;
}

//Write TDMatrix different parameters to file. The handle to the open
//file is sent as input to the function. returns number of items read by
//the function
long TDMatrix::writeToFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = 0;

    count += params->writeToFile(handle); // write the params
    count += aBarCRT.writeToFile(handle); // write the Abar matrix

    long numRows = trapDoorMatR.NumRows();
    long numCols = trapDoorMatR.NumCols();
    count += fwrite(&numRows,sizeof(long),1,handle);
    count += fwrite(&numCols,sizeof(long),1,handle);
    for (long i = 0; i < numRows; i++)  // write the rows of R, one at a time
    {
        count+= fwrite(trapDoorMatR[i].elts(), sizeof(long), numCols, handle);
        // each row is implemented as a C vector
    }

    count += gaussSamp.writeToFile(handle); // write the DGaussSampler

    return count;
}

//Read the TDMatrix parameters from a flie. The handle to the open file is
//received as input to this function, as well as a pointer to the TDMatrixParams
//if the pointer is not provided and params as not been initialized before, an
//error occurs, as a pointer to this structure is needed
//returns number of items read by the function

long TDMatrix::readFromFile(FILE* handle, TDMatrixParams* prmBuf)
{
    FHE_TIMER_START;
    assert(params != NULL || prmBuf != NULL); // some pointer must be provided
    long count=0;

    // If buffer is given, make params point to it and don't overwrite from iput
    if (prmBuf != NULL)
    {
        TDMatrixParams p;
        count += p.readFromFile(handle);
        assert(p == *prmBuf); // sanity check
        params = prmBuf;      // point to given params
    }
    else
        count += params->readFromFile(handle); // overwrite params from input

    count += aBarCRT.readFromFile(handle,prmBuf); // read the Abar matrix

    long numRows,numCols;
    count += fread(&numRows,sizeof(long),1,handle);
    count += fread(&numCols,sizeof(long),1,handle);
    trapDoorMatR.SetDims(numRows,numCols);
    for (long i = 0; i < numRows; i++)  // write the rows of R, one at a time
    {
        count+= fread(trapDoorMatR[i].elts(), sizeof(long), numCols, handle);
        // each row is implemented as a C vector
    }

    count += gaussSamp.readFromFile(handle); // read the DGaussSampler

    return count;
}

// operator==, //compares all variables in TDMatrix
bool operator==(const TDMatrix& A, const TDMatrix& B)
{
    FHE_TIMER_START;
    if (A.getParams() != B.getParams())
        return false;

    if (A.getABar() != B.getABar())
        return false;

    if (A.getR() != B.getR())
        return false;

    return true;
}

#ifdef DEBUG
static void printTrapdoor(const mat_l& td, const TDMatrixParams& params)
{
    FHE_TIMER_START;
    double maxR = 0;
    double AvgR=1;
    //  long TotalItems;

    for (long i=0; i < params.mBar; i++)
        for (long j=0; j < params.wLen; j++)
        {
            if (abs(td[i][j]) > maxR)
                maxR = abs(td[i][j]);
            AvgR *= (double)(i*params.wLen+j)/(i*params.wLen+j+1);
            AvgR+=(double)abs(td[i][j]) /(i*params.wLen+j+1);
        }
    cout << "maxR = " << maxR << ", AvgR = " << AvgR << "\n";

    mat_l rSquare;
    //how large is R^2?
    square(rSquare,td);
    //  cout << "trapDoorMatR=" << td << "rSquare= " << rSquare << "\n";

    maxR = 0;
    AvgR=1;
    for (long i=0; i < params.mBar; i++)
        for (long j=0; j < params.mBar; j++)
        {
            if (abs(rSquare[i][j]) > maxR)
                maxR = abs(rSquare[i][j]);
            AvgR *= (double)(i*params.wLen+j)/(i*params.wLen+j+1);
            AvgR+=(double)abs(rSquare[i][j]) /(i*params.wLen+j+1);
        }
    cout << "maxRSquare = " << maxR << ", AvgRSquare = " << AvgR << "\n";
}
#endif //DEBUG


#ifndef MATHTOOLBOX_BAYESIAN_OPTIMIZATION_HPP
#define MATHTOOLBOX_BAYESIAN_OPTIMIZATION_HPP

#include <Eigen/Core>
#include <memory>
#include <utility>

namespace mathtoolbox
{
    class GaussianProcessRegressor;

    namespace optimization
    {
        enum class KernelType
        {
            ArdSquaredExp,
            ArdMatern52
        };

        /// \brief An optimizer class for managing Bayesian optimization iterations
        ///
        /// \details This optimizer solves a maximization problem with a black-box function.
        class BayesianOptimizer
        {
        public:
            BayesianOptimizer(const std::function<double(const Eigen::VectorXd&)>& f,
                              const Eigen::VectorXd&                               lower_bound,
                              const Eigen::VectorXd&                               upper_bound,
                              const KernelType& kernel_type = KernelType::ArdMatern52);

            /// \brief Perform a single step of the Bayesian optimization algorithm
            /// \return Newly observed data point
            std::pair<Eigen::VectorXd, double> Step();

            /// \brief Calculate the function value
            /// \return Evaluated function value
            double EvaluatePoint(const Eigen::VectorXd& x) const;

            /// \brief Predict the mean value
            /// \return Predicted mean value
            double PredictMean(const Eigen::VectorXd& x) const;

            /// \brief Predict the standard deviation value
            /// \return Predicted standard deviation value
            double PredictStdev(const Eigen::VectorXd& x) const;

            /// \brief Calculate the acquisition function value at the point
            /// \return Acquisition function value at the point
            double CalcAcquisitionValue(const Eigen::VectorXd& x) const;

            /// \brief Retrieve the optimizer found so far
            /// \return Optimizer found so far
            Eigen::VectorXd GetCurrentOptimizer() const;

            /// \brief Get the observed data points and their values
            std::pair<Eigen::MatrixXd, Eigen::VectorXd> GetData() const;

        private:
            const std::function<double(const Eigen::VectorXd&)> m_f;

            const Eigen::VectorXd m_lower_bound;
            const Eigen::VectorXd m_upper_bound;

            const KernelType m_kernel_type;

            Eigen::MatrixXd m_X;
            Eigen::VectorXd m_y;

            std::shared_ptr<GaussianProcessRegressor> m_regressor;

            void AddDataEntry(const Eigen::VectorXd& x_new, const double y_new);
            void ConstructSurrogateFunction();
        };
    } // namespace optimization
} // namespace mathtoolbox

#endif // MATHTOOLBOX_BAYESIAN_OPTIMIZATION_HPP


// Linear Models
//
// Special Naming Convention:
//
// All mathematical matrix variables are named by captial letters
// (e.g. X means input feature dataset). Vectors are named by lowercase
// (e.g. w means weights vector). These variables are always from Eigen.
//
// The key feature of this linear model is it was implemented by
// advanced and efficient c++ matrix library Eigen, but only for the
// training stage. After that, the weights are stored in normal c++
// array for fast predicting and robust interface.
//
// @author: Bingqing Qu
//
// Copyright (C) 2014-2015  Bingqing Qu <sylar.qu@gmail.com>
//
// @license: See LICENSE at root directory

#ifndef OPENLINEAR_LINEAR_H_
#define OPENLINEAR_LINEAR_H_

#include <assert.h>
#include <iostream>
#include <string>
#include <vector>
#include <memory>
#include <algorithm>
#include <exception>
#include <stdexcept>
#include <Eigen/SparseCore>
#include <Eigen/Core>
#include <stdio.h>
#include <stdarg.h>
namespace oplin{

// Define Eigen vector and matrix types we will use
typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SpRowMatrix;
typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SpColMatrix;
typedef Eigen::SparseVector<double, Eigen::RowMajor> SpRowVector;
typedef Eigen::SparseVector<double, Eigen::ColMajor> SpColVector;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1      , Eigen::ColMajor> ColVector;
typedef Eigen::Matrix<double, 1      , Eigen::Dynamic, Eigen::RowMajor> RowVector;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMatrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> ColMatrix;

// smart pointers
typedef std::shared_ptr<SpColMatrix> SpColMatrixPtr;
typedef std::shared_ptr<ColMatrix> ColMatrixPtr;
typedef std::shared_ptr<ColVector> ColVectorPtr;

//
void VOUT(const char* fmt, ...);
// very very light-weight and useful structure for <key,value> pair storage
template <class K, class V>
struct KeyValue
{
    K i;
    V v;
};
/// Dataset parameters
struct Dataset
{
    /** number of samples */
    size_t n_samples;
    /** number of classes */
    size_t n_classes;
    /** feature dimension */
    size_t dimension;
    /** targets */
    std::vector<double> y;
    /** target labels */
    std::vector<double> labels;
    /**
     * features w.r.t order of y
     * dimension is dimension * n_samples
     */
    SpColMatrixPtr X;
    double bias;
    Dataset() : bias(-1.){}

};
enum FormulaType
{
    L1R_LR,
    L2R_LR

};
enum SolverType
{
    GD,
    SGD,
    L_BFGS,
    TRON
};

/// Parameters for training
struct Parameter
{
    /** define the solver type for training model */
    int solver_type;
    /** define the objective function for solving */
    int problem_type;
    /**
     * relative tolerance between two continuous iterations
     * for training stop criteria
     */
    double rela_tol;
    /** absolute loss tolerance for iterative method */
    double abs_tol;
    /** learning rate for iterative method */
    double learning_rate;
    /** maximum iteration for iterative method */
    size_t max_epoch;
    /** C */
    // std::vector<double> C;
    double base_C;
    std::vector<KeyValue<double,double> > adjust_C;

    Parameter() : solver_type(0.), problem_type(0.){}
};
typedef std::shared_ptr<Parameter> ParamPtr;
typedef std::shared_ptr<Dataset> DatasetPtr;

/// Model Parameters
///
/// An important note here is the key part of model - weights:
/// the representation of weight during training phase is an Eigen
/// dense matrix under consideration of the matrix manipultion
/// in fomulas. But finally converted to a normal c++ array as storage.
/// The consideration here is firstly, the low level c++ array is though
/// not safe in memory, I can validate the memory after training and
/// once the double* is stored, no other manipulation should have
/// previlige to modify it until it will be released by constructor.
/// All in all, the segmentation fault will not happen here under the
/// help of class encapsulation.
///
/// Secondly, the performance should have not much difference with
/// vector but slightly better. The linear model always means fast in
/// industry and that "slightly" is good for all users.
///
struct Model
{
    /** number of classes */
    size_t n_classes;
    /** dimension of feature */
    size_t dimension;
    /** define a bias, 0 if no bias setting */
    double bias;
    /** labels of classes */
    std::vector<double> labels;
    Model() : W_(NULL),bias_values_(NULL){}
    // destructor must be called for double*
    ~Model()
    {
        delete [] W_;
        delete [] bias_values_;
    }
    void set_bias_values(double* vals)
    {
        if(bias_values_)
            delete [] bias_values_;
        bias_values_ = vals;
    }
    void set_weights(double* w)
    {
        if(W_)
            delete [] W_;
        W_ = w;
    }
// I encapsulate the two pointers to avoid wrong reference in productive env.
private:
    double* W_;
    double* bias_values_;
    /** weights */

    friend class LinearBase;
};

typedef std::vector<KeyValue<size_t,double> > FeatureVector;
// symbolic links for short implementation views
// typedef std::shared_ptr<Model> ModelPtr;

// the current design keep model to only one owner to avoid wrong reference in
// productive env.
typedef std::unique_ptr<Model> ModelUniPtr;

/// Base class for linear models
///
///
///
class LinearBase
{
protected:
    // model instance
    ModelUniPtr model_;
    bool trained_;
    void predict_WTx(const FeatureVector, std::vector<double>&);
    void preprocess_data(const DatasetPtr, std::vector<size_t>&,
                         std::vector<size_t>&, std::vector<size_t>&);
public:

    LinearBase(void);
    explicit LinearBase(ModelUniPtr);
    virtual ~LinearBase(void){};

    virtual bool is_trained();
    virtual size_t get_n_classes();
    virtual std::vector<double> get_labels();
    virtual void load_model(ModelUniPtr);
    virtual ModelUniPtr export_model();
    virtual void export_model_to_file(const std::string&);
    virtual void train(const DatasetPtr, const ParamPtr) = 0;
    virtual double predict(const FeatureVector);
    virtual double predict_proba(const FeatureVector, std::vector<double>&);
};

} // oplin
// using namespace oplin;
// using namespace oplin::model;
#endif //OPENLINEAR_LINEAR_H_


#include <iostream>
#include <sstream>
#include <fstream>
#include <type_traits>
#include <chrono>
using namespace std;

#include <libsnark/gadgetlib1/gadgets/basic_gadgets.hpp>
#include <libsnark/common/default_types/r1cs_ppzksnark_pp.hpp>
#include <libff/common/utils.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <libsnark/relations/constraint_satisfaction_problems/r1cs/examples/r1cs_examples.hpp>
#include <boost/optional.hpp>
#include <libff/algebra/curves/mnt/mnt4/mnt4_init.hpp>
#include <libff/algebra/curves/mnt/mnt6/mnt6_init.hpp>
#include <libsnark/gadgetlib1/gadgets/pairing/weierstrass_precomputation.hpp>
using namespace libsnark;

#include "gadget.hpp"

// Timer utility functions

chrono::high_resolution_clock::time_point my_start, my_end;

void
my_timer_start ()
{
  my_start = chrono::high_resolution_clock::now ();
}

int
my_timer_end ()
{
  my_end = chrono::high_resolution_clock::now ();
  return std::chrono::duration_cast < std::chrono::milliseconds >
    (my_end - my_start).count ();
}

// Function from pay-to-sudoku
void
convertBytesToVector (const unsigned char *bytes, std::vector < bool > &v)
{
  int
    numBytes = v.size () / 8;
  unsigned char
    c;
  for (int i = 0; i < numBytes; i++)
    {
      c = bytes[i];

      for (int j = 0; j < 8; j++)
	{
	  v.at ((i * 8) + j) = ((c >> (7 - j)) & 1);
	}
    }
}

void
convertBytesVectorToBytes (const std::vector < unsigned char >&v,
			   unsigned char *bytes)
{
  for (size_t i = 0; i < v.size (); i++)
    {
      bytes[i] = v.at (i);
    }
}

void
convertBytesVectorToVector (const std::vector < unsigned char >&bytes,
			    std::vector < bool > &v)
{
  v.resize (bytes.size () * 8);
  unsigned char
  bytesArr[bytes.size ()];
  convertBytesVectorToBytes (bytes, bytesArr);
  convertBytesToVector (bytesArr, v);
}

template < typename ppT >
  bool run_r1cs_ppzksnark (const r1cs_example < libff::Fr < ppT > >&example)
{
  libff::print_header ("R1CS ppzkSNARK Generator");
  r1cs_ppzksnark_keypair < ppT > keypair =
    r1cs_ppzksnark_generator < ppT > (example.constraint_system);

  libff::print_header ("R1CS ppzkSNARK Prover");
  r1cs_ppzksnark_proof < ppT > proof =
    r1cs_ppzksnark_prover < ppT > (keypair.pk, example.primary_input,
				   example.auxiliary_input);

  libff::print_header ("R1CS ppzkSNARK Verifier");
  const bool
    ans =
    r1cs_ppzksnark_verifier_strong_IC < ppT > (keypair.vk,
					       example.primary_input, proof);
  printf ("* The verification result is: %s\n", (ans ? "PASS" : "FAIL"));

  return ans;
}

template < typename ppT > r1cs_example < libff::Fr < ppT >> gen_BLS_example ()
{
  typedef
    libff::Fr <
    ppT >
    FieldT;

  protoboard < FieldT > pb;

  fair_auditing_gadget < ppT > g (pb);
  const int
    num_inputs = g.num_input_variables ();
  g.generate_r1cs_constraints ();

  auto
    cs = pb.get_constraint_system ();

  auto
    sigma = FieldT::random_element () * libff::G1 < other_curve < ppT >>::one ();
  auto
    gen = FieldT::random_element () * libff::G2 < other_curve < ppT >>::one ();
  auto
    M = sigma;
  auto
    y = gen;

  libff::bit_vector
    r;
  libff::bit_vector
    ad;

  const
    vector <
    uint8_t >
  r8bit =
    { 206, 64, 25, 10, 245, 205, 246, 107, 191, 157, 114, 181, 63, 40, 95,
134, 6, 178, 210, 43, 243, 10, 217, 251, 246, 248, 0, 21, 86, 194, 100, 94 };
  const
    vector <
    uint8_t >
  ad8bit =
    { 253, 199, 66, 55, 24, 155, 80, 121, 138, 60, 36, 201, 186, 221, 164,
65, 194, 53, 192, 159, 252, 7, 194, 24, 200, 217, 57, 55, 45, 204, 71, 9 };

  convertBytesVectorToVector (r8bit, r);
  convertBytesVectorToVector (ad8bit, ad);

  g.generate_r1cs_witness (M, y, gen, ad, sigma, r);

  cout << "Num constraints " << cs.num_constraints () << endl;

  return r1cs_example < FieldT > (std::move (cs),
				  std::move (pb.primary_input ()),
				  std::move (pb.auxiliary_input ()));
}

void
single_test ()
{
  libff::init_mnt4_params ();
  default_r1cs_ppzksnark_pp::init_public_params ();

  r1cs_example < libff::Fr < default_r1cs_ppzksnark_pp > >example =
    gen_BLS_example < default_r1cs_ppzksnark_pp > ();

  bool
    it_works = run_r1cs_ppzksnark < default_r1cs_ppzksnark_pp > (example);
  cout << endl;
  cout << (it_works ? "It works!" : "It failed.") << endl;
}

void
benchmark (int numReps)
{

  libff::init_mnt4_params ();
  default_r1cs_ppzksnark_pp::init_public_params ();

  typedef default_r1cs_ppzksnark_pp
    ppT;

  r1cs_example < libff::Fr < default_r1cs_ppzksnark_pp > >example =
    gen_BLS_example < default_r1cs_ppzksnark_pp > ();

  int
    keygen_t,
    prov_t,
    ver_t;
  keygen_t = prov_t = ver_t = 0;

  for (auto i = 1; i <= numReps; i++)
    {
      // Key generation
      my_timer_start ();
      r1cs_ppzksnark_keypair < ppT > keypair =
	r1cs_ppzksnark_generator < ppT > (example.constraint_system);
      keygen_t += my_timer_end ();

      // Proof
      my_timer_start ();
      r1cs_ppzksnark_proof < ppT > proof =
	r1cs_ppzksnark_prover < ppT > (keypair.pk, example.primary_input,
				       example.auxiliary_input);
      prov_t += my_timer_end ();

      // Verification
      my_timer_start ();
      r1cs_ppzksnark_verifier_strong_IC < ppT > (keypair.vk,
						 example.primary_input,
						 proof);
      ver_t += my_timer_end ();
    }

  cout << "Avg Keygen Time: " << keygen_t / numReps << " millis" << endl;
  cout << "Avg Proving Time: " << prov_t / numReps << " millis" << endl;
  cout << "Avg Verification Time: " << ver_t / numReps << " millis" << endl;
}

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

  //benchmark (100);

  return 0;

}