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

#include <pcl/apps/in_hand_scanner/opengl_viewer.h>
#include <pcl/pcl_config.h>

#include <cmath>
#include <cstdlib>
#include <typeinfo>
#ifdef OPENGL_IS_A_FRAMEWORK
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#endif

#include <pcl/apps/in_hand_scanner/visibility_confidence.h>
#include <pcl/common/centroid.h>
#include <pcl/common/impl/centroid.hpp> // TODO: PointIHS is not registered

#include <QApplication>
#include <QtOpenGL>

////////////////////////////////////////////////////////////////////////////////
// FaceVertexMesh
////////////////////////////////////////////////////////////////////////////////

pcl::ihs::detail::FaceVertexMesh::FaceVertexMesh()
: transformation(Eigen::Isometry3d::Identity())
{}

////////////////////////////////////////////////////////////////////////////////

pcl::ihs::detail::FaceVertexMesh::FaceVertexMesh(const Mesh& mesh,
                                                 const Eigen::Isometry3d& T)
: vertices(mesh.getVertexDataCloud()), transformation(T)
{
  if (typeid(Mesh::MeshTag) != typeid(pcl::geometry::TriangleMeshTag)) {
    std::cerr
        << "In opengl_viewer.cpp: Only triangle meshes are currently supported!\n";
    exit(EXIT_FAILURE);
  }

  for (auto& vertex : vertices) {
    std::swap(vertex.r, vertex.b);
  }

  triangles.reserve(mesh.sizeFaces());
  pcl::ihs::detail::FaceVertexMesh::Triangle triangle;

  for (std::size_t i = 0; i < mesh.sizeFaces(); ++i) {
    Mesh::VertexAroundFaceCirculator circ =
        mesh.getVertexAroundFaceCirculator(Mesh::FaceIndex(i));
    triangle.first = (circ++).getTargetIndex().get();
    triangle.second = (circ++).getTargetIndex().get();
    triangle.third = (circ).getTargetIndex().get();

    triangles.push_back(triangle);
  }
}

////////////////////////////////////////////////////////////////////////////////
// OpenGLViewer
////////////////////////////////////////////////////////////////////////////////

