/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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 "CScanMatching.h"

//(*InternalHeaders(CScanMatching)
#include <wx/artprov.h>
#include <wx/bitmap.h>
#include <wx/font.h>
#include <wx/image.h>
#include <wx/intl.h>
#include <wx/string.h>
//*)

#include <mrpt/config/CConfigFileMemory.h>
#include <mrpt/gui/CMyRedirector.h>
#include <mrpt/gui/WxUtils.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservationVelodyneScan.h>
#include <mrpt/opengl/CEllipsoid2D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPosePDFSOG.h>
#include <mrpt/slam/CICP.h>
#include <wx/app.h>
#include <wx/busyinfo.h>
#include <wx/log.h>
#include <wx/msgdlg.h>
#include <wx/progdlg.h>

#include "xRawLogViewerMain.h"

using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::opengl;
using namespace mrpt::config;
using namespace mrpt::io;
using namespace mrpt::poses;
using namespace mrpt::system;
using namespace mrpt::img;
using namespace mrpt::math;
using namespace mrpt::serialization;
using namespace mrpt::obs;
using namespace std;

//(*IdInit(CScanMatching)
const long CScanMatching::ID_STATICTEXT2 = wxNewId();
const long CScanMatching::ID_BITMAPBUTTON1 = wxNewId();
const long CScanMatching::ID_STATICTEXT3 = wxNewId();
const long CScanMatching::ID_TEXTCTRL2 = wxNewId();
const long CScanMatching::ID_STATICTEXT4 = wxNewId();
const long CScanMatching::ID_TEXTCTRL3 = wxNewId();
const long CScanMatching::ID_TEXTCTRL6 = wxNewId();
const long CScanMatching::ID_STATICTEXT5 = wxNewId();
const long CScanMatching::ID_RADIOBUTTON1 = wxNewId();
const long CScanMatching::ID_RADIOBUTTON2 = wxNewId();
const long CScanMatching::ID_TEXTCTRL4 = wxNewId();
const long CScanMatching::ID_TEXTCTRL9 = wxNewId();
const long CScanMatching::ID_NOTEBOOK1 = wxNewId();
const long CScanMatching::ID_TEXTCTRL7 = wxNewId();
const long CScanMatching::ID_PANEL1 = wxNewId();
const long CScanMatching::ID_STATICTEXT6 = wxNewId();
const long CScanMatching::ID_XY_GLCANVAS = wxNewId();
const long CScanMatching::ID_BUTTON1 = wxNewId();
const long CScanMatching::ID_CHECKBOX1 = wxNewId();
const long CScanMatching::ID_BUTTON2 = wxNewId();
const long CScanMatching::ID_STATICTEXT1 = wxNewId();
const long CScanMatching::ID_GAUGE1 = wxNewId();
const long CScanMatching::ID_PANEL5 = wxNewId();
const long CScanMatching::ID_PANEL3 = wxNewId();
const long CScanMatching::ID_TEXTCTRL1 = wxNewId();
const long CScanMatching::ID_PANEL4 = wxNewId();
const long CScanMatching::ID_SPLITTERWINDOW2 = wxNewId();
const long CScanMatching::ID_PANEL2 = wxNewId();
const long CScanMatching::ID_SPLITTERWINDOW1 = wxNewId();
//*)

BEGIN_EVENT_TABLE(CScanMatching, wxDialog)
//(*EventTable(CScanMatching)
//*)
END_EVENT_TABLE()

