// 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

#include <Eigen/Dense>
#include <assimp/Importer.hpp>
#include <assimp/postprocess.h>
#include <assimp/scene.h>
#include <fcl/geometry/bvh/BVH_model.h>
#include <fcl/narrowphase/collision.h>
#include <functional>
#include <memory>
#include <mpt/discrete_motion_validator.hpp>
#include <mpt/goal_state.hpp>
#include <mpt/log.hpp>
#include <mpt/se3_space.hpp>
#include <mpt/uniform_sampler.hpp>
#include <nigh/kdtree_batch.hpp>

#include <cuda.h>

#define BATCH_SIZE 32
#define NN_TYPE KDTreeBatch
#define SCALAR_TYPE float
namespace mpt_demo::impl {
    static constexpr std::intmax_t SO3_WEIGHT = 50;
    using Vec3 = Eigen::Matrix<float, 3, 1>;

    // silence the warnings "taking address of packed member 'a1' of
    // class or structure 'aiMatrix4x4t<float>' may result in an
    // unaligned pointer value" We use static_asserts to ensure
    // unaligned structures will not happen.
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"

    template <typename Scalar>
    auto mapToEigen(const aiMatrix4x4t<Scalar>& m) {
        using EigenType = const Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>;
        static_assert(sizeof(EigenType) == sizeof(m));
        return Eigen::Map<EigenType>(&m.a1);
    }

    template <typename Scalar>
    auto mapToEigen(const aiVector3t<Scalar>& v) {
        using EigenType = const Eigen::Matrix<Scalar, 3, 1>;
        static_assert(sizeof(EigenType) == sizeof(v));
        return Eigen::Map<const EigenType>(&v.x);
    }
#pragma GCC diagnostic pop

    template <typename Scalar, typename Fn>
    std::size_t visitVertices(
        const aiScene* scene, const aiNode *node,
        Eigen::Transform<Scalar, 3, Eigen::Affine> transform,
        Fn&& visitor)
    {
        std::size_t count = 0;
        transform *= mapToEigen(node->mTransformation).template cast<Scalar>();
        for (unsigned i=0 ; i < node->mNumMeshes ; ++i) {
            const aiMesh *mesh = scene->mMeshes[node->mMeshes[i]];
            count += mesh->mNumVertices;
            for (unsigned j=0 ; j < mesh->mNumVertices ; ++j)
                visitor(transform * mapToEigen(mesh->mVertices[j]).template cast<Scalar>());
        }
        for (unsigned i=0 ; i < node->mNumChildren ; ++i)
            count += visitVertices(scene, node->mChildren[i], transform, std::forward<Fn>(visitor));
        return count;
    }

    template <typename Scalar, typename Fn>
    static std::size_t visitTriangles(
        const aiScene *scene, const aiNode *node,
        Eigen::Transform<Scalar, 3, Eigen::Affine> transform,
        Fn&& visitor)
    {
        std::size_t count = 0;

        transform *= mapToEigen(node->mTransformation).template cast<Scalar>();
        for (unsigned i=0 ; i<node->mNumMeshes ; ++i) {
            const aiMesh *mesh = scene->mMeshes[node->mMeshes[i]];
            for (unsigned j=0 ; j<mesh->mNumFaces ; ++j) {
                const aiFace& face = mesh->mFaces[j];
                if (face.mNumIndices < 3)
                    continue;

                // Support trangular decomposition by fanning out
                // around vertex 0.  The indexing follows as:
                //
                //   0---1   0 1 2
                //  /|\ /    0 2 3
                // 4-3-2     0 3 4
                //
                Vec3 v0 = transform * mapToEigen(mesh->mVertices[face.mIndices[0]]).template cast<Scalar>();
                Vec3 v1 = transform * mapToEigen(mesh->mVertices[face.mIndices[1]]).template cast<Scalar>();
                for (unsigned k=2 ; k<face.mNumIndices ; ++k) {
                    Vec3 v2 = transform * mapToEigen(mesh->mVertices[face.mIndices[k]]).template cast<Scalar>();
                    visitor(v0, v1, v2);
                    v1 = v2;
                }
                count += face.mNumIndices - 2;
            }
        }
        for (unsigned i=0 ; i<node->mNumChildren ; ++i)
            count += visitTriangles(scene, node->mChildren[i], transform, std::forward<Fn>(visitor));

        return count;
    }

    // Triangle class encapsulates coordinates of vertices, instead of storing
    // vertex indices and then looking them up in an array.
    // Having local copies of vertex coordinates is less space effecient, but
    // reduces number of accesses to standard memory. It also creates more total
    // work to do, since now transformations need to be done multiple times for
    // each vertex, but since the transformations will be parallelized,  we believe
    // this trade off will be negligible in terms of time saved accessing memory.
    template <typename Scalar>
    class Triangle {
        using Vec3 = Eigen::Matrix<Scalar, 3, 1>;

    public:
        // coordinates of triangle vertices
        Vec3 A;
        Vec3 B;
        Vec3 C;

        Triangle() {}

        __host__ __device__ Triangle(Vec3 vertex_a, Vec3 vertex_b, Vec3 vertex_c):
            A(vertex_a),
            B(vertex_b),
            C(vertex_c) {}

    };

    template <typename Scalar>
    class Mesh {
        using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
        using Transform = Eigen::Transform<Scalar, 3, Eigen::Affine>;

        std::string name_;
        fcl::BVHModel<fcl::OBBRSS<Scalar>> model_;

    public:
        std::vector<Triangle<Scalar>> host_triangles_; // RAM copy of triangles
        Triangle<Scalar> *d_triangles_; //pointer to GPU copy of triangles for a mesh

        Mesh(const std::string& name, bool shiftToCenter)
            : name_(name)
        {
            Assimp::Importer importer;
            // these options are the same as from OMPL app to strive
            // for parity
            static constexpr auto readOpts =
                aiProcess_Triangulate | aiProcess_JoinIdenticalVertices |
                aiProcess_SortByPType | aiProcess_OptimizeGraph | aiProcess_OptimizeMeshes;

            const aiScene *scene = importer.ReadFile(name, readOpts);
            if (scene == nullptr)
                throw std::invalid_argument("could not load mesh file '" + name + "'");

            if (!scene->HasMeshes())
                throw std::invalid_argument("mesh file '" + name + "' does not contain meshes");

            Vec3 center = Vec3::Zero();
            std::size_t nVertices = visitVertices(
                scene,
                scene->mRootNode,
                Transform::Identity(),
                [&] (const Vec3& v) { center += v; });
            center /= nVertices;

            Transform rootTransform = Transform::Identity();

            if (shiftToCenter)
                rootTransform *= Eigen::Translation<Scalar, 3>(-center);

            model_.beginModel();
            std::size_t nTris = visitTriangles(
                scene,
                scene->mRootNode,
                rootTransform,
                [&] (const Vec3& a, const Vec3& b, const Vec3& c) {
                    model_.addTriangle(a, b, c);
                });
            model_.endModel();
            model_.computeLocalAABB();

            for (int i = 0; i < model_.num_tris; i++) {
                host_triangles_.emplace_back(   model_.vertices[model_.tri_indices[i][0]],
                                                model_.vertices[model_.tri_indices[i][1]],
                                                model_.vertices[model_.tri_indices[i][2]]);
            }

            cudaMalloc((void **) &d_triangles_, sizeof(Triangle<Scalar>) * host_triangles_.size());

            cudaMemcpy(d_triangles_, &host_triangles_[0], host_triangles_.size() * sizeof(Triangle<float>), cudaMemcpyHostToDevice);
            cudaDeviceSynchronize(); // consider not having cudaDeviceSynchronize here, instead maybe call it at the start of valid.

            MPT_LOG(INFO) << "Loaded mesh '" << name << "' (" << nVertices << " vertices, " << nTris
                          << " triangles, center=" << center << ")";
        }

    };

    template <auto>
    class member_function;

    template <typename T, typename R, R T::* fn>
    class member_function<fn> {
        T& obj_;
    public:
        member_function(T& obj) : obj_(obj) {}
        template <typename ... Args>
        decltype(auto) operator() (Args&& ... args) { return (obj_.*fn)(std::forward<Args>(args)...); }
        template <typename ... Args>
        decltype(auto) operator() (Args&& ... args) const { return (obj_.*fn)(std::forward<Args>(args)...); }
    };
}

