/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file point_set.hpp
 * \date October 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <map>
#include <cmath>
#include <type_traits>

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/traits.hpp>

#include <fl/distribution/gaussian.hpp>
#include <fl/exception/exception.hpp>
#include <fl/filter/filter_interface.hpp>

/** \cond internal */
/**
 * Checks dimensions
 */
#define INLINE_CHECK_POINT_SET_DIMENSIONS() \
    assert(points_.cols() == int(weights_.size())); \
    if (points_.rows() == 0) \
    { \
        fl_throw(ZeroDimensionException("PointSet")); \
    } \
    if (points_.cols() == 0)  \
    { \
        fl_throw(Exception("PointSet contains no points.")); \
    }

/**
 * Checks for index out of bounds and valid dimensions
 */
#define INLINE_CHECK_POINT_SET_BOUNDS(i) \
    if (i >= int(weights_.size())) \
    { \
        fl_throw(OutOfBoundsException(i, weights_.size())); \
    } \
    INLINE_CHECK_POINT_SET_DIMENSIONS();
/** \endcond */

namespace fl
{

// Forward declaration
template <typename Point, int Points_> class PointSet;

/**
 * Trait struct of a PointSet
 */
template <typename Point_, int Points_>
struct Traits<PointSet<Point_, Points_>>
{
    /**
     * \brief Point type
     */
    typedef Point_ Point;

    /**
     * \brief Number of points for fixed-size set
     */
    enum
    {
        /**
         * \brief Number of points which provided by the PointSet.
         *
         * If the number of points is unknown and there for dynamic, then
         * NumberOfPoints is set to Eigen::Dynamic
         */
        NumberOfPoints = IsFixed<Points_>() ? Points_ : Eigen::Dynamic
    };

    /**
     * \brief Weight harbors the weights of a point. For each of the first two
     * moments there is a separate weight.
     *
     *
     * Generally a single weight suffices. However, some transforms utilize
     * different weights for each moment to select a set of points representing
     * the underlying moments.
     */
    struct Weight
    {
        /**
         * First moment (mean) point weight
         */
        double w_mean;

        /**
         * Second centered moment (covariance) point weight
         */
        double w_cov;
    };

    /**
     * \brief Point container type
     *
     * \details
     * The point container type has a fixed-size dimension of a point
     * and the number of the points is statically known.
     */
    typedef Eigen::Matrix<
                typename Point::Scalar,
                Point::RowsAtCompileTime,
                Points_
            > PointMatrix;

    /**
     * \brief WeightVector
     */
    typedef Eigen::Matrix<
                typename Point::Scalar,
                Points_,
                1
            > WeightVector;

    /**
     * \brief Weight list of all points
     */
    typedef Eigen::Array<Weight, Points_, 1> Weights;
};

/**
 * \ingroup nonlinear_gaussian_filter
 *
 * \brief PointSet represents a container of fixed-size or dynamic-size points each
 *        paired with a set of weights.
 *
 * PointSet has two degree-of-freedoms. The first
 * is the dimension of the points. The second is the number of points within the
 * set. Each of the parameter can either be fixed at compile time or left
 * unspecified. That is, the parameter is set to Eigen::Dynamic.
 *
 * \tparam Point    Gaussian variable type
 * \tparam Points_  Number of points representing the gaussian
 */
template <typename Point_, int Points_ = -1>
class PointSet
{
private:
    /** \brief Typdef of \c This for #from_traits(TypeName) helper */
    typedef PointSet<Point_, Points_> This;

public:
    typedef from_traits(Point);
    typedef from_traits(PointMatrix);
    typedef from_traits(Weight);
    typedef from_traits(Weights);
    typedef from_traits(WeightVector);

    static_assert(Points_ != 0, "Invalid point count");

public:
    /**
     * \brief Creates a PointSet
     *
     * \param points_count   Number of points representing the Gaussian
     * \param dimension      Sample space dimension
     */
    PointSet(int dimension,
             int points_count = ToDimension<Points_>())
        : points_(dimension, points_count),
          weights_(points_count, 1)
    {
        assert(points_count >= 0);
        static_assert(Points_ >= Eigen::Dynamic, "Invalid point count");

        points_.setZero();

        double weight = (points_count > 0) ? 1./double(points_count) : 0;
        weights_.fill(Weight{weight, weight});
    }

    /**
     * \brief Creates a PointSet
     */
    PointSet()
    {
        points_.setZero();

        double weight = (weights_.size() > 0) ? 1./double(weights_.size()) : 0;
        weights_.fill(Weight{weight, weight});
    }

    /**
     * \brief Overridable default destructor
     */
    virtual ~PointSet() noexcept { }

    /**
     * \brief Resizes a dynamic-size PointSet
     *
     * \param Points_   Number of points
     *
     * \throws ResizingFixedSizeEntityException
     */
    void resize(int points_count)
    {
        if (int(weights_.size()) == points_count &&
            int(points_.cols()) == points_count) return;

        if (IsFixed<Points_>())
        {
            fl_throw(
                fl::ResizingFixedSizeEntityException(weights_.size(),
                                                     points_count,
                                                     "poit set Gaussian"));
        }

        points_.setZero(points_.rows(), points_count);

        weights_.resize(points_count, 1);
        const double weight = (points_count > 0)? 1. / double(points_count) : 0;
        for (int i = 0; i < points_count; ++i)
        {
            weights_(i).w_mean = weight;
            weights_(i).w_cov = weight;
        }

        // std::fill(weights_.begin(), weights_.end(), Weight{weight, weight});
    }

    /**
     * \brief Resizes a dynamic-size PointSet
     *
     * \param Points_   Number of points
     *
     * \throws ResizingFixedSizeEntityException
     */
    void resize(int dim, int points_count)
    {
        if (dim == dimension() && points_count == count_points()) return;

        points_.setZero(dim, points_count);

        weights_.resize(points_count, 1);
        const double weight = (points_count > 0)? 1. / double(points_count) : 0;
        for (int i = 0; i < points_count; ++i)
        {
            weights_(i).w_mean = weight;
            weights_(i).w_cov = weight;
        }
    }

    /**
     * \brief Sets the new dimension for dynamic-size points (not to confuse
     * with the number of points)
     *
     * \param dim  Dimension of each point
     */
    void dimension(int dim)
    {
        points_.resize(dim, count_points());
    }

    /**
     * \return Dimension of containing points
     */
    int dimension() const
    {
        return points_.rows();
    }

    /**
     * \brief  The number of points
     */
    int count_points() const
    {
        return points_.cols();
    }

    /**
     * \brief  Read only access on i-th point
     *
     * \param i Index of requested point
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    Point point(int i) const
    {
        INLINE_CHECK_POINT_SET_BOUNDS(i);

        return points_.col(i);
    }

    auto operator[](int i) -> decltype(PointMatrix().col(i))
    {
        return points_.col(i);
    }

    /**
     * \brief  weight of i-th point assuming both weights are the same
     *
     * \param i Index of requested point
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    double weight(int i)
    {
        INLINE_CHECK_POINT_SET_BOUNDS(i);

        return weights_(i).w_mean;
    }

    /**
     * \brief  weights of i-th point
     *
     * \param i Index of requested point
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    const Weight& weights(int i) const
    {
        INLINE_CHECK_POINT_SET_BOUNDS(i);

        return weights_(i);
    }

    /**
     * \brief  Point matrix (read only)
     */
    const PointMatrix& points() const noexcept
    {
        return points_;
    }

    /**
     * \brief  Point matrix
     */
    PointMatrix& points()
    {
        return points_;
    }

    /**
     * \brief  point weights vector
     */
    const Weights& weights() const noexcept
    {
        return weights_;
    }

    /**
     * \brief  Returns the weights for the mean of the points as a vector
     */
    WeightVector mean_weights_vector() const noexcept
    {
        const int point_count = count_points();

        WeightVector weight_vec(point_count);

        for (int i = 0; i < point_count; ++i)
        {
            weight_vec(i) = weights_(i).w_mean;
        }

        return weight_vec;
    }

    /**
     * \brief  Returns the weights for the covariance of the points as a vector
     */
    WeightVector covariance_weights_vector() const noexcept
    {
        const int point_count = count_points();

        WeightVector weight_vec(point_count);

        for (int i = 0; i < point_count; ++i)
        {
            weight_vec(i) = weights_(i).w_cov;
        }

        return weight_vec;
    }

    /**
     * \brief Sets the given point matrix
     */
    void points(const PointMatrix& point_matrix)
    {
        points_ = point_matrix;
    }

