#include "CollisionDetection.h"
#include "RigidBodyInstance.h"
#include "RigidBodyTemplate.h"
#include <Eigen/Core>
#include "VectorMath.h"
#include <Eigen/Geometry>
#include <iostream>

using namespace std;

struct BBox
{
    double mins[3];
    double maxs[3];
};

bool intersects(const BBox &b1, const BBox &b2)
{
    for(int i=0; i<3; i++)
    {
        if( (b1.maxs[i] < b2.mins[i]) || (b2.maxs[i] < b1.mins[i]))
            return false;
    }
    return true;
}

struct AABBNode
{
    AABBNode() : left(NULL), right(NULL), childtet(-1) {}
    ~AABBNode() {delete left; delete right;}

    AABBNode *left;
    AABBNode *right;
    BBox box;
    int childtet;
};

class NodeComparator
{
public:
    NodeComparator(int axis) : axis(axis) {}

    int axis;

    bool operator()(const AABBNode *left, const AABBNode *right) const
    {
        return left->box.mins[axis] < right->box.mins[axis];
    }
};

AABBNode *buildAABB(vector<AABBNode *> nodes)
{
    if(nodes.size() == 0)
        return NULL;
    else if(nodes.size() == 1)
        return nodes[0];

    double axismins[3];
    double axismaxs[3];
    for(int i=0; i<3; i++)
    {
        axismins[i] = numeric_limits<double>::infinity();
        axismaxs[i] = -numeric_limits<double>::infinity();
    }
    int nnodes = (int)nodes.size();
    for(int i=0; i<nnodes; i++)
    {
        for(int j=0; j<3; j++)
        {
            axismins[j] = min(axismins[j], nodes[i]->box.mins[j]);
            axismaxs[j] = max(axismaxs[j], nodes[i]->box.maxs[j]);
        }
    }
    double widths[3];
    for(int i=0; i<3; i++)
        widths[i] = axismaxs[i] - axismins[i];
    int splitaxis = -1;
    if(widths[0] >= widths[1] && widths[0] >= widths[2])
        splitaxis = 0;
    else if(widths[1] >= widths[0] && widths[1] >= widths[2])
        splitaxis = 1;
    else
        splitaxis = 2;
    std::sort(nodes.begin(), nodes.end(), NodeComparator(splitaxis));
    vector<AABBNode *> left(nnodes/2);
    vector<AABBNode *> right(nnodes - nnodes/2);
    for(int i=0; i<nnodes/2; i++)
    {
        left[i] = nodes[i];
    }
    for(int i=nnodes/2; i<nnodes; i++)
    {
        right[i-nnodes/2] = nodes[i];
    }
    AABBNode *node = new AABBNode;
    node->left = buildAABB(left);
    node->right = buildAABB(right);
    for(int i=0; i<3; i++)
    {
        node->box.mins[i] = min(node->left->box.mins[i], node->right->box.mins[i]);
        node->box.maxs[i] = max(node->left->box.maxs[i], node->right->box.maxs[i]);
    }
    return node;
}

void refitAABB(const RigidBodyInstance * instance, AABBNode *node)
{
    if(node->childtet != -1)
    {
        Eigen::Vector4i tet = instance->getTemplate().getTets().row(node->childtet);
        for(int k=0; k<3; k++)
        {
            node->box.mins[k] = numeric_limits<double>::infinity();
            node->box.maxs[k] = -numeric_limits<double>::infinity();
        }
        for(int k=0; k<4; k++)
        {
            Eigen::Vector3d point = instance->c + VectorMath::rotationMatrix(instance->theta)*instance->getTemplate().getVerts().row(tet[k]).transpose();
            for(int l=0; l<3; l++)
            {
                node->box.mins[l] = min(node->box.mins[l], point[l]);
                node->box.maxs[l] = max(node->box.maxs[l], point[l]);
            }
        }
    }
    else if(node->left && node->right)
    {
        refitAABB(instance, node->left);
        refitAABB(instance, node->right);
        for(int i=0; i<3; i++)
        {
            node->box.mins[i] = min(node->left->box.mins[i], node->right->box.mins[i]);
            node->box.maxs[i] = max(node->left->box.maxs[i], node->right->box.maxs[i]);
        }
    }
}

AABBNode *buildAABB(const RigidBodyInstance * instance)
{
    int ntets = (int)instance->getTemplate().getTets().rows();
    vector<AABBNode *> leaves(ntets);
    for(int j=0; j<ntets; j++)
    {
        AABBNode *leaf = new AABBNode;
        leaf->childtet = j;
        Eigen::Vector4i tet = instance->getTemplate().getTets().row(j);
        BBox box;
        for(int k=0; k<3; k++)
        {
            box.mins[k] = numeric_limits<double>::infinity();
            box.maxs[k] = -numeric_limits<double>::infinity();
        }
        for(int k=0; k<4; k++)
        {
            Eigen::Vector3d point = instance->c + VectorMath::rotationMatrix(instance->theta)*instance->getTemplate().getVerts().row(tet[k]).transpose();
            for(int l=0; l<3; l++)
            {
                box.mins[l] = min(box.mins[l], point[l]);
                box.maxs[l] = max(box.maxs[l], point[l]);
            }
        }
        leaf->box = box;
        leaves[j] = leaf;
    }
    return buildAABB(leaves);
}

bool vertInTet(const Eigen::Vector3d &p, const Eigen::Vector3d &q1, const Eigen::Vector3d &q2, const Eigen::Vector3d &q3, const Eigen::Vector3d &q4)
{
    if( (q2-p).cross(q3-p).dot(q4-p) < 0)
        return false;
    if( (p-q1).cross(q3-q1).dot(q4-q1) < 0)
        return false;
    if( (q2-q1).cross(p-q1).dot(q4-q1) < 0)
        return false;
    if( (q2-q1).cross(q3-q1).dot(p-q1) < 0)
        return false;
    return true;
}

void tetTetIntersect(const AABBNode *node1, const AABBNode *node2, int body1, int body2, const std::vector<RigidBodyInstance *> instances, std::set<Collision> &collisions)
{
    if(body1==body2)
        return;

    Eigen::Vector4i tet1 = instances[body1]->getTemplate().getTets().row(node1->childtet);
    Eigen::Vector4i tet2 = instances[body2]->getTemplate().getTets().row(node2->childtet);
    Eigen::Vector3d verts1[4];
    Eigen::Vector3d verts2[4];
    for(int i=0; i<4; i++)
    {
        verts1[i] = instances[body1]->c + VectorMath::rotationMatrix(instances[body1]->theta)*instances[body1]->getTemplate().getVerts().row(tet1[i]).transpose();
        verts2[i] = instances[body2]->c + VectorMath::rotationMatrix(instances[body2]->theta)*instances[body2]->getTemplate().getVerts().row(tet2[i]).transpose();
    }
    for(int i=0; i<4; i++)
    {
        if(vertInTet(verts1[i], verts2[0], verts2[1], verts2[2], verts2[3]))
        {
            Collision c;
            c.body1 = body1;
            c.body2 = body2;
            c.collidingVertex = tet1[i];
            c.collidingTet = node2->childtet;
            collisions.insert(c);
        }
        if(vertInTet(verts2[i], verts1[0], verts1[1], verts1[2], verts1[3]))
        {
            Collision c;
            c.body1 = body2;
            c.body2 = body1;
            c.collidingVertex = tet2[i];
            c.collidingTet = node1->childtet;
            collisions.insert(c);
        }
    }
}

void intersect(const AABBNode *node1, const AABBNode *node2, int body1, int body2, const std::vector<RigidBodyInstance *> instances, std::set<Collision> &collisions)
{
    if(!node1 || !node2)
        return;

    if(!intersects(node1->box, node2->box))
        return;

    if(node1->childtet != -1)
    {
        if(node2->childtet != -1)
        {
            tetTetIntersect(node1, node2, body1, body2, instances, collisions);
        }
        else
        {
            intersect(node1, node2->left, body1, body2, instances, collisions);
            intersect(node1, node2->right, body1, body2, instances, collisions);
        }
    }
    else
    {
        if(node2->childtet != -1)
        {
            intersect(node1->left, node2, body1, body2, instances, collisions);
            intersect(node1->right, node2, body1, body2, instances, collisions);
        }
        else
        {
            intersect(node1->left, node2->left, body1, body2, instances, collisions);
            intersect(node1->left, node2->right, body1, body2, instances, collisions);
            intersect(node1->right, node2->left, body1, body2, instances, collisions);
            intersect(node1->right, node2->right, body1, body2, instances, collisions);
        }
    }
}

void collisionDetection(const std::vector<RigidBodyInstance *> instances, std::set<Collision> &collisions)
{
    collisions.clear();
    int nbodies = (int)instances.size();

    for(int i=0; i<nbodies; i++)
    {
        refitAABB(instances[i], instances[i]->AABB);
    }

    for(int i=0; i<nbodies; i++)
    {
        for(int j=i+1; j<nbodies; j++)
        {
            intersect(instances[i]->AABB, instances[j]->AABB, i, j, instances, collisions);
        }
    }

    // floor

    for (int i=0; i<nbodies; i++)
    {
        int nverts = instances[i]->getTemplate().getVerts().rows();
        for (int j = 0; j < nverts; j++)
        {
            Eigen::Vector3d point = VectorMath::rotationMatrix(instances[i]->theta) * instances[i]->getTemplate().getVerts().row(j).transpose() + instances[i]->c;
            if (point[1] <= -1.0)
            {
                Collision c;
                c.body1 = i;
                c.body2 = -1;
                c.collidingTet = -1;
                c.collidingVertex = j;
                collisions.insert(c);
            }
        }
    }
}


#include "shortest_path.h"
#include <iostream>
#include <fstream>
#include <math.h>

#include <boost/algorithm/string.hpp>

namespace ShortestPath
{
    double eps = 1e-7;

    struct dwell
    {
        int xlabel;
        int ylabel;
        int xnode;
        int ynode;
        double overlap;
        double dist;

        dwell(int _xlabel, int _ylabel, int _xnode, int _ynode, double _overlap, double _dist)
        {
            xlabel = _xlabel;
            ylabel = _ylabel;
            xnode = _xnode;
            ynode = _ynode;
            overlap = _overlap;
            dist = _dist;
        }

        bool operator< (const dwell& that) const
        {
            return this->overlap > that.overlap || (fabs(this->overlap - that.overlap) < eps && this->dist < that.dist);
        }