namespace mpt_demo {
    using namespace unc::robotics;
    using Vec3 = Eigen::Matrix<float, 3, 1>;

    // CUDA helper functions
    //////////////////////////////////////////////////////////////////////////////////
__device__ int coplanar_tri_tri(double N[3],double V0[3],double V1[3],double V2[3],
                     double U0[3],double U1[3],double U2[3]);

// some vector macros

    #define THRESHOLD 0.01f

    #define FABS(x) (x>=0?x:-x)        /* implement as is fastest on your machine */

    #define CROSS(dest,v1,v2)                       \
                dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
                dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
                dest[2]=v1[0]*v2[1]-v1[1]*v2[0];

    #define   sVpsV_2( Vr, s1,  V1,s2, V2);\
        {\
    Vr[0] = s1*V1[0] + s2*V2[0];\
    Vr[1] = s1*V1[1] + s2*V2[1];\
    }\

    #define myVpV(g,v2,v1);\
    {\
        g[0] = v2[0]+v1[0];\
        g[1] = v2[1]+v1[1];\
        g[2] = v2[2]+v1[2];\
        }\

    #define myVmV(g,v2,v1);\
    {\
        g[0] = v2[0]-v1[0];\
        g[1] = v2[1]-v1[1];\
        g[2] = v2[2]-v1[2];\
        }\

    // 2D intersection of segment and triangle.
    #define seg_collide3( q, r)\
    {\
        p1[0]=SF*P1[0];\
        p1[1]=SF*P1[1];\
        p2[0]=SF*P2[0];\
        p2[1]=SF*P2[1];\
        det1 = p1[0]*q[1]-q[0]*p1[1];\
        gama1 = (p1[0]*r[1]-r[0]*p1[1])*det1;\
        alpha1 = (r[0]*q[1] - q[0]*r[1])*det1;\
        alpha1_legal = (alpha1>=0) && (alpha1<=(det1*det1)  && (det1!=0));\
        det2 = p2[0]*q[1] - q[0]*p2[1];\
        alpha2 = (r[0]*q[1] - q[0]*r[1]) *det2;\
        gama2 = (p2[0]*r[1] - r[0]*p2[1]) * det2;\
        alpha2_legal = (alpha2>=0) && (alpha2<=(det2*det2) && (det2 !=0));\
        det3=det2-det1;\
        gama3=((p2[0]-p1[0])*(r[1]-p1[1]) - (r[0]-p1[0])*(p2[1]-p1[1]))*det3;\
        if (alpha1_legal)\
        {\
            if (alpha2_legal)\
            {\
                if ( ((gama1<=0) && (gama1>=-(det1*det1))) || ((gama2<=0) && (gama2>=-(det2*det2))) || (gama1*gama2<0)) return 12;\
            }\
            else\
            {\
                if ( ((gama1<=0) && (gama1>=-(det1*det1))) || ((gama3<=0) && (gama3>=-(det3*det3))) || (gama1*gama3<0)) return 13;\
                }\
        }\
        else\
        if (alpha2_legal)\
        {\
            if ( ((gama2<=0) && (gama2>=-(det2*det2))) || ((gama3<=0) && (gama3>=-(det3*det3))) || (gama2*gama3<0)) return 23;\
            }\
        return 0;\
        }

    //main procedure

    __device__ int tr_tri_intersect3D (double C1[3], double P1[3], double P2[3],
            double D1[3], double Q1[3], double Q2[3])
    {
        double  t[3],p1[3], p2[3],r[3],r4[3];
        double beta1, beta2, beta3;
        double gama1, gama2, gama3;
        double det1, det2, det3;
        double dp0, dp1, dp2;
        double dq1,dq2,dq3,dr, dr3;
        double alpha1, alpha2;
        bool alpha1_legal, alpha2_legal;
        double  SF;
        bool beta1_legal, beta2_legal;

        myVmV(r,D1,C1);
        // determinant computation
        dp0 = P1[1]*P2[2]-P2[1]*P1[2];
        dp1 = P1[0]*P2[2]-P2[0]*P1[2];
        dp2 = P1[0]*P2[1]-P2[0]*P1[1];
        dq1 = Q1[0]*dp0 - Q1[1]*dp1 + Q1[2]*dp2;
        dq2 = Q2[0]*dp0 - Q2[1]*dp1 + Q2[2]*dp2;
        dr  = -r[0]*dp0  + r[1]*dp1  - r[2]*dp2;

        beta1 = dr*dq2;  // beta1, beta2 are scaled so that beta_i=beta_i*dq1*dq2
        beta2 = dr*dq1;
        beta1_legal = (beta2>=0) && (beta2 <=dq1*dq1) && (dq1 != 0);
        beta2_legal = (beta1>=0) && (beta1 <=dq2*dq2) && (dq2 != 0);

        dq3=dq2-dq1;
        dr3=+dr-dq1;   // actually this is -dr3

        if ((dq1 == 0) && (dq2 == 0))
        {
            if (dr!=0) return 0;  // triangles are on parallel planes
            else
            {						// triangles are on the same plane
                double C2[3],C3[3],D2[3],D3[3], N1[3];
                // We use the coplanar test of Moller which takes the 6 vertices and 2 normals
                //as input.
                myVpV(C2,C1,P1);
                myVpV(C3,C1,P2);
                myVpV(D2,D1,Q1);
                myVpV(D3,D1,Q2);
                CROSS(N1,P1,P2);
                return coplanar_tri_tri(N1,C1, C2,C3,D1,D2,D3);
            }
        }

        else if (!beta2_legal && !beta1_legal) return 0;// fast reject-all vertices are on
                                                        // the same side of the triangle plane

        else if (beta2_legal && beta1_legal)    //beta1, beta2
        {
            SF = dq1*dq2;
            sVpsV_2(t,beta2,Q2, (-beta1),Q1);
        }

        else if (beta1_legal && !beta2_legal)   //beta1, beta3
        {
            SF = dq1*dq3;
            beta1 =beta1-beta2;   // all betas are multiplied by a positive SF
            beta3 =dr3*dq1;
            sVpsV_2(t,(SF-beta3-beta1),Q1,beta3,Q2);
        }

        else if (beta2_legal && !beta1_legal) //beta2, beta3
        {
            SF = dq2*dq3;
            beta2 =beta1-beta2;   // all betas are multiplied by a positive SF
            beta3 =dr3*dq2;
            sVpsV_2(t,(SF-beta3),Q1,(beta3-beta2),Q2);
            Q1=Q2;
            beta1=beta2;
        }
        sVpsV_2(r4,SF,r,beta1,Q1);
        seg_collide3(t,r4);  // calculates the 2D intersection
        return 0;
    }

