#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/maptbx/peak_search.h>
#include <boost/python/class.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <scitbx/misc/positive_getitem_index.h>

namespace cctbx { namespace maptbx { namespace boost_python {

namespace {

  struct peak_list_wrappers
  {
    typedef peak_list<> w_t;

    static af::tiny<long, 3>
    grid_indices(w_t const& self, long i)
    {
      using scitbx::positive_getitem_index;
      std::size_t j = positive_getitem_index(i, self.grid_indices().size());
      return self.grid_indices()[j];
    }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("peak_list", no_init)
        .def(init<af::const_ref<float, af::c_grid_padded<3> > const&,
                  af::ref<long, af::c_grid<3> > const&,
                  int, std::size_t, bool>())
        .def(init<af::const_ref<float, af::c_grid_padded<3> > const&,
                  af::ref<long, af::c_grid<3> > const&,
                  int, float, std::size_t, bool>())
        .def(init<af::const_ref<double, af::c_grid_padded<3> > const&,
                  af::ref<long, af::c_grid<3> > const&,
                  int, std::size_t, bool>())
        .def(init<af::const_ref<double, af::c_grid_padded<3> > const&,
                  af::ref<long, af::c_grid<3> > const&,
                  int, double, std::size_t, bool>())
        .def("gridding", &w_t::gridding, ccr())
        .def("size", &w_t::size)
        .def("grid_indices", grid_indices)
        .def("grid_heights", &w_t::grid_heights)
        .def("sites", &w_t::sites)
        .def("heights", &w_t::heights)
      ;
    }
  };

} // namespace <anoymous>

  void wrap_peak_list()
  {
    peak_list_wrappers::wrap();
  }

}}} // namespace cctbx::maptbx::boost_python


// Copyright (c) 2012 Christopher Lux <christopherlux@gmail.com>
// Distributed under the Modified BSD License, see license.txt.

#include <iostream>
#include <sstream>
#include <vector>

#include <boost/assign/list_of.hpp>

#ifdef _WIN32
#include <windows.h>
#endif

#include <scm/core.h>
#include <scm/log.h>
#include <scm/core/pointer_types.h>
#include <scm/core/io/tools.h>
#include <scm/core/time/accum_timer.h>
#include <scm/core/time/high_res_timer.h>

#include <scm/gl_core.h>

#include <scm/gl_util/data/imaging/texture_loader.h>
#include <scm/gl_util/manipulators/trackball_manipulator.h>
#include <scm/gl_util/primitives/box.h>
#include <scm/gl_util/primitives/quad.h>
#include <scm/gl_util/primitives/wavefront_obj.h>

//#include <GL/gl.h>
#include <GL/freeglut.h>
//#include <GL/gl3.h>

static bool fraps_bug = true;

static int winx = 1600;
static int winy = 1024;

const scm::math::vec3f diffuse(0.7f, 0.7f, 0.7f);
const scm::math::vec3f specular(0.2f, 0.7f, 0.9f);
const scm::math::vec3f ambient(0.1f, 0.1f, 0.1f);
const scm::math::vec3f position(1, 1, 1);

#define SCM_XFBEX_MODE 2
#define SCM_XFBEX_GL_MODE 3

struct scm_debug_output : public scm::gl::render_context::debug_output
{
    void operator()(scm::gl::debug_source   src,
                    scm::gl::debug_type     t,
                    scm::gl::debug_severity sev,
                    const std::string&      msg) const
    {
        using namespace scm;
        using namespace scm::gl;
        out() << log::error
              << "gl error: <source: " << debug_source_string(src)
              << ", type: "            << debug_type_string(t)
              << ", severity: "        << debug_severity_string(sev) << "> "
              << msg << log::end;
    }
};

class demo_app
{
public:
    demo_app() {
        _initx = 0;
        _inity = 0;

        _lb_down = false;
        _mb_down = false;
        _rb_down = false;

        _dolly_sens = 10.0f;

        _projection_matrix = scm::math::mat4f::identity();
    }
    virtual ~demo_app();

    bool initialize();
    void display();
    void resize(int w, int h);
    void mousefunc(int button, int state, int x, int y);
    void mousemotion(int x, int y);
    void keyboard(unsigned char key, int x, int y);

private:
    scm::gl::trackball_manipulator _trackball_manip;
    float _initx;
    float _inity;

    bool _lb_down;
    bool _mb_down;
    bool _rb_down;

    float _dolly_sens;

    // stuff... demo stuff
    scm::shared_ptr<scm::gl::render_device>     _device;
    scm::shared_ptr<scm::gl::render_context>    _context;

    scm::gl::program_ptr                        _shader_program;
    scm::gl::program_ptr                        _transformed_shader_program;
    scm::gl::program_ptr                        _pass_through_shader;

    scm::math::mat4f                            _projection_matrix;

    scm::shared_ptr<scm::gl::box_geometry>            _box;
    scm::shared_ptr<scm::gl::wavefront_obj_geometry>  _obj;

    scm::gl::depth_stencil_state_ptr    _dstate_less;
    scm::gl::depth_stencil_state_ptr    _depth_no_z;

    scm::gl::blend_state_ptr            _no_blend;

    scm::gl::rasterizer_state_ptr       _ms_back_cull;

    scm::gl::sampler_state_ptr          _filter_nearest;

    scm::gl::texture_2d_ptr             _color_buffer;
    scm::gl::texture_2d_ptr             _depth_buffer;
    scm::gl::frame_buffer_ptr           _framebuffer;
    scm::gl::texture_2d_ptr             _color_buffer_resolved;
    scm::gl::frame_buffer_ptr           _framebuffer_resolved;

    scm::shared_ptr<scm::gl::quad_geometry>  _quad;

    scm::gl::timer_query_ptr            _timer_view_gen;
    scm::gl::timer_query_ptr            _timer_draw;

    // transform feedback
    scm::gl::transform_feedback_ptr     _transform_feedback;
    scm::gl::buffer_ptr                 _transform_feedback_buffer_0;
    scm::gl::buffer_ptr                 _transform_feedback_buffer_1;
    scm::gl::buffer_ptr                 _transform_feedback_buffer_2;
    scm::gl::vertex_array_ptr           _transform_feedback_vao;
    scm::gl::transform_feedback_statistics_query_ptr _transform_feedback_query;

}; // class demo_app

namespace  {

scm::scoped_ptr<demo_app> _application;

} // namespace

demo_app::~demo_app()
{
    _shader_program.reset();
    _transformed_shader_program.reset();
    _pass_through_shader.reset();

    _box.reset();
    _obj.reset();
    _quad.reset();

    _dstate_less.reset();
    _depth_no_z.reset();
    _no_blend.reset();
    _ms_back_cull.reset();
    _filter_nearest.reset();

    _color_buffer.reset();
    _depth_buffer.reset();
    _framebuffer.reset();
    _color_buffer_resolved.reset();
    _framebuffer_resolved.reset();

    _timer_view_gen.reset();
    _timer_draw.reset();

    _transform_feedback.reset();
    _transform_feedback_buffer_0.reset();
    _transform_feedback_buffer_1.reset();
    _transform_feedback_buffer_2.reset();
    _transform_feedback_vao.reset();
    _transform_feedback_query.reset();

    _context.reset();
    _device.reset();
}

