//
//  MTRenderer.cpp
//
//  Christopher Batty, Fang Da 2014
//
//

#include "MTRenderer.h"
#ifdef __APPLE__
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GLUT/glut.h>
#else
#ifdef _MSC_VER
#define NOMINMAX
#include <windows.h>
#endif
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#endif
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>

typedef Eigen::Matrix<double, 4, 4> Mat4d;
typedef Eigen::Matrix<double, 3, 3> Mat3d;
typedef Eigen::Matrix<double, 4, 1> Vec4d;
typedef Eigen::Matrix<double, 3, 1> Vec3d;
typedef Eigen::Matrix<double, 2, 1> Vec2d;

MTRenderer::MTRenderer() :
    m_st(NULL),
    m_nregions(0),
    m_current_region(0),
    m_mode(RM_REGIONS),
    m_mouse_x(0),
    m_mouse_y(0)
{

}

namespace
{
    //Color routine to use HSV colors instead of RGB taken
    // from http://en.wikipedia.org/wiki/HSL_and_HSV#Converting_to_RGB
    // HSV \in (0-360, 0-1, 0-1)
    void hsvToRgb(double _h, double _s, double _v, double & r, double & g, double & b)
    {
        double c, m, x, hp;
        double h = std::min(std::max(_h, 0.), 360.);
        double s = std::min(std::max(_s, 0.), 1.);
        double v = std::min(std::max(_v, 0.), 1.);
        c = v * s;
        m = v - c;
        hp = h / 60.;
        x = c * (double)(1 - abs((int)hp % 2 - 1));
        assert (hp < 6. && hp >= 0);
        if (hp < 1)
        {
            r = c;
            g = x;
            b = 0;
        } else if (hp < 2)
        {
            r = x;
            g = c;
            b = 0;
        } else if (hp < 3)
        {
            r = 0;
            g = c;
            b = x;
        } else if (hp < 4 )
        {
            r = 0;
            g = x;
            b = c;
        } else if (hp < 5)
        {
            r = x;
            g = 0;
            b = c;
        } else if (hp < 6)
        {
            r = c;
            g = 0;
            b = x;
        }
        r += m;
        g += m;
        c += m;
    }

    void glColorHSV3d(double h, double s, double v)
    {
        double r, g, b;
        hsvToRgb(h, s, v, r, g, b);
        glColor3d(r, g, b);
    }

    void glVertexVec3d(const Vec3d& v)
    {
        glVertex3d((GLdouble)v[0], (GLdouble)v[1], (GLdouble)v[2]);
    }

    void glNormalVec3d(const Vec3d& v)
    {
        glNormal3d((GLdouble)v[0], (GLdouble)v[1], (GLdouble)v[2]);
    }

    class FaceComp
    {
    public:
        bool operator() (const std::pair<size_t, double> & f1, const std::pair<size_t, double> & f2) const
        {
            return f1.second < f2.second;
        }
    };

    int junction(const LosTopos::NonDestructiveTriMesh & mesh, size_t e)
    {
        return (int)mesh.m_edge_to_triangle_map[e].size();
    }

    bool junctionNeighborV(const LosTopos::NonDestructiveTriMesh & mesh, size_t v)
    {
        for (size_t i = 0; i < mesh.m_vertex_to_edge_map[v].size(); i++)
            if (junction(mesh, mesh.m_vertex_to_edge_map[v][i]) > 2)
                return true;

        return false;
    }

    bool junctionNeighborE(const LosTopos::NonDestructiveTriMesh & mesh, size_t e)
    {
        if (junctionNeighborV(mesh, mesh.m_edges[e][0]))
            return true;

        if (junctionNeighborV(mesh, mesh.m_edges[e][1]))
            return true;

        return false;
    }

    bool junctionNeighborF(const LosTopos::NonDestructiveTriMesh & mesh, size_t f)
    {
        for (size_t i = 0; i < 3; i++)
            if (junctionNeighborV(mesh, mesh.m_tris[f][(int)i]))
                return true;

        return false;
    }

    int onBBWall(const Vec3d & pos)
    {
        static const double WALL_THRESHOLD = 1e-6;

        int walls = 0;
        if (pos[0] < 0 + WALL_THRESHOLD) walls |= (1 << 0);
        if (pos[1] < 0 + WALL_THRESHOLD) walls |= (1 << 1);
        if (pos[2] < 0 + WALL_THRESHOLD) walls |= (1 << 2);
        if (pos[0] > 1 - WALL_THRESHOLD) walls |= (1 << 3);
        if (pos[1] > 1 - WALL_THRESHOLD) walls |= (1 << 4);
        if (pos[2] > 1 - WALL_THRESHOLD) walls |= (1 << 5);

        return walls;
    }

    LosTopos::Vec3d EVec(const Vec3d & v) { return LosTopos::Vec3d(v[0], v[1], v[2]); }
    Vec3d          Vec(const LosTopos::Vec3d & v) { return Vec3d(v[0], v[1], v[2]); }

}

bool MTRenderer::visibleV(size_t v) const
{
    bool visible = false;
    for (size_t i = 0; i < m_st->m_mesh.m_vertex_to_triangle_map[v].size(); i++)
    {
        const LosTopos::Vec2i & l = m_st->m_mesh.get_triangle_label(m_st->m_mesh.m_vertex_to_triangle_map[v][i]);
        if (l[0] >= 0 && m_region_visible[l[0]]) visible = true;
        if (l[1] >= 0 && m_region_visible[l[1]]) visible = true;
    }

    return visible;
}

bool MTRenderer::visibleE(size_t e) const
{
    bool visible = false;
    for (size_t i = 0; i < m_st->m_mesh.m_edge_to_triangle_map[e].size(); i++)
    {
        const LosTopos::Vec2i & l = m_st->m_mesh.get_triangle_label(m_st->m_mesh.m_edge_to_triangle_map[e][i]);
        if (l[0] >= 0 && m_region_visible[l[0]]) visible = true;
        if (l[1] >= 0 && m_region_visible[l[1]]) visible = true;
    }

    return visible;
}

bool MTRenderer::visibleF(size_t f) const
{
    bool visible = false;
    const LosTopos::Vec2i & l = m_st->m_mesh.get_triangle_label(f);
    if (l[0] >= 0 && m_region_visible[l[0]]) visible = true;
    if (l[1] >= 0 && m_region_visible[l[1]]) visible = true;

    return visible;
}

void MTRenderer::cycleMode(int inc)
{
    m_mode = (MTRenderer::Mode)((m_mode + RM_COUNT + inc) % RM_COUNT);
}

void MTRenderer::keyboard(unsigned char key, int x, int y)
{
    if (key == 'm' || key == 'M')
    {
        cycleMode(key == 'm' ? 1 : -1);
        glutPostRedisplay();
    } else if (key == '=' || key == '+')
    {
        m_current_region = (m_current_region + (key == '+' ? 10 : 1)) % m_nregions;
        glutPostRedisplay();
        std::cout << "current region = " << m_current_region << "/" << m_nregions << std::endl;
    } else if (key == '-' || key == '_')
    {
        m_current_region = (m_current_region + m_nregions - (key == '_' ? 10 : 1)) % m_nregions;
        glutPostRedisplay();
        std::cout << "current region = " << m_current_region << "/" << m_nregions << std::endl;
    } else if (key == 'l' || key == 'L')
    {
        while ((int)m_region_visible.size() < m_nregions)
            m_region_visible.push_back(true);
        m_region_visible[m_current_region] = !m_region_visible[m_current_region];
        glutPostRedisplay();
        for (size_t i = 0; i < m_region_visible.size(); i++)
            std::cout << "region " << i << " " << (m_region_visible[i] ? "visible" : "invisible") << std::endl;
    } else if (key == 'h' || key == 'H')
    {
        while ((int)m_region_visible.size() < m_nregions)
            m_region_visible.push_back(true);
        bool allinvisible = true;
        for (size_t i = 0; i < m_region_visible.size(); i++)
            if (m_region_visible[i])
                allinvisible = false;
        for (size_t i = 0; i < m_region_visible.size(); i++)
            m_region_visible[i] = allinvisible;
        glutPostRedisplay();
    }

}