    /**
     * \brief Sets a given point at position i
     *
     * \param i         Index of point
     * \param p         The new point
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    void point(int i, Point p)
    {
        INLINE_CHECK_POINT_SET_BOUNDS(i);

        points_.col(i) = p;
    }

    /**
     * \brief Sets a given point at position i along with its weights
     *
     * \param i         Index of point
     * \param p         The new point
     * \param w         Point weights. The weights determinaing the first two
     *                  moments are the same
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    void point(int i, Point p, double w)
    {
        point(i, p, Weight{w, w});
    }

    /**
     * \brief Sets a given point at position i along with its weights
     *
     * \param i         Index of point
     * \param p         The new point
     * \param w_mean    point weight used to compute the first moment
     * \param w_cov     point weight used to compute the second centered moment
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    void point(int i, Point p, double w_mean , double w_cov)
    {
        point(i, p, Weight{w_mean, w_cov});
    }

    /**
     * \brief Sets a given point at given position i along with its weights
     *
     * \param i         Index of point
     * \param p         The new point
     * \param weights   point weights
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    void point(int i, Point p, Weight weights)
    {
        INLINE_CHECK_POINT_SET_BOUNDS(i);

        points_.col(i) = p;
        weights_(i) = weights;
    }

    /**
     * \brief Sets a given weight of a point at position i
     *
     * \param i         Index of point
     * \param w         Point weights. The weights determinaing the first two
     *                  moments are the same
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    void weight(int i, double w)
    {
        weight(i, Weight{w, w});
    }

    /**
     * \brief Sets given weights of a point at position i
     *
     * \param i         Index of point
     * \param w_mean    point weight used to compute the first moment
     * \param w_cov     point weight used to compute the second centered moment
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    void weight(int i, double w_mean , double w_cov)
    {
        weight(i, Weight{w_mean, w_cov});
    }

    /**
     * Sets given weights of a point at position i
     *
     * \param i         Index of point
     * \param weights   point weights
     *
     * \throws OutOfBoundsException
     * \throws ZeroDimensionException
     */
    void weight(int i, Weight weights)
    {
        INLINE_CHECK_POINT_SET_BOUNDS(i);

        weights_(i) = weights;
    }

    /**
     * \brief Creates a PointMatrix populated with zero mean points
     *
     * \return Centered points matrix.
     *
     * \throws ZeroDimensionException
     * \throws Exception
     */
    PointMatrix centered_points() const
    {
        INLINE_CHECK_POINT_SET_DIMENSIONS();

        PointMatrix centered(points_.rows(), points_.cols());

        const Point weighted_mean = mean();
        const int point_count = points_.cols();
        for (int i = 0; i < point_count; ++i)
        {
            centered.col(i) = points_.col(i) - weighted_mean;
        }

        return centered;
    }

    /**
     * \brief Centers all points and returns the mean over all points.
     *
     * Computes the weighted mean of all points and subtracts the mean from all
     * points. Finally returns the weighted mean
     *
     * \return Weighted mean
     */
    Point center()
    {
        Point weighted_mean = mean();
        const int point_count = points_.cols();
        for (int i = 0; i < point_count; ++i)
        {
            points_.col(i) -= weighted_mean;
        }

        return weighted_mean;
    }

    /**
     * \brief Returns the weighted mean of all points
     *
     * \throws ZeroDimensionException
     * \throws Exception
     */
    Point mean() const
    {
        INLINE_CHECK_POINT_SET_DIMENSIONS();

        Point weighted_mean;
        weighted_mean.setZero(points_.rows());

        const int point_count = points_.cols();
        for (int i = 0; i < point_count; ++i)
        {
            weighted_mean += weights_(i).w_mean * points_.col(i);
        }

        return weighted_mean;
    }

protected:
    /**
     * \brief point container
     */
    PointMatrix points_;

    /**
     * \brief weight container
     */
    Weights weights_;
};

}


/*
 * BSD 3-Clause License
 *
 * Full text: https://opensource.org/licenses/BSD-3-Clause
 *
 * Copyright (c) 2018, Viktor Seib
 * All rights reserved.
 *
 */

#include "utils.h"
#include <Eigen/Eigenvalues>

#include "../third_party/libgdiam-1.3/gdiam.hpp"

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

namespace ism3d
{
    Json::Value Utils::vector3fToJson(const Eigen::Vector3f& vector)
    {
        Json::Value json(Json::arrayValue);

        json.append(vector[0]);
        json.append(vector[1]);
        json.append(vector[2]);

        return json;
    }

    bool Utils::jsonToVector3f(Json::Value json, Eigen::Vector3f& vector)
    {
        if (json.isNull() || !json.isArray() || json.size() != 3)
            return false;

        vector[0] = json[0].asFloat();
        vector[1] = json[1].asFloat();
        vector[2] = json[2].asFloat();

        return true;
    }

    Json::Value Utils::quatToJson(const boost::math::quaternion<float>& quat)
    {
        Json::Value json(Json::arrayValue);

        json.append(quat.R_component_1());
        json.append(quat.R_component_2());
        json.append(quat.R_component_3());
        json.append(quat.R_component_4());

        return json;
    }

    bool Utils::jsonToQuat(Json::Value json, boost::math::quaternion<float>& quat)
    {
        if (json.isNull() || !json.isArray() || json.size() != 4)
            return false;

        quat = boost::math::quaternion<float>(json[0].asFloat(),
                json[1].asFloat(), json[2].asFloat(), json[3].asFloat());

        return true;
    }

    float Utils::ln(float x)
    {
        return log(x) / log(10);
    }

    boost::math::quaternion<float> Utils::ln(const boost::math::quaternion<float>& quat)
    {
        float w = quat.R_component_1();
        float x = quat.R_component_2();
        float y = quat.R_component_3();
        float z = quat.R_component_4();

        if (fabs(w) < 1.0f) {
            float angle = acosf(w);
            float sinAngle = sinf(angle);
            if (sinAngle >= 0.00001) {
                float coeff = angle / sinAngle;
                x *= coeff;
                y *= coeff;
                z *= coeff;
            }
        }

        boost::math::quaternion<float> result(w, x, y, z);
        return result;

        /*float qNorm = boost::math::norm(quat);
        float vNorm = sqrtf(quat.R_component_2() * quat.R_component_2() +
                            quat.R_component_3() * quat.R_component_3() +
                            quat.R_component_4() * quat.R_component_4());

        float w = ln(qNorm);
        float x = (quat.R_component_2() / vNorm) * acosf(quat.R_component_1() / qNorm);
        float y = (quat.R_component_3() / vNorm) * acosf(quat.R_component_1() / qNorm);
        float z = (quat.R_component_4() / vNorm) * acosf(quat.R_component_1() / qNorm);*/
    }

    boost::math::quaternion<float> Utils::exp(const boost::math::quaternion<float>& quat)
    {
        float w = quat.R_component_1();
        float x = quat.R_component_2();
        float y = quat.R_component_3();
        float z = quat.R_component_4();

        float angle = sqrtf(x * x + y * y + z * z);
        float sinAngle = sinf(angle);

        w = cosf(angle);

        if (fabs(sinAngle) >= 0.00001f) {
            float coeff = sinAngle / angle;
            x *= coeff;
            y *= coeff;
            z *= coeff;
        }

        boost::math::quaternion<float> result(w, x, y, z);
        return result;
    }

    float Utils::deg2rad(float deg)
    {
        return deg * (M_PI / 180.0f);
    }

    float Utils::rad2deg(float rad)
    {
        return (rad * 180.0f) / M_PI;
    }

    void Utils::getRotQuaternion(const pcl::ReferenceFrame& refFrame, boost::math::quaternion<float>& quaternion)
    {
        // transform the point from the world into the keypoint coordinate system
        Eigen::Matrix3f keypointCoord = Eigen::Matrix3f::Identity();
        keypointCoord(0, 0) = refFrame.x_axis[0];
        keypointCoord(1, 0) = refFrame.x_axis[1];
        keypointCoord(2, 0) = refFrame.x_axis[2];
        keypointCoord(0, 1) = refFrame.y_axis[0];
        keypointCoord(1, 1) = refFrame.y_axis[1];
        keypointCoord(2, 1) = refFrame.y_axis[2];
        keypointCoord(0, 2) = refFrame.z_axis[0];
        keypointCoord(1, 2) = refFrame.z_axis[1];
        keypointCoord(2, 2) = refFrame.z_axis[2];

        // create rotation quaternion
        matrix2Quat(keypointCoord, quaternion);
    }

    Eigen::Vector3f Utils::rotateInto(const Eigen::Vector3f& point, const pcl::ReferenceFrame& refFrame)
    {
        // create rotation quaternion
        boost::math::quaternion<float> rotQuat;
        getRotQuaternion(refFrame, rotQuat);

        // rotate point into coordinate system
        Eigen::Vector3f result(point);
        quatRotateInv(rotQuat, result);

        return result;
    }

    Eigen::Vector3f Utils::rotateBack(const Eigen::Vector3f& point, const pcl::ReferenceFrame& refFrame)
    {
        // create rotation quaternion
        boost::math::quaternion<float> rotQuat;
        getRotQuaternion(refFrame, rotQuat);

        // rotate point from coordinate system back into world
        Eigen::Vector3f result(point);
        quatRotate(rotQuat, result);

        return result;
    }

    std::vector<pcl::ReferenceFrame> Utils::generateFrames(const pcl::ReferenceFrame& refFrame)
    {
        // This function 4 different coordinates system by rotating the reference frame with the
        // right hand rule (see http://www.pointclouds.org/assets/uploads/cglibs13_features.pdf).

        std::vector<pcl::ReferenceFrame> frames(4, refFrame);

        // frames[0] = refFrame

        pcl::ReferenceFrame& frame1 = frames[1];
        frame1.x_axis[0] *= -1;
        frame1.x_axis[1] *= -1;
        frame1.x_axis[2] *= -1;
        frame1.y_axis[0] *= -1;
        frame1.y_axis[1] *= -1;
        frame1.y_axis[2] *= -1;

        pcl::ReferenceFrame& frame2 = frames[2];
        frame2.x_axis[0] *= -1;
        frame2.x_axis[1] *= -1;
        frame2.x_axis[2] *= -1;
        frame2.z_axis[0] *= -1;
        frame2.z_axis[1] *= -1;
        frame2.z_axis[2] *= -1;

        pcl::ReferenceFrame& frame3 = frames[3];
        frame3.y_axis[0] *= -1;
        frame3.y_axis[1] *= -1;
        frame3.y_axis[2] *= -1;
        frame3.z_axis[0] *= -1;
        frame3.z_axis[1] *= -1;
        frame3.z_axis[2] *= -1;

        return frames;
    }