CScanMatching::CScanMatching(wxWindow* parent, wxWindowID)
{
  //(*Initialize(CScanMatching)
  wxFlexGridSizer* FlexGridSizer4;
  wxStaticBoxSizer* StaticBoxSizer4;
  wxFlexGridSizer* FlexGridSizer10;
  wxFlexGridSizer* FlexGridSizer3;
  wxFlexGridSizer* FlexGridSizer5;
  wxFlexGridSizer* FlexGridSizer9;
  wxFlexGridSizer* FlexGridSizer2;
  wxFlexGridSizer* FlexGridSizer7;
  wxStaticBoxSizer* StaticBoxSizer3;
  wxFlexGridSizer* FlexGridSizer8;
  wxFlexGridSizer* FlexGridSizer13;
  wxFlexGridSizer* FlexGridSizer12;
  wxFlexGridSizer* FlexGridSizer6;
  wxStaticBoxSizer* StaticBoxSizer1;
  wxFlexGridSizer* FlexGridSizer1;
  wxFlexGridSizer* FlexGridSizer11;
  wxStaticBoxSizer* StaticBoxSizer5;

  Create(
      parent, wxID_ANY, _("Scan Matching Experimenting Module"), wxDefaultPosition, wxDefaultSize,
      wxCAPTION | wxDEFAULT_DIALOG_STYLE | wxSYSTEM_MENU | wxRESIZE_BORDER | wxCLOSE_BOX |
          wxMAXIMIZE_BOX | wxMINIMIZE_BOX,
      _T("wxID_ANY"));
  SetMinSize(wxSize(200, 200));
  FlexGridSizer1 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer1->AddGrowableCol(0);
  FlexGridSizer1->AddGrowableRow(0);
  SplitterWindow1 = new wxSplitterWindow(
      this, ID_SPLITTERWINDOW1, wxDefaultPosition, wxDefaultSize, wxSP_3D | wxSP_LIVE_UPDATE,
      _T("ID_SPLITTERWINDOW1"));
  SplitterWindow1->SetMinSize(wxSize(50, 50));
  SplitterWindow1->SetMinimumPaneSize(50);
  Panel1 = new wxPanel(
      SplitterWindow1, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
      _T("ID_PANEL1"));
  Panel1->SetMinSize(wxSize(100, -1));
  FlexGridSizer2 = new wxFlexGridSizer(4, 1, 0, 0);
  FlexGridSizer2->AddGrowableCol(0);
  FlexGridSizer2->AddGrowableRow(1);
  FlexGridSizer2->AddGrowableRow(2);
  FlexGridSizer2->AddGrowableRow(3);
  StaticBoxSizer1 = new wxStaticBoxSizer(wxHORIZONTAL, Panel1, _("Observation indexes"));
  FlexGridSizer5 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer5->AddGrowableCol(0);
  FlexGridSizer13 = new wxFlexGridSizer(0, 2, 0, 0);
  FlexGridSizer13->AddGrowableCol(0);
  StaticText2 = new wxStaticText(
      Panel1, ID_STATICTEXT2,
      _("Enter the rawlog indexes of the observations\n to use for "
        "generating the maps to align:"),
      wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT2"));
  FlexGridSizer13->Add(
      StaticText2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  btnHelp = new wxBitmapButton(
      Panel1, ID_BITMAPBUTTON1,
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("wxART_INFORMATION")), wxART_BUTTON),
      wxDefaultPosition, wxDefaultSize, wxBU_AUTODRAW, wxDefaultValidator, _T("ID_BITMAPBUTTON1"));
  FlexGridSizer13->Add(btnHelp, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer5->Add(FlexGridSizer13, 1, wxEXPAND, 0);
  FlexGridSizer6 = new wxFlexGridSizer(0, 2, 0, 0);
  FlexGridSizer6->AddGrowableCol(1);
  StaticText3 = new wxStaticText(
      Panel1, ID_STATICTEXT3, _("First (reference):"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_RIGHT, _T("ID_STATICTEXT3"));
  FlexGridSizer6->Add(StaticText3, 1, wxEXPAND, 5);
  edFirst = new wxTextCtrl(
      Panel1, ID_TEXTCTRL2, _("0"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_TEXTCTRL2"));
  FlexGridSizer6->Add(edFirst, 1, wxEXPAND, 5);
  StaticText4 = new wxStaticText(
      Panel1, ID_STATICTEXT4, _("Second (to align with first):"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_RIGHT, _T("ID_STATICTEXT4"));
  FlexGridSizer6->Add(StaticText4, 1, wxEXPAND, 5);
  edSecond = new wxTextCtrl(
      Panel1, ID_TEXTCTRL3, _("0"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_TEXTCTRL3"));
  FlexGridSizer6->Add(edSecond, 1, wxEXPAND, 5);
  FlexGridSizer5->Add(FlexGridSizer6, 1, wxEXPAND, 0);
  StaticBoxSizer1->Add(FlexGridSizer5, 1, wxEXPAND, 0);
  FlexGridSizer2->Add(StaticBoxSizer1, 1, wxEXPAND, 5);
  StaticBoxSizer5 = new wxStaticBoxSizer(wxHORIZONTAL, Panel1, _("Config of the ICP algorithm:"));
  edOptICP = new wxTextCtrl(
      Panel1, ID_TEXTCTRL6,
      _("; Options for CICP::TConfigParams. See the MRPT reference for "
        "further information\n; "
        "--------------------------------------------------------------------"
        "-----------------\n[InitialPosition]\nx=0 ; The initial position "
        "for the iterative ICP algorithm\ny=0 ; x/y in meters, phi in "
        "degrees\nphi_DEG=0\n\n[ICP]\n; The maximum number of iterations to "
        "execute if convergence is not achieved before\nmaxIterations=40 "
        "\n\n; Initial maximum distance for matching a pair of "
        "points\nthresholdDist=0.75 \n\n; An angular factor (in degrees) to "
        "increase the matching distance for distant "
        "points.\nthresholdAng_DEG=0.15 \n\n; After convergence, the "
        "thresholds are multiplied by this constant and ICP keep running "
        "(provides finer matching)\nALFA=0.30 \n\n; This is the smallest the "
        "distance threshold can become after stopping ICP and accepting the "
        "result.\nsmallestThresholdDist=0.10\n\n; 1: Use the closest points "
        "only, 0: Use all the correspondences within the threshold (more "
        "robust sometimes, but slower)\nonlyClosestCorrespondences=1 \n\n; "
        "This is the variance in X & Y of the input points, used to scale\n; "
        "the covariance of the output estimation (given as a 2D+heading "
        "Gaussian).\n; Refer to "
        "ScanMatching::leastSquareErrorRigidTransformation\ncovariance_"
        "varPoints=0.004\n\n\n; Whether to perform a RANSAC step after "
        "convergence, to improve\n; the result. \ndoRANSAC=0\n\n; Parameters "
        "for the RANSAC step. See reference for "
        "ScanMatching::robustRigidTransformation\nransac_minSetSize="
        "3\nransac_maxSetSize=20\nransac_mahalanobisDistanceThreshold=3."
        "0\nransac_nSimulations=100\nnormalizationStd=0.02\n"),
      wxDefaultPosition, wxSize(300, 87), wxTE_MULTILINE | wxHSCROLL | wxVSCROLL,
      wxDefaultValidator, _T("ID_TEXTCTRL6"));
  wxFont edOptICPFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, _T("Monospace"),
      wxFONTENCODING_DEFAULT);
  edOptICP->SetFont(edOptICPFont);
  StaticBoxSizer5->Add(edOptICP, 1, wxEXPAND, 0);
  FlexGridSizer2->Add(StaticBoxSizer5, 1, wxEXPAND, 2);
  StaticBoxSizer3 =
      new wxStaticBoxSizer(wxHORIZONTAL, Panel1, _("Config of the first (reference) map:"));
  FlexGridSizer3 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer3->AddGrowableCol(0);
  FlexGridSizer3->AddGrowableRow(1);
  FlexGridSizer8 = new wxFlexGridSizer(1, 3, 0, 0);
  FlexGridSizer8->AddGrowableCol(0);
  StaticText5 = new wxStaticText(
      Panel1, ID_STATICTEXT5, _("Map type:"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT5"));
  FlexGridSizer8->Add(
      StaticText5, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  rbPoint = new wxRadioButton(
      Panel1, ID_RADIOBUTTON1, _("Point map"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_RADIOBUTTON1"));
  rbPoint->SetValue(true);
  FlexGridSizer8->Add(rbPoint, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  rbGrid = new wxRadioButton(
      Panel1, ID_RADIOBUTTON2, _("Occupancy grid map"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_RADIOBUTTON2"));
  FlexGridSizer8->Add(rbGrid, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer3->Add(FlexGridSizer8, 1, wxEXPAND, 0);
  Notebook1 =
      new wxNotebook(Panel1, ID_NOTEBOOK1, wxDefaultPosition, wxDefaultSize, 0, _T("ID_NOTEBOOK1"));
  edOptRefPnt = new wxTextCtrl(
      Notebook1, ID_TEXTCTRL4,
      _("; Configuration for CPointsMap::TInsertionOptions - See MRPT "
        "reference for help\n; "
        "--------------------------------------------------------------------"
        "-------------\n[InsertionOptions]\nminDistBetweenLaserPoints=0.05 ; "
        "The minimum distance between points (in 3D): If two points are too "
        "close, one of them is not inserted into the "
        "map.\nalso_interpolate=0 ; If set to true, far points (<1m) are "
        "interpolated with samples at minDistSqrBetweenLaserPoints intervals "
        "(Default is false).\ndisableDeletion=1 ; If set to false (default) "
        "points in the same plane as the inserted scan and inside the free "
        "space, are erased.\nfuseWithExisting=0 ; If set to true (default), "
        "inserted points are fused with previously existent "
        "ones.\nisPlanarMap=1 ; If set to true, only HORIZONTAL (i.e. XY "
        "plane) measurements will be inserted in the map. Default value is "
        "false, thus 3D maps are generated\nmaxDistForInterpolatePoints=1 ; "
        "The maximum distance between two points to interpolate between them "
        "(ONLY when also_interpolate=true).\n"),
      wxDefaultPosition, wxSize(300, 87), wxTE_MULTILINE | wxHSCROLL | wxVSCROLL,
      wxDefaultValidator, _T("ID_TEXTCTRL4"));
  wxFont edOptRefPntFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, _T("Monospace"),
      wxFONTENCODING_DEFAULT);
  edOptRefPnt->SetFont(edOptRefPntFont);
  edOptRefGrid = new wxTextCtrl(
      Notebook1, ID_TEXTCTRL9,
      _("; Configuration for COccupancyGridMap2D::TInsertionOptions - See "
        "MRPT reference for help\n; "
        "--------------------------------------------------------------------"
        "-------------\n[Construction]\nresolution=0.05 ; The grid cell "
        "size, in meters.\n\n[InsertionOptions]\nmapAltitude=0 ; The height "
        "(z axis) over the ground at which this grid map is "
        "at\nuseMapAltitude=0 ; If this is set to 1, only observations that "
        "match the grid height are inserted in the "
        "map.\nmaxDistanceInsertion=40 ; The maximum distance from the scan "
        "origin for updating cells, in "
        "meters\nmaxOccupancyUpdateCertainty=0.8 ; The \"certainty\" used to "
        "update the cells using the Bayes update formulas. Values are in the "
        "range 0.5 (No update) to 1.0 (last value is taken forgetting all "
        "old readings of that cell).\nconsiderInvalidRangesAsFreeSpace=1 ; "
        "Whether to consider \"lost-ranges\" as free space.\n\n"),
      wxDefaultPosition, wxSize(300, 87), wxTE_MULTILINE | wxHSCROLL | wxVSCROLL,
      wxDefaultValidator, _T("ID_TEXTCTRL9"));
  wxFont edOptRefGridFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, _T("Monospace"),
      wxFONTENCODING_DEFAULT);
  edOptRefGrid->SetFont(edOptRefGridFont);
  Notebook1->AddPage(edOptRefPnt, _("Point map config"), false);
  Notebook1->AddPage(edOptRefGrid, _("Grid map config"), false);
  FlexGridSizer3->Add(Notebook1, 1, wxEXPAND, 1);
  StaticBoxSizer3->Add(FlexGridSizer3, 1, wxEXPAND, 1);
  FlexGridSizer2->Add(StaticBoxSizer3, 1, wxEXPAND, 2);
  StaticBoxSizer4 =
      new wxStaticBoxSizer(wxHORIZONTAL, Panel1, _("Config of the second (to align) point map:"));
  edOptAlignMap = new wxTextCtrl(
      Panel1, ID_TEXTCTRL7,
      _("; Configuration for CPointsMap::TInsertionOptions - See MRPT "
        "reference for help\n; "
        "--------------------------------------------------------------------"
        "-------------\n[InsertionOptions]\nminDistBetweenLaserPoints=0.05 ; "
        "The minimum distance between points (in 3D): If two points are too "
        "close, one of them is not inserted into the "
        "map.\nalso_interpolate=0 ; If set to true, far points (<1m) are "
        "interpolated with samples at minDistSqrBetweenLaserPoints intervals "
        "(Default is false).\ndisableDeletion=1 ; If set to false (default) "
        "points in the same plane as the inserted scan and inside the free "
        "space, are erased.\nfuseWithExisting=0 ; If set to true (default), "
        "inserted points are fused with previously existent "
        "ones.\nisPlanarMap=1 ; If set to true, only HORIZONTAL (i.e. XY "
        "plane) measurements will be inserted in the map. Default value is "
        "false, thus 3D maps are generated\nmaxDistForInterpolatePoints=1 ; "
        "The maximum distance between two points to interpolate between them "
        "(ONLY when also_interpolate=true).\n"),
      wxDefaultPosition, wxSize(300, 87), wxTE_MULTILINE | wxHSCROLL | wxVSCROLL,
      wxDefaultValidator, _T("ID_TEXTCTRL7"));
  wxFont edOptAlignMapFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, _T("Monospace"),
      wxFONTENCODING_DEFAULT);
  edOptAlignMap->SetFont(edOptAlignMapFont);
  StaticBoxSizer4->Add(edOptAlignMap, 1, wxEXPAND, 0);
  FlexGridSizer2->Add(StaticBoxSizer4, 1, wxEXPAND, 2);
  Panel1->SetSizer(FlexGridSizer2);
  FlexGridSizer2->Fit(Panel1);
  FlexGridSizer2->SetSizeHints(Panel1);
  Panel2 = new wxPanel(
      SplitterWindow1, ID_PANEL2, wxDefaultPosition, wxSize(200, 200), wxTAB_TRAVERSAL,
      _T("ID_PANEL2"));
  FlexGridSizer4 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer4->AddGrowableCol(0);
  FlexGridSizer4->AddGrowableRow(0);
  SplitterWindow2 = new wxSplitterWindow(
      Panel2, ID_SPLITTERWINDOW2, wxDefaultPosition, wxDefaultSize, wxSP_3D,
      _T("ID_SPLITTERWINDOW2"));
  SplitterWindow2->SetMinSize(wxSize(10, 10));
  SplitterWindow2->SetMinimumPaneSize(10);
  Panel3 = new wxPanel(
      SplitterWindow2, ID_PANEL3, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
      _T("ID_PANEL3"));
  FlexGridSizer9 = new wxFlexGridSizer(3, 1, 0, 0);
  FlexGridSizer9->AddGrowableCol(0);
  FlexGridSizer9->AddGrowableRow(1);
  StaticText6 = new wxStaticText(
      Panel3, ID_STATICTEXT6,
      _("Scan Matching Status:\n(Reference map: Blue "
        "or gridmap; Map to be aligned: Red)"),
      wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT6"));
  FlexGridSizer9->Add(StaticText6, 1, wxEXPAND, 3);
  m_plot3D = new CMyGLCanvas(
      Panel3, ID_XY_GLCANVAS, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
      _T("ID_XY_GLCANVAS"));
  FlexGridSizer9->Add(m_plot3D, 1, wxEXPAND, 0);
  Panel5 = new wxPanel(
      Panel3, ID_PANEL5, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL5"));
  FlexGridSizer7 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer7->AddGrowableCol(0);
  FlexGridSizer7->AddGrowableRow(0);
  FlexGridSizer11 = new wxFlexGridSizer(1, 3, 0, 0);
  FlexGridSizer11->AddGrowableCol(1);
  btnRunICP = new wxButton(
      Panel5, ID_BUTTON1, _("Run ICP"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_BUTTON1"));
  btnRunICP->SetDefault();
  FlexGridSizer11->Add(
      btnRunICP, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  cbAnimate = new wxCheckBox(
      Panel5, ID_CHECKBOX1, _("Animate step by step"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_CHECKBOX1"));
  cbAnimate->SetValue(false);
  FlexGridSizer11->Add(
      cbAnimate, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  btnClose = new wxButton(
      Panel5, ID_BUTTON2, _("CLOSE"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_BUTTON2"));
  FlexGridSizer11->Add(btnClose, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer7->Add(FlexGridSizer11, 1, wxEXPAND, 0);
  FlexGridSizer12 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer12->AddGrowableCol(1);
  txtStep = new wxStaticText(
      Panel5, ID_STATICTEXT1, _("Step: 0 / 0"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT1"));
  FlexGridSizer12->Add(txtStep, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  pbSteps = new wxGauge(
      Panel5, ID_GAUGE1, 100, wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_GAUGE1"));
  FlexGridSizer12->Add(pbSteps, 1, wxEXPAND, 5);
  FlexGridSizer7->Add(FlexGridSizer12, 1, wxEXPAND, 0);
  Panel5->SetSizer(FlexGridSizer7);
  FlexGridSizer7->Fit(Panel5);
  FlexGridSizer7->SetSizeHints(Panel5);
  FlexGridSizer9->Add(Panel5, 1, wxEXPAND, 0);
  Panel3->SetSizer(FlexGridSizer9);
  FlexGridSizer9->Fit(Panel3);
  FlexGridSizer9->SetSizeHints(Panel3);
  Panel4 = new wxPanel(
      SplitterWindow2, ID_PANEL4, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
      _T("ID_PANEL4"));
  FlexGridSizer10 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer10->AddGrowableCol(0);
  FlexGridSizer10->AddGrowableRow(0);
  txtLog = new wxTextCtrl(
      Panel4, ID_TEXTCTRL1, _("(Log messages)"), wxDefaultPosition, wxDefaultSize,
      wxTE_MULTILINE | wxTE_READONLY | wxHSCROLL | wxVSCROLL, wxDefaultValidator,
      _T("ID_TEXTCTRL1"));
  wxFont txtLogFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, _T("Monospace"),
      wxFONTENCODING_DEFAULT);
  txtLog->SetFont(txtLogFont);
  FlexGridSizer10->Add(txtLog, 1, wxEXPAND, 0);
  Panel4->SetSizer(FlexGridSizer10);
  FlexGridSizer10->Fit(Panel4);
  FlexGridSizer10->SetSizeHints(Panel4);
  SplitterWindow2->SplitHorizontally(Panel3, Panel4);
  FlexGridSizer4->Add(SplitterWindow2, 1, wxEXPAND, 0);
  Panel2->SetSizer(FlexGridSizer4);
  FlexGridSizer4->SetSizeHints(Panel2);
  SplitterWindow1->SplitVertically(Panel1, Panel2);
  FlexGridSizer1->Add(SplitterWindow1, 1, wxEXPAND, 0);
  SetSizer(FlexGridSizer1);
  FlexGridSizer1->Fit(this);
  FlexGridSizer1->SetSizeHints(this);
  Center();

  Bind(wxEVT_BUTTON, &CScanMatching::OnbtnHelpClick, this, ID_BITMAPBUTTON1);
  Bind(wxEVT_RADIOBUTTON, &CScanMatching::OChangeSelectedMapType, this, ID_RADIOBUTTON1);
  Bind(wxEVT_RADIOBUTTON, &CScanMatching::OChangeSelectedMapType, this, ID_RADIOBUTTON2);
  Bind(
      wxEVT_COMMAND_NOTEBOOK_PAGE_CHANGING, &CScanMatching::OnNotebook1PageChanging, this,
      ID_NOTEBOOK1);
  Bind(wxEVT_BUTTON, &CScanMatching::OnbtnICPClick, this, ID_BUTTON1);
  Bind(wxEVT_CHECKBOX, &CScanMatching::OncbAnimateClick, this, ID_CHECKBOX1);
  Bind(wxEVT_BUTTON, &CScanMatching::OnbtnCloseClick, this, ID_BUTTON2);
  //*)

  // Initialize 3D view:
  auto openGLSceneRef = m_plot3D->getOpenGLSceneRef();
  openGLSceneRef->insert(
      mrpt::opengl::CGridPlaneXY::Create(-150, 150, -150, 150, 0 /* z */, 5 /* freq */));
  openGLSceneRef->insert(
      mrpt::opengl::stock_objects::CornerXYZSimple(1.0 /*scale*/, 3.0 /*line width*/));

  m_gl_map_ref = mrpt::opengl::CSetOfObjects::Create();
  m_gl_map_new = mrpt::opengl::CSetOfObjects::Create();
  openGLSceneRef->insert(m_gl_map_ref);
  openGLSceneRef->insert(m_gl_map_new);

  // Initially these controls are hiden:
  txtStep->Show(false);
  pbSteps->Show(false);

  Fit();

  // Load default ICP options:
  {
    mrpt::config::CConfigFileMemory cfg;
    mrpt::slam::CICP::TConfigParams icp_params;
    cfg.write("InitialPosition", "x", 0.0);
    cfg.write("InitialPosition", "y", 0.0);
    cfg.write("InitialPosition", "phi_DEG", 0.0);
    icp_params.saveToConfigFile(cfg, "ICP");

    edOptICP->SetValue(cfg.getContent().c_str());
  }

  wxLog* old_log = wxLog::SetActiveTarget(new wxLogTextCtrl(txtLog));
  delete old_log;
}

CScanMatching::~CScanMatching()
{
  //(*Destroy(CScanMatching)
  //*)
}

void CScanMatching::OnInit(wxInitDialogEvent&) {}
// Disable buttons while ICP is in animation:
class CMyButtonsDisabler
{
  CScanMatching* m_wnd;

 public:
  CMyButtonsDisabler(CScanMatching* obj) : m_wnd(obj)
  {
    m_wnd->btnClose->Enable(false);
    m_wnd->btnRunICP->Enable(false);
  }

  ~CMyButtonsDisabler()
  {
    m_wnd->btnClose->Enable(true);
    m_wnd->btnRunICP->Enable(true);
  }
};

static void insert_obs_into_map(const CSerializable::Ptr& obj, mrpt::maps::CMetricMap* theMap)
{
  if (IS_CLASS(*obj, CSensoryFrame))
  {
    auto SF = std::dynamic_pointer_cast<CSensoryFrame>(obj);
    SF->insertObservationsInto(*theMap);
  }
  else if (IS_DERIVED(*obj, CObservation))
  {
    CObservationVelodyneScan::Ptr obs_velodyne;
    if (IS_CLASS(*obj, CObservationVelodyneScan))
    {
      obs_velodyne = mrpt::ptr_cast<CObservationVelodyneScan>::from(obj);
      obs_velodyne->generatePointCloud();
    }
    auto obs_ref = std::dynamic_pointer_cast<CObservation>(obj);
    theMap->insertObservation(*obs_ref);

    // free mem:
    if (obs_velodyne) obs_velodyne->point_cloud.clear_deep();
  }
  else
    THROW_EXCEPTION("Unexpected runtime class found.");
}

// Perform the ICP:
void CScanMatching::OnbtnICPClick(wxCommandEvent&)
{
  WX_START_TRY

  // The reference and the new maps:
  CSimplePointsMap refMapPt;
  COccupancyGridMap2D refMapGrid;
  CSimplePointsMap newMapPt;
  CICP icp;
  CICP::TReturnInfo icpInfo;

  // Get indexes for creating the maps:
  long refIndx;
  edFirst->GetValue().ToLong(&refIndx);
  long newIndx;
  edSecond->GetValue().ToLong(&newIndx);

  // Get the SFs, and check classes:
  if (rawlog.getType(refIndx) != CRawlog::etObservation &&
      rawlog.getType(refIndx) != CRawlog::etSensoryFrame)
  {
    wxMessageBox(
        _("The first index is not a sensory frame or an observation!\n "
          "Select an index that corresponds to observations"),
        _("Error"), wxOK, this);
    return;
  }
  if (rawlog.getType(newIndx) != CRawlog::etObservation &&
      rawlog.getType(newIndx) != CRawlog::etSensoryFrame)
  {
    wxMessageBox(
        _("The second index is not a sensory frame or an observation!\n "
          "Select an index that corresponds to observations"),
        _("Error"), wxOK, this);
    return;
  }

  CSerializable::Ptr obj_ref = rawlog.getAsGeneric(refIndx);
  CSerializable::Ptr obj_new = rawlog.getAsGeneric(newIndx);

  CPosePDF::Ptr poseEst;

  // Load ICP options:
  // ------------------------------------------
  CConfigFileMemory icpCfg((string(edOptICP->GetValue().mb_str())));
  icp.options.loadFromConfigFile(icpCfg, "ICP");

  // EXTRA options:
  CPose2D initialEst(
      icpCfg.read_float("InitialPosition", "x", 0, true),
      icpCfg.read_float("InitialPosition", "y", 0, true),
      DEG2RAD(icpCfg.read_float("InitialPosition", "phi_DEG", 0, true)));

  txtLog->Clear();

  // Redirect cout to the log text:
  CMyRedirector myRedirector(txtLog);

  icp.options.dumpToConsole();

  // Create reference map & load its options:
  // ------------------------------------------
  bool useGridMap = rbGrid->GetValue();
  if (!useGridMap)
  {
    CConfigFileMemory refCfg(string(edOptRefPnt->GetValue().mb_str()));
    refMapPt.insertionOptions.loadFromConfigFile(refCfg, "InsertionOptions");
    cout << "REFERENCE MAP FOR THE ICP:" << endl;
    refMapPt.insertionOptions.dumpToConsole();
    refMapPt.renderOptions.color = mrpt::img::TColorf(.0f, .0f, 1.0f);
  }
  else
  {
    CConfigFileMemory refCfg(string(edOptRefGrid->GetValue().mb_str()));
    float gridRes = refCfg.read_float("Construction", "resolution", 0.05f);
    refMapGrid.setSize(-10, 10, -10, 10, gridRes);
    refMapGrid.insertionOptions.loadFromConfigFile(refCfg, "InsertionOptions");

    cout << "REFERENCE MAP FOR THE ICP:" << endl;
    refMapGrid.insertionOptions.dumpToConsole();
  }

  CMetricMap* refMap = useGridMap ? (CMetricMap*)&refMapGrid : (CMetricMap*)&refMapPt;

  // Load new map options:
  // ----------------------------
  {
    CConfigFileMemory refCfg(string(edOptAlignMap->GetValue().mb_str()));
    newMapPt.insertionOptions.loadFromConfigFile(refCfg, "InsertionOptions");
    newMapPt.renderOptions.color = mrpt::img::TColorf(1.0f, .0f, .0f);

    cout << "NEW MAP (TO ALIGN) FOR THE ICP:" << endl;
    newMapPt.insertionOptions.dumpToConsole();
  }

  // Insert the observations:
  // --------------------------------------
  insert_obs_into_map(obj_ref, refMap);
  insert_obs_into_map(obj_new, &newMapPt);

  // Delete all existing draw layers:
  m_gl_map_ref = refMap->getVisualization();
  m_gl_map_new = newMapPt.getVisualization();

  auto gl_ellipse = mrpt::opengl::CEllipsoid2D::Create();
  gl_ellipse->setQuantiles(3.0f);
  gl_ellipse->setColor_u8(0, 0, 0);
  m_gl_map_new->insert(gl_ellipse);

  // Align:
  // --------------------------------------
  bool isAnimation = cbAnimate->GetValue();
  unsigned int maxSteps = icp.options.maxIterations;
  unsigned int curStep = isAnimation ? 0 : maxSteps;
  CPose2D estMean;
  CMatrixDouble33 estCov;

  {
    wxBusyCursor waitCursor;
    CMyButtonsDisabler buttonsDisabler(this);

    cout << "===========" << endl;
    cout << "  ICP RUN:" << endl;
    cout << "===========" << endl;

    pbSteps->SetRange(maxSteps);

    while (curStep <= maxSteps)
    {
      if (isAnimation)
      {
        txtStep->SetLabel((format("Step: %u / %u", curStep, maxSteps).c_str()));
        pbSteps->SetValue(curStep);
        wxTheApp->Yield();  // Let the app. process messages
      }

      icp.options.maxIterations = curStep;

      poseEst = icp.Align(refMap, (CMetricMap*)&newMapPt, initialEst, icpInfo);

      // Show the final graphs:
      // --------------------------------------
      poseEst->getCovarianceAndMean(estCov, estMean);

      m_gl_map_new->setPose(CPose3D(estMean));
      gl_ellipse->setCovMatrix(estCov.extractMatrix<2, 2>(0, 0));

      m_plot3D->Refresh();

      // Show text log:
      if (isAnimation)
      {
        cout << format("EXECUTING %i steps:\n---------------------------\n", curStep);
      }
      cout << format("Time:%fms\n", icpInfo.executionTime * 1e3f);
      cout << format("Iterations executed: %i\n", icpInfo.nIterations);
      cout << format("Goodness: %.02f%%\n", 100 * icpInfo.goodness);
      cout << format("Quality: %.04f\n", icpInfo.quality);

      // Already converged?
      if (isAnimation && icpInfo.nIterations < (curStep - 3))
      {
        curStep = maxSteps;
        pbSteps->SetValue(maxSteps);
      }

      cout << format(
          "Estimated pose:\n Mean=(%f,%f,%fdeg)\n", estMean.x(), estMean.y(),
          RAD2DEG(estMean.phi()));
      cout << " Covariance:\n";
      cout << estCov << endl;

      cout << " std(x)   = " << sqrt(estCov(0, 0)) << " m. " << endl;
      cout << " std(y)   = " << sqrt(estCov(1, 1)) << " m. " << endl;
      cout << " std(phi) = " << RAD2DEG(sqrt(estCov(2, 2))) << " deg. " << endl;

      cout << format("Output PDF class is: %s\n", poseEst->GetRuntimeClass()->className);
      if (poseEst->GetRuntimeClass() == CLASS_ID(CPosePDFSOG))
      {
        CPosePDFSOG::Ptr SOG = std::dynamic_pointer_cast<CPosePDFSOG>(poseEst);
        size_t i, n = SOG->size();
        cout << format("# of gaussians in SOG: %i\n", (int)n);
        for (i = 0; i < n; i++)
        {
          cout << format("SOG[%02i]:w=%e mean=", (int)i, SOG->get(i).log_w) << SOG->get(i).mean
               << endl;
        }
      }

      // delete poseEst; poseEst=nullptr;

      if (isAnimation)
      {
        wxTheApp->Yield();  // Let the app. process messages
        ::wxMilliSleep(100);
      }

      curStep++;
    }  // end while

    // End of wait cursor
  }

  WX_END_TRY
}

void CScanMatching::OncbAnimateClick(wxCommandEvent&)
{
  bool show = cbAnimate->GetValue();
  txtStep->Show(show);
  pbSteps->Show(show);

  pbSteps->Fit();
  txtStep->Fit();
  Panel5->Fit();
}

void CScanMatching::OChangeSelectedMapType(wxCommandEvent&)
{
  Notebook1->ChangeSelection(rbPoint->GetValue() ? 0 : 1);
}

void CScanMatching::OnNotebook1PageChanging(wxNotebookEvent& event) { event.Veto(); }

void CScanMatching::OnbtnCloseClick(wxCommandEvent&) { Close(); }
void CScanMatching::OnbtnHelpClick(wxCommandEvent&)
{
  wxString s;
  s << _(
      "Scan matching tries to register two maps. Each of them is created by "
      "inserting observations,\n");
  s << _(
      " typically laser scans, into point or grid maps. You must enter here "
      "the indexes of the observations\n");
  s << _(
      " to be inserted in each map. You can select just one index, or "
      "several ones separated by spaces.\n");
  s << _(" All the observations will be inserted at (0,0,0) in each map.");

  wxMessageBox(s);
}