        /* this edge to edge test is based on Franlin Antonio's gem:
    "Faster Line Segment Intersection", in Graphics Gems III,
    pp. 199-202 */
    #define FABS(x) (x>=0?x:-x)        /* implement as is fastest on your machine */
    #define EDGE_EDGE_TEST(V0,U0,U1)                      \
    Bx=U0[i0]-U1[i0];                                   \
    By=U0[i1]-U1[i1];                                   \
    Cx=V0[i0]-U0[i0];                                   \
    Cy=V0[i1]-U0[i1];                                   \
    f=Ay*Bx-Ax*By;                                      \
    d=By*Cx-Bx*Cy;                                      \
    if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f))  \
    {                                                   \
        e=Ax*Cy-Ay*Cx;                                    \
        if(f>0)                                           \
        {                                                 \
        if(e>=0 && e<=f) return 1;                      \
        }                                                 \
        else                                              \
        {                                                 \
        if(e<=0 && e>=f) return 1;                      \
        }                                                 \
    }

    #define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
    {                                              \
    double Ax,Ay,Bx,By,Cx,Cy,e,d,f;               \
    Ax=V1[i0]-V0[i0];                            \
    Ay=V1[i1]-V0[i1];                            \
    /* test edge U0,U1 against V0,V1 */          \
    EDGE_EDGE_TEST(V0,U0,U1);                    \
    /* test edge U1,U2 against V0,V1 */          \
    EDGE_EDGE_TEST(V0,U1,U2);                    \
    /* test edge U2,U1 against V0,V1 */          \
    EDGE_EDGE_TEST(V0,U2,U0);                    \
    }

    #define POINT_IN_TRI(V0,U0,U1,U2)           \
    {                                           \
    double a,b,c,d0,d1,d2;                     \
    /* is T1 completly inside T2? */          \
    /* check if V0 is inside tri(U0,U1,U2) */ \
    a=U1[i1]-U0[i1];                          \
    b=-(U1[i0]-U0[i0]);                       \
    c=-a*U0[i0]-b*U0[i1];                     \
    d0=a*V0[i0]+b*V0[i1]+c;                   \
                                                \
    a=U2[i1]-U1[i1];                          \
    b=-(U2[i0]-U1[i0]);                       \
    c=-a*U1[i0]-b*U1[i1];                     \
    d1=a*V0[i0]+b*V0[i1]+c;                   \
                                                \
    a=U0[i1]-U2[i1];                          \
    b=-(U0[i0]-U2[i0]);                       \
    c=-a*U2[i0]-b*U2[i1];                     \
    d2=a*V0[i0]+b*V0[i1]+c;                   \
    if(d0*d1>0.0)                             \
    {                                         \
        if(d0*d2>0.0) return 1;                 \
    }                                         \
    }

    //This procedure testing for intersection between coplanar triangles is taken
    // from Tomas Moller's
    //"A Fast Triangle-Triangle Intersection Test",Journal of Graphics Tools, 2(2), 1997
    __device__ int coplanar_tri_tri(double N[3],double V0[3],double V1[3],double V2[3],
                        double U0[3],double U1[3],double U2[3])
    {
    double A[3];
    short i0,i1;
    /* first project onto an axis-aligned plane, that maximizes the area */
    /* of the triangles, compute indices: i0,i1. */
    A[0]=FABS(N[0]);
    A[1]=FABS(N[1]);
    A[2]=FABS(N[2]);
    if(A[0]>A[1])
    {
        if(A[0]>A[2])
        {
            i0=1;      /* A[0] is greatest */
            i1=2;
        }
        else
        {
            i0=0;      /* A[2] is greatest */
            i1=1;
        }
    }
    else   /* A[0]<=A[1] */
    {
        if(A[2]>A[1])
        {
            i0=0;      /* A[2] is greatest */
            i1=1;
        }
        else
        {
            i0=0;      /* A[1] is greatest */
            i1=2;
        }
        }

        /* test all edges of triangle 1 against the edges of triangle 2 */
        EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2);
        EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2);
        EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2);

        /* finally, test if tri1 is totally contained in tri2 or vice versa */
        POINT_IN_TRI(V0,U0,U1,U2);
        POINT_IN_TRI(U0,V0,V1,V2);

        return 0;
    }

    // void detect_collision_all_robot_host(
    //         impl::Triangle<float> *obstacles, size_t obs_size,
    //         impl::Triangle<float> *robot, size_t rob_size,
    //         bool *collisions, Eigen::Transform<float, 3, Eigen::Isometry> tf){

    //     float threshold = 0.0001f;

    //     for (size_t idx = 0; idx < obs_size ; idx++){
    //         impl::Triangle<float> obs = obstacles[idx];
    //         Vec3 obs_vec1 = obs.B - obs.A;
    //         Vec3 obs_vec2 = obs.C - obs.A;

    //         Vec3 obs_norm = obs_vec1.cross(obs_vec2);

    //         // scalar that satisfies obs_norm * X + obs_d = 0dot
    //         float obs_d = -1 * obs_norm.dot(obs.A);

    //         // case where vertices of a 'triangle' are colinear- ignore collision
    //         // todo, do line intersection test with triangle
    //         if (fabsf(obs_norm[0]) < threshold && fabsf(obs_norm[1]) < threshold && fabsf(obs_norm[2]) < threshold){
    //             collisions[idx] = false;

    //             return;
    //         }

    //         float obs_center_vertex[] = {obs.A[0], obs.A[1], obs.A[2]};
    //         float obs_edge_B[] =      {obs.B[0] - obs.A[0], obs.B[1] - obs.A[1], obs.B[2] - obs.A[2]};
    //         float obs_edge_C[] =      {obs.C[0] - obs.A[0], obs.C[1] - obs.A[1], obs.C[2] - obs.A[2]};
    //         //test for intersection against all robot triangles
    //         ////////////////////////////////////////////////////////////////////////
    //         bool has_collision = false;
    //         for (int i = 0; i < rob_size; i++){
    //             impl::Triangle<float> pre_trans_rob = robot[i];
    //             impl::Triangle<float> rob(  tf * pre_trans_rob.A,
    //                                         tf * pre_trans_rob.B,
    //                                         tf * pre_trans_rob.C);

    //             float rob_center_vertex[] = {rob.A[0], rob.A[1], rob.A[2]};
    //             float rob_edge_B[] =      {rob.B[0] - rob.A[0], rob.B[1] - rob.A[1], rob.B[2] - rob.A[2]};
    //             float rob_edge_C[] =      {rob.C[0] - rob.A[0], rob.C[1] - rob.A[1], rob.C[2] - rob.A[2]};

    //             if (tr_tri_intersect3D      (obs_center_vertex, obs_edge_B, obs_edge_C,
    //                                         rob_center_vertex, rob_edge_B, rob_edge_C)){
    //                 has_collision=true;
    //                 collisions[idx] = has_collision;
    //                 return;

    //                 // printf("Robot Triangle %d: \n{(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)} \nintersects triangle\n{(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)}\n", i,
    //                 //     rob_center_vertex[0], rob_center_vertex[1], rob_center_vertex[2],
    //                 //     rob_edge_B[0], rob_edge_B[1], rob_edge_B[2],
    //                 //     rob_edge_C[0], rob_edge_C[1], rob_edge_C[2],
    //                 //     obs_center_vertex[0], obs_center_vertex[1], obs_center_vertex[2],
    //                 //     obs_edge_B[0], obs_edge_B[1], obs_edge_B[2],
    //                 //     obs_edge_C[0], obs_edge_C[1], obs_edge_C[2]);
    //                 // break;
    //             }
    //         }
    //         collisions[idx] = has_collision;
    //     }

    // }

    // one environment triangle vs all robot triangles
    // void detect_collision_all_robot(
    __global__  void detect_collision_all_robot(
            impl::Triangle<float> *obstacles, size_t obs_size,
            impl::Triangle<float> *robot, size_t rob_size,
            bool *collisions, Eigen::Transform<float, 3, Eigen::Isometry> tf){

        // want some tolerance when comparing to 0 to account for float errors
        float threshold = 0.0001f;

        int idx = threadIdx.x + blockIdx.x * blockDim.x;

        // edge case where thread doesn't matter
        if (idx >= obs_size){
            return;
        }

        if (idx != 96){
            return;
        }
        // if (idx == 0){
            // printf("There are %d obstacle triangles", obs_size );
            // for (int i = 0; i < obs_size; i++){
            //     printf("Triangle %d: {(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)} \n ", i,
            //                         obstacles[i].A[0], obstacles[i].A[1], obstacles[i].A[2],
            //                         obstacles[i].B[0], obstacles[i].B[1], obstacles[i].B[2],
            //                         obstacles[i].C[0], obstacles[i].C[1], obstacles[i].C[2]);
            // }
        //     printf("There are %d robot triangles", rob_size );
        //     // for (int i = 0; i < rob_size; i++){
        //     //     printf("Triangle %d: {(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)} \n ", i,
        //     //                         robot[i].A[0], robot[i].A[1], robot[i].A[2],
        //     //                         robot[i].B[0], robot[i].B[1], robot[i].B[2],
        //     //                         robot[i].C[0], robot[i].C[1], robot[i].C[2]);
        //     // }
        // }
        impl::Triangle<float> obs = obstacles[idx];

        // calculate normal for our obstacle triangle
        ////////////////////////////////////////////////////////////////////////

        Vec3 obs_vec1 = obs.B - obs.A;
        Vec3 obs_vec2 = obs.C - obs.A;

        Vec3 obs_norm = obs_vec1.cross(obs_vec2);

        // scalar that satisfies obs_norm * X + obs_d = 0dot
        float obs_d = -1 * obs_norm.dot(obs.A);

        // case where vertices of a 'triangle' are colinear- ignore collision
        // todo, do line intersection test with triangle
        // if (fabsf(obs_norm[0]) < threshold && fabsf(obs_norm[1]) < threshold && fabsf(obs_norm[2]) < threshold){
        //     collisions[idx] = false;

        //     return;
        // }

        double obs_center_vertex[] = {obs.A[0], obs.A[1], obs.A[2]};
        double obs_edge_B[] =      {obs.B[0] - obs.A[0], obs.B[1] - obs.A[1], obs.B[2] - obs.A[2]};
        double obs_edge_C[] =      {obs.C[0] - obs.A[0], obs.C[1] - obs.A[1], obs.C[2] - obs.A[2]};
        //test for intersection against all robot triangles
        ////////////////////////////////////////////////////////////////////////
        bool has_collision = false;
        for (int i = 0; i < rob_size; i++){

            impl::Triangle<float> pre_trans_rob = robot[i];
            impl::Triangle<float> rob(  tf * pre_trans_rob.A,
                                        tf * pre_trans_rob.B,
                                        tf * pre_trans_rob.C);

            double rob_center_vertex[] = {rob.A[0], rob.A[1], rob.A[2]};
            double rob_edge_B[] =      {rob.B[0] - rob.A[0], rob.B[1] - rob.A[1], rob.B[2] - rob.A[2]};
            double rob_edge_C[] =      {rob.C[0] - rob.A[0], rob.C[1] - rob.A[1], rob.C[2] - rob.A[2]};

            // printf("Result is %d", tr_tri_intersect3D      (obs_center_vertex, obs_edge_B, obs_edge_C,
            //                              rob_center_vertex, rob_edge_B, rob_edge_C));
            if (tr_tri_intersect3D      (rob_center_vertex, rob_edge_B, rob_edge_C,
                                         obs_center_vertex, obs_edge_B, obs_edge_C)){
                has_collision=true;
                // printf("Robot Triangle %d: \n{(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)} \nintersects env triangle %d\n{(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)}\n", i,
                //     rob_center_vertex[0], rob_center_vertex[1], rob_center_vertex[2],
                //     rob_edge_B[0], rob_edge_B[1], rob_edge_B[2],
                //     rob_edge_C[0], rob_edge_C[1], rob_edge_C[2],
                //     idx,
                //     obs_center_vertex[0], obs_center_vertex[1], obs_center_vertex[2],
                //     obs_edge_B[0], obs_edge_B[1], obs_edge_B[2],
                //     obs_edge_C[0], obs_edge_C[1], obs_edge_C[2]);
                break;
            }
        }
        collisions[idx] = has_collision;
    }

    template <typename Scalar, int nParts = 1, bool selfCollision = false>
    class SE3RigidBodyScenario {

        static_assert(nParts == 1, "only single body motions are supported currently (TODO: add multibody)");

    public:

        using Space = mpt::SE3Space<Scalar, impl::SO3_WEIGHT>; // weight SO(3) by 50
        using Bounds = std::tuple<mpt::Unbounded, mpt::BoxBounds<Scalar, 3>>;
        using State = typename Space::Type;
        using Distance = typename Space::Distance;
        using Goal = mpt::GoalState<Space>;
        using TravelTime = Scalar;

        // TODO: remove explicit Nearest and use default
        using Nearest = unc::robotics::nigh::KDTreeBatch<8>;

    private:
        using Config = typename Space::Type;
        // fcl::Transform3<Scalar> is an alias for
        // Eigen::Transform<Scalar, 3, Eigen::Isometry> though in a
        // previous version the last template parameter was
        // Eigen::AffineCompact.  Isometry seems like a better option,
        // so it seems unlikely to change, but regardless, we use
        // fcl's alias for it instead of directly using Eigen's type.
        using Transform = Eigen::Transform<Scalar, 3, Eigen::Isometry>;

        // The meshes are immutable and can be rather large.  Since
        // scenarios are copied to each thread, we use a shared
        // pointer to avoid copying the environment and robot meshes.
        std::shared_ptr<impl::Mesh<Scalar>> environment_;
        std::shared_ptr<std::vector<impl::Mesh<Scalar>>> robot_;

        Space space_;
        Bounds bounds_;

        static constexpr Distance goalRadius = 1e-6;
        Goal goal_;

        static Transform stateToTransform(const Config& q) {
            return Eigen::Translation<Scalar, 3>(std::get<1>(q))
                * Eigen::Quaternion(std::get<0>(q));
        }

    public:

        // the collision detection function
        // is called in prrt.hpp or pprm.hpp or whatever algorithm this is compiled to use
        bool valid(const Config& q) const {

            int num_env_triangles = environment_->host_triangles_.size();

            if (robot_->size() > 1){
                throw new std::runtime_error("Only single robots supported");
            }
            size_t num_rob_triangles = (*robot_)[0].host_triangles_.size();

            Transform tf = stateToTransform(q);

            // array of booleans, d_collisions[i] = true -> environment_.triangle[i] is
            bool host_collisions[num_env_triangles];
            bool *d_collisions;
            cudaMalloc((void **) &d_collisions, sizeof(bool) * num_env_triangles);

            size_t block_size = 256;
            size_t numBlocks = num_env_triangles / block_size +1;

            // for (int i = 0; i < num_rob_triangles; i++){

            //     printf("Triangle %d: {(%f, %f, %f), (%f, %f, %f), (%f, %f, %f)} \n ", i,
            //                         (*robot_)[0].host_triangles_[i].A[0], (*robot_)[0].host_triangles_[i].A[1], (*robot_)[0].host_triangles_[i].A[2],
            //                         (*robot_)[0].host_triangles_[i].B[0], (*robot_)[0].host_triangles_[i].B[1], (*robot_)[0].host_triangles_[i].B[2],
            //                         (*robot_)[0].host_triangles_[i].C[0], (*robot_)[0].host_triangles_[i].C[1], (*robot_)[0].host_triangles_[i].C[2]);
            // }

            cudaDeviceSynchronize();
            // detect_collision_all_robot_host(&(environment_->host_triangles_[0]), num_env_triangles,
            //                                 &((*robot_)[0].host_triangles_[0]), num_rob_triangles,
            //                                 host_collisions, tf);
            detect_collision_all_robot<<< numBlocks, 256>>>(  environment_->d_triangles_, num_env_triangles,
                                                    (*robot_)[0].d_triangles_, num_rob_triangles,
                                                    d_collisions, tf);
            cudaDeviceSynchronize();

            cudaMemcpy(host_collisions, d_collisions, num_env_triangles * sizeof(bool), cudaMemcpyDeviceToHost);
            cudaDeviceSynchronize();

            bool isValid = true;
            for (int i = 0; i <num_env_triangles; i ++){
                isValid = isValid && !host_collisions[i];
                // if (host_collisions[i]){
                //     int zero = 0;
                //     // std::cout << "env_triangl-e " << i <<" collides\n";
                // }
            }
            // std::cout << isValid << std::endl;
            // if (isValid){
            //     std::cout << "no collisions for state " << q << std::endl;
            // }
            // else {
            //     std::cout << "there is a collision for state " << q << std::endl;
            // }
            return isValid;

            // TODO - write to a single global flag instead of an array of collisions
            // TODO - have each CUDA thread check a single triangle triangle, instead of
            //        all triangles in a robot
            // TODO - check the collisions in a loop instead of using cudaDeviceSynchronize, continuing
            //        whenever we've determined there's a collision0\
            // TODO - initialize vector of bools in construction of scenario, to avoid excessive calls
            //        to cudaMalloc and CUDA free
            // TODO - precompute norms for environment / robot, transform as necessary.
        }

        // // TODO make this use use some cuda algorithm
        // std::vector<bool> validBatch(const std::vector<Config> qs) const {
        //     std::vector<Transform> tfs[robot_->size()];
        //     std::vector<bool> collisions[robot_->size()];

        //     for (const auto& q : qs) {
        //         for (const auto& robot : *robot_) {
        //             for (size_t j = 0)
        //             // consider doing the "state to transform" within the gpu
        //             // could calculate the transform on cpu for one robot point, send a batch, calculate the next transform, send another batch, etc
        //             // ^ that's a good idea, i like it.
        //             Transform tf = stateToTransform(q);
        //         }
        //     }

        //     return collisions;
        // }

    private:
        using Validator = impl::member_function<&SE3RigidBodyScenario::valid>;

    public:
        const mpt::DiscreteMotionValidator<Space, Validator> link_;

        template <typename Min, typename Max>
        SE3RigidBodyScenario(
            const std::string& envMesh,
            const std::vector<std::string>& robotMeshes,
            const Config& goal,
            const Eigen::MatrixBase<Min>& min,
            const Eigen::MatrixBase<Max>& max,
            Scalar checkResolution)
            : environment_(std::make_shared<impl::Mesh<Scalar>>(envMesh, false))
            , robot_(std::make_shared<std::vector<impl::Mesh<Scalar>>>())
            , bounds_(mpt::Unbounded{}, mpt::BoxBounds<Scalar, 3>(min, max)) // environment_.minBounds(), environment_.maxBounds())),
            , goal_(goalRadius, goal)
            // , link_(space_, environment_->extents()*checkResolution, Validator(*this))
            , link_(space_, ((max - min).norm() + Scalar(impl::SO3_WEIGHT*M_PI/2))*checkResolution, Validator(*this))
        {
            robot_->reserve(robotMeshes.size());
            for (const std::string& mesh : robotMeshes) {
                robot_->emplace_back(mesh, true);
            }
            MPT_LOG(DEBUG) << "Volume min: " << min.transpose();
            MPT_LOG(DEBUG) << "Volume max: " << max.transpose();
        }

        const Space& space() const {
            return space_;
        }

        const Bounds& bounds() const {
            return bounds_;
        }

        const Goal& goal() const {
            return goal_;
        }

        bool link(const Config& a, const Config& b) const {
            return link_(a, b);
        }

        // TODO: this shouldn't be necessary
        TravelTime travelTime(Distance dist, bool) const { return dist; }
    };
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// 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_EXTENSIONS_ALGORITHMS_DETAIL_OVERLAY_DISSOLVER_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGORITHMS_DETAIL_OVERLAY_DISSOLVER_HPP