    template
    Utils::BoundingBox Utils::computeAABB<PointT>(const pcl::PointCloud<PointT>::ConstPtr &model);
    template
    Utils::BoundingBox Utils::computeAABB<PointNormalT>(const pcl::PointCloud<PointNormalT>::ConstPtr &model);

    template<typename T>
    Utils::BoundingBox Utils::computeAABB(const typename pcl::PointCloud<T>::ConstPtr &model)
    {
        T minP, maxP;
        pcl::getMinMax3D(*model, minP, maxP);

        Utils::BoundingBox box;
        box.rotQuat = boost::math::quaternion<float>(1, 0, 0, 0);
        box.size = Eigen::Vector3f(maxP.x - minP.x, maxP.y - minP.y, maxP.z - minP.z);
        box.position = Eigen::Vector3f(minP.x + (box.size[0] / 2),
                minP.y + (box.size[1] / 2), minP.z + (box.size[2] / 2));
        return box;
    }

    template
    Utils::BoundingBox Utils::computeMVBB<PointT>(const pcl::PointCloud<PointT>::ConstPtr&);

    template
    Utils::BoundingBox Utils::computeMVBB<PointNormalT>(const pcl::PointCloud<PointNormalT>::ConstPtr&);

    template<typename T>
    Utils::BoundingBox Utils::computeMVBB(const typename pcl::PointCloud<T>::ConstPtr &model)
    {
        // remove nan points to avoid infinite loops
        typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>());
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*model,*cloud, indices);

        gdiam_real* points = (gdiam_point)malloc(sizeof(gdiam_point_t) * cloud->points.size());
        assert(points != NULL);

        for (int i = 0; i < (int)cloud->size(); i++) {
            const T& point = cloud->at(i);
            points[i * 3 + 0] = point.x;
            points[i * 3 + 1] = point.y;
            points[i * 3 + 2] = point.z;
        }

        // compute minimum volume bounding box
        gdiam_point* pnt_arr = gdiam_convert((gdiam_real*)points, cloud->points.size());
        gdiam_bbox bb = gdiam_approx_mvbb(pnt_arr, cloud->points.size(), 0.0);
        free(points);

        Eigen::Vector3d minP, maxP;
        bb.get_min(&minP[0], &minP[1], &minP[2]);
        bb.get_max(&maxP[0], &maxP[1], &maxP[2]);

        Eigen::Vector3f dirX = Eigen::Vector3f(bb.get_dir(0)[0], bb.get_dir(0)[1], bb.get_dir(0)[2]);
        Eigen::Vector3f dirY = Eigen::Vector3f(bb.get_dir(1)[0], bb.get_dir(1)[1], bb.get_dir(1)[2]);
        Eigen::Vector3f dirZ = Eigen::Vector3f(bb.get_dir(2)[0], bb.get_dir(2)[1], bb.get_dir(2)[2]);
        Eigen::Vector3f size = Eigen::Vector3f(maxP[0] - minP[0], maxP[1] - minP[1], maxP[2] - minP[2]);
        Eigen::Vector3f pos = Eigen::Vector3f(minP[0] + (size[0] / 2.0f),
                minP[1] + (size[1] / 2.0f), minP[2] + (size[2] / 2.0f));

        Eigen::Matrix3f rot = Eigen::Matrix3f::Identity();
        rot(0, 0) = dirX[0];
        rot(1, 0) = dirX[1];
        rot(2, 0) = dirX[2];
        rot(0, 1) = dirY[0];
        rot(1, 1) = dirY[1];
        rot(2, 1) = dirY[2];
        rot(0, 2) = dirZ[0];
        rot(1, 2) = dirZ[1];
        rot(2, 2) = dirZ[2];

        Utils::BoundingBox box;
        Utils::matrix2Quat(rot, box.rotQuat);
        box.size = size;
        box.position = pos;
        Utils::quatRotate(box.rotQuat, box.position);

        return box;
    }

    template
    float Utils::computeCloudRadius<PointT>(const pcl::PointCloud<PointT>::Ptr &cloud);

    template
    float Utils::computeCloudRadius<PointNormalT>(const pcl::PointCloud<PointNormalT>::Ptr &cloud);

    template<typename T>
    float Utils::computeCloudRadius(const typename pcl::PointCloud<T>::Ptr &cloud)
    {
        // compute the object centroid
        Eigen::Vector4f centroid4f;
        pcl::compute3DCentroid(*cloud, centroid4f);
        Eigen::Vector3f centroid(centroid4f[0], centroid4f[1], centroid4f[2]);

        // compute radius (maximum distance of a point to centroid)
        float radius = 0.0f;
        for(const T &point : cloud->points)
        {
            Eigen::Vector3f eigpoint = point.getArray3fMap();
            float temp_radius = (eigpoint-centroid).norm();
            if(temp_radius > radius)
            {
                radius = temp_radius;
            }
        }
        return radius;
    }

    // TODO VS check if this is ever used
    float Utils::computeHingeLoss(const std::vector<float> &class_distances, const unsigned class_id)
    {
        float sum = 0;
        float true_class_dist = fabs(class_distances.at(class_id));

        for(int i = 0; i < class_distances.size(); i++)
        {
            if(class_id == i) continue;

            float class_dist = fabs(class_distances.at(i));
            sum += std::max(0.0f, class_dist - true_class_dist + 1);
        }

        return sum;
    }

    void Utils::matrix2Quat(const float* rot, float* quat)
    {
        // convert rotation matrix to quaternion (adapted from OgreQuaternion.cpp)

        float matrix[3][3] = {
            {rot[0], rot[1], rot[2]},
            {rot[3], rot[4], rot[5]},
            {rot[6], rot[7], rot[8]}
        };

        float trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
        float root;

        if (trace > 0.0f) {
            root = sqrtf(trace + 1.0f);
            quat[3] = 0.5f * root;
            root = 0.5f / root;

            quat[0] = (matrix[2][1] - matrix[1][2]) * root;
            quat[1] = (matrix[0][2] - matrix[2][0]) * root;
            quat[2] = (matrix[1][0] - matrix[0][1]) * root;
        }
        else {
            static size_t next[3] = {1, 2, 0};
            size_t i = 0;
            if (matrix[1][1] > matrix[0][0])
                i = 1;
            if (matrix[2][2] > matrix[i][i])
                i = 2;
            size_t j = next[i];
            size_t k = next[j];

            root = sqrtf(matrix[i][i] - matrix[j][j] - matrix[k][k] + 1.0);
            float* apkQuat[3] = {&quat[0], &quat[1], &quat[2]};
            *apkQuat[i] = 0.5f * root;
            root = 0.5f / root;
            quat[3] = (matrix[k][j] - matrix[j][k]) * root;
            *apkQuat[j] = (matrix[j][i] + matrix[i][j]) * root;
            *apkQuat[k] = (matrix[k][i] + matrix[i][k]) * root;
        }
    }

    void Utils::matrix2Quat(const Eigen::Matrix3f& rot, boost::math::quaternion<float>& quat)
    {
        float q[4] = { quat.R_component_2(),
            quat.R_component_3(),
            quat.R_component_4(),
            quat.R_component_1() };

        matrix2Quat((float*)&rot(0, 0), q);

        quat = boost::math::quaternion<float>(q[3], q[0], q[1], q[2]);
    }

    void Utils::quat2Matrix(const float* quat, float* rot)
    {
        // convert quaternion to rotation matrix (adapted from OgreQuaternion.cpp)

        float tx = quat[0] + quat[0];
        float ty = quat[1] + quat[1];
        float tz = quat[2] + quat[2];
        float twx = tx * quat[3];
        float twy = ty * quat[3];
        float twz = tz * quat[3];
        float txx = tx * quat[0];
        float txy = ty * quat[0];
        float txz = tz * quat[0];
        float tyy = ty * quat[1];
        float tyz = tz * quat[1];
        float tzz = tz * quat[2];

        float matrix[3][3];

        matrix[0][0] = 1.0 - (tyy + tzz);
        matrix[0][1] = txy - twz;
        matrix[0][2] = txz + twy;
        matrix[1][0] = txy + twz;
        matrix[1][1] = 1.0 - (txx + tzz);
        matrix[1][2] = tyz - twx;
        matrix[2][0] = txz - twy;
        matrix[2][1] = tyz + twx;
        matrix[2][2] = 1.0 - (txx + tyy);

        rot[0] = matrix[0][0];
        rot[1] = matrix[0][1];
        rot[2] = matrix[0][2];
        rot[3] = matrix[1][0];
        rot[4] = matrix[1][1];
        rot[5] = matrix[1][2];
        rot[6] = matrix[2][0];
        rot[7] = matrix[2][1];
        rot[8] = matrix[2][2];
    }

    void Utils::quat2Matrix(const boost::math::quaternion<float>& quat, Eigen::Matrix3f& rot)
    {
        float q[4] = { quat.R_component_2(),
            quat.R_component_3(),
            quat.R_component_4(),
            quat.R_component_1() };

        quat2Matrix(q, (float*)&rot(0, 0));
    }

    void Utils::euler2Quat(boost::math::quaternion<float>& quat, float angleX, float angleY, float angleZ)
    {
        float r = deg2rad(angleX / 2.0f);
        float p = deg2rad(angleY / 2.0f);
        float y = deg2rad(angleZ / 2.0f);

        float sinp = sinf(p);
        float siny = sinf(y);
        float sinr = sinf(r);
        float cosp = cosf(p);
        float cosy = cosf(y);
        float cosr = cosf(r);

        quat = boost::math::quaternion<float>(
            cosr * cosp * cosy + sinr * sinp * siny,
            sinr * cosp * cosy - cosr * sinp * siny,
            cosr * sinp * cosy + sinr * cosp * siny,
            cosr * cosp * siny - sinr * sinp * cosy);

        // normalize
        quat /= boost::math::norm(quat);
    }

    void Utils::quat2Euler(const boost::math::quaternion<float>& quat, float& angleX, float& angleY, float& angleZ)
    {
        boost::math::quaternion<float> myQuat = quat;
        myQuat /= boost::math::norm(myQuat);

        Eigen::Vector3f euler(0, 0, 0);

        float qW = myQuat.R_component_1();
        float qX = myQuat.R_component_2();
        float qY = myQuat.R_component_3();
        float qZ = myQuat.R_component_4();

        float test = (qW * qY - qZ * qX);
        float unit = qX * qX + qY * qY + qZ * qZ + qW * qW;

        // handle singularities
        if (test > 0.4999999f * unit) {
            euler[0] = 2.0f * atan2(qX, qW);
            euler[1] = M_PI / 2.0f;
            euler[2] = 0;
        }
        else if (test < -0.4999999f * unit) {
            euler[0] = 2.0f * atan2(qX, qW);
            euler[1] = -M_PI / 2.0f;
            euler[2] = 0;
        }
        else {
            euler[0] = atan2(2.0f * (qW * qX + qY * qZ), 1.0f - 2.0f * (qX * qX + qY * qY));
            euler[1] = asin(2.0f * test);
            euler[2] = atan2(2.0f * (qW * qZ + qX * qY), 1.0f - 2.0f * (qY * qY + qZ * qZ));
        }

        angleX = rad2deg(euler[0]);
        angleY = rad2deg(euler[1]);
        angleZ = rad2deg(euler[2]);
    }

    void Utils::axis2Quat(boost::math::quaternion<float>& quat, const Eigen::Vector3f& axis, float angle)
    {
        Eigen::Vector3f myAxis = axis;
        myAxis.normalize();

        float halfAngle = deg2rad(angle / 2.0f);
        float sinAngle = sinf(halfAngle);

        quat = boost::math::quaternion<float>(cosf(halfAngle), myAxis[0] * sinAngle,
                myAxis[1] * sinAngle, myAxis[2] * sinAngle);
    }

    void Utils::quat2Axis(const boost::math::quaternion<float>& quat, Eigen::Vector3f& axis, float& angle)
    {
        boost::math::quaternion<float> myQuat = quat;
        float qw = myQuat.R_component_1();

        // normalize
        if (qw > 1.0f)
            myQuat /= boost::math::norm(myQuat);

        angle = rad2deg(2.0f * acos(qw));
        float s = sqrtf(1.0f - qw * qw);

        if (s < 0.0001f) {
            // avoid divbyzero, any arbitrary axis is valid
            axis[0] = 0;
            axis[1] = 1;
            axis[2] = 0;
        }
        else {
            axis[0] = myQuat.R_component_2() / s;
            axis[1] = myQuat.R_component_3() / s;
            axis[2] = myQuat.R_component_4() / s;
        }
    }

    void Utils::quatRotate(const boost::math::quaternion<float>& quat, Eigen::Vector3f& point)
    {
        boost::math::quaternion<float> pointTemp(0, point[0], point[1], point[2]);
        pointTemp = boost::math::conj(quat) * pointTemp * quat;
        point = Eigen::Vector3f(pointTemp.R_component_2(), pointTemp.R_component_3(),
            pointTemp.R_component_4());
    }

    void Utils::quatRotateInv(const boost::math::quaternion<float>& quat, Eigen::Vector3f& point)
    {
        boost::math::quaternion<float> pointTemp(0, point[0], point[1], point[2]);
        pointTemp = quat * pointTemp * boost::math::conj(quat);
        point = Eigen::Vector3f(pointTemp.R_component_2(), pointTemp.R_component_3(),
            pointTemp.R_component_4());
    }

    void Utils::quatGetRotationTo(boost::math::quaternion<float>& quat,
                                  const Eigen::Vector3f& src, const Eigen::Vector3f& dest)
    {
        // Based on Stan Melax's article in Game Programming Gems

        // Copy, since cannot modify local
        Eigen::Vector3f v0 = src;
        Eigen::Vector3f v1 = dest;
        v0.normalize();
        v1.normalize();

        float d = v0.dot(v1);
        // If dot == 1, vectors are the same
        if (d >= 1.0f) {
            // identity quaternion
            quat = boost::math::quaternion<float>(1, 0, 0, 0);
        }
        else if (d < (1e-6f - 1.0f)) {
            // Generate an axis
            Eigen::Vector3f axis = Eigen::Vector3f(1, 0, 0).cross(src);
            if (axis.norm() < (1e-06 * 1e-06)) // pick another if colinear
                axis = Eigen::Vector3f(0, 1, 0).cross(src);
            axis.normalize();
            axis2Quat(quat, axis, M_PI);
        }
        else {
            float s = sqrtf((1 + d) * 2);
            float invS = 1 / s;

            Eigen::Vector3f c = v0.cross(v1);

            float x = c[0] * invS;
            float y = c[1] * invS;
            float z = c[2] * invS;
            float w = s * 0.5f;

            quat = boost::math::quaternion<float>(w, x, y, z);
            quat /= boost::math::norm(quat);
        }
    }

    void Utils::quatWeightedAverage(const std::vector<boost::math::quaternion<float> >& quaternions,
                                    const std::vector<float>& weights,
                                    boost::math::quaternion<float>& result)
    {
        Eigen::Matrix4f scatterMatrix;
        scatterMatrix.setZero();

        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                float value = 0;

                #pragma omp parallel for
                for (int k = 0; k < (int)quaternions.size(); k++)
                {
                    const boost::math::quaternion<float>& quat1 = quaternions[k];
                    Eigen::Vector4f quatVec1(quat1.R_component_1(), quat1.R_component_2(), quat1.R_component_3(), quat1.R_component_4());
                    float weight = weights[k];
                    float factor = weight * quatVec1[i] * quatVec1[j];
                    #pragma omp critical
                    {
                        value += factor;
                    }
                }
                scatterMatrix(i, j) = value;
            }
        }

        Eigen::EigenSolver<Eigen::Matrix4f> solver(scatterMatrix);
        Eigen::EigenSolver<Eigen::Matrix4f>::EigenvalueType eigenvalues = solver.eigenvalues();
        Eigen::EigenSolver<Eigen::Matrix4f>::EigenvectorsType eigenvectors = solver.eigenvectors();

        int maxEigenvalueIndex = 0;
        float maxEigenvalue = 0;
        for (int i = 0; i < eigenvalues.cols(); i++) {
            const std::complex<float> eigenvalue = eigenvalues[i];
            if (eigenvalue.real() > maxEigenvalue) {
                maxEigenvalue = eigenvalue.real();
                maxEigenvalueIndex = i;
            }
        }

        Eigen::Vector4cf maxEigenvector(eigenvectors(0, maxEigenvalueIndex),
                                       eigenvectors(1, maxEigenvalueIndex),
                                       eigenvectors(2, maxEigenvalueIndex),
                                       eigenvectors(3, maxEigenvalueIndex));

        result = boost::math::quaternion<float>(maxEigenvector[0].real(), maxEigenvector[1].real(),
                maxEigenvector[2].real(), maxEigenvector[3].real());
    }
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
/*

  Paul Moore's request:

  As an example of a practical problem which is not restricted to graph
  "experts", consider file dependencies. It's basically graph construction,
  plus topological sort, but it might make a nice "tutorial" example. Build a
  dependency graph of files, then use the algorithms to do things like

  1. Produce a full recompilation order (topological sort, by modified date)
  2. Produce a "parallel" recompilation order (same as above, but group files
  which can be built in parallel)
  3. Change analysis (if I change file x, which others need recompiling)
  4. Dependency changes (if I add a dependency between file x and file y, what
  are the effects)

*/

