#pragma once

#include <vector>

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Axis aligned bounding-box of some type
class AABB {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    AABB() { }

    AABB(const ArrayMax3d& min, const ArrayMax3d& max);

    AABB(const AABB& aabb1, const AABB& aabb2)
        : AABB(aabb1.min.min(aabb2.min), aabb1.max.max(aabb2.max))
    {
    }

    AABB(const AABB& aabb1, const AABB& aabb2, const AABB& aabb3)
        : AABB(
            aabb1.min.min(aabb2.min).min(aabb3.min),
            aabb1.max.max(aabb2.max).max(aabb3.max))
    {
    }

    static bool are_overlapping(const AABB& a, const AABB& b);

    inline const ArrayMax3d& getMin() const { return min; }
    inline const ArrayMax3d& getMax() const { return max; }
    // inline const ArrayMax3d& getHalfExtent() const { return half_extent; }
    // inline const ArrayMax3d& getCenter() const { return center; }

protected:
    ArrayMax3d min;
    ArrayMax3d max;
    // ArrayMax3d half_extent;
    // ArrayMax3d center;
};

/// @brief An entry into the hash grid as a (key, value) pair.
class HashItem {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    int key;   /// @brief The key of the item.
    int id;    /// @brief The value of the item.
    AABB aabb; /// @brief The axis-aligned bounding box of the element

    /// @brief Construct a hash item as a (key, value) pair.
    HashItem(int key, int id, const AABB& aabb)
        : key(key)
        , id(id)
        , aabb(aabb)
    {
    }

    /// @brief Compare HashItems by their keys for sorting.
    bool operator<(const HashItem& other) const
    {
        if (key == other.key) {
            return id < other.id;
        }
        return key < other.key;
    }
};

class HashGrid {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    double cellSize() const { return m_cellSize; }
    const ArrayMax3i& gridSize() const { return m_gridSize; }
    const ArrayMax3d& domainMin() const { return m_domainMin; }
    const ArrayMax3d& domainMax() const { return m_domainMax; }

    void resizeFromBox(
        const ArrayMax3d& min, const ArrayMax3d& max, double cellSize);

    void resize(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0);

    void resize(
        const Eigen::MatrixXd& vertices,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0)
    {
        resize(vertices, vertices, edges, inflation_radius);
    }

    /// @brief Add a vertex as a AABB containing the time swept edge.
    void addVertex(
        const VectorMax3d& vertex_t0,
        const VectorMax3d& vertex_t1,
        const long index,
        const double inflation_radius = 0.0);

    /// @brief Add a vertex.
    void addVertex(
        const VectorMax3d& vertex,
        const long index,
        const double inflation_radius = 0.0)
    {
        addVertex(vertex, vertex, index, inflation_radius);
    }

    /// @brief Add all vertices as AABBs containing the time swept edge.
    void addVertices(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const double inflation_radius = 0.0);

    /// @brief Add all vertices.
    void addVertices(
        const Eigen::MatrixXd& vertices, const double inflation_radius = 0.0)
    {
        addVertices(vertices, vertices, inflation_radius);
    }

    /// @brief Add all selected vertices as AABBs containing the time swept
    /// edge.
    void addSelectVertices(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::VectorXi& vertex_indices,
        const double inflation_radius = 0.0);

    /// @brief Add all selected vertices.
    void addSelectVertices(
        const Eigen::MatrixXd& vertices,
        const Eigen::VectorXi& vertex_indices,
        const double inflation_radius = 0.0)
    {
        addSelectVertices(vertices, vertices, vertex_indices, inflation_radius);
    }

    /// @brief Add all vertices as AABBs containing the time swept edge.
    void addVerticesFromEdges(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0);

    /// @brief Add all vertices as AABBs containing the time swept edge.
    void addVerticesFromEdges(
        const Eigen::MatrixXd& vertices,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0)
    {
        addVerticesFromEdges(vertices, vertices, edges, inflation_radius);
    }

    /// @brief Add an edge as a AABB containing the time swept quad.
    void addEdge(
        const VectorMax3d& edge_vertex0_t0,
        const VectorMax3d& edge_vertex1_t0,
        const VectorMax3d& edge_vertex0_t1,
        const VectorMax3d& edge_vertex1_t1,
        const long index,
        const double inflation_radius = 0.0);

    /// @brief Add an edge as a AABB.
    void addEdge(
        const VectorMax3d& edge_vertex0,
        const VectorMax3d& edge_vertex1,
        const long index,
        const double inflation_radius = 0.0)
    {
        addEdge(
            edge_vertex0, edge_vertex1, edge_vertex0, edge_vertex1, index,
            inflation_radius);
    }

    /// @brief Add all edges as AABBs containing the time swept quad.
    void addEdges(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0);

    /// @brief Add all edges as AABBs.
    void addEdges(
        const Eigen::MatrixXd& vertices,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0)
    {
        addEdges(vertices, vertices, edges, inflation_radius);
    }

    /// @brief Add an edge as a AABB containing the time swept prism.
    void addFace(
        const VectorMax3d& face_vertex0_t0,
        const VectorMax3d& face_vertex1_t0,
        const VectorMax3d& face_vertex2_t0,
        const VectorMax3d& face_vertex0_t1,
        const VectorMax3d& face_vertex1_t1,
        const VectorMax3d& face_vertex2_t1,
        const long index,
        const double inflation_radius = 0.0);

    /// @brief Add an edge as a AABB.
    void addFace(
        const VectorMax3d& face_vertex0,
        const VectorMax3d& face_vertex1,
        const VectorMax3d& face_vertex2,
        const long index,
        const double inflation_radius = 0.0)
    {
        addFace(
            face_vertex0, face_vertex1, face_vertex2, face_vertex0,
            face_vertex1, face_vertex2, index, inflation_radius);
    }

    /// @brief Add all edges as AABBs containing the time swept prism.
    void addFaces(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::MatrixXi& faces,
        const double inflation_radius = 0.0);

    /// @brief Add all edges as AABBs.
    void addFaces(
        const Eigen::MatrixXd& vertices,
        const Eigen::MatrixXi& faces,
        const double inflation_radius = 0.0)
    {
        addFaces(vertices, vertices, faces, inflation_radius);
    }

    /// @brief Compute the candidate edge-vertex candidate collisisons.
    void getVertexEdgePairs(
        const Eigen::MatrixXi& edges,
        std::vector<EdgeVertexCandidate>& ev_candidates,
        const std::function<bool(size_t, size_t)>& can_vertices_collide =
            [](size_t, size_t) { return true; });

    /// @brief Compute the candidate edge-edge candidate collisions.
    void getEdgeEdgePairs(
        const Eigen::MatrixXi& edges,
        std::vector<EdgeEdgeCandidate>& ee_candidates,
        const std::function<bool(size_t, size_t)>& can_vertices_collide =
            [](size_t, size_t) { return true; });

    /// @brief Compute the candidate edge-face candidate intersections.
    void getEdgeFacePairs(
        const Eigen::MatrixXi& edges,
        const Eigen::MatrixXi& faces,
        std::vector<EdgeFaceCandidate>& ef_candidates,
        const std::function<bool(size_t, size_t)>& can_vertices_collide =
            [](size_t, size_t) { return true; });

    /// @brief Compute the candidate edge-edge candidate collisions.
    void getFaceVertexPairs(
        const Eigen::MatrixXi& faces,
        std::vector<FaceVertexCandidate>& fv_candidates,
        const std::function<bool(size_t, size_t)>& can_vertices_collide =
            [](size_t, size_t) { return true; });

    /// @brief Clear the hash grid.
    inline void clear()
    {
        m_vertexItems.clear();
        m_edgeItems.clear();
        m_faceItems.clear();
    }

protected:
    /// @brief Add an AABB of the extents to the hash grid.
    void addElement(
        const AABB& aabb, const int id, std::vector<HashItem>& items) const;

    /// @brief Add a vertex as a AABB containing the time swept edge.
    void addVertex(
        const VectorMax3d& vertex_t0,
        const VectorMax3d& vertex_t1,
        const long index,
        std::vector<HashItem>& vertex_items,
        const double inflation_radius = 0.0) const;

    /// @brief Add an edge as a AABB containing the time swept quad.
    void addEdge(
        const VectorMax3d& edge_vertex0_t0,
        const VectorMax3d& edge_vertex1_t0,
        const VectorMax3d& edge_vertex0_t1,
        const VectorMax3d& edge_vertex1_t1,
        const long index,
        std::vector<HashItem>& edge_items,
        const double inflation_radius = 0.0) const;

    /// @brief Add an edge as a AABB containing the time swept quad.
    void addFace(
        const VectorMax3d& face_vertex0_t0,
        const VectorMax3d& face_vertex1_t0,
        const VectorMax3d& face_vertex2_t0,
        const VectorMax3d& face_vertex0_t1,
        const VectorMax3d& face_vertex1_t1,
        const VectorMax3d& face_vertex2_t1,
        const long index,
        std::vector<HashItem>& face_items,
        const double inflation_radius = 0.0) const;