void MTRenderer::render()
{
    assert(m_st);

    // find the vertex/edge/face the mouse cursor is nearest to
    Vec2d mousepos = Vec2d(m_mouse_x, m_mouse_y);

    Mat4d MV;
    Mat4d PJ;
    {
        float mv[16];
        glGetFloatv(GL_MODELVIEW_MATRIX, mv);
        float pj[16];
        glGetFloatv(GL_PROJECTION_MATRIX, pj);
        MV << mv[0], mv[4], mv[8], mv[12], mv[1], mv[5], mv[9], mv[13], mv[2], mv[6], mv[10], mv[14], mv[3], mv[7], mv[11], mv[15];
        PJ << pj[0], pj[4], pj[8], pj[12], pj[1], pj[5], pj[9], pj[13], pj[2], pj[6], pj[10], pj[14], pj[3], pj[7], pj[11], pj[15];
    }
    Mat4d MVP = PJ * MV;

    double mind = -1;
    int mind_vertex = -1;
    int mind_edge = -1;
    int mind_face = -1;

    for (size_t i = 0; i < m_st->m_mesh.nv(); i++)
    {
        if (!visibleV(i))
            continue;

        Vec3d pos = Vec(m_st->get_position(i));
        Vec4d scrpos_h = MVP * Vec4d(pos.x(), pos.y(), pos.z(), 1.0);
        Vec2d scrpos = Vec2d(scrpos_h.x(), scrpos_h.y()) / scrpos_h.w();

        double distance = (scrpos - mousepos).norm();
        if (distance < mind || mind < 0)
        {
            mind = distance;
            mind_vertex = (int)i;
        }
    }

    for (size_t i = 0; i < m_st->m_mesh.ne(); i++)
    {
        if (!visibleE(i))
            continue;

        Vec3d v0 = Vec(m_st->get_position(m_st->m_mesh.m_edges[i][0]));
        Vec3d v1 = Vec(m_st->get_position(m_st->m_mesh.m_edges[i][1]));

        Vec4d scrv0_h = MVP * Vec4d(v0.x(), v0.y(), v0.z(), 1.0);
        Vec2d scrv0 = Vec2d(scrv0_h.x(), scrv0_h.y()) / scrv0_h.w();
        Vec4d scrv1_h = MVP * Vec4d(v1.x(), v1.y(), v1.z(), 1.0);
        Vec2d scrv1 = Vec2d(scrv1_h.x(), scrv1_h.y()) / scrv1_h.w();

        double distance = (mousepos - (scrv0 + scrv1) / 2).norm();
//        double distance = (mousepos - scrv0 - (mousepos - scrv0).dot(scrv1 - scrv0) * (scrv1 - scrv0) / (scrv1 - scrv0).squaredNorm()).norm();
        if (distance < mind || mind < 0)
        {
            mind = distance;
            mind_vertex = -1;
            mind_edge = (int)i;
        }
    }

    for (size_t i = 0; i < m_st->m_mesh.nt(); i++)
    {
        if (!visibleF(i))
            continue;

        Vec3d v0 = Vec(m_st->get_position(m_st->m_mesh.m_tris[i][0]));
        Vec3d v1 = Vec(m_st->get_position(m_st->m_mesh.m_tris[i][1]));
        Vec3d v2 = Vec(m_st->get_position(m_st->m_mesh.m_tris[i][2]));

        Vec4d scrv0_h = MVP * Vec4d(v0.x(), v0.y(), v0.z(), 1.0);
        Vec2d scrv0 = Vec2d(scrv0_h.x(), scrv0_h.y()) / scrv0_h.w();
        Vec4d scrv1_h = MVP * Vec4d(v1.x(), v1.y(), v1.z(), 1.0);
        Vec2d scrv1 = Vec2d(scrv1_h.x(), scrv1_h.y()) / scrv1_h.w();
        Vec4d scrv2_h = MVP * Vec4d(v2.x(), v2.y(), v2.z(), 1.0);
        Vec2d scrv2 = Vec2d(scrv2_h.x(), scrv2_h.y()) / scrv2_h.w();

        double distance = (mousepos - (scrv0 + scrv1 + scrv2) / 3).norm();
        if (distance < mind || mind < 0)
        {
            mind = distance;
            mind_vertex = -1;
            mind_edge = -1;
            mind_face = (int)i;
        }
    }

    if (mind_vertex < 0 && mind_edge < 0 && mind_face < 0)  // this can only happen when no region is visible, thus no primitives to pick from
    {
        mind_vertex = 0;
        mind = 0;
    }

    m_nearest_vertex = mind_vertex;
    m_nearest_edge = mind_edge;
    m_nearest_face = mind_face;

    glPushMatrix();

    const LosTopos::NonDestructiveTriMesh & mesh = m_st->m_mesh;

    if (m_mode == RM_INTERFACES || m_mode == RM_JUNCTIONS)
    {
        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        //glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

        std::vector<Vec3d> faceNormals(m_st->m_mesh.nt());
        for (size_t i = 0; i < m_st->m_mesh.nt(); i++) faceNormals[i] = Vec(m_st->get_triangle_normal(i));

        // stats on total number of labels
        int maxlabel = -1;
        for (size_t i = 0; i < mesh.nt(); i++)
        {
            LosTopos::Vec2i regions = mesh.get_triangle_label(i);
            if (regions[0] >= maxlabel) maxlabel = regions[0];
            if (regions[1] >= maxlabel) maxlabel = regions[1];
        }
        m_nregions = maxlabel + 1;
        while (m_region_visible.size() < m_nregions)
            m_region_visible.push_back(true);

        // render bounding box
        glLineWidth(1);
        glColor3f(0.5, 0.5, 0.5);
        glBegin(GL_LINES);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(0.0, 0.0, 1.0);
        glVertex3f(1.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 1.0);
        glVertex3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 1.0, 1.0);
        glVertex3f(1.0, 1.0, 0.0);
        glVertex3f(1.0, 1.0, 1.0);

        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 0.0);
        glVertex3f(1.0, 1.0, 0.0);
        glVertex3f(1.0, 1.0, 0.0);
        glVertex3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 0.0, 0.0);

        glVertex3f(0.0, 0.0, 1.0);
        glVertex3f(1.0, 0.0, 1.0);
        glVertex3f(1.0, 0.0, 1.0);
        glVertex3f(1.0, 1.0, 1.0);
        glVertex3f(1.0, 1.0, 1.0);
        glVertex3f(0.0, 1.0, 1.0);
        glVertex3f(0.0, 1.0, 1.0);
        glVertex3f(0.0, 0.0, 1.0);
        glEnd();

        // Render all edges
        glLineWidth(2);
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        glDepthMask(GL_FALSE);
        glBegin(GL_LINES);
        glColor3f(0, 0, 0);

        for (size_t i = 0; i < mesh.ne(); i++)
        {
            if (m_mode == RM_JUNCTIONS)
                if (!junctionNeighborE(mesh, i))
                    continue;

            Vec3d p0 = Vec(m_st->get_position(mesh.m_edges[i][0]));
            Vec3d p1 = Vec(m_st->get_position(mesh.m_edges[i][1]));
            //Vec3d dir = (p1-p0);
            //p0 = p0 + 0.05*dir;
            //p1 = p1 - 0.05*dir;
            glColor4f(0.0f, 0.0f, 0.0f, 0.2f);

            if (m_mode == RM_JUNCTIONS)
            {
                int ne = junction(mesh, i);
                if (ne == 3)
                    glColor4f(1.0f, 0.0f, 1.0f, 0.2f);
                else if (ne == 4)
                    glColor4f(0.3f, 0.8f, 0.9f, 0.2f);
                else if (ne > 4)
                    glColor4f(0.2f, 0.3f, 1.0f, 0.2f);
            }

            if ((p0 - p1).norm() <= 0.1)
                glColor4f(0.0f, 0.5f, 1.0f, 1.0f);

            if (!visibleE(i))
                glColor4f(0.0f, 0.0f, 0.0f, 0.1f);

            glVertexVec3d(p0);
            glVertexVec3d(p1);
        }
        glEnd();

        if (mind_edge >= 0)
        {
            glColor4f(0.0f, 0.0f, 0.0f, 1.0f);
            glLineWidth(4);
            glBegin(GL_LINES);
            glVertexVec3d(Vec(m_st->get_position(mesh.m_edges[mind_edge][0])));
            glVertexVec3d(Vec(m_st->get_position(mesh.m_edges[mind_edge][1])));
            glEnd();
            glLineWidth(2);
        }
        glDepthMask(GL_TRUE);
        glDisable(GL_BLEND);

        // render face labels
        if (true)
        {
            // generate a list of colors for all the labels present
            std::vector<Vec3d> labelcolors;
            labelcolors.push_back(Vec3d(0, 0, 0));
            for (int i = 0; i < maxlabel + 1; i++)
            {
                float r, g, b;
                float t = (float)i / (maxlabel == 0 ? 1 : maxlabel);
                if (t < 0.5)
                {
                    r = 1 - t * 1.5f;
                    g = t * 1.5f;
                    b = 0;
                } else
                {
                    r = 0;
                    g = 1 - (t - 0.5f) * 1.5f;
                    b = (t - 0.5f) * 1.5f;
                }
                labelcolors.push_back(Vec3d(r, g, b));
            }

            glBegin(GL_LINES);
            for (size_t i = 0; i < mesh.nt(); i++)
            {
                if (m_mode == RM_JUNCTIONS)
                    if (!junctionNeighborF(mesh, i))
                        continue;

                LosTopos::Vec2i regions = mesh.get_triangle_label(i);
                if ((regions[0] < 0 || !m_region_visible[regions[0]]) &&
                    (regions[1] < 0 || !m_region_visible[regions[1]]))
                    continue;

                Vec3d p0 = Vec(m_st->get_position(mesh.m_tris[i][0]));
                Vec3d p1 = Vec(m_st->get_position(mesh.m_tris[i][1]));
                Vec3d p2 = Vec(m_st->get_position(mesh.m_tris[i][2]));

                Vec3d c = (p0 + p1 + p2) / 3;
                Vec3d n = (p1 - p0).cross(p2 - p0).normalized();

                double mean_edge_length = ((p0 - p1).norm() + (p1 - p2).norm() + (p2 - p0).norm()) / 3;

                Vec3d color0 = labelcolors[std::max(0, regions[0] + 1)];
                glColor3d(color0.x(), color0.y(), color0.z());

                if (regions[0] >= 0)
                {
                    glVertexVec3d(c);
                    glVertexVec3d(c - n * mean_edge_length * 0.05);
                }

                Vec3d color1 = labelcolors[std::max(0, regions[1] + 1)];
                glColor3d(color1.x(), color1.y(), color1.z());

                if (regions[1] >= 0)
                {
                    glVertexVec3d(c);
                    glVertexVec3d(c + n * mean_edge_length * 0.05);
                }
            }
            glEnd();
        }

        // render back to front semi transparent
        float mv[16];
        glGetFloatv(GL_MODELVIEW_MATRIX, mv);
        Vec3d view_vec(mv[2], mv[6], mv[10]);  // assuming ModelView matrix contains only translation, rotation and uniform scaling

        std::vector<std::pair<size_t, double> > sorted_faces;
        for (size_t i = 0; i < mesh.nt(); i++)
        {
            if (m_mode == RM_JUNCTIONS)
                if (!junctionNeighborF(mesh, i))
                    continue;

            Vec3d barycentre(0, 0, 0);
            for (int j = 0; j < 3; j++)
                barycentre += Vec(m_st->get_position(mesh.m_tris[i][j]));
            barycentre /= 3.0;

            double depth = barycentre.dot(view_vec);
            sorted_faces.push_back(std::pair<size_t, double>(i, depth));
        }

        FaceComp fc;
        std::sort(sorted_faces.begin(), sorted_faces.end(), fc);

        // Render all faces
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        glDepthMask(GL_FALSE);
        glBegin(GL_TRIANGLES);

        for (size_t i = 0; i < sorted_faces.size(); i++)
        {
            Vec3d barycentre(0, 0, 0);
            for (int j = 0; j < 3; j++)
                barycentre += Vec(m_st->get_position(mesh.m_tris[sorted_faces[i].first][j]));
            barycentre /= 3.0;

            double alpha = 0.1;
            glColor4f(1.0f, 0.0f, 0.0f, (float)alpha);

            if (!visibleF(sorted_faces[i].first))
                glColor4f(0.0f, 0.0f, 0.0f, 0.02f);

            double edge_shrink = 0.1;

            if (sorted_faces[i].first == mind_face)
            {
                glColor4f(1.0, 0.0, 0.0, 0.5);
                edge_shrink = 0.0;
            }

            for (int j = 0; j < 3; j++)
            {
                Vec3d pos = Vec(m_st->get_position(mesh.m_tris[sorted_faces[i].first][j]));
                pos += (barycentre - pos) * edge_shrink;
                glVertexVec3d(pos);
            }
        }

        glEnd();
        glDisable(GL_BLEND);
        glDepthMask(GL_TRUE);

        glColor3f(0.0, 0.0, 0.0);

        //Render all vertices
        glPointSize(4);
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        glDepthMask(GL_FALSE);
        glBegin(GL_POINTS);

        for (size_t i = 0; i < mesh.nv(); i++)
        {
            Vec3d vertPos = Vec(m_st->get_position(i));

            double minangle = -1;
            double maxangle = -1;
            for (size_t j = 0; j < mesh.m_vertex_to_triangle_map[i].size(); j++)
            {
                size_t v0 = mesh.m_tris[mesh.m_vertex_to_triangle_map[i][j]][0];
                size_t v1 = mesh.m_tris[mesh.m_vertex_to_triangle_map[i][j]][1];
                size_t v2 = mesh.m_tris[mesh.m_vertex_to_triangle_map[i][j]][2];

                double angle = 0;
                Vec3d a, b;
                if (v0 == i)
                {
                    a = Vec(m_st->get_position(v1));
                    b = Vec(m_st->get_position(v2));
                } else if (v1 == i)
                {
                    a = Vec(m_st->get_position(v2));
                    b = Vec(m_st->get_position(v0));
                } else
                {
                    a = Vec(m_st->get_position(v0));
                    b = Vec(m_st->get_position(v1));
                }
                angle = acos(((a - vertPos).squaredNorm() + (b - vertPos).squaredNorm() - (a - b).squaredNorm()) / (2 * (a - vertPos).norm() * (b - vertPos).norm()));
                if (minangle < 0 || angle < minangle)
                    minangle = angle;
                if (maxangle < 0 || angle > maxangle)
                    maxangle = angle;
            }

            double minedge = -1;
            for (size_t j = 0; j < mesh.m_vertex_to_edge_map[i].size(); j++)
            {
                double edgelength = m_st->get_edge_length(mesh.m_vertex_to_edge_map[i][j]);
                if (minedge < 0 || edgelength < minedge)
                    minedge = edgelength;
            }

            if (m_mode == RM_JUNCTIONS)
                if (!junctionNeighborV(mesh, i))
                    continue;

            if (minedge < 0.01)
                glColor4f(1.0f, 1.0f, 0.0f, 1.0f);
            else if (minangle < 3 * M_PI / 180)
                glColor4f(0.0f, 1.0f, 1.0f, 1.0f);
            else if (maxangle > 177 * M_PI / 180)
                glColor4f(1.0f, 0.0f, 1.0f, 1.0f);
            else if(onBBWall(vertPos))
                glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
            else
                glColor4f(0.0f, 0.0f, 0.0f, 0.1f);

            glVertexVec3d(vertPos);
        }
        glEnd();

        if (mind_vertex >= 0)
        {
            glPointSize(8);
            glColor4f(0, 0, 0, 1);
            glBegin(GL_POINTS);
            glVertexVec3d(Vec(m_st->get_position(mind_vertex)));
            glEnd();
        }
        glDisable(GL_BLEND);
        glDepthMask(GL_TRUE);

    } else if (m_mode == RM_REGIONS)
    {
        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        //glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

        std::vector<Vec3d> faceNormals(m_st->m_mesh.nt());
        for (size_t i = 0; i < m_st->m_mesh.nt(); i++) faceNormals[i] = Vec(m_st->get_triangle_normal(i));

        // stats on total number of labels
        int maxlabel = -1;
        for (size_t i = 0; i < mesh.nt(); i++)
        {
            LosTopos::Vec2i regions = mesh.get_triangle_label(i);
            if (regions[0] >= maxlabel) maxlabel = regions[0];
            if (regions[1] >= maxlabel) maxlabel = regions[1];
        }
        m_nregions = maxlabel + 1;
        while (m_region_visible.size() < m_nregions)
            m_region_visible.push_back(true);

        // render bounding box
        glLineWidth(1);
        glColor3f(0.5, 0.5, 0.5);
        glBegin(GL_LINES);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(0.0, 0.0, 1.0);
        glVertex3f(1.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 1.0);
        glVertex3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 1.0, 1.0);
        glVertex3f(1.0, 1.0, 0.0);
        glVertex3f(1.0, 1.0, 1.0);

        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 0.0);
        glVertex3f(1.0, 1.0, 0.0);
        glVertex3f(1.0, 1.0, 0.0);
        glVertex3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 0.0, 0.0);

        glVertex3f(0.0, 0.0, 1.0);
        glVertex3f(1.0, 0.0, 1.0);
        glVertex3f(1.0, 0.0, 1.0);
        glVertex3f(1.0, 1.0, 1.0);
        glVertex3f(1.0, 1.0, 1.0);
        glVertex3f(0.0, 1.0, 1.0);
        glVertex3f(0.0, 1.0, 1.0);
        glVertex3f(0.0, 0.0, 1.0);
        glEnd();

        // Render all edges
        glLineWidth(1);
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        glDepthMask(GL_FALSE);
        glBegin(GL_LINES);

        for (size_t i = 0; i < mesh.ne(); i++)
        {
            Vec3d p0 = Vec(m_st->get_position(mesh.m_edges[i][0]));
            Vec3d p1 = Vec(m_st->get_position(mesh.m_edges[i][1]));

            bool visible = visibleE(i);

            bool xb = ((p0.x() < 1e-4 && p1.x() < 1e-4) || (p0.x() > 1 - 1e-4 && p1.x() > 1 - 1e-4));
            bool yb = ((p0.y() < 1e-4 && p1.y() < 1e-4) || (p0.y() > 1 - 1e-4 && p1.y() > 1 - 1e-4));
            bool zb = ((p0.z() < 1e-4 && p1.z() < 1e-4) || (p0.z() > 1 - 1e-4 && p1.z() > 1 - 1e-4));

            bool cubeedge = ((xb && yb) || (xb && zb) || (yb && zb));

            if (!visible && !cubeedge)
                continue;

            glColor4f(0.0f, 0.0f, 0.0f, 0.3f);

            glVertexVec3d(p0);
            glVertexVec3d(p1);
        }
        glEnd();

        if (mind_edge >= 0)
        {
            glColor4f(0.0, 0.0, 0.0, 1.0);
            glLineWidth(2);
            glBegin(GL_LINES);
            glVertexVec3d(Vec(m_st->get_position(mesh.m_edges[mind_edge][0])));
            glVertexVec3d(Vec(m_st->get_position(mesh.m_edges[mind_edge][1])));
            glEnd();
            glLineWidth(1);
        }
        glDisable(GL_BLEND);
        glDepthMask(GL_TRUE);

        // generate a list of colors for all the labels present
        std::vector<Vec3d> labelcolors;
        labelcolors.push_back(Vec3d(0, 0, 0));
        for (int i = 0; i < maxlabel + 1; i++)
        {
            float r, g, b;
            float t = (float)i / (maxlabel == 0 ? 1 : maxlabel);
            if (t < 0.5)
            {
                r = 1 - t * 1.5f;
                g = t * 1.5f;
                b = 0;
            } else
            {
                r = 0;
                g = 1 - (t - 0.5f) * 1.5f;
                b = (t - 0.5f) * 1.5f;
            }
            labelcolors.push_back(Vec3d(r, g, b));
        }

        // render back to front semi transparent
        float mv[16];
        glGetFloatv(GL_MODELVIEW_MATRIX, mv);
        Vec3d view_vec(mv[2], mv[6], mv[10]);  // assuming ModelView matrix contains only translation, rotation and uniform scaling

        std::vector<std::pair<size_t, double> > sorted_faces;
        for (size_t i = 0; i < mesh.nt(); i++)
        {
            Vec3d barycentre(0, 0, 0);
            for (int j = 0; j < 3; j++)
                barycentre += Vec(m_st->get_position(mesh.m_tris[i][j]));
            barycentre /= 3.0;

            double depth = barycentre.dot(view_vec);
            sorted_faces.push_back(std::pair<size_t, double>(i, depth));
        }

        FaceComp fc;
        std::sort(sorted_faces.begin(), sorted_faces.end(), fc);

        // Render all faces
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        glDepthMask(GL_FALSE);
        glBegin(GL_TRIANGLES);

        for (size_t i = 0; i < sorted_faces.size(); i++)
        {
            Vec3d barycentre(0, 0, 0);
            for (int j = 0; j < 3; j++)
                barycentre += Vec(m_st->get_position(mesh.m_tris[sorted_faces[i].first][j]));
            barycentre /= 3.0;

            LosTopos::Vec2i regions = mesh.get_triangle_label(sorted_faces[i].first);

            Vec3d color0 = labelcolors[regions[0] + 1];
            Vec3d color1 = labelcolors[regions[1] + 1];
            Vec3d color_combined = Vec3d::Zero();
            double alpha = 0.02;
            int visible_count = 0;
            if (regions[0] >= 0 && m_region_visible[regions[0]])
            {
                color_combined += color0;
                visible_count++;
                alpha += 0.3;
            }
            if (regions[1] >= 0 && m_region_visible[regions[1]])
            {
                color_combined += color1;
                visible_count++;
                alpha += 0.3;
            }
            if (visible_count == 0)
                color_combined = Vec3d(0.0,0.0,0.0);
            else
                color_combined /= visible_count;

            glColor4d(color_combined.x(), color_combined.y(), color_combined.z(), alpha);

            if (sorted_faces[i].first == mind_face)
                glColor4d(color_combined.x(), color_combined.y(), color_combined.z(), 1.0);

            for (int j = 0; j < 3; j++)
            {
                Vec3d pos = Vec(m_st->get_position(mesh.m_tris[sorted_faces[i].first][j]));
                glVertexVec3d(pos);
            }
        }

        glEnd();
        glDisable(GL_BLEND);
        glDepthMask(GL_TRUE);

        //Render vertex of interest
        if (mind_vertex >= 0)
        {
            glPointSize(4);
            glColor4f(0, 0, 0, 1);
            glBegin(GL_POINTS);
            glVertexVec3d(Vec(m_st->get_position(mind_vertex)));
            glEnd();
        }

    } else if (m_mode == RM_OPAQUE)
    {
        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        //glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

        std::vector<Vec3d> faceNormals(m_st->m_mesh.nt());
        for (size_t i = 0; i < m_st->m_mesh.nt(); i++) faceNormals[i] = Vec(m_st->get_triangle_normal(i));

        // stats on total number of labels
        int maxlabel = -1;
        for (size_t i = 0; i < mesh.nt(); i++)
        {
            LosTopos::Vec2i regions = mesh.get_triangle_label(i);
            if (regions[0] >= maxlabel) maxlabel = regions[0];
            if (regions[1] >= maxlabel) maxlabel = regions[1];
        }
        m_nregions = maxlabel + 1;
        while (m_region_visible.size() < m_nregions)
            m_region_visible.push_back(true);

        // render bounding box
        glLineWidth(1);
        glColor3f(0.5, 0.5, 0.5);
        glBegin(GL_LINES);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(0.0, 0.0, 1.0);
        glVertex3f(1.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 1.0);
        glVertex3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 1.0, 1.0);
        glVertex3f(1.0, 1.0, 0.0);
        glVertex3f(1.0, 1.0, 1.0);

        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 0.0);
        glVertex3f(1.0, 1.0, 0.0);
        glVertex3f(1.0, 1.0, 0.0);
        glVertex3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 0.0, 0.0);

        glVertex3f(0.0, 0.0, 1.0);
        glVertex3f(1.0, 0.0, 1.0);
        glVertex3f(1.0, 0.0, 1.0);
        glVertex3f(1.0, 1.0, 1.0);
        glVertex3f(1.0, 1.0, 1.0);
        glVertex3f(0.0, 1.0, 1.0);
        glVertex3f(0.0, 1.0, 1.0);
        glVertex3f(0.0, 0.0, 1.0);
        glEnd();

        // Render all non-manifold junctions
        glLineWidth(1);
        glDisable(GL_BLEND);
        glDepthMask(GL_TRUE);
        glPolygonOffset(1, 2);
        glEnable(GL_POLYGON_OFFSET_FILL);
        glBegin(GL_LINES);

        for (size_t i = 0; i < mesh.ne(); i++)
        {
            if (junction(mesh, i) == 2)
                continue;

            Vec3d p0 = Vec(m_st->get_position(mesh.m_edges[i][0]));
            Vec3d p1 = Vec(m_st->get_position(mesh.m_edges[i][1]));

            bool visible = visibleE(i);

            bool xb = ((p0.x() < 1e-4 && p1.x() < 1e-4) || (p0.x() > 1 - 1e-4 && p1.x() > 1 - 1e-4));
            bool yb = ((p0.y() < 1e-4 && p1.y() < 1e-4) || (p0.y() > 1 - 1e-4 && p1.y() > 1 - 1e-4));
            bool zb = ((p0.z() < 1e-4 && p1.z() < 1e-4) || (p0.z() > 1 - 1e-4 && p1.z() > 1 - 1e-4));

            bool cubeedge = ((xb && yb) || (xb && zb) || (yb && zb));

            if (!visible && !cubeedge)
                continue;

            glColor4f(0.0f, 0.0f, 0.0f, 0.3f);

            glVertexVec3d(p0);
            glVertexVec3d(p1);
        }
        glEnd();

        if (mind_edge >= 0)
        {
            glColor4f(0.0, 0.0, 0.0, 1.0);
            glLineWidth(2);
            glBegin(GL_LINES);
            glVertexVec3d(Vec(m_st->get_position(mesh.m_edges[mind_edge][0])));
            glVertexVec3d(Vec(m_st->get_position(mesh.m_edges[mind_edge][1])));
            glEnd();
            glLineWidth(1);
        }

        // generate a list of colors for all the labels present
        std::vector<Vec3d> labelcolors;
        labelcolors.push_back(Vec3d(0, 0, 0));
        for (int i = 0; i < maxlabel + 1; i++)
        {
            float r, g, b;
            float t = (float)i / (maxlabel == 0 ? 1 : maxlabel);
            if (t < 0.5)
            {
                r = 1 - t * 1.5f;
                g = t * 1.5f;
                b = 0;
            } else
            {
                r = 0;
                g = 1 - (t - 0.5f) * 1.5f;
                b = (t - 0.5f) * 1.5f;
            }
            labelcolors.push_back(Vec3d(r, g, b));
        }

        // render all faces opaque
        float mv[16];
        glGetFloatv(GL_MODELVIEW_MATRIX, mv);