#include <boost/config.hpp> // put this first to suppress some VC++ warnings

#include <iostream>
#include <iterator>
#include <algorithm>
#include <time.h>

#include <boost/utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/visitors.hpp>

using namespace std;
using namespace boost;

/*

struct print_visitor : public bfs_visitor<> {
  template <class Vertex, class Graph>
  void discover_vertex(Vertex v, Graph&) {
    cout << name[v] << " ";
  }
};

*/

struct cycle_detector : public dfs_visitor<>
{
  cycle_detector(bool& has_cycle)
    : m_has_cycle(has_cycle) { }

  template <class Edge, class Graph>
  void back_edge(Edge, Graph&) { m_has_cycle = true; }
protected:
  bool& m_has_cycle;
};


/*******************************************************************************
* nemesis. an experimental finite element code.                                *
* Copyright (C) 2004-2011 F.E.Karaoulanis [http://www.nemesis-project.org]     *
*                                                                              *
* This program is free software; you can redistribute it and/or modify         *
* it under the terms of the GNU General Public License version 3, as           *
* published by the Free Software Foundation.                                   *
*                                                                              *
* This program is distributed in the hope that it will be useful,              *
* but WITHOUT ANY WARRANTY; without even the implied warranty of               *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the                *
* GNU General Public License for more details.                                 *
*                                                                              *
* You should have received a copy of the GNU General Public License            *
* along with this program.  If not, see < http://www.gnu.org/licenses/>.       *
*******************************************************************************/