#include <deque>
#include <vector>

#include <boost/range.hpp>

#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>

#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/detail/overlay/get_turns.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/union.hpp>
#include <boost/geometry/algorithms/reverse.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL

namespace detail { namespace inserter
{

template<typename Tag1, typename Tag2>
struct insert_geometry
{};

template<>
struct insert_geometry<ring_tag, polygon_tag>
{
    template<typename Ring, typename Collection>
    static inline void apply(Ring const& ring, Collection& collection)
    {
        collection.resize(collection.size() + 1);
        geometry::exterior_ring(collection.back()) = ring;
    }
};

template<>
struct insert_geometry<polygon_tag, polygon_tag>
{
    template<typename Geometry, typename Collection>
    static inline void apply(Geometry const& geometry, Collection& collection)
    {
        collection.push_back(geometry);
    }
};

template<typename Geometry, typename Collection>
inline void insert(Geometry const& geometry, Collection& collection)
{
    insert_geometry
        <
            typename geometry::tag<Geometry>::type,
            typename geometry::tag
                <
                    typename boost::range_value<Collection>::type
                >::type
        >::apply(geometry, collection);
}

}} // namespace detail::inserter

namespace detail { namespace dissolver
{

class plusmin_policy
{
    template
    <
        typename Geometry1,
        typename Geometry2,
        typename RescalePolicy,
        typename OutputCollection
    >
    static inline bool check_negative(Geometry1 a, Geometry2 b, // pass-by-value
                    RescalePolicy const& rescale_policy,
                    OutputCollection& output_collection)
    {
        // Precondition: a = positive, b = negative

        // 1: make b positive to get proper intersection
        geometry::reverse(b);
        {
            // 2: Check if there is overlap
            OutputCollection difference;
            geometry::intersection(a, b, difference);
            if(difference.size() <= 0)
            {
                return false;
            }
        }

        // There is overlap and we want to remove it, by subtracting it from b

        //negative = true;

        typedef typename geometry::point_type<Geometry2>::type point_type;

        typedef overlay::turn_info
            <
                point_type,
                typename segment_ratio_type<point_type, RescalePolicy>::type
            > turn_info;
        std::deque<turn_info> turns;

        // Get (and stop on) any intersection
        detail::disjoint::disjoint_interrupt_policy policy;
        geometry::get_turns
            <
                false, false,
                overlay::assign_null_policy
            >(a, b, rescale_policy, turns, policy);

        if (! policy.has_intersections)
        {
            // There is overlap but no intersections -> b is inside a.
            // So keep A and keep B, do not change anything
            return false;
        }

        // There are intersections.
        // 3: make a negative
        geometry::reverse(a); // now negative

        // This will calculate B minus A, result is then positive
        OutputCollection difference;
        geometry::intersection(a, b, difference);

        // Add original a to output (NOT necessary! TODO avoid this)
        {
            geometry::reverse(a); // positive again
            detail::inserter::insert(a, output_collection);
        }

        // Make negative output negative again
        typedef typename boost::range_iterator<OutputCollection>::type iterator_type;
        for(iterator_type it = boost::begin(difference);
            it != boost::end(difference);
            ++it)
        {
            geometry::reverse(*it);
            detail::inserter::insert(*it, output_collection);
        }
        return true;
    }

public :

