#include "config.h"

#include <boost/config.hpp>
#if defined(BOOST_MSVC)
#  pragma warning( disable : 4503)
#endif

#include <CGAL/Three/Viewer_interface.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>

#include "Scene_c3t3_item.h"

#include <fstream>
#include <map>
#include <set>
#include <cmath>

#include <QFileInfo>
#include <QFileDialog>
#include <QAction>
#include <QMainWindow>
#include <QColor>
#include <QPixmap>
#include <QBitmap>
#include "ui_Rib_dialog.h"

// Constants
#define CGAL_RIB_NON_TRANSPARENT_MATERIAL_ALPHA 1
using namespace CGAL::Three;

class C3t3_rib_exporter_plugin :
  public QObject,
  protected Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")

public:
  C3t3_rib_exporter_plugin();
  virtual ~C3t3_rib_exporter_plugin() {}

  void init(QMainWindow* mainWindow, Scene_interface* scene_interface, Messages_interface*);
  QList<QAction*> actions() const
  {
    return QList<QAction*>() << actionCreateRib;
  }
  bool applicable(QAction*)const{
    Scene_c3t3_item* item = qobject_cast<Scene_c3t3_item*>(scene->item(scene->mainSelectionIndex()));
    return item && item->is_valid();}

public Q_SLOTS:
  void create_rib();
  void height_changed(int i);
  void width_changed(int i);

private:
  typedef Tr::Bare_point                    Bare_point;
  typedef Tr::Weighted_point                Weighted_point;

  typedef Geom_traits::Vector_3             Vector_3;
  typedef Geom_traits::Plane_3              Plane;
  typedef Geom_traits::FT                   FT;
  typedef Geom_traits::Aff_transformation_3 Aff_transformation_3;

  typedef qglviewer::Vec qglVec;

  enum Rib_exporter_mode { CUT=0, MESH, TRIANGULATION };

  struct Rib_exporter_parameters
  {
    // Materials
    double sphere_radius;
    double cylinder_radius;

    // Lights
    bool ambientOn;
    double ambientIntensity;
    bool shadowOn;
    double shadowIntensity;

    // Picture
    int width;
    int height;
    Rib_exporter_mode mode;
    bool is_preview;
  };

private:
  void update_mask();

  bool get_parameters_from_dialog();

  QStringList nameFilters() const;
  bool save(const Scene_c3t3_item&, const QFileInfo& fileinfo);
  void init_maps(const C3t3& c3t3, const QColor& color);
  void init_point_radius(const C3t3& c3t3);
  void init_parameters();

  Bare_point camera_coordinates(const Bare_point& p);
  void fill_points_and_edges_map(const C3t3& c3t3);

  void add_edge(const Bare_point& p, const Bare_point& q, const QColor& color);
  void add_vertex(const Bare_point& p, const QColor& color);

  void write_header(const std::string& filename, std::ofstream& out);

  void write_lights(std::ofstream& out);
  void write_turn_background_light(bool turn_on, std::ofstream& out);

  void write_facets(const C3t3& c3t3, std::ofstream& out);
  void write_facets(const C3t3& c3t3, const Plane& plane, std::ofstream& out);
  void write_surface_cells(const C3t3& c3t3, const Plane& plane, std::ofstream& out);
  void write_cells_intersecting_a_plane(const C3t3& c3t3, const Plane& plane, std::ofstream& out);
  void write_cells_on_the_positive_side_of_a_plane(const C3t3& c3t3, const Plane& plane, std::ofstream& out);

  void write_triangle(const Bare_point& p, const Bare_point& q, const Bare_point& r,
                      const QColor& color, const QColor& edge_color, std::ofstream& out);
  void write_tetrahedron (const Bare_point& p, const Bare_point& q, const Bare_point& r, const Bare_point& s,
                          const QColor& color, const QColor& edge_color, std::ofstream& out);

  void write_point(const Bare_point& p, std::ofstream& out);
  void write_point_sphere(const Bare_point& p, std::ofstream& out);

  void write_edge_cylinder(const Bare_point& p, const Bare_point& q, std::ofstream& out);

  // Writes data which has been stored during triangle drawing
  void write_edges_flat(std::ofstream& out);
  void write_edges_volumic(std::ofstream& out);
  void write_vertices_volumic(std::ofstream& out);

  void write_color(const QColor& color, bool use_transparency, std::ofstream& out);
  void write_opacity(const double alpha, std::ofstream& out);

  // Background
  void write_background(const QColor& color, std::ofstream& out);

private:
  QAction* actionCreateRib;

  // Viewer
  Viewer_interface* viewer_;

  typedef std::map<C3t3::Surface_patch_index, QColor> Surface_map;
  typedef std::map<C3t3::Subdomain_index, QColor> Subdomain_map;

  Surface_map surface_map_;
  Subdomain_map subdomain_map_;

  typedef std::map<std::pair<Bare_point, Bare_point>,QColor> Edge_map;
  typedef std::map<Bare_point, QColor> Vertex_map;

  Edge_map edges_;
  Vertex_map vertices_;

  double zmax_;
  double diag_;

  // Cache data to avoid writing too much lines in rib file
  QColor prev_color_;
  double prev_alpha_;
  const Scene_c3t3_item* prev_c3t3_;

  Rib_exporter_parameters parameters_;
};

C3t3_rib_exporter_plugin::
C3t3_rib_exporter_plugin()
  : actionCreateRib(NULL)
  , viewer_(NULL)
  , zmax_(0)
  , diag_(0)
  , prev_color_(0,0,0)
  , prev_alpha_(1)
  , prev_c3t3_(NULL)
{

}

void
C3t3_rib_exporter_plugin::
init(QMainWindow* mainWindow, Scene_interface* scene_interface, Messages_interface*)
{
  this->scene = scene_interface;
  this->mw = mainWindow;

  actionCreateRib = new QAction("Export C3t3 to RIB", mw);
  if( NULL != actionCreateRib )
  {
    actionCreateRib->setProperty("subMenuName", "Tetrahedral Mesh Generation");
    connect(actionCreateRib, SIGNAL(triggered()), this, SLOT(create_rib()));
  }

  viewer_ = mw->findChild<Viewer_interface*>("viewer");
  if ( NULL == viewer_ )
  {
    std::cerr << "Can't get QGLViewer" << std::endl;
  }

  init_parameters();
}

void
C3t3_rib_exporter_plugin::create_rib()
{
  if ( NULL == viewer_ )
  {
    std::cerr << "Can't find viewer" << std::endl;
    return;
  }

  // Get Scene_c3t3_item
  Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_c3t3_item* c3t3_item =
    qobject_cast<Scene_c3t3_item*>(scene->item(index));

  if ( NULL == c3t3_item )
  {
    return;
  }

  // Init data
  //if ( c3t3_item != prev_c3t3_ ) // Commented because it was causing problems
                                   // when changing the color of the c3t3
  {
    init_maps(c3t3_item->c3t3(), c3t3_item->color());
    init_point_radius(c3t3_item->c3t3());
    init_parameters();

    prev_c3t3_ = c3t3_item;
  }

  // Get parameters from user dialog
  if ( !get_parameters_from_dialog() )
  {

    QBitmap bitmap;
    bitmap.clear();
    viewer_->setMask(bitmap);
    return;
  }

  // Disable Mask
  QBitmap bitmap;
  bitmap.clear();
  viewer_->setMask(bitmap);

  // Save dialog
  QStringList filters;
  filters << nameFilters();
  filters << tr("All files (*)");

  QString filename = QFileDialog::getSaveFileName(mw,
                                                  tr("Save to File..."),
                                                  QString(),
                                                  filters.join(";;"));

  QFileInfo fileinfo(filename);

  // Save rib file
  save(*c3t3_item,fileinfo);

  std::cout << "Rib file created successfully" << std::endl;
}

void
C3t3_rib_exporter_plugin::
height_changed(int i)
{
  parameters_.height = i;
  update_mask();
}

void
C3t3_rib_exporter_plugin::
width_changed(int i)
{
  parameters_.width = i;
  update_mask();
}

void
C3t3_rib_exporter_plugin::
update_mask()
{
  double ratio = double(parameters_.width) / double(parameters_.height);

  if ( NULL == viewer_ )
  {
    std::cerr << "Can't find viewer..." << std::endl;
    return;
  }
  QBitmap bitmap;
  bitmap.setDevicePixelRatio(ratio);
  bitmap.fill();
  viewer_->setMask(bitmap);
}

bool
C3t3_rib_exporter_plugin::
get_parameters_from_dialog()
{
  QDialog dialog(mw);
  Ui::Rib_dialog ui;
  ui.setupUi(&dialog);

  connect(ui.buttonBox, SIGNAL(accepted()), &dialog, SLOT(accept()));
  connect(ui.buttonBox, SIGNAL(rejected()), &dialog, SLOT(reject()));
  connect(ui.resWidth, SIGNAL(valueChanged(int)), this, SLOT(width_changed(int)));
  connect(ui.resHeight, SIGNAL(valueChanged(int)), this, SLOT(height_changed(int)));

  // -----------------------------------
  // Set data
  // -----------------------------------

  // Materials
  ui.sphereRadius->setValue(parameters_.sphere_radius);
  ui.cylinderRadius->setValue(parameters_.cylinder_radius);

  // Lights
  ui.isAmbientOn->setChecked(parameters_.ambientOn);
  ui.ambientIntensity->setValue(parameters_.ambientIntensity);
  ui.isShadowOn->setChecked(parameters_.shadowOn);
  ui.shadowIntensity->setValue(parameters_.shadowIntensity);

  // Picture
  QStringList mode_list;
  mode_list << "Export Cut (draws current cut view)"
            << "Export Mesh (draws all surface facets)"
            << "Export Triangulation (draws all points and edges)";

  ui.exportMode->insertItems(0,mode_list);

  ui.resWidth->setValue(parameters_.width);
  ui.resHeight->setValue(parameters_.height);
  ui.exportMode->setCurrentIndex(static_cast<int>(parameters_.mode));
  ui.isPreview->setChecked(parameters_.is_preview);

  // Update mask

  update_mask();

  // -----------------------------------
  // Get data
  // -----------------------------------
  int i = dialog.exec();
  if(i == QDialog::Rejected)
    return false;

  // Materials
  parameters_.sphere_radius = ui.sphereRadius->value();
  parameters_.cylinder_radius = ui.cylinderRadius->value();

  // Lights
  parameters_.ambientOn = ui.isAmbientOn->isChecked();
  parameters_.ambientIntensity = ui.ambientIntensity->value();
  parameters_.shadowOn = ui.isShadowOn->isChecked();
  parameters_.shadowIntensity = ui.shadowIntensity->value();

  // Picture
  parameters_.width = ui.resWidth->value();
  parameters_.height = ui.resHeight->value();
  parameters_.mode = static_cast<Rib_exporter_mode>(ui.exportMode->currentIndex());
  parameters_.is_preview = ui.isPreview->isChecked();

  return true;
}

QStringList
C3t3_rib_exporter_plugin::nameFilters() const
{
  return QStringList() << "RenderMan file (*.rib)";
}

bool
C3t3_rib_exporter_plugin::
save(const Scene_c3t3_item& c3t3_item, const QFileInfo& fileInfo)
{
  QString path = fileInfo.absoluteFilePath();
  std::ofstream rib_file (qPrintable(path));
  rib_file.precision(8);

  // Header
  QString basename = fileInfo.baseName();
  write_header(qPrintable(basename), rib_file);

  // Lights
  write_lights(rib_file);

  // Triangles
  switch ( parameters_.mode )
  {
    case CUT:
      rib_file << "Surface \"plastic\" \"Ka\" 0.65 \"Kd\" 0.85 \"Ks\" 0.25 \"roughness\" 0.1" << std::endl;
      write_facets(c3t3_item.c3t3(), c3t3_item.plane(), rib_file);

      rib_file << "Surface \"plastic\" \"Ka\" 0.65 \"Kd\" 0.65 \"Ks\" 0.35 \"roughness\" 0.2" << std::endl;
      //write_cells_intersecting_a_plane(c3t3_item.c3t3(), c3t3_item.plane(), rib_file);
      write_cells_on_the_positive_side_of_a_plane(c3t3_item.c3t3(), c3t3_item.plane(), rib_file);
      break;

    case MESH:
      rib_file << "Surface \"plastic\" \"Ka\" 0.65 \"Kd\" 0.85 \"Ks\" 0.25 \"roughness\" 0.1" << std::endl;
      //write_facets(c3t3_item.c3t3(), rib_file);
      write_surface_cells(c3t3_item.c3t3(), c3t3_item.plane(), rib_file);
      break;

    case TRIANGULATION:
      fill_points_and_edges_map(c3t3_item.c3t3());
      break;

    default:
      std::cerr << "Unexpected mode found" << std::endl;
      return false;
      break;
  }

  // Edges and vertices
  rib_file << "Surface \"plastic\" \"Ka\" 0.65 \"Kd\" 0.85 \"Ks\" 0.25 \"roughness\" 0.1" << std::endl;
  write_edges_volumic(rib_file);
  write_vertices_volumic(rib_file);

  // Background
  write_background(QColor(255,255,255), rib_file);

  rib_file << "WorldEnd" << std::endl;

  return true;
}

void
C3t3_rib_exporter_plugin::init_maps(const C3t3& c3t3, const QColor& color)
{
  surface_map_.clear();
  subdomain_map_.clear();
  edges_.clear();
  vertices_.clear();

  // Fill maps with 0 as value
  for ( C3t3::Facets_in_complex_iterator fit = c3t3.facets_in_complex_begin(),
       fend = c3t3.facets_in_complex_end() ; fit != fend ; ++fit )
  {
    surface_map_.insert(std::make_pair(c3t3.surface_patch_index(*fit),QColor(0,0,0)));
  }

  for ( C3t3::Cells_in_complex_iterator cit = c3t3.cells_in_complex_begin(),
       cend = c3t3.cells_in_complex_end() ; cit != cend ; ++cit )
  {
    subdomain_map_.insert(std::make_pair(c3t3.subdomain_index(cit),QColor(0,0,0)));
  }

  // Fill value of maps
  size_t nb_colors = subdomain_map_.size(); // + surface_map_.size();

  // Starting hue
  double c = color.hueF();
  int i = 0;
  for ( Subdomain_map::iterator it = subdomain_map_.begin(), end = subdomain_map_.end();
       it != end ; ++it, ++i )
  {
    double hue = c + 1./nb_colors * i;
    if ( hue > 1 ) { hue -= 1.; }
    it->second = QColor::fromHsvF(hue, color.saturationF(), color.valueF());
  }
}