bool
demo_app::initialize()
{
    using namespace scm;
    using namespace scm::gl;
    using namespace scm::math;
    using boost::assign::list_of;

    _device.reset(new scm::gl::render_device());

    _device->main_context()->register_debug_callback(make_shared<scm_debug_output>());

    _context = _device->main_context();

    _shader_program = _device->create_program(
        list_of(_device->create_shader_from_file(STAGE_VERTEX_SHADER,   "../../../src/shaders/phong_lighting.glslv"))
               (_device->create_shader_from_file(STAGE_GEOMETRY_SHADER, "../../../src/shaders/phong_lighting.glslg"))
               (_device->create_shader_from_file(STAGE_FRAGMENT_SHADER, "../../../src/shaders/phong_lighting.glslf")),
#if   SCM_XFBEX_MODE == 1
        stream_capture_array("per_vertex.position")("per_vertex.normal"),
        true,  // rasterizer discard
#elif SCM_XFBEX_MODE == 2
        interleaved_stream_capture("per_vertex.position")("per_vertex.normal"),
        true,  // rasterizer discard
#endif
        //interleaved_stream_capture("per_vertex.vertex.ws_position")("per_vertex.vertex.texcoord_hm")(stream_capture::skip_4_float), // GL4+ only
        //stream_capture_array("per_vertex.vertex.ws_position")
        //stream_capture_array("per_vertex.vertex.ws_position")("per_vertex.vertex.texcoord_hm")("per_vertex.vertex.texcoord_dm"),
        //stream_capture_array(interleaved_stream_capture("per_vertex.vertex.ws_position")("per_vertex.vertex.texcoord_hm"))
        //                    (separate_stream_capture("per_vertex.vertex.texcoord_dm")),
        //stream_capture_array(interleaved_stream_capture("per_vertex.vertex.ws_position")("per_vertex.vertex.texcoord_hm")(stream_capture::skip_4_float))
        //                    (separate_stream_capture("per_vertex.vertex.texcoord_dm")),

        "_shader_program"       );

    _transformed_shader_program = _device->create_program(
        list_of(_device->create_shader_from_file(STAGE_VERTEX_SHADER,   "../../../src/shaders/draw_xfb.glslv"))
               (_device->create_shader_from_file(STAGE_FRAGMENT_SHADER, "../../../src/shaders/draw_xfb.glslf")),
        "_transformed_shader_program");

    if (!_shader_program) {
        scm::err() << "error creating shader program" << log::end;
        return (false);
    }

    _shader_program->uniform("light_ambient", ambient);
    _shader_program->uniform("light_diffuse", diffuse);
    _shader_program->uniform("light_specular", specular);
    _shader_program->uniform("light_position", position);

    _shader_program->uniform("material_ambient", ambient);
    _shader_program->uniform("material_diffuse", diffuse);
    _shader_program->uniform("material_specular", specular);
    _shader_program->uniform("material_shininess", 128.0f);
    _shader_program->uniform("material_opacity", 1.0f);

    scm::out() << *_device << scm::log::end;

    _dstate_less    = _device->create_depth_stencil_state(true, true, COMPARISON_LESS);
    _depth_no_z     = _device->create_depth_stencil_state(false, false);

    _no_blend       = _device->create_blend_state(false, FUNC_ONE, FUNC_ZERO, FUNC_ONE, FUNC_ZERO);

    _ms_back_cull   = _device->create_rasterizer_state(FILL_SOLID, CULL_BACK, ORIENT_CCW, true);

    _filter_nearest = _device->create_sampler_state(FILTER_MIN_MAG_NEAREST, WRAP_CLAMP_TO_EDGE);

    _box.reset(new box_geometry(_device, vec3f(-0.5f), vec3f(0.5f)));
    _obj.reset(new wavefront_obj_geometry(_device, "../../../res/geometry/some_objects.obj"));
    _quad.reset(new quad_geometry(_device, vec2f(0.0f, 0.0f), vec2f(1.0f, 1.0f)));

    // initialize framebuffer /////////////////////////////////////////////////////////////////////
    _color_buffer          = _device->create_texture_2d(vec2ui(winx, winy) * 1, FORMAT_RGBA_8, 1, 1, 1);
    _depth_buffer          = _device->create_texture_2d(vec2ui(winx, winy) * 1, FORMAT_D24, 1, 1, 1);
    _framebuffer           = _device->create_frame_buffer();
    _framebuffer->attach_color_buffer(0, _color_buffer);
    _framebuffer->attach_depth_stencil_buffer(_depth_buffer);

    _color_buffer_resolved = _device->create_texture_2d(vec2ui(winx, winy) * 1, FORMAT_RGBA_8);
    _framebuffer_resolved  = _device->create_frame_buffer();
    _framebuffer_resolved->attach_color_buffer(0, _color_buffer_resolved);

    std::string v_pass = "\
        #version 330\n\
        \
        uniform mat4 mvp;\
        out vec2 tex_coord;\
        layout(location = 0) in vec3 in_position;\
        layout(location = 2) in vec2 in_texture_coord;\
        void main()\
        {\
            gl_Position = mvp * vec4(in_position, 1.0);\
            tex_coord = in_texture_coord;\
        }\
        ";
    std::string f_pass = "\
        #version 330\n\
        \
        in vec2 tex_coord;\
        uniform sampler2D in_texture;\
        layout(location = 0) out vec4 out_color;\
        void main()\
        {\
        out_color = texelFetch(in_texture, ivec2(gl_FragCoord.xy), 0).rgba;\n\
        }\
        ";

    _pass_through_shader = _device->create_program(list_of(_device->create_shader(STAGE_VERTEX_SHADER, v_pass))
                                                          (_device->create_shader(STAGE_FRAGMENT_SHADER, f_pass)));

    _timer_view_gen = _device->create_timer_query();
    _timer_draw     = _device->create_timer_query();

    _trackball_manip.dolly(5.5f);

    // transform feedback
    _transform_feedback_buffer_0    = _device->create_buffer(BIND_TRANSFORM_FEEDBACK_BUFFER, USAGE_STATIC_DRAW, 6 * _obj->vertex_buffer()->descriptor()._size);
    _transform_feedback_buffer_1    = _device->create_buffer(BIND_TRANSFORM_FEEDBACK_BUFFER, USAGE_STATIC_DRAW, 5 * _obj->vertex_buffer()->descriptor()._size);
    _transform_feedback_buffer_2    = _device->create_buffer(BIND_TRANSFORM_FEEDBACK_BUFFER, USAGE_STATIC_DRAW, 5 * _obj->vertex_buffer()->descriptor()._size);
    _transform_feedback_query       = _device->create_transform_feedback_statistics_query();

#if   SCM_XFBEX_MODE == 1
    // separate
    _transform_feedback             = _device->create_transform_feedback(stream_output_setup(_transform_feedback_buffer_0)
                                                                                (_transform_feedback_buffer_1)); // (pos, nrm)
    _transform_feedback_vao         = _device->create_vertex_array(vertex_format(0, 0, TYPE_VEC3F)  // pos
                                                                                (1, 1, TYPE_VEC3F), // nrm
                                                                   list_of(_transform_feedback_buffer_0)
                                                                          (_transform_feedback_buffer_1));
#elif SCM_XFBEX_MODE == 2
     //interleaved
    _transform_feedback             = _device->create_transform_feedback(stream_output_setup(_transform_feedback_buffer_0));
    _transform_feedback_vao         = _device->create_vertex_array(vertex_format(0, 0, TYPE_VEC3F, 2 * sizeof(math::vec3f))  // pos
                                                                                (0, 1, TYPE_VEC3F, 2 * sizeof(math::vec3f)), // nrm
                                                                   list_of(_transform_feedback_buffer_0));
#endif

#if SCM_XFBEX_MODE > 0
    if (   !_transform_feedback_buffer_0
        || !_transform_feedback_buffer_1
        || !_transform_feedback_buffer_2
        || !_transform_feedback
        || !_transform_feedback_vao
        || !_transform_feedback_query) {
        scm::err() << "error creating transform feedback objects" << log::end;
        return false;
    }
#endif

    return true;
}

unsigned plah = 0;
double  time_view_gen = 0.0;
double  time_draw     = 0.0;
int     time_accum_count = 0;

void
demo_app::display()
{
    using namespace scm::gl;
    using namespace scm::math;

    // clear the color and depth buffer
    //glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

    mat4f    view_matrix         = _trackball_manip.transform_matrix();
    mat4f    model_matrix        = mat4f::identity();
    //scale(model_matrix, 0.01f, 0.01f, 0.01f);
    mat4f    model_view_matrix   = view_matrix * model_matrix;
    mat4f    mv_inv_transpose    = transpose(inverse(model_view_matrix));

    _shader_program->uniform("projection_matrix", _projection_matrix);
    _shader_program->uniform("model_view_matrix", model_view_matrix);
    _shader_program->uniform("model_view_matrix_inverse_transpose", mv_inv_transpose);

    _context->clear_default_color_buffer(FRAMEBUFFER_BACK, vec4f(.2f, .2f, .2f, 1.0f));
    _context->clear_default_depth_stencil_buffer();

    _context->reset();

#if SCM_XFBEX_MODE > 0
    { // transform feedback pass
        context_program_guard psg(_context);

        _context->bind_program(_shader_program);

#if SCM_XFBEX_GL_MODE == 3
        _context->begin_query(_transform_feedback_query);
#endif
        _context->begin_transform_feedback(_transform_feedback, PRIMITIVE_TRIANGLES); {
            _obj->draw_raw(_context);
        } _context->end_transform_feedback();
    }
#if SCM_XFBEX_GL_MODE == 3
        _context->end_query(_transform_feedback_query);
#endif
#endif
    _context->begin_query(_timer_view_gen);
    { // multi sample pass
        context_state_objects_guard csg(_context);
        context_texture_units_guard tug(_context);
        context_framebuffer_guard   fbg(_context);

        _context->clear_color_buffer(_framebuffer, 0, vec4f( .2f, .2f, .2f, 1.0f));
        _context->clear_depth_stencil_buffer(_framebuffer);

        _context->set_frame_buffer(_framebuffer);
        //_context->set_viewport(viewport(vec2ui(0, 0), 1 * vec2ui(winx, winy)));
        // build array of two viewports (lower left and upper right)
        vec2f o = vec2f(0.0f, 0.0f);
        vec2f s = vec2f(static_cast<float>(winx), static_cast<float>(winy));
        vec2f m = s * 0.5f;
        // viewport has an () operator which returns a viewport array,
        // so it is easy to concatenate viewports this way viewport(vp_1)(vp_2)(vp_3)...(vp_n)
        // you also can directly use viewport_array like this viewport_array(vp_1)(vp_2)(vp_3)...(vp_n)
        //_context->set_viewports(viewport(o, m)(m, m));
        _context->set_viewport(viewport(o, s));

        _context->set_depth_stencil_state(_dstate_less);
        _context->set_blend_state(_no_blend);
        _context->set_rasterizer_state(_ms_back_cull);

#if SCM_XFBEX_MODE > 0
        context_vertex_input_guard  cvg(_context);

        _context->bind_program(_transformed_shader_program);
        _context->bind_vertex_array(_transform_feedback_vao);
        _context->apply();
#if SCM_XFBEX_GL_MODE == 3
        _context->collect_query_results(_transform_feedback_query);
        _context->draw_arrays(PRIMITIVE_TRIANGLE_LIST, 0, 3 * _transform_feedback_query->result()._primitives_written);
#else
        _context->draw_transform_feedback(PRIMITIVE_TRIANGLE_LIST, _transform_feedback);
#endif
#else
        _context->bind_program(_shader_program);

        //_box->draw(_context, geometry::MODE_SOLID);
        _obj->draw(_context);
#endif
    }
    _context->end_query(_timer_view_gen);

    //_context->resolve_multi_sample_buffer(_framebuffer, _framebuffer_resolved);
    //_context->generate_mipmaps(_color_buffer_resolved);

    _context->reset();

    _context->begin_query(_timer_draw);
    { // fullscreen pass
        mat4f   pass_mvp = mat4f::identity();
        ortho_matrix(pass_mvp, 0.0f, 1.0f, 0.0f, 1.0f, -1.0f, 1.0f);

        _pass_through_shader->uniform_sampler("in_texture", 0);
        _pass_through_shader->uniform("mvp", pass_mvp);

        _context->set_default_frame_buffer();

        _context->set_depth_stencil_state(_depth_no_z);
        _context->set_blend_state(_no_blend);

        _context->bind_program(_pass_through_shader);

        //_context->bind_texture(_color_buffer_resolved, _filter_nearest, 0);
        _context->bind_texture(_color_buffer, _filter_nearest, 0);

        _quad->draw(_context);
    }
    _context->end_query(_timer_draw);

    _context->collect_query_results(_timer_view_gen);
    _context->collect_query_results(_timer_draw);

    time_view_gen    += scm::time::to_milliseconds(scm::time::nanosec(_timer_view_gen->result()));
    time_draw        += scm::time::to_milliseconds(scm::time::nanosec(_timer_draw->result()));
    time_accum_count += 1;

    if ((time_view_gen + time_draw) > 1000.0) {
        boost::io::ios_all_saver ias(std::cout);
        std::cout << "view_gen time:  " << std::fixed << time_view_gen / time_accum_count  << "msec "
                  << "(fps: " << static_cast<double>(time_accum_count * 1000) / time_view_gen << "), "
                  << "view_draw time: " << std::fixed << time_draw / time_accum_count << "msec"
                  << "(fps: " << static_cast<double>(time_accum_count * 1000) / time_draw << "), "
                  << "combined time: " << std::fixed <<  (time_accum_count + time_draw) / time_accum_count  << "msec "
                  << "(fps: " << static_cast<double>(time_accum_count * 1000) / (time_accum_count + time_draw) << ")" << std::endl;
        time_view_gen    = 0.0;
        time_draw        = 0.0;
        time_accum_count = 0;
    }

    // swap the back and front buffer, so that the drawn stuff can be seen
    glutSwapBuffers();

    if (fraps_bug) {
        if (glGetError() != GL_NO_ERROR) {
            std::cout << "fraps bug after swap handled" << std::endl;
        }
        else {
            //std::cout << "nothing after swap" << std::endl;
        }
        fraps_bug = false;
    }
}