    template
    <
        typename Geometry1,
        typename Geometry2,
        typename RescalePolicy,
        typename OutputCollection
    >
    static inline bool apply(Geometry1 const& a, Geometry2 const& b,
                    RescalePolicy const& rescale_policy,
                    OutputCollection& output_collection)
    {
        typedef typename geometry::coordinate_type<Geometry2>::type coordinate_type;
        coordinate_type area_a = geometry::area(a);
        coordinate_type area_b = geometry::area(b);

        // DEBUG
        /*
        int n = boost::size(output_collection);
        typedef typename geometry::point_type<Geometry2>::type point_type;
        std::cout << "Combine "
            << area_a << " with " << " " << area_b
            << " { " << geometry::wkt(geometry::return_centroid<point_type>(a))
            << geometry::wkt(geometry::return_centroid<point_type>(b)) << " }"
             << std::endl;
        */
        // END DEBUG

        coordinate_type zero = coordinate_type();
        if (area_a > zero && area_b > zero)
        {
            geometry::union_(a, b, output_collection);
            return true;
        }
        else if (area_a > zero && area_b < zero)
        {
            return check_negative(a, b, rescale_policy, output_collection);
        }
        else if (area_a < zero && area_b > zero)
        {
            return check_negative(b, a, rescale_policy, output_collection);
        }

        // both negative (?) TODO
        // DEBUG
        /*
        for (int i = n; i < boost::size(output_collection); i++)
        {
            typedef typename geometry::point_type<Geometry2>::type point_type;
            std::cout << "Result "
                << geometry::area(output_collection[i])
                << " " << geometry::wkt(geometry::return_centroid<point_type>(output_collection[i]))
                << std::endl;
        }
        */
        // END DEBUG
        return false;

    }

};

template <typename CombinePolicy>
struct dissolver_generic
{

    // Small structure to access elements by index;
    // this avoids copying or accessing elements by address (pointer)
    template <typename Box>
    struct dissolve_helper
    {
        int source; // 0,1
        int index; // index in the original array
        bool dissolved;
        Box box;
        double area;

        dissolve_helper()
        {}

        dissolve_helper(int i, Box b, double a, int s)
            : source(s)
            , index(i)
            , dissolved(false)
            , box(b)
            , area(a)
        {}
    };

    struct get_geometry
    {
        template <typename Range>
        inline static typename boost::range_value<Range>::type const& apply(
            Range const& range, int index)
        {
            return range[index];
        }
    };

    template
    <
        typename Vector,
        typename HelperVector
    >
    static inline void init_helper(Vector const& v, HelperVector& helper,
        int index = 0, int source = 0)
    {
        typedef typename boost::range_value<Vector>::type value_type;
        typedef typename geometry::point_type<value_type>::type point_type;
        typedef model::box<point_type> box_type;
        for(typename boost::range_iterator<Vector const>::type
            it = boost::begin(v);
            it != boost::end(v);
            ++it, ++index)
        {
            helper.push_back(dissolve_helper<box_type>(index,
                    geometry::return_envelope<box_type>(*it),
                    geometry::area(*it),
                    source));
        }
    }