pcl::ihs::OpenGLViewer::OpenGLViewer(QWidget* parent)
: QOpenGLWidget(parent)
, timer_vis_(new QTimer(this))
, colormap_(Colormap::Constant(255))
, vis_conf_norm_(1)
, mesh_representation_(MR_POINTS)
, coloring_(COL_RGB)
, draw_box_(false)
, scaling_factor_(1.)
, R_cam_(1., 0., 0., 0.)
, t_cam_(0., 0., 0.)
, cam_pivot_(0., 0., 0.)
, cam_pivot_id_("")
, mouse_pressed_begin_(false)
, x_prev_(0)
, y_prev_(0)
{
  // Timer: Defines the update rate for the visualization
  connect(timer_vis_.get(), SIGNAL(timeout()), this, SLOT(timerCallback()));
  timer_vis_->start(33);

  // http://doc.qt.digia.com/qt/opengl-overpainting.html
  QWidget::setAutoFillBackground(false);

  // http://doc.qt.digia.com/qt/qwidget.html#keyPressEvent
  this->setFocusPolicy(Qt::StrongFocus);

  // http://doc.qt.digia.com/qt/qmetatype.html#qRegisterMetaType
  qRegisterMetaType<pcl::ihs::OpenGLViewer::MeshRepresentation>("MeshRepresentation");
  qRegisterMetaType<pcl::ihs::OpenGLViewer::Coloring>("Coloring");

  //////////////////////////////////////////////////////////////////////////////
  // Code to generate the colormap (I don't want to link against vtk just for the
  // colormap).
  //////////////////////////////////////////////////////////////////////////////

  //#include <cstdlib>
  //#include <iomanip>

  //#include <vtkColorTransferFunction.h>
  //#include <vtkSmartPointer.h>

  // int
  // main()
  // {
  //   static const unsigned int n = 256;
  //   // double rgb_1 [] = { 59./255., 76./255., 192./255.};
  //   // double rgb_2 [] = {180./255.,  4./255.,  38./255.};
  //   double rgb_1 [] = {180./255.,   0./255., 0./255.};
  //   double rgb_2 [] = {  0./255., 180./255., 0./255.};
  //
  //   vtkSmartPointer <vtkColorTransferFunction> ctf = vtkColorTransferFunction::New();
  //   ctf->SetColorSpaceToDiverging();
  //   ctf->AddRGBPoint(0., rgb_1 [0], rgb_1 [1], rgb_1 [2]);
  //   ctf->AddRGBPoint(1., rgb_2 [0], rgb_2 [1], rgb_2 [2]);
  //   ctf->Build();
  //
  //   const unsigned char* colormap = ctf->GetTable (0., 1., n);
  //
  //   for (unsigned int i=0; i<n; ++i)
  //   {
  //     const unsigned int r = static_cast <unsigned int> (colormap [3 * i    ]);
  //     const unsigned int g = static_cast <unsigned int> (colormap [3 * i + 1]);
  //     const unsigned int b = static_cast <unsigned int> (colormap [3 * i + 2]);
  //
  //     std::cerr << "colormap_.col ("
  //               << std::setw (3) << i << ") = Color ("
  //               << std::setw (3) << r << ", "
  //               << std::setw (3) << g << ", "
  //               << std::setw (3) << b << ");\n";
  //   }
  //
  //   return (EXIT_SUCCESS);
  // }

  colormap_.col(0) = Color(180, 0, 0);
  colormap_.col(1) = Color(182, 9, 1);
  colormap_.col(2) = Color(184, 17, 1);
  colormap_.col(3) = Color(186, 24, 2);
  colormap_.col(4) = Color(188, 29, 2);
  colormap_.col(5) = Color(190, 33, 3);
  colormap_.col(6) = Color(192, 38, 4);
  colormap_.col(7) = Color(194, 42, 5);
  colormap_.col(8) = Color(196, 46, 6);
  colormap_.col(9) = Color(197, 49, 7);
  colormap_.col(10) = Color(199, 53, 9);
  colormap_.col(11) = Color(201, 56, 10);
  colormap_.col(12) = Color(203, 59, 12);
  colormap_.col(13) = Color(205, 63, 13);
  colormap_.col(14) = Color(207, 66, 15);
  colormap_.col(15) = Color(208, 69, 17);
  colormap_.col(16) = Color(210, 72, 18);
  colormap_.col(17) = Color(212, 75, 20);
  colormap_.col(18) = Color(214, 78, 21);
  colormap_.col(19) = Color(215, 81, 23);
  colormap_.col(20) = Color(217, 84, 25);
  colormap_.col(21) = Color(219, 87, 26);
  colormap_.col(22) = Color(221, 89, 28);
  colormap_.col(23) = Color(222, 92, 30);
  colormap_.col(24) = Color(224, 95, 32);
  colormap_.col(25) = Color(225, 98, 33);
  colormap_.col(26) = Color(227, 101, 35);
  colormap_.col(27) = Color(229, 103, 37);
  colormap_.col(28) = Color(230, 106, 39);
  colormap_.col(29) = Color(232, 109, 40);
  colormap_.col(30) = Color(233, 112, 42);
  colormap_.col(31) = Color(235, 114, 44);
  colormap_.col(32) = Color(236, 117, 46);
  colormap_.col(33) = Color(238, 120, 48);
  colormap_.col(34) = Color(239, 122, 50);
  colormap_.col(35) = Color(241, 125, 52);
  colormap_.col(36) = Color(242, 127, 54);
  colormap_.col(37) = Color(244, 130, 56);
  colormap_.col(38) = Color(245, 133, 58);
  colormap_.col(39) = Color(246, 135, 60);
  colormap_.col(40) = Color(248, 138, 62);
  colormap_.col(41) = Color(249, 140, 64);
  colormap_.col(42) = Color(250, 143, 66);
  colormap_.col(43) = Color(252, 145, 68);
  colormap_.col(44) = Color(253, 148, 70);
  colormap_.col(45) = Color(254, 150, 73);
  colormap_.col(46) = Color(255, 153, 75);
  colormap_.col(47) = Color(255, 154, 76);
  colormap_.col(48) = Color(255, 156, 78);
  colormap_.col(49) = Color(255, 158, 80);
  colormap_.col(50) = Color(255, 159, 82);
  colormap_.col(51) = Color(255, 161, 84);
  colormap_.col(52) = Color(255, 163, 86);
  colormap_.col(53) = Color(255, 164, 88);
  colormap_.col(54) = Color(255, 166, 90);
  colormap_.col(55) = Color(255, 168, 92);
  colormap_.col(56) = Color(255, 169, 93);
  colormap_.col(57) = Color(255, 171, 95);
  colormap_.col(58) = Color(255, 172, 97);
  colormap_.col(59) = Color(255, 174, 99);
  colormap_.col(60) = Color(255, 176, 101);
  colormap_.col(61) = Color(255, 177, 103);
  colormap_.col(62) = Color(255, 179, 105);
  colormap_.col(63) = Color(255, 180, 107);
  colormap_.col(64) = Color(255, 182, 109);
  colormap_.col(65) = Color(255, 183, 111);
  colormap_.col(66) = Color(255, 185, 113);
  colormap_.col(67) = Color(255, 186, 115);
  colormap_.col(68) = Color(255, 188, 117);
  colormap_.col(69) = Color(255, 189, 119);
  colormap_.col(70) = Color(255, 191, 122);
  colormap_.col(71) = Color(255, 192, 124);
  colormap_.col(72) = Color(255, 194, 126);
  colormap_.col(73) = Color(255, 195, 128);
  colormap_.col(74) = Color(255, 196, 130);
  colormap_.col(75) = Color(255, 198, 132);
  colormap_.col(76) = Color(255, 199, 134);
  colormap_.col(77) = Color(255, 201, 136);
  colormap_.col(78) = Color(255, 202, 139);
  colormap_.col(79) = Color(255, 203, 141);
  colormap_.col(80) = Color(255, 205, 143);
  colormap_.col(81) = Color(255, 206, 145);
  colormap_.col(82) = Color(255, 207, 147);
  colormap_.col(83) = Color(255, 209, 149);
  colormap_.col(84) = Color(255, 210, 152);
  colormap_.col(85) = Color(255, 211, 154);
  colormap_.col(86) = Color(255, 213, 156);
  colormap_.col(87) = Color(255, 214, 158);
  colormap_.col(88) = Color(255, 215, 161);
  colormap_.col(89) = Color(255, 216, 163);
  colormap_.col(90) = Color(255, 218, 165);
  colormap_.col(91) = Color(255, 219, 168);
  colormap_.col(92) = Color(255, 220, 170);
  colormap_.col(93) = Color(255, 221, 172);
  colormap_.col(94) = Color(255, 223, 175);
  colormap_.col(95) = Color(255, 224, 177);
  colormap_.col(96) = Color(255, 225, 179);
  colormap_.col(97) = Color(255, 226, 182);
  colormap_.col(98) = Color(255, 227, 184);
  colormap_.col(99) = Color(255, 228, 186);
  colormap_.col(100) = Color(255, 230, 189);
  colormap_.col(101) = Color(255, 231, 191);
  colormap_.col(102) = Color(255, 232, 193);
  colormap_.col(103) = Color(255, 233, 196);
  colormap_.col(104) = Color(255, 234, 198);
  colormap_.col(105) = Color(255, 235, 201);
  colormap_.col(106) = Color(255, 236, 203);
  colormap_.col(107) = Color(255, 237, 205);
  colormap_.col(108) = Color(255, 238, 208);
  colormap_.col(109) = Color(255, 239, 210);
  colormap_.col(110) = Color(255, 240, 213);
  colormap_.col(111) = Color(255, 241, 215);
  colormap_.col(112) = Color(255, 242, 218);
  colormap_.col(113) = Color(255, 243, 220);
  colormap_.col(114) = Color(255, 244, 222);
  colormap_.col(115) = Color(255, 245, 225);
  colormap_.col(116) = Color(255, 246, 227);
  colormap_.col(117) = Color(255, 247, 230);
  colormap_.col(118) = Color(255, 248, 232);
  colormap_.col(119) = Color(255, 249, 235);
  colormap_.col(120) = Color(255, 249, 237);
  colormap_.col(121) = Color(255, 250, 239);
  colormap_.col(122) = Color(255, 251, 242);
  colormap_.col(123) = Color(255, 252, 244);
  colormap_.col(124) = Color(255, 253, 247);
  colormap_.col(125) = Color(255, 253, 249);
  colormap_.col(126) = Color(255, 254, 251);
  colormap_.col(127) = Color(255, 255, 254);
  colormap_.col(128) = Color(255, 255, 254);
  colormap_.col(129) = Color(254, 255, 253);
  colormap_.col(130) = Color(253, 255, 252);
  colormap_.col(131) = Color(252, 255, 250);
  colormap_.col(132) = Color(251, 255, 249);
  colormap_.col(133) = Color(250, 255, 248);
  colormap_.col(134) = Color(249, 255, 246);
  colormap_.col(135) = Color(248, 255, 245);
  colormap_.col(136) = Color(247, 255, 244);
  colormap_.col(137) = Color(246, 255, 242);
  colormap_.col(138) = Color(245, 255, 241);
  colormap_.col(139) = Color(244, 255, 240);
  colormap_.col(140) = Color(243, 255, 238);
  colormap_.col(141) = Color(242, 255, 237);
  colormap_.col(142) = Color(241, 255, 236);
  colormap_.col(143) = Color(240, 255, 235);
  colormap_.col(144) = Color(239, 255, 233);
  colormap_.col(145) = Color(238, 255, 232);
  colormap_.col(146) = Color(237, 255, 231);
  colormap_.col(147) = Color(236, 255, 229);
  colormap_.col(148) = Color(235, 255, 228);
  colormap_.col(149) = Color(234, 255, 227);
  colormap_.col(150) = Color(234, 255, 225);
  colormap_.col(151) = Color(233, 255, 224);
  colormap_.col(152) = Color(232, 255, 223);
  colormap_.col(153) = Color(231, 255, 221);
  colormap_.col(154) = Color(230, 255, 220);
  colormap_.col(155) = Color(229, 255, 219);
  colormap_.col(156) = Color(228, 255, 218);
  colormap_.col(157) = Color(227, 255, 216);
  colormap_.col(158) = Color(226, 255, 215);
  colormap_.col(159) = Color(225, 255, 214);
  colormap_.col(160) = Color(224, 255, 212);
  colormap_.col(161) = Color(223, 255, 211);
  colormap_.col(162) = Color(222, 255, 210);
  colormap_.col(163) = Color(221, 255, 208);
  colormap_.col(164) = Color(220, 255, 207);
  colormap_.col(165) = Color(219, 255, 206);
  colormap_.col(166) = Color(218, 255, 204);
  colormap_.col(167) = Color(217, 255, 203);
  colormap_.col(168) = Color(216, 255, 202);
  colormap_.col(169) = Color(215, 255, 201);
  colormap_.col(170) = Color(214, 255, 199);
  colormap_.col(171) = Color(213, 255, 198);
  colormap_.col(172) = Color(211, 255, 197);
  colormap_.col(173) = Color(210, 255, 195);
  colormap_.col(174) = Color(209, 255, 194);
  colormap_.col(175) = Color(208, 255, 193);
  colormap_.col(176) = Color(207, 255, 191);
  colormap_.col(177) = Color(206, 255, 190);
  colormap_.col(178) = Color(205, 255, 188);
  colormap_.col(179) = Color(204, 255, 187);
  colormap_.col(180) = Color(203, 255, 186);
  colormap_.col(181) = Color(202, 255, 184);
  colormap_.col(182) = Color(201, 255, 183);
  colormap_.col(183) = Color(199, 255, 182);
  colormap_.col(184) = Color(198, 255, 180);
  colormap_.col(185) = Color(197, 255, 179);
  colormap_.col(186) = Color(196, 255, 177);
  colormap_.col(187) = Color(195, 255, 176);
  colormap_.col(188) = Color(194, 255, 174);
  colormap_.col(189) = Color(192, 255, 173);
  colormap_.col(190) = Color(191, 255, 172);
  colormap_.col(191) = Color(190, 255, 170);
  colormap_.col(192) = Color(189, 255, 169);
  colormap_.col(193) = Color(188, 255, 167);
  colormap_.col(194) = Color(186, 255, 166);
  colormap_.col(195) = Color(185, 255, 164);
  colormap_.col(196) = Color(184, 255, 163);
  colormap_.col(197) = Color(183, 255, 161);
  colormap_.col(198) = Color(181, 255, 160);
  colormap_.col(199) = Color(180, 255, 158);
  colormap_.col(200) = Color(179, 255, 157);
  colormap_.col(201) = Color(177, 255, 155);
  colormap_.col(202) = Color(176, 255, 154);
  colormap_.col(203) = Color(175, 255, 152);
  colormap_.col(204) = Color(173, 255, 150);
  colormap_.col(205) = Color(172, 255, 149);
  colormap_.col(206) = Color(170, 255, 147);
  colormap_.col(207) = Color(169, 255, 145);
  colormap_.col(208) = Color(166, 253, 143);
  colormap_.col(209) = Color(164, 252, 141);
  colormap_.col(210) = Color(162, 251, 138);
  colormap_.col(211) = Color(159, 250, 136);
  colormap_.col(212) = Color(157, 248, 134);
  colormap_.col(213) = Color(155, 247, 131);
  colormap_.col(214) = Color(152, 246, 129);
  colormap_.col(215) = Color(150, 245, 127);
  colormap_.col(216) = Color(148, 243, 124);
  colormap_.col(217) = Color(145, 242, 122);
  colormap_.col(218) = Color(143, 240, 119);
  colormap_.col(219) = Color(140, 239, 117);
  colormap_.col(220) = Color(138, 238, 114);
  colormap_.col(221) = Color(135, 236, 112);
  colormap_.col(222) = Color(133, 235, 110);
  colormap_.col(223) = Color(130, 233, 107);
  colormap_.col(224) = Color(128, 232, 105);
  colormap_.col(225) = Color(125, 230, 102);
  colormap_.col(226) = Color(122, 229, 100);
  colormap_.col(227) = Color(120, 227, 97);
  colormap_.col(228) = Color(117, 226, 94);
  colormap_.col(229) = Color(114, 224, 92);
  colormap_.col(230) = Color(111, 223, 89);
  colormap_.col(231) = Color(109, 221, 87);
  colormap_.col(232) = Color(106, 220, 84);
  colormap_.col(233) = Color(103, 218, 82);
  colormap_.col(234) = Color(100, 217, 79);
  colormap_.col(235) = Color(97, 215, 76);
  colormap_.col(236) = Color(94, 213, 73);
  colormap_.col(237) = Color(91, 212, 71);
  colormap_.col(238) = Color(88, 210, 68);
  colormap_.col(239) = Color(85, 208, 65);
  colormap_.col(240) = Color(82, 207, 62);
  colormap_.col(241) = Color(78, 205, 59);
  colormap_.col(242) = Color(75, 203, 57);
  colormap_.col(243) = Color(71, 201, 54);
  colormap_.col(244) = Color(68, 200, 50);
  colormap_.col(245) = Color(64, 198, 47);
  colormap_.col(246) = Color(60, 196, 44);
  colormap_.col(247) = Color(56, 195, 41);
  colormap_.col(248) = Color(52, 193, 37);
  colormap_.col(249) = Color(47, 191, 33);
  colormap_.col(250) = Color(42, 189, 29);
  colormap_.col(251) = Color(37, 187, 25);
  colormap_.col(252) = Color(31, 186, 20);
  colormap_.col(253) = Color(24, 184, 15);
  colormap_.col(254) = Color(14, 182, 7);
  colormap_.col(255) = Color(0, 180, 0);
}