        friend std::ostream& operator<< (std::ostream& out, const dwell& that)
        {
            out << "overlap " << that.xlabel << "[" << that.xnode << "] " << that.ylabel << "[" << that.ynode << "] = " << that.overlap;
            out << " distance = " << that.dist;
            return out;
        }
    };

    double euclidDistance(const Eigen::Vector3d& x, const Eigen::Vector3d& y)
    {
        Eigen::Vector3d z;
        z = x - y;
        return z.norm();
    }

    double arcDistance(const Eigen::Vector3d& x, const Eigen::Vector3d& y)
    {
        assert(fabs(x.norm() - 1.0) < 1e-7);
        assert(fabs(y.norm() - 1.0) < 1e-7);

        double costheta = x.dot(y);
        if (costheta < -1.0) costheta = -1.0;
        if (costheta > 1.0) costheta = 1.0;
        return acos(costheta);
    }

    std::vector<std::set<int>> init(const std::string& fileName)
    {
        std::ifstream poseKeypointResult("pose_keypoint_200_" + fileName + ".txt");
        std::vector<std::set<int>> visibleResult;
        std::string s;
        while (getline(poseKeypointResult, s))
        {
            std::set<int> set;
            getline(poseKeypointResult, s);
            std::vector<std::string> indices;
            boost::split(indices, s, boost::is_any_of(" "));
            for (const std::string& id: indices)
            {
                if (id.size() > 0)
                {
                    set.insert(std::stoi(id));
                }
            }
            visibleResult.push_back(set);
        }
        poseKeypointResult.close();
        return visibleResult;
    }

    double calcOverlap(const std::set<int>& A, const std::set<int>& B)
    {
        std::vector<int> i, u;
        std::set_intersection(A.begin(), A.end(), B.begin(), B.end(), std::back_inserter(i));
        std::set_union(A.begin(), A.end(), B.begin(), B.end(), std::back_inserter(u));
        return (double)i.size() / (double)u.size();
    }

    std::vector<Eigen::Vector3d> getView(const Eigen::Vector3d& camera_normal, int horizontal_count, int vertical_count)
    {
        std::vector<Eigen::Vector3d> views;

        double horizontal_angle = 2 * M_PI / horizontal_count;
        double vertical_angle = M_PI / (vertical_count - 1);

        for (int i = 0; i < vertical_count; ++i)
        {
            for (int j = 0; j < horizontal_count; ++j)
            {
                if ((i == 0 || i == vertical_count - 1) && j)
                {
                    continue;
                }

                double horizontal_spin = horizontal_angle * j;
                double vertical_spin = vertical_angle * i;

                Eigen::Matrix3d rotation, rotation_inv;
                rotation = Eigen::AngleAxisd(vertical_spin, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(horizontal_spin, camera_normal);
                rotation_inv = rotation.inverse();
                views.push_back(rotation_inv * camera_normal);
            }
        }
        return views;
    }

    void execute(const std::string& fileName)
    {
        std::cout << "name = " << fileName << std::endl;
        std::vector<std::set<int>> group = init(fileName);
        std::vector<Eigen::Vector3d> views = getView(Eigen::Vector3d(0, 0, -1), 18, 13);

        std::vector<int> selected_views = {31, 23, 84, 57, 94, 148, 179, 171, 50, 35, 26};

        int ecnt = 0;
        std::vector<dwell> dummy;

        int n = selected_views.size();
        double** g = new double*[n];
        for (int i = 0; i < n; ++i)
        {
            g[i] = new double[n];
        }
        double** f = new double*[1 << n];
        for (int i = 0; i < (1 << n); ++i)
        {
            f[i] = new double[n];
            for (int j = 0; j < n; ++j)
            {
                f[i][j] = 1e10;
            }
        }
        int** fa = new int*[1 << n];
        for (int i = 0; i < (1 << n); ++i)
        {
            fa[i] = new int[n];
            for (int j = 0; j < n; ++j)
            {
                fa[i][j] = -1;
            }
        }

        for (int i = 0; i < selected_views.size(); ++i)
        {
            g[i][i] = 0.0;
            for (int j = i + 1; j < selected_views.size(); ++j)
            {
                int x = selected_views[i];
                int y = selected_views[j];
                double overlap = calcOverlap(group[x], group[y]);
                double dist = arcDistance(views[x], views[y]);
                g[i][j] = g[j][i] = dist;
                /* if (overlap + eps >= 0.5 || dist <= 2.0 + eps)
                {
                    ++ecnt;
                    std::cout << "overlap " << i << "[" << x << "] " << j << "[" << y << "] = ";
                    std::cout << calcOverlap(group[x], group[y]) << "(" << group[x].size() << ", " << group[y].size() << ")";
                    std::cout << " distance = " << arcDistance(views[x], views[y]);
                    std::cout << std::endl;
                } */
                dummy.push_back(dwell(i, j, x, y, overlap, dist));
            }
        }
        std::sort(dummy.begin(), dummy.end());
        for (const dwell& dd: dummy)
        {
            std::cout << dd << std::endl;
        }

        // std::cout << "edge count = " << ecnt << std::endl;

        /*for (int selected_view: selected_views)
        {
            Eigen::Vector3d vv = views[selected_view];
            std::cout << "view " << selected_view << ": [" << vv[0] << ", " << vv[1] << ", " << vv[2] << "]" << std::endl;
        }*/

        // TSP

        for (int i = 0; i < n; ++i)
        {
            f[0][i] = 0.0;
        }
        for (int i = 1; i < (1 << n); ++i)
        {
            for (int v = 0; v < n; ++v)
            {
                if (i & (1 << v))
                {
                    // f[i][v] = ...
                    for (int u = 0; u < n; ++u)
                    {
                        double dist = f[i - (1 << v)][u] + g[u][v];
                        if (dist < f[i][v])
                        {
                            f[i][v] = dist;
                            fa[i][v] = u;
                        }
                    }
                }
            }
        }

        std::vector<int> answer;
        double mindist = 1e10;
        int where = -1;
        for (int i = 0; i < n; ++i)
        {
            if (f[(1 << n) - 1][i] < mindist)
            {
                mindist = f[(1 << n) - 1][i];
                where = i;
            }
        }

        int curbin = (1 << n) - 1;
        int where0 = where;
        while (curbin)
        {
            answer.push_back(where0);
            int nxt = fa[curbin][where0];
            curbin -= (1 << where0);
            where0 = nxt;
        }

        std::cout << "minimum distance = " << f[(1 << n) - 1][where] << std::endl;
        for (int i = 0; i < answer.size() - 1; ++i)
        {
            int x = selected_views[answer[i]];
            int y = selected_views[answer[i + 1]];
            std::cout << answer[i] << "(" << x << ") " << answer[i + 1] << "(" << y << ")";
            std::cout << " dis = " << g[answer[i]][answer[i + 1]] << " overlap = " << calcOverlap(group[x], group[y]);
            std::cout << std::endl;
        }

    }
}


// Software License Agreement (BSD-3-Clause)
//
// Copyright 2018 The University of North Carolina at Chapel Hill
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.

//! @author Jeff Ichnowski

#pragma once
#ifndef NIGH_IMPL_KDTREE_MEDIAN_TRAVERSAL_SO3_HPP
#define NIGH_IMPL_KDTREE_MEDIAN_TRAVERSAL_SO3_HPP

#include "traversal.hpp"
#include "so3_root.hpp"
#include "so3_branch.hpp"
#include "../so3.hpp"
#include <Eigen/Dense>

namespace unc::robotics::nigh::impl::kdtree_median {
    template <class Tree, class Key, class Get>
    class Traversal<Tree, Key, metric::SO3, Get> {
        using Metric = metric::SO3;
        using Space = metric::Space<Key, Metric>;
        using Distance = typename Space::Distance;

        int vol_{-1};
        Distance dotMax_{0};
        Distance distToRegion_{0};
        Eigen::Matrix<Distance, 2, 3> dots_;

        void initVol(const Key& key) {
            Distance qv = Space::coeff(key, vol_);
            Distance s = qv < 0 ? -SQRT1_2<Distance> : SQRT1_2<Distance>;
            dotMax_ = 0;
            for (unsigned a=0 ; a<3 ; ++a) {
                Distance qa = Space::coeff(key, (vol_ + a + 1)%4);
                dots_(0, a) = (qa + qv) * s;
                dots_(1, a) = (qa - qv) * s;
                if (dots_(0, a) < 0 || dots_(1, a) > 0)
                    dotMax_ = std::max(dotMax_, dots_.col(a).cwiseAbs().minCoeff());
            }
            distToRegion_ = std::asin(dotMax_);
        }

        template <class Nearest, class Iter>
        void recur(
            Nearest& nearest, const Node *child, unsigned axis, unsigned splitIndex,
            Distance dot, Iter first, Iter last)
        {
            Distance oldDist = distToRegion_;
            Distance oldDotMax = dotMax_;
            Distance oldDot = dots_(splitIndex, axis);

            dots_(splitIndex, axis) = dot;
            if (dots_(0, axis) < 0 || dots_(1, axis) > 0) {
                Distance minDot = dots_.col(axis).cwiseAbs().minCoeff();
                if (minDot > oldDotMax)
                    distToRegion_ = std::asin(dotMax_ = minDot);
            }

            nearest(child, first, last);

            dots_(splitIndex, axis) = oldDot;
            dotMax_ = oldDotMax;
            distToRegion_ = oldDist;
        }

    public:
        Traversal(const Space&) {
        }

        Distance distToRegion() const {
            return distToRegion_;
        }