    /// @brief Create the hash of a cell location.
    inline long hash(int x, int y, int z) const
    {
        assert(x >= 0 && y >= 0 && z >= 0);
        assert(
            x < m_gridSize[0] && y < m_gridSize[1]
            && (m_gridSize.size() == 2 || z < m_gridSize[2]));
        return (z * m_gridSize[1] + y) * m_gridSize[0] + x;
    }

private:
    template <typename T>
    void getPairs(
        const std::function<bool(int, int)>& is_endpoint,
        const std::function<bool(int, int)>& can_collide,
        std::vector<HashItem>& items0,
        std::vector<HashItem>& items1,
        T& candidates);

    template <typename T>
    void getPairs(
        const std::function<bool(int, int)>& is_endpoint,
        const std::function<bool(int, int)>& can_collide,
        std::vector<HashItem>& items,
        T& candidates);

protected:
    double m_cellSize;
    ArrayMax3i m_gridSize;
    ArrayMax3d m_domainMin;
    ArrayMax3d m_domainMax;

    std::vector<HashItem> m_vertexItems;
    std::vector<HashItem> m_edgeItems;
    std::vector<HashItem> m_faceItems;
};

} // namespace ipc


#include "predictor.h"
#include <Eigen/Unsupported/KroneckerProduct>

namespace statiskit
{
    namespace glm
    {
        ScalarPredictor::ScalarPredictor(const MultivariateSampleSpace& explanatory_space)
        { _explanatory_space = explanatory_space.copy().release(); }

        ScalarPredictor::~ScalarPredictor()
        { delete _explanatory_space; }

        ScalarPredictor::ScalarPredictor(const ScalarPredictor& predictor)
        { _explanatory_space = predictor._explanatory_space->copy().release(); }

        const MultivariateSampleSpace* ScalarPredictor::get_explanatory_space() const
        { return _explanatory_space; }

        CompleteScalarPredictor::CompleteScalarPredictor(const MultivariateSampleSpace& explanatory_space) : CompleteScalarPredictor(explanatory_space, explanatory_space.encode())
        {}

        CompleteScalarPredictor::CompleteScalarPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& size) : ScalarPredictor(explanatory_space)
        {
            alpha = 0.;
            _delta = Eigen::VectorXd::Zero(size);
        }

        CompleteScalarPredictor::CompleteScalarPredictor(const CompleteScalarPredictor& predictor) : ScalarPredictor(predictor)
        {
            alpha = predictor.alpha;
            _delta = predictor._delta;
        }

        double CompleteScalarPredictor::operator() (const MultivariateEvent& event) const
        { return alpha + _explanatory_space->encode(event) * _delta; }

    	void CompleteScalarPredictor::set_beta(const Eigen::VectorXd& beta)
    	{
    		if(beta.rows() != (1 + _delta.rows()) )
    		{ throw statiskit::size_error("beta", beta.rows(), 1+_delta.rows()); }
    		alpha = beta(0);
    		_delta = beta.segment(1, _delta.rows());
    	}

    	size_t CompleteScalarPredictor::size() const
    	{ return (1+_delta.size()); }

    	const Eigen::VectorXd& CompleteScalarPredictor::get_delta() const
    	{ return _delta; }

        void CompleteScalarPredictor::set_delta(const Eigen::VectorXd& delta)
        {
            if(delta.rows() != _delta.rows() )
            { throw statiskit::size_error("delta", delta.rows(), _delta.rows()); }
            _delta = delta;
        }

        std::unique_ptr< ScalarPredictor > CompleteScalarPredictor::copy() const
        { return std::make_unique< CompleteScalarPredictor >(*this); }

        ConstrainedScalarPredictor::ConstrainedScalarPredictor(const MultivariateSampleSpace& explanatory_space, const Eigen::MatrixXd& constraint) : CompleteScalarPredictor(explanatory_space, constraint.cols())
        {
        	if(constraint.rows() != explanatory_space.encode() )
        	{ throw statiskit::size_error("constraint", constraint.rows(), explanatory_space.encode() ); }
        	if(constraint.cols() > constraint.rows())
        	{ throw statiskit::size_error("constraint", constraint.cols(), constraint.rows(), statiskit::size_error::size_type::superior); }
        	_constraint = constraint;
        }

        ConstrainedScalarPredictor::ConstrainedScalarPredictor(const ConstrainedScalarPredictor& predictor) : CompleteScalarPredictor(predictor)
        { _constraint = predictor._constraint; }

        double ConstrainedScalarPredictor::operator() (const MultivariateEvent& event) const
        { return alpha + _explanatory_space->encode(event) * _constraint * _delta; }

        const Eigen::MatrixXd& ConstrainedScalarPredictor::get_constraint() const
        { return _constraint; }

    	void ConstrainedScalarPredictor::set_constraint(const Eigen::MatrixXd& constraint)
    	{
    		if(constraint.rows() != _constraint.rows())
    		{ throw statiskit::size_error("constraint", constraint.rows(), _constraint.rows()); }
    		if(constraint.cols() != _constraint.cols())
    		{ throw statiskit::size_error("constraint", constraint.cols(), _constraint.cols()); }
    		_constraint = constraint;
    	}

        std::unique_ptr< ScalarPredictor > ConstrainedScalarPredictor::copy() const
        { return std::make_unique< ConstrainedScalarPredictor >(*this); }

        VectorPredictor::VectorPredictor(const MultivariateSampleSpace& explanatory_space)
        { _explanatory_space = explanatory_space.copy().release(); }

        VectorPredictor::~VectorPredictor()
        { delete _explanatory_space; }

        VectorPredictor::VectorPredictor(const VectorPredictor& predictor)
        { _explanatory_space = predictor._explanatory_space->copy().release(); }

        const MultivariateSampleSpace* VectorPredictor::get_explanatory_space() const
        { return _explanatory_space; }

