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


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