void
C3t3_rib_exporter_plugin::
init_point_radius(const C3t3& c3t3)
{
  const CGAL::Bbox_3 bbox = c3t3.bbox();

  const double xdelta = bbox.xmax() - bbox.xmin();
  const double ydelta = bbox.ymax() - bbox.ymin();
  const double zdelta = bbox.zmax() - bbox.zmin();

  diag_ = std::sqrt(xdelta*xdelta + ydelta*ydelta + zdelta*zdelta);

  parameters_.sphere_radius =  diag_ * 0.0015;
  parameters_.cylinder_radius =  diag_ * 0.00065;
}

void
C3t3_rib_exporter_plugin::
init_parameters()
{
  // Lights
  parameters_.ambientOn = true;
  parameters_.ambientIntensity = 0.20;
  parameters_.shadowOn = true;
  parameters_.shadowIntensity = 0.85;

  // Picture
  parameters_.width = 800;
  parameters_.height = 800;
  parameters_.mode = CUT;
  parameters_.is_preview = false;
}

C3t3_rib_exporter_plugin::Bare_point
C3t3_rib_exporter_plugin::
camera_coordinates(const Bare_point& p)
{
  qglVec p_vec ( p.x(), p.y(), p.z() );
  qglVec p_cam = viewer_->camera()->cameraCoordinatesOf(p_vec);

  // Store maximal depth
  zmax_ = (std::max)(zmax_, double(-p_cam[2]));

  return Bare_point(p_cam[0],p_cam[1],p_cam[2]);
}

void
C3t3_rib_exporter_plugin::
fill_points_and_edges_map(const C3t3& c3t3)
{
  Geom_traits::Construct_point_3 wp2p
    = c3t3.triangulation().geom_traits().construct_point_3_object();

  for ( C3t3::Cells_in_complex_iterator it = c3t3.cells_in_complex_begin(),
       end = c3t3.cells_in_complex_end() ; it != end ; ++it )
  {
    const Bare_point& p1 = wp2p(it->vertex(0)->point());
    const Bare_point& p2 = wp2p(it->vertex(1)->point());
    const Bare_point& p3 = wp2p(it->vertex(2)->point());
    const Bare_point& p4 = wp2p(it->vertex(3)->point());

    const QColor& edge_color = subdomain_map_[c3t3.subdomain_index(it)];

    add_edge(p1,p2,edge_color);
    add_edge(p1,p3,edge_color);
    add_edge(p1,p4,edge_color);
    add_edge(p2,p3,edge_color);
    add_edge(p2,p4,edge_color);
    add_edge(p3,p4,edge_color);

    add_vertex(p1,edge_color);
    add_vertex(p2,edge_color);
    add_vertex(p3,edge_color);
    add_vertex(p4,edge_color);
  }
}

void
C3t3_rib_exporter_plugin::
add_edge(const Bare_point& p, const Bare_point& q, const QColor& color)
{
  if ( p < q )
  {
    edges_.insert(std::make_pair(std::make_pair(p,q),color));
  }
  else
  {
    edges_.insert(std::make_pair(std::make_pair(q,p),color));
  }
}

void
C3t3_rib_exporter_plugin::
add_vertex(const Bare_point& p, const QColor& color)
{
  vertices_.insert(std::make_pair(p,color));
}

void
C3t3_rib_exporter_plugin::
write_header(const std::string& filename, std::ofstream& out)
{
  out << "Option \"limits\" \"numthreads\" [16]" << std::endl
      << "Option \"searchpath\" \"shader\" \".:./shaders:%PIXIE_SHADERS%:%PIXIEHOME%/shaders\"" << std::endl;

  if ( ! parameters_.is_preview )
  {
    out << "Attribute \"visibility\" \"specular\" 1" << std::endl
        << "Attribute \"visibility\" \"transmission\" 1" << std::endl << std::endl;
  }

  out << "Display \""<< filename << ".tif\" \"file\" \"rgb\"" << std::endl;

  if ( ! parameters_.is_preview )
  {
    out << "Format " << parameters_.width << " " << parameters_.height << " 1" << std::endl;
  }
  else
  {
    double ratio = double(parameters_.height) / double(parameters_.width);

    int width = (ratio < 1.)  ? 300               : int(300. / ratio);
    int height = (ratio < 1.) ? int(ratio * 300.) : 300;

    out << "Format " << width << " " << height << " 1" << std::endl;
  }

  if ( parameters_.width > parameters_.height )
  {
    double ratio = double(parameters_.height) / double(parameters_.width);
    out << "ScreenWindow -1 1 " << -ratio << " " << ratio << std::endl;
  }
  else if ( parameters_.height > parameters_.width )
  {
    double ratio = double(parameters_.width) / double(parameters_.height);
    out << "ScreenWindow " << -ratio << " " << ratio << " -1 1" << std::endl;
  }

  out << "Projection \"perspective\" \"fov\" 45" << std::endl
      << "PixelSamples 4 4" << std::endl
      << "PixelFilter \"catmull-rom\" 3 3" << std::endl
      << "ShadingInterpolation \"smooth\"" << std::endl
      << "Rotate 180 0 0 1" << std::endl
      << "WorldBegin" << std::endl;
}

void
C3t3_rib_exporter_plugin::
write_lights(std::ofstream& out)
{
  if ( ! parameters_.is_preview )
  {
    // ShadowLight
    out << "LightSource \"shadowdistant\" 1 \"from\" [0 0 0] \"to\" [0 0 1]"
        << " \"shadowname\" \"raytrace\" \"intensity\" " << parameters_.shadowIntensity << std::endl;

    // Ambient light
    out << "LightSource \"ambientlight\" 2 \"intensity\" " << parameters_.ambientIntensity << std::endl;
  }
  else
  {
    out << "LightSource \"distantLight\" 1 \"from\" [0 0 0] \"to\" [0 0 1]"
    << " \"intensity\" 0.85" << std::endl;
  }

  // Background light
  out << "LightSource \"ambientlight\" 99 \"intensity\" 1" << std::endl;
  write_turn_background_light(false,out);
}

void
C3t3_rib_exporter_plugin::
write_turn_background_light(bool turn_on, std::ofstream& out)
{
  if (!turn_on)
  {
    out << "Illuminate 1 1" << std::endl;
    if ( ! parameters_.is_preview ) { out << "Illuminate 2 1" << std::endl; }
    out << "Illuminate 99 0" << std::endl;
  }
  else
  {
    out << "Illuminate 1 0" << std::endl;
    if ( ! parameters_.is_preview ) { out << "Illuminate 2 0" << std::endl; }
    out << "Illuminate 99 1" << std::endl;
  }
}

void
C3t3_rib_exporter_plugin::
write_facets(const C3t3& c3t3, std::ofstream& out)
{
  Geom_traits::Construct_point_3 wp2p
    = c3t3.triangulation().geom_traits().construct_point_3_object();

  for ( C3t3::Facets_in_complex_iterator it = c3t3.facets_in_complex_begin(),
       end = c3t3.facets_in_complex_end() ; it != end ; ++it )
  {
    const C3t3::Cell_handle& c = it->first;
    const int& k = it->second;

    const Bare_point& p1 = wp2p(c->vertex((k+1)&3)->point());
    const Bare_point& p2 = wp2p(c->vertex((k+2)&3)->point());
    const Bare_point& p3 = wp2p(c->vertex((k+3)&3)->point());

    QColor color = c3t3.is_in_complex(c) ? subdomain_map_[c3t3.subdomain_index(c)]
    : subdomain_map_[c3t3.subdomain_index(c->neighbor(k))];

    write_triangle(p1, p2, p3, color, color.darker(125), out );
  }
}

void
C3t3_rib_exporter_plugin::
write_facets(const C3t3& c3t3, const Plane& plane, std::ofstream& out)
{
  typedef Kernel::Oriented_side Side;

  Geom_traits::Construct_point_3 wp2p
    = c3t3.triangulation().geom_traits().construct_point_3_object();

  for ( C3t3::Facets_in_complex_iterator it = c3t3.facets_in_complex_begin(),
       end = c3t3.facets_in_complex_end() ; it != end ; ++it )
  {
    const C3t3::Cell_handle& c = it->first;
    const int& k = it->second;

    const Bare_point& p1 = wp2p(c->vertex((k+1)&3)->point());
    const Bare_point& p2 = wp2p(c->vertex((k+2)&3)->point());
    const Bare_point& p3 = wp2p(c->vertex((k+3)&3)->point());

    const Side s1 = plane.oriented_side(p1);
    const Side s2 = plane.oriented_side(p2);
    const Side s3 = plane.oriented_side(p3);

    if(   s1 == CGAL::ON_NEGATIVE_SIDE && s2 == CGAL::ON_NEGATIVE_SIDE
       && s3 == CGAL::ON_NEGATIVE_SIDE )
    {
      QColor color = c3t3.is_in_complex(c) ? subdomain_map_[c3t3.subdomain_index(c)]
                                           : subdomain_map_[c3t3.subdomain_index(c->neighbor(k))];

      write_triangle(p1, p2, p3, color, color.darker(125), out );
    }
  }
}

void
C3t3_rib_exporter_plugin::
write_surface_cells(const C3t3& c3t3, const Plane& /* plane */, std::ofstream& out)
{
  Geom_traits::Construct_point_3 wp2p
    = c3t3.triangulation().geom_traits().construct_point_3_object();

  for ( C3t3::Cells_in_complex_iterator it_cell = c3t3.cells_in_complex_begin(),
       end = c3t3.cells_in_complex_end() ; it_cell != end ; ++it_cell )
  {
    C3t3::Cell_handle c = it_cell;

    // Compute the number of surface vertices in c
    // Keep one of them in memory
    int num_2D_vertices = 0;
    int last_2D_vertex_index = -1;
    for (int i = 0 ; i < 4 ; ++i)
    {
      if (c3t3.in_dimension(c->vertex(i)) == 2)
      {
        ++num_2D_vertices;
        last_2D_vertex_index = i;
      }
    }

    //const int TRANSPARENCY_ALPHA_VALUE = 100;
    CGAL::Bbox_3 bbox = c3t3.bbox();
    float relPos = (c->weighted_circumcenter().x() - bbox.xmin())
                   / (bbox.xmax() - bbox.xmin());
    float TRANSPARENCY_ALPHA_VALUE =
      1.f -
      (relPos < 0.25f ?
        0.0f :
        (relPos > 0.75f ?
          1.0f :
          (relPos - 0.25f)*1.0f/0.5f
        )
      );

    /*
    // ONLY SURFACE FACETS ARE TRANSPARENT
    if (num_2D_vertices >= 3)
    {
      QColor basecolor = subdomain_map_[c3t3.subdomain_index(c)];
      QColor facecolor = basecolor.darker(150);
      QColor edgecolor = facecolor.darker(150);

      for (int i = 0 ; i < 4 ; ++i)
      {
        if (c3t3.in_dimension(c->vertex((i+1)%4)) == 2
            && c3t3.in_dimension(c->vertex((i+2)%4)) == 2
            && c3t3.in_dimension(c->vertex((i+3)%4)) == 2)
        {
          edgecolor.setAlpha(TRANSPARENCY_ALPHA_VALUE);
        }
        else
        {
          edgecolor.setAlpha(255);
        }

        write_triangle(wp2p(c->vertex((i+1)%4)->point()),
                       wp2p(c->vertex((i+2)%4)->point()),
                       wp2p(c->vertex((i+3)%4)->point()),
                       facecolor, edgecolor, out );
      }
    }*/

    // SURFACE CELLS ARE TRANSPARENT
    if (num_2D_vertices >= 2)
    {
      QColor basecolor = subdomain_map_[c3t3.subdomain_index(c)];
      QColor facecolor = basecolor.darker(150);
      QColor edgecolor = facecolor.darker(150);

      /*
      typedef Kernel::Oriented_side Side;

      // Transparency on the negative side of the plane
      const Side s0 = plane.oriented_side(wp2p(c->vertex(0)->point()));
      const Side s1 = plane.oriented_side(wp2p(c->vertex(1)->point()));
      const Side s2 = plane.oriented_side(wp2p(c->vertex(2)->point()));
      const Side s3 = plane.oriented_side(wp2p(c->vertex(3)->point()));
      if(   s0 == CGAL::ON_NEGATIVE_SIDE && s1 == CGAL::ON_NEGATIVE_SIDE
         && s2 == CGAL::ON_NEGATIVE_SIDE && s3 == CGAL::ON_NEGATIVE_SIDE )
      {
        edgecolor.setAlpha(TRANSPARENCY_ALPHA_VALUE);
      }
      else
      {
        edgecolor.setAlpha(255);
      }*/

      edgecolor.setAlphaF(TRANSPARENCY_ALPHA_VALUE);

      for (int i = 0 ; i < 4 ; ++i)
      {
        write_triangle(wp2p(c->vertex((i+1)%4)->point()),
                       wp2p(c->vertex((i+2)%4)->point()),
                       wp2p(c->vertex((i+3)%4)->point()),
                       facecolor, edgecolor, out );
      }
    }
    else if (num_2D_vertices == 1)
    {
      QColor basecolor = subdomain_map_[c3t3.subdomain_index(c)];
      QColor facecolor = basecolor.darker(150);
      QColor edgecolor = facecolor.darker(150);

      for (int i = 0 ; i < 4 ; ++i)
      {
        if (i == last_2D_vertex_index)
        {
          edgecolor.setAlpha(TRANSPARENCY_ALPHA_VALUE);
        }
        else
        {
          edgecolor.setAlphaF(TRANSPARENCY_ALPHA_VALUE);
        }

        write_triangle(wp2p(c->vertex((i+1)%4)->point()),
                       wp2p(c->vertex((i+2)%4)->point()),
                       wp2p(c->vertex((i+3)%4)->point()),
                       facecolor, edgecolor, out );
      }
    }
  }
}