        CompleteVectorPredictor::CompleteVectorPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& dimension) : VectorPredictor(explanatory_space)
        {
        	_alpha = Eigen::VectorXd::Zero(dimension);
        	_delta = Eigen::MatrixXd::Zero(dimension, explanatory_space.encode());
        }

        CompleteVectorPredictor::CompleteVectorPredictor(const CompleteVectorPredictor& predictor) : VectorPredictor(predictor)
        {
        	_alpha = predictor._alpha;
        	_delta = predictor._delta;
        }

        Eigen::VectorXd CompleteVectorPredictor::operator() (const MultivariateEvent& event) const
        { return _alpha + _delta * (_explanatory_space->encode(event)).transpose(); }

        size_t CompleteVectorPredictor::size() const
        { return _alpha.rows() + _delta.size(); }

        void CompleteVectorPredictor::set_beta(const Eigen::VectorXd& beta)
    	{
    		if(beta.rows() != _alpha.rows() + _delta.rows() * _delta.cols())
    		{ throw statiskit::size_error("beta", beta.rows(),  _alpha.rows() + _delta.rows() * _delta.cols()); }
    		_alpha = beta.segment(0, _alpha.rows());
    		for(size_t j=0; j<_alpha.rows(); ++j)
            { _delta.block(j, 0, 1, _delta.cols()) = ( beta.segment(_alpha.rows() + j * _delta.cols(),_delta.cols() ) ).transpose(); } //_alpha.rows() + (j+1) * _delta.cols() - 1) ).transpose(); }
    	}

    	const Eigen::VectorXd& CompleteVectorPredictor::get_alpha() const
    	{ return _alpha; }

    	void CompleteVectorPredictor::set_alpha(const Eigen::VectorXd& alpha)
        {
            if(alpha.rows() != _alpha.rows() )
            { throw statiskit::size_error("alpha", alpha.rows(),  _alpha.rows() ); }
            _alpha = alpha;
        }

    	const Eigen::MatrixXd& CompleteVectorPredictor::get_delta() const
    	{ return _delta; }

        void CompleteVectorPredictor::set_delta(const Eigen::MatrixXd& delta)
        {
            if(delta.rows() != _delta.rows() )
            { throw statiskit::size_error("delta nb rows", delta.rows(),  _delta.rows() ); }
            if(delta.cols() != _delta.cols() )
            { throw statiskit::size_error("delta nb cols", delta.cols(),  _delta.cols() ); }
            _delta = delta;
        }

        std::unique_ptr< VectorPredictor > CompleteVectorPredictor::copy() const
        { return std::make_unique< CompleteVectorPredictor >(*this); }

        ProportionalVectorPredictor::ProportionalVectorPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& dimension) : VectorPredictor(explanatory_space)
        {
        	_alpha = Eigen::VectorXd::Zero(dimension);
        	_delta = Eigen::VectorXd::Zero(explanatory_space.encode());
        }

        ProportionalVectorPredictor::ProportionalVectorPredictor(const ProportionalVectorPredictor& predictor) : VectorPredictor(predictor)
        {
         	_alpha = predictor._alpha;
        	_delta = predictor._delta;
        }

        Eigen::VectorXd ProportionalVectorPredictor::operator() (const MultivariateEvent& event) const
        { return _alpha + _explanatory_space->encode(event) * _delta * Eigen::VectorXd::Ones(_alpha.rows()); }

        size_t ProportionalVectorPredictor::size() const
        { return _alpha.rows() + _delta.rows(); }

        void ProportionalVectorPredictor::set_beta(const Eigen::VectorXd& beta)
    	{
    		if(beta.rows() != _alpha.rows() + _delta.rows() * _delta.cols())
    		{ throw statiskit::size_error("beta", beta.rows(),  _alpha.rows() + _delta.rows() * _delta.cols()); }
    		_alpha = beta.segment(0, _alpha.rows() );
    		_delta = beta.segment(_alpha.rows(),  _delta.rows() );
    	}

    	const Eigen::VectorXd& ProportionalVectorPredictor::get_alpha() const
    	{ return _alpha; }

        void ProportionalVectorPredictor::set_alpha(const Eigen::VectorXd& alpha)
        {
            if(alpha.rows() != _alpha.rows() )
            { throw statiskit::size_error("alpha", alpha.rows(),  _alpha.rows() ); }
            _alpha = alpha;
        }

    	const Eigen::VectorXd& ProportionalVectorPredictor::get_delta() const
    	{ return _delta; }

        void ProportionalVectorPredictor::set_delta(const Eigen::VectorXd& delta)
        {
            if(delta.rows() != _delta.rows() )
            { throw statiskit::size_error("delta", delta.rows(),  _delta.rows() ); }
            _delta = delta;
        }

        std::unique_ptr< VectorPredictor > ProportionalVectorPredictor::copy() const
        { return std::make_unique< ProportionalVectorPredictor >(*this); }

        ProportionalVectorPredictor::ProportionalVectorPredictor(const MultivariateSampleSpace& explanatory_space) : VectorPredictor(explanatory_space)
        {}

        ConstrainedVectorPredictor::ConstrainedVectorPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& dimension, const Eigen::MatrixXd& constraint) : ProportionalVectorPredictor(explanatory_space)
        {
        	if(constraint.rows() != ( explanatory_space.encode() * dimension ) )
        	{ throw statiskit::size_error("constraint", constraint.rows(),  explanatory_space.encode() * dimension ); }
        	if(constraint.cols() > constraint.rows())
        	{ throw statiskit::size_error("constraint", constraint.cols(), constraint.rows(), statiskit::size_error::size_type::superior); }
        	_constraint = constraint;
        	_intercept_constraint = Eigen::MatrixXd::Identity(dimension, dimension);
            _alpha = Eigen::VectorXd::Zero(dimension);
            _delta = Eigen::VectorXd::Zero(constraint.cols());
        }

        ConstrainedVectorPredictor::ConstrainedVectorPredictor(const MultivariateSampleSpace& explanatory_space, const Eigen::MatrixXd& constraint, const Eigen::MatrixXd& intercept_constraint) : ProportionalVectorPredictor(explanatory_space)
        {
        	if(constraint.rows() != ( explanatory_space.encode() * intercept_constraint.rows()) )
        	{ throw statiskit::size_error("constraint", constraint.rows(),  explanatory_space.encode() * intercept_constraint.rows()); }
        	if(constraint.cols() >  constraint.rows())
        	{ throw statiskit::size_error("constraint", constraint.cols(), constraint.rows(), statiskit::size_error::size_type::superior); }
        	_constraint = constraint;
        	if(intercept_constraint.cols() > intercept_constraint.rows() )
        	{ throw statiskit::size_error("intercept_constraint", intercept_constraint.cols(), intercept_constraint.rows(), statiskit::size_error::size_type::superior); }
        	_intercept_constraint = intercept_constraint;
            _alpha = Eigen::VectorXd::Zero(intercept_constraint.cols());
            _delta = Eigen::VectorXd::Zero(constraint.cols());
        }

        ConstrainedVectorPredictor::ConstrainedVectorPredictor(const MultivariateSampleSpace& explanatory_space, const size_t& dimension, const Indices& proportional) : ConstrainedVectorPredictor(explanatory_space, dimension, partial_proportional_constraint(explanatory_space, dimension, proportional))
        {}

        ConstrainedVectorPredictor::ConstrainedVectorPredictor(const ConstrainedVectorPredictor& predictor) : ProportionalVectorPredictor(predictor)
        {
        	_constraint = predictor._constraint;
        	_intercept_constraint = predictor._intercept_constraint;
        }

        Eigen::VectorXd ConstrainedVectorPredictor::operator() (const MultivariateEvent& event) const
        {
            Eigen::VectorXd xt = _explanatory_space->encode(event).transpose();
            Eigen::MatrixXd Identity = Eigen::MatrixXd::Identity(_intercept_constraint.rows(), _intercept_constraint.rows());
            return _intercept_constraint * _alpha + Eigen::kroneckerProduct(Identity, xt) * _constraint * _delta;
        }

        const Eigen::MatrixXd& ConstrainedVectorPredictor::get_constraint() const
        { return _constraint; }

    	void ConstrainedVectorPredictor::set_constraint(const Eigen::MatrixXd& constraint)
    	{
    		if(constraint.rows() != _constraint.rows())
    		{ throw statiskit::size_error("constraint", constraint.rows(), _constraint.rows()); }
    		if(constraint.cols() != _constraint.cols())
    		{ throw statiskit::size_error("constraint", constraint.cols(), _constraint.cols()); }
    		_constraint = constraint;
    	}

        const Eigen::MatrixXd& ConstrainedVectorPredictor::get_intercept_constraint() const
        { return _intercept_constraint; }

    	void ConstrainedVectorPredictor::set_intercept_constraint(const Eigen::MatrixXd& intercept_constraint)
    	{
    		if(intercept_constraint.rows() != _intercept_constraint.rows())
    		{ throw statiskit::size_error("intercept_constraint", intercept_constraint.rows(), _intercept_constraint.rows()); }
    		if(intercept_constraint.cols() != _intercept_constraint.cols())
    		{ throw statiskit::size_error("intercept_constraint", intercept_constraint.cols(), _intercept_constraint.cols()); }
    		_intercept_constraint = intercept_constraint;
    	}

        std::unique_ptr< VectorPredictor > ConstrainedVectorPredictor::copy() const
        { return std::make_unique< ConstrainedVectorPredictor >(*this); }

        Eigen::MatrixXd ConstrainedVectorPredictor::partial_proportional_constraint(const MultivariateSampleSpace& explanatory_space, const size_t& dimension, const Indices& proportional)
        {
            Index nb_cols=0, nb_block_rows=0, current_row = 0, current_col=0;
            std::vector< Index > rows, cols;
            for(Index i=0; i<explanatory_space.size(); ++i)
            {
                if(explanatory_space.get(i)->get_outcome() == outcome_type::CATEGORICAL)
                {
                    Index K = static_cast< const CategoricalSampleSpace* >( explanatory_space.get(i) )->get_cardinality();
                    rows.push_back(K-1);
                }
                else
                { rows.push_back(1); }
                nb_block_rows += rows.back();

                if(proportional.find(i) == proportional.end()) // complete
                { cols.push_back(rows.back() * dimension); }
                else
                { cols.push_back(rows.back()); }
                nb_cols += cols.back();
            }
            Eigen::MatrixXd constraint = Eigen::MatrixXd::Zero(nb_block_rows*dimension, nb_cols);
            for(Index i=0; i<explanatory_space.size(); ++i)
            {
                Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(rows[i], rows[i]);
                if(proportional.find(i) == proportional.end())
                {
                    for(Index j=0; j<dimension; ++j)
                    {
                        Eigen::RowVectorXd indicator = Eigen::RowVectorXd::Zero(dimension);
                        indicator[j] = 1;
                        constraint.block(j*nb_block_rows + current_row, current_col, rows[i], cols[i]) = Eigen::kroneckerProduct(indicator, identity);
                    }
                }
                else
                {
                    for(Index j=0; j<dimension; ++j)
                    {constraint.block(j*nb_block_rows + current_row, current_col, rows[i], cols[i]) = identity; }
                }
                current_col += cols[i];
                current_row += rows[i];
            }
            return constraint;
        }

        Eigen::MatrixXd ConstrainedVectorPredictor::partial_proportional_constraint(const UnivariateConditionalData& data, const Indices& proportional)
        {
            Index J = static_cast< const CategoricalSampleSpace* >(data.get_response()->get_sample_space())->get_cardinality();
            const MultivariateData* _data = data.get_explanatories();
            const MultivariateSampleSpace* explanatory_space = _data->get_sample_space();
            return partial_proportional_constraint(*explanatory_space, J-1, proportional);
        }
    }
}


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