        template <typename Nearest, typename Iter>
        void follow(
            Nearest& nearest,
            const Space&,
            const Node *node, unsigned axis,
            const Key& key,
            Iter first, Iter last)
        {
            if (vol_ < 0) {
                const auto* root = static_cast<const SO3Root*>(node);
                unsigned keyVol = so3::volIndex(key);
                for (unsigned i=0 ; i<4 ; ++i) {
                    vol_ = (keyVol + i) % 4;
                    initVol(key);
                    nearest(root->child(vol_),
                            vol_ == 0 ? first : (first + root->offset(vol_-1)),
                            vol_ == 3 ? last : (first + root->offset(vol_)));
                }
                vol_ = -1;
                distToRegion_ = 0;
            } else {
                const auto* branch = static_cast<const SO3Branch<Distance>*>(node);
                std::array<Iter, 3> iters{{first, first + std::distance(first, last)/2, last}};
                Distance dot = so3::dotSplit(branch->split(), key, vol_, axis);
                int childNo = dot > 0;
                recur(nearest, branch->child(childNo), axis, !childNo, dot,
                      iters[childNo], iters[childNo+1]);
                recur(nearest, branch->child(!childNo), axis, childNo, dot,
                      iters[1-childNo], iters[2-childNo]);
            }
        }
    };
}

#endif


#ifndef CYLINDER_H
#define CYLINDER_H

//PointCloud#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/surface/concave_hull.h>

#include <Eigen/Core>
#include <boost/shared_ptr.hpp>
#include "sophus/sim3.hpp"
#include "typedef.h"

// ------------------------- Cylinder -------------------------
class Cylinder {

public:
    typedef boost::shared_ptr<Cylinder> Ptr;

    Cylinder(std::vector<float> coefficients)
    {
        if (coefficients.size() != 7) {
            ROS_ERROR("Cylinder with wrong number of coefficients created");
            return;
        }
        point_ = Eigen::Vector4f(coefficients[0], coefficients[1], coefficients[2], 1);
        dir_ = Eigen::Vector4f(coefficients[3], coefficients[4], coefficients[5], 0);
        radius_ = coefficients[6];
    }

    Eigen::VectorXf getCoefficients()
    {
        Eigen::VectorXf coeff(7);
        coeff << point_[0], point_[1], point_[2], dir_[0], dir_[1], dir_[2], radius_;
        return coeff;
    }

    void setCoefficients(const Eigen::VectorXf coefficients)
    {
        if (coefficients.size() != 7) {
            ROS_ERROR("Cylinder with wrong number of coefficients created");
            return;
        }
        point_ = Eigen::Vector4f(coefficients[0], coefficients[1], coefficients[2], 1);
        dir_ = Eigen::Vector4f(coefficients[3], coefficients[4], coefficients[5], 0);
        radius_ = coefficients[6];
    }

    Eigen::Quaternionf getRotation()
    {
        return Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f(0.0f,0.0f,1.0f), Eigen::Vector3f(dir_.x(), dir_.y(), dir_.z())).normalized();
    }

    void transform(const Eigen::Matrix4f &transform)
    {
        point_ = transform * point_;
        dir_ = transform * dir_;
    }

    static void transformCylinder(const Eigen::Matrix4f transform, Eigen::VectorXf &coefficients) {
        if (coefficients.size() != 7) {
            ROS_ERROR("Cylinder with wrong number of coefficients transformed");
            return;
        }
        Eigen::Vector4f point(coefficients[0], coefficients[1], coefficients[2], 1);
        Eigen::Vector4f dir(coefficients[3], coefficients[4], coefficients[5], 0);
        float radius = coefficients[6];

        point = transform * point;
        dir = transform * dir;

        Sophus::Sim3f scaleTransform;
        memcpy(scaleTransform.data(), transform.data(), 7*sizeof(float));
        radius = scaleTransform.scale() * radius;

        for (int i=0; i<3; i++) {
            coefficients[i] = point[i];
            coefficients[i+3] = dir[i];
        }
        coefficients[6] = radius;
    }

    // returns closest point to point_in on the plane
    Eigen::Vector3f rayIntersection(Eigen::Vector3f point_in, Eigen::Vector3f direction_in) //TODO
    {
//         direction_in.normalize();
//         Eigen::Vector3f planeNormal;
//         Eigen::Vector3f planePoint;
//         calculateNormalForm(planePoint, planeNormal);
//         EigenPlane plane = EigenPlane(planeNormal, planePoint);
//         Eigen::ParametrizedLine<float,3> pline = Eigen::ParametrizedLine<float,3>(point_in, direction_in);
//         Eigen::Vector3f intersection = pline.intersectionPoint(plane);
//         return intersection;
    }

private:

// Member variables ----------
private:
    Eigen::Vector4f point_, dir_;
    float radius_;

};

#endif //CYLINDER_H


#include "minear.hpp"
#include "debug.hpp"
#include <armadillo>
#include <cassert>

using namespace arma;
using namespace std;

int main()
{
    const int max_iters = 1000;
    int outter_a, inner, outter_b;
    mat arma_A, arma_B, arma_C, mult_result_arma, add_result_arma;
    minear::Matrix<double> min_A, min_B, min_C, mult_result_min, add_result_min;

    cout << "Max iterations: " << max_iters << endl;
    cout << "Starting tests..." << endl;

    for (unsigned int i = 0; i < max_iters; i++)
    {
        cout << endl << "iter " << i+1 << endl;
        cout << "-----------------------------" << endl;
        outter_a = rand() % 100 + 1;
        inner = rand() % 100 + 1;
        outter_b = rand() % 100 + 1;

        arma_A = mat(outter_a, inner);
        arma_A.randn();

        arma_B = mat(inner, outter_b);
        arma_B.randu();

        arma_C = mat(outter_a, inner);
        arma_C.randu();

        cout << "resizing" << endl;
        min_A.resize(outter_a, inner);
        min_B.resize(inner, outter_b);
        min_C.resize(outter_a, inner);

        cout << "copying" << endl;
        for (unsigned int i = 0; i < outter_a; i++)
            for (unsigned int j = 0; j < inner; j++)
                min_A(i,j) = arma_A(i,j);

        for (unsigned int i = 0; i < inner; i++)
            for (unsigned int j = 0; j < outter_b; j++)
                min_B(i,j) = arma_B(i,j);

        for (unsigned int i = 0; i < outter_a; i++)
            for (unsigned int j = 0; j < inner; j++)
                min_C(i,j) = arma_C(i,j);

        cout << "computations" << endl;

        cout << "multiply" << endl;
        mult_result_arma = arma_A*arma_B;
        mult_result_min = min_A*min_B;
        for (unsigned int i = 0; i < mult_result_arma.n_rows; i++)
            for (unsigned int j = 0; j < mult_result_arma.n_cols; j++)
                ASSERT_NEAR(mult_result_min(i,j), mult_result_arma(i,j), 1e-3);

        cout << "add" << endl;
        add_result_arma = arma_A+arma_C;
        add_result_min = min_A+min_C;
        for (unsigned int i = 0; i < add_result_arma.n_rows; i++)
            for (unsigned int j = 0; j < add_result_arma.n_cols; j++)
                ASSERT_NEAR(add_result_min(i,j), add_result_arma(i,j), 1e-3);

        cout << "subtract" << endl;
        add_result_arma = arma_A-arma_C;
        add_result_min = min_A-min_C;
        for (unsigned int i = 0; i < add_result_arma.n_rows; i++)
            for (unsigned int j = 0; j < add_result_arma.n_cols; j++)
                ASSERT_NEAR(add_result_min(i,j), add_result_arma(i,j), 1e-3);

        cout << "subtract" << endl;
        add_result_arma = arma_C-arma_A;
        add_result_min = min_C-min_A;
        for (unsigned int i = 0; i < add_result_arma.n_rows; i++)
            for (unsigned int j = 0; j < add_result_arma.n_cols; j++)
                ASSERT_NEAR(add_result_min(i,j), add_result_arma(i,j), 1e-3);

        cout << "subtract" << endl;
        add_result_arma = arma_C-arma_C;
        add_result_min = min_C-min_C;
        for (unsigned int i = 0; i < add_result_arma.n_rows; i++)
            for (unsigned int j = 0; j < add_result_arma.n_cols; j++)
                ASSERT_NEAR(add_result_min(i,j), add_result_arma(i,j), 1e-3);
    }

    cout << "... tests complete." << endl;

    return 0;
}


/*  raycasting %{Cpp:License:ClassName} - Yann BOUCHER (yann) 12/04/2016
**
**
**            DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE
**                    Version 2, December 2004
**
** Copyright (C) 2004 Sam Hocevar <sam@hocevar.net>
**
** Everyone is permitted to copy and distribute verbatim or modified
** copies of this license document, and changing it is allowed as long
** as the name is changed.
**
**            DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE
**   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
**
**  0. You just DO WHAT THE FUCK YOU WANT TO.
*/
#ifndef RAYCASTING_HPP
#define RAYCASTING_HPP

#include <functional>
#include <vector>

#include <boost/optional.hpp>

#include <SFML/Config.hpp>

#include "common.hpp"

#include "tile.hpp"

namespace Rayfun
{

class Camera;
class Map;
class DrawableActor;

namespace Raycasting
{

struct RaycastResult
{
        const Side side { Side::North };
        boost::optional<Tile&> tileHit {};
        const sf::Vector2d hitPos {};
        const sf::Vector2i mapPos {};
        const double distance { 0 };
        const double perpDistance { 0 };
};

enum class HitMode
{
    Visibility,
    Clipping
};

class raycasting_error : public std::runtime_error
{
        using std::runtime_error::runtime_error;
};

[[noreturn]] inline void throw_raycasting_error(const std::string& t_why)
{
    throw raycasting_error("Error during : " + t_why);
}

bool rayIntersectsSprite(const sf::Vector2d &t_begin, const sf::Vector2d &t_dir, Map &t_map,
                         DrawableActor *&t_hitSprite, size_t t_maxDistance = 9999999999);

std::vector<RaycastResult> castRay(const sf::Vector2d& t_begin, const sf::Vector2d& t_dir, Map &t_map,
                                   HitMode t_hitmode = HitMode::Visibility);

std::vector<sf::Uint8> render(const Camera& t_cam, const Map& t_map,
                              bool bilinear_filtering = false, bool t_bilinear_sprites = false);

namespace detail
{
void spriteCasting(const Camera& t_cam, const std::vector<std::unique_ptr<DrawableActor>>& t_sprites,
                   const std::vector<double>& t_zbuffer, bool t_bilinear_sprites, unsigned char t_ambientLight,
                   std::vector<sf::Uint8>& t_image);

void floorCasting(const Camera& t_cam, const Map& t_map, const RaycastResult& t_result, double t_wallX,
                  bool t_bilinear_filtering, long t_drawEnd, size_t t_screenX,
                  std::vector<sf::Uint8>& t_image);

sf::Color pixelIntensity(sf::Color t_pixel, double t_dist);
}

}

}