void
demo_app::resize(int w, int h)
{
    // safe the new dimensions
    winx = w;
    winy = h;

    // set the new viewport into which now will be rendered
    using namespace scm::gl;
    using namespace scm::math;

    _context->set_viewport(viewport(vec2ui(0, 0), vec2ui(w, h)));

    scm::math::perspective_matrix(_projection_matrix, 60.f, float(w)/float(h), 0.1f, 100.0f);
}

void
demo_app::mousefunc(int button, int state, int x, int y)
{
    switch (button) {
        case GLUT_LEFT_BUTTON:
            {
                _lb_down = (state == GLUT_DOWN) ? true : false;
            }break;
        case GLUT_MIDDLE_BUTTON:
            {
                _mb_down = (state == GLUT_DOWN) ? true : false;
            }break;
        case GLUT_RIGHT_BUTTON:
            {
                _rb_down = (state == GLUT_DOWN) ? true : false;
            }break;
    }

    _initx = 2.f * float(x - (winx/2))/float(winx);
    _inity = 2.f * float(winy - y - (winy/2))/float(winy);
}

void
demo_app::mousemotion(int x, int y)
{
    float nx = 2.f * float(x - (winx/2))/float(winx);
    float ny = 2.f * float(winy - y - (winy/2))/float(winy);

    //std::cout << "nx " << nx << " ny " << ny << std::endl;

    if (_lb_down) {
        _trackball_manip.rotation(_initx, _inity, nx, ny);
    }
    if (_rb_down) {
        _trackball_manip.dolly(_dolly_sens * (ny - _inity));
    }
    if (_mb_down) {
        _trackball_manip.translation(nx - _initx, ny - _inity);
    }

    _inity = ny;
    _initx = nx;
}

void
demo_app::keyboard(unsigned char key, int x, int y)
{
}

void
glut_display()
{
    if (_application)
        _application->display();

}

void
glut_resize(int w, int h)
{
    if (_application)
        _application->resize(w, h);
}

void
glut_mousefunc(int button, int state, int x, int y)
{
    if (_application)
        _application->mousefunc(button, state, x, y);
}

void
glut_mousemotion(int x, int y)
{
    if (_application)
        _application->mousemotion(x, y);
}

void
glut_idle()
{
    glutPostRedisplay();
}

void
glut_keyboard(unsigned char key, int x, int y)
{
    switch (key) {
        // ESC key
        case 27: {
            _application.reset();
            std::cout << "reset application" << std::endl;
            exit (0);
                 }
            break;
        case 'f':
            glutFullScreenToggle();
            break;
        default:
            _application->keyboard(key, x, y);
    }
}

int main(int argc, char **argv)
{
    std::ios_base::sync_with_stdio(false);

    scm::shared_ptr<scm::core>      scm_core(new scm::core(argc, argv));
    _application.reset(new demo_app());

    // the stuff that has to be done
    glutInit(&argc, argv);
    glutInitContextVersion(4, 2);
    glutInitContextProfile(GLUT_CORE_PROFILE);
    glutInitContextFlags(GLUT_DEBUG | GLUT_FORWARD_COMPATIBLE);

    //glutInitContextProfile(GLUT_COMPATIBILITY_PROFILE);

    // init a double buffered framebuffer with depth buffer and 4 channels
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGBA | GLUT_ALPHA | GLUT_MULTISAMPLE);
    // create window with initial dimensions
    glutInitWindowSize(winx, winy);
    glutCreateWindow("simple_glut");

    // init the GL context
    if (!_application->initialize()) {
        std::cout << "error initializing gl context" << std::endl;
        return (-1);
    }

    // set the callbacks for resize, draw and idle actions
    glutReshapeFunc(glut_resize);
    glutDisplayFunc(glut_display);
    glutKeyboardFunc(glut_keyboard);
    glutMouseFunc(glut_mousefunc);
    glutMotionFunc(glut_mousemotion);
    glutIdleFunc(glut_idle);

    // and finally start the event loop
    glutMainLoop();

    return (0);
}


#include <fstream>
#include <filesystem>

#include <Eigen/Geometry>
#include <utility>

#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/common/norms.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>

#include "analysis.h"
#include "filter.h"

#define N_BINS 4

namespace fs = std::filesystem;

std::pair<float, float> calculate_rotation_and_translation_errors(const Eigen::Matrix4f &transformation,
                                                                  const Eigen::Matrix4f &transformation_gt) {
    Eigen::Matrix3f rotation_diff = transformation.block<3, 3>(0, 0).inverse() * transformation_gt.block<3, 3>(0, 0);
    Eigen::Vector3f translation_diff = transformation.block<3, 1>(0, 3) - transformation_gt.block<3, 1>(0, 3);
    float rotation_error = Eigen::AngleAxisf(rotation_diff).angle();
    float translation_error = translation_diff.norm();
    return {rotation_error, translation_error};
}

inline float dist2(const PointN &p1, const PointN &p2) {
    return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
};

float calculate_point_cloud_rmse(const PointNCloud::ConstPtr &pcd,
                                 const Eigen::Matrix4f &transformation,
                                 const Eigen::Matrix4f &transformation_gt) {
    PointNCloud pcd_transformed;
    Eigen::Matrix4f transformation_diff = transformation.inverse() * transformation_gt;
    pcl::transformPointCloud(*pcd, pcd_transformed, transformation_diff);

    float rmse = 0.f;
    for (int i = 0; i < pcd->size(); ++i) {
        rmse += dist2(pcd->points[i], pcd_transformed.points[i]);
    }
    rmse = std::sqrt(rmse / (float) pcd->size());
    return rmse;
}

float calculate_overlap_rmse(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                             const Eigen::Matrix4f &transformation,
                             const Eigen::Matrix4f &transformation_gt,
                             float inlier_threshold) {
    PointNCloud src_aligned, src_aligned_gt;
    pcl::transformPointCloud(*src, src_aligned, transformation);
    pcl::transformPointCloud(*src, src_aligned_gt, transformation_gt);

    pcl::KdTreeFLANN<PointN> tree_tgt;
    tree_tgt.setInputCloud(tgt);

    int overlap_size = 0;
    float rmse = 0.f;
    pcl::Indices nn_indices;
    std::vector<float> nn_dists;
    for (int i = 0; i < src->size(); ++i) {
        tree_tgt.nearestKSearch(src_aligned_gt[i], 1, nn_indices, nn_dists);
        if (nn_dists[0] < inlier_threshold * inlier_threshold) {    // ith point in overlap
            rmse += dist2(src_aligned.points[i], src_aligned_gt.points[i]);
            overlap_size++;
        }
    }
    if (overlap_size != 0) {
        rmse = std::sqrt(rmse / (float)overlap_size);
    } else {
        rmse = std::numeric_limits<float>::quiet_NaN();
    }
    return rmse;

}

float calculate_correspondence_uniformity(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                          const pcl::Correspondences &correct_correspondences,
                                          const AlignmentParameters &parameters,
                                          const Eigen::Matrix4f &transformation_gt) {
    PointNCloud::Ptr src_aligned(new PointNCloud);
    pcl::transformPointCloud(*src, *src_aligned, transformation_gt);
    pcl::KdTreeFLANN<PointN>::Ptr tree(new pcl::KdTreeFLANN<PointN>());
    tree->setInputCloud(tgt);

    pcl::Indices indices;
    std::vector<float> distances;
    // calculate bbox of overlapping area
    PointN min_point, max_point;
    float error_thr = parameters.distance_thr_coef * parameters.voxel_size;
    for (int i = 0; i < src_aligned->size(); ++i) {
        (float) tree->nearestKSearch(*src_aligned, i, 1, indices, distances);
        const PointN &p_src(src_aligned->points[i]), p_tgt(tgt->points[indices[0]]);
        if (std::sqrt(distances[0]) < error_thr && std::isfinite(p_src.normal_x) && std::isfinite(p_tgt.normal_x)) {
            min_point.x = std::min(min_point.x, p_src.x);
            min_point.y = std::min(min_point.y, p_src.y);
            min_point.z = std::min(min_point.z, p_src.z);
            max_point.x = std::max(max_point.x, p_src.x);
            max_point.y = std::max(max_point.y, p_src.y);
            max_point.z = std::max(max_point.z, p_src.z);
        }
    }
    int count[3][N_BINS][N_BINS]{0};

    for (auto const &corr: correct_correspondences) {
        const auto &point = src_aligned->points[corr.index_query];
        if (pointInBoundingBox(point, min_point, max_point)) {
            int bin[3];
            bin[0] = std::floor((point.x - min_point.x) / (max_point.x - min_point.x) * N_BINS);
            bin[1] = std::floor((point.y - min_point.y) / (max_point.y - min_point.y) * N_BINS);
            bin[2] = std::floor((point.z - min_point.z) / (max_point.z - min_point.z) * N_BINS);
            // count 3D points projected to YZ, ZX, XY and fallen in 2D bin
            for (int k = 0; k < 3; ++k) {
                count[k][bin[(k + 1) % 3]][bin[(k + 2) % 3]]++;
            }
        }
    }
    float entropy[3]{0.f};
    float n = correct_correspondences.size();
    for (int k = 0; k < 3; ++k) {
        for (int i = 0; i < N_BINS; ++i) {
            for (int j = 0; j < N_BINS; ++j) {
                float p = (float) count[k][i][j] / n;
                if (p == 0.f) continue;
                entropy[k] -= p * std::log(p);
            }
        }
        entropy[k] /= std::log((float) (N_BINS * N_BINS));
    }
    return std::cbrt(entropy[0] * entropy[1] * entropy[2]);
}