void
C3t3_rib_exporter_plugin::
write_cells_intersecting_a_plane(const C3t3& c3t3, const Plane& plane, std::ofstream& out)
{
  typedef Kernel::Oriented_side Side;

  Geom_traits::Construct_point_3 wp2p
    = c3t3.triangulation().geom_traits().construct_point_3_object();

  for ( C3t3::Cells_in_complex_iterator it = c3t3.cells_in_complex_begin(),
       end = c3t3.cells_in_complex_end() ; it != end ; ++it )
  {
    const Bare_point& p1 = wp2p(it->vertex(0)->point());
    const Bare_point& p2 = wp2p(it->vertex(1)->point());
    const Bare_point& p3 = wp2p(it->vertex(2)->point());
    const Bare_point& p4 = wp2p(it->vertex(3)->point());

    const Side s1 = plane.oriented_side(p1);
    const Side s2 = plane.oriented_side(p2);
    const Side s3 = plane.oriented_side(p3);
    const Side s4 = plane.oriented_side(p4);

    if(   s1 == CGAL::ON_ORIENTED_BOUNDARY || s2 == CGAL::ON_ORIENTED_BOUNDARY
       || s3 == CGAL::ON_ORIENTED_BOUNDARY || s4 == CGAL::ON_ORIENTED_BOUNDARY
       || s2 != s1 || s3 != s1 || s4 != s1 )
    {
      QColor basecolor = subdomain_map_[c3t3.subdomain_index(it)];
      QColor facecolor = basecolor.darker(150);
      QColor edgecolor = facecolor.darker(150);

      edgecolor.setAlpha(20);

      // Don't write facet twice
      if ( s1 != CGAL::ON_NEGATIVE_SIDE || s2 != CGAL::ON_NEGATIVE_SIDE || s3 != CGAL::ON_NEGATIVE_SIDE )
        write_triangle(p1, p2, p3, facecolor, edgecolor, out );

      if ( s1 != CGAL::ON_NEGATIVE_SIDE || s2 != CGAL::ON_NEGATIVE_SIDE || s4 != CGAL::ON_NEGATIVE_SIDE )
        write_triangle(p1, p2, p4, facecolor, edgecolor, out );

      if ( s1 != CGAL::ON_NEGATIVE_SIDE || s3 != CGAL::ON_NEGATIVE_SIDE || s4 != CGAL::ON_NEGATIVE_SIDE )
        write_triangle(p1, p3, p4, facecolor, edgecolor, out );

      if ( s2 != CGAL::ON_NEGATIVE_SIDE || s3 != CGAL::ON_NEGATIVE_SIDE || s4 != CGAL::ON_NEGATIVE_SIDE )
        write_triangle(p2, p3, p4, facecolor, edgecolor, out );
    }
  }
}

void
C3t3_rib_exporter_plugin::
write_cells_on_the_positive_side_of_a_plane(const C3t3& c3t3, const Plane& plane, std::ofstream& out)
{
  typedef Kernel::Oriented_side Side;

  Geom_traits::Construct_point_3 wp2p
    = c3t3.triangulation().geom_traits().construct_point_3_object();

  for ( C3t3::Cells_in_complex_iterator it = c3t3.cells_in_complex_begin(),
       end = c3t3.cells_in_complex_end() ; it != end ; ++it )
  {
    const Bare_point& p1 = wp2p(it->vertex(0)->point());
    const Bare_point& p2 = wp2p(it->vertex(1)->point());
    const Bare_point& p3 = wp2p(it->vertex(2)->point());
    const Bare_point& p4 = wp2p(it->vertex(3)->point());

    const Side s1 = plane.oriented_side(p1);
    const Side s2 = plane.oriented_side(p2);
    const Side s3 = plane.oriented_side(p3);
    const Side s4 = plane.oriented_side(p4);

    if( (   s1 == CGAL::ON_POSITIVE_SIDE || s2 == CGAL::ON_POSITIVE_SIDE
         || s3 == CGAL::ON_POSITIVE_SIDE || s4 == CGAL::ON_POSITIVE_SIDE )
        /*&&
        (   c3t3.surface_patch_index(it, 0) != C3t3::Surface_patch_index()
         || c3t3.surface_patch_index(it, 1) != C3t3::Surface_patch_index()
         || c3t3.surface_patch_index(it, 2) != C3t3::Surface_patch_index()
         || c3t3.surface_patch_index(it, 3) != C3t3::Surface_patch_index() )*/
    )
    {
      QColor basecolor = subdomain_map_[c3t3.subdomain_index(it)];
      QColor facecolor = basecolor.darker(150);
      QColor edgecolor = facecolor.darker(150);

      edgecolor.setAlpha(10);

      // Don't write facet twice
      if ( s1 != CGAL::ON_NEGATIVE_SIDE || s2 != CGAL::ON_NEGATIVE_SIDE || s3 != CGAL::ON_NEGATIVE_SIDE )
        write_triangle(p1, p2, p3, facecolor, edgecolor, out );

      if ( s1 != CGAL::ON_NEGATIVE_SIDE || s2 != CGAL::ON_NEGATIVE_SIDE || s4 != CGAL::ON_NEGATIVE_SIDE )
        write_triangle(p1, p2, p4, facecolor, edgecolor, out );

      if ( s1 != CGAL::ON_NEGATIVE_SIDE || s3 != CGAL::ON_NEGATIVE_SIDE || s4 != CGAL::ON_NEGATIVE_SIDE )
        write_triangle(p1, p3, p4, facecolor, edgecolor, out );

      if ( s2 != CGAL::ON_NEGATIVE_SIDE || s3 != CGAL::ON_NEGATIVE_SIDE || s4 != CGAL::ON_NEGATIVE_SIDE )
        write_triangle(p2, p3, p4, facecolor, edgecolor, out );
    }
  }
}

void
C3t3_rib_exporter_plugin::
write_triangle (const Bare_point& p, const Bare_point& q, const Bare_point& r,
                const QColor& color, const QColor& edge_color, std::ofstream& out)
{
  // Color
  write_color(color, true, out);

  // Triangle
  out << "Polygon \"P\" [";
  write_point(p,out);
  write_point(q,out);
  write_point(r,out);
  out << "]" << std::endl;

  // Edges (will be drawn later on)
  add_edge(p,q,edge_color);
  add_edge(p,r,edge_color);
  add_edge(q,r,edge_color);

  // Vertices (will be drawn later on)
  add_vertex(p,edge_color);
  add_vertex(q,edge_color);
  add_vertex(r,edge_color);
}

void
C3t3_rib_exporter_plugin::
write_tetrahedron (const Bare_point& p, const Bare_point& q, const Bare_point& r, const Bare_point& s,
                   const QColor& color, const QColor& edge_color, std::ofstream& out)
{
  // Color
  write_color(color, true, out);

  // Triangle
  out << "Polygon \"P\" [";
  write_point(p,out);
  write_point(q,out);
  write_point(r,out);
  write_point(s,out);
  out << "]" << std::endl;

  // Edges (will be drawn later on)
  add_edge(p,q,edge_color);
  add_edge(p,r,edge_color);
  add_edge(q,r,edge_color);
  add_edge(s,p,edge_color);
  add_edge(s,q,edge_color);
  add_edge(s,r,edge_color);

  // Vertices (will be drawn later on)
  add_vertex(p,edge_color);
  add_vertex(q,edge_color);
  add_vertex(r,edge_color);
  add_vertex(s,edge_color);
}

void
C3t3_rib_exporter_plugin::
write_point (const Bare_point& p, std::ofstream& out)
{
  // Transform point in camera coordinates
  const Bare_point& p_cam = camera_coordinates(p);

  // Write it
  out << " " << -p_cam.x() << " " << -p_cam.y() << " " << -p_cam.z() << " ";
}

void
C3t3_rib_exporter_plugin::
write_point_sphere(const Bare_point& p, std::ofstream& out)
{
  // Transform point in camera coordinates
  const Bare_point& p_cam = camera_coordinates(p);

  // radius
  const double& r = parameters_.sphere_radius;

  out << "Translate " << -p_cam.x() << " " << -p_cam.y() << " " << -p_cam.z() << std::endl;

  // Sphere radius zmin zmax thetamax
  out << "Sphere " << r << " " << -r << " " << r << " 360" << std::endl;
  out << "Identity" << std::endl;
}

void
C3t3_rib_exporter_plugin::
write_edge_cylinder(const Bare_point& p, const Bare_point& q, std::ofstream& out)
{
  // Transform point in camera coordinates
  const Bare_point& p_cam = camera_coordinates(p);
  const Bare_point& q_cam = camera_coordinates(q);

  double pq = CGAL::to_double(CGAL::sqrt(CGAL::squared_distance(p_cam,q_cam)));

  Aff_transformation_3 t (CGAL::Translation(), Vector_3(p_cam,CGAL::ORIGIN));
  const Bare_point& q_cam_t = q_cam.transform(t);

  Vector_3 Oq (CGAL::ORIGIN,q_cam_t);
  Vector_3 Oz (FT(0),FT(0),FT(1));

  Vector_3 r_axis = CGAL::cross_product(Oq,Oz);
  double cos_angle = CGAL::to_double((Oq*Oz)/CGAL::sqrt(Oq.squared_length()));
  double angle = std::acos(cos_angle) * 180. / CGAL_PI;

  // radius
  const double& r = parameters_.cylinder_radius;

  out << "Translate " << -p_cam.x() << " " << -p_cam.y() << " " << -p_cam.z() << std::endl;
  out << "Rotate " << (angle+180.) << " " << -r_axis.x() << " " << -r_axis.y() << " " << -r_axis.z() << std::endl;

  // Cylinder radius zmin zmax thetamax
  out << "Cylinder " << r << " 0 " << pq << " 360" << std::endl;
  out << "Identity" << std::endl;
}

void
C3t3_rib_exporter_plugin::
write_edges_flat(std::ofstream& out)
{
  // Lights
  write_turn_background_light(true,out);

  out << "Surface \"constant\"" << std::endl;
  write_opacity(CGAL_RIB_NON_TRANSPARENT_MATERIAL_ALPHA, out);

  // Translation
  out << "Translate 0 0 -0.1" << std::endl;

  for ( Edge_map::iterator it = edges_.begin(), end = edges_.end() ;
       it != end ; ++it )
  {
    // Color
    write_color(it->second, true, out);

    // Edge
    out << "Curves \"linear\" [2] \"nonperiodic\" \"P\" [";
    write_point(it->first.first,out);
    write_point(it->first.second,out);
    out << "] \"constantwidth\" [0.15]" << std::endl;
  }
}

void
C3t3_rib_exporter_plugin::
write_edges_volumic(std::ofstream& out)
{
  // Material
  write_opacity(CGAL_RIB_NON_TRANSPARENT_MATERIAL_ALPHA, out);

  for ( Edge_map::iterator it = edges_.begin(), end = edges_.end() ;
       it != end ; ++it )
  {
    // Color
    write_color(it->second, true, out);
    // Edge
    write_edge_cylinder(it->first.first, it->first.second, out);
  }
}

void
C3t3_rib_exporter_plugin::
write_vertices_volumic(std::ofstream& out)
{
  // Material
  write_opacity(CGAL_RIB_NON_TRANSPARENT_MATERIAL_ALPHA, out);

  for ( Vertex_map::iterator it = vertices_.begin(), end = vertices_.end() ;
       it != end ; ++it )
  {
    // Color
    write_color(it->second, false, out);
    // Vertex
    write_point_sphere(it->first, out);
  }
}

void
C3t3_rib_exporter_plugin::
write_color(const QColor& color, bool use_transparency, std::ofstream& out)
{
  if ( prev_color_ == color )
  {
    return;
  }

  // Cache data
  prev_color_ = color;

  // Write opacity data
  if (use_transparency)
  {
    write_opacity(color.alphaF(),out);
  }

  // Write color data
  out << "Color [ " << color.redF() << " " << color.greenF() << " "
      << color.blueF() <<  " ]" << std::endl;
}

void
C3t3_rib_exporter_plugin::
write_opacity(const double alpha, std::ofstream& out)
{
  if ( alpha == prev_alpha_ )
  {
    return;
  }

  // Cache data
  prev_alpha_ = alpha;

  // Write opacity data
  out << "Opacity " << alpha << " " << alpha << " " << alpha << std::endl;
}

void
C3t3_rib_exporter_plugin::
write_background(const QColor& color, std::ofstream& out)
{
  write_turn_background_light(false,out);

  out << "Surface \"constant\"" << std::endl;
  write_color(color,false,out);

  double corner = zmax_ * 2.;
  double depth_pos = zmax_ * 1.6;

  out << "Polygon \"P\" [";
  out << " " << -corner << " " << -corner << " " << depth_pos << " ";
  out << " " <<  corner << " " << -corner << " " << depth_pos << " ";
  out << " " <<  corner << " " <<  corner << " " << depth_pos << " ";
  out << " " << -corner << " " <<  corner << " " << depth_pos << " ";
  out << "]" << std::endl;
}

#include "C3t3_rib_exporter_plugin.moc"


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

#include "apps-precomp.h"  // Precompiled headers
//
#include <mrpt/apps/MonteCarloLocalization_App.h>
#include <mrpt/bayes/CParticleFilter.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/gui/CDisplayWindowPlots.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimpleMap.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/math/distributions.h>
#include <mrpt/math/ops_vectors.h>	// << for vector<>
#include <mrpt/math/utils.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/opengl/CDisk.h>
#include <mrpt/opengl/CEllipsoid2D.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose2DInterpolator.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/slam/CMonteCarloLocalization2D.h>
#include <mrpt/slam/CMonteCarloLocalization3D.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <Eigen/Dense>

using namespace mrpt::apps;

// ---------------------------------------
//   MonteCarloLocalization_Base
// ---------------------------------------
MonteCarloLocalization_Base::MonteCarloLocalization_Base()
{
	// Set logger display name:
	this->setLoggerName("MonteCarloLocalization_Base");
}