#endif // RAYCASTING_HPP


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_translation_hpp__
#define __pinocchio_joint_translation_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/skew.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options=0> struct MotionTranslationTpl;
  typedef MotionTranslationTpl<double> MotionTranslation;

  template<typename Scalar, int Options>
  struct SE3GroupAction< MotionTranslationTpl<Scalar,Options> >
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< MotionTranslationTpl<Scalar,Options>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename _Scalar, int _Options>
  struct traits< MotionTranslationTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits MotionTranslationTpl

  template<typename _Scalar, int _Options>
  struct MotionTranslationTpl
  : MotionBase< MotionTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    MOTION_TYPEDEF_TPL(MotionTranslationTpl);

    MotionTranslationTpl() {}

    template<typename Vector3Like>
    MotionTranslationTpl(const Eigen::MatrixBase<Vector3Like> & v)
    : m_v(v)
    {}

    MotionTranslationTpl(const MotionTranslationTpl & other)
    : m_v(other.m_v)
    {}

    Vector3 & operator()() { return m_v; }
    const Vector3 & operator()() const { return m_v; }

    inline PlainReturnType plain() const
    {
      return PlainReturnType(m_v,PlainReturnType::Vector3::Zero());
    }

    bool isEqual_impl(const MotionTranslationTpl & other) const
    {
      return m_v == other.m_v;
    }

    MotionTranslationTpl & operator=(const MotionTranslationTpl & other)
    {
      m_v = other.m_v;
      return *this;
    }

    template<typename Derived>
    void addTo(MotionDense<Derived> & other) const
    {
      other.linear() += m_v;
    }

    template<typename Derived>
    void setTo(MotionDense<Derived> & other) const
    {
      other.linear() = m_v;
      other.angular().setZero();
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.angular().setZero();
      v.linear().noalias() = m.rotation() * m_v; // TODO: check efficiency
    }

    template<typename S2, int O2>
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3Action_impl(m,res);
      return res;
    }

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Linear
      v.linear().noalias() = m.rotation().transpose() * m_v;

      // Angular
      v.angular().setZero();
    }

    template<typename S2, int O2>
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3ActionInverse_impl(m,res);
      return res;
    }

    template<typename M1, typename M2>
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
      mout.linear().noalias() = v.angular().cross(m_v);

      // Angular
      mout.angular().setZero();
    }

    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }

    const Vector3 & linear() const { return m_v; }
    Vector3 & linear() { return m_v; }

  protected:

    Vector3 m_v;

  }; // struct MotionTranslationTpl

  template<typename S1, int O1, typename MotionDerived>
  inline typename MotionDerived::MotionPlain
  operator+(const MotionTranslationTpl<S1,O1> & m1,
            const MotionDense<MotionDerived> & m2)
  {
    return typename MotionDerived::MotionPlain(m2.linear() + m1.linear(), m2.angular());
  }

  template<typename Scalar, int Options> struct TransformTranslationTpl;

  template<typename _Scalar, int _Options>
  struct traits< TransformTranslationTpl<_Scalar,_Options> >
  {
    enum {
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef SE3Tpl<Scalar,Options> PlainType;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef typename Matrix3::IdentityReturnType AngularType;
    typedef AngularType AngularRef;
    typedef AngularType ConstAngularRef;
    typedef Vector3 LinearType;
    typedef LinearType & LinearRef;
    typedef const LinearType & ConstLinearRef;
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
  }; // traits TransformTranslationTpl

  template<typename Scalar, int Options>
  struct SE3GroupAction< TransformTranslationTpl<Scalar,Options> >
  { typedef typename traits <TransformTranslationTpl<Scalar,Options> >::PlainType ReturnType; };

  template<typename _Scalar, int _Options>
  struct TransformTranslationTpl
  : SE3Base< TransformTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_SE3_TYPEDEF_TPL(TransformTranslationTpl);
    typedef typename traits<TransformTranslationTpl>::Vector3 Vector3;

    TransformTranslationTpl() {}

    template<typename Vector3Like>
    TransformTranslationTpl(const Eigen::MatrixBase<Vector3Like> & translation)
    : m_translation(translation)
    {}

    PlainType plain() const
    {
      PlainType res(PlainType::Identity());
      res.rotation().setIdentity();
      res.translation() = translation();

      return res;
    }

    operator PlainType() const { return plain(); }

    template<typename S2, int O2>
    typename SE3GroupAction<TransformTranslationTpl>::ReturnType
    se3action(const SE3Tpl<S2,O2> & m) const
    {
      typedef typename SE3GroupAction<TransformTranslationTpl>::ReturnType ReturnType;
      ReturnType res(m);
      res.translation() += translation();

      return res;
    }

    ConstLinearRef translation() const { return m_translation; }
    LinearRef translation() { return m_translation; }

    AngularType rotation() const { return AngularType(3,3); }

    bool isEqual(const TransformTranslationTpl & other) const
    {
      return m_translation == other.m_translation;
    }

  protected:

    LinearType m_translation;
  };

  template<typename Scalar, int Options> struct ConstraintTranslationTpl;

  template<typename _Scalar, int _Options>
  struct traits< ConstraintTranslationTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;

    enum { Options = _Options };
    enum { LINEAR = 0, ANGULAR = 3 };

    typedef MotionTranslationTpl<Scalar,Options> JointMotion;
    typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;

    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
  }; // traits ConstraintTranslationTpl

  template<typename _Scalar, int _Options>
  struct ConstraintTranslationTpl
  : ConstraintBase< ConstraintTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTranslationTpl)

    enum { NV = 3 };

    ConstraintTranslationTpl() {}

//    template<typename S1, int O1>
//    Motion operator*(const MotionTranslationTpl<S1,O1> & vj) const
//    { return Motion(vj(), Motion::Vector3::Zero()); }

    template<typename Vector3Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
      return JointMotion(v);
    }

    int nv_impl() const { return NV; }

    struct ConstraintTranspose
    {
      const ConstraintTranslationTpl & ref;
      ConstraintTranspose(const ConstraintTranslationTpl & ref) : ref(ref) {}

      template<typename Derived>
      typename ForceDense<Derived>::ConstLinearType
      operator* (const ForceDense<Derived> & phi)
      {
        return phi.linear();
      }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename MatrixDerived>
      const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
      operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
      {
        assert(F.rows()==6);
        return F.derived().template middleRows<3>(LINEAR);
      }

    }; // struct ConstraintTranspose

    ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }

    DenseBase matrix_impl() const
    {
      DenseBase S;
      S.template middleRows<3>(LINEAR).setIdentity();
      S.template middleRows<3>(ANGULAR).setZero();
      return S;
    }

    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<S1,6,3,O1> M;
      M.template middleRows<3>(LINEAR) = m.rotation();
      M.template middleRows<3>(ANGULAR).setZero();

      return M;
    }

    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<S1,6,3,O1> M;
      M.template middleRows<3>(LINEAR) = m.rotation().transpose();
      M.template middleRows<3>(ANGULAR).setZero();

      return M;
    }

    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & m) const
    {
      const typename MotionDerived::ConstAngularType w = m.angular();

      DenseBase res;
      skew(w,res.template middleRows<3>(LINEAR));
      res.template middleRows<3>(ANGULAR).setZero();

      return res;
    }

    bool isEqual(const ConstraintTranslationTpl &) const { return true; }

  }; // struct ConstraintTranslationTpl

  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
            const MotionTranslationTpl<S2,O2> & m2)
  {
    return m2.motionAction(m1);
  }

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template<typename S1, int O1, typename S2, int O2>
  inline Eigen::Matrix<S2,6,3,O2>
  operator*(const InertiaTpl<S1,O1> & Y,
            const ConstraintTranslationTpl<S2,O2> &)
  {
    typedef ConstraintTranslationTpl<S2,O2> Constraint;
    Eigen::Matrix<S2,6,3,O2> M;
    alphaSkew(Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::ANGULAR));
    M.template middleRows<3>(Constraint::LINEAR).setZero();
    M.template middleRows<3>(Constraint::LINEAR).diagonal().fill(Y.mass ());

    return M;
  }

  /* [ABA] Y*S operator*/
  template<typename M6Like, typename S2, int O2>
  inline const typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
  operator*(const Eigen::MatrixBase<M6Like> & Y,
            const ConstraintTranslationTpl<S2,O2> &)
  {
    typedef ConstraintTranslationTpl<S2,O2> Constraint;
    return Y.derived().template middleCols<3>(Constraint::LINEAR);
  }

  template<typename S1, int O1>
  struct SE3GroupAction< ConstraintTranslationTpl<S1,O1> >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };

  template<typename S1, int O1, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintTranslationTpl<S1,O1>,MotionDerived >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };

  template<typename Scalar, int Options> struct JointTranslationTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointTranslationTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 3,
      NV = 3
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataTranslationTpl<Scalar,Options> JointDataDerived;
    typedef JointModelTranslationTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintTranslationTpl<Scalar,Options> Constraint_t;
    typedef TransformTranslationTpl<Scalar,Options> Transformation_t;
    typedef MotionTranslationTpl<Scalar,Options> Motion_t;
    typedef MotionZeroTpl<Scalar,Options> Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  }; // traits JointTranslationTpl

  template<typename Scalar, int Options>
  struct traits< JointDataTranslationTpl<Scalar,Options> >
  { typedef JointTranslationTpl<Scalar,Options> JointDerived; };

  template<typename Scalar, int Options>
  struct traits< JointModelTranslationTpl<Scalar,Options> >
  { typedef JointTranslationTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataTranslationTpl
  : public JointDataBase< JointDataTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointTranslationTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataTranslationTpl()
    : M(Transformation_t::Vector3::Zero())
    , v(Motion_t::Vector3::Zero())
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}

    static std::string classname() { return std::string("JointDataTranslation"); }
    std::string shortname() const { return classname(); }
  }; // struct JointDataTranslationTpl

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTranslationTpl);
  template<typename _Scalar, int _Options>
  struct JointModelTranslationTpl
  : public JointModelBase< JointModelTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointTranslationTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    typedef JointModelBase<JointModelTranslationTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    JointDataDerived createData() const { return JointDataDerived(); }

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      data.M.translation() = this->jointConfigSelector(qs);
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      data.v.linear() = this->jointVelocitySelector(vs);
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
      data.U = I.template middleCols<3>(Inertia::LINEAR);

      // compute inverse