    template
    <
        typename Element,
        typename Geometry1, typename Geometry2,
        typename RescalePolicy,
        typename OutputCollection
    >
    static inline bool call_policy(
            Element const& , Element const& ,
            Geometry1 const& geometry1, Geometry2 const& geometry2,
            RescalePolicy const& rescale_policy,
            OutputCollection& output_collection)
    {
        if (! geometry::disjoint(geometry1, geometry2))
        {
            /*std::cout << "Process " << element1.source << "/" << element1.index
                << " and " << element2.source << "/" << element2.index
                << "  (" << element2.dissolved << "," << element2.dissolved << ")"
                << std::endl;
            */
            return CombinePolicy::apply(geometry1, geometry2,
                            rescale_policy, output_collection);
        }
        return false;
    }

    template
    <
        int Dimension,
        typename HelperVector,
        typename IndexVector,
        typename InputRange,
        typename RescalePolicy,
        typename OutputCollection,
        typename Box
    >
    static inline bool divide_and_conquer(HelperVector& helper_vector
                , IndexVector& index_vector
                , InputRange const& input_range
                , RescalePolicy const& rescale_policy
                , OutputCollection& output_collection
                , Box const& total_box
                , bool& changed
                , int iteration = 0
                )
    {
        //std::cout << "divide_and_conquer " << iteration << std::endl;
        typedef typename geometry::coordinate_type<Box>::type coordinate_type;
        typedef typename boost::range_value<HelperVector>::type helper_type;
        typedef typename boost::range_iterator<IndexVector const>::type iterator_type;

        //if (boost::size(index_vector) >= 16 && iteration < 100)
        // Not yet using divide and conquer
        if (false)
        {
            // 1: separate box into 2 (either horizontally or vertically)
            Box lower_box = total_box, upper_box = total_box;
            coordinate_type two = 2.0;
            coordinate_type mid
                = (geometry::get<min_corner, Dimension>(total_box)
                    + geometry::get<max_corner, Dimension>(total_box)) / two;

            geometry::set<max_corner, Dimension>(lower_box, mid);
            geometry::set<min_corner, Dimension>(upper_box, mid);

            // 2: divide indices into two sublists
            IndexVector lower_list, upper_list;
            for(iterator_type it = boost::begin(index_vector);
                it != boost::end(index_vector);
                ++it)
            {
                helper_type const& element = helper_vector[*it];
                if (! geometry::disjoint(lower_box, element.box))
                {
                    lower_list.push_back(*it);
                }
                if (! geometry::disjoint(upper_box, element.box))
                {
                    upper_list.push_back(*it);
                }
            }

            //std::cout << lower_list.size() << ", " << upper_list.size()<< std::endl;

            // 3: recursively call function (possibly divide in other dimension)
            divide_and_conquer<1 - Dimension>(helper_vector,
                lower_list, input_range, rescale_policy, output_collection, lower_box, changed, iteration + 1);
            divide_and_conquer<1 - Dimension>(helper_vector,
                upper_list, input_range, rescale_policy, output_collection, upper_box, changed, iteration + 1);
            return changed;
        }

        // There are less then 16 elements, handle them quadraticly

        int n = boost::size(output_collection);

        for(iterator_type it1 = boost::begin(index_vector);
            it1 != boost::end(index_vector);
            ++it1)
        {
            helper_type& element1 = helper_vector[*it1];

            bool unioned = false;
            for(iterator_type it2 = boost::begin(index_vector);
                ! unioned && it2 != it1;
                ++it2)
            {
                helper_type& element2 = helper_vector[*it2];

                // If they are NOT disjoint, union them
                if (! element1.dissolved
                    && ! element2.dissolved
                    && ! geometry::disjoint(element1.box, element2.box))
                {
                    // Runtime type check here...
                    if ((element1.source == 0 && element2.source == 0
                        && call_policy
                            (
                                element1, element2,
                                get_geometry::apply(input_range, element1.index),
                                get_geometry::apply(input_range, element2.index),
                                rescale_policy,
                                output_collection
                            )
                        )
                        || (element1.source == 0 && element2.source == 1
                        && call_policy
                            (
                                element1, element2,
                                get_geometry::apply(input_range, element1.index),
                                get_geometry::apply(output_collection, element2.index),
                                rescale_policy,
                                output_collection
                            )
                        )
                        || (element1.source == 1 && element2.source == 0
                        && call_policy
                            (
                                element1, element2,
                                get_geometry::apply(output_collection, element1.index),
                                get_geometry::apply(input_range, element2.index),
                                rescale_policy,
                                output_collection
                            )
                        )
                        || (element1.source == 1 && element2.source == 1
                        && call_policy
                            (
                                element1, element2,
                                get_geometry::apply(output_collection, element1.index),
                                get_geometry::apply(output_collection, element2.index),
                                rescale_policy,
                                output_collection
                            )
                        )
                        )
                    {
                        changed = true;
                        element1.dissolved = true;
                        element2.dissolved = true;

                        unioned = true;
/*std::cout << "Assign " << element1.source << "/" << element1.index
<< " and " << element2.source << "/" << element2.index
<< "  (" << element2.dissolved << "," << element2.dissolved << ")"
<< std::endl;
*/
                    }
                }
            }
        }

        // Append new records in output collection to helper class
        init_helper(std::make_pair(boost::begin(output_collection) + n,
            boost::end(output_collection)), helper_vector, n, 1);

        return changed;
    }

    template <typename T>
    static inline bool helper_dissolved(T const& t)
    {
      return t.dissolved;
    }