void MonteCarloLocalization_Base::initialize(int argc, const char** argv)
{
	MRPT_START

	MRPT_LOG_INFO_FMT(
		" pf-localization - Part of the MRPT\n"
		" MRPT C++ Library: %s - Sources timestamp: %s\n\n",
		mrpt::system::MRPT_getVersion().c_str(),
		mrpt::system::MRPT_getCompilationDate().c_str());

	// Process arguments:
	if (argc < 2)
	{ THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str()); }

	// Config file:
	const std::string configFile = std::string(argv[1]);

	ASSERT_FILE_EXISTS_(configFile);
	params.setContent(mrpt::io::file_get_contents(configFile));

	impl_initialize(argc, argv);

	MRPT_END
}

// All these "using"s, to easily reuse old code from the pf-localization app:
using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::opengl;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace mrpt::bayes;
using namespace mrpt::obs;
using namespace mrpt::io;
using namespace mrpt::img;
using namespace mrpt::config;
using namespace mrpt::serialization;
using namespace std;

// Auxiliary trait classes for template implementations below:
template <class PDF>
struct pf2gauss_t
{
};

template <>
struct pf2gauss_t<mrpt::slam::CMonteCarloLocalization2D>
{
	using type = mrpt::poses::CPosePDFGaussian;
	static constexpr bool PF_IS_3D = false;

	static void resetOnFreeSpace(
		CMonteCarloLocalization2D& pdf, mrpt::maps::CMultiMetricMap& metricMap,
		size_t PARTICLE_COUNT, const mrpt::math::TPose3D& init_min,
		const mrpt::math::TPose3D& init_max)
	{
		pdf.resetUniformFreeSpace(
			metricMap.mapByClass<COccupancyGridMap2D>().get(), 0.7f,
			PARTICLE_COUNT, init_min.x, init_max.x, init_min.y, init_max.y,
			init_min.yaw, init_max.yaw);
	}

	static void resetUniform(
		CMonteCarloLocalization2D& pdf, size_t PARTICLE_COUNT,
		const mrpt::math::TPose3D& init_min,
		const mrpt::math::TPose3D& init_max)
	{
		pdf.resetUniform(
			init_min.x, init_max.x, init_min.y, init_max.y, init_min.yaw,
			init_max.yaw, PARTICLE_COUNT);
	}
};
template <>
struct pf2gauss_t<CMonteCarloLocalization3D>
{
	using type = CPose3DPDFGaussian;
	static constexpr bool PF_IS_3D = true;

	static void resetOnFreeSpace(
		[[maybe_unused]] CMonteCarloLocalization3D& pdf,
		[[maybe_unused]] mrpt::maps::CMultiMetricMap& metricMap,
		[[maybe_unused]] size_t PARTICLE_COUNT,
		[[maybe_unused]] const mrpt::math::TPose3D& init_min,
		[[maybe_unused]] const mrpt::math::TPose3D& init_max)
	{
		THROW_EXCEPTION("init_PDF_mode=0 not supported for 3D particles");
	}

	static void resetUniform(
		CMonteCarloLocalization3D& pdf, size_t PARTICLE_COUNT,
		const mrpt::math::TPose3D& init_min,
		const mrpt::math::TPose3D& init_max)
	{
		pdf.resetUniform(init_min, init_max, PARTICLE_COUNT);
	}
};