////////////////////////////////////////////////////////////////////////////////

pcl::ihs::OpenGLViewer::~OpenGLViewer() { this->stopTimer(); }

////////////////////////////////////////////////////////////////////////////////

bool
pcl::ihs::OpenGLViewer::addMesh(const MeshConstPtr& mesh,
                                const std::string& id,
                                const Eigen::Isometry3d& T)
{
  if (!mesh) {
    std::cerr << "ERROR in opengl_viewer.cpp: Input mesh is not valid.\n";
    return (false);
  }

  std::lock_guard<std::mutex> lock(mutex_vis_);

  if (this->getMeshIsAdded(id))
    drawn_meshes_[id] = FaceVertexMeshPtr(new FaceVertexMesh(*mesh, T));
  else
    drawn_meshes_.insert(
        std::make_pair(id, FaceVertexMeshPtr(new FaceVertexMesh(*mesh, T))));

  return (true);
}

////////////////////////////////////////////////////////////////////////////////

bool
pcl::ihs::OpenGLViewer::addMesh(const CloudXYZRGBNormalConstPtr& cloud,
                                const std::string& id,
                                const Eigen::Isometry3d& T)
{
  if (!cloud) {
    std::cerr << "ERROR in opengl_viewer.cpp: Input cloud is not valid.\n";
    return (false);
  }
  if (!cloud->isOrganized()) {
    std::cerr << "ERROR in opengl_viewer.cpp: Input cloud is not organized.\n";
    return (false);
  }

  // Convert the cloud to a mesh using the following pattern
  // 2 - 1 //
  // | / | //
  // 3 - 0 //
  const int w = cloud->width;
  const int h = cloud->height;
  const int offset_1 = -w;
  const int offset_2 = -w - 1;
  constexpr int offset_3 = -1;

  FaceVertexMeshPtr mesh(new FaceVertexMesh());
  mesh->transformation = T;

  std::vector<int> indices(w * h,
                           -1); // Map the original indices to the vertex indices.
  CloudIHS& vertices = mesh->vertices;
  std::vector<FaceVertexMesh::Triangle>& triangles = mesh->triangles;
  vertices.reserve(cloud->size());
  triangles.reserve(2 * (w - 1) * (h - 1));

  // Helper functor
  struct AddVertex {
    inline int
    operator()(const PointXYZRGBNormal& pt, CloudIHS& vertices, int& ind_o) const
    {
      if (ind_o == -1) {
        ind_o = vertices.size();
        vertices.push_back(PointIHS(pt, -pt.normal_z));
        std::swap(vertices.back().r, vertices.back().b);
      }
      return (ind_o);
    }
  };
  AddVertex addVertex;

  int ind_o_0, ind_o_1, ind_o_2, ind_o_3; // Index into the organized cloud.
  int ind_v_0, ind_v_1, ind_v_2, ind_v_3; // Index to the new vertices.

  for (int r = 1; r < h; ++r) {
    for (int c = 1; c < w; ++c) {
      ind_o_0 = r * w + c;
      ind_o_1 = ind_o_0 + offset_1;
      ind_o_2 = ind_o_0 + offset_2;
      ind_o_3 = ind_o_0 + offset_3;

      const PointXYZRGBNormal& pt_0 = cloud->operator[](ind_o_0);
      const PointXYZRGBNormal& pt_1 = cloud->operator[](ind_o_1);
      const PointXYZRGBNormal& pt_2 = cloud->operator[](ind_o_2);
      const PointXYZRGBNormal& pt_3 = cloud->operator[](ind_o_3);

      if (!std::isnan(pt_1.x) && !std::isnan(pt_3.x)) {
        if (!std::isnan(pt_2.x)) // 1-2-3 is valid
        {
          if (std::abs(pt_1.z - pt_2.z) < 1 && std::abs(pt_1.z - pt_3.z) < 1 &&
              std::abs(pt_2.z - pt_3.z) < 1) // distance threshold
          {
            ind_v_1 = addVertex(pt_1, vertices, indices[ind_o_1]);
            ind_v_2 = addVertex(pt_2, vertices, indices[ind_o_2]);
            ind_v_3 = addVertex(pt_3, vertices, indices[ind_o_3]);

            triangles.emplace_back(ind_v_1, ind_v_2, ind_v_3);
          }
        }
        if (!std::isnan(pt_0.x)) // 0-1-3 is valid
        {
          if (std::abs(pt_0.z - pt_1.z) < 1 && std::abs(pt_0.z - pt_3.z) < 1 &&
              std::abs(pt_1.z - pt_3.z) < 1) // distance threshold
          {
            ind_v_1 = addVertex(pt_1, vertices, indices[ind_o_1]);
            ind_v_3 = addVertex(pt_3, vertices, indices[ind_o_3]);
            ind_v_0 = addVertex(pt_0, vertices, indices[ind_o_0]);

            triangles.emplace_back(ind_v_1, ind_v_3, ind_v_0);
          }
        }
      }
    }
  }

  // Finally add the mesh.
  std::lock_guard<std::mutex> lock(mutex_vis_);

  if (this->getMeshIsAdded(id))
    drawn_meshes_[id] = mesh;
  else
    drawn_meshes_.insert(std::make_pair(id, mesh));

  return (true);
}

