/*
Copyright 2016-2017 Robotics and Biology Lab, TU Berlin. All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

    Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
    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.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.

The views and conclusions contained in the software and documentation are those of the authors and should not be interpreted as representing official policies, either expressed or implied, of the FreeBSD Project.
*/

#include <ecto_rbo_grasping/PoseSet.h>

#include <tf_conversions/tf_eigen.h>
#include <Eigen/Geometry>

#include <CGAL/Gmpz.h>
#include <CGAL/Extended_homogeneous.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Nef_polyhedron_3.h>

#include "Wm5IntrSegment3Box3.h"
#include "Wm5IntrBox3Box3.h"
#include "Wm5ContMinBox3.h"

#include <ecto_rbo_grasping/gdiam.hpp>

//typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;
//typedef CGAL::Extended_homogeneous<CGAL::Gmpz>  Kernel;
typedef CGAL::Extended_cartesian< CGAL::Lazy_exact_nt<CGAL::Gmpq> > Kernel;
typedef CGAL::Nef_polyhedron_3<Kernel>  Nef_polyhedron;
typedef Nef_polyhedron::Vertex_const_iterator Vertex_const_iterator;
typedef Nef_polyhedron::Plane_3  Plane_3;
typedef Nef_polyhedron::Point_3  Point_3;
typedef Nef_polyhedron::Vector_3  Vector_3;

namespace posesets
{

PoseSet::PoseSet(const tf::Transform& origin) :
    origin(origin)
{
}

PoseSet::~PoseSet()
{
}

void PoseSet::updateBoxVertices(Wm5::Vector3d box_vertices[8], Wm5::Segment3d edges[12]) const
{
    Wm5::Vector3d origin_a(this->origin.getOrigin().x(), this->origin.getOrigin().y(), this->origin.getOrigin().z());
    //    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(a.getBasis().getRow(0).x(), a.getBasis().getRow(0).y(), a.getBasis().getRow(0).z()),
    //                               Wm5::Vector3d(a.getBasis().getRow(1).x(), a.getBasis().getRow(1).y(), a.getBasis().getRow(1).z()),
    //                               Wm5::Vector3d(a.getBasis().getRow(2).x(), a.getBasis().getRow(2).y(), a.getBasis().getRow(2).z()) };
    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(this->origin.getBasis().getColumn(0).x(), this->origin.getBasis().getColumn(0).y(), this->origin.getBasis().getColumn(0).z()),
                               Wm5::Vector3d(this->origin.getBasis().getColumn(1).x(), this->origin.getBasis().getColumn(1).y(), this->origin.getBasis().getColumn(1).z()),
                               Wm5::Vector3d(this->origin.getBasis().getColumn(2).x(), this->origin.getBasis().getColumn(2).y(), this->origin.getBasis().getColumn(2).z()) };
    double size_a[3] = {0.5 * this->getPositions()[0], 0.5 * this->getPositions()[1], 0.5 * this->getPositions()[2]};

    Wm5::Box3d box_a(origin_a, dim_a, size_a);
    box_a.ComputeVertices(box_vertices);

    edges[0] = Wm5::Segment3d(box_vertices[0], box_vertices[1]);
    edges[1] = Wm5::Segment3d(box_vertices[0], box_vertices[3]);
    edges[2] = Wm5::Segment3d(box_vertices[0], box_vertices[4]);
    edges[3] = Wm5::Segment3d(box_vertices[1], box_vertices[2]);
    edges[4] = Wm5::Segment3d(box_vertices[1], box_vertices[5]);
    edges[5] = Wm5::Segment3d(box_vertices[2], box_vertices[6]);
    edges[6] = Wm5::Segment3d(box_vertices[2], box_vertices[3]);
    edges[7] = Wm5::Segment3d(box_vertices[3], box_vertices[7]);
    edges[8] = Wm5::Segment3d(box_vertices[4], box_vertices[7]);
    edges[9] = Wm5::Segment3d(box_vertices[4], box_vertices[5]);
    edges[10] = Wm5::Segment3d(box_vertices[5], box_vertices[6]);
    edges[11] = Wm5::Segment3d(box_vertices[6], box_vertices[7]);
}

void PoseSet::setOrigin(const tf::Transform& origin)
{
    this->origin = origin;
}

void PoseSet::setOrientations(const OrientationSet& orientations)
{
    this->orientations = orientations;
}

void PoseSet::setPositions(const tf::Vector3& positions)
{
    this->positions = positions;
}

const tf::Transform& PoseSet::getOrigin() const
{
    return origin;
}

OrientationSet& PoseSet::getOrientations()
{
    return orientations;
}

const OrientationSet& PoseSet::getOrientations() const
{
    return orientations;
}

const tf::Vector3& PoseSet::getPositions() const
{
    return positions;
}

Wm5::Box3d PoseSet::getWm5Box3d() const
{
    Wm5::Vector3d origin_a(origin.getOrigin().x(), origin.getOrigin().y(), origin.getOrigin().z());
    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(origin.getBasis().getColumn(0).x(), origin.getBasis().getColumn(0).y(), origin.getBasis().getColumn(0).z()),
                               Wm5::Vector3d(origin.getBasis().getColumn(1).x(), origin.getBasis().getColumn(1).y(), origin.getBasis().getColumn(1).z()),
                               Wm5::Vector3d(origin.getBasis().getColumn(2).x(), origin.getBasis().getColumn(2).y(), origin.getBasis().getColumn(2).z()) };
    double size_a[3] = {0.5 * positions[0], 0.5 * positions[1], 0.5 * positions[2]};

    return Wm5::Box3d(origin_a, dim_a, size_a);
}

bool PoseSet::isIntersecting(const PoseSet& other) const
{
//    if (this->orientations.getIntersection(other.getOrientations()).empty())
    if (!this->orientations.hasIntersection(other.getOrientations()))
    {
//        std::cout << "orientation false " << std::endl;
        return false;
    }

    Wm5::IntrBox3Box3d intersector(this->getWm5Box3d(), other.getWm5Box3d());

//    if (!intersector.Test())
//        std::cout << "position false " << std::endl;

    return intersector.Test();
}

bool PoseSet::intersect(const PoseSet& other)
{
    this->orientations.intersect(other.getOrientations());

    if (this->orientations.empty())
        return false;

    return intersectBoxesGeometricTools(this->origin, this->positions, other.getOrigin(), other.getPositions(), this->origin, this->positions);

    /*
    if (intersectBoxes(this->origin, this->positions, other.getOrigin(), other.getPositions()))
    {
        ROS_INFO("Intersection! (maybe)");
//        this->origin.setOrigin(origin.getOrigin().lerp(other.getOrigin().getOrigin(), 0.5));
//        this->positions.setMin(other.getPositions());
    }
    else {
        ROS_INFO("They DONT intersect!");
        this->origin.setIdentity();
        this->positions.setZero();
    }
    */
}