template <class MONTECARLO_TYPE>
void MonteCarloLocalization_Base::do_pf_localization()
{
	auto& cfg = this->params;  // rename for reusing old code

	// Number of initial particles (if size>1, run the experiments N times)
	std::vector<int> particles_count;

	// Load parameters:

	// Mandatory entries:
	cfg.read_vector(
		sect, "particles_count", std::vector<int>(1, 0), particles_count,
		/*Fail if not found*/ true);
	string OUT_DIR_PREFIX =
		cfg.read_string(sect, "logOutput_dir", "", /*Fail if not found*/ true);

	// Non-mandatory entries:
	string MAP_FILE = cfg.read_string(sect, "map_file", "");
	size_t rawlog_offset = cfg.read_int(sect, "rawlog_offset", 0);
	string GT_FILE = cfg.read_string(sect, "ground_truth_path_file", "");
	const auto NUM_REPS = cfg.read_uint64_t(sect, "experimentRepetitions", 1);
	int SCENE3D_FREQ = cfg.read_int(sect, "3DSceneFrequency", 10);
	bool SCENE3D_FOLLOW = cfg.read_bool(sect, "3DSceneFollowRobot", true);
	unsigned int testConvergenceAt =
		cfg.read_int(sect, "experimentTestConvergenceAtStep", -1);

	bool SAVE_STATS_ONLY = cfg.read_bool(sect, "SAVE_STATS_ONLY", false);
	bool DO_RELIABILITY_ESTIMATE = false;
	bool DO_SCAN_LIKELIHOOD_DEBUG = false;
	MRPT_LOAD_CONFIG_VAR(DO_RELIABILITY_ESTIMATE, bool, cfg, sect);
	MRPT_LOAD_CONFIG_VAR(DO_SCAN_LIKELIHOOD_DEBUG, bool, cfg, sect);

	bool SHOW_PROGRESS_3D_REAL_TIME =
		cfg.read_bool(sect, "SHOW_PROGRESS_3D_REAL_TIME", false);
	int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS =
		cfg.read_int(sect, "SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS", 1);
	double STATS_CONF_INTERVAL =
		cfg.read_double(sect, "STATS_CONF_INTERVAL", 0.2);

	CPose2D initial_odo;
	initial_odo.x(cfg.read_double(sect, "initial_odo_x", 0));
	initial_odo.y(cfg.read_double(sect, "initial_odo_y", 0));
	initial_odo.phi(cfg.read_double(sect, "initial_odo_phi", 0));

#if !MRPT_HAS_WXWIDGETS
	SHOW_PROGRESS_3D_REAL_TIME = false;
#endif

	// Default odometry uncertainty parameters in "actOdom2D_params" depending
	// on how fast the robot moves, etc...
	//  Only used for observations-only rawlogs:
	CActionRobotMovement2D::TMotionModelOptions actOdom2D_params;
	actOdom2D_params.modelSelection = CActionRobotMovement2D::mmGaussian;
	actOdom2D_params.gaussianModel.minStdXY =
		cfg.read_double("DummyOdometryParams", "minStdXY", 0.04);
	actOdom2D_params.gaussianModel.minStdPHI =
		DEG2RAD(cfg.read_double("DummyOdometryParams", "minStdPHI", 2.0));

	CActionRobotMovement3D::TMotionModelOptions actOdom3D_params;
	actOdom3D_params.mm6DOFModel.additional_std_XYZ =
		cfg.read_double("DummyOdometryParams", "additional_std_XYZ", 0.01);
	actOdom3D_params.mm6DOFModel.additional_std_angle = DEG2RAD(
		cfg.read_double("DummyOdometryParams", "additional_std_angle", 0.1));

	// PF-algorithm Options:
	// ---------------------------
	CParticleFilter::TParticleFilterOptions pfOptions;
	pfOptions.loadFromConfigFile(cfg, "PF_options");

	// PDF Options:
	// ------------------
	TMonteCarloLocalizationParams pdfPredictionOptions;
	pdfPredictionOptions.KLD_params.loadFromConfigFile(cfg, "KLD_options");

	// Metric map options:
	// -----------------------------
	TSetOfMetricMapInitializers mapList;
	mapList.loadFromConfigFile(cfg, "MetricMap");

	MRPT_LOG_INFO_STREAM(
		"" << endl
		   << "MAP_FILE       : " << MAP_FILE << endl
		   << "GT_FILE        : " << GT_FILE << endl
		   << "OUT_DIR_PREFIX : " << OUT_DIR_PREFIX << endl
		   << "particles_count :" << particles_count << endl);

	// --------------------------------------------------------------------
	//						EXPERIMENT PREPARATION
	// --------------------------------------------------------------------
	CTicTac tictac;
	CSimpleMap simpleMap;
	CParticleFilter::TParticleFilterStats PF_stats;

	// Load the set of metric maps to consider in the experiments:
	auto metricMap = CMultiMetricMap::Create();
	metricMap->setListOfMaps(mapList);

	{
		std::stringstream ss;
		pfOptions.dumpToTextStream(ss);
		mapList.dumpToTextStream(ss);

		MRPT_LOG_INFO_STREAM(ss.str());
	}

	getRandomGenerator().randomize();

	// Load the map (if any):
	// -------------------------
	if (MAP_FILE.size())
	{
		ASSERT_(fileExists(MAP_FILE));

		// Detect file extension:
		// -----------------------------
		string mapExt = lowerCase(extractFileExtension(
			MAP_FILE, true));  // Ignore possible .gz extensions

		if (!mapExt.compare("simplemap"))
		{
			// It's a ".simplemap":
			// -------------------------
			MRPT_LOG_INFO_STREAM("Loading '.simplemap' file: " << MAP_FILE);
			{
				CFileGZInputStream f(MAP_FILE);
				archiveFrom(f) >> simpleMap;
			}
			MRPT_LOG_INFO_STREAM(
				"Map loaded ok, with " << simpleMap.size() << " frames.");

			ASSERT_(simpleMap.size() > 0);

			// Build metric map:
			// ------------------------------
			MRPT_LOG_INFO("Building metric map(s) from '.simplemap'...");
			metricMap->loadFromProbabilisticPosesAndObservations(simpleMap);
			MRPT_LOG_INFO("Done.");
		}
		else if (!mapExt.compare("gridmap"))
		{
			// It's a ".gridmap":
			// -------------------------
			MRPT_LOG_INFO("Loading gridmap from '.gridmap'...");
			auto grid = metricMap->mapByClass<COccupancyGridMap2D>();
			ASSERT_(grid);
			{
				CFileGZInputStream f(MAP_FILE);
				archiveFrom(f) >> (*grid);
			}
			MRPT_LOG_INFO("Done.");
		}
		else
		{
			THROW_EXCEPTION_FMT(
				"Map file has unknown extension: '%s'", mapExt.c_str());
		}
	}

	// Load the Ground Truth:
	CMatrixDouble GT(0, 0);
	if (fileExists(GT_FILE))
	{
		MRPT_LOG_INFO("Loading ground truth file...");
		GT.loadFromTextFile(GT_FILE);
		MRPT_LOG_INFO("Done.");

		prepareGT(GT);
	}
	else
		MRPT_LOG_INFO("Ground truth file: not available.");

	// PDF initialization uniform distribution limits:
	const auto init_min = mrpt::math::TPose3D(
		cfg.read_double(sect, "init_PDF_min_x", 0),
		cfg.read_double(sect, "init_PDF_min_y", 0),
		cfg.read_double(sect, "init_PDF_min_z", 0),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_yaw_deg", -180.0)),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_pitch_deg", 0)),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_roll_deg", 0)));

	const auto init_max = mrpt::math::TPose3D(
		cfg.read_double(sect, "init_PDF_max_x", 0),
		cfg.read_double(sect, "init_PDF_max_y", 0),
		cfg.read_double(sect, "init_PDF_max_z", 0),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_yaw_deg", +180.0)),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_pitch_deg", 0)),
		mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_roll_deg", 0)));

	MRPT_LOG_INFO_STREAM(
		"Initial PDF limits:\n Min=" << init_min.asString()
									 << "\n Max=" << init_max.asString());

	// Gridmap / area of initial uncertainty:
	COccupancyGridMap2D::TEntropyInfo gridInfo;
	if (auto grid = metricMap->mapByClass<COccupancyGridMap2D>(); grid)
	{
		grid->computeEntropy(gridInfo);
		MRPT_LOG_INFO_FMT(
			"The gridmap has %.04fm2 observed area, %u observed cells.",
			gridInfo.effectiveMappedArea,
			(unsigned)gridInfo.effectiveMappedCells);
	}
	else
	{
		gridInfo.effectiveMappedArea =
			(init_max.x - init_min.x) * (init_max.y - init_min.y);
	}

	for (int PARTICLE_COUNT : particles_count)
	{
		MRPT_LOG_INFO_FMT(
			"Initial PDF: %f particles/m2",
			PARTICLE_COUNT / gridInfo.effectiveMappedArea);

		// Global stats for all the experiment loops:
		int nConvergenceTests = 0, nConvergenceOK = 0;
		CVectorDouble convergenceErrors;
		std::mutex convergenceErrors_mtx;

		// --------------------------------------------------------------------
		//					EXPERIMENT REPETITIONS LOOP
		// --------------------------------------------------------------------

		// Generate list of repetition indices:
		std::vector<unsigned int> rep_indices(NUM_REPS);
		std::iota(rep_indices.begin(), rep_indices.end(), 0);

		// Run each repetition in parallel:
		// GCC 7 still has not implemented C++17 for_each(std::execution::par
		// So: home-made parallel loops:

		// std::for_each(std::execution::par, rep_indices.begin(),
		// rep_indices.end(),
		auto run_localization_code = [&](const size_t repetition) {
			CVectorDouble indivConvergenceErrors, executionTimes, odoError;
			MRPT_LOG_INFO_STREAM(
				"====== RUNNING FOR " << PARTICLE_COUNT
									  << " INITIAL PARTICLES  - Repetition "
									  << 1 + repetition << " / " << NUM_REPS);

			// Create 3D window if requested:
			CDisplayWindow3D::Ptr win3D;
			if (SHOW_PROGRESS_3D_REAL_TIME)
			{
				win3D = std::make_shared<CDisplayWindow3D>(
					"pf-localization - The MRPT project", 1000, 600);
				win3D->setCameraAzimuthDeg(-45);
			}

			// Create the 3D scene and get the map only once, later we'll modify
			// only the particles, etc..
			COpenGLScene scene;
			if (SCENE3D_FREQ > 0 || SHOW_PROGRESS_3D_REAL_TIME)
			{
				mrpt::math::TBoundingBoxf bbox({-50, -50, 0}, {50, 50, 0});
				if (auto pts = metricMap->getAsSimplePointsMap(); pts)
					bbox = pts->boundingBox();

				scene.insert(mrpt::opengl::CGridPlaneXY::Create(
					bbox.min.x, bbox.max.x, bbox.min.y, bbox.max.y, 0, 5));

				if (win3D)
					win3D->setCameraZoom(
						2 *
						std::max(
							bbox.max.x - bbox.min.x, bbox.max.y - bbox.min.y));

				scene.insert(metricMap->getVisualization());
			}

			// The experiment directory is:
			string sOUT_DIR_PARTS, sOUT_DIR_3D;
			const auto sOUT_DIR = format(
				"%s_%03u_%07i", OUT_DIR_PREFIX.c_str(),
				static_cast<unsigned int>(repetition), PARTICLE_COUNT);
			MRPT_LOG_INFO_FMT("Creating directory: %s", sOUT_DIR.c_str());
			deleteFilesInDirectory(sOUT_DIR);
			createDirectory(sOUT_DIR);
			ASSERT_DIRECTORY_EXISTS_(sOUT_DIR);

			if (!SAVE_STATS_ONLY)
			{
				sOUT_DIR_PARTS = format("%s/particles", sOUT_DIR.c_str());
				sOUT_DIR_3D = format("%s/3D", sOUT_DIR.c_str());

				MRPT_LOG_INFO_FMT(
					"Creating directory: %s", sOUT_DIR_PARTS.c_str());
				deleteFilesInDirectory(sOUT_DIR_PARTS);
				createDirectory(sOUT_DIR_PARTS);
				ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_PARTS);

				MRPT_LOG_INFO_FMT(
					"Creating directory: %s", sOUT_DIR_3D.c_str());
				deleteFilesInDirectory(sOUT_DIR_3D);
				createDirectory(sOUT_DIR_3D);
				ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_3D);

				using namespace std::string_literals;
				metricMap->saveMetricMapRepresentationToFile(
					sOUT_DIR + "/map"s);
			}

			MONTECARLO_TYPE pdf;

			// PDF Options:
			pdf.options = pdfPredictionOptions;

			pdf.options.metricMap = metricMap;

			// Create the PF object:
			CParticleFilter PF;
			PF.m_options = pfOptions;

			size_t step = 0;
			size_t rawlogEntry = 0;

			// Initialize the PDF:
			// -----------------------------
			tictac.Tic();
			if (!cfg.read_bool(
					sect, "init_PDF_mode", false,
					/*Fail if not found*/ true))
			{
				// Reset uniform on free space:
				pf2gauss_t<MONTECARLO_TYPE>::resetOnFreeSpace(
					pdf, *metricMap, PARTICLE_COUNT, init_min, init_max);
			}
			else
			{
				// Reset uniform:
				pf2gauss_t<MONTECARLO_TYPE>::resetUniform(
					pdf, PARTICLE_COUNT, init_min, init_max);
			}

			MRPT_LOG_INFO_FMT(
				"PDF of %u particles initialized in %.03fms", PARTICLE_COUNT,
				1000 * tictac.Tac());

			pdf.saveToTextFile(
				format("%s/particles_0_initial.txt", sOUT_DIR_PARTS.c_str()));

			// -----------------------------
			//		Particle filter
			// -----------------------------
			CPose2D odometryEstimation = initial_odo;

			using PDF_MEAN_TYPE = typename MONTECARLO_TYPE::type_value;

			PDF_MEAN_TYPE pdfEstimation;

			bool end = false;

			CFileOutputStream f_cov_est, f_pf_stats, f_odo_est;

			if (!SAVE_STATS_ONLY)
			{
				f_cov_est.open(sOUT_DIR.c_str() + string("/cov_est.txt"));
				f_pf_stats.open(sOUT_DIR.c_str() + string("/PF_stats.txt"));
				f_odo_est.open(sOUT_DIR.c_str() + string("/odo_est.txt"));
			}

			Clock::time_point cur_obs_timestamp;
			CPose2D last_used_abs_odo(0, 0, 0),
				pending_most_recent_odo(0, 0, 0);

			for (; !end; step++)
			{
				// Finish if ESC is pushed:
				if (allow_quit_on_esc_key && os::kbhit())
					if (os::getch() == 27)
					{
						end = true;
						break;
					}

				// Load pose change from the rawlog:
				// ----------------------------------------
				CActionCollection::Ptr action;
				CSensoryFrame::Ptr observations;
				CObservation::Ptr obs;

				if (!impl_get_next_observations(action, observations, obs))
				{
					// EOF
					end = true;
					break;
				}

				// Determine if we are reading a Act-SF or an Obs-only
				// rawlog:
				if (obs)
				{
					// It's an observation-only rawlog: build an auxiliary
					// pair of action-SF, since
					//  montecarlo-localization only accepts those pairs as
					//  input:

					// If it's an odometry reading, don't feed it to the PF.
					// Instead,
					// store its value for use as an "action" together with
					// the next actual observation:
					if (IS_CLASS(*obs, CObservationOdometry))
					{
						auto obs_odo =
							std::dynamic_pointer_cast<CObservationOdometry>(
								obs);
						pending_most_recent_odo = obs_odo->odometry;
						static bool is_1st_odo = true;
						if (is_1st_odo)
						{
							is_1st_odo = false;
							last_used_abs_odo = pending_most_recent_odo;
						}
						continue;
					}
					else
					{
						// SF: Just one observation:
						// ------------------------------------------------------
						observations = std::make_shared<CSensoryFrame>();
						observations->insert(obs);

						// ActionCollection: Just one action with a dummy
						// odometry
						// ------------------------------------------------------
						action = std::make_shared<CActionCollection>();

						if (pf2gauss_t<MONTECARLO_TYPE>::PF_IS_3D)
						{
							CActionRobotMovement3D actOdom3D;

							const CPose3D odo_incr = CPose3D(
								pending_most_recent_odo - last_used_abs_odo);
							last_used_abs_odo = pending_most_recent_odo;

							actOdom3D.computeFromOdometry(
								odo_incr, actOdom3D_params);
							action->insert(actOdom3D);
						}
						else
						{
							CActionRobotMovement2D actOdom2D;

							const CPose2D odo_incr =
								pending_most_recent_odo - last_used_abs_odo;
							last_used_abs_odo = pending_most_recent_odo;

							actOdom2D.computeFromOdometry(
								odo_incr, actOdom2D_params);
							action->insert(actOdom2D);
						}
					}
				}
				else
				{
					// Already in Act-SF format, nothing else to do!
				}

				CPose2D expectedPose;  // Ground truth

				if (observations->size() > 0)
					cur_obs_timestamp =
						observations->getObservationByIndex(0)->timestamp;
				else if (obs)
					cur_obs_timestamp = obs->timestamp;

				// Test for end condition if we are testing convergence:
				if (step == testConvergenceAt)
				{
					nConvergenceTests++;

					// Convergence??
					if (sqrt(pdf.getCovariance().det()) < 2)
					{
						if (pdfEstimation.distanceTo(expectedPose) < 2)
							nConvergenceOK++;
					}
					end = true;
				}

				if (step < rawlog_offset) continue;

				// Do not execute the PF at "step=0", to let the initial
				// PDF to be reflected in the logs.
				if (step > rawlog_offset)
				{
					// Show 3D?
					if (SHOW_PROGRESS_3D_REAL_TIME)
					{
						const auto [cov, meanPose] = pdf.getCovarianceAndMean();

						if (rawlogEntry >= 2)
							getGroundTruth(
								expectedPose, rawlogEntry - 2, GT,
								cur_obs_timestamp);

						// The particles' cov:
						{
							CRenderizable::Ptr ellip =
								scene.getByName("parts_cov");
							if (!ellip)
							{
								if (pf2gauss_t<MONTECARLO_TYPE>::PF_IS_3D)
								{
									auto el = CEllipsoid3D::Create();
									ellip =
										mrpt::ptr_cast<CRenderizable>::from(el);
									el->setLineWidth(2);
									el->setQuantiles(3);
									el->enableDrawSolid3D(false);
								}
								else
								{
									auto el = CEllipsoid2D::Create();
									ellip =
										mrpt::ptr_cast<CRenderizable>::from(el);
									el->setLineWidth(2);
									el->setQuantiles(3);
									el->enableDrawSolid3D(false);
								}
								ellip->setName("parts_cov");
								ellip->setColor(1, 0, 0, 0.6);
								scene.insert(ellip);
							}
							else
							{
								if (pf2gauss_t<MONTECARLO_TYPE>::PF_IS_3D)
								{
									mrpt::ptr_cast<CEllipsoid3D>::from(ellip)
										->setCovMatrix(
											cov.template blockCopy<3, 3>());
								}
								else
								{
									mrpt::ptr_cast<CEllipsoid2D>::from(ellip)
										->setCovMatrix(
											cov.template blockCopy<2, 2>());
								}
							}
							double ellipse_z =
								mrpt::poses::CPose3D(meanPose).z() + 0.01;

							ellip->setLocation(
								meanPose.x(), meanPose.y(), ellipse_z);
						}

						COpenGLScene::Ptr ptrSceneWin =
							win3D->get3DSceneAndLock();

						win3D->setCameraPointingToPoint(
							meanPose.x(), meanPose.y(), 0);

						win3D->addTextMessage(
							10, 10,
							mrpt::format(
								"timestamp: %s",
								mrpt::system::dateTimeLocalToString(
									cur_obs_timestamp)
									.c_str()),
							6001);

						win3D->addTextMessage(
							10, 33,
							mrpt::format(
								"#particles= %7u",
								static_cast<unsigned int>(pdf.size())),
							6002);

						win3D->addTextMessage(
							10, 55,
							mrpt::format(
								"mean pose (x y phi_deg)= %s",
								meanPose.asString().c_str()),
							6003);

						*ptrSceneWin = scene;
						win3D->unlockAccess3DScene();

						// Update:
						win3D->forceRepaint();

						std::this_thread::sleep_for(std::chrono::milliseconds(
							SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
					}  // end show 3D real-time

					// ----------------------------------------
					// RUN ONE STEP OF THE PARTICLE FILTER:
					// ----------------------------------------
					tictac.Tic();
					if (!SAVE_STATS_ONLY)
					{
						MRPT_LOG_INFO_STREAM(
							"Step " << step << ". Executing ParticleFilter on "
									<< pdf.particlesCount() << " particles...");
					}

					PF.executeOn(
						pdf,
						action.get(),  // Action
						observations.get(),	 // Obs.
						&PF_stats  // Output statistics
					);

					double run_time = tictac.Tac();
					executionTimes.push_back(run_time);
					if (!SAVE_STATS_ONLY)
					{
						MRPT_LOG_INFO_FMT(
							"Done in %.03fms, ESS=%f", 1e3 * run_time,
							pdf.ESS());
					}
				}

				// Avrg. error:
				// ----------------------------------------
				CActionRobotMovement2D::Ptr best_mov_estim =
					action->getBestMovementEstimation();
				if (best_mov_estim)
				{
					odometryEstimation = odometryEstimation +
						best_mov_estim->poseChange->getMeanVal();
				}

				pdf.getMean(pdfEstimation);

				// save estimated mean in history:
				if (cur_obs_timestamp != INVALID_TIMESTAMP &&
					fill_out_estimated_path)
				{
					out_estimated_path.insert(
						cur_obs_timestamp,
						mrpt::math::TPose3D(pdfEstimation.asTPose()));
				}

				getGroundTruth(
					expectedPose, rawlogEntry, GT, cur_obs_timestamp);

				if (expectedPose.x() != 0 || expectedPose.y() != 0 ||
					expectedPose.phi() != 0)
				{  // Averaged error to GT
					double sumW = 0;
					double locErr = 0;
					for (size_t k = 0; k < pdf.size(); k++)
						sumW += exp(pdf.getW(k));
					for (size_t k = 0; k < pdf.size(); k++)
					{
						const auto pk = pdf.getParticlePose(k);
						locErr += mrpt::hypot_fast(
									  expectedPose.x() - pk.x,
									  expectedPose.y() - pk.y) *
							exp(pdf.getW(k)) / sumW;
					}
					convergenceErrors_mtx.lock();
					convergenceErrors.push_back(locErr);
					convergenceErrors_mtx.unlock();

					indivConvergenceErrors.push_back(locErr);
					odoError.push_back(
						expectedPose.distanceTo(odometryEstimation));
				}

				const auto [cov, meanPose] = pdf.getCovarianceAndMean();
				const auto current_pdf_gaussian =
					typename pf2gauss_t<MONTECARLO_TYPE>::type(meanPose, cov);

				// Text output:
				// ----------------------------------------
				if (!SAVE_STATS_ONLY)
				{
					MRPT_LOG_INFO_STREAM(
						"Odometry estimation: " << odometryEstimation);
					MRPT_LOG_INFO_STREAM(
						"PDF estimation: "
						<< pdfEstimation << ", ESS (B.R.)= "
						<< PF_stats.ESS_beforeResample << " tr(cov): "
						<< std::sqrt(current_pdf_gaussian.cov.trace()));

					if (GT.rows() > 0)
						MRPT_LOG_INFO_STREAM("Ground truth: " << expectedPose);
				}

				// Evaluate the "reliability" of the pose estimation
				// (for now, only for 2D laser scans + grid maps)
				double obs_reliability_estim = .0;
				if (DO_RELIABILITY_ESTIMATE)
				{
					// We need: a gridmap & a 2D LIDAR:
					CObservation2DRangeScan::Ptr obs_scan;
					if (observations)
						obs_scan = observations->getObservationByClass<
							CObservation2DRangeScan>(0);  // Get the 0'th
					// scan, if
					// several are
					// present.
					COccupancyGridMap2D::Ptr gridmap =
						metricMap->mapByClass<COccupancyGridMap2D>();
					if (obs_scan && gridmap)  // We have both, go on:
					{
						// Simulate scan + uncertainty:
						COccupancyGridMap2D::TLaserSimulUncertaintyParams
							ssu_params;
						COccupancyGridMap2D::TLaserSimulUncertaintyResult
							ssu_out;
						ssu_params.method = COccupancyGridMap2D::sumUnscented;
						// ssu_params.UT_alpha = 0.99;
						// obs_scan->stdError = 0.07;
						// obs_scan->maxRange = 10.0;

						ssu_params.robotPose =
							CPosePDFGaussian(current_pdf_gaussian);
						ssu_params.aperture = obs_scan->aperture;
						ssu_params.rangeNoiseStd = obs_scan->stdError;
						ssu_params.nRays = obs_scan->getScanSize();
						ssu_params.rightToLeft = obs_scan->rightToLeft;
						ssu_params.sensorPose = obs_scan->sensorPose;
						ssu_params.maxRange = obs_scan->maxRange;

						gridmap->laserScanSimulatorWithUncertainty(
							ssu_params, ssu_out);

						// Evaluate reliability:
						CObservation2DRangeScanWithUncertainty::TEvalParams
							evalParams;
						// evalParams.prob_outliers = 0.40;
						// evalParams.max_prediction_std_dev = 1.0;
						obs_reliability_estim =
							ssu_out.scanWithUncert.evaluateScanLikelihood(
								*obs_scan, evalParams);

						if (DO_SCAN_LIKELIHOOD_DEBUG)
						{
							static mrpt::gui::CDisplayWindowPlots win;

							std::vector<float> ranges_mean, ranges_obs;
							for (size_t i = 0; i < obs_scan->getScanSize(); i++)
							{
								ranges_mean.push_back(
									ssu_out.scanWithUncert.rangeScan
										.getScanRange(i));
								ranges_obs.push_back(obs_scan->getScanRange(i));
							}

							win.plot(ranges_mean, "3k-", "mean");
							win.plot(ranges_obs, "r-", "obs");

							Eigen::VectorXd ci1 =
								ssu_out.scanWithUncert.rangesMean.asEigen() +
								3 *
									ssu_out.scanWithUncert.rangesCovar.asEigen()
										.diagonal()
										.array()
										.sqrt()
										.matrix();
							Eigen::VectorXd ci2 =
								ssu_out.scanWithUncert.rangesMean.asEigen() -
								3 *
									ssu_out.scanWithUncert.rangesCovar.asEigen()
										.diagonal()
										.array()
										.sqrt()
										.matrix();
							win.plot(ci1, "k-", "CI+");
							win.plot(ci2, "k-", "CI-");

							win.setWindowTitle(mrpt::format(
								"obs_reliability_estim: %f",
								obs_reliability_estim));
							win.axis_fit();
						}
					}
					MRPT_LOG_INFO_STREAM(
						"Reliability measure [0-1]: " << obs_reliability_estim);
				}

				if (!SAVE_STATS_ONLY)
				{
					f_cov_est.printf("%e\n", sqrt(cov.det()));
					f_pf_stats.printf(
						"%u %e %e %f %f\n", (unsigned int)pdf.size(),
						PF_stats.ESS_beforeResample,
						PF_stats.weightsVariance_beforeResample,
						obs_reliability_estim,
						sqrt(current_pdf_gaussian.cov.det()));
					f_odo_est.printf(
						"%f %f %f\n", odometryEstimation.x(),
						odometryEstimation.y(), odometryEstimation.phi());
				}

				if ((!SAVE_STATS_ONLY && SCENE3D_FREQ > 0) ||
					SHOW_PROGRESS_3D_REAL_TIME)
				{
					// Generate 3D scene:
					// ------------------------------

					// The Ground Truth (GT):
					if (GT.rows() > 0)
					{
						CRenderizable::Ptr GTpt = scene.getByName("GT");
						if (!GTpt)
						{
							GTpt = std::make_shared<CDisk>();
							GTpt = std::make_shared<CDisk>();
							GTpt->setName("GT");
							GTpt->setColor(0, 0, 0, 0.9);

							mrpt::ptr_cast<CDisk>::from(GTpt)->setDiskRadius(
								0.04f);
							scene.insert(GTpt);
						}

						GTpt->setPose(expectedPose);
					}

					// The particles:
					{
						CRenderizable::Ptr parts = scene.getByName("particles");
						if (parts) scene.removeObject(parts);

						auto p =
							pdf.template getAs3DObject<CSetOfObjects::Ptr>();
						p->setName("particles");
						scene.insert(p);
					}

					// The laser scan:
					{
						CRenderizable::Ptr scanPts = scene.getByName("scan");
						if (!scanPts)
						{
							scanPts = std::make_shared<CPointCloud>();
							scanPts->setName("scan");
							scanPts->setColor(1, 0, 0, 0.9);
							mrpt::ptr_cast<CPointCloud>::from(scanPts)
								->enableColorFromZ(false);
							mrpt::ptr_cast<CPointCloud>::from(scanPts)
								->setPointSize(4);
							scene.insert(scanPts);
						}

						CSimplePointsMap map;

						CPose3D robotPose3D(meanPose);

						map.clear();
						observations->insertObservationsInto(map);

						mrpt::ptr_cast<CPointCloud>::from(scanPts)
							->loadFromPointsMap(&map);
						mrpt::ptr_cast<CPointCloud>::from(scanPts)->setPose(
							robotPose3D);
					}

					// The camera:
					scene.enableFollowCamera(SCENE3D_FOLLOW);

					// Views:
					COpenGLViewport::Ptr view1 = scene.getViewport("main");
					{
						CCamera& cam = view1->getCamera();
						cam.setAzimuthDegrees(-90);
						cam.setElevationDegrees(90);
						cam.setPointingAt(meanPose);
						cam.setZoomDistance(5);
						cam.setOrthogonal();
					}
				}

				if (!SAVE_STATS_ONLY && SCENE3D_FREQ != -1 &&
					((step + 1) % SCENE3D_FREQ) == 0)
				{
					// Save 3D scene:
					CFileGZOutputStream f(format(
						"%s/progress_%05u.3Dscene", sOUT_DIR_3D.c_str(),
						(unsigned)step));
					archiveFrom(f) << scene;

					// Generate text files for matlab:
					// ------------------------------------
					pdf.saveToTextFile(format(
						"%s/particles_%05u.txt", sOUT_DIR_PARTS.c_str(),
						(unsigned)step));
				}

			};	// while rawlogEntries

			indivConvergenceErrors.saveToTextFile(sOUT_DIR + "/GT_error.txt");
			odoError.saveToTextFile(sOUT_DIR + "/ODO_error.txt");
			executionTimes.saveToTextFile(sOUT_DIR + "/exec_times.txt");

			if (win3D && NUM_REPS == 1) mrpt::system::pause();
		};	// for repetitions

		CTicTac tictacGlobal;
		tictacGlobal.Tic();

		const auto max_num_threads = std::thread::hardware_concurrency();
		size_t runs_per_thread = NUM_REPS;
		if (max_num_threads > 1)
			runs_per_thread = static_cast<size_t>(
				std::ceil((NUM_REPS) / static_cast<double>(max_num_threads)));

		MRPT_LOG_INFO_STREAM(
			"Running " << NUM_REPS << " repetitions, on max " << max_num_threads
					   << " parallel threads: " << runs_per_thread
					   << " runs/thread.");

		std::vector<std::thread> running_tasks;
		for (size_t r = 0; r < NUM_REPS; r += runs_per_thread)
		{
			auto runner = [&](size_t i_start, size_t i_end) {
				if (i_end > NUM_REPS) i_end = NUM_REPS;	 // sanity check
				for (size_t i = i_start; i < i_end; i++)
					run_localization_code(i);
			};

			running_tasks.emplace_back(runner, r, r + runs_per_thread);
			std::this_thread::sleep_for(std::chrono::milliseconds(100));
		}

		// Wait for all threads to end:
		for (auto& t : running_tasks)
			if (t.joinable()) t.join();

		double repetitionTime = tictacGlobal.Tac();

		// Avr. error:
		double covergenceErrorMean = 0, convergenceErrorsMin = 0,
			   convergenceErrorsMax = 0;
		if (!convergenceErrors.empty())
			math::confidenceIntervals(
				convergenceErrors, covergenceErrorMean, convergenceErrorsMin,
				convergenceErrorsMax, STATS_CONF_INTERVAL);

		// Save overall results:
		{
			CFileOutputStream f(
				format("%s_SUMMARY.txt", OUT_DIR_PREFIX.c_str()),
				OpenMode::APPEND);

			f.printf(
				"%% Ratio_covergence_success  #particles  "
				"average_time_per_execution  convergence_mean_error "
				"convergence_error_conf_int_inf "
				"convergence_error_conf_int_sup "
				"\n");
			if (!nConvergenceTests) nConvergenceTests = 1;
			f.printf(
				"%f %u %f %f %f %f\n",
				((double)nConvergenceOK) / nConvergenceTests, PARTICLE_COUNT,
				repetitionTime / NUM_REPS, covergenceErrorMean,
				convergenceErrorsMin, convergenceErrorsMax);
		}

		MRPT_LOG_INFO_FMT("Total execution time: %.06f sec", repetitionTime);

	}  // end of loop for different # of particles
}