////////////////////////////////////////////////////////////////////////////////

bool
pcl::ihs::OpenGLViewer::removeMesh(const std::string& id)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);
  if (!this->getMeshIsAdded(id))
    return (false);

  drawn_meshes_.erase(id);

  return (true);
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::removeAllMeshes()
{
  std::lock_guard<std::mutex> lock(mutex_vis_);
  drawn_meshes_.clear();
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::setBoxCoefficients(const BoxCoefficients& coeffs)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);
  box_coefficients_ = coeffs;
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::setDrawBox(const bool enabled)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);
  draw_box_ = enabled;
}

////////////////////////////////////////////////////////////////////////////////

bool
pcl::ihs::OpenGLViewer::getDrawBox() const
{
  return (draw_box_);
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::setPivot(const Eigen::Vector3d& pivot)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);
  cam_pivot_ = pivot;
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::setPivot(const std::string& id)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);
  cam_pivot_id_ = id;
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::stopTimer()
{
  std::lock_guard<std::mutex> lock(mutex_vis_);
  if (timer_vis_) {
    timer_vis_->stop();
  }
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::setVisibilityConfidenceNormalization(const float vis_conf_norm)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);

  vis_conf_norm_ = vis_conf_norm < 1 ? 1 : vis_conf_norm;
}