bool PoseSet::intersectBoxesGeometricTools(const tf::Transform& a, const tf::Vector3& extents_a, const tf::Transform& b, const tf::Vector3& extents_b, tf::Transform& n, tf::Vector3& extents_n)
{
    Wm5::Vector3d origin_a(a.getOrigin().x(), a.getOrigin().y(), a.getOrigin().z());
//    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(a.getBasis().getRow(0).x(), a.getBasis().getRow(0).y(), a.getBasis().getRow(0).z()),
//                               Wm5::Vector3d(a.getBasis().getRow(1).x(), a.getBasis().getRow(1).y(), a.getBasis().getRow(1).z()),
//                               Wm5::Vector3d(a.getBasis().getRow(2).x(), a.getBasis().getRow(2).y(), a.getBasis().getRow(2).z()) };
    Wm5::Vector3d dim_a[3] = { Wm5::Vector3d(a.getBasis().getColumn(0).x(), a.getBasis().getColumn(0).y(), a.getBasis().getColumn(0).z()),
                               Wm5::Vector3d(a.getBasis().getColumn(1).x(), a.getBasis().getColumn(1).y(), a.getBasis().getColumn(1).z()),
                               Wm5::Vector3d(a.getBasis().getColumn(2).x(), a.getBasis().getColumn(2).y(), a.getBasis().getColumn(2).z()) };
    double size_a[3] = {0.5 * extents_a[0], 0.5 * extents_a[1], 0.5 * extents_a[2]};

    Wm5::Vector3d origin_b(b.getOrigin().x(), b.getOrigin().y(), b.getOrigin().z());
//    Wm5::Vector3d dim_b[3] = { Wm5::Vector3d(b.getBasis().getRow(0).x(), b.getBasis().getRow(0).y(), b.getBasis().getRow(0).z()),
//                               Wm5::Vector3d(b.getBasis().getRow(1).x(), b.getBasis().getRow(1).y(), b.getBasis().getRow(1).z()),
//                               Wm5::Vector3d(b.getBasis().getRow(2).x(), b.getBasis().getRow(2).y(), b.getBasis().getRow(2).z()) };
    Wm5::Vector3d dim_b[3] = { Wm5::Vector3d(b.getBasis().getColumn(0).x(), b.getBasis().getColumn(0).y(), b.getBasis().getColumn(0).z()),
                               Wm5::Vector3d(b.getBasis().getColumn(1).x(), b.getBasis().getColumn(1).y(), b.getBasis().getColumn(1).z()),
                               Wm5::Vector3d(b.getBasis().getColumn(2).x(), b.getBasis().getColumn(2).y(), b.getBasis().getColumn(2).z()) };
    double size_b[3] = {0.5 * extents_b[0], 0.5 * extents_b[1], 0.5 * extents_b[2]};

    Wm5::Box3d box_a(origin_a, dim_a, size_a);
    Wm5::Box3d box_b(origin_b, dim_b, size_b);
    Wm5::IntrBox3Box3d intersector(box_a, box_b);

    if (!intersector.Test())
    {
        //ROS_INFO("NO intersection.");
        n.setIdentity();
        extents_n.setZero();
        return false;
    }

    // do static check differently:
    // check intersections between 12 edges (segments) of one box with the other box
    intersection_points.clear();
//    std::vector<Wm5::Vector3d> intersection_points;
    for (int l = 0; l < 2; ++l)
    {
        const Wm5::Box3d& box = (l == 0) ? box_a : box_b;
        const Wm5::Box3d& other_box = (l == 1) ? box_a : box_b;

        Wm5::Vector3d vertices[8];
        box.ComputeVertices(vertices);

//        if (l == 0)
//            for (int k = 0; k < 8; ++k)
//                this->box_vertices[k] = vertices[k];

        Wm5::Segment3d edges[12] = {
            Wm5::Segment3d(vertices[0], vertices[1]),
            Wm5::Segment3d(vertices[0], vertices[3]),
            Wm5::Segment3d(vertices[0], vertices[4]),
            Wm5::Segment3d(vertices[1], vertices[2]),
            Wm5::Segment3d(vertices[1], vertices[5]),
            Wm5::Segment3d(vertices[2], vertices[6]),
            Wm5::Segment3d(vertices[2], vertices[3]),
            Wm5::Segment3d(vertices[3], vertices[7]),
            Wm5::Segment3d(vertices[4], vertices[7]),
            Wm5::Segment3d(vertices[4], vertices[5]),
            Wm5::Segment3d(vertices[5], vertices[6]),
            Wm5::Segment3d(vertices[6], vertices[7])
        };

        for (int i = 0; i < 12; ++i)
        {
            Wm5::IntrSegment3Box3d intersector2(edges[i], other_box, true);
            if (!intersector2.Find())
                continue;

            int no_intersections = intersector2.GetQuantity();

    //        std::cout << "Found: " << no_intersections << std::endl;
            for (int j = 0; j < no_intersections; ++j)
            {
//                bool already_in_there = false;
                const Wm5::Vector3d& candidate = intersector2.GetPoint(j);
//                // check if for some reasons this point is already in there
//                for (int k = 0; k < intersection_points.size(); ++k)
//                {
//                    if (fabs(candidate.X() - intersection_points[k].X()) < 0.001f ||
//                        fabs(candidate.Y() - intersection_points[k].Y()) < 0.001f ||
//                        fabs(candidate.Z() - intersection_points[k].Z()) < 0.001f)
//                    {
//                        already_in_there = true;
//                        break;
//                    }
//                }
//                if (!already_in_there)
                    intersection_points.push_back(intersector2.GetPoint(j));
            }
        }
    }

    /*
    std::cout << "In Total: " << intersection_points.size() << std::endl;
    for (std::vector<Wm5::Vector3d>::iterator it = intersection_points.begin(); it != intersection_points.end(); ++it)
    {
        std::cout << " (" << it->X() << " " << it->Y() << " " << it->Z() << ")";
    }
    std::cout << std::endl;
    */

    if (intersection_points.size() < 4)
    {
        n.setIdentity();
        extents_n.setZero();
        return false;
    }

//    Wm5::Box3d minBox = Wm5::MinBox3<double>((int) intersection_points.size(), &intersection_points[0], 0.001, Wm5::Query::QT_REAL);

//    n.setBasis(tf::Matrix3x3(minBox.Axis[0].X(), minBox.Axis[0].Y(), minBox.Axis[0].Z(),
//                             minBox.Axis[1].X(), minBox.Axis[1].Y(), minBox.Axis[1].Z(),
//                             minBox.Axis[2].X(), minBox.Axis[2].Y(), minBox.Axis[2].Z()));

//    n.setOrigin(tf::Vector3(minBox.Center.X(), minBox.Center.Y(), minBox.Center.Z()));

//    std::cout << "New Box extents: " << extents_n[0] << " " << extents_n[1] << " " << extents_n[2] << std::endl;

//    extents_n.setX(minBox.Extent[0]);
//    extents_n.setY(minBox.Extent[1]);
//    extents_n.setZ(minBox.Extent[2]);

//    if (extents_n[0] == 0 || extents_n[1] == 0 || extents_n[2] == 0)
//    {
//        n.setIdentity();
//        extents_n.setZero();
//        return false;
//    }

//    intersector.Find(0.0, Wm5::Vector3d::ZERO, Wm5::Vector3d::ZERO);
//    intersector.Find(0.1, Wm5::Vector3d(0.1, 0.1, 0.1), Wm5::Vector3d(0.1, 0.1, 0.1));
//    int number_of_intersection_points = intersector.GetQuantity();

//    std::cout << "Number of intersection points: " << number_of_intersection_points << std::endl;
//    if (number_of_intersection_points == 0)
//    {
//        n.setIdentity();
//        extents_n.setZero();
//        return false;
//    }

    gdiam_real* points;
    points = (gdiam_point)malloc( sizeof( gdiam_point_t ) * intersection_points.size());

    for (int i = 0; i < intersection_points.size(); ++i)
    {
        const Wm5::Vector3d& p = intersection_points[i];
        points[i * 3 + 0] = p.X();
        points[i * 3 + 1] = p.Y();
        points[i * 3 + 2] = p.Z();
//        std::cout << "HA: " << p.X() << " " << p.Y() << " " << p.Z() << std::endl;
    }

    gdiam_point* pnt_arr;
    gdiam_bbox bb;
    pnt_arr = gdiam_convert( (gdiam_real *)points, intersection_points.size());

//    printf( "Computing a tight-fitting bounding box of the point-set\n" );
//    bb = gdiam_approx_mvbb_grid_sample(pnt_arr, nefI.number_of_vertices(), 5, 400 );
    bb = gdiam_approx_mvbb(pnt_arr, intersection_points.size(), 0.01f);

    //printf( "Resulting bounding box:\n" );
    bb.dump();

    gdiam_point bb_dir0 = bb.get_dir(0);
    gdiam_point bb_dir1 = bb.get_dir(1);
    gdiam_point bb_dir2 = bb.get_dir(2);

    n.setBasis(tf::Matrix3x3(bb_dir0[0], bb_dir1[0], bb_dir2[0],
                             bb_dir0[1], bb_dir1[1], bb_dir2[1],
                             bb_dir0[2], bb_dir1[2], bb_dir2[2]));

    double x, y, z;
    bb.get_vertex(0.5, 0.5, 0.5, &x, &y, &z);
    n.setOrigin(tf::Vector3(x, y, z));

    extents_n.setX(bb.get_len(0) * 1.f);
    extents_n.setY(bb.get_len(1) * 1.f);
    extents_n.setZ(bb.get_len(2) * 1.f);

//    if (extents_n[0] == 0 || extents_n[1] == 0 || extents_n[2] == 0)
    if (bb.volume() == 0)
    {
        //ROS_INFO("Too Small!");
        n.setIdentity();
        extents_n.setZero();
        return false;
    }

    return true;
}