void MonteCarloLocalization_Base::getGroundTruth(
	mrpt::poses::CPose2D& expectedPose, size_t rawlogEntry,
	const mrpt::math::CMatrixDouble& GT, const Clock::time_point& cur_time)
{
	// Either:
	// - time x y phi
	// or
	// - time x y z yaw pitch roll

	if (GT.cols() == 4 || GT.cols() == 7)
	{
		bool GT_index_is_time;

		// First column can be: timestamps, or rawlogentries:
		//  Auto-figure it out:
		if (GT.rows() > 2)
		{
			GT_index_is_time =
				floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
		}
		else
		{
			GT_index_is_time = false;
		}

		if (GT_index_is_time)
		{
			// Look for the timestamp:
			using namespace std::chrono_literals;

			bool interp_ok = false;
			GT_path.interpolate(cur_time, expectedPose, interp_ok);
			if (!interp_ok)
			{
				/*
				cerr << format(
					"GT time not found: %f\n",
					mrpt::system::timestampTotime_t(cur_time));
					*/
			}
		}
		else
		{
			// Look for the rawlogEntry:
			size_t k, N = GT.rows();
			for (k = 0; k < N; k++)
			{
				if (GT(k, 0) == rawlogEntry) break;
			}

			if (k < N)
			{
				expectedPose.x(GT(k, 1));
				expectedPose.y(GT(k, 2));
				expectedPose.phi(GT(k, 3));
			}
		}
	}
	else if (GT.cols() == 3)
	{
		if ((int)rawlogEntry < GT.rows())
		{
			expectedPose.x(GT(rawlogEntry, 0));
			expectedPose.y(GT(rawlogEntry, 1));
			expectedPose.phi(GT(rawlogEntry, 2));
		}
	}
	else if (GT.cols() > 0)
		THROW_EXCEPTION("Unexpected number of columns in ground truth file");
}

void MonteCarloLocalization_Base::prepareGT(const mrpt::math::CMatrixDouble& GT)
{
	// Either:
	// - time x y phi
	// or
	// - time x y z yaw pitch roll
	if (GT.cols() == 4 || GT.cols() == 7)
	{
		const bool GT_is_3D = (GT.cols() == 7);
		bool GT_index_is_time = false;

		// First column can be: timestamps, or rawlogentries:
		//  Auto-figure it out:
		if (GT.rows() > 2)
		{
			GT_index_is_time =
				floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
		}

		if (GT_index_is_time)
		{
			// Look for the timestamp:
			using namespace std::chrono_literals;
			GT_path.setMaxTimeInterpolation(200ms);

			for (int i = 0; i < GT.rows(); i++)
			{
				GT_path.insert(
					mrpt::Clock::fromDouble(GT(i, 0)),
					TPose2D(GT(i, 1), GT(i, 2), GT(i, GT_is_3D ? 4 : 3)));
			}
		}
	}
}

void MonteCarloLocalization_Base::run()
{
	MRPT_START

	// Detect 2D vs 3D particle filter?
	const bool is_3D = params.read_bool(sect, "use_3D_poses", false);

	if (is_3D)
	{
		MRPT_LOG_INFO("Running for: CMonteCarloLocalization3D");
		do_pf_localization<CMonteCarloLocalization3D>();
	}
	else
	{
		MRPT_LOG_INFO("Running for: CMonteCarloLocalization2D");
		do_pf_localization<CMonteCarloLocalization2D>();
	}

	MRPT_END
}

// ---------------------------------------
//   MonteCarloLocalization_Rawlog
// ---------------------------------------
MonteCarloLocalization_Rawlog::MonteCarloLocalization_Rawlog()
{
	setLoggerName("MonteCarloLocalization_Rawlog");
}

void MonteCarloLocalization_Rawlog::impl_initialize(int argc, const char** argv)
{
	MRPT_START
	// Rawlog file: from args. line or from config file:
	if (argc == 3) m_rawlogFileName = std::string(argv[2]);
	else
		m_rawlogFileName = params.read_string(
			sect, "rawlog_file", std::string("log.rawlog"), true);

	m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0);

	ASSERT_FILE_EXISTS_(m_rawlogFileName);

	MRPT_END
}


#include <hotplate/hotplate.h>
#include <hotplate/build_info.h>
#include <boost/program_options.hpp>
#include <iostream>

namespace bpo = boost::program_options;

constexpr int HOTPLATE_START_TEMP = 288;
constexpr int WATER_START_TEMP = 288;
constexpr int WATER_MELT_TEMP = 273;
constexpr int WATER_BOIL_TEMP = 373;

auto operator<<(std::ostream& out, hotplate::substance_state state) -> std::ostream&
{
	switch (state)
	{
	case hotplate::substance_state::solid:
		out << "solid";
		break;
	case hotplate::substance_state::liquid:
		out << "liquid";
		break;
	case hotplate::substance_state::gas:
		out << "gas";
		break;
	default:
		out << "unknown";
		break;
	}
	return out;
}