//        Vec3d view_vec(mv[2], mv[6], mv[10]);  // assuming ModelView matrix contains only translation, rotation and uniform scaling
        // since the face between two regions would switch between two distinct colors when it flips from facing the viewer with one side to another, simply
        // using the z axis in the model view matrix does not suffice in the perspective projection mode.
        Vec3d eye = (MV.inverse() * Vec4d(0, 0, 0, 1)).segment<3>(0);

        glBegin(GL_TRIANGLES);

        for (size_t i = 0; i < mesh.nt(); i++)
        {
            const LosTopos::Vec3st & f = mesh.m_tris[i];

            Vec3d x0 = Vec(m_st->get_position(f[0]));
            Vec3d x1 = Vec(m_st->get_position(f[1]));
            Vec3d x2 = Vec(m_st->get_position(f[2]));

            LosTopos::Vec2i regions = mesh.get_triangle_label(i);

            Vec3d color0 = labelcolors[regions[0] + 1];
            Vec3d color1 = labelcolors[regions[1] + 1];
            Vec3d color_final = Vec3d::Zero();
            if ((regions[0] >= 0 && m_region_visible[regions[0]]) && (regions[1] >= 0 && m_region_visible[regions[1]]))
            {
                color_final = ((x1 - x0).cross(x2 - x0).dot(eye - x0) < 0 ? color0 : color1);
            } else if (regions[0] >= 0 && m_region_visible[regions[0]])
            {
                color_final = color0;
            } else if (regions[1] >= 0 && m_region_visible[regions[1]])
            {
                color_final = color1;
            } else
            {
                continue; // neither region visible: don't render this face at all
            }

            glColor3d(0.4 + 0.6 * color_final.x(), 0.4 + 0.6 * color_final.y(), 0.4 + 0.6 * color_final.z());

            if (i == mind_face)
                glColor3d(color_final.x() * 0.6, color_final.y() * 0.6, color_final.z() * 0.6);

            glVertexVec3d(x0);
            glVertexVec3d(x1);
            glVertexVec3d(x2);
        }

        glEnd();
        glDisable(GL_POLYGON_OFFSET_FILL);

        //Render vertex of interest
        if (mind_vertex >= 0)
        {
            glPointSize(4);
            glColor4f(0, 0, 0, 1);
            glBegin(GL_POINTS);
            glVertexVec3d(Vec(m_st->get_position(mind_vertex)));
            glEnd();
        }

    }

    glPopMatrix();

}


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_TYPE_TRAITS_INTERVAL_TYPE_DEFAULT_HPP_JOFA_100403
#define BOOST_ICL_TYPE_TRAITS_INTERVAL_TYPE_DEFAULT_HPP_JOFA_100403