#include "enumerate.hpp"

#include <boost/dynamic_bitset.hpp>
#include <boost/format.hpp>

#include <core/utils/bitset_utils.hpp>
#include <reversible/circuit.hpp>
#include <reversible/truth_table.hpp>
#include <cli/reversible_stores.hpp>
#include <reversible/io/print_circuit.hpp>
#include <reversible/synthesis/transformation_based_synthesis.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

enumerate_command::enumerate_command( const environment::ptr& env )
  : cirkit_command( env, "Different enumeration routines" )
{
  opts.add_options()
    ( "four_stg_pqcs", "Enumerate all four variable single-target gates for pQCS" )
    ( "from_store_pqcs", "Creates circuits for pQCS for all truth tables in the store" )
    ;
}

bool enumerate_command::execute()
{
  if ( is_set( "four_stg_pqcs" ) )
  {
    const auto n = 3u; /* number of lines - 1 */

    boost::dynamic_bitset<> bs( 1u << n );

    inc( bs ); /* skip empty circuit */

    do {
      /* create function */
      binary_truth_table spec;

      boost::dynamic_bitset<> ibs( n );

      do {
        const auto val = ibs.to_ulong();

        binary_truth_table::cube_type in, out;

        for ( auto i = 0u; i < n; ++i )
        {
          in.push_back( ibs.test( i ) );
          out.push_back( ibs.test( i ) );
        }

        auto in_0 = in;
        auto out_0 = out;
        in_0.push_back( false );
        out_0.push_back( bs.test( val ) );
        in.push_back( true );
        out.push_back( !bs.test( val ) );

        spec.add_entry( in_0, out_0 );
        spec.add_entry( in, out );

        inc( ibs );
      } while ( ibs.any() );

      /* create circuit */
      circuit circ;
      transformation_based_synthesis( circ, spec );

      /* print circuit */
      std::cout << "STG-4-" << bs << std::endl
                << format_iqc( circ ) << std::endl;

      inc( bs );
    } while ( bs.any() );
  }

  if ( is_set( "from_store_pqcs" ) )
  {
    const auto& specs = env->store<binary_truth_table>();

    for ( auto i = 0u; i < specs.size(); ++i )
    {
      circuit circ;
      transformation_based_synthesis( circ, specs[i] );

      /* print circuit */
      std::cout << "SPEC-" << i << std::endl
                << format_iqc( circ ) << std::endl;
    }
  }

  return true;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 "boost::dispatch::meta::as_real"

#include <boost/simd/sdk/config/types.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/mpl/placeholders.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE(as_floating)
{
  using boost::mpl::_;
  using boost::dispatch::meta::as_floating;

  NT2_TEST_TYPE_IS( as_floating<double>::type                ,double);
  NT2_TEST_TYPE_IS( as_floating<float>::type                 ,float );
  NT2_TEST_TYPE_IS( as_floating<boost::simd::int64_t>::type  ,double);
  NT2_TEST_TYPE_IS( as_floating<boost::simd::int32_t >::type ,float );
  NT2_TEST_TYPE_IS( as_floating<boost::simd::uint64_t>::type ,double);
  NT2_TEST_TYPE_IS( as_floating<boost::simd::uint32_t>::type ,float );
}

NT2_TEST_CASE(as_floating_2_args)
{
  using boost::mpl::_;
  using boost::dispatch::meta::as_floating;

  NT2_TEST_TYPE_IS( (as_floating<double,double>::type               ), double);
  NT2_TEST_TYPE_IS( (as_floating<double,float>::type                ), double);
  NT2_TEST_TYPE_IS( (as_floating<double,boost::simd::int64_t>::type ), double);
  NT2_TEST_TYPE_IS( (as_floating<double,boost::simd::int32_t>::type ), double);
  NT2_TEST_TYPE_IS( (as_floating<double,boost::simd::uint64_t>::type), double);
  NT2_TEST_TYPE_IS( (as_floating<double,boost::simd::uint32_t>::type), double);
  NT2_TEST_TYPE_IS( (as_floating<float,double>::type                ), double);
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int64_t,double>::type ), double);
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int32_t,double>::type ), double);
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint64_t,double>::type), double);
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint32_t,double>::type), double);

  NT2_TEST_TYPE_IS( (as_floating<float,float>::type)                , float );
  NT2_TEST_TYPE_IS( (as_floating<float,boost::simd::int64_t>::type) , double);
  NT2_TEST_TYPE_IS( (as_floating<float,boost::simd::int32_t>::type) , float );
  NT2_TEST_TYPE_IS( (as_floating<float,boost::simd::uint64_t>::type), double);
  NT2_TEST_TYPE_IS( (as_floating<float,boost::simd::uint32_t>::type), float );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int64_t,float>::type) , double);
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int32_t,float>::type) , float );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint64_t,float>::type), double);
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint32_t,float>::type), float );

  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int64_t,boost::simd::int64_t>::type), double );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int64_t,boost::simd::int32_t>::type) , double );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int64_t,boost::simd::uint64_t>::type), double);
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int64_t,boost::simd::uint32_t>::type), double );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int32_t,boost::simd::int64_t>::type) , double );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint64_t,boost::simd::int64_t>::type), double);
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint32_t,boost::simd::int64_t>::type), double );

  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint64_t,boost::simd::uint64_t>::type), double );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint64_t,boost::simd::int32_t>::type) , double );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint64_t,boost::simd::uint32_t>::type), double );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int32_t,boost::simd::uint64_t>::type) , double );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint32_t,boost::simd::uint64_t>::type), double );

  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int32_t,boost::simd::int32_t>::type) , float  );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int32_t,boost::simd::uint32_t>::type), float  );
  NT2_TEST_TYPE_IS( (as_floating<boost::simd::int32_t,boost::simd::uint64_t>::type), double );

  NT2_TEST_TYPE_IS( (as_floating<boost::simd::uint32_t,boost::simd::int32_t>::type), float );
}


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * \file cloud.hpp
 * \brief PointXYZ, PointXY, PointCloud class definition
 *
 * \author Hugues Thomas, Autonomous Space Robotics Lab (ASRL)
 */
#pragma once

#include <stdio.h>
#include <string.h>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <map>
#include <numeric>
#include <unordered_map>
#include <vector>

#include <time.h>

#include <Eigen/Dense>

#include <vtr_lidar/npm_ply/ply_file_in.h>
#include <vtr_lidar/npm_ply/ply_file_out.h>
#include <vtr_lidar/npm_ply/ply_types.h>

namespace vtr {
namespace lidar {

//------------------------------------------------------------------------------------------------------------
// Point class
// ***********
//
//------------------------------------------------------------------------------------------------------------

class PointXYZ {
 public:
  using Vector3fMap = Eigen::Map<Eigen::Vector3f>;
  using Vector3fMapConst = const Eigen::Map<const Eigen::Vector3f>;
  // Elements
  // ********
  union {
    struct {
      float x;
      float y;
      float z;
    };
    float data[3];
  };

  // Methods
  // *******

  // Constructor
  PointXYZ(float x0 = 0, float y0 = 0, float z0 = 0) : x(x0), y(y0), z(z0) {}

  Vector3fMap getVector3fMap() { return (Vector3fMap(data)); }
  Vector3fMapConst getVector3fMap() const { return (Vector3fMapConst(data)); }

  // array type accessor
  float operator[](int i) const {
    if (i == 0)
      return x;
    else if (i == 1)
      return y;
    else
      return z;
  }

  // opperations
  float dot(const PointXYZ P) const { return x * P.x + y * P.y + z * P.z; }

  float sq_norm() const { return x * x + y * y + z * z; }

  PointXYZ cross(const PointXYZ P) const {
    return PointXYZ(y * P.z - z * P.y, z * P.x - x * P.z, x * P.y - y * P.x);
  }

  PointXYZ& operator+=(const PointXYZ& P) {
    x += P.x;
    y += P.y;
    z += P.z;
    return *this;
  }

  PointXYZ& operator-=(const PointXYZ& P) {
    x -= P.x;
    y -= P.y;
    z -= P.z;
    return *this;
  }