auto main(int argc, char** argv) -> int
{
	// describe positional arguments
	bpo::positional_options_description pos_desc;
	pos_desc.add("name", -1);

	// describe command line options
	int start_temp = -1;
	bpo::options_description opt_desc("hotplate options");
	auto add_arg = opt_desc.add_options();
	add_arg("help,h",
	        "Write this help description");
	add_arg("version,v",
	        "print version information and exit");
	add_arg("start-temp,s",
	        bpo::value<int>(&start_temp)->default_value(0),
	        "Starting temperature");

	// parse command line
	bpo::command_line_parser parser(argc, argv);
	parser.options(opt_desc);
	parser.positional(pos_desc);
	bpo::variables_map opt_vars;
	bpo::store(parser.run(), opt_vars);
	bpo::notify(opt_vars);

	if (opt_vars.count("help")) {
		std::cout << opt_desc << '\n';
		return 0;
	}

	if (opt_vars.count("version")) {
		std::cout << "hotplate version: " << hotplate::build_info::SEMVER << '\n';
		return 0;
	}

	// --------
	std::cout << "Hello, hotplate!\n";
	hotplate::hotplate hp{ HOTPLATE_START_TEMP };
	hotplate::substance water{ WATER_START_TEMP, WATER_MELT_TEMP, WATER_BOIL_TEMP };
	hp.heat_substance(water);
	int final_temp = start_temp + 400;
	for (int temp = start_temp; temp <= final_temp; temp += 50) {
		hp.set_temperature(temp);
		std::cout << "Hotplate temperature " << temp << ". "
		          << "Water is " << water.get_state() << ".\n";
	}
}


#pragma once

// for the newly defined pointtype
#define PCL_NO_PRECOMPILE
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <boost/shared_ptr.hpp>
#include <pcl/impl/point_types.hpp>

namespace pcl {
  template <unsigned int NUM_CLASS>
  struct PointSegmentedDistribution
  {
    PCL_ADD_POINT4D;
    PCL_ADD_RGB;
    int   label;
    float label_distribution[NUM_CLASS];   // templated on any number of classes. TODO
    //float label_distribution[14];
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
  } EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment

}

namespace SegmentationMapping {

  template <unsigned int NUM_CLASS, typename PointWithXYZRGB>
  void PointSeg_to_PointXYZRGB(const pcl::PointCloud<pcl::PointSegmentedDistribution<NUM_CLASS>> & pc_seg,
                               typename pcl::PointCloud<PointWithXYZRGB> & pc_rgb) {
    pc_rgb.resize(pc_seg.size());
    for (int i = 0; i < pc_rgb.size(); i++) {
      auto & p_rgb = pc_rgb[i];
      auto & p_seg = pc_seg[i];

      p_rgb.x = p_seg.x;
      p_rgb.y = p_seg.y;
      p_rgb.z = p_seg.z;
      p_rgb.r = p_seg.r;
      p_rgb.g = p_seg.g;
      p_rgb.b = p_seg.b;

    }
    pc_rgb.header = pc_seg.header;
  }

}


/*
@copyright Louis Dionne 2014
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/ext/std/tuple.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/integral.hpp>

#include <tuple>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto tuple = [](auto ...xs) {
    return std::make_tuple(xs...);
};

int main() {
    BOOST_HANA_CONSTEXPR_ASSERT(flatten(tuple(tuple(), tuple())) == tuple());
    BOOST_HANA_CONSTEXPR_ASSERT(flatten(tuple(tuple(int_<0>), tuple())) == tuple(int_<0>));
    BOOST_HANA_CONSTEXPR_ASSERT(flatten(tuple(tuple(), tuple(int_<0>))) == tuple(int_<0>));
    BOOST_HANA_CONSTEXPR_ASSERT(flatten(tuple(tuple(int_<0>), tuple(int_<1>))) == tuple(int_<0>, int_<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(
        flatten(tuple(tuple(0, int_<1>), tuple(), tuple('2', 3.3), tuple(int_<4>)))
        ==
        tuple(0, int_<1>, '2', 3.3, int_<4>)
    );
}


/*
    Copyright 2013 Adobe
    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*/
/**************************************************************************************************/

#ifndef ADOBE_ALGORITHM_RANDOM_SHUFFLE_HPP
#define ADOBE_ALGORITHM_RANDOM_SHUFFLE_HPP

#include <adobe/config.hpp>

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>

#include <algorithm>
#include <functional>

/**************************************************************************************************/

namespace adobe {

/**************************************************************************************************/
/*!
\defgroup random_shuffle random_shuffle
\ingroup mutating_algorithm

\see
    - STL documentation for \ref stldoc_random_shuffle
*/
/**************************************************************************************************/
/*!
    \ingroup random_shuffle

    \brief random_shuffle implementation
*/
template <class RandomAccessRange>
inline void random_shuffle(RandomAccessRange& range) {
    return std::random_shuffle(boost::begin(range), boost::end(range));
}

/*!
    \ingroup random_shuffle

    \brief random_shuffle implementation
*/
template <class RandomAccessIterator, class RandomNumberGenerator>
inline void random_shuffle(RandomAccessIterator first, RandomAccessIterator last,
                           RandomNumberGenerator& rand) {
    return std::random_shuffle(first, last, std::bind(rand, std::placeholders::_1));
}

/*!
    \ingroup random_shuffle

    \brief random_shuffle implementation
*/
template <class RandomAccessRange, class RandomNumberGenerator>
inline void random_shuffle(RandomAccessRange& range, RandomNumberGenerator& rand) {
    return adobe::random_shuffle(boost::begin(range), boost::end(range), rand);
}

/**************************************************************************************************/

} // namespace adobe

/**************************************************************************************************/

#endif

/**************************************************************************************************/


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/lazy_argument.hpp>
#include <mpllibs/metamonad/lazy.hpp>

#include <boost/test/unit_test.hpp>

#include "common.hpp"

#include <boost/mpl/int.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/assert.hpp>

BOOST_AUTO_TEST_CASE(test_lazy_argument)
{
  using mpllibs::metamonad::lazy;
  using mpllibs::metamonad::lazy_argument;

  using boost::mpl::equal_to;
  using boost::mpl::false_;

  //test_custom_eval_if_with_lazy_expression_as_selected_case_with_lazy_argument
  BOOST_MPL_ASSERT((
    equal_to<
      int26,
      lazy<
        custom_eval_if<
          false_,
          lazy_argument<breaking_expr>,
          lazy_argument<non_lazy_plus<returns13, returns13> >
        >
      >::type
    >
  ));
}


#include <chrono>
#include <memory>

#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>

#include "common.h"
#include "stereo_calibration.h"
#include "target_board.h"
#include "relocalizer.h"
#include "params_config.h"
#include "cost_function.h"

StereoCalibrator::StereoCalibrator(camera::model_type_t camModelType,
                                   target::target_type_t targetModelType,
                                   const std::string &leftIntrinsicFileName,
                                   const std::string &rightIntrinsicFileName,
                                   const std::string &targetFileName,
                                   const bool b_homogeneous) : b_homo_multicamera_(b_homogeneous)
{
    target_ptr_ = std::make_shared<target::TargetBoard>(targetFileName, targetModelType);
    v_object_points_ = target_ptr_->objectPoints();

    if (b_homo_multicamera_)
    {
        int img_width = readImageWidth(leftIntrinsicFileName);
        int img_height = readImageHeight(leftIntrinsicFileName);
        std::shared_ptr<Relocalizer> relocalizer = std::make_shared<Relocalizer>(v_object_points_,
                                                                                ParamsConfig::GetPnPMinInlierNum(),
                                                                                ParamsConfig::GetPnPMinMatchesNum(),
                                                                                img_width, img_height);
        left_calibrator_ptr_ = std::make_shared<MonoCalibrator>(camModelType, leftIntrinsicFileName, v_object_points_, relocalizer);
        right_calibrator_ptr_ = std::make_shared<MonoCalibrator>(camModelType, rightIntrinsicFileName, v_object_points_, relocalizer);
    }
    else
    {
        left_calibrator_ptr_ = std::make_shared<MonoCalibrator>(camModelType, targetModelType, leftIntrinsicFileName, targetFileName);
        right_calibrator_ptr_ = std::make_shared<MonoCalibrator>(camModelType, targetModelType, rightIntrinsicFileName, targetFileName);
    }
}

bool StereoCalibrator::addCctagData(const std::vector<cv::Point2d> &v_left_centers, const std::vector<cv::Point2d> &v_right_centers)
{
    bool sts = left_calibrator_ptr_->addCctagData(v_left_centers);
    if (!sts)
    {
        ERROR_STREAM("[StereoCalibrator::addCctagData] Fail to add CCTAG centers in  left camera!");
        return false;
    }
    sts = right_calibrator_ptr_->addCctagData(v_right_centers);
    if (!sts)
    {
        ERROR_STREAM("[StereoCalibrator::addCctagData] Fail to add CCTAG centers in right camera!");
        return false;
    }

    return true;
}

bool StereoCalibrator::addCctagData(const std::string &leftImgFilePath, const std::vector<cv::Point2d> &v_left_centers,
                                    const std::string &rightImgFilePath, const std::vector<cv::Point2d> &v_right_centers)
{
    bool sts = left_calibrator_ptr_->addCctagData(leftImgFilePath, v_left_centers);
    if (!sts)
    {
        ERROR_STREAM("[StereoCalibrator::addCctagData] Failed to add cctag data in left camera!");
        return false;
    }
    sts = right_calibrator_ptr_->addCctagData(rightImgFilePath, v_right_centers);
    if (!sts)
    {
        ERROR_STREAM("[StereoCalibrator::addCctagData] Failed to add cctag data in right camera!");
        return false;
    }

    return true;
}

camera::Camera::Ptr &StereoCalibrator::leftCamera(void)
{
    return left_calibrator_ptr_->camera();
}

camera::Camera::Ptr &StereoCalibrator::rightCamera(void)
{
    return right_calibrator_ptr_->camera();
}

int StereoCalibrator::readImageWidth(const std::string &filename)
{
    if (filename.empty())
    {
        ERROR_STREAM("[StereoCalibrator::readImageWidth] Empty file name!");
        return -1;
    }

    cv::FileStorage fs(filename, cv::FileStorage::READ);
    if (!fs.isOpened())
    {
        ERROR_STREAM("[StereoCalibrator::readImageWidth] Fail to open file: " + filename);
        return -1;
    }

    int width = (int)fs["image_width"];
    fs.release();
    return width;
}

int StereoCalibrator::readImageHeight(const std::string &filename)
{
    if (filename.empty())
    {
        ERROR_STREAM("[StereoCalibrator::readImageHeight] Empty file name!");
        return -1;
    }

    cv::FileStorage fs(filename, cv::FileStorage::READ);
    if (!fs.isOpened())
    {
        ERROR_STREAM("[StereoCalibrator::readImageHeight] Fail to open file: " + filename);
        return -1;
    }

    int height = (int)fs["image_height"];
    fs.release();
    return height;
}

int StereoCalibrator::sampleCount(void) const
{
    return samples_num_;
}

const std::vector<Eigen::Vector3d> StereoCalibrator::objectPoints() const
{
    return v_object_points_;
}

std::vector<std::pair<Eigen::Matrix4d, Eigen::Matrix4d>> StereoCalibrator::validCameraPosePairs(void) const
{
    std::vector<std::pair<Eigen::Matrix4d, Eigen::Matrix4d>> v_cam_poses_pair;
    for (int i = 0; i < samples_num_; ++i)
    {
        if (!left_calibrator_ptr_->isValid(i) || !right_calibrator_ptr_->isValid(i))
            continue;

        Eigen::Matrix4d left_pose = left_calibrator_ptr_->cameraPose(i);
        Eigen::Matrix4d right_pose = right_calibrator_ptr_->cameraPose(i);

        v_cam_poses_pair.emplace_back(std::make_pair(left_pose, right_pose));
    }
    return v_cam_poses_pair;
}

bool StereoCalibrator::writeExtrinsicParams(const std::string &directory) const
{
    if (directory.empty())
    {
        ERROR_STREAM("[StereoCalibrator::writeExtrinsicParams] Empty directory");
        return false;
    }

    std::string file_stem;
    if (b_homo_multicamera_)
        file_stem = "stereo0_to_stereo1.yml";
    else
        file_stem = "stereo0_to_teche0.yml";

    std::string file_name;
    if (directory.back() == '/')
        file_name = directory + file_stem;
    else
        file_name = directory + "/" + file_stem;

    cv::FileStorage fs(file_name, cv::FileStorage::WRITE);
    if (!fs.isOpened())
    {
        ERROR_STREAM("[StereoCalibrator::writeExtrinsicParams] Fail to open " << file_name);
        return false;
    }
    cv::Mat rotation, translation;
    Eigen::Matrix3d R = T_r_l_.block<3, 3>(0, 0);
    Eigen::Vector3d t = T_r_l_.block<3, 1>(0, 3);
    cv::eigen2cv(R, rotation);
    cv::eigen2cv(t, translation);

    fs << "extrinsic_rotation" << rotation;
    fs << "extrinsic_translation" << translation;
    fs.release();

    return true;
}

Eigen::Matrix4d StereoCalibrator::validExtrinsics() const
{
    return T_r_l_;
}