#include <boost/mpl/if.hpp>
#include <boost/icl/detail/design_config.hpp>
#include <boost/icl/continuous_interval.hpp>
#include <boost/icl/discrete_interval.hpp>
#include <boost/icl/right_open_interval.hpp>
#include <boost/icl/left_open_interval.hpp>
#include <boost/icl/closed_interval.hpp>
#include <boost/icl/open_interval.hpp>
#include <boost/icl/type_traits/is_continuous.hpp>
#include <boost/icl/type_traits/is_discrete.hpp>

namespace boost{ namespace icl
{

    template <class DomainT, ICL_COMPARE Compare = ICL_COMPARE_INSTANCE(std::less, DomainT)>
    struct interval_type_default
    {
#ifdef BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS
        typedef
            typename mpl::if_< is_discrete<DomainT>
#  ifdef BOOST_ICL_DISCRETE_STATIC_INTERVAL_DEFAULT
                             , BOOST_ICL_DISCRETE_STATIC_INTERVAL_DEFAULT<DomainT,Compare>
#  else
                             , right_open_interval<DomainT,Compare>
#  endif

#  ifdef BOOST_ICL_CONTINUOUS_STATIC_INTERVAL_DEFAULT
                             , BOOST_ICL_CONTINUOUS_STATIC_INTERVAL_DEFAULT<DomainT,Compare>
#  else
                             , right_open_interval<DomainT,Compare>
#  endif
                             >::type type;
#else
        typedef
            typename mpl::if_< is_discrete<DomainT>
                             ,   discrete_interval<DomainT,Compare>
                             , continuous_interval<DomainT,Compare> >::type type;
#endif
    };

}} // namespace boost icl

#endif


#include <boost/numeric/odeint/stepper/symplectic_euler.hpp>


#include <iostream>

#include <Eigen/Core>
#include <catch2/catch.hpp>
#include <nlopt.hpp>

#include <opt/displacement_opt.hpp>

using namespace ipc::rigid;
using namespace opt;

TEST_CASE("Displacement optimization test", "[!shouldfail][opt][displacements]")
{
    Eigen::MatrixX2d V;
    V.resize(4, 2);
    Eigen::MatrixX2d U;
    U.resize(4, 2);
    Eigen::MatrixX2i E;
    E.resize(2, 2);
    double volume_epsilon = 1.0;
    DetectionMethod ccd_detection_method = DetectionMethod::BRUTE_FORCE;
    Eigen::MatrixX2d Uopt;
    Uopt.resize(4, 2);
    Eigen::MatrixX2d actual_Uopt;
    actual_Uopt.resize(4, 2);
    OptimizationMethod opt_method;
    unsigned max_iter = 1000;

    SECTION("Horizontal edge falling")
    {
        // TODO: Fix this test
        SECTION("MMA Optimizer") { opt_method = OptimizationMethod::MMA; }
        SECTION("SLSQP Optimizer") { opt_method = OptimizationMethod::SLSQP; }
        SECTION("IP Optimizer") { opt_method = OptimizationMethod::IP; }
        V << -1, 1, 1, 1, -2, 0, 2, 0;
        U << 0, -2, 0, -2, 0, 0, 0, 0;
        E << 0, 1, 2, 3;
        displacements_optimization(V, U, E, volume_epsilon,
            ccd_detection_method, opt_method, max_iter, Uopt);
        actual_Uopt << 0, -1, 0, -1, 0, 0, 0, 0;
        CHECK((Uopt - actual_Uopt).squaredNorm() == Approx(0.0).margin(1e-8));
    }
    // SECTION("Vertical edge falling")
    // {
    //     V << 0, 2, 0, 1, -1, 0, 1, 0;
    //     U << 0, -3, 0, -3, 0, 0, 0, 0;
    //     E << 0, 1, 2, 3;
    //     displacements_nlopt_step(
    //         V, U, E, volume_epsilon, ccd_detection_method, Uopt);
    //     V(2, 0) *= 2;
    //     V(3, 0) *= 2;
    //     displacements_nlopt_step(
    //         V, U, E, volume_epsilon, ccd_detection_method, actual_Uopt);
    //     CHECK((Uopt - actual_Uopt).squaredNorm() ==
    //     Approx(0.0).margin(1e-8));
    // }
}


/* A program to test the Boost library on the BeagleBone
* Written by Derek Molloy for the book "Exploring BeagleBone: Tools and
* Techniques for Building with Embedded Linux" by John Wiley & Sons, 2014
* ISBN 9781118935125. Please see the file README.md in the repository root
* directory for copyright and GNU GPLv3 license information.            */

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
using namespace boost::geometry::model::d2;
#include <iostream>

int main(){
   point_xy<float> p1(1.0,2.0), p2(3.0,4.0);
   float distance = boost::geometry::distance(p1,p2);
   std::cout << "Distance between points is: " << distance << std::endl;
   return 0;
}


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_DEALII_MATRIX_OPERATOR_HPP
#define MFMG_DEALII_MATRIX_OPERATOR_HPP

#include <mfmg/common/operator.hpp>

#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparsity_pattern.h>

namespace mfmg
{
template <typename VectorType>
class DealIIMatrixOperator : Operator<VectorType>
{
public:
  using operator_type = DealIIMatrixOperator<VectorType>;
  using vector_type = VectorType;

  DealIIMatrixOperator(
      std::shared_ptr<dealii::SparsityPattern> sparsity_pattern,
      std::shared_ptr<dealii::SparseMatrix<typename VectorType::value_type>>
          sparse_matrix);

  virtual ~DealIIMatrixOperator() override = default;

  void apply(vector_type const &x, vector_type &y,
             OperatorMode mode = OperatorMode::NO_TRANS) const override final;

  std::shared_ptr<Operator<VectorType>> multiply_transpose(
      std::shared_ptr<Operator<VectorType> const> b) const override final;

  std::shared_ptr<vector_type> build_domain_vector() const override final;

  std::shared_ptr<vector_type> build_range_vector() const override final;

  size_t grid_complexity() const override final;

  size_t operator_complexity() const override final;

private:
  // The sparsity pattern needs to outlive the sparse matrix, so we declare it
  // first.
  std::shared_ptr<dealii::SparsityPattern> _sparsity_pattern;

  std::shared_ptr<dealii::SparseMatrix<typename VectorType::value_type>>
      _sparse_matrix;
};
} // namespace mfmg

#endif


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"	// Precompiled headers
//
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/poses/CPose3DPDFSOG.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/poses/CPosePDFSOG.h>
#include <mrpt/poses/CPoseRandomSampler.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::random;

/*---------------------------------------------------------------
		Constructor
  ---------------------------------------------------------------*/
CPoseRandomSampler::CPoseRandomSampler() = default;
CPoseRandomSampler::CPoseRandomSampler(const CPoseRandomSampler& o)
{
	*this = o;
}

CPoseRandomSampler& CPoseRandomSampler::operator=(const CPoseRandomSampler& o)
{
	if (&o == this) return *this;
	m_pdf2D.reset();
	m_pdf3D.reset();
	if (o.m_pdf2D) m_pdf2D.reset(dynamic_cast<CPosePDF*>(o.m_pdf2D->clone()));
	if (o.m_pdf3D) m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(o.m_pdf3D->clone()));
	m_fastdraw_gauss_Z3 = o.m_fastdraw_gauss_Z3;
	m_fastdraw_gauss_Z6 = o.m_fastdraw_gauss_Z6;
	m_fastdraw_gauss_M_2D = o.m_fastdraw_gauss_M_2D;
	m_fastdraw_gauss_M_3D = o.m_fastdraw_gauss_M_3D;
	return *this;
}

CPoseRandomSampler::CPoseRandomSampler(CPoseRandomSampler&& o)
	: m_pdf2D(nullptr), m_pdf3D(nullptr)
{
	if (o.m_pdf2D)
	{
		m_pdf2D = std::move(o.m_pdf2D);
		o.m_pdf2D = nullptr;
	}
	if (o.m_pdf3D)
	{
		m_pdf3D = std::move(o.m_pdf3D);
		o.m_pdf3D = nullptr;
	}
	m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
	m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
	m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
	m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
}
CPoseRandomSampler& CPoseRandomSampler::operator=(CPoseRandomSampler&& o)
{
	if (this == &o) return *this;
	this->clear();
	if (o.m_pdf2D)
	{
		m_pdf2D = std::move(o.m_pdf2D);
		o.m_pdf2D = nullptr;
	}
	if (o.m_pdf3D)
	{
		m_pdf3D = std::move(o.m_pdf3D);
		o.m_pdf3D = nullptr;
	}
	m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
	m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
	m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
	m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
	return *this;
}

/*---------------------------------------------------------------
			clear
  ---------------------------------------------------------------*/
void CPoseRandomSampler::clear()
{
	m_pdf2D.reset();
	m_pdf3D.reset();
}

/*---------------------------------------------------------------
						setPosePDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::setPosePDF(const CPosePDF& pdf)
{
	MRPT_START

	clear();
	m_pdf2D.reset(dynamic_cast<CPosePDF*>(pdf.clone()));

	// According to the PDF type:
	if (IS_CLASS(pdf, CPosePDFGaussian))
	{
		const auto& gPdf = dynamic_cast<const CPosePDFGaussian&>(pdf);
		const CMatrixDouble33& cov = gPdf.cov;

		m_fastdraw_gauss_M_2D = gPdf.mean;

		std::vector<double> eigVals;
		cov.eig_symmetric(m_fastdraw_gauss_Z3, eigVals);

		// Scale eigenvectors with eigenvalues:
		mrpt::math::CMatrixDouble33 D;
		D.setDiagonal(eigVals);
		D = D.asEigen().array().sqrt().matrix();
		m_fastdraw_gauss_Z3.matProductOf_AB(m_fastdraw_gauss_Z3, D);
	}
	else if (IS_CLASS(pdf, CPosePDFParticles))
	{
		return;	 // Nothing to prepare.
	}
	else
	{
		THROW_EXCEPTION_FMT(
			"Unsupported class: %s", m_pdf2D->GetRuntimeClass()->className);
	}

	MRPT_END
}

/*---------------------------------------------------------------
						setPosePDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::setPosePDF(const CPose3DPDF& pdf)
{
	MRPT_START

	clear();
	m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(pdf.clone()));

	// According to the PDF type:
	if (IS_CLASS(pdf, CPose3DPDFGaussian))
	{
		const auto& gPdf = dynamic_cast<const CPose3DPDFGaussian&>(pdf);
		const CMatrixDouble66& cov = gPdf.cov;

		m_fastdraw_gauss_M_3D = gPdf.mean;

		std::vector<double> eigVals;
		cov.eig_symmetric(m_fastdraw_gauss_Z6, eigVals);

		// Scale eigenvectors with eigenvalues:
		mrpt::math::CMatrixDouble66 D;
		D.setDiagonal(eigVals);

		// Scale eigenvectors with eigenvalues:
		D = D.asEigen().array().sqrt().matrix();
		m_fastdraw_gauss_Z6.matProductOf_AB(m_fastdraw_gauss_Z6, D);
	}
	else if (IS_CLASS(pdf, CPose3DPDFParticles))
	{
		return;	 // Nothing to prepare.
	}
	else
	{
		THROW_EXCEPTION_FMT(
			"Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
	}

	MRPT_END
}

/*---------------------------------------------------------------
					drawSample
  ---------------------------------------------------------------*/
CPose2D& CPoseRandomSampler::drawSample(CPose2D& p) const
{
	MRPT_START

	if (m_pdf2D) { do_sample_2D(p); }
	else if (m_pdf3D)
	{
		CPose3D q;
		do_sample_3D(q);
		p.x(q.x());
		p.y(q.y());
		p.phi(q.yaw());
	}
	else
		THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");

	return p;
	MRPT_END
}