  PointXYZ& operator*=(const float& a) {
    x *= a;
    y *= a;
    z *= a;
    return *this;
  }
};

// Point Opperations
// *****************

inline PointXYZ operator+(const PointXYZ A, const PointXYZ B) {
  return PointXYZ(A.x + B.x, A.y + B.y, A.z + B.z);
}

inline PointXYZ operator-(const PointXYZ A, const PointXYZ B) {
  return PointXYZ(A.x - B.x, A.y - B.y, A.z - B.z);
}

inline PointXYZ operator*(const PointXYZ P, const float a) {
  return PointXYZ(P.x * a, P.y * a, P.z * a);
}

inline PointXYZ operator*(const float a, const PointXYZ P) {
  return PointXYZ(P.x * a, P.y * a, P.z * a);
}

inline PointXYZ operator/(const PointXYZ P, const float a) {
  return PointXYZ(P.x / a, P.y / a, P.z / a);
}

inline PointXYZ operator/(const float a, const PointXYZ P) {
  return PointXYZ(P.x / a, P.y / a, P.z / a);
}

inline std::ostream& operator<<(std::ostream& os, const PointXYZ P) {
  return os << "[" << P.x << ", " << P.y << ", " << P.z << "]";
}

inline bool operator==(const PointXYZ A, const PointXYZ B) {
  return A.x == B.x && A.y == B.y && A.z == B.z;
}

inline PointXYZ floor(const PointXYZ P) {
  return PointXYZ(std::floor(P.x), std::floor(P.y), std::floor(P.z));
}

PointXYZ max_point(const std::vector<PointXYZ>& points);
PointXYZ min_point(const std::vector<PointXYZ>& points);
PointXYZ max_point(const PointXYZ A, const PointXYZ B);
PointXYZ min_point(const PointXYZ A, const PointXYZ B);

//------------------------------------------------------------------------------------------------------------
// Point class 2D
// **************
//
//------------------------------------------------------------------------------------------------------------

class PointXY {
 public:
  // Elements
  // ********

  float x, y;

  // Methods
  // *******

  // Constructor
  PointXY() {
    x = 0;
    y = 0;
  }
  PointXY(float x0, float y0) {
    x = x0;
    y = y0;
  }
  PointXY(PointXYZ P) {
    x = P.x;
    y = P.y;
  }

  // array type accessor
  float operator[](int i) const {
    if (i == 0)
      return x;
    else
      return y;
  }

  // opperations
  float dot(const PointXY P) const { return x * P.x + y * P.y; }

  float sq_norm() const { return x * x + y * y; }

  float cross(const PointXY P) const { return x * P.y - y * P.x; }

  PointXY& operator+=(const PointXY& P) {
    x += P.x;
    y += P.y;
    return *this;
  }

  PointXY& operator-=(const PointXY& P) {
    x -= P.x;
    y -= P.y;
    return *this;
  }

  PointXY& operator*=(const float& a) {
    x *= a;
    y *= a;
    return *this;
  }
};

// Point Opperations
// *****************

inline PointXY operator+(const PointXY A, const PointXY B) {
  return PointXY(A.x + B.x, A.y + B.y);
}

inline PointXY operator-(const PointXY A, const PointXY B) {
  return PointXY(A.x - B.x, A.y - B.y);
}

inline PointXY operator*(const PointXY P, const float a) {
  return PointXY(P.x * a, P.y * a);
}

inline PointXY operator*(const float a, const PointXY P) {
  return PointXY(P.x * a, P.y * a);
}

inline PointXY operator/(const PointXY P, const float a) {
  return PointXY(P.x / a, P.y / a);
}

inline PointXY operator/(const float a, const PointXY P) {
  return PointXY(P.x / a, P.y / a);
}

inline std::ostream& operator<<(std::ostream& os, const PointXY P) {
  return os << "[" << P.x << ", " << P.y << "]";
}

inline bool operator==(const PointXY A, const PointXY B) {
  return A.x == B.x && A.y == B.y;
}

inline PointXY floor(const PointXY P) {
  return PointXY(std::floor(P.x), std::floor(P.y));
}

//------------------------------------------------------------------------------------------------------------
// Pointcloud class
// ****************
//
//------------------------------------------------------------------------------------------------------------

struct PointCloud {
  std::vector<PointXYZ> pts;

  // Must return the number of data points
  inline size_t kdtree_get_point_count() const { return pts.size(); }

  // Returns the dim'th component of the idx'th point in the class:
  // Since this is inlined and the "dim" argument is typically an immediate
  // value, the
  //  "if/else's" are actually solved at compile time.
  inline float kdtree_get_pt(const size_t idx, const size_t dim) const {
    if (dim == 0)
      return pts[idx].x;
    else if (dim == 1)
      return pts[idx].y;
    else
      return pts[idx].z;
  }

  // Optional bounding-box computation: return false to default to a standard
  // bbox computation loop.
  //   Return true if the BBOX was already computed by the class and returned in
  //   "bb" so it can be avoided to redo it again. Look at bb.size() to find out
  //   the expected dimensionality (e.g. 2 or 3 for point clouds)
  template <class BBOX>
  bool kdtree_get_bbox(BBOX& /* bb */) const {
    return false;
  }
};

// Utility function for pointclouds
template <typename T1, typename T2>
void filterVector(std::vector<T1>& vec, std::vector<T2>& scores,
                  T2 filter_value) {
  // Remove every element whose score is < filter_value
  auto vec_address = vec.data();
  vec.erase(std::remove_if(vec.begin(), vec.end(),
                           [&scores, vec_address, filter_value](const T1& f) {
                             return scores[(size_t)(&f - vec_address)] <
                                    filter_value;
                           }),
            vec.end());
}

template <typename T>
void filterVector(std::vector<T>& vec, T filter_value) {
  vec.erase(std::remove_if(
                vec.begin(), vec.end(),
                [filter_value](const float s) { return s < filter_value; }),
            vec.end());
}

template <typename T>
void filterFloatVector(std::vector<T>& vec, std::vector<float>& scores,
                       float filter_value) {
  // Remove every element whose score is < filter_value
  auto vec_address = vec.data();
  vec.erase(std::remove_if(vec.begin(), vec.end(),
                           [&scores, vec_address, filter_value](const T& f) {
                             return scores[(size_t)(&f - vec_address)] <
                                    filter_value;
                           }),
            vec.end());
}

void filterPointCloud(std::vector<PointXYZ>& pts, std::vector<float>& scores,
                      float filter_value);
void filterFloatVector(std::vector<float>& vec, float filter_value);

// PLY reading/saving functions
void save_cloud(std::string dataPath, std::vector<PointXYZ>& points,
                std::vector<PointXYZ>& normals, std::vector<float>& features);
void save_cloud(std::string dataPath, std::vector<PointXYZ>& points,
                std::vector<float>& features);
void save_cloud(std::string dataPath, std::vector<PointXYZ>& points,
                std::vector<PointXYZ>& normals);
void save_cloud(std::string dataPath, std::vector<PointXYZ>& points);

void load_cloud(std::string& dataPath, std::vector<PointXYZ>& points);

void load_cloud(std::string& dataPath, std::vector<PointXYZ>& points,
                std::vector<float>& float_scalar,
                std::string& float_scalar_name, std::vector<int>& int_scalar,
                std::string& int_scalar_name);

void load_cloud_normals(std::string& dataPath, std::vector<PointXYZ>& points,
                        std::vector<PointXYZ>& normals,
                        std::vector<float>& float_scalar,
                        std::string& float_scalar_name,
                        std::vector<int>& int_scalar,
                        std::string& int_scalar_name);

//------------------------------------------------------------------------------------------------------------
// Plane3D class
// *************
//
//------------------------------------------------------------------------------------------------------------

class Plane3D {
 public:
  // Elements
  // ********

  // The plane is define by the equation a*x + b*y + c*z = d. The values (a, b,
  // c) are stored in a PointXYZ called u.
  PointXYZ u;
  float d;

  // Methods
  // *******

  // Constructor
  Plane3D() {
    u.x = 1;
    u.y = 0;
    u.z = 0;
    d = 0;
  }
  Plane3D(const float a0, const float b0, const float c0, const float d0) {
    u.x = a0;
    u.y = b0;
    u.z = c0;
    d = d0;
  }
  Plane3D(const PointXYZ P0, const PointXYZ N0) {
    // Init with point and normal
    u = N0;
    d = N0.dot(P0);
  }
  Plane3D(const PointXYZ A, const PointXYZ B, const PointXYZ C) {
    // Init with three points
    u = (B - A).cross(C - A);
    d = u.dot(A);
  }

  // Method getting distance to one point
  float point_distance(const PointXYZ P) {
    return std::abs((u.dot(P) - d) / std::sqrt(u.sq_norm()));
  }

  // Method getting square distance to one point
  float point_sq_dist(const PointXYZ P) {
    float tmp = u.dot(P) - d;
    return tmp * tmp / u.sq_norm();
  }

  // Method getting distances to some points
  void point_distances(std::vector<PointXYZ>& points,
                       std::vector<float>& distances) {
    if (distances.size() != points.size())
      distances = std::vector<float>(points.size());
    size_t i = 0;
    float inv_norm_u = 1 / std::sqrt(u.sq_norm());
    for (auto& p : points) {
      distances[i] = std::abs((u.dot(p) - d) * inv_norm_u);
      i++;
    }
  }