float calculate_normal_difference(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                  const AlignmentParameters &parameters, const Eigen::Matrix4f &transformation_gt) {
    PointNCloud::Ptr src_aligned(new PointNCloud);
    pcl::transformPointCloud(*src, *src_aligned, transformation_gt);
    pcl::KdTreeFLANN<PointN>::Ptr tree(new pcl::KdTreeFLANN<PointN>());
    tree->setInputCloud(tgt);

    pcl::Indices indices;
    std::vector<float> distances;
    float error_thr = parameters.distance_thr_coef * parameters.voxel_size;
    float difference = 0.f;
    int n_points_overlap = 0;
    for (int i = 0; i < src_aligned->size(); ++i) {
        (float) tree->nearestKSearch(*src_aligned, i, 1, indices, distances);
        const PointN &p_src(src_aligned->points[i]), p_tgt(tgt->points[indices[0]]);
        if (std::sqrt(distances[0]) < error_thr && std::isfinite(p_src.normal_x) && std::isfinite(p_tgt.normal_x)) {
            float cos = std::clamp(p_src.normal_x * p_tgt.normal_x + p_src.normal_y * p_tgt.normal_y +
                                   p_src.normal_z * p_tgt.normal_z, -1.f, 1.f);
            difference += std::abs(std::acos(cos));
            n_points_overlap++;
        }
    }
    return difference / (float) n_points_overlap;
}

void buildCorrectCorrespondences(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                 const pcl::Correspondences &correspondences,
                                 pcl::Correspondences &correct_correspondences,
                                 const Eigen::Matrix4f &transformation_gt, float error_threshold) {
    correct_correspondences.clear();
    correct_correspondences.reserve(correspondences.size());

    PointNCloud input_transformed;
    input_transformed.resize(src->size());
    pcl::transformPointCloud(*src, input_transformed, transformation_gt);

    for (const auto &correspondence: correspondences) {
        PointN source_point(input_transformed.points[correspondence.index_query]);
        PointN target_point(tgt->points[correspondence.index_match]);
        float e = pcl::L2_Norm(source_point.data, target_point.data, 3);
        if (e < error_threshold) {
            correct_correspondences.push_back(correspondence);
        }
    }
}

AlignmentAnalysis::AlignmentAnalysis(const AlignmentParameters &parameters,
                                     const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                     const pcl::Correspondences &correspondences,
                                     int iterations, const Eigen::Matrix4f &transformation, double time) {
    parameters_ = parameters;
    src_ = src;
    tgt_ = tgt;
    correspondences_ = correspondences;
    iterations_ = iterations;
    transformation_ = transformation;
    time_ = time;
    has_converged_ = true;
    metric_estimator_ = getMetricEstimator(parameters);
    metric_estimator_->setSourceCloud(src);
    metric_estimator_->setTargetCloud(tgt);
    metric_estimator_->setCorrespondences(correspondences);
    metric_estimator_->setInlierThreshold(parameters.distance_thr_coef * parameters.voxel_size);
}

void AlignmentAnalysis::start(const Eigen::Matrix4f &transformation_gt, const std::string &testname) {
    testname_ = testname;
    float error_thr = parameters_.distance_thr_coef * parameters_.voxel_size;
    transformation_gt_ = transformation_gt;

    buildCorrectCorrespondences(src_, tgt_, correspondences_, correct_correspondences_, transformation_gt_, error_thr);
    metric_estimator_->buildInlierPairsAndEstimateMetric(transformation_, inlier_pairs_, rmse_, fitness_);
    metric_estimator_->buildCorrectInlierPairs(inlier_pairs_, correct_inlier_pairs_, transformation_gt_);
    pcd_error_ = calculate_point_cloud_rmse(src_, transformation_, transformation_gt_);
    overlap_error_ = calculate_overlap_rmse(src_, tgt_, transformation_, transformation_gt_, error_thr);
    normal_diff_ = calculate_normal_difference(src_, tgt_, parameters_, transformation_gt_);
    corr_uniformity_ = calculate_correspondence_uniformity(src_, tgt_, correct_correspondences_,
                                                           parameters_, transformation_gt_);
    std::tie(r_error_, t_error_) = calculate_rotation_and_translation_errors(transformation_, transformation_gt_);

    print();
    save(testname);
}

void AlignmentAnalysis::print() {
    // Print results
    printf("\n");
    printTransformation(transformation_);
    printTransformation(transformation_gt_);
    pcl::console::print_info("fitness: %0.7f\n", fitness_);
    pcl::console::print_info("inliers_rmse: %0.7f\n", rmse_);
    pcl::console::print_info("correct inliers: %i/%i\n", correct_inlier_pairs_.size(), inlier_pairs_.size());
    pcl::console::print_info("correct correspondences: %i/%i\n",
                             correct_correspondences_.size(), correspondences_.size());
    pcl::console::print_info("rotation error: %0.7f\n", r_error_);
    pcl::console::print_info("translation error: %0.7f\n", t_error_);
    pcl::console::print_info("point cloud mean error: %0.7f\n", pcd_error_);
    pcl::console::print_info("normal mean difference: %0.7f\n", normal_diff_);
    pcl::console::print_info("uniformity of correct correspondences' distribution: %0.7f\n", corr_uniformity_);
}

void AlignmentAnalysis::save(const std::string &testname) {
    // Save test parameters and results
    std::string filepath = constructPath("test", "results", "csv", false);
    bool file_exists = std::filesystem::exists(filepath);
    std::fstream fout;
    if (!file_exists) {
        fout.open(filepath, std::ios_base::out);
    } else {
        fout.open(filepath, std::ios_base::app);
    }
    if (fout.is_open()) {
        if (!file_exists) {
            printAnalysisHeader(fout);
        }
        fout << *this;
        fout.close();
    } else {
        perror(("error while opening file " + filepath).c_str());
    }
}

void printAnalysisHeader(std::ostream &out) {
    out << "version,descriptor,testname,fitness,rmse,correspondences,correct_correspondences,inliers,correct_inliers,";
    out << "voxel_size,normal_radius_coef,feature_radius_coef,distance_thr_coef,edge_thr,";
    out << "iteration,matching,randomness,filter,threshold,n_random,r_err,t_err,pcd_err,use_normals,";
    out << "normal_diff,corr_uniformity,lrf,metric,time,overlap_rmse\n";
}

std::ostream &operator<<(std::ostream &stream, const AlignmentAnalysis &analysis) {
    stream << VERSION << "," << analysis.parameters_.descriptor_id << "," << analysis.testname_ << ","
           << analysis.fitness_ << "," << analysis.rmse_ << ",";
    stream << analysis.correspondences_.size() << "," << analysis.correct_correspondences_.size() << ",";
    stream << analysis.inlier_pairs_.size() << "," << analysis.correct_inlier_pairs_.size() << ",";
    stream << analysis.parameters_.voxel_size << ",";
    stream << analysis.parameters_.normal_radius_coef << ",";
    stream << analysis.parameters_.feature_radius_coef << ",";
    stream << analysis.parameters_.distance_thr_coef << ",";
    stream << analysis.parameters_.edge_thr_coef << ",";
    stream << analysis.iterations_ << ",";
    stream << analysis.parameters_.matching_id << ",";
    stream << analysis.parameters_.randomness << ",";
    auto func = getUniquenessFunction(analysis.parameters_.func_id);
    if (func != nullptr) {
        stream << analysis.parameters_.func_id << "," << UNIQUENESS_THRESHOLD << "," << N_RANDOM_FEATURES << ",";
    } else {
        stream << ",,,";
    }
    stream << analysis.r_error_ << "," << analysis.t_error_ << "," << analysis.pcd_error_ << ",";
    stream << analysis.parameters_.use_normals << "," << analysis.normal_diff_ << ",";
    stream << analysis.corr_uniformity_ << "," << analysis.parameters_.lrf_id << ",";
    stream << analysis.parameters_.metric_id << "," << analysis.time_ << "," << analysis.overlap_error_ << "\n";
    return stream;
}


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Math/Vector.h>
#include <vw/FileIO/DiskImageResourceGDAL.h>
#include <vw/FileIO/FileUtils.h>
#include <vw/Cartography/Datum.h>
#include <vw/Cartography/GeoReference.h>
#include <asp/Camera/RPCModel.h>
#include <asp/Core/Common.h>

#include <gdal.h>
#include <gdal_priv.h>

#include <boost/smart_ptr/scoped_ptr.hpp>
#include <boost/smart_ptr/shared_ptr.hpp>

using namespace vw;

namespace asp {