/*---------------------------------------------------------------
					drawSample
  ---------------------------------------------------------------*/
CPose3D& CPoseRandomSampler::drawSample(CPose3D& p) const
{
	MRPT_START

	if (m_pdf2D)
	{
		CPose2D q;
		do_sample_2D(q);
		p.setFromValues(q.x(), q.y(), 0, q.phi(), 0, 0);
	}
	else if (m_pdf3D)
	{
		do_sample_3D(p);
	}
	else
		THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");

	return p;
	MRPT_END
}

/*---------------------------------------------------------------
				  do_sample_2D: Sample from a 2D PDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::do_sample_2D(CPose2D& p) const
{
	MRPT_START
	ASSERT_(m_pdf2D);

	// According to the PDF type:
	if (IS_CLASS(*m_pdf2D, CPosePDFGaussian))
	{
		// ------------------------------
		//      A single gaussian:
		// ------------------------------
		CVectorDouble rndVector(3);
		rndVector.setZero();
		for (size_t i = 0; i < 3; i++)
		{
			double rnd = getRandomGenerator().drawGaussian1D_normalized();
			for (size_t d = 0; d < 3; d++)
				rndVector[d] += (m_fastdraw_gauss_Z3(d, i) * rnd);
		}

		p.x(m_fastdraw_gauss_M_2D.x() + rndVector[0]);
		p.y(m_fastdraw_gauss_M_2D.y() + rndVector[1]);
		p.phi(m_fastdraw_gauss_M_2D.phi() + rndVector[2]);
		p.normalizePhi();
	}
	else if (IS_CLASS(*m_pdf2D, CPosePDFSOG))
	{
		// -------------------------------------
		//      			SOG
		// -------------------------------------
		THROW_EXCEPTION("TODO");
	}
	else if (IS_CLASS(*m_pdf2D, CPosePDFParticles))
	{
		// -------------------------------------
		//      Particles: just sample as usual
		// -------------------------------------
		const auto& pdf = dynamic_cast<const CPosePDFParticles&>(*m_pdf2D);
		pdf.drawSingleSample(p);
	}
	else
		THROW_EXCEPTION_FMT(
			"Unsoported class: %s", m_pdf2D->GetRuntimeClass()->className);

	MRPT_END
}

/*---------------------------------------------------------------
				  do_sample_3D: Sample from a 3D PDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::do_sample_3D(CPose3D& p) const
{
	MRPT_START
	ASSERT_(m_pdf3D);

	// According to the PDF type:
	if (IS_CLASS(*m_pdf3D, CPose3DPDFGaussian))
	{
		// ------------------------------
		//      A single gaussian:
		// ------------------------------
		CVectorDouble rndVector(6);
		rndVector.setZero();
		for (size_t i = 0; i < 6; i++)
		{
			double rnd = getRandomGenerator().drawGaussian1D_normalized();
			for (size_t d = 0; d < 6; d++)
				rndVector[d] += (m_fastdraw_gauss_Z6(d, i) * rnd);
		}

		p.setFromValues(
			m_fastdraw_gauss_M_3D.x() + rndVector[0],
			m_fastdraw_gauss_M_3D.y() + rndVector[1],
			m_fastdraw_gauss_M_3D.z() + rndVector[2],
			m_fastdraw_gauss_M_3D.yaw() + rndVector[3],
			m_fastdraw_gauss_M_3D.pitch() + rndVector[4],
			m_fastdraw_gauss_M_3D.roll() + rndVector[5]);
	}
	else if (IS_CLASS(*m_pdf3D, CPose3DPDFSOG))
	{
		// -------------------------------------
		//      			SOG
		// -------------------------------------
		THROW_EXCEPTION("TODO");
	}
	else if (IS_CLASS(*m_pdf3D, CPose3DPDFParticles))
	{
		// -------------------------------------
		//      Particles: just sample as usual
		// -------------------------------------
		const auto& pdf = dynamic_cast<const CPose3DPDFParticles&>(*m_pdf3D);
		pdf.drawSingleSample(p);
	}
	else
		THROW_EXCEPTION_FMT(
			"Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);

	MRPT_END
}

/*---------------------------------------------------------------
				  isPrepared
  ---------------------------------------------------------------*/
bool CPoseRandomSampler::isPrepared() const { return m_pdf2D || m_pdf3D; }
/*---------------------------------------------------------------
				  getOriginalPDFCov2D
  ---------------------------------------------------------------*/
void CPoseRandomSampler::getOriginalPDFCov2D(CMatrixDouble33& cov3x3) const
{
	MRPT_START
	ASSERT_(this->isPrepared());

	if (m_pdf2D) { m_pdf2D->getCovariance(cov3x3); }
	else
	{
		ASSERT_(m_pdf3D);

		CPosePDFGaussian P;
		P.copyFrom(*m_pdf3D);
		cov3x3 = P.cov;
	}

	MRPT_END
}

/*---------------------------------------------------------------
				  getOriginalPDFCov3D
  ---------------------------------------------------------------*/
void CPoseRandomSampler::getOriginalPDFCov3D(CMatrixDouble66& cov6x6) const
{
	MRPT_START
	ASSERT_(this->isPrepared());

	if (m_pdf2D)
	{
		CPose3DPDFGaussian P;
		P.copyFrom(*m_pdf2D);
		cov6x6 = P.cov;
	}
	else
	{
		ASSERT_(m_pdf3D);
		m_pdf3D->getCovariance(cov6x6);
	}

	MRPT_END
}

/*---------------------------------------------------------------
				  getSamplingMean2D
  ---------------------------------------------------------------*/
CPose2D& CPoseRandomSampler::getSamplingMean2D(CPose2D& out_mean) const
{
	MRPT_START
	ASSERT_(this->isPrepared());

	if (m_pdf2D) out_mean = m_fastdraw_gauss_M_2D;
	else
		out_mean = CPose2D(m_fastdraw_gauss_M_3D);

	return out_mean;
	MRPT_END
}

/*---------------------------------------------------------------
				  getSamplingMean3D
  ---------------------------------------------------------------*/
CPose3D& CPoseRandomSampler::getSamplingMean3D(CPose3D& out_mean) const
{
	MRPT_START
	ASSERT_(this->isPrepared());

	if (m_pdf3D) out_mean = m_fastdraw_gauss_M_3D;
	else
		out_mean = CPose3D(m_fastdraw_gauss_M_2D);

	return out_mean;
	MRPT_END
}

void CPoseRandomSampler::getOriginalPDFCov2D(
	mrpt::math::CMatrixDouble& cov3x3) const
{
	mrpt::math::CMatrixDouble33 M;
	this->getOriginalPDFCov2D(M);
	cov3x3 = mrpt::math::CMatrixDouble(M);
}

void CPoseRandomSampler::getOriginalPDFCov3D(
	mrpt::math::CMatrixDouble& cov6x6) const
{
	mrpt::math::CMatrixDouble66 M;
	this->getOriginalPDFCov3D(M);
	cov6x6 = M;
}


#include <iostream>
#include <math.h>
#include <deque>
#include <sdf/sdf.hh>

#include "gazebo/gazebo.hh"
#include "gazebo/common/common.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/transport/transport.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/msgs/msgs.hh"

#include "map_request.pb.h"
#include "vector2d.pb.h"

#include <ros/ros.h>
#include <boost/filesystem/path.hpp>

using namespace std;

bool createVector(const char * vectorString,
                       gazebo::msgs::Vector2d* origin)
{
    string cornersStr = vectorString;
    size_t opening=0;
    size_t closing=0;

    opening = cornersStr.find('(', closing);
    closing = cornersStr.find(')', opening);
    if (opening == string::npos || closing == string::npos)
    {
        ROS_ERROR("Poorly formed string: %s", cornersStr.c_str());
        ROS_ERROR("( found at: %ld ) found at: %ld", opening, closing);
        return false;
    }
    string oneCornerStr = cornersStr.substr(opening + 1, closing - opening - 1);
    size_t commaLoc = oneCornerStr.find(",");
    string x = oneCornerStr.substr(0,commaLoc);
    string y = oneCornerStr.substr(commaLoc + 1, oneCornerStr.length() - commaLoc);
    origin->set_x(atof(x.c_str()));
    origin->set_y(atof(y.c_str()));

    return true;
}

int main(int argc, char * argv[])
{
    if (argc > 6)
    {
        dronedoc::msgs::MapRequest request;
        gazebo::msgs::Vector2d* origin = request.mutable_origin();

        if (!createVector(argv[3], origin))
        {
            ROS_ERROR("Failed to create rasterize region.");
            return -1;
        }

        request.set_height(atof(argv[1]));
        request.set_resolution(atof(argv[2]));
        request.set_map_width(atof(argv[4]));
        request.set_map_height(atof(argv[5]));

        std::string filename;
        if(argc < 7){
            filename = "map.png";
        } else {
            filename = argv[6];
        }
        boost::filesystem::path fname(filename);
        fname = boost::filesystem::absolute(fname);
        request.set_filename(fname.string());

        if (argc == 8)
        {
            request.set_threshold(atoi(argv[7]));
        }

        gazebo::transport::init();
        gazebo::transport::run();
        gazebo::transport::NodePtr node(new gazebo::transport::Node());
        node->Init("default");

        ROS_INFO("\nRequest: \n"
                 " origin.x: %f origin.y: %f\n"
                 " map_width: %f map_height: %f\n"
                 " Height: %f\n Resolution: %f\n Filename: %s\n Threshold: %d",
                 request.origin().x(),
                 request.origin().y(),
                 request.map_width(),
                 request.map_height(),
                 request.height(),
                 request.resolution(),
                 request.filename().c_str(),
                 request.threshold());

        gazebo::transport::PublisherPtr imagePub =
                node->Advertise<dronedoc::msgs::MapRequest>("~/collision_map/command");
        imagePub->WaitForConnection();
        imagePub->Publish(request);

        gazebo::transport::fini();
        return 0;
    }

    std::cout << "Usage: " <<
                 "rosrun dronedoc request_publisher a1 a2 a3 a4 a5 [a6 a7]\n" <<
                 "\ta1: height\n" <<
                 "\ta2: resolution\n" <<
                 "\ta3: \"(origin.x, origin.y)\"\n" <<
                 "\t  Origin is the point on lower left corner of map image\n"
                 "\ta4: map_width\n" <<
                 "\ta5: map_height\n" <<
                 "\ta6: filename\t[default=\"map\"]\n" <<
                 "\ta7: threshold\t[default=255]\n" << std::endl;

    return -1;
}


#ifndef __SPARSEQUADRATURE__CLASS__
#define __SPARSEQUADRATURE__CLASS__

#include <algorithm>

#include <Eigen/Dense>

#include "CONSTANTS.hpp"

#include "SparseIndexSet.hpp"
#include "TensorProductQuadrature.hpp"
#include "UnivariateQuadrature.hpp"

class SparseQuadrature {
 public:
  SparseQuadrature(void){};
  SparseQuadrature(const SparseIndexSet &spInd, UnivariateQuadrature &Q);
  void computeSparseQuadrature(const SparseIndexSet &spInd,
                               UnivariateQuadrature &Q);
  void purgeSparseQuadrature(void);
  const Eigen::MatrixXd &get_qPoints(void) const;
  const Eigen::VectorXd &get_qWeights(void) const;

 protected:
  Eigen::MatrixXd _qPoints;
  Eigen::VectorXd _qWeights;

  struct lexiCompareInd {
    const double *array;
    int N;
    lexiCompareInd(const Eigen::MatrixXd &A)
        : array(&(A(0, 0))), N((int)A.rows()){};

    bool operator()(const int &i, const int &j) {
      return std::lexicographical_compare(array + i * N, array + i * N + N,
                                          array + j * N, array + j * N + N);
    }
  };

  bool isEqual(const Eigen::VectorXd &vec1, const Eigen::VectorXd &vec2) {
    if (vec1.size() != vec2.size())
      return false;
    else if ((vec1 - vec2).lpNorm<Eigen::Infinity>() < __PRECISION__)
      return true;
    else
      return false;
  }
};

#endif


#include <iostream>                    // std::flush
#include <fstream>                     // std::ofstream
#include <string>                      // std::string
#include <tuple>                       // std::tuple
#include <vector>                      // std::vector
// For External Library
#include <torch/torch.h>               // torch
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "loss.hpp"                    // Loss
#include "networks.hpp"                // MC_AlexNet
#include "dataloader.hpp"              // DataLoader::ImageFolderClassesWithPaths
#include "visualizer.hpp"              // visualizer::graph