  int in_range(std::vector<PointXYZ>& points, float threshold) {
    int count = 0;
    float inv_norm_u = 1 / std::sqrt(u.sq_norm());
    for (auto& p : points) {
      if (std::abs((u.dot(p) - d) * inv_norm_u) < threshold) count++;
    }
    return count;
  }
};

}  // namespace lidar
}  // namespace vtr


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE RegroupTest
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_support/logging.hpp>
#include <popops/Rearrange.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/exceptions.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace popops;
using namespace poputil;
using namespace poplibs_support;

const OptionFlags options;

constexpr unsigned dim0 = 32;
constexpr unsigned dim1 = 16;

static std::pair<std::vector<GroupingInfo>, std::vector<GroupingInfo>>
runTest(const GroupingInfo &requestedGrouping) {
  auto device = createTestDevice(TEST_TARGET, 1, 4);
  Graph graph(device.getTarget());
  popops::addCodelets(graph);
  auto in = graph.addVariable(FLOAT, {dim0, dim1});
  float hIn[dim0][dim1];
  float hOut[dim0][dim1];

  for (unsigned i = 0; i != dim0; ++i) {
    for (unsigned j = 0; j != dim1; ++j) {
      hIn[i][j] = i * dim1 + j;
      hOut[i][j] = dim0 * dim1 - hIn[i][j];
    }
  }

  mapTensorLinearly(graph, in, 8, 8);
  auto prog = Sequence();
  auto out =
      popops::rearrange::regroupIfPossible(graph, in, prog, requestedGrouping);

  graph.createHostWrite("in", in);
  graph.createHostRead("out", out);

  Engine eng(graph, prog, options);
  device.bind([&](const Device &d) {
    eng.load(d);
    eng.writeTensor("in", hIn, &hIn[0][0] + dim0 * dim1);
    eng.run();
    eng.readTensor("out", hOut, &hOut[0][0] + dim0 * dim1);
  });

  for (unsigned i = 0; i != dim0; ++i) {
    for (unsigned j = 0; j != dim1; ++j) {
      BOOST_CHECK_EQUAL(hIn[i][j], hOut[i][j]);
    }
  }

  return std::make_pair(detectDimGroupings(graph, in),
                        detectDimGroupings(graph, out));
}

// Requested regrouping is performed
BOOST_AUTO_TEST_CASE(RegroupGrouping_0_16) {
  auto [inG, outG] = runTest({0, 16});
  BOOST_CHECK_EQUAL(std::get<0>(inG[0]), 1);
  BOOST_CHECK_EQUAL(std::get<1>(inG[0]), 16);
  BOOST_CHECK_EQUAL(std::get<0>(inG[1]), 0);
  BOOST_CHECK_EQUAL(std::get<1>(inG[1]), 8);

  BOOST_CHECK_EQUAL(std::get<0>(outG[0]), 0);
  BOOST_CHECK_EQUAL(std::get<1>(outG[0]), 16);
  BOOST_CHECK_EQUAL(std::get<0>(outG[1]), 1);
  BOOST_CHECK_EQUAL(std::get<1>(outG[1]), 8);
}

// No change in grouping as grouping is requested exists in the innermost
// dimension
BOOST_AUTO_TEST_CASE(RegroupGrouping_1_16) {
  auto [inG, outG] = runTest({1, 16});
  BOOST_CHECK_EQUAL(std::get<0>(inG[0]), 1);
  BOOST_CHECK_EQUAL(std::get<1>(inG[0]), 16);
  BOOST_CHECK_EQUAL(std::get<0>(inG[1]), 0);
  BOOST_CHECK_EQUAL(std::get<1>(inG[1]), 8);

  BOOST_CHECK_EQUAL(std::get<0>(outG[0]), 1);
  BOOST_CHECK_EQUAL(std::get<1>(outG[0]), 16);
  BOOST_CHECK_EQUAL(std::get<0>(outG[1]), 0);
  BOOST_CHECK_EQUAL(std::get<1>(outG[1]), 8);
}

// There be no change in the grouping as requested grouping is not a multiple
// of the dimension
BOOST_AUTO_TEST_CASE(RegroupGrouping_0_7) {
  auto [inG, outG] = runTest({0, 7});
  BOOST_CHECK_EQUAL(std::get<0>(inG[0]), 1);
  BOOST_CHECK_EQUAL(std::get<1>(inG[0]), 16);
  BOOST_CHECK_EQUAL(std::get<0>(inG[1]), 0);
  BOOST_CHECK_EQUAL(std::get<1>(inG[1]), 8);

  BOOST_CHECK_EQUAL(std::get<0>(outG[0]), 1);
  BOOST_CHECK_EQUAL(std::get<1>(outG[0]), 16);
  BOOST_CHECK_EQUAL(std::get<0>(outG[1]), 0);
  BOOST_CHECK_EQUAL(std::get<1>(outG[1]), 8);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  CombinedImuFactor.cpp
 *  @author Luca Carlone
 *  @author Stephen Williams
 *  @author Richard Roberts
 *  @author Vadim Indelman
 *  @author David Jensen
 *  @author Frank Dellaert
 *  @author Varun Agrawal
 **/

#include <gtsam/navigation/CombinedImuFactor.h>
#include <boost/serialization/export.hpp>

/* External or standard includes */
#include <ostream>

namespace gtsam {

using namespace std;

//------------------------------------------------------------------------------
// Inner class PreintegrationCombinedParams
//------------------------------------------------------------------------------
void PreintegrationCombinedParams::print(const string& s) const {
  PreintegrationParams::print(s);
  cout << "biasAccCovariance:\n[\n" << biasAccCovariance << "\n]"
       << endl;
  cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
       << endl;
  cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
       << endl;
}

//------------------------------------------------------------------------------
bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& other,
                                  double tol) const {
  auto e = dynamic_cast<const PreintegrationCombinedParams*>(&other);
  return e != nullptr && PreintegrationParams::equals(other, tol) &&
         equal_with_abs_tol(biasAccCovariance, e->biasAccCovariance,
                            tol) &&
         equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
                            tol) &&
         equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
}

//------------------------------------------------------------------------------
// Inner class PreintegratedCombinedMeasurements
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::print(const string& s) const {
  PreintegrationType::print(s);
  cout << "  preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
}

//------------------------------------------------------------------------------
bool PreintegratedCombinedMeasurements::equals(
    const PreintegratedCombinedMeasurements& other, double tol) const {
  return PreintegrationType::equals(other, tol)
      && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
}

//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::resetIntegration() {
  PreintegrationType::resetIntegration();
  preintMeasCov_.setZero();
}

//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
#define D_a_a(H) (H)->block<3,3>(9,9)
#define D_g_g(H) (H)->block<3,3>(12,12)

//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement(
    const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
  // Update preintegrated measurements.
  Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
  Matrix93 B, C;
  PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);

  // Update preintegrated measurements covariance: as in [2] we consider a first
  // order propagation that can be seen as a prediction phase in an EKF
  // framework. In this implementation, in contrast to [2], we consider the
  // uncertainty of the bias selection and we keep correlation between biases
  // and preintegrated measurements

  // Single Jacobians to propagate covariance
  // TODO(frank): should we not also account for bias on position?
  Matrix3 theta_H_biasOmega = -C.topRows<3>();
  Matrix3 vel_H_biasAcc = -B.bottomRows<3>();

  // overall Jacobian wrt preintegrated measurements (df/dx)
  Eigen::Matrix<double, 15, 15> F;
  F.setZero();
  F.block<9, 9>(0, 0) = A;
  F.block<3, 3>(0, 12) = theta_H_biasOmega;
  F.block<3, 3>(6, 9) = vel_H_biasAcc;
  F.block<6, 6>(9, 9) = I_6x6;

  // propagate uncertainty
  // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
  const Matrix3& aCov = p().accelerometerCovariance;
  const Matrix3& wCov = p().gyroscopeCovariance;
  const Matrix3& iCov = p().integrationCovariance;

  // first order uncertainty propagation
  // Optimized matrix multiplication   (1/dt) * G * measurementCovariance *
  // G.transpose()
  Eigen::Matrix<double, 15, 15> G_measCov_Gt;
  G_measCov_Gt.setZero(15, 15);

  // BLOCK DIAGONAL TERMS
  D_t_t(&G_measCov_Gt) = dt * iCov;
  D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
      * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
      * (vel_H_biasAcc.transpose());
  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
      * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
      * (theta_H_biasOmega.transpose());
  D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
  D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;

  // OFF BLOCK DIAGONAL TERMS
  Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
      * theta_H_biasOmega.transpose();
  D_v_R(&G_measCov_Gt) = temp;
  D_R_v(&G_measCov_Gt) = temp.transpose();
  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
}

//------------------------------------------------------------------------------
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
    Key vel_j, Key bias_i, Key bias_j,
    const PreintegratedCombinedMeasurements& pim) :
    Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
        pose_j, vel_j, bias_i, bias_j), _PIM_(pim) {
}