    template
    <
        typename InputRange,
        typename RescalePolicy,
        typename OutputCollection
    >
    static inline void apply(InputRange const& input_range
                , RescalePolicy const& rescale_policy
                , OutputCollection& output_collection
                )
    {
        typedef typename boost::range_value<OutputCollection>::type output_type;

        typedef typename geometry::point_type<output_type>::type point_type;
        typedef model::box<point_type> box_type;
        typedef dissolve_helper<box_type> dissolve_helper_type;
        typedef std::vector<dissolve_helper_type> helper_vector_type;

        // Vector with indices to both input_range (source 0) and output_collection (source 1)
        helper_vector_type helper_vector;

        // Vector with indices to helper-vector, for divide and conquer
        std::vector<int> index_vector;

        init_helper(input_range, helper_vector);

        // Fill intrusive list with copies, and determine bounding box
        box_type total_box;
        geometry::assign_inverse(total_box);
        int index = 0;
        for(typename boost::range_iterator<helper_vector_type const>::type
            it = boost::begin(helper_vector);
            it != boost::end(helper_vector);
            ++it, ++index)
        {
            index_vector.push_back(index);
            geometry::expand(total_box, it->box);
        }

        std::vector<output_type> unioned_collection;

        int size = 0, previous_size = 0;
        int n = 0;

        bool changed = false;
        while(divide_and_conquer<1>
            (helper_vector, index_vector, input_range, rescale_policy, unioned_collection, total_box, changed) && n < 5)
        {
            // Remove everything which is already dissolved.
            helper_vector.erase
                (
                    std::remove_if
                        (
                            helper_vector.begin(),
                            helper_vector.end(),
                            helper_dissolved<dissolve_helper_type>
                        ),
                    helper_vector.end()
                );

            previous_size = size;
            size = helper_vector.size();
            n = previous_size == size ? n + 1 : 0;

            // Re-initialize the list
            index_vector.clear();
            index = 0;
            for(typename boost::range_iterator<helper_vector_type const>::type
                it = boost::begin(helper_vector);
                it != boost::end(helper_vector);
                ++it, ++index)
            {
                index_vector.push_back(index);
            }

            changed = false;

            //std::cout << " " << size;
        }

        // Add input+output to real output
        typedef typename boost::range_iterator<helper_vector_type>::type iterator_type;
        for(iterator_type it = boost::begin(helper_vector);
            it != boost::end(helper_vector);
            ++it)
        {
            if (! it->dissolved)
            {
                switch(it->source)
                {
                    case 0 :
                        detail::inserter::insert(
                            get_geometry::apply(input_range, it->index),
                            output_collection);
                        break;
                    case 1 :
                        detail::inserter::insert(
                            get_geometry::apply(unioned_collection, it->index),
                            output_collection);
                        break;
                }
            }
        }
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename GeometryTag1,
    typename GeometryTag2,
    typename Policy
>
struct dissolver
{};

template<typename Policy>
struct dissolver<ring_tag, polygon_tag, Policy>
    : detail::dissolver::dissolver_generic<Policy>
{};

template<typename Policy>
struct dissolver<polygon_tag, polygon_tag, Policy>
    : detail::dissolver::dissolver_generic<Policy>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template
<
    typename InputRange,
    typename OutputCollection
>
inline void dissolver(InputRange const& input_range,
        OutputCollection& output_collection)
{
    typedef typename boost::range_value<InputRange>::type geometry_in;
    typedef typename boost::range_value<OutputCollection>::type geometry_out;
    concept::check<geometry_in const>();
    concept::check<geometry_out>();

    dispatch::dissolver
    <
        typename tag<geometry_in>::type,
        typename tag<geometry_out>::type,
        detail::dissolver::plusmin_policy
    >::apply(input_range, output_collection);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGORITHMS_DETAIL_OVERLAY_DISSOLVER_HPP


//  Copyright (c) 2007-2012 Hartmut Kaiser
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <hpx/hpx_fwd.hpp>
#include <hpx/runtime/components/component_factory.hpp>

#include <hpx/util/assert.hpp>

#include <cmath>
#include <memory>

#include <boost/move/move.hpp>

#include "partition3d.hpp"
#include "../read_values.hpp"

///////////////////////////////////////////////////////////////////////////////
namespace sheneos { namespace server
{
    partition3d::partition3d()
      : energy_shift_(0)
    {
        std::memset(min_value_, 0, sizeof(min_value_));
        std::memset(max_value_, 0, sizeof(max_value_));
        std::memset(delta_, 0, sizeof(delta_));
    }

    inline void
    partition3d::init_dimension(std::string const& datafilename, int d,
        dimension const& dim, char const* name, boost::scoped_array<double>& values)
    {
        // Store all parameters.
        dim_[d] = dim;

        // Account for necessary overlap on the right hand side of the data
        // interval (the interpolation algorithm we're using does not go beyond
        // the left hand side).
        std::size_t count = dim.count_;
        if (dim.offset_ + dim.count_ < dim.size_-2) {
            dim_[d].count_ += 2;
            ++count;
        }

        // Read the full data range.
        values.reset(new double[dim_[d].count_]);
        extract_data(datafilename, name, values.get(), dim.offset_, dim_[d].count_);

        // Extract range (without ghost-zones).
        min_value_[d] = values[0];
        max_value_[d] = values[count-1];
        delta_[d] = values[1] - values[0];
    }

    inline void
    partition3d::init_data(std::string const& datafilename,
        char const* name, boost::scoped_array<double>& values,
        std::size_t array_size)
    {
        values.reset(new double[array_size]);
        extract_data(datafilename, name, values.get(), dim_[dimension::ye],
            dim_[dimension::temp], dim_[dimension::rho]);
    }

    void partition3d::init(std::string const& datafilename,
        dimension const& dimx, dimension const& dimy, dimension const& dimz)
    {
        init_dimension(datafilename, dimension::ye, dimx, "ye", ye_values_);
        init_dimension(datafilename, dimension::temp, dimy, "logtemp", logtemp_values_);
        init_dimension(datafilename, dimension::rho, dimz, "logrho", logrho_values_);

        // Initialize the energy shift.
        extract_data(datafilename, "energy_shift", &energy_shift_, 0, 1);

        // Read our slice of data.
        std::size_t array_size = dim_[dimension::ye].count_ *
            dim_[dimension::temp].count_ * dim_[dimension::rho].count_;

        init_data(datafilename, "logpress", logpress_values_, array_size);
        init_data(datafilename, "logenergy", logenergy_values_, array_size);
        init_data(datafilename, "entropy", entropy_values_, array_size);
        init_data(datafilename, "munu", munu_values_, array_size);
        init_data(datafilename, "cs2", cs2_values_, array_size);
        init_data(datafilename, "dedt", dedt_values_, array_size);
        init_data(datafilename, "dpdrhoe", dpdrhoe_values_, array_size);
        init_data(datafilename, "dpderho", dpderho_values_, array_size);
#if SHENEOS_SUPPORT_FULL_API
        init_data(datafilename, "muhat", muhat_values_, array_size);
        init_data(datafilename, "mu_e", mu_e_values_, array_size);
        init_data(datafilename, "mu_p", mu_p_values_, array_size);
        init_data(datafilename, "mu_n", mu_n_values_, array_size);
        init_data(datafilename, "Xa", xa_values_, array_size);
        init_data(datafilename, "Xh", xh_values_, array_size);
        init_data(datafilename, "Xp", xp_values_, array_size);
        init_data(datafilename, "Xn", xn_values_, array_size);
        init_data(datafilename, "Abar", abar_values_, array_size);
        init_data(datafilename, "Zbar", zbar_values_, array_size);
        init_data(datafilename, "gamma", gamma_values_, array_size);
#endif
    }

    inline std::size_t
    partition3d::get_index(dimension::type d, double value)
    {
        if (value < min_value_[d] || value > max_value_[d]) {
            HPX_THROW_EXCEPTION(hpx::bad_parameter,
                "sheneos::partition3d::get_index",
                "argument out of range");
            return 0;
        }

        std::size_t index = static_cast<std::size_t>(
            (value - min_value_[d]) / delta_[d]);

        // Either the index has to be inside bounds or the requested value
        // corresponds to the right end edge of the managed data range.
        HPX_ASSERT(index < dim_[d].count_ ||
            (index == dim_[d].count_ && value == max_value_[d]));

        return index;
    }

    ///////////////////////////////////////////////////////////////////////////
    inline std::size_t
    index(std::size_t x, std::size_t y, std::size_t z, dimension const* dim)
    {
        std::size_t idx = z + (y + x * dim[dimension::temp].count_) *
            dim[dimension::rho].count_;

        HPX_ASSERT(idx < dim[dimension::ye].count_ *
            dim[dimension::temp].count_ * dim[dimension::rho].count_);

        return idx;
    }

    ///////////////////////////////////////////////////////////////////////////
    inline double partition3d::tl_interpolate(double* values,
        std::size_t idx_x, std::size_t idx_y, std::size_t idx_z,
        double delta_ye, double delta_logtemp, double delta_logrho)
    {
        double value000 = values[index(idx_x,   idx_y,   idx_z,   dim_)];
        double value001 = values[index(idx_x,   idx_y,   idx_z+1, dim_)];
        double value010 = values[index(idx_x,   idx_y+1, idx_z,   dim_)];
        double value011 = values[index(idx_x,   idx_y+1, idx_z+1, dim_)];
        double value100 = values[index(idx_x+1, idx_y,   idx_z,   dim_)];
        double value101 = values[index(idx_x+1, idx_y,   idx_z+1, dim_)];
        double value110 = values[index(idx_x+1, idx_y+1, idx_z,   dim_)];
        double value111 = values[index(idx_x+1, idx_y+1, idx_z+1, dim_)];

        double comp_delta_ye = 1. - delta_ye;
        double comp_delta_logtemp = 1. - delta_logtemp;
        double comp_delta_logrho = 1. - delta_logrho;

        return value000 * comp_delta_ye * comp_delta_logtemp * comp_delta_logrho +
               value001 * comp_delta_ye * comp_delta_logtemp * delta_logrho +
               value010 * comp_delta_ye * delta_logtemp      * comp_delta_logrho +
               value011 * comp_delta_ye * delta_logtemp      * delta_logrho +
               value100 * delta_ye      * comp_delta_logtemp * comp_delta_logrho +
               value101 * delta_ye      * comp_delta_logtemp * delta_logrho +
               value110 * delta_ye      * delta_logtemp      * comp_delta_logrho +
               value111 * delta_ye      * delta_logtemp      * delta_logrho;
    }

    ///////////////////////////////////////////////////////////////////////////
    std::vector<double> partition3d::interpolate(double ye, double temp,
        double rho, boost::uint32_t eosvalues)
    {
        double logrho = std::log10(rho);
        double logtemp = std::log10(temp);

        std::size_t idx_ye = get_index(dimension::ye, ye);
        std::size_t idx_logtemp = get_index(dimension::temp, logtemp);
        std::size_t idx_logrho = get_index(dimension::rho, logrho);

        double delta_ye = (ye - ye_values_[idx_ye]) / delta_[dimension::ye];
        double delta_logtemp = (logtemp - logtemp_values_[idx_logtemp]) / delta_[dimension::temp];
        double delta_logrho = (logrho - logrho_values_[idx_logrho]) / delta_[dimension::rho];

        std::vector<double> results;
        results.reserve(19);

        // Calculate all required values.
        if (eosvalues & logpress) {
            double value = tl_interpolate(logpress_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho);
            results.push_back(std::pow(10., value));
        }
        if (eosvalues & logenergy) {
            double value = tl_interpolate(logenergy_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho);
            results.push_back(std::pow(10., value) - energy_shift_);
        }
        if (eosvalues & entropy) {
            results.push_back(tl_interpolate(entropy_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho));
        }
        if (eosvalues & munu) {
            results.push_back(tl_interpolate(munu_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho));
        }
        if (eosvalues & cs2) {
            results.push_back(tl_interpolate(cs2_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho));
        }
        if (eosvalues & dedt) {
            results.push_back(tl_interpolate(dedt_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho));
        }
        if (eosvalues & dpdrhoe) {
            results.push_back(tl_interpolate(dpdrhoe_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho));
        }
        if (eosvalues & dpderho) {
            results.push_back(tl_interpolate(dpderho_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho));
        }

        return results;
    }

    ///////////////////////////////////////////////////////////////////////////
    namespace detail
    {
        inline int numberof_setbits(boost::uint32_t i)
        {
            i = i - ((i >> 1) & 0x55555555);
            i = (i & 0x33333333) + ((i >> 2) & 0x33333333);
            return (((i + (i >> 4)) & 0x0F0F0F0F) * 0x01010101) >> 24;
        }

        inline bool more_than_one_value_requested(boost::uint32_t i)
        {
            return numberof_setbits(i) > 1;
        }
    }

    ///////////////////////////////////////////////////////////////////////////
    double partition3d::interpolate_one(double ye, double temp,
        double rho, boost::uint32_t eosvalue)
    {
        double logrho = std::log10(rho);
        double logtemp = std::log10(temp);

        std::size_t idx_ye = get_index(dimension::ye, ye);
        std::size_t idx_logtemp = get_index(dimension::temp, logtemp);
        std::size_t idx_logrho = get_index(dimension::rho, logrho);

        double delta_ye = (ye - ye_values_[idx_ye]) / delta_[dimension::ye];
        double delta_logtemp = (logtemp - logtemp_values_[idx_logtemp]) / delta_[dimension::temp];
        double delta_logrho = (logrho - logrho_values_[idx_logrho]) / delta_[dimension::rho];

        if (detail::more_than_one_value_requested(eosvalue)) {
            HPX_THROW_EXCEPTION(hpx::bad_parameter,
                "partition3d::interpolate_one",
                "requested to interpolate more than one physical value: " +
                boost::lexical_cast<std::string>(eosvalue));
        }

        // Calculate all required values.
        switch (eosvalue) {
        case logpress:
            {
                double value = tl_interpolate(logpress_values_.get(),
                    idx_ye, idx_logtemp, idx_logrho,
                    delta_ye, delta_logtemp, delta_logrho);
                return std::pow(10., value);
            }
        case logenergy:
            {
                double value = tl_interpolate(logenergy_values_.get(),
                    idx_ye, idx_logtemp, idx_logrho,
                    delta_ye, delta_logtemp, delta_logrho);
                return std::pow(10., value) - energy_shift_;
            }
        case entropy:
            return tl_interpolate(entropy_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho);

        case munu:
            return tl_interpolate(munu_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho);

        case cs2:
            return tl_interpolate(cs2_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho);

        case dedt:
            return tl_interpolate(dedt_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho);

        case dpdrhoe:
            return tl_interpolate(dpdrhoe_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho);

        case dpderho:
            return tl_interpolate(dpderho_values_.get(),
                idx_ye, idx_logtemp, idx_logrho,
                delta_ye, delta_logtemp, delta_logrho);

        default:
            break;
        }

        HPX_THROW_EXCEPTION(hpx::bad_parameter, "partition3d::interpolate_one",
            "requested to interpolate unknown physical value: " +
            boost::lexical_cast<std::string>(eosvalue));

        return 0;
    }

    ///////////////////////////////////////////////////////////////////////////
    inline double partition3d::interpolate_one(sheneos_coord const& c,
        boost::uint32_t eosvalue)
    {
        return interpolate_one(c.ye_, c.temp_, c.rho_, eosvalue);
    }

    inline std::vector<double> partition3d::interpolate(
        sheneos_coord const& c, boost::uint32_t eosvalues)
    {
        return interpolate(c.ye_, c.temp_, c.rho_, eosvalues);
    }

    ///////////////////////////////////////////////////////////////////////////
    std::vector<double>
    partition3d::interpolate_one_bulk(std::vector<sheneos_coord> const& coords,
        boost::uint32_t eosvalue)
    {
        std::vector<double> result;
        result.reserve(coords.size());

        // interpolate as requested
        std::vector<sheneos_coord>::const_iterator end = coords.end();
        for (std::vector<sheneos_coord>::const_iterator it = coords.begin();
            it != end; ++it)
        {
            result.push_back(interpolate_one(*it, eosvalue));
        }

        return result;
    }

    ///////////////////////////////////////////////////////////////////////////
    std::vector<std::vector<double> >
    partition3d::interpolate_bulk(std::vector<sheneos_coord> const& coords,
        boost::uint32_t eosvalues)
    {
        std::vector<std::vector<double> > result;
        result.reserve(coords.size());

        // interpolate as requested
        std::vector<sheneos_coord>::const_iterator end = coords.end();
        for (std::vector<sheneos_coord>::const_iterator it = coords.begin();
            it != end; ++it)
        {
            result.push_back(boost::move(interpolate(*it, eosvalues)));
        }

        return result;
    }
}}

///////////////////////////////////////////////////////////////////////////////
namespace hpx { namespace serialization
{
    ///////////////////////////////////////////////////////////////////////////
    // Implement the serialization functions.
    void serialize(input_archive& ar,
        sheneos::sheneos_coord& coord, unsigned int const)
    {
        ar & coord.ye_ & coord.temp_ & coord.rho_;
    }

    void serialize(output_archive& ar,
        sheneos::sheneos_coord& coord, unsigned int const)
    {
        ar & coord.ye_ & coord.temp_ & coord.rho_;
    }
}}

///////////////////////////////////////////////////////////////////////////////
typedef sheneos::server::partition3d partition3d_type;

///////////////////////////////////////////////////////////////////////////////
// Serialization support for the actions.
HPX_REGISTER_ACTION(partition3d_type::init_action,
    sheneos_partition3d_init_action);
HPX_REGISTER_ACTION(partition3d_type::interpolate_action,
    sheneos_partition3d_interpolate_action);
HPX_REGISTER_ACTION(partition3d_type::interpolate_one_action,
    sheneos_partition3d_interpolate_one_action);
HPX_REGISTER_ACTION(partition3d_type::interpolate_bulk_action,
    sheneos_partition3d_interpolate_bulk_action);
HPX_REGISTER_ACTION(partition3d_type::interpolate_one_bulk_action,
    sheneos_partition3d_interpolate_one_bulk_action);

HPX_REGISTER_COMPONENT(
    hpx::components::simple_component<partition3d_type>,
    sheneos_partition_type);

HPX_REGISTER_ACTION(
    hpx::lcos::base_lco_with_value<std::vector<std::vector<double> > >::set_value_action,
    set_value_action_vector_vector_double);
HPX_DEFINE_GET_COMPONENT_TYPE_STATIC(
    hpx::lcos::base_lco_with_value<std::vector<std::vector<double> > >,
    hpx::components::component_base_lco_with_value);

HPX_REGISTER_ACTION(
    hpx::lcos::base_lco_with_value<std::vector<double> >::set_value_action,
    set_value_action_vector_double);
HPX_DEFINE_GET_COMPONENT_TYPE_STATIC(
    hpx::lcos::base_lco_with_value<std::vector<double> >,
    hpx::components::component_base_lco_with_value);