// Define Namespace
namespace po = boost::program_options;

// -------------------
// Validation Function
// -------------------
void valid(po::variables_map &vm, DataLoader::ImageFolderClassesWithPaths &valid_dataloader, torch::Device &device, Loss &criterion, MC_AlexNet &model, const std::vector<std::string> class_names, const size_t epoch, visualizer::graph &writer, visualizer::graph &writer_accuracy, visualizer::graph &writer_each_accuracy){

    constexpr size_t class_num_thresh = 10;  // threshold for the number of classes for determining whether to display accuracy graph for each class

    // (0) Initialization and Declaration
    size_t iteration;
    size_t mini_batch_size;
    size_t class_num;
    size_t total_match, total_counter;
    long int response, answer;
    float total_accuracy;
    float ave_loss, total_loss;
    std::ofstream ofs;
    std::vector<size_t> class_match, class_counter;
    std::vector<float> class_accuracy;
    std::tuple<torch::Tensor, torch::Tensor, std::vector<std::string>> mini_batch;
    torch::Tensor loss, image, label, output, responses;

    // (1) Memory Allocation
    class_num = class_names.size();
    class_match = std::vector<size_t>(class_num, 0);
    class_counter = std::vector<size_t>(class_num, 0);
    class_accuracy = std::vector<float>(class_num, 0.0);

    // (2) Tensor Forward per Mini Batch
    torch::NoGradGuard no_grad;
    model->eval();
    iteration = 0;
    total_loss = 0.0;
    total_match = 0; total_counter = 0;
    while (valid_dataloader(mini_batch)){

        image = std::get<0>(mini_batch).to(device);
        label = std::get<1>(mini_batch).to(device);
        mini_batch_size = image.size(0);

        output = model->forward(image);
        loss = criterion(output, label);

        responses = output.exp().argmax(/*dim=*/1);
        for (size_t i = 0; i < mini_batch_size; i++){
            response = responses[i].item<long int>();
            answer = label[i].item<long int>();
            class_counter[answer]++;
            total_counter++;
            if (response == answer){
                class_match[answer]++;
                total_match++;
            }
        }

        total_loss += loss.item<float>();
        iteration++;
    }

    // (3) Calculate Average Loss
    ave_loss = total_loss / (float)iteration;

    // (4) Calculate Accuracy
    for (size_t i = 0; i < class_num; i++){
        class_accuracy[i] = (float)class_match[i] / (float)class_counter[i];
    }
    total_accuracy = (float)total_match / (float)total_counter;

    // (5.1) Record Loss (Log/Loss)
    ofs.open("checkpoints/" + vm["dataset"].as<std::string>() + "/log/valid.txt", std::ios::app);
    ofs << "epoch:" << epoch << '/' << vm["epochs"].as<size_t>() << ' ' << std::flush;
    ofs << "classify:" << ave_loss << ' ' << std::flush;
    ofs << "accuracy:" << total_accuracy << std::endl;
    ofs.close();

    // (5.2) Record Loss (Log/Accuracy)
    ofs.open("checkpoints/" + vm["dataset"].as<std::string>() + "/log/valid.csv", std::ios::app);
    if (epoch == 1){
        ofs << "epoch," << std::flush;
        ofs << "accuracy," << std::flush;
        for (size_t i = 0; i < class_num; i++){
            ofs << i << "(" << class_names.at(i) << ")," << std::flush;
        }
        ofs << std::endl;
    }
    ofs << epoch << '/' << vm["epochs"].as<size_t>() << ',' << std::flush;
    ofs << total_accuracy << ',' << std::flush;
    for (size_t i = 0; i < class_num; i++){
        ofs << class_accuracy[i] << ',' << std::flush;
    }
    ofs << std::endl;
    ofs.close();

    // (5.3) Record Loss (Graph)
    writer.plot(/*base=*/epoch, /*value=*/{ave_loss});
    writer_accuracy.plot(/*base=*/epoch, /*value=*/{total_accuracy});
    if (class_num <= class_num_thresh){
        writer_each_accuracy.plot(/*base=*/epoch, /*value=*/class_accuracy);
    }

    // End Processing
    return;

}


#pragma once

#include <armadillo>
#include "net_raw.hpp"
#include "net.hpp"

inline Raw_Matrix to_raw(arma::Mat<float> & mat) {
  Raw_Matrix matrix;
  matrix.n_rows = mat.n_rows;
  matrix.n_cols= mat.n_cols;
  matrix.data = mat.memptr();
  return matrix;
}

template <typename activation, typename error>
inline Raw_FeedForward_Network<activation, error> convert_to_raw(FeedForward_Network<activation, error> & network) {

  Raw_FeedForward_Network<activation, error> raw;

  raw.num_layers = network.layer_sizes.size();
  raw.layer_sizes = new int [raw.num_layers];
  for (int i=0; i < raw.num_layers; ++i) {
    raw.layer_sizes[i] = network.layer_sizes[i];
  }

  raw.weights = new Raw_Matrix [network.weights.size()];
  raw.last_weights = new Raw_Matrix [network.last_weights.size()];
  for (int i=0; i < network.weights.size(); ++i) {
    raw.weights[i] = to_raw(network.weights[i]);
    raw.last_weights[i] = to_raw(network.last_weights[i]);
  }

  raw.activations = new Raw_Matrix [network.activations.size()];
  for (int i=0; i < network.activations.size(); ++i) {
    raw.activations[i] = to_raw(network.activations[i]);
  }

  raw.deltas = new Raw_Matrix [network.deltas.size()];
  for (int i=0; i < network.deltas.size(); ++i) {
    raw.deltas[i] = to_raw(network.deltas[i]);
  }
  return raw;
}

inline arma::Mat<float> from_raw(const Raw_Matrix & raw) {
  return arma::Mat<float>(raw.data, raw.n_rows, raw.n_cols);
}