// *****************************************************************************
// $LastChangedDate$
// $LastChangedRevision$
// $LastChangedBy$
// $HeadURL$
// Author(s): F.E. Karaoulanis (fkar@nemesis-project.org)
// *****************************************************************************

#include "reorderer/reverse_sloan.h"
#include <boost/graph/sloan_ordering.hpp>
#include <stdio.h>
#include "analysis/analysis.h"
#include "model/model.h"

ReverseSloan::ReverseSloan(double w1, double w2)
    : weight1(w1),
      weight2(w2) {
  myTag = TAG_REORDERER_REVERSE_SLOAN;
}

ReverseSloan::~ReverseSloan() {
}

int ReverseSloan::get_perm(std::vector<int>& perm) {
  // Create the Graph and additional vectors
  UndirectedGraph G(pA->get_model()->get_num_eqns());
  pA->get_model()->get_undirected_graph(&G);
  property_map<UndirectedGraph, vertex_index_t>::type
    index_map = get(vertex_index, G);
  std::vector<int> inv_perm(num_vertices(G));
  perm.resize(num_vertices(G));

  // Find old bandwidth, profile and wavefronts
  int oldBandwidth = bandwidth(G);
  int oldProfile = profile(G);
  /// @todo check for warnings in wavefront()
  // double oldMaxWavefront = max_wavefront(G);
  // double oldAverWavefront = aver_wavefront(G);
  // double oldRmsWavefront = rms_wavefront(G);

  // Call Sloan algorithm
  sloan_ordering(G, inv_perm.rbegin(), get(vertex_color, G),
    make_degree_map(G), get(vertex_priority, G), weight1, weight2);
  for (unsigned k = 0; k != inv_perm.size(); k++)
    perm[index_map[inv_perm[k]]]=k;

  // Find new bandwidth and profile
  int newBandwidth = bandwidth(G, make_iterator_property_map(&perm[0],
                               index_map, perm[0]));
  int newProfile = profile(G, make_iterator_property_map(&perm[0], index_map));
  // double newMaxWavefront = max_wavefront(G,
  //                          make_iterator_property_map(&perm[0], index_map));
  // double newAverWavefront = aver_wavefront(G,
  //                          make_iterator_property_map(&perm[0], index_map));
  // double newRmsWavefront = rms_wavefront(G,
  //                          make_iterator_property_map(&perm[0], index_map));

  // Print optimized sizes
  printf("reo: Optimized (original) bandwidth : %d (%d)\n",
         newBandwidth, oldBandwidth);
  printf("reo: Optimized (original) profile   : %d (%d)\n",
         newProfile, oldProfile);
  // cout   << "rSloan: Optimized (original) maximum wavefront   : "
  //        <<newMaxWavefront   << " (" << oldMaxWavefront   << ")" << endl;
  // cout   << "rSloan: Optimized (original) average wavefront   : "
  //        <<newAverWavefront  << " (" << oldAverWavefront  << ")" << endl;
  // cout   << "rSloan: Optimized (original) rms wavefront       : "
  //        << newRmsWavefront  << " (" << oldRmsWavefront   << ")" << endl;

  // Check if optimization is needed
  if (newBandwidth > oldBandwidth) return -1;
  return 1;
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#define NT2_UNIT_MODULE "nt2 polynomials toolbox - tchebeval/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// Test behavior of polynomials components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 06/03/2011
/// modified by jt the 06/03/2011
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/no_ulp_tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/constant/infinites.hpp>
#include <nt2/include/functions/ulpdist.hpp>
#include <nt2/toolbox/polynomials/include/tchebeval.hpp>
// specific includes for arity 2 tests


#include <string>
#include <vector>
#include <iostream>
#include <algorithm>
#include <boost/unordered_map.hpp>
#include <boost/lexical_cast.hpp>

// Perfect hash

struct ihash{
    std::size_t operator()(std::size_t i) const{
        return i;
    }
};
typedef boost::unordered_map<int, const char*,  ihash > MapT;

int main()
{

//////////////////
	MapT map1(53);
	const char* s1 = "xxxxx";
	const char* s2 = "yyyyy";
	map1[25] = s1; map1[10] = s2;
	for(MapT::iterator v = map1.begin(), end = map1.end(); v != end; ++v) {
	  std::cout <<"bucket #=" << v->first<<"; bucket_size="<< map1.bucket_size(v->first);
	  if(v->second) {
		std::cout <<"; value="<< v->second;
	  }
		 std::cout << std::endl;
	}

  return 0;
}


#include "catch.hpp"

#include <blitzml/base/common.h>
#include <blitzml/dataset/sparse_dataset.h>
#include <blitzml/dataset/dense_dataset.h>
#include <blitzml/dataset/sparse_binary_dataset.h>

using std::vector;

using namespace BlitzML;

TEST_CASE( "test_sparse_dataset", "[dataset]" ) {
  vector<index_t> indices;
  vector<float> data;
  vector<size_t> indptr;
  indptr.push_back(0);

  // Column 1
  indptr.push_back(0);

  // Column 2
  indices.push_back(2); data.push_back(2.5);
  indices.push_back(4); data.push_back(-0.5);
  indptr.push_back(2);

  // Column 3
  indptr.push_back(2);

  // Column 4
  indices.push_back(0); data.push_back(-0.5);
  indices.push_back(4); data.push_back(0.3);
  indptr.push_back(4);

  // Column 5
  indptr.push_back(4);

  // Column 6
  indices.push_back(1); data.push_back(2.0);
  indices.push_back(2); data.push_back(2.0);
  indices.push_back(3); data.push_back(2.0);
  indptr.push_back(7);

  vector<value_t> b(5, 0.);
  b[0] = 1.0;
  b[2] = 2.0;
  b[4] = -0.5;

  SparseDataset<float> ds(&indices[0], &indptr[0], &data[0],
                          5, 6, data.size(), &b[0], 5);

  vector<value_t> v(5, 0.);
  v[0] = 2.0;
  v[3] = 1.0;
  v[4] = 0.2;
  value_t val = ds.column(3)->inner_product(v);
  REQUIRE( val == Approx( -0.94 ) );

  val = ds.column(0)->inner_product(v);
  REQUIRE( val == 0. );

  val = ds.column(1)->inner_product(v);
  REQUIRE( val == Approx( -0.1 ) );

  REQUIRE( ds.any_columns_overlapping_in_submatrix(0, 4) == true );
  REQUIRE( ds.any_columns_overlapping_in_submatrix(2, 4) == false );
  REQUIRE( ds.any_columns_overlapping_in_submatrix(0, 3) == false );

  REQUIRE( ds.b_value(0) == 1.0 );
  REQUIRE( ds.b_value(1) == 0.0 );
  REQUIRE( ds.b_value(4) == -0.5 );

  REQUIRE( ds.num_rows() == 5 );
  REQUIRE( ds.num_cols() == 6 );
  REQUIRE( ds.nnz() == 7 );
}

TEST_CASE( "test_sparse_dataset_types", "[dataset]" ) {
  vector<double> data_double(2, 1.0);
  vector<float> data_float(2, 1.0);
  vector<int> data_int(2, 1);
  bool data_bool[2] = {true, true};

  vector<index_t> indices;
  indices.push_back(0); indices.push_back(1);

  vector<size_t> indptr;
  indptr.push_back(0); indptr.push_back(2);

  vector<value_t> b(2, 0.);

  SparseDataset<double> ds_double(&indices[0], &indptr[0], &data_double[0],
                                  2, 1, 2, &b[0], 2);
  SparseDataset<float> ds_float(&indices[0], &indptr[0], &data_float[0],
                                  2, 1, 2, &b[0], 2);
  SparseDataset<int> ds_int(&indices[0], &indptr[0], &data_int[0],
                                  2, 1, 2, &b[0], 2);
  SparseDataset<bool> ds_bool(&indices[0], &indptr[0], &data_bool[0],
                                  2, 1, 2, &b[0], 2);

  vector<value_t> v;
  v.push_back(-2.0);
  v.push_back(2.6);

  REQUIRE( ds_double.column(0)->inner_product(v) == Approx( 0.6 ) );
  REQUIRE( ds_float.column(0)->inner_product(v) == Approx( 0.6 ) );
  REQUIRE( ds_int.column(0)->inner_product(v) == Approx( 0.6 ) );
  REQUIRE( ds_bool.column(0)->inner_product(v) == Approx( 0.6 ) );
}

TEST_CASE( "test_dense_dataset", "[datset]" ) {
  double data[12] = {1., 2., 3., 4., 5., 6., 7., 8., 9., 10., 11., 12.};
  value_t b[4] = {1., 1., -3., 1.};
  DenseDataset<double> ds(data, 4, 3, b, 4);

  value_t val = ds.column(0)->l2_norm_sq();
  REQUIRE( ds.column(0)->l2_norm_sq() == Approx( 30. ) );

  vector<value_t> vec(4, 0.0);
  vec[3] = -1.0;
  vec[1] = -0.5;
  REQUIRE( ds.column(2)->inner_product(vec) == Approx( -17. ) );

  REQUIRE( ds.any_columns_overlapping_in_submatrix(0, 2) == true );
  REQUIRE( ds.any_columns_overlapping_in_submatrix(0, 3) == true );
  REQUIRE( ds.any_columns_overlapping_in_submatrix(0, 1) == false );
  REQUIRE( ds.any_columns_overlapping_in_submatrix(1, 2) == false );

  REQUIRE( ds.b_value(2) == -3. );
  REQUIRE( ds.b_value(0) == 1. );
  REQUIRE( ds.b_value(3) == 1. );

  REQUIRE( ds.num_rows() == 4 );
  REQUIRE( ds.num_cols() == 3 );
  REQUIRE( ds.nnz() == 12 );
}

TEST_CASE( "test_dense_dataset_types", "[dataset]" ) {
  double data_double[2] = {1., 1.};
  float data_float[2] = {1., 1.};
  int data_int[2] = {1., 1.};
  bool data_bool[2] = {true, true};

  DenseDataset<double> ds_double(data_double, 2, 1, NULL, 0);
  DenseDataset<float> ds_float(data_float, 2, 1, NULL, 0);
  DenseDataset<int> ds_int(data_int, 2, 1, NULL, 0);
  DenseDataset<bool> ds_bool(data_bool, 2, 1, NULL, 0);

  vector<value_t> vec;
  vec.push_back(-2.6); vec.push_back(0.3);
  REQUIRE( ds_double.column(0)->inner_product(vec) == Approx(-2.3) );
  REQUIRE( ds_float.column(0)->inner_product(vec) == Approx(-2.3) );
  REQUIRE( ds_int.column(0)->inner_product(vec) == Approx(-2.3) );
  REQUIRE( ds_bool.column(0)->inner_product(vec) == Approx(-2.3) );
}

TEST_CASE( "test_binary_dataset", "[datset]" ) {
  index_t indices[8] = {0, 2, 3, 0, 3, 1, 2, 4};
  size_t indptr[5] = {0, 0, 3, 5, 8};
  value_t b[5] = {0., 1.0, 0., -2.5, 3.0};
  SparseBinaryDataset ds(indices, indptr, 5, 4, 8, b, 5);

  vector<value_t> vec;
  vec.push_back(0.1); vec.push_back(0.2); vec.push_back(-0.3);
  vec.push_back(-0.4); vec.push_back(0.5);

  REQUIRE( ds.column(0)->inner_product(vec) == 0. );
  REQUIRE( ds.column(3)->inner_product(vec) == Approx(0.4) );

  REQUIRE( ds.any_columns_overlapping_in_submatrix(2, 4) == false );
  REQUIRE( ds.any_columns_overlapping_in_submatrix(3, 4) == false );
  REQUIRE( ds.any_columns_overlapping_in_submatrix(1, 4) == true );
  REQUIRE( ds.any_columns_overlapping_in_submatrix(0, 2) == false );
  REQUIRE( ds.any_columns_overlapping_in_submatrix(0, 3) == true );

  REQUIRE( ds.b_value(0) == 0. );
  REQUIRE( ds.b_value(4) == 3. );
  REQUIRE( ds.b_value(3) == -2.5 );

  REQUIRE( ds.num_rows() == 5 );
  REQUIRE( ds.num_cols() == 4 );
  REQUIRE( ds.nnz() == 8 );
}

TEST_CASE( "test_dataset_general", "[dataset]" ) {
  double data[12] = {1., -0.3, 0., 2.1, 5., 1.0, 7., 8., 9., 0., -1., 12.};
  value_t b[4] = {1., 1., -3., 1.};
  DenseDataset<double> ds(data, 4, 3, b, 4);

  vector<value_t> vec(4, 0.);
  vec[0] = -1.1;
  vec[1] = -2.1;
  vec[2] = 1.8;
  vec[3] = 10.1;

  vector<value_t> result(2, 0.);

  ds.contiguous_submatrix_multiply(vec, &result[0], 1, 3);
  REQUIRE( result[0] == Approx(85.8) );
  REQUIRE( result[1] == Approx(109.5) );
}


///////////////////////////////////////////////////////////////////////////////
// data_field.hpp                                                            //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_TOOLS_DETAIL_SERIES_DATA_FIELD_HPP_ER_2010
#define BOOST_STATISTICS_TOOLS_DETAIL_SERIES_DATA_FIELD_HPP_ER_2010
#include <iostream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace series{

    // This data_field is meant to keep the result of extracting an
    // accumulator_set<>'s feature specified by Tag, and evaluating to an
    // object of type T.
    template<typename Tag,typename T>
    struct data_field
    {
        typedef T value_type;

        data_field(){}

        value_type value;

        private:
        friend class boost::serialization::access;
        template<class Archive>
        void serialize(Archive & ar, const unsigned int version)
        {
            ar & this->value;
        }

    };

	template<typename Tag,typename T>
	data_field<Tag,T>&
    get_field(data_field<Tag,T>& field){ return field; }

    template<typename Tag,typename T>
	std::ostream&
    operator<<(std::ostream& os,const data_field<Tag,T>& field)
    {
        return (os << field.value);
    }

}// series
}// detail
}// statistics
}// boost