//      data.Dinv.setIdentity();
//      data.U.template middleRows<3>(Inertia::LINEAR).llt().solveInPlace(data.Dinv);
      internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::LINEAR),data.Dinv);

      data.UDinv.template middleRows<3>(Inertia::LINEAR).setIdentity(); // can be put in data constructor
      data.UDinv.template middleRows<3>(Inertia::ANGULAR).noalias() = data.U.template middleRows<3>(Inertia::ANGULAR) * data.Dinv;

      if (update_I)
      {
        Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
        I_.template block<3,3>(Inertia::ANGULAR,Inertia::ANGULAR)
        -= data.UDinv.template middleRows<3>(Inertia::ANGULAR) * I_.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR);
        I_.template middleCols<3>(Inertia::LINEAR).setZero();
        I_.template block<3,3>(Inertia::LINEAR,Inertia::ANGULAR).setZero();
      }
    }

    static std::string classname() { return std::string("JointModelTranslation"); }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelTranslationTpl<NewScalar,Options> cast() const
    {
      typedef JointModelTranslationTpl<NewScalar,Options> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

  }; // struct JointModelTranslationTpl

} // namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelTranslationTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelTranslationTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataTranslationTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataTranslationTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_translation_hpp__


// Boost.Geometry

// Copyright (c) 2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_MULTI_POINT_GEOMETRY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_MULTI_POINT_GEOMETRY_HPP

#include <boost/range.hpp>

#include <boost/geometry/algorithms/detail/disjoint/box_box.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_box.hpp>
#include <boost/geometry/algorithms/detail/expand_by_epsilon.hpp>
#include <boost/geometry/algorithms/detail/relate/result.hpp>
#include <boost/geometry/algorithms/detail/relate/topology_check.hpp>
#include <boost/geometry/algorithms/detail/within/point_in_geometry.hpp>
#include <boost/geometry/algorithms/envelope.hpp>

#include <boost/geometry/core/is_areal.hpp>
#include <boost/geometry/core/point_type.hpp>

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

#include <boost/geometry/index/rtree.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace relate
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct multi_point_geometry_eb
{
    template <typename MultiPoint>
    static inline bool apply(MultiPoint const& ,
                             detail::relate::topology_check<Geometry> const& )
    {
        return true;
    }
};

template <typename Geometry>
struct multi_point_geometry_eb<Geometry, linestring_tag>
{
    template <typename Points>
    struct boundary_visitor
    {
        boundary_visitor(Points const& points)
            : m_points(points)
            , m_boundary_found(false)
        {}

        template <typename Point>
        struct find_pred
        {
            find_pred(Point const& point)
                : m_point(point)
            {}

            template <typename Pt>
            bool operator()(Pt const& pt) const
            {
                return detail::equals::equals_point_point(pt, m_point);
            }

            Point const& m_point;
        };

        template <typename Point>
        bool apply(Point const& boundary_point)
        {
            if (std::find_if(m_points.begin(), m_points.end(), find_pred<Point>(boundary_point)) == m_points.end())
            {
                m_boundary_found = true;
                return false;
            }
            return true;
        }

        bool result() const { return m_boundary_found; }

    private:
        Points const& m_points;
        bool m_boundary_found;
    };

    template <typename MultiPoint>
    static inline bool apply(MultiPoint const& multi_point,
                             detail::relate::topology_check<Geometry> const& tc)
    {
        boundary_visitor<MultiPoint> visitor(multi_point);
        tc.for_each_boundary_point(visitor);
        return visitor.result();
    }
};

template <typename Geometry>
struct multi_point_geometry_eb<Geometry, multi_linestring_tag>
{
    template <typename Points>
    struct boundary_visitor
    {
        boundary_visitor(Points const& points)
            : m_points(points)
            , m_boundary_found(false)
        {}

        template <typename Point>
        bool apply(Point const& boundary_point)
        {
            if (! std::binary_search(m_points.begin(), m_points.end(), boundary_point, geometry::less<>()))
            {
                m_boundary_found = true;
                return false;
            }
            return true;
        }

        bool result() const { return m_boundary_found; }

    private:
        Points const& m_points;
        bool m_boundary_found;
    };

    template <typename MultiPoint>
    static inline bool apply(MultiPoint const& multi_point,
                             detail::relate::topology_check<Geometry> const& tc)
    {
        typedef typename boost::range_value<MultiPoint>::type point_type;
        typedef std::vector<point_type> points_type;
        points_type points(boost::begin(multi_point), boost::end(multi_point));
        std::sort(points.begin(), points.end(), geometry::less<>());

        boundary_visitor<points_type> visitor(points);
        tc.for_each_boundary_point(visitor);
        return visitor.result();
    }
};

// SingleGeometry - Linear or Areal
template <typename MultiPoint, typename SingleGeometry, bool Transpose = false>
struct multi_point_single_geometry
{
    static const bool interruption_enabled = true;

    template <typename Result, typename Strategy>
    static inline void apply(MultiPoint const& multi_point,
                             SingleGeometry const& single_geometry,
                             Result & result,
                             Strategy const& strategy)
    {
        typedef typename point_type<SingleGeometry>::type point2_type;
        typedef model::box<point2_type> box2_type;

        box2_type box2;
        geometry::envelope(single_geometry, box2, strategy.get_envelope_strategy());
        geometry::detail::expand_by_epsilon(box2);

        typedef typename boost::range_const_iterator<MultiPoint>::type iterator;
        for ( iterator it = boost::begin(multi_point) ; it != boost::end(multi_point) ; ++it )
        {
            if (! (relate::may_update<interior, interior, '0', Transpose>(result)
                || relate::may_update<interior, boundary, '0', Transpose>(result)
                || relate::may_update<interior, exterior, '0', Transpose>(result) ) )
            {
                break;
            }

            // The default strategy is enough for Point/Box
            if (detail::disjoint::disjoint_point_box(*it, box2))
            {
                relate::set<interior, exterior, '0', Transpose>(result);
            }
            else
            {
                int in_val = detail::within::point_in_geometry(*it, single_geometry, strategy);

                if (in_val > 0) // within
                {
                    relate::set<interior, interior, '0', Transpose>(result);
                }
                else if (in_val == 0)
                {
                    relate::set<interior, boundary, '0', Transpose>(result);
                }
                else // in_val < 0 - not within
                {
                    relate::set<interior, exterior, '0', Transpose>(result);
                }
            }

            if ( BOOST_GEOMETRY_CONDITION(result.interrupt) )
            {
                return;
            }
        }

        typedef detail::relate::topology_check<SingleGeometry> tc_t;
        if ( relate::may_update<exterior, interior, tc_t::interior, Transpose>(result)
          || relate::may_update<exterior, boundary, tc_t::boundary, Transpose>(result) )
        {
            tc_t tc(single_geometry);

            if ( relate::may_update<exterior, interior, tc_t::interior, Transpose>(result)
              && tc.has_interior() )
            {
                // TODO: this is not true if a linestring is degenerated to a point
                // then the interior has topological dimension = 0, not 1
                relate::set<exterior, interior, tc_t::interior, Transpose>(result);
            }

            if ( relate::may_update<exterior, boundary, tc_t::boundary, Transpose>(result)
              && tc.has_boundary() )
            {
                if (multi_point_geometry_eb<SingleGeometry>::apply(multi_point, tc))
                    relate::set<exterior, boundary, tc_t::boundary, Transpose>(result);
            }
        }

        relate::set<exterior, exterior, result_dimension<MultiPoint>::value, Transpose>(result);
    }
};

// MultiGeometry - Linear or Areal
// part of the algorithm calculating II and IB when no IE has to be calculated
// using partition()
template <typename MultiPoint, typename MultiGeometry, bool Transpose>
class multi_point_multi_geometry_ii_ib
{
    struct expand_box_point
    {
        template <typename Box, typename Point>
        static inline void apply(Box& total, Point const& point)
        {
            geometry::expand(total, point);
        }
    };

    struct expand_box_box_pair
    {
        template <typename Box, typename BoxPair>
        static inline void apply(Box& total, BoxPair const& box_pair)
        {
            geometry::expand(total, box_pair.first);
        }
    };

    struct overlaps_box_point
    {
        template <typename Box, typename Point>
        static inline bool apply(Box const& box, Point const& point)
        {
            // The default strategy is enough for Point/Box
            return ! detail::disjoint::disjoint_point_box(point, box);
        }
    };

    struct overlaps_box_box_pair
    {
        template <typename Box, typename BoxPair>
        static inline bool apply(Box const& box, BoxPair const& box_pair)
        {
            // The default strategy is enough for Box/Box
            return ! detail::disjoint::disjoint_box_box(box_pair.first, box);
        }
    };

    template <typename Result, typename PtSegStrategy>
    class item_visitor_type
    {
    public:
        item_visitor_type(MultiGeometry const& multi_geometry,
                          detail::relate::topology_check<MultiGeometry> const& tc,
                          Result & result,
                          PtSegStrategy const& strategy)
            : m_multi_geometry(multi_geometry)
            , m_tc(tc)
            , m_result(result)
            , m_strategy(strategy)
        {}

        template <typename Point, typename BoxPair>
        inline bool apply(Point const& point, BoxPair const& box_pair)
        {
            // The default strategy is enough for Point/Box
            if (! detail::disjoint::disjoint_point_box(point, box_pair.first))
            {
                typename boost::range_value<MultiGeometry>::type const&
                    single = range::at(m_multi_geometry, box_pair.second);

                int in_val = detail::within::point_in_geometry(point, single, m_strategy);

                if (in_val > 0) // within
                {
                    relate::set<interior, interior, '0', Transpose>(m_result);
                }
                else if (in_val == 0)
                {
                    if (m_tc.check_boundary_point(point))
                        relate::set<interior, boundary, '0', Transpose>(m_result);
                    else
                        relate::set<interior, interior, '0', Transpose>(m_result);
                }
            }

            if ( BOOST_GEOMETRY_CONDITION(m_result.interrupt) )
            {
                return false;
            }

            if (! (relate::may_update<interior, interior, '0', Transpose>(m_result)
                || relate::may_update<interior, boundary, '0', Transpose>(m_result) ) )
            {
                return false;
            }

            return true;
        }

    private:
        MultiGeometry const& m_multi_geometry;
        detail::relate::topology_check<MultiGeometry> const& m_tc;
        Result & m_result;
        PtSegStrategy const& m_strategy;
    };

public:
    typedef typename point_type<MultiPoint>::type point1_type;
    typedef typename point_type<MultiGeometry>::type point2_type;
    typedef model::box<point1_type> box1_type;
    typedef model::box<point2_type> box2_type;
    typedef std::pair<box2_type, std::size_t> box_pair_type;