bool StereoCalibrator::calibrate(void)
{
    DEBUG_STREAM("[StereoCalibrator::calibrate] ---------------- calibrate left camera ------------------");
    if (!left_calibrator_ptr_->calibrate())
    {
        ERROR_STREAM("[StereoCalibrator::calibrate] Fail to calibrate left camera!");
        return false;
    }

    DEBUG_STREAM("[StereoCalibrator::calibrate] ---------------- calibrate right camera ------------------");
    if (!right_calibrator_ptr_->calibrate())
    {
        ERROR_STREAM("[StereoCalibrator::calibrate] Fail to calibrate right camera!");
        return false;
    }

    size_t left_img_num = left_calibrator_ptr_->imagePoints().size();
    size_t right_img_num = right_calibrator_ptr_->imagePoints().size();
    assert(left_img_num == right_img_num && "Left and right image number is inconsistent!");

    samples_num_ = left_img_num;

    // find best estimate of extrinsic between left and right camera
    double min_reprojection_err = std::numeric_limits<double>::max();
    for (size_t i = 0; i < static_cast<size_t>(samples_num_); ++i)
    {
        if (!left_calibrator_ptr_->isValid(i) || !right_calibrator_ptr_->isValid(i))
        {
            ERROR_STREAM("[StereoCalibrator::calibrate] Stereo Frame " << i << " has invalid camera pose!");
            continue;
        }
        Eigen::Matrix4d T_l_wc = left_calibrator_ptr_->cameraPose(i);
        Eigen::Matrix4d T_r_wc = right_calibrator_ptr_->cameraPose(i);
        Eigen::Matrix4d T_r_l = T_r_wc.inverse() * T_l_wc;

        double reproj_err = 0.0;
        unsigned int reproj_cnt = 0;
        for (size_t j = 0; j < right_img_num; ++j)
        {
            if (!left_calibrator_ptr_->isValid(j) || !right_calibrator_ptr_->isValid(j))
            {
                ERROR_STREAM("[StereoCalibrator::calibrate] Stereo Frame " << j << " has invalid camera pose!");
                continue;
            }
            T_l_wc = left_calibrator_ptr_->cameraPose(j);
            Eigen::Matrix4d T_r_cw_tmp = T_r_l * T_l_wc.inverse();
            Eigen::Matrix3d R_r_cw = T_r_cw_tmp.block<3, 3>(0, 0);
            Eigen::Vector3d t_r_cw = T_r_cw_tmp.block<3, 1>(0, 3);

            std::vector<Eigen::Vector3d> &v_scene_points = right_calibrator_ptr_->inlierObjectPoints(j);
            std::vector<cv::Point2d> &v_img_points = right_calibrator_ptr_->inlierImagePoints(j);
            reproj_err += rightCamera()->reprojectionError(v_scene_points,
                                                           v_img_points,
                                                           R_r_cw, t_r_cw);
            reproj_cnt++;
        }

        reproj_err /= reproj_cnt;
        if (reproj_err < min_reprojection_err)
        {
            min_reprojection_err = reproj_err;
            T_r_l_ = T_r_l;
            DEBUG_STREAM("[StereoCalibrator::calibrate] mininal average reprojection error : " << reproj_err);
        }
    }
    //! optimized variables

    // camera pose of rig base
    // here we choose the left camera as rig base
    double opt_transform_base[samples_num_ * 6] = {0.0};
    // camera pose of left camera, reltive to rig
    double opt_transform_cam0[6] = {0.0};
    // camera pose of right camera, reltive to rig
    double opt_transform_cam1[6] = {0.0};

    // print initial reprojection error
    {
        Eigen::Matrix3d rotation = T_r_l_.block<3, 3>(0, 0);
        Eigen::Vector3d eulers = rotation.eulerAngles(2, 0, 1);
        Eigen::Vector3d trans = T_r_l_.block<3, 1>(0, 3);
        DEBUG_STREAM("[StereoCalibrator::calibrate] Initial extrinsics: "
                     << "r: " << eulers(0) << "  p: " << eulers(1) << "  yaw: " << eulers(2)
                     << " x: " << trans(0) << "  y: " << trans(1) << "  z: " << trans(2));

        // initialize extrinsic params
        ceres::RotationMatrixToAngleAxis(rotation.data(), opt_transform_cam1);
        opt_transform_cam1[3] = trans[0];
        opt_transform_cam1[4] = trans[1];
        opt_transform_cam1[5] = trans[2];

        unsigned int valid_frame_count = 0;
        double left_cam_error = 0.0, right_cam_error = 0.0;
        for (size_t i = 0; i < static_cast<size_t>(samples_num_); ++i)
        {
            if (!left_calibrator_ptr_->isValid(i) || !right_calibrator_ptr_->isValid(i))
                continue;

            Eigen::Matrix4d T_l_wc = left_calibrator_ptr_->cameraPose(i);
            Eigen::Matrix4d T_l_cw = T_l_wc.inverse();
            Eigen::Matrix3d R_l_cw = T_l_cw.block<3, 3>(0, 0);
            Eigen::Vector3d t_l_cw = T_l_cw.block<3, 1>(0, 3);

            // initialize left camera pose(c2w)
            ceres::RotationMatrixToAngleAxis(R_l_cw.data(), opt_transform_base + 6 * i);
            opt_transform_base[6 * i + 3] = t_l_cw[0];
            opt_transform_base[6 * i + 4] = t_l_cw[1];
            opt_transform_base[6 * i + 5] = t_l_cw[2];

            std::vector<Eigen::Vector3d> &v_left_scene_points = left_calibrator_ptr_->inlierObjectPoints(i);
            std::vector<cv::Point2d> &v_left_img_points = left_calibrator_ptr_->inlierImagePoints(i);
            left_cam_error += leftCamera()->reprojectionError(v_left_scene_points,
                                                              v_left_img_points,
                                                              R_l_cw, t_l_cw);
            Eigen::Matrix4d T_r_cw_tmp = T_r_l_ * T_l_cw;
            Eigen::Matrix3d R_r_cw = T_r_cw_tmp.block<3, 3>(0, 0);
            Eigen::Vector3d t_r_cw = T_r_cw_tmp.block<3, 1>(0, 3);

            std::vector<Eigen::Vector3d> &v_right_scene_points = right_calibrator_ptr_->inlierObjectPoints(i);
            std::vector<cv::Point2d> &v_right_img_points = right_calibrator_ptr_->inlierImagePoints(i);
            right_cam_error += rightCamera()->reprojectionError(v_right_scene_points,
                                                                v_right_img_points,
                                                                R_r_cw, t_r_cw);
            valid_frame_count++;
        }
        DEBUG_STREAM("[StereoCalibrator::calibrate] Left camera Initial reprojection error: " << left_cam_error / valid_frame_count << " pixels");
        DEBUG_STREAM("[StereoCalibrator::calibrate] Right camera Initial reprojection error: " << right_cam_error / valid_frame_count << " pixels");
    }

    ceres::Problem problem;

    for (size_t i = 0; i < static_cast<size_t>(samples_num_); ++i)
    {
        if (!left_calibrator_ptr_->isValid(i) || !right_calibrator_ptr_->isValid(i))
            continue;

        // optimize left camera
        {
            const std::vector<cv::Point2d> &v_obs_kpts = left_calibrator_ptr_->inlierImagePoints(i);
            const std::vector<Eigen::Vector3d> &v_world_points = left_calibrator_ptr_->inlierObjectPoints(i);
            const Eigen::Matrix3d &intrinsic = leftCamera()->intrinsic();
            for (size_t j = 0; j < v_obs_kpts.size(); ++j)
            {
                ceres::CostFunction *cost_function = StereoReprojectionError2::Create(v_world_points[j], v_obs_kpts[j], intrinsic);
                problem.AddResidualBlock(cost_function, NULL, opt_transform_base + 6 * i, opt_transform_cam0);
            }

            problem.SetParameterBlockConstant(opt_transform_cam0);
        }
        // optimize right camera
        {
            const std::vector<cv::Point2d> &v_obs_kpts = right_calibrator_ptr_->inlierImagePoints(i);
            const std::vector<Eigen::Vector3d> &v_world_points = right_calibrator_ptr_->inlierObjectPoints(i);
            const Eigen::Matrix3d &intrinsic = rightCamera()->intrinsic();
            for (size_t j = 0; j < v_obs_kpts.size(); ++j)
            {
                ceres::CostFunction *cost_function = StereoReprojectionError2::Create(v_world_points[j], v_obs_kpts[j], intrinsic);
                problem.AddResidualBlock(cost_function, NULL, opt_transform_base + 6 * i, opt_transform_cam1);
            }
        }
    }

    bool sts = ceresOptimize(problem, 1e-8);
    if (!sts)
    {
        ERROR_STREAM("[StereoCalibrator::calibrate] Fail to optimize");
        return false;
    }

    // optimized extrinsic
    Eigen::Matrix3d R_r_l = Eigen::Matrix3d::Identity();
    Eigen::Vector3d t_r_l = Eigen::Vector3d::Zero();
    ceres::AngleAxisToRotationMatrix(opt_transform_cam1, R_r_l.data());
    t_r_l[0] = opt_transform_cam1[3];
    t_r_l[1] = opt_transform_cam1[4];
    t_r_l[2] = opt_transform_cam1[5];
    DEBUG_STREAM("[StereoCalibrator::calibrate] Extrinsic R : \n"
                 << R_r_l);
    DEBUG_STREAM("[StereoCalibrator::calibrate] Extrinsic t : " << t_r_l.transpose());
    T_r_l_.block<3, 3>(0, 0) = R_r_l;
    T_r_l_.block<3, 1>(0, 3) = t_r_l;

    for (size_t i = 0; i < static_cast<size_t>(samples_num_); ++i)
    {
        if (!left_calibrator_ptr_->isValid(i) || !right_calibrator_ptr_->isValid(i))
            continue;

        Eigen::Matrix3d rotation_base = Eigen::Matrix3d::Identity();
        Eigen::Vector3d t_base = Eigen::Vector3d::Zero();
        unsigned int tmp_inlier_num;

        ceres::AngleAxisToRotationMatrix(opt_transform_base + 6 * i, rotation_base.data());
        t_base[0] = opt_transform_base[6 * i + 3];
        t_base[1] = opt_transform_base[6 * i + 4];
        t_base[2] = opt_transform_base[6 * i + 5];

        Eigen::Matrix3d left_intrinsic = leftCamera()->intrinsic();
        Eigen::Matrix3d right_intrinsic = rightCamera()->intrinsic();
        // check left camera optimized result
        {
            Eigen::Matrix3d R_cam0 = Eigen::Matrix3d::Identity();
            Eigen::Vector3d t_cam0 = Eigen::Vector3d::Zero();
            ceres::AngleAxisToRotationMatrix(opt_transform_cam0, R_cam0.data());
            t_cam0[0] = opt_transform_cam0[3];
            t_cam0[1] = opt_transform_cam0[4];
            t_cam0[2] = opt_transform_cam0[5];
            t_cam0 = R_cam0 * t_base + t_cam0;
            R_cam0 = R_cam0 * rotation_base;
            // DEBUG_STREAM("[StereoCalibrator::calibrate] Camera 0 optimized R : \n"
            //              << R_cam0);

            double mean_reproject_error = 0;
            double origin_mean_reproject_error = left_calibrator_ptr_->meanReprojectionError(i);
            // check inliers after optimization
            const std::vector<Eigen::Vector3d> &v_world_points = left_calibrator_ptr_->inlierObjectPoints(i);
            const std::vector<cv::Point2d> &v_obs_keypoints = left_calibrator_ptr_->inlierImagePoints(i);

            tmp_inlier_num = MonoCalibrator::checkInliers(v_world_points, v_obs_keypoints, left_intrinsic, R_cam0, t_cam0, mean_reproject_error);

            unsigned int origin_inlier_num = v_world_points.size();

            if (tmp_inlier_num * 10 > origin_inlier_num * 7 || mean_reproject_error < origin_mean_reproject_error)
            {
                sts = true;
                Eigen::Matrix4d T_cw_opt = Eigen::Matrix4d::Identity();
                T_cw_opt.block<3, 3>(0, 0) = R_cam0;
                T_cw_opt.block<3, 1>(0, 3) = t_cam0;

                left_calibrator_ptr_->cameraPose(i) = T_cw_opt.inverse();
                left_calibrator_ptr_->meanReprojectionError(i) = mean_reproject_error;
            }
            else
            {
                sts = false;
                ERROR_STREAM("[MonoCalibrator::optimize] Optimize frame " << i << " left camera failed!");
            }
            ERROR_STREAM("[MonoCalibrator::optimize] Frame " << i << " left camera original inlier num: " << origin_inlier_num);
            ERROR_STREAM("[MonoCalibrator::optimize] Frame " << i << " left camera Optimized inlier num: " << tmp_inlier_num);
            ERROR_STREAM("[MonoCalibrator::optimize] Frame " << i << " left camera Original mean reprojection error : " << origin_mean_reproject_error);
            ERROR_STREAM("[MonoCalibrator::optimize] Frame " << i << " left camera Optimized mean reprojection error : " << mean_reproject_error);
        }
        // check right camera optimized result
        {
            Eigen::Matrix3d R_cam1 = Eigen::Matrix3d::Identity();
            Eigen::Vector3d t_cam1 = t_r_l;
            t_cam1 = R_r_l * t_base + t_cam1;
            R_cam1 = R_r_l * rotation_base;
            // DEBUG_STREAM("[StereoCalibrator::calibrate] Camera 1 optimized R : \n"
            //              << R_cam1);

            double mean_reproject_error = 0;
            double origin_mean_reproject_error = right_calibrator_ptr_->meanReprojectionError(i);
            // check inliers after optimization
            const std::vector<Eigen::Vector3d> &v_world_points = right_calibrator_ptr_->inlierObjectPoints(i);
            const std::vector<cv::Point2d> &v_obs_keypoints = right_calibrator_ptr_->inlierImagePoints(i);

            tmp_inlier_num = MonoCalibrator::checkInliers(v_world_points, v_obs_keypoints, right_intrinsic, R_cam1, t_cam1, mean_reproject_error);

            unsigned int origin_inlier_num = v_world_points.size();

            if (tmp_inlier_num * 10 > origin_inlier_num * 7 || mean_reproject_error < origin_mean_reproject_error)
            {
                sts = true;
                Eigen::Matrix4d T_cw_opt = Eigen::Matrix4d::Identity();
                T_cw_opt.block<3, 3>(0, 0) = R_cam1;
                T_cw_opt.block<3, 1>(0, 3) = t_cam1;

                right_calibrator_ptr_->cameraPose(i) = T_cw_opt.inverse();
                right_calibrator_ptr_->meanReprojectionError(i) = mean_reproject_error;
            }
            else
            {
                sts = false;
                ERROR_STREAM("[MonoCalibrator::optimize] Optimize frame " << i << " right camera failed!");
            }
            ERROR_STREAM("[MonoCalibrator::optimize] Frame " << i << " right camera original inlier num: " << origin_inlier_num);
            ERROR_STREAM("[MonoCalibrator::optimize] Frame " << i << " right camera optimized inlier num: " << tmp_inlier_num);
            ERROR_STREAM("[MonoCalibrator::optimize] Frame " << i << " right camera original mean reprojection error : " << origin_mean_reproject_error);
            ERROR_STREAM("[MonoCalibrator::optimize] Frame " << i << " right camera optimized mean reprojection error : " << mean_reproject_error);
        }
    }

    return sts;
}