#endif


#ifndef INCLUDE_OKVIS_CAMERAS_FOVDISTORTION_HPP_
#define INCLUDE_OKVIS_CAMERAS_FOVDISTORTION_HPP_

#include <Eigen/Core>
#include <memory>
#include "okvis/cameras/DistortionBase.hpp"

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief cameras Namespace for camera-related functionality.
namespace cameras {

class FovDistortion : public DistortionBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /// \brief The default constructor with all zero ki
  inline FovDistortion();

  /// \brief Constructor initialising ki
  /// @param[in] w half FOV

  inline FovDistortion(double w);

  //////////////////////////////////////////////////////////////
  /// \name Methods related to generic parameters
  /// @{

  /// \brief set the generic parameters
  /// @param[in] parameters Parameter vector -- length must correspond
  /// numDistortionIntrinsics().
  /// @return    True if the requirements were followed.
  inline bool setParameters(const Eigen::VectorXd& parameters);

  /// \brief Obtain the generic parameters.
  inline bool getParameters(Eigen::VectorXd& parameters) const {
    parameters = parameters_;
    return true;
  }

  /// \brief The class type.
  inline std::string type() const { return "FovDistortion"; }

  /// \brief Number of distortion parameters
  inline int numDistortionIntrinsics() const { return NumDistortionIntrinsics; }

  static const int NumDistortionIntrinsics =
      1;  ///< The Number of distortion parameters.
  /// @}

  /// \brief Unit test support -- create a test distortion object
  static std::shared_ptr<DistortionBase> createTestObject() {
    return std::shared_ptr<DistortionBase>(new FovDistortion(1.0));
  }
  /// \brief Unit test support -- create a test distortion object
  static FovDistortion testObject() { return FovDistortion(1.0); }

  //////////////////////////////////////////////////////////////
  /// \name Distortion functions
  /// @{

  /// \brief Distortion only
  /// @param[in]  pointUndistorted The undistorted normalised (!) image point.
  /// @param[out] pointDistorted   The distorted normalised (!) image point.
  /// @return     True on success (no singularity)
  inline bool distort(const Eigen::Vector2d& pointUndistorted,
                      Eigen::Vector2d* pointDistorted) const;

  /// \brief Distortion and Jacobians.
  /// @param[in]  pointUndistorted  The undistorted normalised (!) image point.
  /// @param[out] pointDistorted    The distorted normalised (!) image point.
  /// @param[out] pointJacobian     The Jacobian w.r.t. changes on the image
  /// point.
  /// @param[out] parameterJacobian The Jacobian w.r.t. changes on the
  /// intrinsics vector.
  /// @return     True on success (no singularity)
  inline bool distort(const Eigen::Vector2d& pointUndistorted,
                      Eigen::Vector2d* pointDistorted,
                      Eigen::Matrix2d* pointJacobian,
                      Eigen::Matrix2Xd* parameterJacobian = NULL) const;

  /// \brief Distortion and Jacobians using external distortion intrinsics
  /// parameters.
  /// @param[in]  pointUndistorted  The undistorted normalised (!) image point.
  /// @param[in]  parameters        The distortion intrinsics vector.
  /// @param[out] pointDistorted    The distorted normalised (!) image point.
  /// @param[out] pointJacobian     The Jacobian w.r.t. changes on the image
  /// point.
  /// @param[out] parameterJacobian The Jacobian w.r.t. changes on the
  /// intrinsics vector.
  /// @return     True on success (no singularity)
  inline bool distortWithExternalParameters(
      const Eigen::Vector2d& pointUndistorted,
      const Eigen::VectorXd& parameters, Eigen::Vector2d* pointDistorted,
      Eigen::Matrix2d* pointJacobian = NULL,
      Eigen::Matrix2Xd* parameterJacobian = NULL) const;
  /// @}

  //////////////////////////////////////////////////////////////
  /// \name Undistortion functions
  /// @{

  /// \brief Undistortion only
  /// @param[in]  pointDistorted   The distorted normalised (!) image point.
  /// @param[out] pointUndistorted The undistorted normalised (!) image point.
  /// @return     True on success (no singularity)
  inline bool undistort(const Eigen::Vector2d& pointDistorted,
                        Eigen::Vector2d* pointUndistorted) const;

  /// \brief Undistortion only
  /// @param[in]  pointDistorted   The distorted normalised (!) image point.
  /// @param[out] pointUndistorted The undistorted normalised (!) image point.
  /// @param[out] pointJacobian    The Jacobian w.r.t. changes on the image
  /// point.
  /// @return     True on success (no singularity)
  inline bool undistort(const Eigen::Vector2d& pointDistorted,
                        Eigen::Vector2d* pointUndistorted,
                        Eigen::Matrix2d* pointJacobian) const;
  /// @}

 protected:
  Eigen::Matrix<double, NumDistortionIntrinsics, 1>
      parameters_;  ///< all distortion parameters

  double w_;
  static constexpr double kMaxValidAngle = (89.0 * M_PI / 180.0);
};

}  // namespace cameras
}  // namespace okvis