//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
      gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

//------------------------------------------------------------------------------
void CombinedImuFactor::print(const string& s,
    const KeyFormatter& keyFormatter) const {
  cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor("
       << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
       << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << ","
       << keyFormatter(this->key5()) << "," << keyFormatter(this->key6())
       << ")\n";
  _PIM_.print("  preintegrated measurements:");
  this->noiseModel_->print("  noise model: ");
}

//------------------------------------------------------------------------------
bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
  const This* e = dynamic_cast<const This*>(&other);
  return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
}

//------------------------------------------------------------------------------
Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
    const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
    const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
    boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
    boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {

  // error wrt bias evolution model (random walk)
  Matrix6 Hbias_i, Hbias_j;
  Vector6 fbias = traits<imuBias::ConstantBias>::Between(bias_j, bias_i,
      H6 ? &Hbias_j : 0, H5 ? &Hbias_i : 0).vector();

  Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i;
  Matrix93 D_r_vel_i, D_r_vel_j;

  // error wrt preintegrated measurements
  Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
      bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
      H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);

  // if we need the jacobians
  if (H1) {
    H1->resize(15, 6);
    H1->block<9, 6>(0, 0) = D_r_pose_i;
    // adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
    H1->block<6, 6>(9, 0).setZero();
  }
  if (H2) {
    H2->resize(15, 3);
    H2->block<9, 3>(0, 0) = D_r_vel_i;
    // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
    H2->block<6, 3>(9, 0).setZero();
  }
  if (H3) {
    H3->resize(15, 6);
    H3->block<9, 6>(0, 0) = D_r_pose_j;
    // adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
    H3->block<6, 6>(9, 0).setZero();
  }
  if (H4) {
    H4->resize(15, 3);
    H4->block<9, 3>(0, 0) = D_r_vel_j;
    // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
    H4->block<6, 3>(9, 0).setZero();
  }
  if (H5) {
    H5->resize(15, 6);
    H5->block<9, 6>(0, 0) = D_r_bias_i;
    // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
    H5->block<6, 6>(9, 0) = Hbias_i;
  }
  if (H6) {
    H6->resize(15, 6);
    H6->block<9, 6>(0, 0).setZero();
    // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
    H6->block<6, 6>(9, 0) = Hbias_j;
  }

  // overall error
  Vector r(15);
  r << r_Rpv, fbias; // vector of size 15
  return r;
}

//------------------------------------------------------------------------------
std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
  f._PIM_.print("combined preintegrated measurements:\n");
  os << "  noise model sigmas: " << f.noiseModel_->sigmas().transpose();
  return os;
}
}
 /// namespace gtsam

/// Boost serialization export definition for derived class
BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams);


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_UNARY_MINUS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_UNARY_MINUS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing unary_minus capabilities

    return the elementwise unary minus of the parameter

    Infix notation can be used with operator '-'

    @par Semantic:

    For every parameter of type T

    @code
    T r = unary_minus(x);
    @endcode

    is similar to:

    @code
    T r = -x;
    @endcode

    @par Alias:
    @c neg

    @see  minus, unary_plus

  **/
  const boost::dispatch::functor<tag::unary_minus_> unary_minus = {};
} }
#endif

#include <boost/simd/function/scalar/unary_minus.hpp>
#include <boost/simd/function/simd/unary_minus.hpp>

#endif


#ifndef BOOST_TEST_RIGHT_SHIFT_HPP
#define BOOST_TEST_RIGHT_SHIFT_HPP

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

#include <iostream>

#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/safe_numerics/range_value.hpp>

template<class T1, class T2>
bool test_right_shift(
    T1 v1,
    T2 v2,
    const char *av1,
    const char *av2,
    char expected_result
){
    std::cout
        << "testing  "
        << av1 << " >> " << av2
        << std::endl;
    {
        safe_t<T1> t1 = v1;
        using result_type = decltype(t1 >> v2);
        std::cout << "safe<" << av1 << "> << " << av2 << " -> ";
        static_assert(
            boost::safe_numerics::is_safe<result_type>::value,
            "Expression failed to return safe type"
        );

        try{
            // use auto to avoid checking assignment.
            auto result = t1 >> v2;
            std::cout << make_result_display(result);
            if(expected_result == 'x'){
                std::cout
                    << " ! = "<< av1 << " >> " << av2
                    << " failed to detect error in right shift"
                    << std::endl;
                t1 >> v2;
                return false;
            }
            std::cout << std::endl;
        }
        catch(const std::exception & e){
            if(expected_result == '.'){
                std::cout
                    << " == "<< av1 << " >> " << av2
                    << " erroneously detected error in right shift"
                    << ' ' << e.what()
                    << std::endl;
                try{
                    t1 >> v2;
                }
                catch(const std::exception &){}
                return false;
            }
            std::cout << std::endl;
        }
    }
    {
        safe_t<T2> t2 = v2;
        using result_type = decltype(v1 >> t2);
        std::cout << av1 << " >> " << "safe<" << av2 << "> -> ";
        static_assert(
            boost::safe_numerics::is_safe<result_type>::value,
            "Expression failed to return safe type"
        );

        try{
            // use auto to avoid checking assignment.
            auto result = v1 >> t2;
            std::cout << make_result_display(result);
            if(expected_result == 'x'){
                std::cout
                    << " ! = "<< av1 << " >> " << av2
                    << " failed to detect error in right shift"
                    << std::hex << result << "(" << std::dec << result << ")"
                    << std::endl;
                v1 >> t2;
                return false;
            }
            std::cout << std::endl;
        }
        catch(const std::exception & e){
            if(expected_result == '.'){
                std::cout
                    << " == "<< av1 << " >> " << av2
                    << " erroneously detected error in right shift"
                    << ' ' << e.what()
                    << std::endl;
                try{
                    v1 >> t2;
                }
                catch(const std::exception &){}
                return false;
            }
            std::cout << std::endl;
        }
    }
    {
        safe_t<T1> t1 = v1;
        safe_t<T2> t2 = v2;
        using result_type = decltype(t1 >> t2);
        std::cout << "safe<" << av1 << "> >> " << "safe<" << av2 << "> -> ";
        static_assert(
            boost::safe_numerics::is_safe<result_type>::value,
            "Expression failed to return safe type"
        );

        try{
            // use auto to avoid checking assignment.
            auto result = t1 >> t2;
            std::cout << make_result_display(result);
            if(expected_result == 'x'){
                std::cout
                    << " ! = "<< av1 << " << " << av2
                    << " failed to detect error in right shift"
                    << std::endl;
                t1 >> t2;
                return false;
            }
            std::cout << std::endl;
        }
        catch(const std::exception & e){
            if(expected_result == '.'){
                std::cout
                    << " == "<< av1 << " << " << av2
                    << " erroneously detected error in right shift"
                    << ' ' << e.what()
                    << std::endl;
                try{
                    t1 >> t2;
                }
                catch(const std::exception &){}
                return false;
            }
            std::cout << std::endl;
        }
    }
    return true; // correct result
}

#endif // BOOST_TEST_RIGHT_SHIFT_HPP


#ifndef HPP_PIECE_TREE
#define HPP_PIECE_TREE

#include <boost/serialization/access.hpp>

#include <set>
#include <vector>
#include <algorithm>

#include "piece.hpp"
#include "piece_node.hpp"

class piece_tree {
private:
    piece_node d_root;

public:
    explicit piece_tree(const std::vector<piece>& pieces)
        : d_root(pieces) {}

    std::vector<piece> delete_overlaps(const std::vector<piece>& pieces) {
        std::vector<piece> result(pieces.begin(), pieces.end());

        std::sort(result.begin(), result.end(), [](const piece& a, const piece& b) -> bool {
            if (b.size() - a.size() == 0) {
                return a.start() > b.start();
            }

            return a.size() > b.size();
        });

        std::set<piece> remove_tmp;

        for (const auto& p : result) {
            if (remove_tmp.find(p) != remove_tmp.end()) {
                continue;
            }

            auto overlaps = d_root.overlaps(p);

            for (const auto& overlap : overlaps) {
                remove_tmp.insert(overlap);
            }
        }

        for (const auto& p : remove_tmp) {
            result.erase(
                std::find(result.begin(), result.end(), p)
            );
        }

        std::sort(result.begin(), result.end(), [](const piece& a, const piece& b) -> bool {
            return a.start() < b.start();
        });

        return std::vector<piece>(result);
    }

private:
    friend class boost::serialization::access;

    template <typename Archive>
    void serialize(Archive &ar, const unsigned int version) {
        ar & d_root;
    }
};

#endif


#include "pch.h"
#include "ExternalImporter.h"

#include <fstream>
#include <boost/algorithm/algorithm.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <experimental/filesystem>
#include <openvdb/tools/Dense.h>