  void RPCModel::initialize(DiskImageResourceGDAL* resource) {
    // Extract the datum (by means of georeference)
    cartography::GeoReference georef;
    cartography::read_georeference(georef, *resource);
    m_datum = georef.datum();

    // Extract RPC Info
    boost::shared_ptr<GDALDataset> dataset = resource->get_dataset_ptr();
    if (!dataset)
      vw_throw(NotFoundErr() << "RPCModel: Could not read data. No file has been opened.");

    GDALRPCInfo gdal_rpc;
    if (!GDALExtractRPCInfo(dataset->GetMetadata("RPC"),
                            &gdal_rpc))
      vw_throw(NotFoundErr() << "RPCModel: GDAL resource appears not to have RPC metadata.");

    // Copy information over to our data structures.
    m_lonlatheight_offset = Vector3(gdal_rpc.dfLONG_OFF,
                                    gdal_rpc.dfLAT_OFF,
                                    gdal_rpc.dfHEIGHT_OFF);
    m_lonlatheight_scale = Vector3(gdal_rpc.dfLONG_SCALE,
                                   gdal_rpc.dfLAT_SCALE,
                                   gdal_rpc.dfHEIGHT_SCALE);
    m_xy_offset = Vector2(gdal_rpc.dfSAMP_OFF,   gdal_rpc.dfLINE_OFF);
    m_xy_scale  = Vector2(gdal_rpc.dfSAMP_SCALE, gdal_rpc.dfLINE_SCALE);

    m_line_num_coeff   = CoeffVec(gdal_rpc.adfLINE_NUM_COEFF);
    m_line_den_coeff   = CoeffVec(gdal_rpc.adfLINE_DEN_COEFF);
    m_sample_num_coeff = CoeffVec(gdal_rpc.adfSAMP_NUM_COEFF);
    m_sample_den_coeff = CoeffVec(gdal_rpc.adfSAMP_DEN_COEFF);
  }

  RPCModel::RPCModel(std::string const& filename) {
    std::string ext = get_extension(filename);
    if (ext == ".rpb") {
      load_rpb_file(filename);
      return;
    }

    // Must have this check, otherwise GDAL prints an error.
    if (has_image_extension(filename)) {
      boost::scoped_ptr<DiskImageResourceGDAL> s_ptr(new DiskImageResourceGDAL(filename));
      initialize(s_ptr.get());
    }else{
      // Throw an error. It will be caught, but it will get printed
      // only if no other approaches turn out to work later on.
      vw_throw(ArgumentErr() << "Not an image " << filename);
    }

  }

  RPCModel::RPCModel(DiskImageResourceGDAL* resource ) {
    initialize(resource);
  }

  // The constructor just copies all of the input data
  RPCModel::RPCModel(cartography::Datum const& datum,
                     Vector<double,20> const& line_num_coeff,
                     Vector<double,20> const& line_den_coeff,
                     Vector<double,20> const& samp_num_coeff,
                     Vector<double,20> const& samp_den_coeff,
                     Vector2           const& xy_offset,
                     Vector2           const& xy_scale,
                     Vector3           const& lonlatheight_offset,
                     Vector3           const& lonlatheight_scale) :
    m_datum(datum),
    m_line_num_coeff(line_num_coeff),
    m_line_den_coeff(line_den_coeff),
    m_sample_num_coeff(samp_num_coeff),
    m_sample_den_coeff(samp_den_coeff),
    m_xy_offset(xy_offset),
    m_xy_scale(xy_scale),
    m_lonlatheight_offset(lonlatheight_offset),
    m_lonlatheight_scale(lonlatheight_scale) {}

  void RPCModel::load_rpb_file(std::string const& filename) {
    //vw_out() << "Reading RPC model from RPB file, defaulting to WGS84 datum.\n";
    m_datum.set_well_known_datum("WGS84");
    std::ifstream f(filename.c_str());

    std::string line;
    std::vector<std::string> tokens;
    bool lineNumCoeffs = false,
      lineDenCoeffs = false,
      sampNumCoeffs = false,
      sampDenCoeffs = false;
    int coeff_index = 0, max_coeff_index = 0;

    // Read through each line in the file
    while (std::getline(f, line)) {

      try {
        // Break up the line
        boost::split(tokens, line, boost::is_any_of("=,;"));

        // Parse keywords
        if (line.find("lineOffset") != std::string::npos)
          m_xy_offset[1] = atof(tokens[1].c_str());
        if (line.find("sampOffset") != std::string::npos)
          m_xy_offset[0] = atof(tokens[1].c_str());
        if (line.find("latOffset") != std::string::npos)
          m_lonlatheight_offset[1] = atof(tokens[1].c_str());
        if (line.find("longOffset") != std::string::npos)
          m_lonlatheight_offset[0] = atof(tokens[1].c_str());
        if (line.find("heightOffset") != std::string::npos)
          m_lonlatheight_offset[2] = atof(tokens[1].c_str());
        if (line.find("lineScale") != std::string::npos)
          m_xy_scale[1] = atof(tokens[1].c_str());
        if (line.find("sampScale") != std::string::npos)
          m_xy_scale[0] = atof(tokens[1].c_str());
        if (line.find("latScale") != std::string::npos)
          m_lonlatheight_scale[1] = atof(tokens[1].c_str());
        if (line.find("longScale") != std::string::npos)
          m_lonlatheight_scale[0] = atof(tokens[1].c_str());
        if (line.find("heightScale") != std::string::npos)
          m_lonlatheight_scale[2] = atof(tokens[1].c_str());

        // Handle the RPC coefficients.
        if (lineNumCoeffs) {
          m_line_num_coeff[coeff_index] = atof(tokens[0].c_str());
          ++coeff_index;
        }
        if (lineDenCoeffs) {
          m_line_den_coeff[coeff_index] = atof(tokens[0].c_str());
          ++coeff_index;
        }
        if (sampNumCoeffs) {
          m_sample_num_coeff[coeff_index] = atof(tokens[0].c_str());
          ++coeff_index;
        }
        if (sampDenCoeffs) {
          m_sample_den_coeff[coeff_index] = atof(tokens[0].c_str());
          ++coeff_index;
        }

        // Start of a coefficient sequence
        if (line.find("lineNumCoef") != std::string::npos)
          lineNumCoeffs = true;
        if (line.find("lineDenCoef") != std::string::npos)
          lineDenCoeffs = true;
        if (line.find("sampNumCoef") != std::string::npos)
          sampNumCoeffs = true;
        if (line.find("sampDenCoef") != std::string::npos)
          sampDenCoeffs = true;

        // Done reading a coefficient sequence.
        if (line.find(")") != std::string::npos) {
          lineNumCoeffs = false;
          lineDenCoeffs = false;
          sampNumCoeffs = false;
          sampDenCoeffs = false;
          if (coeff_index > max_coeff_index)
            max_coeff_index = coeff_index;
          coeff_index = 0;
        }
      } catch(...) {
        vw_throw(ArgumentErr() << "Error reading file " << filename
                 << ", line = "  << line);
      }
    } // End loop through lines.
    f.close();
    // Basic error check
    if (max_coeff_index != 20)
      vw_throw(ArgumentErr() << "Error reading file " << filename
               << ", loaded wrong number of coefficients!");
  }

  // All of these implementations are largely inspired by the GDAL
  // code. We don't use the GDAL code unfortunately because they don't
  // make that part of the API available. However I believe this is a
  // safe reinterpretation that is safe to distribute.
  Vector2 RPCModel::point_to_pixel(Vector3 const& point) const {
    return geodetic_to_pixel(m_datum.cartesian_to_geodetic(point));
  }

  Vector2 RPCModel::geodetic_to_pixel(Vector3 const& geodetic) const {

    // Should we verify that the  input geodetic is in the box?

    Vector3 normalized_geodetic =
      elem_quot(geodetic - m_lonlatheight_offset, m_lonlatheight_scale);

    Vector2 normalized_pixel = normalized_geodetic_to_normalized_pixel(normalized_geodetic);

    return elem_prod(normalized_pixel, m_xy_scale) + m_xy_offset;
  }

  Vector2 RPCModel::normalized_geodetic_to_normalized_pixel
  (Vector3 const& normalized_geodetic,
   RPCModel::CoeffVec const& line_num_coeff,
   RPCModel::CoeffVec const& line_den_coeff,
   RPCModel::CoeffVec const& sample_num_coeff,
   RPCModel::CoeffVec const& sample_den_coeff){

    CoeffVec term = calculate_terms(normalized_geodetic);
    Vector2 normalized_pixel(dot_prod(term,sample_num_coeff) /
                             dot_prod(term,sample_den_coeff),
                             dot_prod(term,line_num_coeff) /
                             dot_prod(term,line_den_coeff));

    return normalized_pixel;
  }

  Vector2 RPCModel::normalized_geodetic_to_normalized_pixel
  (Vector3 const& normalized_geodetic) const {

    return normalized_geodetic_to_normalized_pixel(normalized_geodetic,
                                                   m_line_num_coeff,
                                                   m_line_den_coeff,
                                                   m_sample_num_coeff,
                                                   m_sample_den_coeff
                                                   );
  }

  RPCModel::CoeffVec RPCModel::calculate_terms(vw::Vector3 const& normalized_geodetic) {

    double x = normalized_geodetic.x(); // normalized lon
    double y = normalized_geodetic.y(); // normalized lat
    double z = normalized_geodetic.z(); // normalized height
    CoeffVec result;
    result[ 0] = 1.0;
    result[ 1] = x;
    result[ 2] = y;
    result[ 3] = z;
    result[ 4] = x*y;
    result[ 5] = x*z;
    result[ 6] = y*z;
    result[ 7] = x*x;
    result[ 8] = y*y;
    result[ 9] = z*z;
    result[10] = x*y*z;
    result[11] = x*x*x;
    result[12] = x*y*y;
    result[13] = x*z*z;
    result[14] = x*x*y;
    result[15] = y*y*y;
    result[16] = y*z*z;
    result[17] = x*x*z;
    result[18] = y*y*z;
    result[19] = z*z*z;
    return result;
  }

  vw::Vector<int,20> RPCModel::get_coeff_order() {
    vw::Vector<int,20> result;
    for (int i= 0; i< 3; ++i) result[i] = 1;
    for (int i= 3; i<10; ++i) result[i] = 2;
    for (int i=10; i<20; ++i) result[i] = 3;
    return result;
  }