    template <typename Result, typename Strategy>
    static inline void apply(MultiPoint const& multi_point,
                             MultiGeometry const& multi_geometry,
                             std::vector<box_pair_type> const& boxes,
                             detail::relate::topology_check<MultiGeometry> const& tc,
                             Result & result,
                             Strategy const& strategy)
    {
        item_visitor_type<Result, Strategy> visitor(multi_geometry, tc, result, strategy);

        geometry::partition
            <
                box1_type
            >::apply(multi_point, boxes, visitor,
                     expand_box_point(),
                     overlaps_box_point(),
                     expand_box_box_pair(),
                     overlaps_box_box_pair());
    }

};

// MultiGeometry - Linear or Areal
// part of the algorithm calculating II, IB and IE
// using rtree
template <typename MultiPoint, typename MultiGeometry, bool Transpose>
struct multi_point_multi_geometry_ii_ib_ie
{
    typedef typename point_type<MultiPoint>::type point1_type;
    typedef typename point_type<MultiGeometry>::type point2_type;
    typedef model::box<point1_type> box1_type;
    typedef model::box<point2_type> box2_type;
    typedef std::pair<box2_type, std::size_t> box_pair_type;
    typedef std::vector<box_pair_type> boxes_type;
    typedef typename boxes_type::const_iterator boxes_iterator;

    template <typename Result, typename Strategy>
    static inline void apply(MultiPoint const& multi_point,
                             MultiGeometry const& multi_geometry,
                             std::vector<box_pair_type> const& boxes,
                             detail::relate::topology_check<MultiGeometry> const& tc,
                             Result & result,
                             Strategy const& strategy)
    {
        index::rtree<box_pair_type, index::rstar<4> > rt(boxes.begin(), boxes.end());

        typedef typename boost::range_const_iterator<MultiPoint>::type iterator;
        for ( iterator it = boost::begin(multi_point) ; it != boost::end(multi_point) ; ++it )
        {
            if (! (relate::may_update<interior, interior, '0', Transpose>(result)
                || relate::may_update<interior, boundary, '0', Transpose>(result)
                || relate::may_update<interior, exterior, '0', Transpose>(result) ) )
            {
                return;
            }

            typename boost::range_value<MultiPoint>::type const& point = *it;

            boxes_type boxes_found;
            rt.query(index::intersects(point), std::back_inserter(boxes_found));

            bool found_ii_or_ib = false;
            for (boxes_iterator bi = boxes_found.begin() ; bi != boxes_found.end() ; ++bi)
            {
                typename boost::range_value<MultiGeometry>::type const&
                    single = range::at(multi_geometry, bi->second);

                int in_val = detail::within::point_in_geometry(point, single, strategy);

                if (in_val > 0) // within
                {
                    relate::set<interior, interior, '0', Transpose>(result);
                    found_ii_or_ib = true;
                }
                else if (in_val == 0) // on boundary of single
                {
                    if (tc.check_boundary_point(point))
                        relate::set<interior, boundary, '0', Transpose>(result);
                    else
                        relate::set<interior, interior, '0', Transpose>(result);
                    found_ii_or_ib = true;
                }
            }

            // neither interior nor boundary found -> exterior
            if (found_ii_or_ib == false)
            {
                relate::set<interior, exterior, '0', Transpose>(result);
            }

            if ( BOOST_GEOMETRY_CONDITION(result.interrupt) )
            {
                return;
            }
        }
    }
};

// MultiGeometry - Linear or Areal
template <typename MultiPoint, typename MultiGeometry, bool Transpose = false>
struct multi_point_multi_geometry
{
    static const bool interruption_enabled = true;

    template <typename Result, typename Strategy>
    static inline void apply(MultiPoint const& multi_point,
                             MultiGeometry const& multi_geometry,
                             Result & result,
                             Strategy const& strategy)
    {
        typedef typename point_type<MultiGeometry>::type point2_type;
        typedef model::box<point2_type> box2_type;
        typedef std::pair<box2_type, std::size_t> box_pair_type;

        typename Strategy::envelope_strategy_type const
            envelope_strategy = strategy.get_envelope_strategy();

        std::size_t count2 = boost::size(multi_geometry);
        std::vector<box_pair_type> boxes(count2);
        for (std::size_t i = 0 ; i < count2 ; ++i)
        {
            geometry::envelope(range::at(multi_geometry, i), boxes[i].first, envelope_strategy);
            geometry::detail::expand_by_epsilon(boxes[i].first);
            boxes[i].second = i;
        }

        typedef detail::relate::topology_check<MultiGeometry> tc_t;
        tc_t tc(multi_geometry);

        if ( relate::may_update<interior, interior, '0', Transpose>(result)
          || relate::may_update<interior, boundary, '0', Transpose>(result)
          || relate::may_update<interior, exterior, '0', Transpose>(result) )
        {
            // If there is no need to calculate IE, use partition
            if (! relate::may_update<interior, exterior, '0', Transpose>(result) )
            {
                multi_point_multi_geometry_ii_ib<MultiPoint, MultiGeometry, Transpose>
                    ::apply(multi_point, multi_geometry, boxes, tc, result, strategy);
            }
            else // otherwise use rtree
            {
                multi_point_multi_geometry_ii_ib_ie<MultiPoint, MultiGeometry, Transpose>
                    ::apply(multi_point, multi_geometry, boxes, tc, result, strategy);
            }
        }

        if ( BOOST_GEOMETRY_CONDITION(result.interrupt) )
        {
            return;
        }

        if ( relate::may_update<exterior, interior, tc_t::interior, Transpose>(result)
          || relate::may_update<exterior, boundary, tc_t::boundary, Transpose>(result) )
        {
            if ( relate::may_update<exterior, interior, tc_t::interior, Transpose>(result)
              && tc.has_interior() )
            {
                // TODO: this is not true if a linestring is degenerated to a point
                // then the interior has topological dimension = 0, not 1
                relate::set<exterior, interior, tc_t::interior, Transpose>(result);
            }

            if ( relate::may_update<exterior, boundary, tc_t::boundary, Transpose>(result)
              && tc.has_boundary() )
            {
                if (multi_point_geometry_eb<MultiGeometry>::apply(multi_point, tc))
                    relate::set<exterior, boundary, tc_t::boundary, Transpose>(result);
            }
        }

        relate::set<exterior, exterior, result_dimension<MultiPoint>::value, Transpose>(result);
    }

};

template
<
    typename MultiPoint, typename Geometry,
    bool Transpose = false,
    bool isMulti = boost::is_same
                    <
                        typename tag_cast
                            <
                                typename tag<Geometry>::type, multi_tag
                            >::type,
                            multi_tag
                    >::value
>
struct multi_point_geometry
    : multi_point_single_geometry<MultiPoint, Geometry, Transpose>
{};

template <typename MultiPoint, typename Geometry, bool Transpose>
struct multi_point_geometry<MultiPoint, Geometry, Transpose, true>
    : multi_point_multi_geometry<MultiPoint, Geometry, Transpose>
{};

// transposed result of multi_point_geometry
template <typename Geometry, typename MultiPoint>
struct geometry_multi_point
{
    static const bool interruption_enabled = true;

    template <typename Result, typename Strategy>
    static inline void apply(Geometry const& geometry, MultiPoint const& multi_point,
                             Result & result, Strategy const& strategy)
    {
        multi_point_geometry<MultiPoint, Geometry, true>::apply(multi_point, geometry, result, strategy);
    }
};

}} // namespace detail::relate
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_MULTI_POINT_GEOMETRY_HPP


#pragma once

#include <avoidance/usm.h>
#include <safe_landing_planner/grid.hpp>

#include <Eigen/Dense>

#include <functional>
#include <vector>

#include "avoidance/common.h"

namespace avoidance {

const std::vector<Eigen::Vector2f> exploration_pattern = {
    Eigen::Vector2f(1.f, 0.f),  Eigen::Vector2f(1.f, 1.f),   Eigen::Vector2f(0.f, 1.f),  Eigen::Vector2f(-1.f, 1.f),
    Eigen::Vector2f(-1.f, 0.f), Eigen::Vector2f(-1.f, -1.f), Eigen::Vector2f(0.f, -1.f), Eigen::Vector2f(1.f, -1.f)};

enum class SLPState { GOTO, LOITER, LAND, ALTITUDE_CHANGE, EVALUATE_GRID, GOTO_LAND };
std::string toString(SLPState state);  // for logging

static const float LAND_SPEED = 0.7f;

class WaypointGenerator : public usm::StateMachine<SLPState> {
 public:
  WaypointGenerator();
  virtual ~WaypointGenerator() = default;

  /**
  * @brief     computes the setpoints to be sent to the FCU
  **/
  void calculateWaypoint();

 protected:
  // config
  float yaw_setpoint_ = NAN;
  float yaw_speed_setpoint_ = NAN;
  float loiter_yaw_ = NAN;
  float yaw_ = NAN;
  float beta_ = 0.9f;
  float landing_radius_ = 2.f;
  float can_land_thr_ = 0.4f;
  float loiter_height_ = 4.f;
  float factor_exploration_ = 1.f;
  float vertical_range_error_ = 1.f;
  float spiral_width_ = 2.f;
  float altitude_landing_area_percentile_ = -1.f;
  int smoothing_land_cell_ = 6;

  // state
  bool trigger_reset_ = false;
  SLPState prev_slp_state_ = SLPState::GOTO;

  bool is_land_waypoint_ = false;
  bool decision_taken_ = false;
  bool can_land_ = true;
  bool update_smoothing_size_ = false;
  bool explorarion_is_active_ = false;
  bool state_changed_ = false;
  int start_seq_landing_decision_ = 0;
  int grid_slp_seq_ = 0;
  int n_explored_pattern_ = -1;
  int stride_ = 1;

  Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN);
  Eigen::Vector3f goal_ = Eigen::Vector3f(NAN, NAN, NAN);
  Eigen::Vector3f velocity_setpoint_ = Eigen::Vector3f(NAN, NAN, NAN);
  Eigen::Vector3f loiter_position_ = Eigen::Vector3f(NAN, NAN, NAN);
  Eigen::Vector3f exploration_anchor_ = Eigen::Vector3f(NAN, NAN, NAN);
  Eigen::Vector2i pos_index_ = Eigen::Vector2i::Zero();