void PoseSet::intersectBoxesCGAL(const tf::Transform& a, const tf::Vector3& extents_a, const tf::Transform& b, const tf::Vector3& extents_b, tf::Transform& n, tf::Vector3& extents_n)
{
    // generate a Nef Polyhedron for box A
    Point_3 origin_a(a.getOrigin().x(), a.getOrigin().x(), a.getOrigin().x());
    Vector_3 dim1_a(a.getBasis().getRow(0).x(), a.getBasis().getRow(0).y(), a.getBasis().getRow(0).z());
    Vector_3 dim2_a(a.getBasis().getRow(1).x(), a.getBasis().getRow(1).y(), a.getBasis().getRow(1).z());
    Vector_3 dim3_a(a.getBasis().getRow(2).x(), a.getBasis().getRow(2).y(), a.getBasis().getRow(2).z());
    Nef_polyhedron NA1(Plane_3(origin_a + dim1_a * extents_a[0], dim1_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA2(Plane_3(origin_a - dim1_a * extents_a[0], -dim1_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA3(Plane_3(origin_a + dim2_a * extents_a[1], dim2_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA4(Plane_3(origin_a - dim2_a * extents_a[1], -dim2_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA5(Plane_3(origin_a + dim3_a * extents_a[2], dim3_a), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NA6(Plane_3(origin_a - dim3_a * extents_a[2], -dim3_a), Nef_polyhedron::INCLUDED);

//    Nef_polyhedron nefA = NA1 * NA2 * NA3 * NA4 * NA5 * NA6;

    // generate a Nef Polyhedron for box B
    Point_3 origin_b(b.getOrigin().x(), b.getOrigin().x(), b.getOrigin().x());
    Vector_3 dim1_b(b.getBasis().getRow(0).x(), b.getBasis().getRow(0).y(), b.getBasis().getRow(0).z());
    Vector_3 dim2_b(b.getBasis().getRow(1).x(), b.getBasis().getRow(1).y(), b.getBasis().getRow(1).z());
    Vector_3 dim3_b(b.getBasis().getRow(2).x(), b.getBasis().getRow(2).y(), b.getBasis().getRow(2).z());
    Nef_polyhedron NB1(Plane_3(origin_b + dim1_b * extents_b[0], dim1_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB2(Plane_3(origin_b - dim1_b * extents_b[0], -dim1_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB3(Plane_3(origin_b + dim2_b * extents_b[1], dim2_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB4(Plane_3(origin_b - dim2_b * extents_b[1], -dim2_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB5(Plane_3(origin_b + dim3_b * extents_b[2], dim3_b), Nef_polyhedron::INCLUDED);
    Nef_polyhedron NB6(Plane_3(origin_b - dim3_b * extents_b[2], -dim3_b), Nef_polyhedron::INCLUDED);

//    Nef_polyhedron nefB = NB1 * NB2 * NB3 * NB4 * NB5 * NB6;

//    Nef_polyhedron nefI = nefA.intersection(nefB);
    Nef_polyhedron nefI = NA1 * NA2 * NA3 * NA4 * NA5 * NA6 * NB1 * NB2 * NB3 * NB4 * NB5 * NB6;

    /*
    std::cout << "properties: " << std::endl;
    std::cout << "is_simple: " << nefI.is_simple() << std::endl;
    std::cout << "is_bounded: " << nefI.is_bounded() << std::endl;
//    std::cout << "is_convex: " << nefI.is_convex() << std::endl;
    std::cout << "vertices: " << nefI.number_of_vertices() << std::endl;
    std::cout << "is_empty: " << nefI.is_empty() << std::endl;
    */

    if (nefI.is_empty())
        return;

    Vertex_const_iterator it;
//    gdiam_point* pnt_arr;// = new gdiam_point[nefI.number_of_vertices()];
//    pnt_arr = (gdiam_point *)malloc( sizeof( gdiam_point ) * nefI.number_of_vertices());
    gdiam_real* points;
    points = (gdiam_point)malloc( sizeof( gdiam_point_t ) * nefI.number_of_vertices());

    size_t i = 0;
    CGAL_forall_vertices(it, nefI)
    {
        //std::cout << CGAL::to_double(it->point().x()) << " HAH " << CGAL::to_double(it->point().y()) << " " << CGAL::to_double(it->point().z()) << std::endl;
//        pnt_init((gdiam_point)(pnt_arr[i]), 0, 0, 0);
        points[i * 3 + 0] = CGAL::to_double(it->point().x());
        points[i * 3 + 1] = CGAL::to_double(it->point().y());
        points[i * 3 + 2] = CGAL::to_double(it->point().z());
//        pnt_init(pnt_arr[i], CGAL::to_double(it->point().x()), CGAL::to_double(it->point().y()), CGAL::to_double(it->point().z()));
        ++i;
    }

    gdiam_point* pnt_arr;
    gdiam_bbox bb;
    pnt_arr = gdiam_convert( (gdiam_real *)points, nefI.number_of_vertices());

//    printf( "Computing a tight-fitting bounding box of the point-set\n" );
//    bb = gdiam_approx_mvbb_grid_sample(pnt_arr, nefI.number_of_vertices(), 5, 400 );
    bb = gdiam_approx_mvbb(pnt_arr, nefI.number_of_vertices(), 0.1f);

    //printf( "Resulting bounding box:\n" );
    bb.dump();

    gdiam_point bb_dir0 = bb.get_dir(0);
    gdiam_point bb_dir1 = bb.get_dir(1);
    gdiam_point bb_dir2 = bb.get_dir(2);

    n.setBasis(tf::Matrix3x3(bb_dir0[0], bb_dir0[1], bb_dir0[2],
                             bb_dir1[0], bb_dir1[1], bb_dir1[2],
                             bb_dir2[0], bb_dir2[1], bb_dir2[2]));

    double x, y, z;
    bb.get_vertex(0.5, 0.5, 0.5, &x, &y, &z);
    n.setOrigin(tf::Vector3(x, y, z));

    extents_n.setX(bb.get_len(0) * 0.5f);
    extents_n.setY(bb.get_len(1) * 0.5f);
    extents_n.setZ(bb.get_len(2) * 0.5f);
}

void PoseSet::intersectBoxes(const tf::Transform& a, const tf::Vector3& extents_a, const tf::Transform& b, const tf::Vector3& extents_b, tf::Transform& n, tf::Vector3& extents_n)
{
//    tf::Transform b_a = a.inverseTimes(b);
//    tf::Transform a_b = b.inverseTimes(a);

//    // do aabb-intersection along axes of a
//    tf::Matrix3x3 all_dot_products = a.getBasis().transposeTimes(b_a.getBasis());
//    tf::Vector3 extents_b_a(all_dot_products.getRow(0)[all_dot_products.getRow(0).maxAxis()],
//                            all_dot_products.getRow(1)[all_dot_products.getRow(1).maxAxis()],
//                            all_dot_products.getRow(2)[all_dot_products.getRow(2).maxAxis()]);
//    extents_b_a *= extents_b;

//    // do aabb-intersection along axes of b
////    all_dot_products = b.getBasis().transposeTimes(a_b.getBasis());
////    tf::Vector3 extents_a_b(all_dot_products.getRow(0)[all_dot_products.getRow(0).maxAxis()],
////                            all_dot_products.getRow(1)[all_dot_products.getRow(1).maxAxis()],
////                            all_dot_products.getRow(2)[all_dot_products.getRow(2).maxAxis()]);
////    extents_a_b *= extents_a;

////    if (extents_b_a.length2() < extents_a_b.length2())
////    {
//        // a is the reference frame
//        Eigen::Vector3d extents, origin;
//        tf::vectorTFToEigen(extents_a, extents);
//        tf::vectorTFToEigen(a.getOrigin(), origin);
//        Eigen::AlignedBox<float, 3> box_a(origin + extents, origin - extents);

////        Eigen::AlignedBox<float, 3> box_b(v, v);

////        Eigen::AlignedBox<float, 3> box_i = box_a.intersection(box_b);

////        tf::vectorEigenToTF(box_i.center(), n.getOrigin());
////        tf::vectorEigenToTF(box_i.max(), extents_n);
////        n.setRotation(a.getRotation());
////    }
////    else {
////        extents_n = (extents_b - extents_a_b).absolute();
////        n.setOrigin(b.getOrigin() + extents_b - extents_n * 0.5);
////        n.setRotation(b.getRotation());
////    }
}

bool PoseSet::intersectBoxes(const tf::Transform& a, const tf::Vector3& extents_a, const tf::Transform& b, const tf::Vector3& extents_b)
{
    tf::Matrix3x3 A = a.getBasis();
    tf::Matrix3x3 B = b.getBasis();

    //translation, in parent frame
    tf::Vector3 v = b.getOrigin() - a.getOrigin();
    //translation, in A's frame
    tf::Vector3 T(v.dot(A.getColumn(0)), v.dot(A.getColumn(1)), v.dot(A.getColumn(2)));

    //B's basis with respect to A's local frame
    tf::Matrix3x3 R = A.transposeTimes(B);
    float ra, rb, t;
    long i, k;

    //calculate rotation matrix
//        for( i=0 ; i<3 ; i++ )
//            for( k=0 ; k<3 ; k++ )
//                R[i][k] = A.getColumn(i).dot(B.getColumn(k));
    /*ALGORITHM: Use the separating axis test for all 15 potential
separating axes. If a separating axis could not be found, the two
boxes overlap. */

    //A's basis vectors
    for( i=0 ; i<3 ; i++ )
    {
        ra = extents_a[i];
        rb = extents_b[0]*fabs(R[i][0]) + extents_b[1]*fabs(R[i][1]) + extents_b[2]*fabs(R[i][2]);

        t = fabs( T[i] );

        if( t > ra + rb )
            return false;
    }

    //B's basis vectors
    for( k=0 ; k<3 ; k++ )
    {
        ra = extents_a[0]*fabs(R[0][k]) + extents_a[1]*fabs(R[1][k]) + extents_a[2]*fabs(R[2][k]);
        rb = extents_b[k];

        t = fabs( T[0]*R[0][k] + T[1]*R[1][k] + T[2]*R[2][k] );

        if( t > ra + rb )
            return false;
    }

    //9 cross products

    //L = A0 x B0
    ra = extents_a[1]*fabs(R[2][0]) + extents_a[2]*fabs(R[1][0]);

    rb = extents_b[1]*fabs(R[0][2]) + extents_b[2]*fabs(R[0][1]);

    t = fabs( T[2]*R[1][0] - T[1]*R[2][0] );

    if( t > ra + rb )
        return false;

    //L = A0 x B1
    ra = extents_a[1]*fabs(R[2][1]) + extents_a[2]*fabs(R[1][1]);

    rb = extents_b[0]*fabs(R[0][2]) + extents_b[2]*fabs(R[0][0]);

    t = fabs( T[2]*R[1][1] - T[1]*R[2][1] );

    if( t > ra + rb )
        return false;

    //L = A0 x B2
    ra = extents_a[1]*fabs(R[2][2]) + extents_a[2]*fabs(R[1][2]);

    rb = extents_b[0]*fabs(R[0][1]) + extents_b[1]*fabs(R[0][0]);

    t = fabs( T[2]*R[1][2] - T[1]*R[2][2] );

    if( t > ra + rb )
        return false;

    //L = A1 x B0
    ra = extents_a[0]*fabs(R[2][0]) + extents_a[2]*fabs(R[0][0]);

    rb = extents_b[1]*fabs(R[1][2]) + extents_b[2]*fabs(R[1][1]);

    t = fabs( T[0]*R[2][0] - T[2]*R[0][0] );

    if( t > ra + rb )
        return false;

    //L = A1 x B1
    ra = extents_a[0]*fabs(R[2][1]) + extents_a[2]*fabs(R[0][1]);

    rb = extents_b[0]*fabs(R[1][2]) + extents_b[2]*fabs(R[1][0]);

    t = fabs( T[0]*R[2][1] - T[2]*R[0][1] );

    if( t > ra + rb )
        return false;

    //L = A1 x B2
    ra = extents_a[0]*fabs(R[2][2]) + extents_a[2]*fabs(R[0][2]);

    rb = extents_b[0]*fabs(R[1][1]) + extents_b[1]*fabs(R[1][0]);

    t = fabs( T[0]*R[2][2] - T[2]*R[0][2] );

    if( t > ra + rb )
        return false;

    //L = A2 x B0
    ra = extents_a[0]*fabs(R[1][0]) + extents_a[1]*fabs(R[0][0]);

    rb = extents_b[1]*fabs(R[2][2]) + extents_b[2]*fabs(R[2][1]);

    t = fabs( T[1]*R[0][0] - T[0]*R[1][0] );

    if( t > ra + rb )
        return false;

    //L = A2 x B1
    ra = extents_a[0]*fabs(R[1][1]) + extents_a[1]*fabs(R[0][1]);

    rb = extents_b[0] *fabs(R[2][2]) + extents_b[2]*fabs(R[2][0]);

    t = fabs( T[1]*R[0][1] - T[0]*R[1][1] );

    if( t > ra + rb )
        return false;

    //L = A2 x B2
    ra = extents_a[0]*fabs(R[1][2]) + extents_a[1]*fabs(R[0][2]);

    rb = extents_b[0]*fabs(R[2][1]) + extents_b[1]*fabs(R[2][0]);

    t = fabs( T[1]*R[0][2] - T[0]*R[1][2] );

    if( t > ra + rb )
        return false;

    /*no separating axis found, the two boxes overlap */

    return true;
}

}


// boost\math\distributions\non_central_beta.hpp

// Copyright John Maddock 2008.

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

#ifndef BOOST_MATH_SPECIAL_NON_CENTRAL_BETA_HPP
#define BOOST_MATH_SPECIAL_NON_CENTRAL_BETA_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/beta.hpp> // for incomplete gamma. gamma_q
#include <boost/math/distributions/complement.hpp> // complements
#include <boost/math/distributions/beta.hpp> // central distribution
#include <boost/math/distributions/detail/generic_mode.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp> // isnan.
#include <boost/math/tools/roots.hpp> // for root finding.
#include <boost/math/tools/series.hpp>

namespace boost
{
   namespace math
   {

      template <class RealType, class Policy>
      class non_central_beta_distribution;

      namespace detail{

         template <class T, class Policy>
         T non_central_beta_p(T a, T b, T lam, T x, T y, const Policy& pol, T init_val = 0)
         {
            BOOST_MATH_STD_USING
               using namespace boost::math;
            //
            // Variables come first:
            //
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();
            T l2 = lam / 2;
            //
            // k is the starting point for iteration, and is the
            // maximum of the poisson weighting term,
            // note that unlike other similar code, we do not set
            // k to zero, when l2 is small, as forward iteration
            // is unstable:
            //
            int k = itrunc(l2);
            if(k == 0)
               k = 1;
            T pois;
            if(k == 0)
            {
               // Starting Poisson weight:
               pois = exp(-l2);
            }
            else
            {
               // Starting Poisson weight:
               pois = gamma_p_derivative(T(k+1), l2, pol);
            }
            if(pois == 0)
               return init_val;
            // recurance term:
            T xterm;
            // Starting beta term:
            T beta = x < y
               ? detail::ibeta_imp(T(a + k), b, x, pol, false, true, &xterm)
               : detail::ibeta_imp(b, T(a + k), y, pol, true, true, &xterm);

            xterm *= y / (a + b + k - 1);
            T poisf(pois), betaf(beta), xtermf(xterm);
            T sum = init_val;

            if((beta == 0) && (xterm == 0))
               return init_val;

            //
            // Backwards recursion first, this is the stable
            // direction for recursion:
            //
            T last_term = 0;
            boost::uintmax_t count = k;
            for(int i = k; i >= 0; --i)
            {
               T term = beta * pois;
               sum += term;
               if(((fabs(term/sum) < errtol) && (last_term >= term)) || (term == 0))
               {
                  count = k - i;
                  break;
               }
               pois *= i / l2;
               beta += xterm;
               xterm *= (a + i - 1) / (x * (a + b + i - 2));
               last_term = term;
            }
            for(int i = k + 1; ; ++i)
            {
               poisf *= l2 / i;
               xtermf *= (x * (a + b + i - 2)) / (a + i - 1);
               betaf -= xtermf;

               T term = poisf * betaf;
               sum += term;
               if((fabs(term/sum) < errtol) || (term == 0))
               {
                  break;
               }
               if(static_cast<boost::uintmax_t>(count + i - k) > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "cdf(non_central_beta_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
            }
            return sum;
         }

         template <class T, class Policy>
         T non_central_beta_q(T a, T b, T lam, T x, T y, const Policy& pol, T init_val = 0)
         {
            BOOST_MATH_STD_USING
               using namespace boost::math;
            //
            // Variables come first:
            //
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();
            T l2 = lam / 2;
            //
            // k is the starting point for iteration, and is the
            // maximum of the poisson weighting term:
            //
            int k = itrunc(l2);
            T pois;
            if(k <= 30)
            {
               //
               // Might as well start at 0 since we'll likely have this number of terms anyway:
               //
               if(a + b > 1)
                  k = 0;
               else if(k == 0)
                  k = 1;
            }
            if(k == 0)
            {
               // Starting Poisson weight:
               pois = exp(-l2);
            }
            else
            {
               // Starting Poisson weight:
               pois = gamma_p_derivative(T(k+1), l2, pol);
            }
            if(pois == 0)
               return init_val;
            // recurance term:
            T xterm;
            // Starting beta term:
            T beta = x < y
               ? detail::ibeta_imp(T(a + k), b, x, pol, true, true, &xterm)
               : detail::ibeta_imp(b, T(a + k), y, pol, false, true, &xterm);

            xterm *= y / (a + b + k - 1);
            T poisf(pois), betaf(beta), xtermf(xterm);
            T sum = init_val;
            if((beta == 0) && (xterm == 0))
               return init_val;
            //
            // Forwards recursion first, this is the stable
            // direction for recursion, and the location
            // of the bulk of the sum:
            //
            T last_term = 0;
            boost::uintmax_t count = 0;
            for(int i = k + 1; ; ++i)
            {
               poisf *= l2 / i;
               xtermf *= (x * (a + b + i - 2)) / (a + i - 1);
               betaf += xtermf;

               T term = poisf * betaf;
               sum += term;
               if((fabs(term/sum) < errtol) && (last_term >= term))
               {
                  count = i - k;
                  break;
               }
               if(static_cast<boost::uintmax_t>(i - k) > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "cdf(non_central_beta_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
               last_term = term;
            }
            for(int i = k; i >= 0; --i)
            {
               T term = beta * pois;
               sum += term;
               if(fabs(term/sum) < errtol)
               {
                  break;
               }
               if(static_cast<boost::uintmax_t>(count + k - i) > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "cdf(non_central_beta_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
               pois *= i / l2;
               beta -= xterm;
               xterm *= (a + i - 1) / (x * (a + b + i - 2));
            }
            return sum;
         }

         template <class RealType, class Policy>
         inline RealType non_central_beta_cdf(RealType x, RealType y, RealType a, RealType b, RealType l, bool invert, const Policy&)
         {
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;

            BOOST_MATH_STD_USING

            if(x == 0)
               return invert ? 1.0f : 0.0f;
            if(y == 0)
               return invert ? 0.0f : 1.0f;
            value_type result;
            value_type c = a + b + l / 2;
            value_type cross = 1 - (b / c) * (1 + l / (2 * c * c));
            if(l == 0)
               result = cdf(boost::math::beta_distribution<RealType, Policy>(a, b), x);
            else if(x > cross)
            {
               // Complement is the smaller of the two:
               result = detail::non_central_beta_q(
                  static_cast<value_type>(a),
                  static_cast<value_type>(b),
                  static_cast<value_type>(l),
                  static_cast<value_type>(x),
                  static_cast<value_type>(y),
                  forwarding_policy(),
                  static_cast<value_type>(invert ? 0 : -1));
               invert = !invert;
            }
            else
            {
               result = detail::non_central_beta_p(
                  static_cast<value_type>(a),
                  static_cast<value_type>(b),
                  static_cast<value_type>(l),
                  static_cast<value_type>(x),
                  static_cast<value_type>(y),
                  forwarding_policy(),
                  static_cast<value_type>(invert ? -1 : 0));
            }
            if(invert)
               result = -result;
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               "boost::math::non_central_beta_cdf<%1%>(%1%, %1%, %1%)");
         }

         template <class T, class Policy>
         struct nc_beta_quantile_functor
         {
            nc_beta_quantile_functor(const non_central_beta_distribution<T,Policy>& d, T t, bool c)
               : dist(d), target(t), comp(c) {}

            T operator()(const T& x)
            {
               return comp ?
                  T(target - cdf(complement(dist, x)))
                  : T(cdf(dist, x) - target);
            }

         private:
            non_central_beta_distribution<T,Policy> dist;
            T target;
            bool comp;
         };

         //
         // This is more or less a copy of bracket_and_solve_root, but
         // modified to search only the interval [0,1] using similar
         // heuristics.
         //
         template <class F, class T, class Tol, class Policy>
         std::pair<T, T> bracket_and_solve_root_01(F f, const T& guess, T factor, bool rising, Tol tol, boost::uintmax_t& max_iter, const Policy& pol)
         {
            BOOST_MATH_STD_USING
               static const char* function = "boost::math::tools::bracket_and_solve_root_01<%1%>";
            //
            // Set up inital brackets:
            //
            T a = guess;
            T b = a;
            T fa = f(a);
            T fb = fa;
            //
            // Set up invocation count:
            //
            boost::uintmax_t count = max_iter - 1;

            if((fa < 0) == (guess < 0 ? !rising : rising))
            {
               //
               // Zero is to the right of b, so walk upwards
               // until we find it:
               //
               while((boost::math::sign)(fb) == (boost::math::sign)(fa))
               {
                  if(count == 0)
                  {
                     b = policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", b, pol);
                     return std::make_pair(a, b);
                  }
                  //
                  // Heuristic: every 20 iterations we double the growth factor in case the
                  // initial guess was *really* bad !
                  //
                  if((max_iter - count) % 20 == 0)
                     factor *= 2;
                  //
                  // Now go ahead and move are guess by "factor",
                  // we do this by reducing 1-guess by factor:
                  //
                  a = b;
                  fa = fb;
                  b = 1 - ((1 - b) / factor);
                  fb = f(b);
                  --count;
                  BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
               }
            }
            else
            {
               //
               // Zero is to the left of a, so walk downwards
               // until we find it:
               //
               while((boost::math::sign)(fb) == (boost::math::sign)(fa))
               {
                  if(fabs(a) < tools::min_value<T>())
                  {
                     // Escape route just in case the answer is zero!
                     max_iter -= count;
                     max_iter += 1;
                     return a > 0 ? std::make_pair(T(0), T(a)) : std::make_pair(T(a), T(0));
                  }
                  if(count == 0)
                  {
                     a = policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", a, pol);
                     return std::make_pair(a, b);
                  }
                  //
                  // Heuristic: every 20 iterations we double the growth factor in case the
                  // initial guess was *really* bad !
                  //
                  if((max_iter - count) % 20 == 0)
                     factor *= 2;
                  //
                  // Now go ahead and move are guess by "factor":
                  //
                  b = a;
                  fb = fa;
                  a /= factor;
                  fa = f(a);
                  --count;
                  BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
               }
            }
            max_iter -= count;
            max_iter += 1;
            std::pair<T, T> r = toms748_solve(
               f,
               (a < 0 ? b : a),
               (a < 0 ? a : b),
               (a < 0 ? fb : fa),
               (a < 0 ? fa : fb),
               tol,
               count,
               pol);
            max_iter += count;
            BOOST_MATH_INSTRUMENT_CODE("max_iter = " << max_iter << " count = " << count);
            return r;
         }

         template <class RealType, class Policy>
         RealType nc_beta_quantile(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& p, bool comp)
         {
            static const char* function = "quantile(non_central_beta_distribution<%1%>, %1%)";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;

            value_type a = dist.alpha();
            value_type b = dist.beta();
            value_type l = dist.non_centrality();
            value_type r;
            if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !detail::check_probability(
               function,
               static_cast<value_type>(p),
               &r,
               Policy()))
                  return (RealType)r;
            //
            // Special cases first:
            //
            if(p == 0)
               return comp
               ? 1.0f
               : 0.0f;
            if(p == 1)
               return !comp
               ? 1.0f
               : 0.0f;

            value_type c = a + b + l / 2;
            value_type mean = 1 - (b / c) * (1 + l / (2 * c * c));
            /*
            //
            // Calculate a normal approximation to the quantile,
            // uses mean and variance approximations from:
            // Algorithm AS 310:
            // Computing the Non-Central Beta Distribution Function
            // R. Chattamvelli; R. Shanmugam
            // Applied Statistics, Vol. 46, No. 1. (1997), pp. 146-156.
            //
            // Unfortunately, when this is wrong it tends to be *very*
            // wrong, so it's disabled for now, even though it often
            // gets the initial guess quite close.  Probably we could
            // do much better by factoring in the skewness if only
            // we could calculate it....
            //
            value_type delta = l / 2;
            value_type delta2 = delta * delta;
            value_type delta3 = delta * delta2;
            value_type delta4 = delta2 * delta2;
            value_type G = c * (c + 1) + delta;
            value_type alpha = a + b;
            value_type alpha2 = alpha * alpha;
            value_type eta = (2 * alpha + 1) * (2 * alpha + 1) + 1;
            value_type H = 3 * alpha2 + 5 * alpha + 2;
            value_type F = alpha2 * (alpha + 1) + H * delta
               + (2 * alpha + 4) * delta2 + delta3;
            value_type P = (3 * alpha + 1) * (9 * alpha + 17)
               + 2 * alpha * (3 * alpha + 2) * (3 * alpha + 4) + 15;
            value_type Q = 54 * alpha2 + 162 * alpha + 130;
            value_type R = 6 * (6 * alpha + 11);
            value_type D = delta
               * (H * H + 2 * P * delta + Q * delta2 + R * delta3 + 9 * delta4);
            value_type variance = (b / G)
               * (1 + delta * (l * l + 3 * l + eta) / (G * G))
               - (b * b / F) * (1 + D / (F * F));
            value_type sd = sqrt(variance);

            value_type guess = comp
               ? quantile(complement(normal_distribution<RealType, Policy>(static_cast<RealType>(mean), static_cast<RealType>(sd)), p))
               : quantile(normal_distribution<RealType, Policy>(static_cast<RealType>(mean), static_cast<RealType>(sd)), p);

            if(guess >= 1)
               guess = mean;
            if(guess <= tools::min_value<value_type>())
               guess = mean;
            */
            value_type guess = mean;
            detail::nc_beta_quantile_functor<value_type, Policy>
               f(non_central_beta_distribution<value_type, Policy>(a, b, l), p, comp);
            tools::eps_tolerance<value_type> tol(policies::digits<RealType, Policy>());
            boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();

            std::pair<value_type, value_type> ir
               = bracket_and_solve_root_01(
                  f, guess, value_type(2.5), true, tol,
                  max_iter, Policy());
            value_type result = ir.first + (ir.second - ir.first) / 2;

            if(max_iter >= policies::get_max_root_iterations<Policy>())
            {
               return policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
                  " either there is no answer to quantile of the non central beta distribution"
                  " or the answer is infinite.  Current best guess is %1%",
                  policies::checked_narrowing_cast<RealType, forwarding_policy>(
                     result,
                     function), Policy());
            }
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }

         template <class T, class Policy>
         T non_central_beta_pdf(T a, T b, T lam, T x, T y, const Policy& pol)
         {
            BOOST_MATH_STD_USING
               using namespace boost::math;
            //
            // Variables come first:
            //
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();
            T l2 = lam / 2;
            //
            // k is the starting point for iteration, and is the
            // maximum of the poisson weighting term:
            //
            int k = itrunc(l2);
            // Starting Poisson weight:
            T pois = gamma_p_derivative(T(k+1), l2, pol);
            // Starting beta term:
            T beta = x < y ?
               ibeta_derivative(a + k, b, x, pol)
               : ibeta_derivative(b, a + k, y, pol);
            T sum = 0;
            T poisf(pois);
            T betaf(beta);

            //
            // Stable backwards recursion first:
            //
            boost::uintmax_t count = k;
            for(int i = k; i >= 0; --i)
            {
               T term = beta * pois;
               sum += term;
               if((fabs(term/sum) < errtol) || (term == 0))
               {
                  count = k - i;
                  break;
               }
               pois *= i / l2;
               beta *= (a + i - 1) / (x * (a + i + b - 1));
            }
            for(int i = k + 1; ; ++i)
            {
               poisf *= l2 / i;
               betaf *= x * (a + b + i - 1) / (a + i - 1);

               T term = poisf * betaf;
               sum += term;
               if((fabs(term/sum) < errtol) || (term == 0))
               {
                  break;
               }
               if(static_cast<boost::uintmax_t>(count + i - k) > max_iter)
               {
                  return policies::raise_evaluation_error(
                     "pdf(non_central_beta_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               }
            }
            return sum;
         }

         template <class RealType, class Policy>
         RealType nc_beta_pdf(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& x)
         {
            BOOST_MATH_STD_USING
            static const char* function = "pdf(non_central_beta_distribution<%1%>, %1%)";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;

            value_type a = dist.alpha();
            value_type b = dist.beta();
            value_type l = dist.non_centrality();
            value_type r;
            if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !beta_detail::check_x(
               function,
               static_cast<value_type>(x),
               &r,
               Policy()))
                  return (RealType)r;

            if(l == 0)
               return pdf(boost::math::beta_distribution<RealType, Policy>(dist.alpha(), dist.beta()), x);
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               non_central_beta_pdf(a, b, l, static_cast<value_type>(x), value_type(1 - static_cast<value_type>(x)), forwarding_policy()),
               "function");
         }

         template <class T>
         struct hypergeometric_2F2_sum
         {
            typedef T result_type;
            hypergeometric_2F2_sum(T a1_, T a2_, T b1_, T b2_, T z_) : a1(a1_), a2(a2_), b1(b1_), b2(b2_), z(z_), term(1), k(0) {}
            T operator()()
            {
               T result = term;
               term *= a1 * a2 / (b1 * b2);
               a1 += 1;
               a2 += 1;
               b1 += 1;
               b2 += 1;
               k += 1;
               term /= k;
               term *= z;
               return result;
            }
            T a1, a2, b1, b2, z, term, k;
         };

         template <class T, class Policy>
         T hypergeometric_2F2(T a1, T a2, T b1, T b2, T z, const Policy& pol)
         {
            typedef typename policies::evaluation<T, Policy>::type value_type;

            const char* function = "boost::math::detail::hypergeometric_2F2<%1%>(%1%,%1%,%1%,%1%,%1%)";

            hypergeometric_2F2_sum<value_type> s(a1, a2, b1, b2, z);
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
            value_type zero = 0;
            value_type result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<value_type, Policy>(), max_iter, zero);
#else
            value_type result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<value_type, Policy>(), max_iter);
#endif
            policies::check_series_iterations<T>(function, max_iter, pol);
            return policies::checked_narrowing_cast<T, Policy>(result, function);
         }

      } // namespace detail

      template <class RealType = double, class Policy = policies::policy<> >
      class non_central_beta_distribution
      {
      public:
         typedef RealType value_type;
         typedef Policy policy_type;

         non_central_beta_distribution(RealType a_, RealType b_, RealType lambda) : a(a_), b(b_), ncp(lambda)
         {
            const char* function = "boost::math::non_central_beta_distribution<%1%>::non_central_beta_distribution(%1%,%1%)";
            RealType r;
            beta_detail::check_alpha(
               function,
               a, &r, Policy());
            beta_detail::check_beta(
               function,
               b, &r, Policy());
            detail::check_non_centrality(
               function,
               lambda,
               &r,
               Policy());
         } // non_central_beta_distribution constructor.

         RealType alpha() const
         { // Private data getter function.
            return a;
         }
         RealType beta() const
         { // Private data getter function.
            return b;
         }
         RealType non_centrality() const
         { // Private data getter function.
            return ncp;
         }
      private:
         // Data member, initialized by constructor.
         RealType a;   // alpha.
         RealType b;   // beta.
         RealType ncp; // non-centrality parameter
      }; // template <class RealType, class Policy> class non_central_beta_distribution

      typedef non_central_beta_distribution<double> non_central_beta; // Reserved name of type double.

      // Non-member functions to give properties of the distribution.

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> range(const non_central_beta_distribution<RealType, Policy>& /* dist */)
      { // Range of permissible values for random variable k.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
      }

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> support(const non_central_beta_distribution<RealType, Policy>& /* dist */)
      { // Range of supported values for random variable k.
         // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
      }

      template <class RealType, class Policy>
      inline RealType mode(const non_central_beta_distribution<RealType, Policy>& dist)
      { // mode.
         static const char* function = "mode(non_central_beta_distribution<%1%> const&)";

         RealType a = dist.alpha();
         RealType b = dist.beta();
         RealType l = dist.non_centrality();
         RealType r;
         if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy()))
                  return (RealType)r;
         RealType c = a + b + l / 2;
         RealType mean = 1 - (b / c) * (1 + l / (2 * c * c));
         return detail::generic_find_mode_01(
            dist,
            mean,
            function);
      }

      //
      // We don't have the necessary information to implement
      // these at present.  These are just disabled for now,
      // prototypes retained so we can fill in the blanks
      // later:
      //
      template <class RealType, class Policy>
      inline RealType mean(const non_central_beta_distribution<RealType, Policy>& dist)
      {
         BOOST_MATH_STD_USING
         RealType a = dist.alpha();
         RealType b = dist.beta();
         RealType d = dist.non_centrality();
         RealType apb = a + b;
         return exp(-d / 2) * a * detail::hypergeometric_2F2<RealType, Policy>(1 + a, apb, a, 1 + apb, d / 2, Policy()) / apb;
      } // mean

      template <class RealType, class Policy>
      inline RealType variance(const non_central_beta_distribution<RealType, Policy>& dist)
      {
         //
         // Relative error of this function may be arbitarily large... absolute
         // error will be small however... that's the best we can do for now.
         //
         BOOST_MATH_STD_USING
         RealType a = dist.alpha();
         RealType b = dist.beta();
         RealType d = dist.non_centrality();
         RealType apb = a + b;
         RealType result = detail::hypergeometric_2F2(RealType(1 + a), apb, a, RealType(1 + apb), RealType(d / 2), Policy());
         result *= result * -exp(-d) * a * a / (apb * apb);
         result += exp(-d / 2) * a * (1 + a) * detail::hypergeometric_2F2(RealType(2 + a), apb, a, RealType(2 + apb), RealType(d / 2), Policy()) / (apb * (1 + apb));
         return result;
      }

      // RealType standard_deviation(const non_central_beta_distribution<RealType, Policy>& dist)
      // standard_deviation provided by derived accessors.
      template <class RealType, class Policy>
      inline RealType skewness(const non_central_beta_distribution<RealType, Policy>& /*dist*/)
      { // skewness = sqrt(l).
         const char* function = "boost::math::non_central_beta_distribution<%1%>::skewness()";
         typedef typename Policy::assert_undefined_type assert_type;
         BOOST_STATIC_ASSERT(assert_type::value == 0);

         return policies::raise_evaluation_error<RealType>(
            function,
            "This function is not yet implemented, the only sensible result is %1%.",
            std::numeric_limits<RealType>::quiet_NaN(), Policy()); // infinity?
      }

      template <class RealType, class Policy>
      inline RealType kurtosis_excess(const non_central_beta_distribution<RealType, Policy>& /*dist*/)
      {
         const char* function = "boost::math::non_central_beta_distribution<%1%>::kurtosis_excess()";
         typedef typename Policy::assert_undefined_type assert_type;
         BOOST_STATIC_ASSERT(assert_type::value == 0);

         return policies::raise_evaluation_error<RealType>(
            function,
            "This function is not yet implemented, the only sensible result is %1%.",
            std::numeric_limits<RealType>::quiet_NaN(), Policy()); // infinity?
      } // kurtosis_excess

      template <class RealType, class Policy>
      inline RealType kurtosis(const non_central_beta_distribution<RealType, Policy>& dist)
      {
         return kurtosis_excess(dist) + 3;
      }

      template <class RealType, class Policy>
      inline RealType pdf(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& x)
      { // Probability Density/Mass Function.
         return detail::nc_beta_pdf(dist, x);
      } // pdf

      template <class RealType, class Policy>
      RealType cdf(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& x)
      {
         const char* function = "boost::math::non_central_beta_distribution<%1%>::cdf(%1%)";
            RealType a = dist.alpha();
            RealType b = dist.beta();
            RealType l = dist.non_centrality();
            RealType r;
            if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !beta_detail::check_x(
               function,
               x,
               &r,
               Policy()))
                  return (RealType)r;

         if(l == 0)
            return cdf(beta_distribution<RealType, Policy>(a, b), x);

         return detail::non_central_beta_cdf(x, RealType(1 - x), a, b, l, false, Policy());
      } // cdf

      template <class RealType, class Policy>
      RealType cdf(const complemented2_type<non_central_beta_distribution<RealType, Policy>, RealType>& c)
      { // Complemented Cumulative Distribution Function
         const char* function = "boost::math::non_central_beta_distribution<%1%>::cdf(%1%)";
         non_central_beta_distribution<RealType, Policy> const& dist = c.dist;
            RealType a = dist.alpha();
            RealType b = dist.beta();
            RealType l = dist.non_centrality();
            RealType x = c.param;
            RealType r;
            if(!beta_detail::check_alpha(
               function,
               a, &r, Policy())
               ||
            !beta_detail::check_beta(
               function,
               b, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !beta_detail::check_x(
               function,
               x,
               &r,
               Policy()))
                  return (RealType)r;

         if(l == 0)
            return cdf(complement(beta_distribution<RealType, Policy>(a, b), x));

         return detail::non_central_beta_cdf(x, RealType(1 - x), a, b, l, true, Policy());
      } // ccdf

      template <class RealType, class Policy>
      inline RealType quantile(const non_central_beta_distribution<RealType, Policy>& dist, const RealType& p)
      { // Quantile (or Percent Point) function.
         return detail::nc_beta_quantile(dist, p, false);
      } // quantile

      template <class RealType, class Policy>
      inline RealType quantile(const complemented2_type<non_central_beta_distribution<RealType, Policy>, RealType>& c)
      { // Quantile (or Percent Point) function.
         return detail::nc_beta_quantile(c.dist, c.param, true);
      } // quantile complement.

   } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_SPECIAL_NON_CENTRAL_BETA_HPP


#include "focal_grid.h"

#include <string>
#include <stdint.h>
#include <iostream>
#include <iomanip>
#include <math.h>
#include <chrono>
#include <vector>
#include <map>
#include <unordered_map>
#include <iterator>
#include <future>
#include <thread>
#include <algorithm>
#include <dirent.h>
#include <armadillo>

using namespace std;

FocalGrid::FocalGrid() {
	// empty
}

bool FocalGrid::LoadCalibration(session_param &ses_par, vox_grid &vox, frames &frame) {

	bool grid_loaded = true;

	try {

		vox.image_size.clear();
		vox.calib_mat.clear();
		vox.X_xyz.clear();
		vox.X_uv.clear();
		vox.uv_offset.clear();

		image_size.clear();
		calib_mat.clear();
		X_xyz.clear();
		X_uv.clear();
		uv_offset.clear();

		string calib_file = ses_par.session_loc + "/" + ses_par.cal_loc + "/" + ses_par.cal_name;

		arma::Mat<double> CalibMatrix;

		CalibMatrix.load(calib_file);

		for (int i=0; i<ses_par.N_cam; i++) {
			vox.image_size.push_back(frame.image_size[i]);
			vox.calib_mat.push_back(CalibMatrix.col(i));
			vox.X_xyz.push_back(FocalGrid::Camera2WorldMatrix(CalibMatrix.col(i)));
			vox.X_uv.push_back(FocalGrid::World2CameraMatrix(CalibMatrix.col(i)));
			arma::Col<double> uv_off_i = {CalibMatrix(10,i)/2.0-CalibMatrix(12,i)/2.0, CalibMatrix(9,i)/2.0-CalibMatrix(11,i)/2.0, 0.0};
			vox.uv_offset.push_back(uv_off_i);

			image_size.push_back(frame.image_size[i]);
			calib_mat.push_back(CalibMatrix.col(i));
			X_xyz.push_back(FocalGrid::Camera2WorldMatrix(CalibMatrix.col(i)));
			X_uv.push_back(FocalGrid::World2CameraMatrix(CalibMatrix.col(i)));
			uv_offset.push_back(uv_off_i);
		}

		// Populate internal parameters

		N_cam = vox.N_cam;
		N_threads = vox.N_threads;
		nx = vox.nx;
		ny = vox.ny;
		nz = vox.nz;
		ds = vox.ds;
		x0 = vox.x0;
		y0 = vox.y0;
		z0 = vox.z0;

	}
	catch (...) {
		grid_loaded = false;
	}

	return grid_loaded;
}

bool FocalGrid::ConstructFocalGrid(vox_grid &vox) {

	bool grid_build = true;

	int voxel_ind = 0;

	try {

		vox.pix2vox.clear();

		vox.vox2pix.clear();

		for (int k=0; k<nz; k++) {
			cout << k << endl;
			for (int j=0; j<ny; j++) {
				for (int i=0; i<nx; i++) {
					vector<int> uv_voxel = FocalGrid::CheckVoxel(i, j, k);
					if (uv_voxel.size()==N_cam) {
						voxel_ind = k*nx*ny+j*nx+i;
						vox.pix2vox.insert(pair<int,int>(uv_voxel[0],voxel_ind));
						vox.vox2pix.insert(pair<int,vector<int>>(voxel_ind,uv_voxel));
					}
				}
			}
		}

	}
	catch (...) {
		grid_build = false;
	}

	return grid_build;
}

vector<int> FocalGrid::CheckVoxel(int i, int j, int k) {

	vector<int> uv_out;

	int uv_ind = -1;

	arma::Col<double> xyz(4);

	xyz = {x0-((nx-1)/2.0)*ds+i*ds,
		y0-((ny-1)/2.0)*ds+j*ds,
		z0-((nz-1)/2.0)*ds+k*ds,
		1.0};

	arma::Col<double> uv(3);

	int n=0;
	bool uv_out_of_range = false;

	while (n<N_cam && uv_out_of_range==false) {

		uv = X_uv[n]*xyz-uv_offset[n];

		if (uv(0)>=0 && uv(0)<(get<1>(image_size[n]))) {
			if (uv(1)>=0 && uv(1)<(get<0>(image_size[n]))) {
				uv_ind = ((int) uv(1))*get<1>(image_size[n])+((int) uv(0));
				uv_out.push_back(uv_ind);
			}
			else {
				uv_out.clear();
				uv_out_of_range = true;
			}
		}
		else {
			uv_out.clear();
			uv_out_of_range = true;
		}
		n++;
	}

	return uv_out;
}

arma::Col<double> FocalGrid::PointCloudMatching(arma::Mat<double> &dest_pcl, arma::Mat<double> &src_pcl, double search_radius) {

	// Create a multimap of the two pointclouds and try to find dest-src pairs in the x, y and z direction:

	multimap<int,double> xy_map;
	multimap<int,double> xz_map;
	multimap<int,double> yz_map;

	int N_dest = dest_pcl.n_cols;

	cout << "N_dest" << endl;
	cout << N_dest << endl;

	for (int i=0; i<N_dest; i++) {

		int i_x = (int) (dest_pcl(1,i)-x0)/ds+((nx-1)/2.0);
		int i_y = (int) (dest_pcl(2,i)-y0)/ds+((ny-1)/2.0);
		int i_z = (int) (dest_pcl(3,i)-z0)/ds+((nz-1)/2.0);

		xy_map.insert(pair<int,double>(i_y*nx+i_x,dest_pcl(1,i)));
		xz_map.insert(pair<int,double>(i_x*nz+i_z,dest_pcl(2,i)));
		yz_map.insert(pair<int,double>(i_z*ny+i_y,dest_pcl(3,i)));

	}

	int N_src = src_pcl.n_cols;

	cout << "N_src" << endl;
	cout << N_src << endl;

	arma::Col<double> src_cg;
	src_cg.zeros(3);

	vector<tuple<double,double,double>> delta_x_vec;
	vector<tuple<double,double,double>> delta_y_vec;
	vector<tuple<double,double,double>> delta_z_vec;

	for (int j=0; j<N_src; j++) {

		src_cg(0) += src_pcl(1,j)/(N_src*1.0);
		src_cg(1) += src_pcl(2,j)/(N_src*1.0);
		src_cg(2) += src_pcl(3,j)/(N_src*1.0);

		int j_x = (int) (src_pcl(1,j)-x0)/ds+((nx-1)/2.0);
		int j_y = (int) (src_pcl(2,j)-y0)/ds+((ny-1)/2.0);
		int j_z = (int) (src_pcl(3,j)-z0)/ds+((nz-1)/2.0);

		// delta z
		pair<multimap<int,double>::iterator, multimap<int,double>::iterator> dest_z_voxels;
		dest_z_voxels = xy_map.equal_range(j_y*nx+j_x);
		double delta_z = 100000.0;
		double delta_z_new;
		double z_val;

		for (multimap<int,double>::iterator it_z=dest_z_voxels.first; it_z != dest_z_voxels.second; ++it_z) {
			z_val = it_z->second;
			delta_z_new = z_val-src_pcl(3,j);
			if (abs(delta_z_new)<abs(delta_z)) {
				delta_z = delta_z_new;
			}
		}

		if (abs(delta_z)<search_radius) {
			delta_z_vec.push_back(make_tuple(delta_z,src_pcl(1,j),src_pcl(2,j)));
		}

		// delta y
		pair<multimap<int,double>::iterator, multimap<int,double>::iterator> dest_y_voxels;
		dest_y_voxels = xz_map.equal_range(j_x*nz+j_z);
		double delta_y = 100000.0;
		double delta_y_new;
		double y_val;

		for (multimap<int,double>::iterator it_y=dest_y_voxels.first; it_y != dest_y_voxels.second; ++it_y) {
			y_val = it_y->second;
			delta_y_new = y_val-src_pcl(2,j);
			if (abs(delta_y_new)<abs(delta_y)) {
				delta_y = delta_y_new;
			}
		}

		if (abs(delta_y)<search_radius) {
			delta_y_vec.push_back(make_tuple(delta_y,src_pcl(1,j),src_pcl(3,j)));
		}

		// delta x
		pair<multimap<int,double>::iterator, multimap<int,double>::iterator> dest_x_voxels;
		dest_x_voxels = yz_map.equal_range(j_z*ny+j_y);
		double delta_x = 100000.0;
		double delta_x_new;
		double x_val;

		for (multimap<int,double>::iterator it_x=dest_x_voxels.first; it_x != dest_x_voxels.second; ++it_x) {
			x_val = it_x->second;
			delta_x_new = x_val-src_pcl(1,j);
			if (abs(delta_x_new)<abs(delta_x)) {
				delta_x = delta_x_new;
			}
		}

		if (abs(delta_x)<search_radius) {
			delta_x_vec.push_back(make_tuple(delta_x,src_pcl(2,j),src_pcl(3,j)));
		}

	}

	double theta_x = 0.0;
	double trans_x = 0.0;
	double theta_y = 0.0;
	double trans_y = 0.0;
	double theta_z = 0.0;
	double trans_z = 0.0;

	int N_dx = delta_x_vec.size();
	int N_dy = delta_y_vec.size();
	int N_dz = delta_z_vec.size();

	cout << "delta x size" << endl;
	cout << N_dx << endl;
	cout << "delta y size" << endl;
	cout << N_dy << endl;
	cout << "delta z size" << endl;
	cout << N_dz << endl;

	double mse_x = 0.0;
	double mse_y = 0.0;
	double mse_z = 0.0;

	double theta_x_update;
	double theta_y_update;
	double theta_z_update;
	double trans_x_update;
	double trans_y_update;
	double trans_z_update;

	int N_theta_x = 0;
	int N_theta_y = 0;
	int N_theta_z = 0;
	int N_trans_x = 0;
	int N_trans_y = 0;
	int N_trans_z = 0;

	for (int k=0; k<N_dx; k++) {

		trans_x_update = get<0>(delta_x_vec[k]);
		theta_y_update = atan2(get<0>(delta_x_vec[k]),get<2>(delta_x_vec[k])-src_cg(2));
		theta_z_update = atan2(get<0>(delta_x_vec[k]),get<1>(delta_x_vec[k])-src_cg(1));

		if (isfinite(trans_x_update)==true) {
			trans_x += trans_x_update;
			mse_x += pow(get<0>(delta_x_vec[k]),2);
			N_trans_x++;
		}
		if (isfinite(theta_y_update)==true) {
			theta_y += theta_y_update;
			N_theta_y++;
		}
		if (isfinite(theta_z_update)==true) {
			theta_z -= theta_z_update;
			N_theta_z++;
		}
	}

	for (int k=0; k<N_dy; k++) {

		trans_y_update = get<0>(delta_y_vec[k]);
		theta_x_update = atan2(get<0>(delta_y_vec[k]),get<2>(delta_y_vec[k])-src_cg(2));
		theta_z_update = atan2(get<0>(delta_y_vec[k]),get<2>(delta_y_vec[k])-src_cg(0));

		if (isfinite(trans_y_update)==true) {
			trans_y += trans_y_update;
			mse_y += pow(get<0>(delta_y_vec[k]),2);
			N_trans_y++;
		}
		if (isfinite(theta_x_update)==true) {
			theta_x -= theta_x_update;
			N_theta_x++;
		}
		if (isfinite(theta_z_update)==true) {
			theta_z += theta_z_update;
			N_theta_z++;
		}
	}

	for (int k=0; k<N_dz; k++) {

		trans_z_update = get<0>(delta_z_vec[k]);
		theta_x_update = atan2(get<0>(delta_z_vec[k]),get<2>(delta_z_vec[k])-src_cg(1));
		theta_y_update = atan2(get<0>(delta_z_vec[k]),get<1>(delta_z_vec[k])-src_cg(0));

		if (isfinite(trans_z_update)==true) {
			trans_z += trans_z_update;
			mse_z += pow(get<0>(delta_z_vec[k]),2);
			N_trans_z++;
		}
		if (isfinite(theta_x_update)==true) {
			theta_x += theta_x_update;
			N_theta_x++;
		}
		if (isfinite(theta_y_update)==true) {
			theta_y -= theta_y_update;
			N_theta_y++;
		}
	}

	arma::Col<double> state_update(9);

	if (N_theta_x>3) {
		state_update(0) = theta_x/(N_theta_x*1.0);
	}
	else {
		state_update(0) = 0.0;
	}
	if (N_theta_y>3) {
		state_update(1) = theta_y/(N_theta_y*1.0);
	}
	else {
		state_update(1) = 0.0;
	}
	if (N_theta_z>3) {
		state_update(2) = theta_z/(N_theta_z*1.0);
	}
	else {
		state_update(2) = 0.0;
	}
	if (N_trans_x>3) {
		state_update(3) = trans_x/(N_trans_x*1.0);
		state_update(6) = mse_x/(N_trans_x*1.0);
	}
	else {
		state_update(3) = 0.0;
		state_update(6) = 1.0;
	}
	if (N_trans_y>3) {
		state_update(4) = trans_y/(N_trans_y*1.0);
		state_update(7) = mse_y/(N_trans_y*1.0);
	}
	else {
		state_update(4) = 0.0;
		state_update(7) = 1.0;
	}
	if (N_trans_z>3) {
		state_update(5) = trans_z/(N_trans_z*1.0);
		state_update(8) = mse_z/(N_trans_z*1.0);
	}
	else {
		state_update(5) = 0.0;
		state_update(8) = 1.0;
	}

	cout << "state update" << endl;
	cout << state_update << endl;

	return state_update;
}

vector<tuple<int,double,double,double,double,double,double>> FocalGrid::ProjectImage2Cloud(vector<arma::Col<int>> &frame_in, vox_grid &vox) {

	//vector<tuple<double,double,double,int>> pcl_now;

	vector<tuple<int,double,double,double,double,double,double>> pcl_now;

	unordered_map<int,int> pcl_voxels;

	int N_row = get<0>(vox.image_size[0]);
	int N_col = get<1>(vox.image_size[0]);

	pair<multimap<int,int>::iterator, multimap<int,int>::iterator> voxels_i;

	int vox_now;
	vector<int> uv_now;

	int n = 0;
	bool is_voxel = true;

	int frame_val_0 = 0;
	int frame_val_n = 0;

	int code_now = 0;

	int count = 0;

	// Insert voxels into an unordered map and give them a segment code:
	for (int i=0; i<(N_row*N_col); i++) {
		frame_val_0 = frame_in[0](i);
		if (frame_val_0>0) {
			voxels_i = vox.pix2vox.equal_range(i);
			for (multimap<int,int>::iterator it=voxels_i.first; it != voxels_i.second; ++it) {
				vox_now = it->second;
				uv_now = vox.vox2pix[vox_now];
				n = 1;
				is_voxel = true;
				code_now = frame_val_0;
				while (is_voxel==true && n < N_cam) {
					frame_val_n = frame_in[n](uv_now[n]);
					if (frame_val_n>0) {
						code_now = code_now+pow(max_n_seg,n)*frame_val_n;
					}
					else {
						is_voxel = false;
					}
					n++;
				}
				if (is_voxel==true) {
					count++;
					pcl_voxels.insert(pair<int,int>(vox_now,code_now));
				}
			}
		}
	}

	// For each voxel in the pcl_voxels map:
	// -> find the neighboring voxels
	// -> calculate normal
	// -> if a normal could be calculated, add the voxel_id, xyz_pos and normal to pcl_now

	unordered_map<int,int>::iterator nb_it;

	arma::Col<int> neighbors(27);

	arma::Col<double> nb_vec(27);

	arma::Col<double> normal(3);

	arma::Col<double> xyz_pos(3);

	int nb_vox_id;
	int nb_vox_code;

	for (nb_it = pcl_voxels.begin(); nb_it != pcl_voxels.end(); nb_it++) {
		vox_now = nb_it->first;
		code_now = nb_it->second;
		neighbors = FocalGrid::FindNeighbors(vox_now);
		if (neighbors(13)>0) {
			for (int m=0; m<27; m++) {
				if (pcl_voxels.find(neighbors(m)) != pcl_voxels.end()) {
					nb_vec(m) = 0.0;
				}
				else {
					nb_vec(m) = 1.0;
				}
			}
			normal = FocalGrid::CalculateNormal(nb_vec);
			if (arma::norm(normal,2)>0.5) {
				xyz_pos = FocalGrid::CalculatePosition(vox_now);
				pcl_now.push_back(make_tuple(code_now,xyz_pos(0),xyz_pos(1),xyz_pos(2),normal(0),normal(1),normal(2)));
			}
		}
	}

	if (pcl_now.size() <= 0) {
		pcl_now.push_back(make_tuple(0,0.0,0.0,0.0,0.0,0.0,0.0));
	}
	return pcl_now;
}

arma::Col<int> FocalGrid::FindNeighbors(int vox_ind) {

	// find the neighboring voxels:

	arma::Col<int> vox_int_mat(27);

	int i = vox_ind % nx;
	int j = ((vox_ind-i)/nx) % ny;
	int k = (vox_ind-i-j*nx)/(nx*ny);

	if ((i>0 && i<(nx-1)) && (j>0 && j<(ny-1)) && (k>0 && k<(nz-1))) {

		vox_int_mat(0) = vox_ind-nx*ny-nx-1;
		vox_int_mat(1) = vox_ind-nx*ny-nx;
		vox_int_mat(2) = vox_ind-nx*ny-nx+1;
		vox_int_mat(3) = vox_ind-nx*ny-1;
		vox_int_mat(4) = vox_ind-nx*ny;
		vox_int_mat(5) = vox_ind-nx*ny+1;
		vox_int_mat(6) = vox_ind-nx*ny+nx-1;
		vox_int_mat(7) = vox_ind-nx*ny+nx;
		vox_int_mat(8) = vox_ind-nx*ny+nx+1;
		vox_int_mat(9) = vox_ind-nx-1;
		vox_int_mat(10) = vox_ind-nx;
		vox_int_mat(11) = vox_ind-nx+1;
		vox_int_mat(12) = vox_ind-1;
		vox_int_mat(13) = vox_ind;
		vox_int_mat(14) = vox_ind+1;
		vox_int_mat(15) = vox_ind+nx-1;
		vox_int_mat(16) = vox_ind+nx;
		vox_int_mat(17) = vox_ind+nx+1;
		vox_int_mat(18) = vox_ind+nx*ny-nx-1;
		vox_int_mat(19) = vox_ind+nx*ny-nx;
		vox_int_mat(20) = vox_ind+nx*ny-nx+1;
		vox_int_mat(21) = vox_ind+nx*ny-1;
		vox_int_mat(22) = vox_ind+nx*ny;
		vox_int_mat(23) = vox_ind+nx*ny+1;
		vox_int_mat(24) = vox_ind+nx*ny+nx-1;
		vox_int_mat(25) = vox_ind+nx*ny+nx;
		vox_int_mat(26) = vox_ind+nx*ny+nx+1;

	}
	else {
		vox_int_mat.zeros();
	}

	return vox_int_mat;
}

arma::Col<double> FocalGrid::CalculatePosition(int vox_ind) {

	int i = vox_ind % nx;
	int j = ((vox_ind-i)/nx) % ny;
	int k = (vox_ind-i-j*nx)/(nx*ny);

	arma::Col<double> xyz_pos(3);

	xyz_pos(0) = x0-((nx-1)/2.0)*ds+i*ds;
	xyz_pos(1) = y0-((ny-1)/2.0)*ds+j*ds;
	xyz_pos(2) = z0-((nz-1)/2.0)*ds+k*ds;

	return xyz_pos;
}

arma::Col<double> FocalGrid::CalculateNormal(arma::Col<double> neighbor_vector) {

	arma::Col<double> normal;

	normal.zeros(3);

	if (arma::sum(neighbor_vector)>1.0 && arma::sum(neighbor_vector)<20.0) {
		normal = normal_mat.t()*neighbor_vector;
		if (arma::norm(normal,2)>0.5){
			normal = arma::normalise(normal);
		}
		else {
			normal.zeros(3);
		}
	}

	return normal;
}

vector<arma::Col<int>> FocalGrid::ProjectCloud2Image(vector<tuple<double,double,double,int>> &cloud_in) {

	vector<arma::Col<int>> frame_now;

	int N_vox = cloud_in.size();

	for (int n=0; n<N_cam; n++) {
		arma::Col<int> frame_n;
		frame_n.zeros(get<0>(image_size[n])*get<1>(image_size[n]));
		frame_now.push_back(frame_n);
	}

	arma::Col<double> uv;
	arma::Col<double> xyz;
	int u = 0;
	int v = 0;

	for (int i=0; i<N_vox; i++) {
		for (int j=0; j<N_cam; j++) {
			xyz = {get<0>(cloud_in[i]),get<1>(cloud_in[i]),get<2>(cloud_in[i]),1.0};
			uv = X_uv[j]*xyz-uv_offset[j];
			if (uv(0)>=0 && uv(0)<(get<1>(image_size[j]))) {
				if (uv(1)>=0 && uv(1)<(get<0>(image_size[j]))) {
					u = (int) uv(0);
					v = (int) uv(1);
					if (frame_now[j](get<1>(image_size[j])*v+u)==0) {
						frame_now[j](get<1>(image_size[j])*v+u) = get<3>(cloud_in[i]);
					}
					else {
						if (frame_now[j](get<1>(image_size[j])*v+u)>get<3>(cloud_in[i])) {
							frame_now[j](get<1>(image_size[j])*v+u) = get<3>(cloud_in[i]);
						}
					}
				}
			}
		}
	}
	return frame_now;
}

arma::Mat<int> FocalGrid::TransformXYZ2UV(arma::Col<double> xyz_pos) {

	arma::Mat<int> uv_mat(2,N_cam);

	for (int n=0; n<N_cam; n++) {
		arma::Col<double> uv = X_uv[n]*xyz_pos-uv_offset[n];
		uv_mat(0,n) = int(uv(0));
		uv_mat(1,n) = int(uv(1));
	}

	return uv_mat;
}

tuple<arma::Mat<int>, arma::Col<double>> FocalGrid::RayCasting(int cam_nr, arma::Col<double> xyz_pos_prev, arma::Col<double> uv_pos_prev, arma::Col<double> uv_pos_now) {

	// Calculate the 3D translation vector:
	arma::Col<double> xyz_uv_prev = X_xyz[cam_nr]*(uv_pos_prev+uv_offset[cam_nr]);
	arma::Col<double> xyz_uv_now = X_xyz[cam_nr]*(uv_pos_now+uv_offset[cam_nr]);
	arma::Col<double> trans_vec = xyz_uv_now-xyz_uv_prev;
	trans_vec(3) = 0.0;

	// Add the translation to the xyz position:
	arma::Col<double> xyz_pos_now = xyz_pos_prev + trans_vec;

	// Project the new position back to the camera views:

	arma::Mat<int> uv_mat(3,N_cam);

	for (int n=0; n<N_cam; n++) {
		arma::Col<double> uv = X_uv[n]*xyz_pos_now-uv_offset[n];
		if (uv(0)>=0 && uv(0)<(get<1>(image_size[n])) && uv(1)>=0 && uv(1)<(get<0>(image_size[n]))) {
			uv_mat(0,n) = int(uv(0));
			uv_mat(1,n) = int(uv(1));
			uv_mat(2,n) = int(uv(2));
		}
		else {
			arma::Col<double> uv_old = X_uv[n]*xyz_pos_prev-uv_offset[n];
			uv_mat(0,n) = int(uv_old(0));
			uv_mat(1,n) = int(uv_old(1));
			uv_mat(2,n) = int(uv_old(2));
		}
	}

	return make_tuple(uv_mat,xyz_pos_now);
}

arma::Mat<double> FocalGrid::Camera2WorldMatrix(arma::Col<double> calib_param) {

	// return the world to camera projection matrix

	arma::Mat<double> C = {{calib_param(0), calib_param(2), 0, 0},
		 			{0, calib_param(1), 0, 0},
		 			{0, 0, 0, 1}};

	double theta = sqrt(pow(calib_param(3),2)+pow(calib_param(4),2)+pow(calib_param(5),2));

	arma::Mat<double> omega = {{0, -calib_param(5), calib_param(4)},
			 			{calib_param(5), 0, -calib_param(3)},
			 			{-calib_param(4), calib_param(3), 0}};

	arma::Mat<double> R(3,3); R.eye();

	R = R+(sin(theta)/theta)*omega+((1-cos(theta))/pow(theta,2))*(omega*omega);

	arma::Col<double> T = {calib_param(6), calib_param(7), calib_param(8)};

	arma::Mat<double> K = {{R(0,0), R(0,1), R(0,2), T(0)},
		 				{R(1,0), R(1,1), R(1,2), T(1)},
		 				{R(2,0), R(2,1), R(2,2), T(2)},
		 				{0, 0, 0, 1}};

	return arma::inv(K)*arma::pinv(C);

}

arma::Mat<double> FocalGrid::World2CameraMatrix(arma::Col<double> calib_param) {

	// return the world to camera projection matrix

	arma::Mat<double> C = {{calib_param(0), calib_param(2), 0, 0},
		 			{0, calib_param(1), 0, 0},
		 			{0, 0, 0, 1}};

	double theta = sqrt(pow(calib_param(3),2)+pow(calib_param(4),2)+pow(calib_param(5),2));

	arma::Mat<double> omega = {{0, -calib_param(5), calib_param(4)},
			 			{calib_param(5), 0, -calib_param(3)},
			 			{-calib_param(4), calib_param(3), 0}};

	arma::Mat<double> R(3,3); R.eye();

	R = R+(sin(theta)/theta)*omega+((1-cos(theta))/pow(theta,2))*(omega*omega);

	arma::Col<double> T = {calib_param(6), calib_param(7), calib_param(8)};

	arma::Mat<double> K = {{R(0,0), R(0,1), R(0,2), T(0)},
		 				{R(1,0), R(1,1), R(1,2), T(1)},
		 				{R(2,0), R(2,1), R(2,2), T(2)},
		 				{0, 0, 0, 1}};

	return C*K;

}


// Copyright (C) 2015 The Regents of the University of California (Regents)
// and Google, Inc. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * 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.
//
//     * Neither the name of The Regents or University of California, Google,
//       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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/two_point_pose_partial_rotation.h"

#include <math.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "theia/math/closed_form_polynomial_solver.h"

namespace theia {
using Eigen::Map;
using Eigen::Vector3d;
using Eigen::Quaterniond;

namespace {

// Adds a specific pose solution corresponding to the ray lengths.
inline void AddPoseSolution(const Vector3d& axis,
                            const Vector3d& model_point_1,
                            const Vector3d& model_point_2,
                            const Vector3d& image_ray_1,
                            const Vector3d& image_ray_2,
                            const double ray_length_1,
                            const double ray_length_2,
                            Quaterniond* rotation,
                            Vector3d* translation) {
  // Scales the image_rays by the calculated ray lengths, computing the points
  // in image space.
  const Vector3d point_in_image_space_1 = ray_length_1 * image_ray_1;
  const Vector3d point_in_image_space_2 = ray_length_2 * image_ray_2;
  const Vector3d image_points_diff =
      point_in_image_space_1 - point_in_image_space_2;

  const Vector3d model_points_diff = model_point_1 - model_point_2;

  // Computes two basis vectors that lie on the plane orthogonal to the axis.
  const Vector3d basis_vector_2 = axis.cross(model_points_diff).normalized();
  const Vector3d basis_vector_1 = basis_vector_2.cross(axis).normalized();

  assert(0.0 < basis_vector_1.dot(basis_vector_1));

  // Finds the projection of the image_points_diff vector in this basis.
  const double dp_1 = basis_vector_1.dot(image_points_diff);
  const double dp_2 = basis_vector_2.dot(image_points_diff);

  // Finds the angle around the axis.
  const double angle = atan2(dp_2, dp_1);
  *rotation = Quaterniond(Eigen::AngleAxisd(angle, axis));

  // Calculates the translation, after taking into account the rotation.
  *translation = point_in_image_space_1 - *rotation * model_point_1;
}

int TwoPointPoseCore(const Vector3d& axis,
                     const Vector3d& model_point_1,
                     const Vector3d& model_point_2,
                     const Vector3d& image_ray_1,
                     const Vector3d& image_ray_2,
                     Quaterniond soln_rotations[2],
                     Vector3d soln_translations[2]) {
  // Let the points in the camera coordinate system be
  // y * image_ray_1, x * image_ray_2, where x, y are the lengths of the
  // rays. Since there is only rotation about the passed axis, the difference
  // between the values in the model and camera coordinate systems, projected
  // on the axis, should be the same. So:
  //
  // DotProd(axis, y * image_ray_1 - x * image_ray_2)  =
  //     DotProd(axis, model_points[0] - model_points[1])
  //
  // This allows x to be expressed in terms of y:
  //
  // x = m + n * y
  const double ray_1_axis_dp = image_ray_1.dot(axis);
  const double ray_2_axis_dp = image_ray_2.dot(axis);
  const double model_diff_axis_dp = (model_point_1 - model_point_2).dot(axis);

  const double m = model_diff_axis_dp / ray_1_axis_dp;
  const double n = ray_2_axis_dp / ray_1_axis_dp;

  // Next, the distance between the model points and the image points should
  // be the same:
  //
  // |y * image_ray_1 - x * image_ray_2| =
  //     |model_points[0] - model_points[1]|
  //
  // Using this and the substitution for x above we can create a quadratic
  // equation in y.

  // Computes the coefficients of the quadratic equation ay^2 + by + c = 0.0.
  const double ray_dp = image_ray_1.dot(image_ray_2);

  const long double a = n * (n - 2.0 * ray_dp) + 1.0;
  const long double b = 2.0 * m * (n - ray_dp);
  const long double c = m * m - (model_point_1 - model_point_2).squaredNorm();

  double roots[2] = { 0.0, 0.0 };
  const int number_of_roots = SolveQuadraticReals(a, b, c, roots);

  int num_solutions = 0;
  for (int i = 0; i < number_of_roots; ++i) {
    // Only accept positive ray distances.
    if (roots[i] > 0) {
      // Computes the other ray distance.
      const double ray_distance = m + n * roots[i];
      if (ray_distance > 0) {
        AddPoseSolution(axis, model_point_1, model_point_2, image_ray_1,
                        image_ray_2, ray_distance, roots[i],
                        &soln_rotations[num_solutions],
                        &soln_translations[num_solutions]);
        num_solutions++;
      }
    }
  }
  return num_solutions;
}

}  // namespace

int TwoPointPosePartialRotation(const Vector3d& axis,
                                const Vector3d& model_point_1,
                                const Vector3d& model_point_2,
                                const Vector3d& image_ray_1,
                                const Vector3d& image_ray_2,
                                Quaterniond soln_rotations[2],
                                Vector3d soln_translations[2]) {
  static const double kEpsilon = 1e-9;
  assert(fabs(image_ray_1.squaredNorm() - 1.0) < kEpsilon);
  assert(fabs(image_ray_2.squaredNorm() - 1.0) < kEpsilon);

  if (fabs(image_ray_1.dot(axis)) < kEpsilon) {
    if (fabs(image_ray_2.dot(axis)) > kEpsilon) {
      // If image_ray_1.y() == 0 then the function above doesn't work because
      // the calculate m and n will have a divide by 0.
      // However if image_ray_2.y() is not equal to zero then we can swap
      // and the points and call the above function with the swapped points.
      // TODO(cmsweeney): Maybe always swap to improve the conditioning?
      return TwoPointPoseCore(axis, model_point_2, model_point_1, image_ray_2,
                              image_ray_1, soln_rotations, soln_translations);
    }
    return 0;
  } else {
    return TwoPointPoseCore(axis, model_point_1, model_point_2, image_ray_1,
                            image_ray_2, soln_rotations, soln_translations);
  }
}

}  // namespace theia