  vw::Matrix<double, 20, 2> RPCModel::terms_Jacobian2(vw::Vector3 const& normalized_geodetic) {
    // Partial derivatives of the terms returned by the
    // calculate_terms() function in respect to the first two
    // variables only (unlike the terms_Jacobian3() function).

    vw::Matrix<double, 20, 2> M;
    double x = normalized_geodetic.x(); // normalized lon
    double y = normalized_geodetic.y(); // normalized lat
    double z = normalized_geodetic.z(); // normalized height

    // df/dx            df/dy               // f
    M[ 0][0] = 0.0;     M[ 0][1] = 0.0;     // 1
    M[ 1][0] = 1.0;     M[ 1][1] = 0.0;     // x
    M[ 2][0] = 0.0;     M[ 2][1] = 1.0;     // y
    M[ 3][0] = 0.0;     M[ 3][1] = 0.0;     // z
    M[ 4][0] = y;       M[ 4][1] = x;       // xy
    M[ 5][0] = z;       M[ 5][1] = 0.0;     // xz
    M[ 6][0] = 0.0;     M[ 6][1] = z;       // yz
    M[ 7][0] = 2.0*x;   M[ 7][1] = 0.0;     // xx
    M[ 8][0] = 0.0;     M[ 8][1] = 2.0*y;   // yy
    M[ 9][0] = 0.0;     M[ 9][1] = 0.0;     // zz
    M[10][0] = y*z;     M[10][1] = x*z;     // xyz
    M[11][0] = 3.0*x*x; M[11][1] = 0.0;     // xxx
    M[12][0] = y*y;     M[12][1] = 2.0*x*y; // xyy
    M[13][0] = z*z;     M[13][1] = 0.0;     // xzz
    M[14][0] = 2.0*x*y; M[14][1] = x*x;     // xxy
    M[15][0] = 0.0;     M[15][1] = 3.0*y*y; // yyy
    M[16][0] = 0.0;     M[16][1] = z*z;     // yzz
    M[17][0] = 2.0*x*z; M[17][1] = 0.0;     // xxz
    M[18][0] = 0.0;     M[18][1] = 2.0*y*z; // yyz
    M[19][0] = 0.0;     M[19][1] = 0.0;     // zzz

    return M;
  }

  vw::Matrix<double, 20, 3> RPCModel::terms_Jacobian3(vw::Vector3 const& normalized_geodetic) {
    // Partial derivatives of the terms returned by the
    // calculate_terms() function in respect to all three
    // variables only (unlike the terms_Jacobian2() function).

    vw::Matrix<double, 20, 3> M;
    double x = normalized_geodetic.x(); // normalized lon
    double y = normalized_geodetic.y(); // normalized lat
    double z = normalized_geodetic.z(); // normalized height

    // df/dx            df/dy               df/dz               // f
    M[ 0][0] = 0.0;     M[ 0][1] = 0.0;     M[ 0][2] = 0.0;     // 1
    M[ 1][0] = 1.0;     M[ 1][1] = 0.0;     M[ 1][2] = 0.0;     // x
    M[ 2][0] = 0.0;     M[ 2][1] = 1.0;     M[ 2][2] = 0.0;     // y
    M[ 3][0] = 0.0;     M[ 3][1] = 0.0;     M[ 3][2] = 1.0;     // z
    M[ 4][0] = y;       M[ 4][1] = x;       M[ 4][2] = 0.0;     // xy
    M[ 5][0] = z;       M[ 5][1] = 0.0;     M[ 5][2] = x;       // xz
    M[ 6][0] = 0.0;     M[ 6][1] = z;       M[ 6][2] = y;       // yz
    M[ 7][0] = 2.0*x;   M[ 7][1] = 0.0;     M[ 7][2] = 0.0;     // xx
    M[ 8][0] = 0.0;     M[ 8][1] = 2.0*y;   M[ 8][2] = 0.0;     // yy
    M[ 9][0] = 0.0;     M[ 9][1] = 0.0;     M[ 9][2] = 2.0*z;   // zz
    M[10][0] = y*z;     M[10][1] = x*z;     M[10][2] = x*y;     // xyz
    M[11][0] = 3.0*x*x; M[11][1] = 0.0;     M[11][2] = 0.0;     // xxx
    M[12][0] = y*y;     M[12][1] = 2.0*x*y; M[12][2] = 0.0;     // xyy
    M[13][0] = z*z;     M[13][1] = 0.0;     M[13][2] = 2.0*x*z; // xzz
    M[14][0] = 2.0*x*y; M[14][1] = x*x;     M[14][2] = 0.0;     // xxy
    M[15][0] = 0.0;     M[15][1] = 3.0*y*y; M[15][2] = 0.0;     // yyy
    M[16][0] = 0.0;     M[16][1] = z*z;     M[16][2] = 2.0*y*z; // yzz
    M[17][0] = 2.0*x*z; M[17][1] = 0.0;     M[17][2] = x*x;     // xxz
    M[18][0] = 0.0;     M[18][1] = 2.0*y*z; M[18][2] = y*y;     // yyz
    M[19][0] = 0.0;     M[19][1] = 0.0;     M[19][2] = 3.0*z*z; // zzz

    return M;
  }

  RPCModel::CoeffVec
  RPCModel::quotient_Jacobian(RPCModel::CoeffVec const& c,
                              RPCModel::CoeffVec const& d,
                              RPCModel::CoeffVec const& u) {

    // Return the Jacobian of dot_prod(c, u) / dot_prod(d, u)
    // as a vector with 20 elements.

    double cu  = dot_prod(c, u);
    double du  = dot_prod(d, u);
    double den = du*du;

    return elem_quot(du * c - cu * d, den);
  }

  vw::Matrix3x3 RPCModel::normalization_Jacobian(Vector3 const& q) {

    // Return the Jacobian of the function
    // f(x1, x2, x3) = ((x1 - c1)/q1, (x2 - c2)/q2, (x3 - c3)/q3)

    vw::Matrix3x3 M;
    M[0][0] = 1.0/q[0]; M[0][1] = 0.0;      M[0][2] = 0.0;
    M[1][0] = 0.0;      M[1][1] = 1.0/q[1]; M[1][2] = 0.0;
    M[2][0] = 0.0;      M[2][1] = 0.0;      M[2][2] = 1.0/q[2];
    return M;
  }

  Matrix<double, 2, 3> RPCModel::geodetic_to_pixel_Jacobian(Vector3 const& geodetic) const {

    Vector3 normalized_geodetic = elem_quot(geodetic - m_lonlatheight_offset, m_lonlatheight_scale);

    CoeffVec term = calculate_terms(normalized_geodetic);

    CoeffVec Qs = quotient_Jacobian(sample_num_coeff(), sample_den_coeff(), term);
    CoeffVec Ql = quotient_Jacobian(line_num_coeff(),   line_den_coeff(),   term);
    Matrix<double, 20, 3> MN = terms_Jacobian3(normalized_geodetic) *
      normalization_Jacobian(m_lonlatheight_scale);

    Matrix<double, 2, 3> J;
    select_row(J, 0) = m_xy_scale[0] * transpose(Qs) * MN;
    select_row(J, 1) = m_xy_scale[1] * transpose(Ql) * MN;

    return J;
  }

  Matrix<double, 2, 2> RPCModel::normalized_geodetic_to_pixel_Jacobian(Vector3 const& normalized_geodetic) const {

    // This function is different from geodetic_to_pixel_Jacobian() in several respects:

    // 1. The input is the normalized geodetic, and the derivatives
    //    are in respect to the normalized geodetic as well.

    // 2. The derivatives are taken only in respect to the first two
    //    variables (normalized lon and lat, no height).

    // 3. The output is in normalized pixels (see m_xy_scale and m_xy_offset).

    CoeffVec term = calculate_terms(normalized_geodetic);

    CoeffVec Qs = quotient_Jacobian(sample_num_coeff(), sample_den_coeff(), term);
    CoeffVec Ql = quotient_Jacobian(line_num_coeff(),   line_den_coeff(),   term);

    Matrix<double, 20, 2> Jt = terms_Jacobian2(normalized_geodetic);

    Matrix<double, 2, 2> J;
    select_row(J, 0) = transpose(Qs) * Jt;
    select_row(J, 1) = transpose(Ql) * Jt;

    return J;
  }

  Matrix<double, 2, 3> RPCModel::geodetic_to_pixel_numerical_Jacobian(Vector3 const& geodetic, double tol) const {

    // Find the Jacobian of geodetic_to_pixel using numerical
    // differentiation. This is used for testing purposes.

    Matrix<double, 2, 3> J;

    Vector2 B  = geodetic_to_pixel(geodetic);

    Vector2 B0 = (geodetic_to_pixel(geodetic + Vector3(tol, 0,   0 )) - B)/tol;
    Vector2 B1 = (geodetic_to_pixel(geodetic + Vector3(0,   tol, 0 )) - B)/tol;
    Vector2 B2 = (geodetic_to_pixel(geodetic + Vector3(0,   0,   tol)) - B)/tol;

    select_col(J, 0) = B0;
    select_col(J, 1) = B1;
    select_col(J, 2) = B2;

    return J;
  }