  Eigen::MatrixXf mean_ = Eigen::MatrixXf(40, 40);
  Eigen::MatrixXi land_ = Eigen::MatrixXi(40, 40);
  Eigen::MatrixXf can_land_hysteresis_matrix_ = Eigen::MatrixXf::Zero(40, 40);
  Eigen::MatrixXi can_land_hysteresis_result_ = Eigen::MatrixXi::Zero(40, 40);
  Eigen::MatrixXi mask_ = Eigen::MatrixXi(13, 13);

  Grid grid_slp_ = Grid(10.f, 1.f);

  // outside world link
  std::function<void(const Eigen::Vector3f& pos_sp, const Eigen::Vector3f& vel_sp, float yaw_sp, float yaw_speed_sp)>
      publishTrajectorySetpoints_;

  /**
  * @brief     update the waypoint generator state based on the vehicle status
  **/
  void updateSLPState();

  /**
  * @brief iterate the statemachine
  */
  usm::Transition runCurrentState() override final;

  /**
  * @brief the setup of the statemachine
  */
  SLPState chooseNextState(SLPState currentState, usm::Transition transition) override final;

  usm::Transition runGoTo();
  usm::Transition runLoiter();
  usm::Transition runLand();
  usm::Transition runAltitudeChange();
  usm::Transition runEvaluateGrid();
  usm::Transition runGoToLand();
  /**
  * @brief     checks if the vehicle is within the xy acceptance radius of a
  *            waypoint
  * @returns   true, if the vehicle is within the xy acceptance radius of the
  *            landing waypoint
  **/
  bool withinLandingRadius();
  /**
  * @brief     checks if the vehicle is within the z acceptance to the loiter
  *            height from the ground under it
  * @returns   true, if the vehicle is within the z acceptance to the loiter
  *            height
  **/
  bool inVerticalRange();
  /**
  * @brief      compute the percentile height of the landing area
  * @params[in] percentile, percentile be computed
  * @returns    height of the landing area
  **/
  float landingAreaHeightPercentile(float percentile);

  bool evaluatePatch(Eigen::Vector2i& left_upper_corner);

  void initializeMask();
  friend class WaypointGeneratorNode;  // TODO make an API and get rid of this
};
}


// Copyright (c) 2006-2007 Sean M. Kent
//
// 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 )
//
// $LastChangedBy$ - $LastChangedDate$
//
// Dec 26, 2006 - Created

#ifndef IG_INDUSTRY_RANGE_NUMERIC_ITERATOR
#define IG_INDUSTRY_RANGE_NUMERIC_ITERATOR

#include <boost/iterator/iterator_facade.hpp>
#include <iterator>
#include <cassert>

namespace std {
	template<class T>
	struct equal_to;
}

namespace industry {
	template<typename ValueT, typename StepT = ValueT, typename EqualityP = std::equal_to<ValueT> >
	class numeric_iterator : public boost::iterator_facade<
		numeric_iterator<ValueT, StepT, EqualityP>,
		ValueT,
		std::random_access_iterator_tag,
		ValueT >
	{
		typedef boost::iterator_facade<numeric_iterator<ValueT, StepT, EqualityP>, ValueT, std::random_access_iterator_tag> base_type;
	public:
		numeric_iterator() : infinite(true) {}
		numeric_iterator(ValueT value, StepT step, EqualityP equality_comparer = EqualityP()) : equality_comparer(equality_comparer), value(value), step(step), infinite(false) {}
	private:
		friend class boost::iterator_core_access;

		ValueT dereference() const {
			return value;
		}

		template<typename YValueT, typename YStepT, typename YEqualityP>
		bool equal(numeric_iterator<YValueT, YStepT, YEqualityP> const& other) const {
			return infinite || other.infinite ? false : equality_comparer(value, ValueT(other.value));
		}

		void increment() {
			assert(value + step > value);
			value += step;
		}

		void decrement() {
			assert(value - step < value);
			value -= step;
		}

		void advance(typename base_type::difference_type n) {
			assert(value + n * step > value);
			value += n * step;
		}

		template<typename YValueT, typename YStepT, typename YEqualityP>
		typename base_type::difference_type distance_to(numeric_iterator<YValueT, YStepT, YEqualityP> const& other) const {
			if(infinite)
				return -std::numeric_limits<typename base_type::difference_type>::max();
			else if(other.infinite)
				return std::numeric_limits<typename base_type::difference_type>::min();
			else
				return typename base_type::difference_type((other.value - value) / step);
		}

		EqualityP equality_comparer;
		ValueT value;
		StepT step;
		const bool infinite;
	};
}

#endif //IG_INDUSTRY_RANGE_NUMERIC_ITERATOR


/*
	Copyright (C) 2003-2013 by David White <davewx7@gmail.com>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 2 of the License, or
    (at your option) any later version.

    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/>.
*/
#ifndef DECIMAL_HPP_INCLUDED
#define DECIMAL_HPP_INCLUDED

#include <string>

#include <iosfwd>
#include <boost/cstdint.hpp>
#if defined(TARGET_BLACKBERRY)
#include <math.h>
#endif

static const int64_t DECIMAL_PRECISION = 1000000;
static const int64_t DECIMAL_PLACES = 6;

class decimal
{
public:
	static decimal from_string(const std::string& s);
	static decimal from_int(int v) { return decimal(v); }
	static decimal from_raw_value(int64_t v) { decimal d; d.value_ = v; return d; }
	static decimal epsilon() { return decimal::from_raw_value(static_cast<int64_t>(1)); }
	decimal() : value_(0) {}
	explicit decimal(int value) : value_(int64_t(value)*DECIMAL_PRECISION) {}
#if defined(TARGET_BLACKBERRY)
	explicit decimal(double value) : value_(llround(value*DECIMAL_PRECISION)) {}
#else
	explicit decimal(double value) : value_(int64_t(value*DECIMAL_PRECISION)) {}
#endif

	int64_t value() const { return value_; }
	int as_int() const { return int( value_/DECIMAL_PRECISION ); }
	double as_float() const { return value_/double(DECIMAL_PRECISION); }
	int64_t fractional() const { return value_%DECIMAL_PRECISION; }

	decimal operator-() const {
		return decimal(from_raw_value(-value_));
	}

	friend decimal operator+(const decimal& a, const decimal& b);
	friend decimal operator-(const decimal& a, const decimal& b);
	friend decimal operator*(const decimal& a, const decimal& b);
	friend decimal operator/(const decimal& a, const decimal& b);

	void operator+=(decimal a) { *this = *this + a; }
	void operator-=(decimal a) { *this = *this - a; }
	void operator*=(decimal a) { *this = *this * a; }
	void operator/=(decimal a) { *this = *this / a; }

	void operator+=(int a) { operator+=(decimal::from_int(a)); }
	void operator-=(int a) { operator-=(decimal::from_int(a)); }
	void operator*=(int a) { operator*=(decimal::from_int(a)); }
	void operator/=(int a) { operator/=(decimal::from_int(a)); }

private:
	int64_t value_;
};

inline decimal operator+(const decimal& a, const decimal& b) {
	return decimal::from_raw_value(a.value() + b.value());
}

inline decimal operator-(const decimal& a, const decimal& b) {
	return decimal::from_raw_value(a.value() - b.value());
}

decimal operator*(const decimal& a, const decimal& b);
decimal operator/(const decimal& a, const decimal& b);

inline bool operator==(const decimal& a, const decimal& b) {
	return a.value() == b.value();
}

inline bool operator!=(const decimal& a, const decimal& b) {
	return !operator==(a, b);
}

inline bool operator<=(const decimal& a, const decimal& b) {
	return a.value() <= b.value();
}

inline bool operator>=(const decimal& a, const decimal& b) {
	return b <= a;
}

inline bool operator<(const decimal& a, const decimal& b) {
	return !(b <= a);
}

inline bool operator>(const decimal& a, const decimal& b) {
	return !(a <= b);
}

inline decimal operator+(decimal a, int b) { return operator+(a, decimal::from_int(b)); }
inline decimal operator-(decimal a, int b) { return operator-(a, decimal::from_int(b)); }
inline decimal operator*(decimal a, int b) { return operator*(a, decimal::from_int(b)); }
inline decimal operator/(decimal a, int b) { return operator/(a, decimal::from_int(b)); }
inline bool operator<(decimal a, int b) { return operator<(a, decimal::from_int(b)); }
inline bool operator>(decimal a, int b) { return operator>(a, decimal::from_int(b)); }
inline bool operator<=(decimal a, int b) { return operator<=(a, decimal::from_int(b)); }
inline bool operator>=(decimal a, int b) { return operator>=(a, decimal::from_int(b)); }

inline decimal operator+(int a, decimal b) { return operator+(decimal::from_int(a), b); }
inline decimal operator-(int a, decimal b) { return operator-(decimal::from_int(a), b); }
inline decimal operator*(int a, decimal b) { return operator*(decimal::from_int(a), b); }
inline decimal operator/(int a, decimal b) { return operator/(decimal::from_int(a), b); }
inline bool operator<(int a, decimal b) { return operator<(decimal::from_int(a), b); }
inline bool operator>(int a, decimal b) { return operator>(decimal::from_int(a), b); }
inline bool operator<=(int a, decimal b) { return operator<=(decimal::from_int(a), b); }
inline bool operator>=(int a, decimal b) { return operator>=(decimal::from_int(a), b); }

std::ostream& operator<<(std::ostream& s, decimal d);

#endif


#include <iostream>
#include <iterator>
#include <iomanip>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <cmath>
#ifdef _OPENMP
#include <omp.h>
#endif

#include <boost/scope_exit.hpp>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <amgcl/amg.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/mpi/subdomain_deflation.hpp>
#include <amgcl/mpi/solver/runtime.hpp>
#include <amgcl/mpi/direct_solver/runtime.hpp>
#include <amgcl/profiler.hpp>

namespace amgcl {
    profiler<> prof;
}

using amgcl::precondition;