static void printProgress(const std::string& prefix, float progress)
{
	int barWidth = 50;
	std::cout << prefix << " [";
	int pos = barWidth * progress;
	for (int i = 0; i < barWidth; ++i) {
		if (i < pos) std::cout << "=";
		else if (i == pos) std::cout << ">";
		else std::cout << " ";
	}
	std::cout << "] " << int(progress * 100.0) << " %\r";
	std::cout.flush();
	if (progress >= 1) std::cout << std::endl;
}

openvdb::FloatGrid::Ptr ExternalImporter::importRAW(
	const std::string& filename, int downsampling, float lowerThreshold)
{
	if (!boost::algorithm::ends_with(filename, ".dat"))
	{
		std::cout << "Filename does not point to the .dat file" << std::endl;
		return nullptr;
	}

	//read descriptor file
	std::ifstream file(filename);
	if (!file.is_open())
	{
		std::cout << "Unable to open file " << filename << std::endl;
		return nullptr;
	}
	std::string line;
	std::string objectFileName = "";
	size_t resolutionX = 0;
	size_t resolutionY = 0;
	size_t resolutionZ = 0;
	std::string datatype = "";
	const std::string DATATYPE_UCHAR = "UCHAR";
	const std::string DATATYPE_USHORT = "USHORT";
	const std::string DATATYPE_BYTE = "BYTE";
	while (std::getline(file, line))
	{
		if (line.empty()) continue;
		std::istringstream iss(line);
		std::string token;
		iss >> token;
		if (!iss) continue; //no token in the current line
		if (token == "ObjectFileName:")
			iss >> objectFileName;
		else if (token == "Resolution:")
			iss >> resolutionX >> resolutionY >> resolutionZ;
		else if (token == "Format:")
			iss >> datatype;
		if (!iss)
		{
			std::cout << "Unable to parse line with token " << token << std::endl;
			return nullptr;
		}
	}
	file.close();
	if (objectFileName.empty() || resolutionX == 0 || datatype.empty())
	{
		std::cout << "Descriptor file does not contain ObjectFileName, Resolution and Format" << std::endl;
		return nullptr;
	}
	if (!(datatype == DATATYPE_UCHAR || datatype == DATATYPE_USHORT || datatype == DATATYPE_BYTE))
	{
		std::cout << "Unknown format " << datatype << std::endl;
		return nullptr;
	}
	std::cout << "Descriptor file read"
		<< "\nObjectFileName: " << objectFileName
		<< "\nResolution: " << resolutionX << ", " << resolutionY << ", " << resolutionZ
		<< "\nFormat: " << datatype << std::endl;

	// read volume
	size_t bytesPerEntry = 0;
	if (datatype == DATATYPE_UCHAR) bytesPerEntry = 1;
	if (datatype == DATATYPE_BYTE) bytesPerEntry = 1;
	if (datatype == DATATYPE_USHORT) bytesPerEntry = 2;
	size_t bytesToRead = resolutionX * resolutionY * resolutionZ * bytesPerEntry;
	std::string bfilename = std::experimental::filesystem::path(filename).replace_filename(objectFileName).generic_string();

	std::cout << "Load " << bytesToRead << " bytes from " << bfilename << std::endl;
	std::ifstream bfile(bfilename, std::ifstream::binary | std::ifstream::ate);
	if (!bfile.is_open())
	{
		std::cout << "Unable to open file " << bfilename << std::endl;
		return nullptr;
	}
	size_t filesize = bfile.tellg();
	int headersize = static_cast<int>(filesize - static_cast<long long>(bytesToRead));
	if (headersize < 0)
	{
		std::cout << "File is too small, " << (-headersize) << " bytes missing" << std::endl;
		return nullptr;
	}
	std::cout << "Skipping header of length " << headersize << std::endl;
	bfile.seekg(std::ifstream::pos_type(headersize));

#if 1
	bytesToRead = resolutionX * resolutionY * downsampling * bytesPerEntry;
	std::vector<char> data(bytesToRead);

	//convert to dense grid and build vdb grid slice by slice
	//openvdb::CoordBBox gridDim(0, 0, 0, resolutionX - 1, resolutionY - 1, resolutionZ - 1);
	//typedef openvdb::tools::Dense<float, openvdb::tools::LayoutZYX> DenseT;
	typedef openvdb::tools::Dense<float, openvdb::tools::LayoutXYZ> DenseT;
	openvdb::FloatGrid::Ptr grid = openvdb::FloatGrid::create();
	const int sliceSize = resolutionX * resolutionY / (downsampling*downsampling);

	for (int z=0; z<resolutionZ/ downsampling; ++z)
	{
		bfile.read(&data[0], bytesToRead);
		if (!bfile)
		{
			std::cout << "Loading data file failed" << std::endl;
			return nullptr;
		}

		if (z%10 == 0)
			printProgress("Convert", z / float(resolutionZ));
		openvdb::CoordBBox sliceDim(0, 0, z,
			resolutionX / downsampling - 1, resolutionY / downsampling - 1, z);
		DenseT d(sliceDim);

		if (datatype == DATATYPE_UCHAR || datatype==DATATYPE_BYTE) {
			const unsigned char* raw = reinterpret_cast<unsigned char*>(data.data());
#pragma omp parallel for
			for (int y = 0; y < resolutionY / downsampling; ++y)
				for (int x = 0; x < resolutionX / downsampling; ++x)
			{
				float value = 0;
				for (int iz = 0; iz < downsampling; ++iz)
					for (int iy=0; iy<downsampling; ++iy)
						for (int ix = 0; ix < downsampling; ++ix)
				{
					long long idx2 = (ix + downsampling * x)
							+ resolutionX * ((iy + downsampling * y) + resolutionY * iz);
					float val = raw[idx2] / 255.0f;
					value += val;
				}
				long long idx = x + y * resolutionX;
				value /= downsampling*downsampling;
				if (value < lowerThreshold) value = 0;
				d.setValue(size_t(idx), value);
			}
		}
		else if (datatype == DATATYPE_USHORT) {
			const unsigned short* raw = reinterpret_cast<unsigned short*>(data.data());
#pragma omp parallel for
			for (int y = 0; y < resolutionY / downsampling; ++y)
				for (int x = 0; x < resolutionX / downsampling; ++x)
			{
				float value = 0;
				for (int iz = 0; iz < downsampling; ++iz)
					for (int iy=0; iy<downsampling; ++iy)
						for (int ix = 0; ix < downsampling; ++ix)
				{
					long long idx2 = (ix + downsampling * x)
							+ resolutionX * ((iy + downsampling * y) + resolutionY * iz);
					float val = raw[idx2] / 65535.0f;
					value += val;
				}
				long long idx = x + y * resolutionX/ downsampling;
				value /= downsampling*downsampling;
				if (value < lowerThreshold) value = 0;
				d.setValue(size_t(idx), value);
			}
		}

		openvdb::tools::copyFromDense(d, *grid, 0.001f, false);
	}
	printProgress("Convert", 1.0f);

#else
	std::vector<char> data(bytesToRead);
	bfile.read(&data[0], bytesToRead);
	if (!bfile)
	{
		std::cout << "Loading data file failed" << std::endl;
		return nullptr;
	}

	// convert to dense grid
	openvdb::CoordBBox gridDim(0, 0, 0, resolutionX - 1, resolutionY - 1, resolutionZ - 1);
	typedef openvdb::tools::Dense<float, openvdb::tools::LayoutXYZ> DenseT;
	DenseT d(gridDim);
	if (datatype == DATATYPE_UCHAR) {
		const unsigned char* raw = reinterpret_cast<unsigned char*>(data.data());
#pragma omp parallel for
		// I have no idea about the order in which the volume is stored.
		// Let's just try it out
		for (long long idx = 0; idx < resolutionX*resolutionY*resolutionZ; ++idx)
		{
			float value = raw[idx] / 255.0f;
			if (value < lowerThreshold) value = 0;
			d.setValue(size_t(idx), value);
		}
	}
	else if (datatype == DATATYPE_USHORT) {
		const unsigned short* raw = reinterpret_cast<unsigned short*>(data.data());
#pragma omp parallel for
		// I have no idea about the order in which the volume is stored.
		// Let's just try it out
		for (long long idx = 0; idx < resolutionX*resolutionY*resolutionZ; ++idx)
		{
			float value = raw[idx] / 65535.0f;
			if (value < lowerThreshold) value = 0;
			d.setValue(size_t(idx), value);
		}
	}

	//convert to vdb grid
	std::cout << "Build VDB grid" << std::endl;
	openvdb::FloatGrid::Ptr grid = openvdb::FloatGrid::create();
	openvdb::tools::copyFromDense(d, *grid, 0.001f, false);
#endif

	//done
	std::cout << "Done!" << std::endl;
	return grid;
}


#include <boost/numeric/odeint/iterator/const_step_iterator.hpp>


#include <boost/sort/spreadsort/string_sort.hpp>