  Vector2 RPCModel::image_to_ground(Vector2 const& pixel, double height, Vector2 lonlat_guess) const {

    // The absolute tolerance is experimental, needs more investigation
    double abs_tolerance = 1e-6;

    Vector2 normalized_pixel = elem_quot(pixel - m_xy_offset, m_xy_scale);

    // Initial guess for the normalized lon and lat
    if (lonlat_guess == Vector2(0.0, 0.0)){
      lonlat_guess = subvector(m_lonlatheight_offset, 0, 2);
    }
    Vector2 normalized_lonlat = elem_quot(lonlat_guess - subvector(m_lonlatheight_offset, 0, 2),
                                          subvector(m_lonlatheight_scale, 0, 2)
                                          );
    double len = norm_2(normalized_lonlat);
    if (len != len || len > 1.5){
      // If the input guess is NaN or unreasonable, use 0 as initial guess
      normalized_lonlat = Vector2(0.0, 0.0);
    }

    // 10 iterations should be enough for Newton's method to converge
    for (int iter = 0; iter < 10; iter++){

      Vector3 normalized_geodetic;
      normalized_geodetic[0] = normalized_lonlat[0];
      normalized_geodetic[1] = normalized_lonlat[1];
      normalized_geodetic[2] = (height - m_lonlatheight_offset[2])/m_lonlatheight_scale[2];

      Vector2              p = normalized_geodetic_to_normalized_pixel(normalized_geodetic);
      Matrix<double, 2, 2> J = normalized_geodetic_to_pixel_Jacobian(normalized_geodetic);

      // The inverse matrix computed analytically
      double det = J[0][0]*J[1][1] - J[0][1]*J[1][0];
      Matrix<double, 2, 2> invJ;
      invJ[0][0] =  J[1][1];
      invJ[0][1] = -J[0][1];
      invJ[1][0] = -J[1][0];
      invJ[1][1] =  J[0][0];
      invJ /= det;

      // Newton's method for F(x) = y is
      // x = x - J^{-1}(F(x) - y)
      Vector2 error_try = p - normalized_pixel;
      normalized_lonlat -= invJ*error_try;

      // Absolute error convergence criterion
      double  norm_try = norm_2(error_try);
      if (norm_try < abs_tolerance) {
        break;
      }

    }

    Vector2 lonlat = elem_prod(normalized_lonlat, subvector(m_lonlatheight_scale, 0, 2))
      + subvector(m_lonlatheight_offset, 0, 2);

    return lonlat;

  }

  void RPCModel::point_and_dir(Vector2 const& pix, Vector3 & P, Vector3 & dir) const {

    // For an RPC model there is no defined origin so it and the ray need to be computed.

    // Center of valid region to bottom of valid region (normalized)
    const double VERT_SCALE_FACTOR = 0.9; // - The virtual center should be above the terrain
    double  height_up = m_lonlatheight_offset[2] + m_lonlatheight_scale[2]*VERT_SCALE_FACTOR;
    double  height_dn = m_lonlatheight_offset[2] - m_lonlatheight_scale[2]*VERT_SCALE_FACTOR;

    //vw_out() << "m_lonlatheight_offset = " << m_lonlatheight_offset << std::endl;
    //vw_out() << "m_lonlatheight_scale = " << m_lonlatheight_scale << std::endl;

    //vw_out() << "Height up = " << height_up << std::endl;
    //vw_out() << "Height dn = " << height_dn << std::endl;

    // Given the pixel and elevation, estimate lon-lat.
    // Use m_lonlatheight_offset as initial guess for lonlat_up,
    // and then use lonlat_up as initial guess for lonlat_dn.
    Vector2 lonlat_up = image_to_ground(pix, height_up, subvector(m_lonlatheight_offset, 0, 2));
    Vector2 lonlat_dn = image_to_ground(pix, height_dn, lonlat_up);

    //vw_out() << "lonlat_up = " << lonlat_up << std::endl;
    //vw_out() << "lonlat_dn = " << lonlat_dn << std::endl;

    Vector3 geo_up = Vector3(lonlat_up[0], lonlat_up[1], height_up);
    Vector3 geo_dn = Vector3(lonlat_dn[0], lonlat_dn[1], height_dn);

    //vw_out() << "geo_up = " << geo_up << std::endl;
    //vw_out() << "geo_dn = " << geo_dn << std::endl;

    Vector3 P_up = m_datum.geodetic_to_cartesian(geo_up);
    Vector3 P_dn = m_datum.geodetic_to_cartesian(geo_dn);

    dir = normalize(P_dn - P_up);

    // Set the origin location very far in the opposite direction of the pointing vector,
    //  to put it high above the terrain.
    const double LONG_SCALE_UP = 10000; // This is a distance in meters approx from the top of the llh valid cube
    P = P_up - dir*LONG_SCALE_UP;
  }

  Vector3 RPCModel::camera_center(Vector2 const& pix) const{
    // Return an arbitrarily chosen point on the ray back-projected
    // through the camera from the current pixel.
    Vector3 P;
    Vector3 dir;
    point_and_dir(pix, P, dir);
    return P;
  }

  Vector3 RPCModel::pixel_to_vector(Vector2 const& pix) const {
    // Find the normalized direction of the ray back-projected through
    // the camera from the current pixel.
    Vector3 P;
    Vector3 dir;
    point_and_dir(pix, P, dir);
    return dir;
  }

  std::ostream& operator<<(std::ostream& os, const RPCModel& rpc) {
    os << "RPC Model:"         << std::endl
       << "Line Numerator: "   << rpc.line_num_coeff()      << std::endl
       << "Line Denominator: " << rpc.line_den_coeff()      << std::endl
       << "Samp Numerator: "   << rpc.sample_num_coeff()    << std::endl
       << "Samp Denominator: " << rpc.sample_den_coeff()    << std::endl
       << "XY Offset: "        << rpc.xy_offset()           << std::endl
       << "XY Scale: "         << rpc.xy_scale()            << std::endl
       << "Geodetic Offset: "  << rpc.lonlatheight_offset() << std::endl
       << "Geodetic Scale: "   << rpc.lonlatheight_scale();
    return os;
  }
}