////////////////////////////////////////////////////////////////////////////////

QSize
pcl::ihs::OpenGLViewer::minimumSizeHint() const
{
  return (QSize(160, 120));
}

////////////////////////////////////////////////////////////////////////////////

QSize
pcl::ihs::OpenGLViewer::sizeHint() const
{
  return (QSize(640, 480));
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::setScalingFactor(const double scale)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);
  scaling_factor_ = scale;
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::timerCallback()
{
  QWidget::update();
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::resetCamera()
{
  std::lock_guard<std::mutex> lock(mutex_vis_);

  R_cam_ = Eigen::Quaterniond(1., 0., 0., 0.);
  t_cam_ = Eigen::Vector3d(0., 0., 0.);
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::toggleMeshRepresentation()
{
  switch (mesh_representation_) {
  case MR_POINTS:
    this->setMeshRepresentation(MR_EDGES);
    break;
  case MR_EDGES:
    this->setMeshRepresentation(MR_FACES);
    break;
  case MR_FACES:
    this->setMeshRepresentation(MR_POINTS);
    break;
  default:
    std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n";
    exit(EXIT_FAILURE);
  }
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::setMeshRepresentation(const MeshRepresentation& representation)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);

  switch (mesh_representation_) {
  case MR_POINTS:
    std::cerr << "Drawing the points.\n";
    break;
  case MR_EDGES:
    std::cerr << "Drawing the edges.\n";
    break;
  case MR_FACES:
    std::cerr << "Drawing the faces.\n";
    break;
  default:
    std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n";
    exit(EXIT_FAILURE);
  }

  mesh_representation_ = representation;
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::toggleColoring()
{
  switch (coloring_) {
  case COL_RGB:
    this->setColoring(COL_ONE_COLOR);
    break;
  case COL_ONE_COLOR:
    this->setColoring(COL_VISCONF);
    break;
  case COL_VISCONF:
    this->setColoring(COL_RGB);
    break;
  default:
    std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n";
    exit(EXIT_FAILURE);
  }
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::setColoring(const Coloring& coloring)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);

  switch (coloring) {
  case COL_RGB:
    std::cerr << "Coloring according to the rgb values.\n";
    break;
  case COL_ONE_COLOR:
    std::cerr << "Use one color for all points.\n";
    break;
  case COL_VISCONF:
    std::cerr << "Coloring according to the visibility confidence.\n";
    break;
  default:
    std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n";
    exit(EXIT_FAILURE);
  }
  coloring_ = coloring;
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::paintEvent(QPaintEvent* /*event*/)
{
  this->calcPivot();
  this->makeCurrent();

  // Clear information from the last draw
  glClearColor(0.f, 0.f, 0.f, 0.f);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  this->setupViewport(this->width(), this->height());

  // Move light with camera (see example 5-7)
  // http://www.glprogramming.com/red/chapter05.html
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  // light 0 (directional)
  glLightfv(
      GL_LIGHT0, GL_AMBIENT, Eigen::Vector4f(0.1f, 0.1f, 0.1f, 1.0f).eval().data());
  glLightfv(
      GL_LIGHT0, GL_DIFFUSE, Eigen::Vector4f(0.6f, 0.6f, 0.6f, 1.0f).eval().data());
  glLightfv(
      GL_LIGHT0, GL_SPECULAR, Eigen::Vector4f(0.2f, 0.2f, 0.2f, 1.0f).eval().data());
  glLightfv(GL_LIGHT0,
            GL_POSITION,
            Eigen::Vector4f(0.3f, 0.5f, 0.8f, 0.0f).normalized().eval().data());

  // light 1 (directional)
  glLightfv(
      GL_LIGHT1, GL_AMBIENT, Eigen::Vector4f(0.0f, 0.0f, 0.0f, 1.0f).eval().data());
  glLightfv(
      GL_LIGHT1, GL_DIFFUSE, Eigen::Vector4f(0.3f, 0.3f, 0.3f, 1.0f).eval().data());
  glLightfv(
      GL_LIGHT1, GL_SPECULAR, Eigen::Vector4f(0.1f, 0.1f, 0.1f, 1.0f).eval().data());
  glLightfv(GL_LIGHT1,
            GL_POSITION,
            Eigen::Vector4f(-0.3f, 0.5f, 0.8f, 0.0f).normalized().eval().data());

  // Material
  glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
  glMaterialfv(
      GL_FRONT, GL_SPECULAR, Eigen::Vector4f(0.1f, 0.1f, 0.1f, 1.0f).eval().data());
  glMaterialf(GL_FRONT, GL_SHININESS, 25.f);

  glEnable(GL_DEPTH_TEST);
  glEnable(GL_NORMALIZE);
  glEnable(GL_COLOR_MATERIAL);
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glEnable(GL_LIGHT1);

  // Projection matrix
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(43., 4. / 3., 0.01 / scaling_factor_, 10. / scaling_factor_);
  glMatrixMode(GL_MODELVIEW);

  // ModelView matrix
  Eigen::Quaterniond R_cam;
  Eigen::Vector3d t_cam;
  {
    std::lock_guard<std::mutex> lock(mutex_vis_);
    R_cam = R_cam_;
    t_cam = t_cam_;
  }

  const Eigen::Vector3d o = Eigen::Vector3d::Zero();
  const Eigen::Vector3d ey = Eigen::Vector3d::UnitY();
  const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ();

  const Eigen::Vector3d eye = R_cam * o + t_cam;
  const Eigen::Vector3d center = R_cam * ez + t_cam;
  const Eigen::Vector3d up = (R_cam * -ey).normalized();

  glMatrixMode(GL_MODELVIEW);
  gluLookAt(eye.x(),
            eye.y(),
            eye.z(),
            center.x(),
            center.y(),
            center.z(),
            up.x(),
            up.y(),
            up.z());

  // Draw everything
  this->drawMeshes();

  glDisable(GL_LIGHTING); // This is needed so the color is right.
  this->drawBox();
}

////////////////////////////////////////////////////////////////////////////////

bool
pcl::ihs::OpenGLViewer::getMeshIsAdded(const std::string& id)
{
  // std::lock_guard<std::mutex> lock (mutex_vis_);
  return (drawn_meshes_.find(id) != drawn_meshes_.end());
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::calcPivot()
{
  std::unique_lock<std::mutex> lock(mutex_vis_);
  if (this->getMeshIsAdded(cam_pivot_id_)) {
    Eigen::Vector4f pivot;
    const FaceVertexMeshConstPtr mesh = drawn_meshes_[cam_pivot_id_];

    if (pcl::compute3DCentroid(mesh->vertices, pivot)) {
      const Eigen::Vector3d p = mesh->transformation * pivot.head<3>().cast<double>();
      lock.unlock();
      this->setPivot(p);
    }
  }
  cam_pivot_id_.clear();
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::drawMeshes()
{
  std::lock_guard<std::mutex> lock(mutex_vis_);

  glEnableClientState(GL_VERTEX_ARRAY);
  glEnableClientState(GL_NORMAL_ARRAY);
  switch (coloring_) {
  case COL_RGB:
    glEnableClientState(GL_COLOR_ARRAY);
    break;
  case COL_ONE_COLOR:
    glDisableClientState(GL_COLOR_ARRAY);
    break;
  case COL_VISCONF:
    glEnableClientState(GL_COLOR_ARRAY);
    break;
  default:
    std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n";
    exit(EXIT_FAILURE);
  }
  switch (mesh_representation_) {
  case MR_POINTS:
    break;
  case MR_EDGES:
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    break;
  case MR_FACES:
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    break;
  default:
    std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n";
    exit(EXIT_FAILURE);
  }

  for (const auto& drawn_mesh : drawn_meshes_) {
    if (drawn_mesh.second && !drawn_mesh.second->vertices.empty()) {
      const FaceVertexMesh& mesh = *drawn_mesh.second;

      glVertexPointer(3, GL_FLOAT, sizeof(PointIHS), &(mesh.vertices[0].x));
      glNormalPointer(GL_FLOAT, sizeof(PointIHS), &(mesh.vertices[0].normal_x));

      Colors colors(3, mesh.vertices.size());

      switch (coloring_) {
      case COL_RGB: {
        glColorPointer(3, GL_UNSIGNED_BYTE, sizeof(PointIHS), &(mesh.vertices[0].b));
        break;
      }
      case COL_ONE_COLOR: {
        glColor3f(.7f, .7f, .7f);
        break;
      }
      case COL_VISCONF: {
        for (std::size_t i = 0; i < mesh.vertices.size(); ++i) {
          const unsigned int n = pcl::ihs::countDirections(mesh.vertices[i].directions);
          const unsigned int index =
              static_cast<unsigned int>(static_cast<float>(colormap_.cols()) *
                                        static_cast<float>(n) / vis_conf_norm_);

          colors.col(i) = colormap_.col(index < 256 ? index : 255);
        }

        glColorPointer(3, GL_UNSIGNED_BYTE, 0, colors.data());
      }
      }

      glPushMatrix();
      {
        glMultMatrixd(mesh.transformation.matrix().data());

        switch (mesh_representation_) {
        case MR_POINTS: {
          glDrawArrays(GL_POINTS, 0, mesh.vertices.size());
          break;
        }
        case MR_EDGES:
        case MR_FACES: {
          glDrawElements(GL_TRIANGLES,
                         3 * mesh.triangles.size(),
                         GL_UNSIGNED_INT,
                         &mesh.triangles[0]);
          break;
        }
        }
      }
      glPopMatrix();
    }
  }

  glDisableClientState(GL_VERTEX_ARRAY);
  glDisableClientState(GL_NORMAL_ARRAY);
  glDisableClientState(GL_COLOR_ARRAY);
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::drawBox()
{
  BoxCoefficients coeffs;
  {
    std::lock_guard<std::mutex> lock(mutex_vis_);
    if (draw_box_)
      coeffs = box_coefficients_;
    else
      return;
  }

  glColor3f(1.f, 1.f, 1.f);

  glPushMatrix();
  {
    glMultMatrixd(coeffs.transformation.matrix().data());

    // Front
    glBegin(GL_LINE_STRIP);
    {
      glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_min);
      glVertex3f(coeffs.x_max, coeffs.y_min, coeffs.z_min);
      glVertex3f(coeffs.x_max, coeffs.y_max, coeffs.z_min);
      glVertex3f(coeffs.x_min, coeffs.y_max, coeffs.z_min);
      glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_min);
    }
    glEnd();

    // Back
    glBegin(GL_LINE_STRIP);
    {
      glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_max);
      glVertex3f(coeffs.x_max, coeffs.y_min, coeffs.z_max);
      glVertex3f(coeffs.x_max, coeffs.y_max, coeffs.z_max);
      glVertex3f(coeffs.x_min, coeffs.y_max, coeffs.z_max);
      glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_max);
    }
    glEnd();

    // Sides
    glBegin(GL_LINES);
    {
      glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_min);
      glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_max);

      glVertex3f(coeffs.x_max, coeffs.y_min, coeffs.z_min);
      glVertex3f(coeffs.x_max, coeffs.y_min, coeffs.z_max);

      glVertex3f(coeffs.x_max, coeffs.y_max, coeffs.z_min);
      glVertex3f(coeffs.x_max, coeffs.y_max, coeffs.z_max);

      glVertex3f(coeffs.x_min, coeffs.y_max, coeffs.z_min);
      glVertex3f(coeffs.x_min, coeffs.y_max, coeffs.z_max);
    }
    glEnd();
  }
  glPopMatrix();
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::initializeGL()
{}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::setupViewport(const int w, const int h)
{
  constexpr float aspect_ratio = 4. / 3.;

  // Use the biggest possible area of the window to draw to
  //    case 1 (w < w_scaled):        case 2 (w >= w_scaled):
  //      w
  //    |---|         ^               |-------------|  ^
  //    |---| ^       |               |    |   |    |  | h
  //    |   | | h_sc  | h             |-------------|  v
  //    |---| v       |                    <---> w_sc
  //    |---|         v               <----- w ----->
  const float w_scaled = h * aspect_ratio;
  const float h_scaled = w / aspect_ratio;

  if (w < w_scaled) {
    glViewport(
        0, static_cast<GLint>((h - h_scaled) / 2.f), w, static_cast<GLsizei>(h_scaled));
  }
  else {
    glViewport(
        static_cast<GLint>((w - w_scaled) / 2.f), 0, static_cast<GLsizei>(w_scaled), h);
  }
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::resizeGL(int w, int h)
{
  this->setupViewport(w, h);
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::mousePressEvent(QMouseEvent* /*event*/)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);
  mouse_pressed_begin_ = true;
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::mouseMoveEvent(QMouseEvent* event)
{
  std::lock_guard<std::mutex> lock(mutex_vis_);

  if (mouse_pressed_begin_) {
    x_prev_ = event->pos().x();
    y_prev_ = event->pos().y();
    mouse_pressed_begin_ = false;
    return;
  }
  if (event->pos().x() == x_prev_ && event->pos().y() == y_prev_)
    return;
  if (this->width() == 0 || this->height() == 0)
    return;

  const double dx =
      static_cast<double>(event->pos().x()) - static_cast<double>(x_prev_);
  const double dy =
      static_cast<double>(event->pos().y()) - static_cast<double>(y_prev_);
  const double w = static_cast<double>(this->width());
  const double h = static_cast<double>(this->height());
  const double d = std::sqrt(w * w + h * h);

  const Eigen::Vector3d o = Eigen::Vector3d::Zero();
  const Eigen::Vector3d ex = Eigen::Vector3d::UnitX();
  const Eigen::Vector3d ey = Eigen::Vector3d::UnitY();
  const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ();

  // Scale with the distance between the pivot and camera eye.
  const double scale =
      std::max((cam_pivot_ - R_cam_ * o - t_cam_).norm(), .1 / scaling_factor_) / d;

  if (QApplication::mouseButtons() == Qt::LeftButton) {
    const double rot_angle = -7. * std::atan(std::sqrt((dx * dx + dy * dy)) / d);
    const Eigen::Vector3d rot_axis = (R_cam_ * ex * dy - R_cam_ * ey * dx).normalized();

    const Eigen::Quaterniond dR(Eigen::AngleAxisd(rot_angle, rot_axis));
    t_cam_ = dR * (t_cam_ - cam_pivot_) + cam_pivot_;
    R_cam_ = (dR * R_cam_).normalized();
  }
  else if (QApplication::mouseButtons() == Qt::MiddleButton) {
    t_cam_ += 1.3 * scale * Eigen::Vector3d(R_cam_ * (ey * -dy + ex * -dx));
  }
  else if (QApplication::mouseButtons() == Qt::RightButton) {
    t_cam_ += 2.6 * scale * Eigen::Vector3d(R_cam_ * (ez * -dy));
  }

  x_prev_ = event->pos().x();
  y_prev_ = event->pos().y();
}

////////////////////////////////////////////////////////////////////////////////

void
pcl::ihs::OpenGLViewer::wheelEvent(QWheelEvent* event)
{
  if (QApplication::mouseButtons() == Qt::NoButton) {
    std::lock_guard<std::mutex> lock(mutex_vis_);

    // Scale with the distance between the pivot and camera eye.
    const Eigen::Vector3d o = Eigen::Vector3d::Zero();
    const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ();
    const double w = static_cast<double>(this->width());
    const double h = static_cast<double>(this->height());
    const double d = std::sqrt(w * w + h * h);
    const double scale =
        std::max((cam_pivot_ - R_cam_ * o - t_cam_).norm(), .1 / scaling_factor_) / d;

    // http://doc.qt.digia.com/qt/qwheelevent.html#delta
    t_cam_ += scale * Eigen::Vector3d(
                          R_cam_ * (ez * static_cast<double>(event->angleDelta().y())));
  }
}

////////////////////////////////////////////////////////////////////////////////