template <typename activation, typename error>
inline void update_from_raw(FeedForward_Network<activation, error> & network, const Raw_FeedForward_Network<activation, error> &raw) {
  for (int i=0; i < raw.num_layers; ++i) {
    network.layer_sizes[i] = raw.layer_sizes[i];
  }

  for(int i=0; i < network.weights.size(); ++i) {
    network.weights[i] = from_raw(raw.weights[i]);
    network.last_weights[i] = from_raw(raw.last_weights[i]);
  }

  for(int i=0; i < network.activations.size(); ++i) {
    network.activations[i] = from_raw(raw.activations[i]);
  }

  for(int i=0; i < network.deltas.size(); ++i) {
    network.deltas[i] = from_raw(raw.deltas[i]);
  }
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE StepByStepSolverTest
#include <boost/test/unit_test.hpp>

#include "solve/StepByStepSolver.hpp"
#include "io/PBQP_Serializer.hpp"

namespace pbqppapa {

//This is the problem instance which KAPS could not solve
BOOST_AUTO_TEST_CASE(simpleSolve) {
	PBQPGraph<InfinityWrapper<unsigned int>>* graph = PBQP_Serializer<InfinityWrapper<unsigned int>>::loadFromFile(
	"test/testData/problematic.json");
	PBQPGraph<InfinityWrapper<unsigned int>>* copy = new PBQPGraph<InfinityWrapper<unsigned int>>(graph);
	StepByStepSolver<unsigned int> solver (graph);
	PBQPSolution<InfinityWrapper<unsigned int>>* sol = solver.solveFully();
	InfinityWrapper<unsigned int> cost = sol->getTotalCost(copy);
}

}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_MINMOD_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_MINMOD_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  minmod generic tag

      Represents the minmod function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct minmod_ : ext::elementwise_<minmod_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<minmod_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_minmod_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::minmod_, Site> dispatching_minmod_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::minmod_, Site>();
    }
    template<class... Args>
    struct impl_minmod_;
  }
  /*!
    Return the minimum of the two entries
    if they have the same sign, otherwise 0

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = minmod(x, y);
    @endcode

    is similar to:

    @code
    T r =  x*y > 0 ? min(x, y) : 0;
    @endcode

    @param  a0
    @param  a1

    @return      a value of the same type as the inputs.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::minmod_, minmod, 2)
} }

#endif


///////////////////////////////////////////////////////////////////////////////
// view::example::step_iterator.cpp                                          //
//                                                                           //
//  (C) Copyright 2009 Erwann Rogard                                         //
//  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)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <list>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/view//step_count_down_iterator.hpp>
#include <libs/view/example/step_iterator.h>

void example_step_iterator(std::ostream& os)
{
    os << "->example_step_iterator" << std::endl;
    using namespace boost;
    using namespace view;
    typedef int val_;
    typedef std::vector<val_> vals_;
    typedef range_iterator<vals_>::type vals_it_;
    typedef range_difference<vals_>::type diff_;
    typedef view::detail::step_count_down_iterator<vals_it_> step_it_;

    const unsigned n = 3;
    const diff_ m = 2;
    vals_ vals;
    vals.reserve( n * m );

    for(unsigned i = 0; i<n*m; i++){
        vals.push_back(i);
    }
    vals_it_ b = boost::begin( vals );
    vals_it_ e = boost::end( vals );

    for(unsigned j = 0; j<m; j++){
        vals_it_ it = boost::next(b,j);
        step_it_ sb
            = view::detail::make_step_count_down_iterator(it,e,m);
        step_it_ se
            = view::detail::make_end_step_count_down_iterator(it,e,m);
        BOOST_ASSERT(se.count_down() == 0);
        BOOST_ASSERT(std::distance(sb,se)==n);
        {
            step_it_ si = sb;
            unsigned i = 0;
            //jth column
            while(si<se){
                val_ k = i*m + j;
                BOOST_ASSERT(k == *si);
                ++i;
                ++si;
            }
            si = boost::prior(si,n);
            BOOST_ASSERT(si==sb);
        }
        {
            step_it_ si = se;
            unsigned i = n;
            do{
                --si;
                --i;
                val_ k = i*m + j;
                BOOST_ASSERT(*si==k);
            }while(
                si!=sb
            );
            si = boost::next(si,n);
            BOOST_ASSERT(si==se);
        }
    }
    os << " end." << std::endl;
}


//
// Copyright 2019 Mateusz Loskot <mateusz at loskot dot net>
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
//
// 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
//
#define BOOST_DISABLE_ASSERTS 1 // kernel_1d_adaptor assertions are too strict
#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/kernel.hpp>

#include <boost/core/lightweight_test.hpp>

#include <vector>

namespace gil = boost::gil;

void test_kernel_1d_default_constructor()
{
    gil::kernel_1d<int> k;
    BOOST_TEST_EQ(k.center(), 0);
    BOOST_TEST_EQ(k.left_size(), 0);
    BOOST_TEST_EQ(static_cast<int>(k.right_size()), -1);
    // std::vector interface
    BOOST_TEST_EQ(k.size(), 0);
}

void test_kernel_2d_default_constructor()
{
    gil::detail::kernel_2d<int> k;
    BOOST_TEST_EQ(k.center_y(), 0);
    BOOST_TEST_EQ(k.center_x(), 0);

    //BOOST_TEST_EQ(k.left_size(), 0);
    //BOOST_TEST_EQ(k.right_size(), -1);
    BOOST_TEST_EQ(k.upper_size(), 0);
    BOOST_TEST_EQ(static_cast<int>(k.lower_size()), -1);
    // std::vector interface
    BOOST_TEST_EQ(k.size(), 0);
}

void test_kernel_1d_parameterized_constructor()
{
    gil::kernel_1d<int> k(9, 4);
    BOOST_TEST_EQ(k.center(), 4);
    BOOST_TEST_EQ(k.left_size(), 4);
    BOOST_TEST_EQ(k.right_size(), 4);
    // std::vector interface
    BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_2d_parameterized_constructor()
{
    gil::detail::kernel_2d<int> k(9, 4, 4);
    BOOST_TEST_EQ(k.center_y(), 4);
    BOOST_TEST_EQ(k.center_x(), 4);
    BOOST_TEST_EQ(k.left_size(), 4);
    BOOST_TEST_EQ(k.right_size(), 4);
    BOOST_TEST_EQ(k.upper_size(), 4);
    BOOST_TEST_EQ(k.lower_size(), 4);
    // std::vector interface
    BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_1d_parameterized_constructor_with_iterator()
{
    std::vector<int> v(9);
    gil::kernel_1d<int> k(v.cbegin(), v.size(), 4);
    BOOST_TEST_EQ(k.center(), 4);
    BOOST_TEST_EQ(k.left_size(), 4);
    BOOST_TEST_EQ(k.right_size(), 4);
    // std::vector interface
    BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_2d_parameterized_constructor_with_iterator()
{
    std::vector<int> v(81);
    gil::detail::kernel_2d<int> k(v.cbegin(), v.size(), 4, 4);
    BOOST_TEST_EQ(k.center_y(), 4);
    BOOST_TEST_EQ(k.center_x(), 4);
    BOOST_TEST_EQ(k.left_size(), 4);
    BOOST_TEST_EQ(k.right_size(), 4);
    BOOST_TEST_EQ(k.upper_size(), 4);
    BOOST_TEST_EQ(k.lower_size(), 4);
    // std::vector interface
    BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_1d_copy_constructor()
{
    gil::kernel_1d<int> d(9, 4);
    gil::kernel_1d<int> k(d);
    BOOST_TEST_EQ(k.center(), 4);
    BOOST_TEST_EQ(k.center(), d.center());
    BOOST_TEST_EQ(k.left_size(), d.left_size());
    BOOST_TEST_EQ(k.right_size(), d.right_size());
    // std::vector interface
    BOOST_TEST_EQ(k.size(), d.size());
}

void test_kernel_2d_copy_constructor()
{
    gil::detail::kernel_2d<int> d(9, 4, 4);
    gil::detail::kernel_2d<int> k(d);
    BOOST_TEST_EQ(k.center_y(), 4);
    BOOST_TEST_EQ(k.center_x(), 4);
    BOOST_TEST_EQ(k.center_y(), d.center_y());
    BOOST_TEST_EQ(k.center_x(), d.center_x());
    BOOST_TEST_EQ(k.left_size(), d.left_size());
    BOOST_TEST_EQ(k.right_size(), d.right_size());
    BOOST_TEST_EQ(k.lower_size(), d.lower_size());
    BOOST_TEST_EQ(k.upper_size(), d.upper_size());
    // std::vector interface
    BOOST_TEST_EQ(k.size(), d.size());
}

void test_kernel_1d_assignment_operator()
{
    gil::kernel_1d<int> d(9, 4);
    gil::kernel_1d<int> k;
    k = d;
    BOOST_TEST_EQ(k.center(), 4);
    BOOST_TEST_EQ(k.center(), d.center());
    BOOST_TEST_EQ(k.left_size(), d.left_size());
    BOOST_TEST_EQ(k.right_size(), d.right_size());
    // std::vector interface
    BOOST_TEST_EQ(k.size(), d.size());
}

void test_kernel_2d_assignment_operator()
{
    gil::detail::kernel_2d<int> d(9, 4, 4);
    gil::detail::kernel_2d<int> k;
    k = d;
    BOOST_TEST_EQ(k.center_y(), 4);
    BOOST_TEST_EQ(k.center_x(), 4);
    BOOST_TEST_EQ(k.center_y(), d.center_y());
    BOOST_TEST_EQ(k.center_x(), d.center_x());
    BOOST_TEST_EQ(k.left_size(), d.left_size());
    BOOST_TEST_EQ(k.right_size(), d.right_size());
    BOOST_TEST_EQ(k.lower_size(), d.lower_size());
    BOOST_TEST_EQ(k.upper_size(), d.upper_size());
    // std::vector interface
    BOOST_TEST_EQ(k.size(), d.size());
}

void test_kernel_1d_reverse_kernel()
{
    gil::kernel_1d<int> d(12, 4);
    BOOST_TEST_EQ(d.center(), 4);
    auto k = gil::reverse_kernel(d);
    BOOST_TEST_EQ(k.center(), d.right_size());
    // std::vector interface
    BOOST_TEST_EQ(k.size(), d.size());
}

void test_kernel_2d_reverse_kernel()
{
    std::vector<int> data = {1, 2, 3, 4, 5, 6, 7, 8, 9};
    gil::detail::kernel_2d<int> d(data.begin(), data.size(), 2, 0);

    BOOST_TEST_EQ(d.center_x(), 0);
    BOOST_TEST_EQ(d.center_y(), 2);

    auto k = gil::detail::reverse_kernel(d);

    BOOST_TEST_EQ(k.center_x(), d.lower_size());
    BOOST_TEST_EQ(k.center_x(), 0);

    BOOST_TEST_EQ(k.center_y(), d.right_size());
    BOOST_TEST_EQ(k.center_y(), 2);

    // std::vector interface
    BOOST_TEST_EQ(k.size(), d.size());

    for (size_t i = 0; i < k.size() * k.size(); i++)
    {
        BOOST_TEST_EQ(d[i], k[(k.size()*k.size()) - 1 - i]);
    }
}

int main()
{
    test_kernel_1d_default_constructor();
    test_kernel_2d_default_constructor();
    test_kernel_1d_parameterized_constructor();
    test_kernel_2d_parameterized_constructor();
    test_kernel_1d_parameterized_constructor_with_iterator();
    test_kernel_2d_parameterized_constructor_with_iterator();
    test_kernel_1d_copy_constructor();
    test_kernel_2d_copy_constructor();
    test_kernel_1d_assignment_operator();
    test_kernel_2d_assignment_operator();
    test_kernel_1d_reverse_kernel();
    test_kernel_2d_reverse_kernel();

    return ::boost::report_errors();
}


#include "cnpy/cnpy.h"
#include <Eigen/Dense>
#include <boost/program_options.hpp>
#include <iostream>
#include <npy.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

namespace po = boost::program_options;

int main(int argc, char** argv)
{
    // Program options
    po::options_description desc("Read a .npz file containing data types from python\n\n"
                                 "Options");
    std::string filepath;
    desc.add_options()("help,h", "Print this help")(
        "filepath,f", po::value<std::string>(&filepath)->required(), "Set the input path for the data");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    std::cout << "opening: " << filepath << std::endl;
    cnpy::npz_t npz = cnpy::npz_load(filepath);

    // --- Processing Normal (Numpy) Matrix

    auto numpyArray = npz["ht_matrix"];
    Eigen::MatrixXd eigenMat =
        Eigen::Map<Eigen::MatrixXd>(numpyArray.data<double>(), numpyArray.shape[1], numpyArray.shape[0]);
    // need the following if the matrix is stored with C-order (this is due Eigen is using F-order)
    eigenMat.transposeInPlace();
    std::cout << "Numpy matrix (as Eigen matrix): " << std::endl << eigenMat << std::endl;

    // ---

    // Processing Image (OpenCV) Matrix

    cnpy::NpyArray npzImageArray = npz["img"];
    // need to know data type before hand
    // can be fixed with modifying the cnpy library
    // the typename is actually stored in the file.
    auto* imageData = npzImageArray.data<uint8_t>();

    std::cout << "image mat shape: ";
    for (const auto& s : npzImageArray.shape) {
        std::cout << s << ",";
    }
    std::cout << std::endl;

    cv::Mat image(npzImageArray.shape[0], npzImageArray.shape[1], CV_8UC3, cv::Scalar(0, 0, 0));
    // see https://www.geeksforgeeks.org/display-numpy-array-in-fortran-order
    // for the ordering of data
    // by default numpy save in C-order (row-major)
    // Note: there may be a way to avoid copy using constructor:
    //       cv::Mat(const std::vector< _Tp > &vec, bool copyData=false)
    for (auto i = 0; i < npzImageArray.shape[0]; i++) {
        for (auto j = 0; j < npzImageArray.shape[1]; j++) {
            auto& p = image.at<cv::Vec3b>(i, j);

            p[0] = imageData[i * npzImageArray.shape[1] * 3 + j * 3];
            p[1] = imageData[i * npzImageArray.shape[1] * 3 + j * 3 + 1];
            p[2] = imageData[i * npzImageArray.shape[1] * 3 + j * 3 + 2];
        }
    }

    cv::imshow("Image", image);
    cv::waitKey(0);

    // ---

    // --- Processing PointCloud (PCL) Matrix
    cnpy::NpyArray pointCloudArray = npz["pcd_numpy"];
    auto* pointCloudData = pointCloudArray.data<double>();
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    // pack r/g/b into rgb
    const std::uint8_t r = 0, g = 255, b = 0;
    std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
    for (auto i = 0; i < pointCloudArray.shape[0]; i++) {
        pcl::PointXYZRGB point;
        point.x = pointCloudData[i*pointCloudArray.shape[1]];
        point.y = pointCloudData[i*pointCloudArray.shape[1]+1];
        point.z = pointCloudData[i*pointCloudArray.shape[1]+2];
        point.rgb  = *reinterpret_cast<float*>(&rgb);
        cloud->push_back(point);
    }

    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped()) {
    }

    // ---

    return 0;
}


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_SDK_META_UPGRADE_HPP_INCLUDED
#define NT2_SDK_META_UPGRADE_HPP_INCLUDED

//////////////////////////////////////////////////////////////////////////////
// Returns the input type rebound with a type that is twice as big
// as its primitive, with optional sign.
// If the primitive type is floating-point, sign is ignored.
// See: http://nt2.metascale.org/sdk/meta/traits/upgrade.html
//////////////////////////////////////////////////////////////////////////////
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/sdk/meta/sign_of.hpp>
#include <nt2/sdk/meta/make_integer.hpp>
#include <nt2/sdk/meta/primitive_of.hpp>
#include <nt2/sdk/meta/factory_of.hpp>
#include <nt2/sdk/meta/is_fundamental.hpp>
#include <boost/mpl/apply.hpp>

namespace nt2 { namespace details
{
  //////////////////////////////////////////////////////////////////////////////
  // Integral types are upgraded using make_integer
  //////////////////////////////////////////////////////////////////////////////
  template<class T,std::size_t Size, class Sign, class Lambda>
  struct upgrade : meta::make_integer<Size*2,Sign,Lambda> {};

  //////////////////////////////////////////////////////////////////////////////
  // If type size is 8, return the type itself for any category
  //////////////////////////////////////////////////////////////////////////////
  template<class T, class Sign, class Lambda>
  struct   upgrade<T,8,Sign,Lambda>
        : meta::make_integer<8,Sign,Lambda> {};

  template<class Sign, class Lambda>
  struct upgrade<double,sizeof(double),Sign,Lambda>
  {
    typedef typename boost::mpl::apply1<Lambda, double>::type type;
  };

  template<class Sign, class Lambda>
  struct upgrade<float,sizeof(float),Sign,Lambda>
  {
    typedef typename boost::mpl::apply1<Lambda, double>::type type;
  };
} }

namespace nt2 { namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // For any type, return the integer type of size equals to sizeof(T)*2
  // with an optional sign change
  //////////////////////////////////////////////////////////////////////////////
  template<class T,class Sign=typename meta::sign_of<T>::type>
  struct  upgrade
        : details::upgrade< typename meta::primitive_of<typename meta::strip<T>::type>::type
                          , sizeof(typename meta::primitive_of<typename meta::strip<T>::type>::type)
                          ,Sign
                          , typename meta::factory_of<typename meta::strip<T>::type>::type
                          >
  {
    NT2_STATIC_ASSERT ( (is_fundamental<typename meta::primitive_of<typename meta::strip<T>::type>::type>::value)
                      , NT2_NON_FUNDAMENTAL_PRIMITIVE_USED_IN_META_DOWNGRADE
                      , "A type with a non-fundamental primitive is used in nt2::meta::downgrade."
                      );
  };

} }

#endif


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 2, 3> test_long_2_3(const Eigen::Matrix<boost::int64_t, 2, 3> & M)
{
	return M;
}
void export_long_2_3()
{
	boost::python::def("test_long_2_3",test_long_2_3);
}


//
//=======================================================================
// Copyright 2012
// Author: Alex Hagen-Zanker
//
// 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)
//=======================================================================
//
// dijkstra_state is a class to hold all property_map, parameters, the
// queue and graph reference, that are used in the dijkstra shortest paths
// algorithm.
//
//=======================================================================
//

#ifndef BLINK_GRAPH_DIJKSTRA_STATE_HPP
#define BLINK_GRAPH_DIJKSTRA_STATE_HPP

#include <blink/graph/dijkstra_parameter_helper.hpp>

#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>

namespace blink {

template< typename Graph, typename EdgeWeight, typename VertexIndex
  , typename VertexDistance, typename VertexPredecessor, typename VertexColor
  , typename DistanceCompare, typename DistanceCombine, typename DistanceZero, typename DistanceInf
  , typename MaxPriorityQueue, typename GraphVisitor>
struct dijkstra_state
{
private:
	typedef typename MaxPriorityQueue queue_stored_type;
	typedef typename processed_type<queue_stored_type>::type queue_type;
	typedef typename processed_type<queue_stored_type>::stored_type_tag queue_stored_type_tag;

public:
    // The queue must be wrapped in either a boost::reference_wrapper or a shared_ptr
  dijkstra_state(const Graph& graph, EdgeWeight weight, VertexIndex index,
    VertexDistance distance, VertexPredecessor predecessor , VertexColor
    color, DistanceCompare compare, DistanceCombine combine, DistanceZero zero,
    DistanceInf inf, MaxPriorityQueue queue, GraphVisitor visitor)
    : m_graph(graph), m_edge_weight(weight), m_vertex_index(index),
    m_vertex_distance(distance), m_vertex_predecessor(predecessor),
    m_vertex_color(color), m_distance_compare(compare), m_distance_combine(combine),
    m_distance_zero(zero), m_distance_inf(inf), m_max_priority_queue(queue),
    m_graph_visitor(visitor)
  {}

	typedef Graph graph_type;

	template<typename Tag> struct param {};
	template<> struct param<boost::edge_weight_t>        { typedef EdgeWeight        type; };
	template<> struct param<boost::vertex_index_t>       { typedef VertexIndex       type; };
	template<> struct param<boost::vertex_distance_t>    { typedef VertexDistance    type; };
	template<> struct param<boost::vertex_predecessor_t> { typedef VertexPredecessor type; };
	template<> struct param<boost::vertex_color_t>       { typedef VertexColor       type; };
	template<> struct param<boost::distance_compare_t>   { typedef DistanceCompare   type; };
	template<> struct param<boost::distance_combine_t>   { typedef DistanceCombine   type; };
	template<> struct param<boost::distance_inf_t>       { typedef DistanceInf       type; };
	template<> struct param<boost::distance_zero_t>      { typedef DistanceZero      type; };
	template<> struct param<boost::graph_visitor_t>      { typedef GraphVisitor      type; };
	template<> struct param<boost::max_priority_queue_t> { typedef queue_type        type; };

  EdgeWeight&        get(const boost::edge_weight_t&)         { return m_edge_weight; }
	VertexIndex&       get(const boost::vertex_index_t&)        { return m_vertex_index; }
	VertexDistance&    get(const boost::vertex_distance_t&)     { return m_vertex_distance; }
	VertexPredecessor& get(const boost::vertex_predecessor_t&)  { return m_vertex_predecessor; }
	VertexColor&       get(const boost::vertex_color_t&)        { return m_vertex_color; }
	DistanceCompare&   get(const boost::distance_compare_t&)    { return m_distance_compare; }
	DistanceCombine&   get(const boost::distance_combine_t&)    { return m_distance_combine; }
	DistanceInf&       get(const boost::distance_inf_t&)        { return m_distance_inf; }
	DistanceZero&      get(const boost::distance_zero_t&)       { return m_distance_zero; }
	GraphVisitor&      get(const boost::graph_visitor_t&)       { return m_graph_visitor; }

  queue_type&        get(const boost::max_priority_queue_t&)
	{
		return get_queue(queue_stored_type_tag());
	}

	template<typename Tag>
	typename param<Tag>::type& get()
	{
		return get(Tag());
	}

	inline const Graph& get_graph() const
  {
    return m_graph;
  }

private:
	queue_type& get_queue(const stored_ref_tag&)
	{
		return m_max_priority_queue.get();
	}

	queue_type& get_queue(const stored_ptr_tag&)
	{
		return *m_max_priority_queue;
	}

	// The distance in the distance map must be the same as the zero and inf distance types
	// as well as the weight in the edges.
  typedef typename boost::property_traits<
    typename param<boost::vertex_distance_t>::type>::value_type distance_type;

  typedef typename boost::property_traits<
    typename param<boost::edge_weight_t>::type>::value_type distance_type_check;

	BOOST_STATIC_ASSERT((boost::is_same<distance_type,
    typename param<boost::distance_zero_t>::type>::value));
  BOOST_STATIC_ASSERT((boost::is_same<distance_type,
    typename param<boost::distance_inf_t>::type>::value));
  BOOST_STATIC_ASSERT((boost::is_same<distance_type, distance_type_check>::value));

  const Graph& m_graph;
  EdgeWeight m_edge_weight;
  VertexPredecessor m_vertex_predecessor;
  VertexDistance m_vertex_distance;
  VertexColor m_vertex_color;
  VertexIndex m_vertex_index;
  DistanceCompare m_distance_compare;
  DistanceCombine m_distance_combine;
  DistanceZero m_distance_zero;
  DistanceInf m_distance_inf;
  MaxPriorityQueue m_max_priority_queue;
  GraphVisitor m_graph_visitor;
};// dijkstra_state

// extends dijkstra_parameter_helper with the dijkstra_state type
template<typename Graph, typename Params>
struct dijkstra_state_helper : protected dijkstra_parameter_helper<Graph, Params>
{
  typedef typename dijkstra_parameter_helper<Graph, Params> parent;

  template<typename Tag>
  struct param : parent::param<Tag>
  {};

  typedef dijkstra_state<Graph
    , typename param<boost::edge_weight_t>::stored_type
    , typename param<boost::vertex_index_t>::stored_type
    , typename param<boost::vertex_distance_t>::stored_type
    , typename param<boost::vertex_predecessor_t>::stored_type
    , typename param<boost::vertex_color_t>::stored_type
    , typename param<boost::distance_compare_t>::stored_type
    , typename param<boost::distance_combine_t>::stored_type
    , typename param<boost::distance_zero_t>::stored_type
    , typename param<boost::distance_inf_t>::stored_type
    , typename param<boost::max_priority_queue_t>::stored_type
    , typename param<boost::graph_visitor_t>::stored_type
    > type;

  static type make(const Graph& g, const Params& params)
  {
    parent::param<boost::edge_weight_t>::stored_type edge_weight_map
      = parent::make(boost::edge_weight_t(), params, g);

    parent::param<boost::vertex_index_t>::stored_type vertex_index_map
      = parent::make(boost::vertex_index_t(), params,  g);

    parent::param<boost::vertex_distance_t>::stored_type vertex_distance_map
      = parent::make(boost::vertex_distance_t(), params, g, vertex_index_map);

    parent::param<boost::vertex_predecessor_t>::stored_type  vertex_predecessor_map
      = parent::make(boost::vertex_predecessor_t(), params);

    parent::param<boost::vertex_color_t>::stored_type vertex_color_map
      = parent::make(boost::vertex_color_t(), params, g, vertex_index_map);

    parent::param<boost::distance_compare_t>::stored_type comparison_object
      = parent::make(boost::distance_compare_t(), params);

    parent::param<boost::distance_inf_t>::stored_type distance_inf_value
      = parent::make(boost::distance_inf_t(), params);

    parent::param<boost::distance_combine_t>::stored_type combine_object
      = parent::make(boost::distance_combine_t(), params, distance_inf_value);

    parent::param<boost::distance_zero_t>::stored_type distance_zero_value
      = parent::make(boost::distance_zero_t(), params);

    parent::param<boost::max_priority_queue_t>::stored_type max_priority_queue_object
      = parent::make(boost::max_priority_queue_t(), params, g,
      vertex_distance_map, comparison_object, vertex_index_map);

    parent::param<boost::graph_visitor_t>::stored_type visitor
      = parent::make(boost::graph_visitor_t(), params);

    return type(g, edge_weight_map, vertex_index_map,
      vertex_distance_map, vertex_predecessor_map, vertex_color_map,
      comparison_object, combine_object, distance_zero_value,
      distance_inf_value, max_priority_queue_object, visitor);
  }
}; // struct dijkstra_state_helper

// Make a dijkstra_state object
template <typename Graph, typename Params>
typename dijkstra_state_helper<Graph, Params>::type
  make_dijkstra_state(const Graph& g, const Params& params)
{
  return dijkstra_state_helper<Graph, Params>::make(g, params);
}

// Make a dijkstra_state object, using only default parameters
template <typename Graph>
typename dijkstra_state_helper<Graph, boost::no_named_parameters>::type
  make_dijkstra_state(const Graph& g)
{
  return make_dijkstra_state(g, boost::no_named_parameters() );
}

// by deriving from dijktra_state_mixin, a class has easy access to all dijkstra parameters and typedefs
template<typename DijkstraState>
struct dijkstra_state_mixin
{
  dijkstra_state_mixin(const boost::shared_ptr<DijkstraState>& state)
    : m_dijkstra_state(state)
    , m_graph              (m_dijkstra_state->get_graph())
    , m_distance_combine   (m_dijkstra_state->get(boost::distance_combine_t()))
    , m_distance_compare   (m_dijkstra_state->get(boost::distance_compare_t()))
    , m_distance_inf       (m_dijkstra_state->get(boost::distance_inf_t()))
    , m_distance_zero      (m_dijkstra_state->get(boost::distance_zero_t()))
    , m_edge_weight        (m_dijkstra_state->get(boost::edge_weight_t()))
    , m_max_priority_queue (m_dijkstra_state->get(boost::max_priority_queue_t()))
    , m_vertex_color       (m_dijkstra_state->get(boost::vertex_color_t()))
    , m_vertex_distance    (m_dijkstra_state->get(boost::vertex_distance_t()))
    , m_vertex_index       (m_dijkstra_state->get(boost::vertex_index_t()))
    , m_vertex_predecessor (m_dijkstra_state->get(boost::vertex_predecessor_t()))
    , m_graph_visitor      (m_dijkstra_state->get(boost::graph_visitor_t()))
  {}

  template<typename Tag>
  struct param
  {
    typedef typename DijkstraState::template param<Tag>::type type;
  };

protected:
  typedef typename DijkstraState::graph_type graph_type;
  typedef typename param<boost::distance_combine_t    >::type distance_combine_type;
  typedef typename param<boost::distance_compare_t    >::type distance_compare_type;
  typedef typename param<boost::distance_inf_t        >::type distance_inf_type;
  typedef typename param<boost::distance_zero_t       >::type distance_zero_type;
  typedef typename param<boost::edge_weight_t         >::type edge_weight_type;
  typedef typename param<boost::max_priority_queue_t>  ::type max_priority_queue_type;
  typedef typename param<boost::vertex_color_t        >::type vertex_color_type;
  typedef typename param<boost::vertex_distance_t     >::type vertex_distance_type;
  typedef typename param<boost::vertex_index_t        >::type vertex_index_type;
  typedef typename param<boost::vertex_predecessor_t  >::type vertex_predecessor_type;
  typedef typename param<boost::graph_visitor_t       >::type graph_visitor_type;

  typedef typename boost::graph_traits<graph_type> graph_traits;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::vertex_iterator   vertex_iterator;
  typedef typename graph_traits::edge_descriptor   edge_descriptor;
  typedef typename graph_traits::out_edge_iterator out_edge_iterator;

  typedef typename boost::property_traits<vertex_distance_type>::value_type distance_type;
  typedef typename boost::property_traits<vertex_color_type   >::value_type color_type;

  typedef typename boost::color_traits<color_type> color_traits;

public:

  const DijkstraState& get_dijkstra_state() const
  {
    return *m_dijkstra_state;
  }

  DijkstraState& get_dijkstra_state()
  {
    return *m_dijkstra_state;
  }

  template<typename Tag>
  typename param<Tag>::type& get(Tag tag = Tag())
  {
    return m_dijkstra_state->get<Tag>();
  }

  template<typename Tag>
  const typename param<Tag>::type& get(Tag tag = Tag()) const
  {
    return m_dijkstra_state->get<Tag>();
  }

  inline const graph_type& get_graph() const
  {
    return graph;
  }

private:
  boost::shared_ptr<DijkstraState> m_dijkstra_state; // MUST BE DECLARED BEFORE ALL REFERENCES

protected:
  const graph_type& m_graph;
  distance_combine_type&   m_distance_combine;
  distance_compare_type&   m_distance_compare;
  distance_inf_type&       m_distance_inf;
  distance_zero_type&      m_distance_zero;
  edge_weight_type&        m_edge_weight;
  graph_visitor_type&      m_graph_visitor;
  max_priority_queue_type& m_max_priority_queue;
  vertex_color_type&       m_vertex_color;
  vertex_distance_type&    m_vertex_distance;
  vertex_index_type&       m_vertex_index;
  vertex_predecessor_type& m_vertex_predecessor;
};
}// namespace blink

#endif //BLINK_GRAPH_DIJKSTRA_STATE_HPP