#include "implementation/FovDistortion.hpp"

#endif /* INCLUDE_OKVIS_CAMERAS_FOVDISTORTION_HPP_ */


/*
 *  Created by Andrea Bedini on 24/Nov/2011.
 *  Copyright 2011-2014 Andrea Bedini <andrea@andreabedini.com>
 *
 *  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 heuristics_hpp
#define heuristics_hpp

#include <boost/range/algorithm.hpp>

namespace heuristics {
  using namespace boost;

  template<class vertex, class graph>
  void eliminate_vertex(vertex v, graph& g) {
    for (auto a : as_range(adjacent_vertices(v, g))) {
      for (auto b : as_range(adjacent_vertices(v, g))) {
        if (a != b and not edge(a, b, g).second) {
          add_edge(a, b, g);
        }
      }
    }
    clear_vertex(v, g);
    remove_vertex(v, g);
  }

  template<class graph, class vertex>
  unsigned int num_non_adjacent_neighbors(vertex v, graph const& g)
  {
    unsigned int n = 0;
    for (auto u : as_range(adjacent_vertices(v, g))) {
      auto adj = adjacent_vertices(u, g);
      for (auto z : as_range(adjacent_vertices(v, g))) {
        if (boost::find(adj, z) != end(adj))
          n ++;
      }
    }
    return n;
  }

  template<class graph, class outputiterator>
  void greedy_degree_order(graph g, outputiterator out) {
    using vertex = typename graph::vertex_descriptor;
    while (num_vertices(g) > 0) {
      auto v = *min_element(vertices(g), [&](vertex v, vertex u) {
        return degree(v, g) < degree(u, g);
      });
      *out++ = get(vertex_index, g, v);
      eliminate_vertex(v, g);
    }
  }

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

  template<class graph, class outputiterator>
  void greedy_fillin_order(graph g, outputiterator out) {
    using vertex = typename graph::vertex_descriptor;
    while (num_vertices(g) > 0) {
      auto v = *min_element(vertices(g), [&](vertex v, vertex u) {
        return num_non_adjacent_neighbors(v, g) < num_non_adjacent_neighbors(u, g);
      });
      *out++ = get(vertex_index, g, v);
      eliminate_vertex(v, g);
    }
  }

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

  template<class graph, class outputiterator>
  void greedy_local_degree_order(graph g, outputiterator out) {
    using vertex = typename graph::vertex_descriptor;
    auto compare = [&](vertex v, vertex u) {
      return degree(v, g) < degree(u, g);
    };
    vertex current = *min_element(vertices(g), compare);
    while (num_vertices(g) > 0) {
      *out++ = get(vertex_index, g, current);
      vertex next = *min_element(vertices(g), compare);
      eliminate_vertex(current, g);
      current = next;
    }
  }

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

  template<class graph, class outputiterator>
  void greedy_local_fillin_order(graph g, outputiterator out) {
    using vertex = typename graph::vertex_descriptor;
    auto compare =  [&](vertex v, vertex u) {
      return num_non_adjacent_neighbors(v, g) < num_non_adjacent_neighbors(u, g);
    };
    vertex current = *min_element(vertices(g), compare);
    while (num_vertices(g) > 0) {
      *out++ = get(vertex_index, g, current);
      vertex next = *min_element(adjacent_vertices(current, g), compare);
      eliminate_vertex(current, g);
      current = next;
    }
  }
}

#endif


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// Distributed under the Boost
// Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
// copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_DYN_LINK 1
#define BOOST_TEST_MODULE "Boost Bloom Filter" 1

#include <boost/bloom_filter/counting_bloom_filter.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <boost/bloom_filter/detail/exceptions.hpp>

#include <boost/cstdint.hpp>

using boost::bloom_filters::counting_bloom_filter;
using boost::bloom_filters::detail::bin_underflow_exception;
using boost::bloom_filters::detail::bin_overflow_exception;
using boost::bloom_filters::boost_hash;

BOOST_AUTO_TEST_CASE(allBitsPerBinCompile)
{
  counting_bloom_filter<size_t, 2, 1> bloom1;
  counting_bloom_filter<size_t, 2, 2> bloom2;
  counting_bloom_filter<size_t, 2, 4> bloom4;
  counting_bloom_filter<size_t, 2, 8> bloom8;
  counting_bloom_filter<size_t, 2, 16> bloom16;
  counting_bloom_filter<size_t, 2, 32> bloom32;
}

BOOST_AUTO_TEST_CASE(allReasonableBlockTypesCompile)
{
  typedef boost::mpl::vector<boost_hash<int, 0> > default_hash;

  counting_bloom_filter<int, 2, 2, default_hash, unsigned char> a;
  counting_bloom_filter<int, 2, 2, default_hash, unsigned short> b;
  counting_bloom_filter<int, 2, 2, default_hash, unsigned int> c;
  counting_bloom_filter<int, 2, 2, default_hash, unsigned long> d;
  counting_bloom_filter<int, 2, 2, default_hash, size_t> e;

  counting_bloom_filter<int, 2, 2, default_hash, uint8_t> aa;
  counting_bloom_filter<int, 2, 2, default_hash, uint16_t> ab;
  counting_bloom_filter<int, 2, 2, default_hash, uint32_t> ac;
  counting_bloom_filter<int, 2, 2, default_hash, uintmax_t> ad;
}

BOOST_AUTO_TEST_CASE(defaultConstructor) {
  typedef boost::mpl::vector<
    boost_hash<int, 13>,
    boost_hash<int, 17>,
    boost_hash<int, 19> > BoostHashFunctions;

  counting_bloom_filter<int, 2> bloom1;
  counting_bloom_filter<int, 2, 1> bloom2;
  counting_bloom_filter<int, 2, 2, BoostHashFunctions> bloom3;
  counting_bloom_filter<int, 2, 4,
			BoostHashFunctions, unsigned char> bloom4;
}

BOOST_AUTO_TEST_CASE(countSingle)
{
  counting_bloom_filter<int, 2> bloom;

  bloom.insert(1);
  BOOST_CHECK_EQUAL(bloom.count(), 1ul);
  bloom.remove(1);
  BOOST_CHECK_EQUAL(bloom.count(), 0ul);
}

#include <iostream>

BOOST_AUTO_TEST_CASE(countMulti)
{
  counting_bloom_filter<int, 100> bloom_default;
  counting_bloom_filter<int, 100, 1> bloom1;
  counting_bloom_filter<int, 100, 2> bloom2;
  counting_bloom_filter<int, 100, 8> bloom8;
  counting_bloom_filter<int, 100, 16> bloom16;
  counting_bloom_filter<int, 100, 32> bloom32;

  for (size_t i = 0; i < 100; ++i) {
    bloom_default.insert(i);
    bloom1.insert(i);
    bloom2.insert(i);
    bloom8.insert(i);
    bloom16.insert(i);
    bloom32.insert(i);
  }

  BOOST_CHECK_EQUAL(bloom_default.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom1.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom2.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom8.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom16.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom32.count(), 100ul);

  for (size_t i = 0; i < 100; ++i) {
    bloom_default.insert(i);
    bloom2.insert(i);
    bloom8.insert(i);
    bloom16.insert(i);
    bloom32.insert(i);
  }

  BOOST_CHECK_EQUAL(bloom_default.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom2.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom8.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom16.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom32.count(), 100ul);

  for (size_t i = 0; i < 100; ++i) {
    bloom_default.remove(i);
    bloom2.remove(i);
    bloom8.remove(i);
    bloom16.remove(i);
    bloom32.remove(i);
  }

  BOOST_CHECK_EQUAL(bloom_default.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom2.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom8.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom16.count(), 100ul);
  BOOST_CHECK_EQUAL(bloom32.count(), 100ul);

  for (size_t i = 0; i < 100; ++i) {
    bloom_default.remove(i);
    bloom1.remove(i);
    bloom2.remove(i);
    bloom8.remove(i);
    bloom16.remove(i);
    bloom32.remove(i);
  }

  BOOST_CHECK_EQUAL(bloom_default.count(), 0ul);
  BOOST_CHECK_EQUAL(bloom1.count(), 0ul);
  BOOST_CHECK_EQUAL(bloom2.count(), 0ul);
  BOOST_CHECK_EQUAL(bloom8.count(), 0ul);
  BOOST_CHECK_EQUAL(bloom16.count(), 0ul);
  BOOST_CHECK_EQUAL(bloom32.count(), 0ul);
}

BOOST_AUTO_TEST_CASE(rangeConstructor) {
  int elems[5] = {1,2,3,4,5};
  counting_bloom_filter<int, 5> bloom(elems, elems+5);

  BOOST_CHECK_EQUAL(bloom.count(), 5ul);
}

#ifndef BOOST_NO_0X_HDR_INITIALIZER_LIST
BOOST_AUTO_TEST_CASE(initListConstructor) {
  counting_bloom_filter<int, 5> bloom = {1,2,3,4,5};

  BOOST_CHECK_EQUAL(bloom.count(), 5ul);
}
#endif

BOOST_AUTO_TEST_CASE(copyConstructor) {
  int elems[5] = {1,2,3,4,5};
  counting_bloom_filter<int, 5> bloom1(elems, elems+5);
  counting_bloom_filter<int, 5> bloom2(bloom1);

  BOOST_CHECK_EQUAL(bloom1.count(), bloom2.count());
}

BOOST_AUTO_TEST_CASE(assignment)
{
  counting_bloom_filter<int, 200> bloom1;
  counting_bloom_filter<int, 200> bloom2;

  for (size_t i = 0; i < 200; ++i) {
    bloom1.insert(i);
    BOOST_CHECK_EQUAL(bloom1.probably_contains(i), true);
  }

  bloom2 = bloom1;

  for (size_t i = 0; i < 200; ++i) {
    BOOST_CHECK_EQUAL(bloom2.probably_contains(i), true);
  }
}

BOOST_AUTO_TEST_CASE(bit_capacity) {
  counting_bloom_filter<size_t, 8> bloom_8;
  counting_bloom_filter<size_t, 256> bloom_256;
  counting_bloom_filter<size_t, 2048> bloom_2048;

  BOOST_CHECK_EQUAL(bloom_8.bit_capacity(),
		    8ul * bloom_8.bits_per_bin());
  BOOST_CHECK_EQUAL(bloom_256.bit_capacity(),
		    256ul * bloom_256.bits_per_bin());
  BOOST_CHECK_EQUAL(bloom_2048.bit_capacity(),
		    2048ul * bloom_2048.bits_per_bin());
}

BOOST_AUTO_TEST_CASE(empty) {
  counting_bloom_filter<size_t, 2> bloom;

  BOOST_CHECK_EQUAL(bloom.empty(), true);
  bloom.insert(1);
  BOOST_CHECK_EQUAL(bloom.empty(), false);
  bloom.clear();
  BOOST_CHECK_EQUAL(bloom.empty(), true);
}

BOOST_AUTO_TEST_CASE(numHashFunctions) {
  counting_bloom_filter<size_t, 1> bloom_1;
  counting_bloom_filter<size_t, 1, 1, boost::mpl::vector<
    boost_hash<size_t, 1>,
    boost_hash<size_t, 2> > > bloom_2;
  counting_bloom_filter<size_t, 1, 1, boost::mpl::vector<
    boost_hash<size_t, 1>,
    boost_hash<size_t, 2>,
    boost_hash<size_t, 3>,
    boost_hash<size_t, 4>,
    boost_hash<size_t, 5>,
    boost_hash<size_t, 6>,
    boost_hash<size_t, 7> > > bloom_7;

  BOOST_CHECK_EQUAL(bloom_1.num_hash_functions(), 1ul);
  BOOST_CHECK_EQUAL(bloom_2.num_hash_functions(), 2ul);
  BOOST_CHECK_EQUAL(bloom_7.num_hash_functions(), 7ul);
}

BOOST_AUTO_TEST_CASE(falsePositiveRate) {
  counting_bloom_filter<size_t, 64> bloom;

  BOOST_CHECK_EQUAL(bloom.false_positive_rate(), 0.0);

  bloom.insert(1);
  BOOST_CHECK_CLOSE(bloom.false_positive_rate(), 0.015504, .01);

  bloom.insert(2);
  BOOST_CHECK_CLOSE(bloom.false_positive_rate(), 0.030768, .01);

  bloom.insert(3);
  BOOST_CHECK_CLOSE(bloom.false_positive_rate(), 0.045794, .01);

  bloom.insert(4);
  BOOST_CHECK_CLOSE(bloom.false_positive_rate(), 0.060588, .01);

  bloom.insert(5);
  BOOST_CHECK_CLOSE(bloom.false_positive_rate(), 0.075151, .01);

  bloom.insert(6);
  BOOST_CHECK_CLOSE(bloom.false_positive_rate(), 0.089491, .01);

  for (size_t i = 7; i < 128; ++i)
    bloom.insert(i);

  BOOST_CHECK_GE(bloom.false_positive_rate(), 0.6);
  BOOST_CHECK_LE(bloom.false_positive_rate(), 1.0);
}

BOOST_AUTO_TEST_CASE(probably_contains) {
  counting_bloom_filter<size_t, 2> bloom;

  bloom.insert(1);
  BOOST_CHECK_EQUAL(bloom.probably_contains(1), true);
  BOOST_CHECK_EQUAL(bloom.count(), 1ul);
}

BOOST_AUTO_TEST_CASE(doesNotContain) {
  counting_bloom_filter<size_t, 2> bloom;

  BOOST_CHECK_EQUAL(bloom.probably_contains(1), false);
}

BOOST_AUTO_TEST_CASE(insertNoFalseNegatives) {
  counting_bloom_filter<size_t, 100> bloom;

  for (size_t i = 0; i < 100; ++i) {
    bloom.insert(i);
    BOOST_CHECK_EQUAL(bloom.probably_contains(i), true);
  }
}

BOOST_AUTO_TEST_CASE(insertOverflowExceptionThrown) {
  counting_bloom_filter<size_t, 2, 1> bloom;
  bool exception_occurred = false;

  bloom.insert(1);
  BOOST_CHECK_EQUAL(bloom.probably_contains(1), true);

  try {
    bloom.insert(1);
  }

  catch (bin_overflow_exception e) {
    exception_occurred = true;
  }

  BOOST_CHECK_EQUAL(exception_occurred, true);
  BOOST_CHECK_EQUAL(bloom.probably_contains(1), true);
}

BOOST_AUTO_TEST_CASE(removeUnderflowExceptionThrown) {
  counting_bloom_filter<size_t, 2, 1> bloom;
  bool exception_occurred = false;

  try {
    bloom.remove(1);
  }

  catch (bin_underflow_exception e) {
    exception_occurred = true;
  }

  BOOST_CHECK_EQUAL(exception_occurred, true);
  BOOST_CHECK_EQUAL(bloom.probably_contains(1), false);
}

BOOST_AUTO_TEST_CASE(insertOverflowException4bit)
{
  counting_bloom_filter<size_t, 2> bloom;
  bool exception_occurred = false;

  try {
    for (size_t i = 0; i < (1ul << bloom.bits_per_bin()); ++i)
      bloom.insert(1);
  }

  catch (bin_overflow_exception e) {
    exception_occurred = true;
  }

  BOOST_CHECK_EQUAL(exception_occurred, true);
  BOOST_CHECK_EQUAL(bloom.probably_contains(1), true);
}

BOOST_AUTO_TEST_CASE(rangeInsert) {
  int elems[5] = {1,2,3,4,5};
  counting_bloom_filter<size_t, 5> bloom;

  bloom.insert(elems, elems+5);
  BOOST_CHECK_EQUAL(bloom.count(), 5ul);
}

BOOST_AUTO_TEST_CASE(_remove) {
  counting_bloom_filter<size_t, 1> bloom;

  bloom.insert(1);
  bloom.remove(1);
  BOOST_CHECK_EQUAL(bloom.count(), 0ul);
}

BOOST_AUTO_TEST_CASE(removeMulti) {
  counting_bloom_filter<size_t, 100> bloom;

  for (size_t i = 0; i < 100; ++i) {
    bloom.insert(i);
    bloom.remove(i);
    BOOST_CHECK_EQUAL(bloom.count(), 0ul);
  }
}

BOOST_AUTO_TEST_CASE(rangeRemove) {
  size_t arr[] = {1,2,3,4,5};
  counting_bloom_filter<size_t, 5> bloom;

  bloom.insert(arr, arr+5);
  bloom.remove(arr, arr+5);
  BOOST_CHECK_EQUAL(bloom.count(), 0ul);
}

BOOST_AUTO_TEST_CASE(clear) {
  counting_bloom_filter<size_t, 1000> bloom;

  for (size_t i = 0; i < 1000; ++i)
    bloom.insert(i);

  bloom.clear();
  BOOST_CHECK_EQUAL(bloom.probably_contains(1), false);
  BOOST_CHECK_EQUAL(bloom.count(), 0ul);
}

struct SwapFixture {
  SwapFixture()
  {
    for (size_t i = 0; i < 5; ++i)
      elems[i] = i+1;

    bloom1.insert(elems, elems+2);
    bloom2.insert(elems+2, elems+5);
  }

  size_t elems[5];
  counting_bloom_filter<size_t, 5> bloom1;
  counting_bloom_filter<size_t, 5> bloom2;
};

BOOST_FIXTURE_TEST_CASE(memberSwap, SwapFixture) {
  bloom1.swap(bloom2);

  BOOST_CHECK_EQUAL(bloom1.count(), 3ul);
  BOOST_CHECK_EQUAL(bloom2.count(), 2ul);
}

BOOST_FIXTURE_TEST_CASE(globalSwap, SwapFixture) {
  swap(bloom1, bloom2);

  BOOST_CHECK_EQUAL(bloom1.count(), 3ul);
  BOOST_CHECK_EQUAL(bloom2.count(), 2ul);
}

struct PairwiseOpsFixture {

  PairwiseOpsFixture() {}

  counting_bloom_filter<size_t, 300> bloom1;
  counting_bloom_filter<size_t, 300> bloom2;
  counting_bloom_filter<size_t, 300> bloom_result;
};

BOOST_FIXTURE_TEST_CASE(equalityOperator, PairwiseOpsFixture) {
  BOOST_CHECK_EQUAL(bloom1 == bloom2, true);
  bloom1.insert(1);
  BOOST_CHECK_EQUAL(bloom1 == bloom2, false);
  bloom2.insert(1);
  BOOST_CHECK_EQUAL(bloom1 == bloom2, true);
}

BOOST_FIXTURE_TEST_CASE(inequalityOperator, PairwiseOpsFixture) {
  BOOST_CHECK_EQUAL(bloom1 != bloom2, false);
  bloom1.insert(1);
  BOOST_CHECK_EQUAL(bloom1 != bloom2, true);
  bloom2.insert(1);
  BOOST_CHECK_EQUAL(bloom1 != bloom2, false);
}