/*!
@file
Defines `boost::hana::Ring`.

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

#ifndef BOOST_HANA_RING_HPP
#define BOOST_HANA_RING_HPP

#include <boost/hana/fwd/ring.hpp>

#include <boost/hana/bool.hpp>
#include <boost/hana/constant.hpp>
#include <boost/hana/core/common.hpp>
#include <boost/hana/core/convert.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/core/default.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/core/operators.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/core/wrong.hpp>
#include <boost/hana/detail/has_common_embedding.hpp>
#include <boost/hana/detail/std/enable_if.hpp>
#include <boost/hana/detail/std/forward.hpp>
#include <boost/hana/detail/std/is_arithmetic.hpp>

// for default power
#include <boost/hana/comparable.hpp>
#include <boost/hana/enumerable.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/logical.hpp>
#include <boost/hana/monoid.hpp>

namespace boost { namespace hana {
    //////////////////////////////////////////////////////////////////////////
    // Operators
    //////////////////////////////////////////////////////////////////////////
    namespace operators {
        template <typename X, typename Y, typename = typename detail::std::enable_if<
            has_operator<datatype_t<X>, decltype(mult)>::value ||
            has_operator<datatype_t<Y>, decltype(mult)>::value
        >::type>
        constexpr decltype(auto) operator*(X&& x, Y&& y) {
            return hana::mult(detail::std::forward<X>(x),
                              detail::std::forward<Y>(y));
        }
    }

    //////////////////////////////////////////////////////////////////////////
    // mult
    //////////////////////////////////////////////////////////////////////////
    template <typename T, typename U, typename>
    struct mult_impl : mult_impl<T, U, when<true>> { };

    template <typename T, typename U, bool condition>
    struct mult_impl<T, U, when<condition>> : default_ {
        template <typename X, typename Y>
        static constexpr void apply(X&&, Y&&) {
            static_assert(wrong<mult_impl<T, U>, X, Y>{},
            "no definition of boost::hana::mult for the given data types");
        }
    };

    // Cross-type overload
    template <typename T, typename U>
    struct mult_impl<T, U, when<
        detail::has_nontrivial_common_embedding<Ring, T, U>{}
    >> {
        using C = typename common<T, U>::type;
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X&& x, Y&& y) {
            return hana::mult(to<C>(detail::std::forward<X>(x)),
                              to<C>(detail::std::forward<Y>(y)));
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // one
    //////////////////////////////////////////////////////////////////////////
    template <typename R, typename>
    struct one_impl : one_impl<R, when<true>> { };

    template <typename R, bool condition>
    struct one_impl<R, when<condition>> : default_ {
        static void apply(...) { }
    };

    //////////////////////////////////////////////////////////////////////////
    // power
    //////////////////////////////////////////////////////////////////////////
    template <typename R, typename>
    struct power_impl : power_impl<R, when<true>> { };

    template <typename R, bool condition>
    struct power_impl<R, when<condition>> : default_ {
        template <typename X, typename N>
        static constexpr decltype(auto) apply(X&& x, N&& n) {
            using Exp = typename datatype<N>::type;
            return eval_if(equal(n, zero<Exp>()),
                always(one<R>()),
                [&n, &x](auto _) -> decltype(auto) {
                    return mult(
                        x, apply(x, _(pred)(detail::std::forward<N>(n)))
                    );
                }
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // models
    //////////////////////////////////////////////////////////////////////////
    template <typename R>
    struct models_impl<Ring, R>
        : _integral_constant<bool,
            !is_default<one_impl<R>>{} &&
            !is_default<mult_impl<R, R>>{}
        >
    { };

    //////////////////////////////////////////////////////////////////////////
    // Model for non-boolean arithmetic data types
    //////////////////////////////////////////////////////////////////////////
    template <typename T>
    struct mult_impl<T, T, when<detail::std::is_non_boolean_arithmetic<T>{}>> {
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X&& x, Y&& y)
        { return detail::std::forward<X>(x) * detail::std::forward<Y>(y); }
    };

    template <typename T>
    struct one_impl<T, when<detail::std::is_non_boolean_arithmetic<T>{}>> {
        static constexpr T apply()
        { return static_cast<T>(1); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Model for Constants over a Ring
    //////////////////////////////////////////////////////////////////////////
    template <typename C>
    struct mult_impl<C, C, when<
        _models<Constant, C>{} && _models<Ring, typename C::value_type>{}
    >> {
        using T = typename C::value_type;
        template <typename X, typename Y>
        struct _constant {
            static constexpr decltype(auto) get() {
                return boost::hana::mult(boost::hana::value<X>(),
                                         boost::hana::value<Y>());
            }
            struct hana { using datatype = detail::CanonicalConstant<T>; };
        };
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X const&, Y const&)
        { return to<C>(_constant<X, Y>{}); }
    };

    template <typename C>
    struct one_impl<C, when<
        _models<Constant, C>{} && _models<Ring, typename C::value_type>{}
    >> {
        using T = typename C::value_type;
        struct _constant {
            static constexpr decltype(auto) get()
            { return boost::hana::one<T>(); }
            struct hana { using datatype = detail::CanonicalConstant<T>; };
        };
        static constexpr decltype(auto) apply()
        { return to<C>(_constant{}); }
    };

}} // end namespace boost::hana

#endif // !BOOST_HANA_RING_HPP


#ifndef PC_SEGMENTATION_HPP_
#define PC_SEGMENTATION_HPP_

#include <common.hpp>

#include <stddef.h>
#include <utility>
#include <vector>

#include <boost/smart_ptr/shared_ptr.hpp>
#include <boost/thread/pthread/mutex.hpp>

#include <Eigen/src/Core/Matrix.h>
#include <Eigen/src/Core/util/Memory.h>
#include <Eigen/src/StlSupport/StdVector.h>

#include <pcl/common/centroid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/vfh.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_cloud.h>
#include <pcl/PointIndices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/search/search.h>
#include <pcl/segmentation/organized_multi_plane_segmentation.h>
#include <pcl/segmentation/planar_region.h>
#include <pcl/segmentation/region_growing_rgb.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree.h>

#include<utils.hpp>
#include<vector>
//#include<objectFeatures.hpp>
#include <pcl/common/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <visualization_msgs/Marker.h>
//#include <PcFeatures.h>

#include <rgb_euclidean_comparator.h>
#include <utils.hpp>
//#include <misc_structures.hpp>
//#include <objectFeatures.hpp>

#define MIN_PLANE_SIZE 10000
#define MIN_CLUSTER_SIZE 50
#define DIST_THRESH 0.03f

#ifdef SIMON
	#define MIN_Y_THRESH -0.3
	#define MAX_Y_THRESH  0.2
	#define MIN_Z_THRESH  0.8
	#define MAX_Z_THRESH  1.4
	#define MIN_X_THRESH -0.4
	#define MAX_X_THRESH  0.5
#elif defined WORLD_FRAME
        #define MIN_Y_THRESH -0.5
        #define MAX_Y_THRESH  0.5
        #define MIN_Z_THRESH  -1.1
        #define MAX_Z_THRESH  -0.6
        #define MIN_X_THRESH -0.5
        #define MAX_X_THRESH  0.5
#else
	#define MIN_Y_THRESH -0.4
	#define MAX_Y_THRESH  0.3
	#define MIN_Z_THRESH  0.6
	#define MAX_Z_THRESH  1.1
	#define MIN_X_THRESH -0.5
	#define MAX_X_THRESH  0.5
#endif

#define COLOR_SEG 2 //0 none, 1 rg, 2 within cc
#define MERGE_CLUSTERS 1 //0 none, 1 rg, 2 within cc

// Useful definitions
//typedef pcl::PointXYZRGBA PointT;

// Set up the visualization window
boost::shared_ptr<pcl::visualization::PCLVisualizer>
cloudViewer (pcl::PointCloud<PointT>::ConstPtr cloud);

class RansacSinglePlaneSegmentation
{
private:
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  pcl::PointCloud<PointT>::ConstPtr prev_cloud;
  boost::mutex cloud_mutex;

  char name[1024];
  float threshs[6];

  bool viewer_enabled;

public:
  RansacSinglePlaneSegmentation () : viewer_enabled(false), verbose(true)
  {
  }
  /*~RansacSinglePlaneSegmentation ()
  {
  }*/

  bool verbose;

  void setViewer(boost::shared_ptr<pcl::visualization::PCLVisualizer> in_viewer){
    viewer = in_viewer;
  }

  void removeCoordinateSystem()
  {
    viewer->removeCoordinateSystem();
  }

  pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
  pcl::NormalEstimation<PointT, pcl::Normal> norm_est;
  pcl::MyEuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_;
  pcl::search::Search <PointT>::Ptr tree;

  //RANSAC
  pcl::SACSegmentation<PointT> seg;
  pcl::VoxelGrid<PointT> vg;

  pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh;
  pcl::RegionGrowingRGB<PointT> reg;

  pcl::StatisticalOutlierRemoval<PointT>::Ptr sor;
 // void pcl::compute3DCentroid(pcl::PointCloud<PointT> &cluster, Eigen::Vector4f centroid);

  void
  removePreviousDataFromScreen (size_t prev_models_size);

  void
  removePreviousCLustersFromScreen (size_t num_clusters);

  // display clusters in with color
  void
  displayEuclideanClusters (const pcl::PointCloud<PointT>::CloudVectorType &clusters, boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer);

  // display each cluster given its color
  void
  displayEuclideanCluster (const pcl::PointCloud<PointT> &cluster, boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer, unsigned char color[], size_t clusterNum);

  void
  displayPlane(pcl::PointCloud<PointT>::Ptr contour);
  void
  setWorkingVolumeThresholds(float* a);
  //allow for reading a file
  void
  initSegmentation(int color_seg = COLOR_SEG, float distance_thresh = 0.05, float color_thresh = 25);

  //isFilter: aray of 6 denoting whether to threshold xmin,xmax,ymin,ymax,zmin,zmax and limits is the corresponding limits
  //probably there is a better built in way of doing this
  void
  threshXYZ(pcl::PointCloud<PointT>::Ptr out_cloud, bool isFilter[], float limits[]);

  void
  preProcPointCloud(pcl::PointCloud<PointT>::Ptr filtered_cloud, bool noiseFilter = false);

  //downsample
  void
  downSample(pcl::PointCloud<PointT>::Ptr &input_cloud, float leaf_size, pcl::PointCloud<PointT>::Ptr &output_cloud);

  // for RANSAC
  void
  planeExtract(
    pcl::PointCloud<PointT>::Ptr &filtered_cloud,
    pcl::PointCloud<PointT>::Ptr &cloud_plane,
    pcl::ModelCoefficients::Ptr &coefficients,
    pcl::PointIndices::Ptr &inliers,
    pcl::PointCloud<pcl::Normal>::Ptr normal_cloud
    );

  void
  segmentPointCloud(pcl::ModelCoefficients::Ptr &coefficients,
                                                         pcl::PointCloud<PointT>::Ptr cloud_filtered,
                                                         pcl::PointCloud<PointT>::CloudVectorType &clusters,
                                                         std::vector<pcl::PointCloud<pcl::Normal> > &cluster_normals,
                                                         pcl::PointCloud<PointT>::Ptr &cloud_plane
                                                         );

  int
  processOnce (pcl::PointCloud<PointT>::ConstPtr prev_cl,
                  pcl::PointCloud<PointT>::CloudVectorType &clustersOut,
                  std::vector<pcl::PointCloud<pcl::Normal> > &clustersOutNormals,
                  Eigen::Vector4f &plane,
		  std::vector<std::vector<int>> &clusterIndices,
		  bool preProc = true, bool merge_clusters = true,
		  bool viewer_enabled = true, bool noiseFilter = false);

};

#endif


#ifndef KDTREE2_TEST_HH
#define KDTREE2_TEST_HH

#include "kdtree2.hpp"
#include <boost/multi_array.hpp>
// #include <boost/random.hpp>

using namespace std;

class TestKDTree2 : public BboxIntersectionTest {
    typedef boost::multi_array<float,2> KDTreeArray;

    private:
        kdtree2::KDTree* tree;
        KDTreeArray *data;

        //rearrange points is designed to make leaves more contiguous in memory/help ensure that topologically close nodes are contiguous
        void _build_tree(const std::vector<point> &pts, const std::vector<size_t> &indices, size_t bucket_size, bool rearrange_points) {
            data = new KDTreeArray;
            data->resize(boost::extents[pts.size()][NUM_DIMS]);

            for (int i=0; i<pts.size(); i++) {
                for (int j=0; j<NUM_DIMS; j++) {
                    (*data)[i][j] = pts[i][j];
                }
            }
            tree = new kdtree2::KDTree(*data,rearrange_points,bucket_size);
        }
        struct kdtree2_result_iterator : kdtree2::KDTreeResultVector::const_iterator
        {
            using base_class = kdtree2::KDTreeResultVector::const_iterator;
            using value_type = size_t;

            using base_class::base_class;

            value_type operator*() const {
                auto const& src = base_class::operator*();
                return (src.idx);
            }
        };
    public:

        bool intersections_exact() { return false; } //circular radius is not exact

        TestKDTree2() {}
        ~TestKDTree2() {
            delete tree;
            delete data;
        }

        void build_tree(const std::vector<point> &pts, const std::vector<size_t> &indices) {
            _build_tree(pts, indices, NUM_ELEMS_PER_NODE, false);
        }

        void build_tree(const std::vector<point> &pts, const std::vector<size_t> &indices, size_t bucket_size, bool rearrange_points=false) {
            _build_tree(pts, indices, bucket_size, rearrange_points);
        }

        //closed region -> includes Points that fall on the boundary
        void get_intersections(const bbox &my_bbox, std::vector<size_t> &intersections_indices) {
            point_f mid_pt;
            float squared_radius_search_bound = 0;
            get_max_squared_radius(my_bbox, mid_pt, squared_radius_search_bound);

            kdtree2::KDTreeResultVector result;

            tree->r_nearest(mid_pt, squared_radius_search_bound, result);
            intersections_indices.assign(kdtree2_result_iterator(result.begin()),
                                       kdtree2_result_iterator(result.end()));
        }

};

#endif //KDTREE2_TEST_HH


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/integral_constant.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/tuple.hpp>

#include <vector>
namespace hana = boost::hana;

using Vector = std::vector<int>;

static_assert(
    sizeof(hana::tuple<
        hana::pair<hana::int_<0>, Vector>,
        hana::pair<hana::int_<1>, Vector>,
        hana::pair<hana::int_<2>, Vector>,
        hana::pair<hana::int_<3>, Vector>
    >)
    ==
    sizeof(hana::tuple<Vector, Vector, Vector, Vector>)
, "");

int main() { }