//---------------------------------------------------------------------------
std::vector<ptrdiff_t> read_problem(
        const amgcl::mpi::communicator &world,
        const std::string &A_file,
        const std::string &rhs_file,
        const std::string &part_file,
        int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs
        )
{
    // Read partition
    std::vector<int> part;
    std::vector<ptrdiff_t> domain(world.size + 1, 0);
    ptrdiff_t n = 0;
    std::string line;

    {
        std::ifstream f(part_file.c_str());
        precondition(f, "Failed to open part file (" + part_file + ")");

        while (std::getline(f, line)) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            ptrdiff_t cols;
            precondition(is >> n >> cols, "Unsupported format in matrix file");
            break;
        }

        precondition(n, "Zero sized partition vector");
        precondition(n % block_size == 0, "Matrix size is not divisible by block_size");

        part.reserve(n);

        while (std::getline(f, line)) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            int p;
            precondition(is >> p, "Unsupported format in part file");
            precondition(p < world.size, "MPI world does not correspond to partition");

            part.push_back(p);
            ++domain[p+1];
        }

        std::partial_sum(domain.begin(), domain.end(), domain.begin());
    }

    ptrdiff_t chunk_beg = domain[world.rank];
    ptrdiff_t chunk_end = domain[world.rank + 1];
    ptrdiff_t chunk     = chunk_end - chunk_beg;

    // Reorder unknowns
    std::vector<ptrdiff_t> order(n);
    {
        for(ptrdiff_t i = 0; i < n; ++i) {
            int p = part[i];
            int j = domain[p]++;

            order[i] = j;
        }

        std::rotate(domain.begin(), domain.end()-1, domain.end());
        domain[0] = 0;
    }

    // Read matrix chunk
    {
        std::ifstream A(A_file.c_str());
        precondition(A, "Failed to open matrix file (" + A_file + ")");

        ptrdiff_t nnz = 0;
        while ( std::getline(A, line) ) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            ptrdiff_t rows, m;
            precondition(is >> rows >> m >> nnz, "Unsupported format in matrix file");
            precondition(rows == n, "Matrix and partition have incompatible sizes");
            precondition(n == m, "Non-square matrix in matrix file");
            break;
        }

        {
            std::vector<ptrdiff_t> I, J;
            std::vector<double>    V;

            ptr.clear();
            ptr.resize(chunk + 1, 0);

            while (std::getline(A, line)) {
                if (line[0] == '%') continue;
                std::istringstream is(line);
                ptrdiff_t i, j;
                double v;
                precondition(is >> i >> j >> v, "Unsupported format in matrix file");
                --i;
                --j;

                if (part[i] != world.rank) continue;

                ++ptr[order[i] + 1 - chunk_beg];

                I.push_back(order[i] - chunk_beg);
                J.push_back(order[j]);
                V.push_back(v);
            }

            std::partial_sum(ptr.begin(), ptr.end(), ptr.begin());

            ptrdiff_t loc_nnz = ptr.back();

            col.clear(); col.resize(loc_nnz);
            val.clear(); val.resize(loc_nnz);

            for(ptrdiff_t i = 0; i < loc_nnz; ++i) {
                ptrdiff_t row = I[i];
                col[ptr[row]] = J[i];
                val[ptr[row]] = V[i];
                ++ptr[row];
            }
            std::rotate(ptr.begin(), ptr.end()-1, ptr.end());
            ptr[0] = 0;
        }
    }

    // Read RHS chunk.
    {
        std::ifstream f(rhs_file.c_str());
        precondition(f, "Failed to open rhs file (" + rhs_file + ")");

        while (std::getline(f, line)) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            ptrdiff_t rows, cols;
            precondition(is >> rows >> cols, "Unsupported format in matrix file");
            precondition(rows == n, "RHS size should coincide with matrix size");
            break;
        }

        rhs.clear();
        rhs.reserve(chunk);

        ptrdiff_t pos = 0;
        while (std::getline(f, line)) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            double v;
            precondition(is >> v, "Unsupported format in RHS file");

            if (part[pos++] != world.rank) continue;

            rhs.push_back(v);
        }

        assert(rhs.size() + 1 == ptr.size());
    }

    return domain;
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
    BOOST_SCOPE_EXIT(void) {
        MPI_Finalize();
    } BOOST_SCOPE_EXIT_END

    amgcl::mpi::communicator world(MPI_COMM_WORLD);

    if (world.rank == 0)
        std::cout << "World size: " << world.size << std::endl;

    // Read configuration from command line
    amgcl::runtime::coarsening::type    coarsening       = amgcl::runtime::coarsening::smoothed_aggregation;
    amgcl::runtime::relaxation::type    relaxation       = amgcl::runtime::relaxation::spai0;
    amgcl::runtime::solver::type        iterative_solver = amgcl::runtime::solver::bicgstabl;
    amgcl::runtime::mpi::direct::type   direct_solver    = amgcl::runtime::mpi::direct::skyline_lu;
    std::string parameter_file;
    std::string A_file    = "A.mtx";
    std::string rhs_file  = "b.mtx";
    std::string part_file = "partition.mtx";
    std::string out_file;

    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        (
         "coarsening,c",
         po::value<amgcl::runtime::coarsening::type>(&coarsening)->default_value(coarsening),
         "ruge_stuben, aggregation, smoothed_aggregation, smoothed_aggr_emin"
        )
        (
         "relaxation,r",
         po::value<amgcl::runtime::relaxation::type>(&relaxation)->default_value(relaxation),
         "gauss_seidel, ilu0, damped_jacobi, spai0, chebyshev"
        )
        (
         "iter_solver,i",
         po::value<amgcl::runtime::solver::type>(&iterative_solver)->default_value(iterative_solver),
         "cg, bicgstab, bicgstabl, gmres"
        )
        (
         "dir_solver,d",
         po::value<amgcl::runtime::mpi::direct::type>(&direct_solver)->default_value(direct_solver),
         "skyline_lu"
#ifdef AMGCL_HAVE_PASTIX
         ", pastix"
#endif
        )
        (
         "params,p",
         po::value<std::string>(&parameter_file),
         "parameter file in json format"
        )
        (
         "matrix,A",
         po::value<std::string>(&A_file)->default_value(A_file),
         "The system matrix in MatrixMarket format"
        )
        (
         "rhs,b",
         po::value<std::string>(&rhs_file)->default_value(rhs_file),
         "The right-hand side in MatrixMarket format"
        )
        (
         "part,s",
         po::value<std::string>(&part_file)->default_value(part_file),
         "Partitioning of the problem in MatrixMarket format"
        )
        (
         "output,o",
         po::value<std::string>(&out_file),
         "The output file (saved in MatrixMarket format)"
        )
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        if (world.rank == 0)
            std::cout << desc << std::endl;
        return 0;
    }

    boost::property_tree::ptree prm;
    if (vm.count("params")) read_json(parameter_file, prm);

    prm.put("local.coarsening.type", coarsening);
    prm.put("local.relax.type",      relaxation);
    prm.put("isolver.type",          iterative_solver);
    prm.put("dsolver.type",          direct_solver);

    using amgcl::prof;

    int block_size = prm.get("precond.coarsening.aggr.block_size", 1);

    prof.tic("read problem");
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    std::vector<ptrdiff_t> domain = read_problem(
            world, A_file, rhs_file, part_file, block_size, ptr, col, val, rhs
            );

    ptrdiff_t chunk = domain[world.rank + 1] - domain[world.rank];
    prof.toc("read problem");

    prof.tic("setup");
    typedef
        amgcl::mpi::subdomain_deflation<
            amgcl::amg<
                amgcl::backend::builtin<double>,
                amgcl::runtime::coarsening::wrapper,
                amgcl::runtime::relaxation::wrapper
                >,
            amgcl::runtime::mpi::solver::wrapper<amgcl::backend::builtin<double>>,
            amgcl::runtime::mpi::direct::solver<double>
        > SDD;

    std::function<double(ptrdiff_t,unsigned)> dv = amgcl::mpi::constant_deflation(block_size);
    prm.put("num_def_vec", block_size);
    prm.put("def_vec", &dv);

    SDD solve(world, std::tie(chunk, ptr, col, val), prm);
    double tm_setup = prof.toc("setup");

    std::vector<double> x(chunk, 0);

    prof.tic("solve");
    size_t iters;
    double resid;
    std::tie(iters, resid) = solve(rhs, x);
    double tm_solve = prof.toc("solve");

    if (vm.count("output")) {
        prof.tic("save");
        for(int r = 0; r < world.size; ++r) {
            if (r == world.rank) {
                std::ofstream f(out_file.c_str(), r == 0 ? std::ios::trunc : std::ios::app);

                if (r == 0) {
                    f << "%%MatrixMarket matrix array real general\n"
                      << domain.back() << " 1\n";
                }

                std::ostream_iterator<double> oi(f, "\n");
                std::copy(x.begin(), x.end(), oi);
            }
            MPI_Barrier(world);
        }
        prof.toc("save");
    }

    if (world.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << resid << std::endl
            << std::endl
            << prof << std::endl;

#ifdef _OPENMP
        int nt = omp_get_max_threads();
#else
        int nt = 1;
#endif
        std::ostringstream log_name;
        log_name << "log_" << domain.back() << "_" << nt << "_" << world.size << ".txt";
        std::ofstream log(log_name.str().c_str(), std::ios::app);
        log << domain.back() << "\t" << nt << "\t" << world.size
            << "\t" << tm_setup << "\t" << tm_solve
            << "\t" << iters << "\t" << std::endl;
    }

}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/lazy.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/functional.hpp>

#include "../comparable.hpp"
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) {
    return x + 1;
};

int main() {
    BOOST_HANA_CONSTEXPR_ASSERT(equal(fmap(f, lazy(1)), lazy(f(1))));
    BOOST_HANA_CONSTEXPR_ASSERT(equal(fmap(f, lazy(2.2)), lazy(f(2.2))));

    BOOST_HANA_CONSTEXPR_ASSERT(equal(fmap(id, lazy(1)), lazy(1)));
    BOOST_HANA_CONSTEXPR_ASSERT(equal(fmap(id, lazy('2')), lazy('2')));
    BOOST_HANA_CONSTEXPR_ASSERT(equal(fmap(id, lazy(3.3)), lazy(3.3)));
}


#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include "../outlier_detection/outlier_helpers.h"

using namespace boost::python;

namespace dials { namespace refinement { namespace boost_python {

  void export_outlier_helpers() {
    def("qchisq", &qchisq, (arg("p"), arg("df")));
    def("mcd_consistency", &mcd_consistency, (arg("df"), arg("alpha")));
  }

}}}  // namespace dials::refinement::boost_python