/* +------------------------------------------------------------------------+
   |                     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 "reactive_navigator_demoMain.h"

#include <wx/msgdlg.h>
#include <wx/textdlg.h>

#include <memory>

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

#include <mrpt/config/CConfigFilePrefixer.h>
#include <mrpt/gui/WxUtils.h>
#include <mrpt/gui/about_box.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/poses/CPoint2D.h>
#include <mrpt/poses/CPose2D.h>

#include "../wx-common/mrpt_logo.xpm"
#include "DEFAULT_GRIDMAP_DATA.h"
#include "imgs/main_icon.xpm"

const double NAV_SIMUL_TIMESTEP_MS = 25;

// A custom Art provider for customizing the icons:
class MyArtProvider : public wxArtProvider
{
 protected:
  wxBitmap CreateBitmap(const wxArtID& id, const wxArtClient& client, const wxSize& size) override;
};

// CreateBitmap function
wxBitmap MyArtProvider::CreateBitmap(
    const wxArtID& id, const wxArtClient& client, const wxSize& size)
{
  if (id == wxART_MAKE_ART_ID(MAIN_ICON)) return wxBitmap(main_icon_xpm);
  if (id == wxART_MAKE_ART_ID(IMG_MRPT_LOGO)) return wxBitmap(mrpt_logo_xpm);

  // Any wxWidgets icons not implemented here
  // will be provided by the default art provider.
  return wxNullBitmap;
}

#include <mrpt/nav.h>
#include <mrpt/opengl.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

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

//(*IdInit(reactive_navigator_demoframe)
const long reactive_navigator_demoframe::ID_BUTTON4 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON5 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON7 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON12 = wxNewId();
const long reactive_navigator_demoframe::ID_STATICTEXT10 = wxNewId();
const long reactive_navigator_demoframe::ID_STATICTEXT9 = wxNewId();
const long reactive_navigator_demoframe::ID_STATICTEXT8 = wxNewId();
const long reactive_navigator_demoframe::ID_TEXTCTRL6 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON6 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON8 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON11 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON1 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON9 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON10 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON2 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON3 = wxNewId();
const long reactive_navigator_demoframe::ID_RADIOBOX2 = wxNewId();
const long reactive_navigator_demoframe::ID_CHECKBOX1 = wxNewId();
const long reactive_navigator_demoframe::ID_CHECKBOX2 = wxNewId();
const long reactive_navigator_demoframe::ID_CHECKBOX3 = wxNewId();
const long reactive_navigator_demoframe::ID_CHECKBOX4 = wxNewId();
const long reactive_navigator_demoframe::ID_CHECKBOX5 = wxNewId();
const long reactive_navigator_demoframe::ID_RADIOBOX1 = wxNewId();
const long reactive_navigator_demoframe::ID_BUTTON13 = wxNewId();
const long reactive_navigator_demoframe::ID_PANEL6 = wxNewId();
const long reactive_navigator_demoframe::ID_TEXTCTRL1 = wxNewId();
const long reactive_navigator_demoframe::ID_PANEL2 = wxNewId();
const long reactive_navigator_demoframe::ID_TEXTCTRL4 = wxNewId();
const long reactive_navigator_demoframe::ID_PANEL4 = wxNewId();
const long reactive_navigator_demoframe::ID_TEXTCTRL3 = wxNewId();
const long reactive_navigator_demoframe::ID_PANEL3 = wxNewId();
const long reactive_navigator_demoframe::ID_NOTEBOOK1 = wxNewId();
const long reactive_navigator_demoframe::ID_PANEL1 = wxNewId();
const long reactive_navigator_demoframe::ID_STATICTEXT3 = wxNewId();
const long reactive_navigator_demoframe::ID_TEXTCTRL5 = wxNewId();
const long reactive_navigator_demoframe::ID_PANEL7 = wxNewId();
const long reactive_navigator_demoframe::ID_STATICTEXT2 = wxNewId();
const long reactive_navigator_demoframe::ID_STATICTEXT1 = wxNewId();
const long reactive_navigator_demoframe::ID_XY_GLCANVAS = wxNewId();
const long reactive_navigator_demoframe::ID_STATICTEXT4 = wxNewId();
const long reactive_navigator_demoframe::ID_CHOICE1 = wxNewId();
const long reactive_navigator_demoframe::ID_CUSTOM1 = wxNewId();
const long reactive_navigator_demoframe::ID_TEXTCTRL2 = wxNewId();
const long reactive_navigator_demoframe::ID_PANEL8 = wxNewId();
const long reactive_navigator_demoframe::ID_SPLITTERWINDOW2 = wxNewId();
const long reactive_navigator_demoframe::ID_PANEL5 = wxNewId();
const long reactive_navigator_demoframe::ID_SPLITTERWINDOW1 = wxNewId();
const long reactive_navigator_demoframe::ID_MENUITEM4 = wxNewId();
const long reactive_navigator_demoframe::idMenuQuit = wxNewId();
const long reactive_navigator_demoframe::ID_MENUITEM1 = wxNewId();
const long reactive_navigator_demoframe::ID_MENUITEM2 = wxNewId();
const long reactive_navigator_demoframe::ID_MENUITEM3 = wxNewId();
const long reactive_navigator_demoframe::idMenuAbout = wxNewId();
const long reactive_navigator_demoframe::ID_STATUSBAR1 = wxNewId();
const long reactive_navigator_demoframe::ID_TIMER1 = wxNewId();
//*)

BEGIN_EVENT_TABLE(reactive_navigator_demoframe, wxFrame)
//(*EventTable(reactive_navigator_demoframe)
//*)
END_EVENT_TABLE()

reactive_navigator_demoframe::reactive_navigator_demoframe(wxWindow* parent, wxWindowID id) :
    m_gridMap(),
    m_targetPoint(.0, .0),
    m_is_running(false),
    m_curCursorPos(0, 0),
    m_cursorPickState(cpsNone)
{
  // Load my custom icons:
  wxArtProvider::Push(new MyArtProvider);

  //(*Initialize(reactive_navigator_demoframe)
  wxFlexGridSizer* FlexGridSizer4;
  wxMenuItem* MenuItem2;
  wxFlexGridSizer* FlexGridSizer10;
  wxFlexGridSizer* FlexGridSizer3;
  wxMenuItem* MenuItem1;
  wxFlexGridSizer* FlexGridSizer5;
  wxFlexGridSizer* FlexGridSizer9;
  wxFlexGridSizer* FlexGridSizer2;
  wxMenu* Menu1;
  wxFlexGridSizer* FlexGridSizer7;
  wxFlexGridSizer* FlexGridSizer8;
  wxFlexGridSizer* FlexGridSizer12;
  wxMenuBar* MenuBar1;
  wxFlexGridSizer* FlexGridSizer6;
  wxFlexGridSizer* FlexGridSizer1;
  wxFlexGridSizer* FlexGridSizer11;
  wxBoxSizer* BoxSizer3;
  wxMenu* Menu2;

  Create(
      parent, wxID_ANY, _("Reactive Navigation Tester - Part of MRPT"), wxDefaultPosition,
      wxDefaultSize, wxDEFAULT_FRAME_STYLE, _T("wxID_ANY"));
  SetClientSize(wxSize(893, 576));
  {
    wxIcon FrameIcon;
    FrameIcon.CopyFromBitmap(
        wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("MAIN_ICON")), wxART_OTHER));
    SetIcon(FrameIcon);
  }
  FlexGridSizer1 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer1->AddGrowableCol(1);
  FlexGridSizer1->AddGrowableRow(0);
  FlexGridSizer4 = new wxFlexGridSizer(0, 1, 0, 0);
  FlexGridSizer4->AddGrowableCol(0);
  btnStart = new wxCustomButton(
      this, ID_BUTTON4, _("RUN"),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_GO_FORWARD")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 60), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON4"));
  btnStart->SetBitmapDisabled(btnStart->CreateBitmapDisabled(btnStart->GetBitmapLabel()));
  btnStart->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer4->Add(btnStart, 1, wxEXPAND, 2);
  btnStop = new wxCustomButton(
      this, ID_BUTTON5, _("STOP"),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_CROSS_MARK")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 60), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON5"));
  btnStop->SetBitmapDisabled(btnStop->CreateBitmapDisabled(btnStop->GetBitmapLabel()));
  btnStop->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer4->Add(btnStop, 1, wxEXPAND, 2);
  FlexGridSizer4->Add(-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 1);
  btnPlaceTarget = new wxCustomButton(
      this, ID_BUTTON7, _("Set target..."),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FIND")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 60), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON7"));
  btnPlaceTarget->SetBitmapDisabled(
      btnPlaceTarget->CreateBitmapDisabled(btnPlaceTarget->GetBitmapLabel()));
  btnPlaceTarget->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer4->Add(btnPlaceTarget, 1, wxEXPAND, 2);
  btnSetWaypointSeq = new wxCustomButton(
      this, ID_BUTTON12, _("Waypoint list..."),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FIND")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 60), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON12"));
  btnSetWaypointSeq->SetBitmapDisabled(
      btnSetWaypointSeq->CreateBitmapDisabled(btnSetWaypointSeq->GetBitmapLabel()));
  btnSetWaypointSeq->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer4->Add(btnSetWaypointSeq, 1, wxEXPAND, 2);
  StaticText8 = new wxStaticText(
      this, ID_STATICTEXT10, _("(non-skip:SHIFT+click)"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT10"));
  FlexGridSizer4->Add(StaticText8, 1, wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  StaticText7 = new wxStaticText(
      this, ID_STATICTEXT9, _("(with yaw: CTRL+click)"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT9"));
  FlexGridSizer4->Add(StaticText7, 1, wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  StaticText6 = new wxStaticText(
      this, ID_STATICTEXT8, _("Waypt yaw (deg):"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT8"));
  FlexGridSizer4->Add(StaticText6, 1, wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  edWayPtHeading = new wxTextCtrl(
      this, ID_TEXTCTRL6, _("0.0"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_TEXTCTRL6"));
  FlexGridSizer4->Add(
      edWayPtHeading, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  btnPlaceRobot = new wxCustomButton(
      this, ID_BUTTON6, _("Replace robot..."),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FIND")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 60), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON6"));
  btnPlaceRobot->SetBitmapDisabled(
      btnPlaceRobot->CreateBitmapDisabled(btnPlaceRobot->GetBitmapLabel()));
  btnPlaceRobot->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer4->Add(btnPlaceRobot, 1, wxEXPAND, 2);
  FlexGridSizer4->Add(-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 1);
  btnDrawMapObs = new wxCustomButton(
      this, ID_BUTTON8, _("Draw occupied..."),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_GO_FORWARD")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 50), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON8"));
  btnDrawMapObs->SetBitmapDisabled(
      btnDrawMapObs->CreateBitmapDisabled(btnDrawMapObs->GetBitmapLabel()));
  btnDrawMapObs->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer4->Add(btnDrawMapObs, 1, wxEXPAND, 2);
  btnDrawEmpty = new wxCustomButton(
      this, ID_BUTTON11, _("Draw empty..."),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_GO_FORWARD")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 50), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON11"));
  btnDrawEmpty->SetBitmapDisabled(
      btnDrawEmpty->CreateBitmapDisabled(btnDrawEmpty->GetBitmapLabel()));
  btnDrawEmpty->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer4->Add(btnDrawEmpty, 1, wxEXPAND, 2);
  FlexGridSizer12 = new wxFlexGridSizer(0, 2, 0, 0);
  FlexGridSizer12->AddGrowableCol(0);
  FlexGridSizer12->AddGrowableCol(1);
  btnLoadMap = new wxCustomButton(
      this, ID_BUTTON1, _("Load map..."),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FILE_OPEN")), wxART_BUTTON),
      wxDefaultPosition, wxSize(-1, 60), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON1"));
  btnLoadMap->SetBitmapDisabled(btnLoadMap->CreateBitmapDisabled(btnLoadMap->GetBitmapLabel()));
  btnLoadMap->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer12->Add(btnLoadMap, 1, wxEXPAND, 2);
  btnEmptyMap = new wxCustomButton(
      this, ID_BUTTON9, _("New map..."),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_NEW")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 50), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON9"));
  btnEmptyMap->SetBitmapDisabled(btnEmptyMap->CreateBitmapDisabled(btnEmptyMap->GetBitmapLabel()));
  btnEmptyMap->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer12->Add(btnEmptyMap, 1, wxEXPAND, 2);
  btnSaveMap = new wxCustomButton(
      this, ID_BUTTON10, _("Save map..."),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FILE_SAVE")), wxART_BUTTON),
      wxDefaultPosition, wxSize(-1, 64), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON10"));
  btnSaveMap->SetBitmapDisabled(btnSaveMap->CreateBitmapDisabled(btnSaveMap->GetBitmapLabel()));
  btnSaveMap->SetBitmapMargin(wxSize(2, 4));
  FlexGridSizer12->Add(btnSaveMap, 1, wxEXPAND, 2);
  btnHelp = new wxCustomButton(
      this, ID_BUTTON2, _("About..."),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_QUESTION")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 60), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON2"));
  btnHelp->SetBitmapDisabled(btnHelp->CreateBitmapDisabled(btnHelp->GetBitmapLabel()));
  btnHelp->SetBitmapMargin(wxSize(5, 4));
  FlexGridSizer12->Add(btnHelp, 1, wxEXPAND, 2);
  btnQuit = new wxCustomButton(
      this, ID_BUTTON3, _("Exit"),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_QUIT")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxSize(-1, 60), wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON3"));
  btnQuit->SetBitmapDisabled(btnQuit->CreateBitmapDisabled(btnQuit->GetBitmapLabel()));
  FlexGridSizer12->Add(btnQuit, 1, wxEXPAND, 2);
  FlexGridSizer4->Add(FlexGridSizer12, 1, wxEXPAND, 0);
  FlexGridSizer1->Add(FlexGridSizer4, 1, wxEXPAND, 0);
  SplitterWindow1 = new wxSplitterWindow(
      this, ID_SPLITTERWINDOW1, wxDefaultPosition, wxDefaultSize, wxSP_3D,
      _T("ID_SPLITTERWINDOW1"));
  SplitterWindow1->SetMinSize(wxSize(10, 10));
  SplitterWindow1->SetMinimumPaneSize(10);
  Panel1 = new wxPanel(
      SplitterWindow1, ID_PANEL1, wxDefaultPosition, wxSize(400, 143), wxTAB_TRAVERSAL,
      _T("ID_PANEL1"));
  FlexGridSizer2 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer2->AddGrowableCol(1);
  FlexGridSizer2->AddGrowableRow(0);
  pnNavSelButtons = new wxPanel(
      Panel1, ID_PANEL6, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL6"));
  FlexGridSizer9 = new wxFlexGridSizer(0, 2, 0, 0);
  FlexGridSizer9->AddGrowableCol(0);
  wxString __wxRadioBoxChoices_1[2] = {
      _("Autonavigation (reactive)"), _("Preprogrammed sequences")};
  rbNavMode = new wxRadioBox(
      pnNavSelButtons, ID_RADIOBOX2, _("Navigator"), wxDefaultPosition, wxDefaultSize, 2,
      __wxRadioBoxChoices_1, 1, wxRA_SPECIFY_COLS, wxDefaultValidator, _T("ID_RADIOBOX2"));
  FlexGridSizer9->Add(rbNavMode, 1, wxEXPAND, 5);
  FlexGridSizer3 = new wxFlexGridSizer(0, 2, 0, 0);
  cbEnableLog = new wxCheckBox(
      pnNavSelButtons, ID_CHECKBOX1, _("Log trajectory"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_CHECKBOX1"));
  cbEnableLog->SetValue(false);
  FlexGridSizer3->Add(cbEnableLog, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer3->Add(-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  cbNavLog = new wxCheckBox(
      pnNavSelButtons, ID_CHECKBOX2, _("Record .navlog files"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_CHECKBOX2"));
  cbNavLog->SetValue(false);
  FlexGridSizer3->Add(cbNavLog, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer3->Add(-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  cbShowPredictedPTG = new wxCheckBox(
      pnNavSelButtons, ID_CHECKBOX3, _("Show selected PTG prediction"), wxDefaultPosition,
      wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX3"));
  cbShowPredictedPTG->SetValue(true);
  FlexGridSizer3->Add(cbShowPredictedPTG, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
  cbDrawShapePath = new wxCheckBox(
      pnNavSelButtons, ID_CHECKBOX4, _("and draw robot shape"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_CHECKBOX4"));
  cbDrawShapePath->SetValue(true);
  FlexGridSizer3->Add(
      cbDrawShapePath, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  cbWaypointsAllowSkip = new wxCheckBox(
      pnNavSelButtons, ID_CHECKBOX5, _("Allow skip waypoints"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_CHECKBOX5"));
  cbWaypointsAllowSkip->SetValue(true);
  FlexGridSizer3->Add(cbWaypointsAllowSkip, 1, wxEXPAND, 5);
  FlexGridSizer9->Add(FlexGridSizer3, 1, wxEXPAND, 5);
  wxString __wxRadioBoxChoices_2[2] = {_("Differential (Ackermann) drive"), _("Holonomic")};
  rbKinType = new wxRadioBox(
      pnNavSelButtons, ID_RADIOBOX1, _("Robot kinematics type"), wxDefaultPosition, wxDefaultSize,
      2, __wxRadioBoxChoices_2, 1, wxRA_SPECIFY_COLS, wxDefaultValidator, _T("ID_RADIOBOX1"));
  FlexGridSizer9->Add(rbKinType, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
  btnGenerateTemplate = new wxButton(
      pnNavSelButtons, ID_BUTTON13, _("Generate template config file..."), wxDefaultPosition,
      wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON13"));
  FlexGridSizer9->Add(btnGenerateTemplate, 1, wxALL | wxALIGN_LEFT | wxALIGN_TOP, 5);
  pnNavSelButtons->SetSizer(FlexGridSizer9);
  FlexGridSizer9->Fit(pnNavSelButtons);
  FlexGridSizer9->SetSizeHints(pnNavSelButtons);
  FlexGridSizer2->Add(pnNavSelButtons, 1, wxEXPAND, 0);
  Notebook1 =
      new wxNotebook(Panel1, ID_NOTEBOOK1, wxDefaultPosition, wxDefaultSize, 0, _T("ID_NOTEBOOK1"));
  Notebook1->SetMinSize(wxSize(100, 50));
  pnParamsGeneral = new wxPanel(
      Notebook1, ID_PANEL2, wxDefaultPosition, wxSize(474, 170), wxTAB_TRAVERSAL, _T("ID_PANEL2"));
  FlexGridSizer7 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer7->AddGrowableCol(0);
  FlexGridSizer7->AddGrowableRow(0);
  edParamsGeneral = new wxTextCtrl(
      pnParamsGeneral, ID_TEXTCTRL1, wxEmptyString, wxDefaultPosition, wxDefaultSize,
      wxTE_PROCESS_ENTER | wxTE_PROCESS_TAB | wxTE_MULTILINE | wxHSCROLL | wxVSCROLL |
          wxALWAYS_SHOW_SB,
      wxDefaultValidator, _T("ID_TEXTCTRL1"));
  edParamsGeneral->SetMinSize(wxSize(-1, 100));
  wxFont edParamsGeneralFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, wxEmptyString,
      wxFONTENCODING_DEFAULT);
  edParamsGeneral->SetFont(edParamsGeneralFont);
  FlexGridSizer7->Add(edParamsGeneral, 1, wxEXPAND, 2);
  pnParamsGeneral->SetSizer(FlexGridSizer7);
  FlexGridSizer7->SetSizeHints(pnParamsGeneral);
  pnParamsReactive = new wxPanel(
      Notebook1, ID_PANEL4, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL4"));
  FlexGridSizer8 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer8->AddGrowableCol(0);
  FlexGridSizer8->AddGrowableRow(0);
  edParamsReactive = new wxTextCtrl(
      pnParamsReactive, ID_TEXTCTRL4,
      _("# "
        "--------------------------------------------------------------------"
        "----\n# Example configuration file for MRPT Reactive Navigation "
        "engine.\n# See C++ documentation: "
        "http://reference.mrpt.org/svn/"
        "classmrpt_1_1nav_1_1_c_reactive_navigation_system.html\n# See ROS "
        "node documentation: http://wiki.ros.org/mrpt_reactivenav2d\n# "
        "--------------------------------------------------------------------"
        "----\n\n[GLOBAL_CONFIG]\n# 0 or `hmVIRTUAL_FORCE_FIELDS`: Virtual "
        "Force Field\n# 1 or `hmSEARCH_FOR_BEST_GAP`: Nearness Diagram "
        "(ND)\n# `hmFULL_EVAL`: Evaluation of all possible "
        "directions\nHOLONOMIC_METHOD = "
        "hmFULL_EVAL\nALARM_SEEMS_NOT_APPROACHING_TARGET_TIMEOUT=100 # "
        "(seconds)\n\nrobotMax_V_mps = 2.0\t\t# Speed "
        "limits\nrobotMax_W_degps =120\n\n\n# Parameters for the "
        "\"FullEval\" Holonomic method\n# "
        "----------------------------------------------------\n[FULL_EVAL_"
        "CONFIG]\nfactorWeights=1.0 1.0 1.0 0.01 3.0\n// 0: Clearness in "
        "direction\n// 1: Closest approach to target along straight line "
        "(Euclidean)\n// 2: Distance of end collision-free point to target "
        "(Euclidean)\n// 3: Hysteresis\n// 4: Clearness to nearest obstacle "
        "along path\n\nTARGET_SLOW_APPROACHING_DISTANCE = 0.70 // Start to "
        "reduce speed when closer than this to target.\nTOO_CLOSE_OBSTACLE = "
        "0.05 // Directions with collision-free distances below this "
        "threshold are not elegible.\nHYSTERESIS_SECTOR_COUNT = 5 // Range "
        "of \"sectors\" (directions) for hysteresis over successive "
        "timesteps\n\nPHASE1_FACTORS = 0 1 2 // Indices of the factors above "
        "to be considered in phase 1\nPHASE2_FACTORS = 1 4 // Indices of the "
        "factors above to be considered in phase 2\nPHASE1_THRESHOLD = 0.6 "
        "// Indices of the factors above to be considered in phase "
        "1\n\n#\tParameters for the \"VFF\" Holonomic method\n# "
        "----------------------------------------------------\n[VFF_CONFIG]"
        "\nTARGET_SLOW_APPROACHING_DISTANCE = 0.1 // For stopping "
        "gradually\nTARGET_ATTRACTIVE_FORCE = 2.000000e+001 // "
        "Dimension-less (may have to be tuned depending on the density of "
        "obstacle sampling)\n\n#\tParameters for the \"Nearness diagram\" "
        "Holonomic method\n# "
        "----------------------------------------------------\n[ND_CONFIG]"
        "\nWIDE_GAP_SIZE_PERCENT = 2.500000e-001 "
        "\nMAX_SECTOR_DIST_FOR_D2_PERCENT = 2.500000e-001 "
        "\nRISK_EVALUATION_SECTORS_PERCENT = 1.000000e-001 "
        "\nRISK_EVALUATION_DISTANCE = 4.000000e-001 // In normalized "
        "ps-meters [0,1]\nTOO_CLOSE_OBSTACLE = 1.500000e-001 // For stopping "
        "gradually\nTARGET_SLOW_APPROACHING_DISTANCE = 0.1 // In normalized "
        "ps-meters\nfactorWeights = 1.00 0.50 2.00 0.40 // [0]=Free space, "
        "[1]=Dist. in sectors, [2]=Closer to target (Euclidean), "
        "[3]=Hysteresis\n\n# "
        "----------------------------------------------------\n#\tParameters "
        "for navigation: DIFFERENTIAL DRIVEN Robot\n# "
        "----------------------------------------------------\n[DIFF_"
        "ReactiveParams]\nweights=0.5 0.05 0.5 2.0 0.5 0.3\n# 1: Free "
        "space\n# 2: Dist. in sectors\t\t\t\n# 3: Heading toward target\n# "
        "4: Closer to target (euclidean)\n# 5: Hysteresis\n# 6: Security "
        "Distance\n\nDIST_TO_TARGET_FOR_SENDING_EVENT=0\t# Minimum. distance "
        "to target for sending the end event. Set to 0 to send it just on "
        "navigation end\n\nMinObstaclesHeight=0.0 \t\t# Minimum coordinate "
        "in the \"z\" axis for an obstacle to be taken into "
        "account.\nMaxObstaclesHeight=1.40 \t# Maximum coordinate in the "
        "\"z\" axis for an obstacle to be taken into "
        "account.\n\nMAX_REFERENCE_DISTANCE = 10.0 # Maximum distance to "
        "build PTGs (in meters), i.e. the visibility \"range\" of tentative "
        "paths\n\n# The constant time of a first-order low-pass filter of "
        "outgoing speed commands, \n# i.e. can be used to impose a maximum "
        "acceleration.\nSPEEDFILTER_TAU = 0.5 // seconds\n\n# PTGs: See "
        "classes derived from mrpt::nav::CParameterizedTrajectoryGenerator ( "
        "http://reference.mrpt.org/svn/"
        "classmrpt_1_1nav_1_1_c_parameterized_trajectory_generator.html)# "
        "refer to papers for "
        "details.\n#---------------------------------------------------------"
        "---------------------\nPTG_COUNT = 3\n\nPTG0_Type = "
        "CPTG_DiffDrive_C\nPTG0_resolution = 0.05 # Look-up-table cell size "
        "or resolution (in meters)\nPTG0_refDistance= 10.0 # Maximum "
        "distance to build PTGs (in meters), i.e. the visibility \"range\" "
        "of tentative paths\nPTG0_num_paths= 121\nPTG0_v_max_mps = "
        "1.0\nPTG0_w_max_dps = 60\nPTG0_K = 1.0\nPTG0_score_priority = "
        "1.0\n\nPTG1_Type = CPTG_DiffDrive_alpha\nPTG1_resolution = 0.05 # "
        "Look-up-table cell size or resolution (in "
        "meters)\nPTG1_refDistance= 10.0 # Maximum distance to build PTGs "
        "(in meters), i.e. the visibility \"range\" of tentative "
        "paths\nPTG1_num_paths = 121\nPTG1_v_max_mps = 1.0\nPTG1_w_max_dps = "
        "60\nPTG1_cte_a0v_deg = 57\nPTG1_cte_a0w_deg = "
        "57\nPTG1_score_priority = 1.0\n\nPTG2_Type = "
        "CPTG_DiffDrive_C\nPTG2_resolution = 0.05 # Look-up-table cell size "
        "or resolution (in meters)\nPTG2_refDistance= 10.0 # Maximum "
        "distance to build PTGs (in meters), i.e. the visibility \"range\" "
        "of tentative paths\nPTG2_num_paths = 121\nPTG2_v_max_mps = "
        "1.0\nPTG2_w_max_dps = 60\nPTG2_K = -1.0\nPTG2_score_priority = "
        "0.5\n\n\n# Default 2D robot shape for collision checks: (ignored in "
        "ROS, superseded by node parameters)\n# Each PTG will use only one "
        "of either (a) polygonal 2D shape or, (b) radius of a circular "
        "shape\nRobotModel_shape2D_xs=-0.2 0.5 0.5 "
        "-0.2\nRobotModel_shape2D_ys=0.3 0.3 -0.3 "
        "-0.3\nRobotModel_circular_shape_radius = 0.5\n\n# "
        "----------------------------------------------------\n#\tParameters "
        "for navigation: DIFFERENTIAL DRIVEN Robot\n# "
        "----------------------------------------------------\n[HOLO_"
        "ReactiveParams]\nweights=0.5 0.05 0.5 2.0 0.2 0.3\n# 1: Free "
        "space\n# 2: Dist. in sectors\t\t\t\n# 3: Heading toward target\n# "
        "4: Closer to target (euclidean)\n# 5: Hysteresis\n# 6: Security "
        "Distance\n\nDIST_TO_TARGET_FOR_SENDING_EVENT=0.6\t# Minimum. "
        "distance to target for sending the end event. Set to 0 to send it "
        "just on navigation end\n\nMinObstaclesHeight=0.0 \t\t# Minimum "
        "coordinate in the \"z\" axis for an obstacle to be taken into "
        "account.\nMaxObstaclesHeight=1.40 \t# Maximum coordinate in the "
        "\"z\" axis for an obstacle to be taken into "
        "account.\n\nMAX_REFERENCE_DISTANCE = 10.0 # Maximum distance to "
        "build PTGs (in meters), i.e. the visibility \"range\" of tentative "
        "paths\n\n# PTGs: See classes derived from "
        "mrpt::nav::CParameterizedTrajectoryGenerator ( "
        "http://reference.mrpt.org/svn/"
        "classmrpt_1_1nav_1_1_c_parameterized_trajectory_generator.html)# "
        "refer to papers for "
        "details.\n#---------------------------------------------------------"
        "---------------------\nPTG_COUNT = 1\n\nPTG0_Type = "
        "CPTG_Holo_Blend\nPTG0_refDistance= 6.0 # Maximum distance to build "
        "PTGs (in meters), i.e. the visibility \"range\" of tentative "
        "paths\nPTG0_num_paths = 100\nPTG0_v_max_mps = 1.5\nPTG0_w_max_dps = "
        "90\nPTG0_T_ramp_max = 0.8\nPTG0_score_priority = 1.0\n\n# Each PTG "
        "will use only one of either (a) polygonal 2D shape or, (b) radius "
        "of a circular shape\nRobotModel_circular_shape_radius = 0.36\n\n"),
      wxDefaultPosition, wxSize(226, 111),
      wxTE_PROCESS_ENTER | wxTE_PROCESS_TAB | wxTE_MULTILINE | wxHSCROLL | wxVSCROLL |
          wxALWAYS_SHOW_SB,
      wxDefaultValidator, _T("ID_TEXTCTRL4"));
  edParamsReactive->SetMinSize(wxSize(-1, 100));
  wxFont edParamsReactiveFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, wxEmptyString,
      wxFONTENCODING_DEFAULT);
  edParamsReactive->SetFont(edParamsReactiveFont);
  FlexGridSizer8->Add(edParamsReactive, 1, wxEXPAND, 2);
  pnParamsReactive->SetSizer(FlexGridSizer8);
  FlexGridSizer8->Fit(pnParamsReactive);
  FlexGridSizer8->SetSizeHints(pnParamsReactive);
  pnParamsPreprog = new wxPanel(
      Notebook1, ID_PANEL3, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL3"));
  FlexGridSizer6 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer6->AddGrowableCol(0);
  FlexGridSizer6->AddGrowableRow(0);
  edManualSeqs = new wxTextCtrl(
      pnParamsPreprog, ID_TEXTCTRL3,
      _("# Params for mode \"Preprogrammed sequences\" \n# Used for debug / "
        "testing. Separate sections for different robot kinematic models. # "
        "Enter below a list of velocity cmds and they will be executed "
        "sequentially. \n\n# Kinematics: Diff driven \n# Format: cmd{%d} = "
        "{time_of_the_cmd_sec} {lin_vel_mps} "
        "{rot_speed_radps}\n[DIFF_CMDS]\ncmd1= 1.0 0.5 0.0\ncmd2= 4.0 1.0 "
        "0.5\ncmd3= 6.0 0.0 0.0\n\n\n# Kinematics: Holonomic \n# Format: "
        "cmd{%d} = {time_of_the_cmd_sec} {velocity_mps} "
        "{global_orientation_rad} {ramp_time_secs} "
        "{rot_speed_radps}\n[HOLO_CMDS]\ncmd1= 1.0 0.5 0.1.0 1.0 0.9\ncmd2= "
        "4.0 1.0 0.4 1.0 0.9\ncmd3= 6.0 0.0 0.0 1.0 0.0\n"),
      wxDefaultPosition, wxSize(230, 167),
      wxTE_PROCESS_ENTER | wxTE_PROCESS_TAB | wxTE_MULTILINE | wxHSCROLL | wxVSCROLL |
          wxALWAYS_SHOW_SB,
      wxDefaultValidator, _T("ID_TEXTCTRL3"));
  edManualSeqs->SetMinSize(wxSize(-1, 100));
  wxFont edManualSeqsFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, wxEmptyString,
      wxFONTENCODING_DEFAULT);
  edManualSeqs->SetFont(edManualSeqsFont);
  FlexGridSizer6->Add(edManualSeqs, 1, wxEXPAND, 0);
  pnParamsPreprog->SetSizer(FlexGridSizer6);
  FlexGridSizer6->Fit(pnParamsPreprog);
  FlexGridSizer6->SetSizeHints(pnParamsPreprog);
  Notebook1->AddPage(pnParamsGeneral, _("Params: Simulation"), true);
  Notebook1->AddPage(pnParamsReactive, _("Params: Reactive navigator"), false);
  Notebook1->AddPage(pnParamsPreprog, _("Params: Preprogrammed seq."), false);
  FlexGridSizer2->Add(Notebook1, 1, wxEXPAND, 2);
  Panel1->SetSizer(FlexGridSizer2);
  FlexGridSizer2->SetSizeHints(Panel1);
  Panel5 = new wxPanel(
      SplitterWindow1, ID_PANEL5, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
      _T("ID_PANEL5"));
  BoxSizer3 = new wxBoxSizer(wxHORIZONTAL);
  SplitterWindow2 = new wxSplitterWindow(
      Panel5, ID_SPLITTERWINDOW2, wxDefaultPosition, wxDefaultSize, wxSP_3D,
      _T("ID_SPLITTERWINDOW2"));
  SplitterWindow2->SetMinSize(wxSize(10, 10));
  SplitterWindow2->SetMinimumPaneSize(10);
  Panel2 = new wxPanel(
      SplitterWindow2, ID_PANEL7, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
      _T("ID_PANEL7"));
  FlexGridSizer10 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer10->AddGrowableCol(1);
  FlexGridSizer10->AddGrowableRow(0);
  StaticText3 = new wxStaticText(
      Panel2, ID_STATICTEXT3, _("Log:"), wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT3"));
  FlexGridSizer10->Add(StaticText3, 1, wxALL | wxALIGN_TOP | wxALIGN_CENTER_HORIZONTAL, 5);
  edLog = new wxTextCtrl(
      Panel2, ID_TEXTCTRL5, wxEmptyString, wxDefaultPosition, wxSize(612, 85),
      wxTE_MULTILINE | wxTE_READONLY | wxTE_DONTWRAP | wxALWAYS_SHOW_SB, wxDefaultValidator,
      _T("ID_TEXTCTRL5"));
  edLog->SetMinSize(wxSize(-1, 70));
  edLog->SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE));
  wxFont edLogFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, wxEmptyString,
      wxFONTENCODING_DEFAULT);
  edLog->SetFont(edLogFont);
  FlexGridSizer10->Add(edLog, 1, wxEXPAND, 0);
  Panel2->SetSizer(FlexGridSizer10);
  FlexGridSizer10->Fit(Panel2);
  FlexGridSizer10->SetSizeHints(Panel2);
  Panel3 = new wxPanel(
      SplitterWindow2, ID_PANEL8, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
      _T("ID_PANEL8"));
  FlexGridSizer5 = new wxFlexGridSizer(2, 2, 0, 0);
  FlexGridSizer5->AddGrowableCol(0);
  FlexGridSizer5->AddGrowableCol(1);
  FlexGridSizer5->AddGrowableRow(1);
  StaticText2 = new wxStaticText(
      Panel3, ID_STATICTEXT2, _("[3D view of the simulated world]"), wxDefaultPosition,
      wxDefaultSize, 0, _T("ID_STATICTEXT2"));
  FlexGridSizer5->Add(
      StaticText2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  StaticText1 = new wxStaticText(
      Panel3, ID_STATICTEXT1, _("[Local view]"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT1"));
  FlexGridSizer5->Add(
      StaticText1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  m_plot3D = new CMyGLCanvas(
      Panel3, ID_XY_GLCANVAS, wxDefaultPosition, wxSize(450, 350), wxTAB_TRAVERSAL,
      _T("ID_XY_GLCANVAS"));
  FlexGridSizer5->Add(
      m_plot3D, 1, wxALL | wxEXPAND | wxFIXED_MINSIZE | wxALIGN_LEFT | wxALIGN_TOP, 1);
  FlexGridSizer11 = new wxFlexGridSizer(4, 1, 0, 0);
  FlexGridSizer11->AddGrowableCol(0);
  FlexGridSizer11->AddGrowableRow(2);
  StaticText4 = new wxStaticText(
      Panel3, ID_STATICTEXT4, _("Show for PTG:"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT4"));
  FlexGridSizer11->Add(StaticText4, 1, wxEXPAND, 5);
  cbSelPTG = new wxChoice(
      Panel3, ID_CHOICE1, wxDefaultPosition, wxDefaultSize, 0, nullptr, 0, wxDefaultValidator,
      _T("ID_CHOICE1"));
  FlexGridSizer11->Add(cbSelPTG, 1, wxEXPAND, 5);
  m_plotLocalView = new CMyGLCanvas(
      Panel3, ID_CUSTOM1, wxDefaultPosition, wxSize(150, 150), wxTAB_TRAVERSAL, _T("ID_CUSTOM1"));
  FlexGridSizer11->Add(m_plotLocalView, 1, wxEXPAND, 1);
  edInfoLocalView = new wxTextCtrl(
      Panel3, ID_TEXTCTRL2, wxEmptyString, wxDefaultPosition, wxDefaultSize,
      wxTE_MULTILINE | wxTE_READONLY | wxTE_DONTWRAP | wxALWAYS_SHOW_SB, wxDefaultValidator,
      _T("ID_TEXTCTRL2"));
  edInfoLocalView->SetMinSize(wxSize(-1, 50));
  edInfoLocalView->SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_BTNFACE));
  wxFont edInfoLocalViewFont(
      8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, wxEmptyString,
      wxFONTENCODING_DEFAULT);
  edInfoLocalView->SetFont(edInfoLocalViewFont);
  FlexGridSizer11->Add(edInfoLocalView, 1, wxEXPAND, 2);
  FlexGridSizer5->Add(FlexGridSizer11, 1, wxEXPAND, 0);
  Panel3->SetSizer(FlexGridSizer5);
  FlexGridSizer5->Fit(Panel3);
  FlexGridSizer5->SetSizeHints(Panel3);
  SplitterWindow2->SplitHorizontally(Panel2, Panel3);
  BoxSizer3->Add(SplitterWindow2, 1, wxEXPAND, 0);
  Panel5->SetSizer(BoxSizer3);
  BoxSizer3->Fit(Panel5);
  BoxSizer3->SetSizeHints(Panel5);
  SplitterWindow1->SplitHorizontally(Panel1, Panel5);
  FlexGridSizer1->Add(SplitterWindow1, 1, wxEXPAND, 0);
  SetSizer(FlexGridSizer1);
  MenuBar1 = new wxMenuBar();
  Menu1 = new wxMenu();
  MenuItem3 = new wxMenuItem(Menu1, ID_MENUITEM4, _("Load map..."), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(MenuItem3);
  MenuItem1 = new wxMenuItem(
      Menu1, idMenuQuit, _("Quit\tAlt-F4"), _("Quit the application"), wxITEM_NORMAL);
  Menu1->Append(MenuItem1);
  MenuBar1->Append(Menu1, _("&File"));
  Menu3 = new wxMenu();
  mnuViewMaxRange =
      new wxMenuItem(Menu3, ID_MENUITEM1, _("Maximum sensor range"), wxEmptyString, wxITEM_CHECK);
  Menu3->Append(mnuViewMaxRange);
  mnuViewRobotPath =
      new wxMenuItem(Menu3, ID_MENUITEM2, _("Robot path"), wxEmptyString, wxITEM_CHECK);
  Menu3->Append(mnuViewRobotPath);
  Menu3->AppendSeparator();
  MenuItem5 =
      new wxMenuItem(Menu3, ID_MENUITEM3, _("Clear robot path"), wxEmptyString, wxITEM_NORMAL);
  Menu3->Append(MenuItem5);
  MenuBar1->Append(Menu3, _("&View"));
  Menu2 = new wxMenu();
  MenuItem2 = new wxMenuItem(
      Menu2, idMenuAbout, _("About\tF1"), _("Show info about this application"), wxITEM_NORMAL);
  Menu2->Append(MenuItem2);
  MenuBar1->Append(Menu2, _("Help"));
  SetMenuBar(MenuBar1);
  StatusBar1 = new wxStatusBar(this, ID_STATUSBAR1, 0, _T("ID_STATUSBAR1"));
  int __wxStatusBarWidths_1[3] = {-2, -2, -3};
  int __wxStatusBarStyles_1[3] = {wxSB_NORMAL, wxSB_NORMAL, wxSB_NORMAL};
  StatusBar1->SetFieldsCount(3, __wxStatusBarWidths_1);
  StatusBar1->SetStatusStyles(3, __wxStatusBarStyles_1);
  SetStatusBar(StatusBar1);
  timRunSimul.SetOwner(this, ID_TIMER1);
  timRunSimul.Start(10, false);
  FlexGridSizer1->SetSizeHints(this);
  Center();

  using rndf = reactive_navigator_demoframe;

  Bind(wxEVT_BUTTON, &rndf::OnbtnStartClick, this, ID_BUTTON4);
  Bind(wxEVT_BUTTON, &rndf::OnbtnStopClick, this, ID_BUTTON5);
  Bind(wxEVT_BUTTON, &rndf::OnbtnPlaceTargetClick, this, ID_BUTTON7);
  Bind(wxEVT_BUTTON, &rndf::OnbtnSetWaypointSeqClick, this, ID_BUTTON12);
  Bind(wxEVT_BUTTON, &rndf::OnbtnPlaceRobotClick, this, ID_BUTTON6);
  Bind(wxEVT_BUTTON, &rndf::OnbtnDrawMapObsClick, this, ID_BUTTON8);
  Bind(wxEVT_BUTTON, &rndf::OnbtnDrawEmptyClick, this, ID_BUTTON11);
  Bind(wxEVT_BUTTON, &rndf::OnbtnLoadMapClick, this, ID_BUTTON1);
  Bind(wxEVT_BUTTON, &rndf::OnbtnEmptyMapClick, this, ID_BUTTON9);
  Bind(wxEVT_BUTTON, &rndf::OnbtnSaveMapClick, this, ID_BUTTON10);
  Bind(wxEVT_BUTTON, &rndf::OnAbout, this, ID_BUTTON2);
  Bind(wxEVT_BUTTON, &rndf::OnbtnQuitClick, this, ID_BUTTON3);
  Bind(wxEVT_RADIOBOX, &rndf::OnrbKinTypeSelect, this, ID_RADIOBOX1);
  Bind(wxEVT_BUTTON, &rndf::OnbtnGenerateTemplateClick, this, ID_BUTTON13);
  Bind(wxEVT_COMMAND_TEXT_UPDATED, &rndf::OnedManualKinRampsText, this, ID_TEXTCTRL3);
  Bind(wxEVT_COMMAND_NOTEBOOK_PAGE_CHANGED, &rndf::OnNotebook1PageChanged1, this, ID_NOTEBOOK1);
  Bind(wxEVT_MENU, &rndf::OnbtnLoadMapClick, this, ID_MENUITEM4);
  Bind(wxEVT_MENU, &rndf::OnQuit, this, idMenuQuit);
  Bind(wxEVT_MENU, &rndf::OnMenuItemChangeVisibleStuff, this, ID_MENUITEM1);
  Bind(wxEVT_MENU, &rndf::OnMenuItemChangeVisibleStuff, this, ID_MENUITEM2);
  Bind(wxEVT_MENU, &rndf::OnMenuItemClearRobotPath, this, ID_MENUITEM3);
  Bind(wxEVT_MENU, &rndf::OnAbout, this, idMenuAbout);
  Bind(wxEVT_TIMER, &rndf::OntimRunSimulTrigger, this, ID_TIMER1);
  //*)

  // Load updated cfg file:
  try
  {
    const std::string sFil = mrpt::system::find_mrpt_shared_dir() +
                             std::string("config_files/navigation-ptgs/reactivenav-app-config.ini");
    edParamsReactive->LoadFile(sFil.c_str());
  }
  catch (...)
  {
  }

  btnStart->SetToolTip(wxT("Initializes and starts the simulator."));
  btnPlaceTarget->SetToolTip(wxT("Left-click on the map to place a navigation command."));
  btnStop->SetToolTip(wxT("Stops the simulation."));
  btnSetWaypointSeq->SetToolTip(
      wxT("Left-click on the map to place waypoints, right click to end and "
          "start navigation."));

  SplitterWindow1->SetSashPosition(200);
  SplitterWindow2->SetSashPosition(90);

  m_plot3D->Bind(wxEVT_MOTION, &reactive_navigator_demoframe::Onplot3DMouseMove, this);
  m_plot3D->Bind(wxEVT_LEFT_DOWN, &reactive_navigator_demoframe::Onplot3DMouseClick, this);
  m_plot3D->Bind(wxEVT_RIGHT_DOWN, &reactive_navigator_demoframe::Onplot3DMouseClick, this);

  mnuViewMaxRange->Check(true);
  mnuViewRobotPath->Check(true);

  updateButtonsEnableState(false);

  // Redirect all output to control:
  m_myRedirector = std::make_unique<CMyRedirector>(edLog, false, 100, true);

  WX_START_TRY

  // Initialize gridmap:
  // -------------------------------
  CMemoryStream s(DEFAULT_GRIDMAP_DATA, sizeof(DEFAULT_GRIDMAP_DATA));
  archiveFrom(s) >> m_gridMap;

  // Populate 3D views:
  // -------------------------------
  auto openGLSceneRef = m_plot3D->getOpenGLSceneRef();
  {
    mrpt::opengl::CGridPlaneXY::Ptr obj =
        mrpt::opengl::CGridPlaneXY::Create(-50, 50, -50, 50, 0, 1);
    obj->setColor_u8(TColor(30, 30, 30, 50));
    openGLSceneRef->insert(obj);
  }

  gl_grid = mrpt::opengl::CSetOfObjects::Create();
  openGLSceneRef->insert(gl_grid);
  this->updateMap3DView();

  // Robot viz is built in OnrbKinTypeSelect()
  gl_robot_local = mrpt::opengl::CSetOfObjects::Create();
  gl_robot = mrpt::opengl::CSetOfObjects::Create();
  {
    mrpt::opengl::CSetOfObjects::Ptr gl_robot_render = mrpt::opengl::CSetOfObjects::Create();
    gl_robot_render->setName("robot_render");
    gl_robot->insert(gl_robot_render);
  }
  openGLSceneRef->insert(gl_robot);

  gl_scan3D = mrpt::opengl::CPlanarLaserScan::Create();
  gl_scan3D->enableLine(false);
  gl_scan3D->enableSurface(true);
  gl_scan3D->setSurfaceColor(1.0f, 0, 0, 0.2f);
  gl_scan3D->setPointSize(2.0);
  gl_scan3D->setLocation(0, 0, 0.01);
  gl_robot->insert(gl_scan3D);

  gl_robot_sensor_range = mrpt::opengl::CDisk::Create(0, 0);
  gl_robot_sensor_range->setColor_u8(TColor(0, 0, 255, 90));
  gl_robot_sensor_range->setLocation(0, 0, 0.01);
  gl_robot->insert(gl_robot_sensor_range);

  gl_robot_path = mrpt::opengl::CSetOfLines::Create();
  gl_robot_path->setLineWidth(1);
  gl_robot_path->setColor_u8(TColor(40, 40, 40, 200));
  gl_robot_path->setLocation(0, 0, 0.01);
  openGLSceneRef->insert(gl_robot_path);

  gl_target = mrpt::opengl::CSetOfObjects::Create();
  gl_target->setVisibility(false);
  {
    mrpt::opengl::CArrow::Ptr obj;
    obj = mrpt::opengl::CArrow::Create(1, 0, 0, 0.2f, 0, 0, 0.4f, 0.05f, 0.15f);
    obj->setColor_u8(TColor(0, 0, 255));
    gl_target->insert(obj);
    obj = mrpt::opengl::CArrow::Create(-1, 0, 0, -0.2f, 0, 0, 0.4f, 0.05f, 0.15f);
    obj->setColor_u8(TColor(0, 0, 255));
    gl_target->insert(obj);
    obj = mrpt::opengl::CArrow::Create(0, 1, 0, 0, 0.2f, 0, 0.4f, 0.05f, 0.15f);
    obj->setColor_u8(TColor(0, 0, 255));
    gl_target->insert(obj);
    obj = mrpt::opengl::CArrow::Create(0, -1, 0, 0, -0.2f, 0, 0.4f, 0.05f, 0.15f);
    obj->setColor_u8(TColor(0, 0, 255));
    gl_target->insert(obj);
    openGLSceneRef->insert(gl_target);
  }

  {
    gl_waypoints_clicking = std::make_shared<opengl::CSetOfObjects>();
    openGLSceneRef->insert(gl_waypoints_clicking);

    gl_waypoints_status = std::make_shared<opengl::CSetOfObjects>();
    openGLSceneRef->insert(gl_waypoints_status);
  }

  {  // Sign of "picking a navigation target":
    m_gl_placing_nav_target = std::make_shared<opengl::CSetOfObjects>();

    mrpt::opengl::CArrow::Ptr obj;
    obj = mrpt::opengl::CArrow::Create(1, 0, 0, 0.2f, 0, 0, 0.4f, 0.05f, 0.15f);
    obj->setColor_u8(TColor(0, 0, 255));
    m_gl_placing_nav_target->insert(obj);
    obj = mrpt::opengl::CArrow::Create(-1, 0, 0, -0.2f, 0, 0, 0.4f, 0.05f, 0.15f);
    obj->setColor_u8(TColor(0, 0, 255));
    m_gl_placing_nav_target->insert(obj);
    obj = mrpt::opengl::CArrow::Create(0, 1, 0, 0, 0.2f, 0, 0.4f, 0.05f, 0.15f);
    obj->setColor_u8(TColor(0, 0, 255));
    m_gl_placing_nav_target->insert(obj);
    obj = mrpt::opengl::CArrow::Create(0, -1, 0, 0, -0.2f, 0, 0.4f, 0.05f, 0.15f);
    obj->setColor_u8(TColor(0, 0, 255));
    m_gl_placing_nav_target->insert(obj);
    m_gl_placing_nav_target->setVisibility(false);  // Start invisible.
    openGLSceneRef->insert(m_gl_placing_nav_target);
  }
  {  // Sign of "replacing the robot":
    m_gl_placing_robot = std::make_shared<opengl::CSetOfObjects>();
    mrpt::opengl::CSetOfObjects::Ptr obj = mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 2.0);
    obj->setColor_u8(TColor(255, 0, 0, 120));
    m_gl_placing_robot->insert(obj);

    m_gl_placing_robot->setVisibility(false);  // Start invisible.
    openGLSceneRef->insert(m_gl_placing_robot);
  }
  {  // Sign of "drawing obstacles":
    m_gl_drawing_obs = std::make_shared<opengl::CSetOfObjects>();

    mrpt::opengl::CCylinder::Ptr obj = mrpt::opengl::CCylinder::Create(0.05f, 0.10f, 1.0f);
    obj->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00, 0x70));
    m_gl_drawing_obs->insert(obj);

    m_gl_drawing_obs->setVisibility(false);  // Start invisible.
    openGLSceneRef->insert(m_gl_drawing_obs);
  }

  openGLSceneRef->insert(mrpt::opengl::stock_objects::CornerXYZ(1));

  gl_robot_ptg_prediction = mrpt::opengl::CSetOfLines::Create();
  gl_robot_ptg_prediction->setName("ptg_prediction");
  gl_robot_ptg_prediction->setLineWidth(2.0);
  gl_robot_ptg_prediction->setColor_u8(mrpt::img::TColor(0x00, 0x00, 0xff));
  gl_robot->insert(gl_robot_ptg_prediction);

  // Set camera:
  m_plot3D->setCameraPointing(0.0f, 0.0f, 0.0f);
  m_plot3D->setZoomDistance(40.0f);
  m_plot3D->setElevationDegrees(70.0f);
  m_plot3D->setAzimuthDegrees(-100.0f);
  m_plot3D->setCameraProjective(true);

  // Init simulator & its adaptor to the navigator
  {
    wxCommandEvent ev;
    OnrbKinTypeSelect(ev);
  }

  // 2D view ==============
  auto openGLSceneLocalViewRef = m_plotLocalView->getOpenGLSceneRef();
  {
    mrpt::opengl::CGridPlaneXY::Ptr obj =
        mrpt::opengl::CGridPlaneXY::Create(-1, 1.001f, -1, 1.001f, 0, 1);
    obj->setColor_u8(TColor(30, 30, 30, 50));
    openGLSceneLocalViewRef->insert(obj);
  }

  gl_line_direction = mrpt::opengl::CSimpleLine::Create();
  gl_line_direction->setLineWidth(4);
  gl_line_direction->setColor_u8(TColor(0, 0, 0));
  openGLSceneLocalViewRef->insert(gl_line_direction);

  gl_rel_target = mrpt::opengl::CPointCloud::Create();
  gl_rel_target->setPointSize(7);
  gl_rel_target->setColor_u8(TColor(0, 0, 255));
  openGLSceneLocalViewRef->insert(gl_rel_target);

  gl_rel_robot = mrpt::opengl::CPointCloud::Create();
  gl_rel_robot->setPointSize(5);
  gl_rel_robot->setColor_u8(TColor(0, 0, 0));
  gl_rel_robot->insertPoint(0, 0, 0);
  openGLSceneLocalViewRef->insert(gl_rel_robot);

  m_plotLocalView->getOpenGLSceneRef()->insert(
      mrpt::opengl::stock_objects::CornerXYSimple(0.1f, 2));
  openGLSceneLocalViewRef->insert(gl_robot_local);

  gl_tp_obstacles = mrpt::opengl::CSetOfLines::Create();
  gl_tp_obstacles->setLineWidth(3);
  gl_tp_obstacles->setColor_u8(TColor(0, 0, 0));
  openGLSceneLocalViewRef->insert(gl_tp_obstacles);

  gl_nd_gaps = mrpt::opengl::CSetOfLines::Create();
  gl_nd_gaps->setLineWidth(1);
  gl_nd_gaps->setColor_u8(TColor(255, 0, 0));
  openGLSceneRef->insert(gl_nd_gaps);

  // Set camera:
  m_plotLocalView->setCameraPointing(0.0f, 0.0f, 0.0f);
  m_plotLocalView->setZoomDistance(2.2f);
  m_plotLocalView->setElevationDegrees(90.0f);
  m_plotLocalView->setAzimuthDegrees(-90.0f);
  m_plotLocalView->setCameraProjective(true);

  // Update positions of stuff:
  this->updateViewsDynamicObjects();

  // Retrieve default parameters for holonomic methods:
  // ------------------------------------------------------
  {
    mrpt::config::CConfigFileMemory cfg;

    m_simul_options.saveToConfigFile(cfg, "SIMULATOR");
    edParamsGeneral->SetValue(cfg.getContent().c_str());
  }

  WX_END_TRY

  this->Maximize();
}

reactive_navigator_demoframe::~reactive_navigator_demoframe()
{
  //(*Destroy(reactive_navigator_demoframe)
  //*)

  // Destroy this first to avoid problems, since it may contain a ref of the
  // simulator object:
  m_navMethod.reset();
}

void reactive_navigator_demoframe::OnQuit(wxCommandEvent&) { Close(); }
void reactive_navigator_demoframe::OnAbout(wxCommandEvent&)
{
  mrpt::gui::show_mrpt_about_box_wxWidgets(this, "ReactiveNavigatorDemo");
}

void reactive_navigator_demoframe::updateMap3DView() { m_gridMap.getVisualizationInto(*gl_grid); }

void reactive_navigator_demoframe::OnbtnPlaceRobotClick(wxCommandEvent& event)
{
  if (m_cursorPickState != cpsPlaceRobot)
  {
    m_cursorPickState = cpsPlaceRobot;
    m_plot3D->SetCursor(*wxCROSS_CURSOR);
  }
  else
  {  // Cancel:
    m_cursorPickState = cpsNone;
    m_plot3D->SetCursor(*wxSTANDARD_CURSOR);
    m_gl_placing_robot->setVisibility(false);
  }
  btnPlaceRobot->SetValue(m_cursorPickState == cpsPlaceRobot);
  btnPlaceRobot->Refresh();
}

void reactive_navigator_demoframe::OnbtnPlaceTargetClick(wxCommandEvent& event)
{
  if (m_cursorPickState != cpsPickTarget)
  {
    m_cursorPickState = cpsPickTarget;
    m_plot3D->SetCursor(*wxCROSS_CURSOR);
  }
  else
  {  // Cancel:
    m_cursorPickState = cpsNone;
    m_plot3D->SetCursor(*wxSTANDARD_CURSOR);
    m_gl_placing_nav_target->setVisibility(false);
  }
  btnPlaceTarget->SetValue(m_cursorPickState == cpsPickTarget);
  btnPlaceTarget->Refresh();
}

void reactive_navigator_demoframe::updateButtonsEnableState(bool is_running)
{
  btnStart->Enable(!is_running);
  btnStart->Refresh();
  btnStop->Enable(is_running);
  btnStop->Refresh();
  btnPlaceTarget->Enable(is_running);
  btnPlaceTarget->Refresh();
  btnSetWaypointSeq->Enable(is_running);
  btnSetWaypointSeq->Refresh();
  Notebook1->Enable(!is_running);
  pnNavSelButtons->Enable(!is_running);
}

void reactive_navigator_demoframe::OnbtnStartClick(wxCommandEvent& event)
{
  updateButtonsEnableState(true);
  if (!reinitSimulator())
  {
    updateButtonsEnableState(false);
    return;
  }
  m_is_running = true;
}

void reactive_navigator_demoframe::OnbtnStopClick(wxCommandEvent& event)
{
  updateButtonsEnableState(false);
  m_is_running = false;
}

// Run simulator (when "running"):
void reactive_navigator_demoframe::OntimRunSimulTrigger(wxTimerEvent& event)
{
  try
  {
    static int decim_call_dump_log = 0;
    if (decim_call_dump_log++ > 10)
    {
      decim_call_dump_log = 0;
      std::cout.flush();
      std::cerr.flush();
    }

    if (m_is_running)
    {
      simulateOneStep(NAV_SIMUL_TIMESTEP_MS * 1e-3);
    }
    updateViewsDynamicObjects();
    timRunSimul.Start(
        NAV_SIMUL_TIMESTEP_MS,
        true);  // execute the simulation step by
                // step ("one shot" timer) so in the
                // event of an exception, we don't
                // output an endless stream of
                // errors.
  }
  catch (const std::exception& e)
  {
    wxMessageBox(mrpt::exception_to_str(e), wxT("Exception"), wxOK, this);
    // Stop:
    wxCommandEvent ev;
    OnbtnStopClick(ev);
  }
}

// Create navigator object & load params from GUI:
bool reactive_navigator_demoframe::reinitSimulator()
{
  WX_START_TRY

  if (m_robotSimul2NavInterface)
  {
    // Params for simulator-to-nav interface:
    CConfigFileMemory cfg;
    cfg.setContent(std::string(edParamsReactive->GetValue().mb_str()));
    // m_robotSimul2NavInterface->loadConfigFile(cfg, "GLOBAL_CONFIG");
  }

  // Delete old & build new navigator:
  m_navMethod.reset();
  CConfigFileMemory cfg;
  switch (rbNavMode->GetSelection())
  {
    case 0:
    {
      mrpt::nav::CReactiveNavigationSystem* react =
          new mrpt::nav::CReactiveNavigationSystem(*m_robotSimul2NavInterface);
      m_navMethod.reset(react);

      react->enableKeepLogRecords();
      react->enableLogFile(cbNavLog->IsChecked());

      cfg.setContent(std::string(edParamsReactive->GetValue().mb_str()));
      break;
    }
    case 1:
    {
      m_navMethod =
          std::make_unique<mrpt::nav::CNavigatorManualSequence>(*m_robotSimul2NavInterface);
      cfg.setContent(std::string(edManualSeqs->GetValue().mb_str()));
      break;
    }
    default:
      throw std::runtime_error("Invalid nav method selected!");
  };
  ASSERT_(m_navMethod.get());
  // Load params:
  std::string sKinPrefix;
  switch (rbKinType->GetSelection())
  {
    case 0:
      sKinPrefix = "DIFF_";
      break;
    case 1:
      sKinPrefix = "HOLO_";
      break;
    default:
      throw std::runtime_error("Invalid kinematic model selected!");
  };

  {
    mrpt::config::CConfigFilePrefixer cfg_prefixer;
    cfg_prefixer.bind(cfg);
    cfg_prefixer.setPrefixes(sKinPrefix /*sections*/, "" /*keys*/);

    m_navMethod->loadConfigFile(cfg_prefixer);
  }
  m_navMethod->initialize();

  // params for simulator itself:
  {
    CConfigFileMemory cfgGeneral;
    cfgGeneral.setContent(std::string(edParamsGeneral->GetValue().mb_str()));
    m_simul_options.loadFromConfigFile(cfgGeneral, "SIMULATOR");
  }

  // Update GUI stuff:
  gl_robot_sensor_range->setDiskRadius(
      m_simul_options.MAX_SENSOR_RADIUS * 1.01, m_simul_options.MAX_SENSOR_RADIUS * 0.99);
  gl_target->setVisibility(false);
  gl_robot_ptg_prediction->clear();

  cbSelPTG->Clear();
  {
    auto* ptg_nav = dynamic_cast<mrpt::nav::CAbstractPTGBasedReactive*>(m_navMethod.get());
    if (ptg_nav)
    {
      for (size_t i = 0; i < ptg_nav->getPTG_count(); i++)
        cbSelPTG->Append(ptg_nav->getPTG(i)->getDescription().c_str());

      if (ptg_nav->getPTG_count() > 0) cbSelPTG->SetSelection(0);
    }
  }

  return true;

  WX_END_TRY
  return false;
}

void reactive_navigator_demoframe::simulateOneStep(double time_step)
{
  WX_START_TRY

  const double simul_time = m_robotSimul->getTime();

  static double LAST_TIM_LIDAR = -1e6;
  static double LAST_TIM_REACTIVE = -1e6;

  if (simul_time - LAST_TIM_LIDAR > 1.0 / m_simul_options.SENSOR_RATE)
  {
    LAST_TIM_LIDAR = simul_time;

    // Simulate 360deg range scan:
    CObservation2DRangeScan simulatedScan;

    simulatedScan.aperture = m_simul_options.SENSOR_FOV;
    simulatedScan.rightToLeft = true;
    simulatedScan.maxRange = m_simul_options.MAX_SENSOR_RADIUS;
    simulatedScan.sensorPose = CPose3D(0.10, 0, 0, 0, 0, 0);

    m_gridMap.laserScanSimulator(
        simulatedScan, CPose2D(m_robotSimul->getCurrentGTPose()), 0.5,
        m_simul_options.SENSOR_NUM_RANGES, m_simul_options.SENSOR_RANGE_NOISE_STD);

    // Build the obstacles points map for the reactive:
    {
      m_latest_obstacles.insertionOptions.minDistBetweenLaserPoints = 0.005f;
      m_latest_obstacles.insertionOptions.also_interpolate = false;

      m_latest_obstacles.clear();  // erase old points
      m_latest_obstacles.insertObservation(simulatedScan);
      // m_latest_obstacles is ref-copied into the robot2nav interface.
    }

    gl_scan3D->setScan(simulatedScan);  // Draw real scan in 3D view
  }

  // Navigate:
  if (simul_time - LAST_TIM_REACTIVE > 1.0 / m_simul_options.NAVIGATION_RATE)
  {
    LAST_TIM_REACTIVE = simul_time;

    m_navMethod->navigationStep();
  }

  // Run robot simulator:
  m_robotSimul->simulateOneTimeStep(time_step);

  // Update path graph:
  const TPoint3D cur_pt(
      m_robotSimul->getCurrentGTPose().x, m_robotSimul->getCurrentGTPose().y, 0.01);

  static int decim_path = 0;
  if (gl_robot_path->empty() || ++decim_path > 10)
  {
    gl_robot_path->appendLine(cur_pt, cur_pt);
  }
  else
  {
    gl_robot_path->appendLineStrip(cur_pt);
    decim_path = 0;
  }

  if (cbEnableLog->IsChecked())
  {
    if (!m_log_trajectory_file.is_open())
    {
      if (m_log_trajectory_file.open("traj_log.txt", mrpt::io::OpenMode::TRUNCATE))
      {
        m_log_trajectory_file.printf("%% File format: TIME  X   Y  PHI  VX  VY OMEGA\n");
      }
    }
    if (m_log_trajectory_file.is_open())  // just in case there was any error opening
    {
      const mrpt::math::TPose2D pose = m_robotSimul->getCurrentGTPose();
      const mrpt::math::TTwist2D vel = m_robotSimul->getCurrentGTVel();
      m_log_trajectory_file.printf(
          "%8.03f  %7.03f %7.03f %7.03f   %7.03f %7.03f %7.03f\n", m_robotSimul->getTime(), pose.x,
          pose.y, mrpt::RAD2DEG(pose.phi), vel.vx, vel.vy, mrpt::RAD2DEG(vel.omega));
    }
  }
  else
  {
    if (m_log_trajectory_file.is_open()) m_log_trajectory_file.close();
  }

  auto* ptg_nav = dynamic_cast<mrpt::nav::CAbstractPTGBasedReactive*>(m_navMethod.get());

  // Clear stuff which will be updated if used below:
  edInfoLocalView->Clear();
  gl_nd_gaps->clear();

  // Update 2D view graphs:
  mrpt::nav::CLogFileRecord lfr;
  if (ptg_nav) ptg_nav->getLastLogRecord(lfr);

  const int sel_PTG = cbSelPTG->GetSelection();
  if (sel_PTG >= 0 && sel_PTG < (int)lfr.infoPerPTG.size())
  {
    const size_t nObs = lfr.infoPerPTG[sel_PTG].TP_Obstacles.size();
    if (lfr.infoPerPTG.size() > 0 && lfr.infoPerPTG[sel_PTG].HLFR &&
        IS_CLASS(*lfr.infoPerPTG[sel_PTG].HLFR, CLogFileRecord_ND))
    {
      CLogFileRecord_ND::Ptr log =
          std::dynamic_pointer_cast<CLogFileRecord_ND>(lfr.infoPerPTG[sel_PTG].HLFR);
      const size_t nGaps = log->gaps_ini.size();

      const string sSitu =
          mrpt::typemeta::TEnumType<CHolonomicND::TSituations>::value2name(log->situation);

      string sLog = mrpt::format("ND situation : %s\n", sSitu.c_str());
      sLog += mrpt::format("Gap count    : %u\n", static_cast<unsigned int>(nGaps));

      edInfoLocalView->SetValue(sLog.c_str());

      gl_nd_gaps->appendLine(0, 0, 0, 0, 0, 0);
      for (size_t i = 0; i < nGaps; i++)
      {
        const size_t N_STEPS = 20;
        for (size_t j = 0; j < N_STEPS; j++)
        {
          const double sec = log->gaps_ini[i] + j * (log->gaps_end[i] - log->gaps_ini[i]) /
                                                    static_cast<double>(N_STEPS - 1);
          const double ang = M_PI * (-1.0 + 2.0 * sec / nObs);

          const double d = lfr.infoPerPTG[sel_PTG].TP_Obstacles[sec] - 0.05;
          gl_nd_gaps->appendLineStrip(d * cos(ang), d * sin(ang), 0);
        }
        gl_nd_gaps->appendLineStrip(0, 0, 0);
      }
    }

    // TP-Obstacles:
    gl_tp_obstacles->clear();
    if (nObs > 1)
    {
      for (size_t i = 0; i <= nObs; i++)
      {
        const double d0 = lfr.infoPerPTG[sel_PTG].TP_Obstacles[i % nObs];
        const double a0 = M_PI * (-1.0 + 2.0 * ((i % nObs) + 0.5) / nObs);
        const double d1 = lfr.infoPerPTG[sel_PTG].TP_Obstacles[(i + 1) % nObs];
        const double a1 = M_PI * (-1.0 + 2.0 * (((i + 1) % nObs) + 0.5) / nObs);
        gl_tp_obstacles->appendLine(
            d0 * cos(a0), d0 * sin(a0), 0.0, d1 * cos(a1), d1 * sin(a1), 0.0);
      }
    }

    // Movement direction:
    {
      const double desiredDirection = lfr.infoPerPTG[sel_PTG].desiredDirection;
      const double d = lfr.infoPerPTG[sel_PTG].desiredSpeed;  /// ROBOT_MAX_SPEED;
      gl_line_direction->setLineCoords(
          0, 0, 0, cos(desiredDirection) * d, sin(desiredDirection) * d, 0);
    }

    // TP Target:
    gl_rel_target->clear();
    for (const auto& t : lfr.infoPerPTG[sel_PTG].TP_Targets)
    {
      gl_rel_target->insertPoint(t.x, t.y, .0);
    }

    // TP Robot:
    gl_rel_robot->setLocation(mrpt::math::TPoint3D(lfr.infoPerPTG[sel_PTG].TP_Robot));

  }  // end valid PTG selected

  // Draw predicted path along selected PTG:
  if (cbShowPredictedPTG->IsChecked() && lfr.infoPerPTG.size() > 0 && ptg_nav)
  {
    // Selected PTG path:
    if (lfr.nSelectedPTG <= (int)ptg_nav->getPTG_count())  // the == case is for "NOP motion cmd"
    {
      const bool is_NOP_op = (lfr.nSelectedPTG == (int)ptg_nav->getPTG_count());
      const int idx_ptg = is_NOP_op ? lfr.ptg_index_NOP : lfr.nSelectedPTG;

      mrpt::nav::CParameterizedTrajectoryGenerator* ptg =
          idx_ptg >= 0 ? ptg_nav->getPTG(idx_ptg) : nullptr;
      if (ptg)
      {
        // Draw path:
        const int selected_k =
            is_NOP_op ? lfr.ptg_last_k_NOP
                      : ptg->alpha2index(lfr.infoPerPTG[lfr.nSelectedPTG].desiredDirection);
        float max_dist = ptg->getRefDistance();
        gl_robot_ptg_prediction->clear();

        ptg->updateNavDynamicState(is_NOP_op ? lfr.ptg_last_navDynState : lfr.navDynState);

        ptg->renderPathAsSimpleLine(selected_k, *gl_robot_ptg_prediction, 0.10, max_dist);
        gl_robot_ptg_prediction->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00));

        // Place it:
        if (is_NOP_op)
        {
          gl_robot_ptg_prediction->setPose(lfr.rel_pose_PTG_origin_wrt_sense_NOP);
        }
        else
        {
          gl_robot_ptg_prediction->setPose(CPose3D());
        }

        // Overlay a sequence of robot shapes:
        if (cbDrawShapePath->IsChecked())
        {
          double min_shape_dists = 1.0;
          for (double d = min_shape_dists; d < max_dist; d += min_shape_dists)
          {
            uint32_t step;
            if (!ptg->getPathStepForDist(selected_k, d, step)) continue;
            const auto p = ptg->getPathPose(selected_k, step);
            ptg->add_robotShape_to_setOfLines(*gl_robot_ptg_prediction, mrpt::poses::CPose2D(p));
          }
        }
      }
    }
  }

  auto* wp_nav = dynamic_cast<CWaypointsNavigator*>(m_navMethod.get());
  if (wp_nav)
  {
    static wxFrame* wxFrWpInfo = nullptr;
    static wxTextCtrl* edWpLog = nullptr;
    if (!wxFrWpInfo)
    {
      wxFrWpInfo = new wxFrame(
          this, -1, wxT("Waypoints info"), wxDefaultPosition, wxSize(400, 150),
          wxRESIZE_BORDER | wxMINIMIZE_BOX | wxMAXIMIZE_BOX | wxCAPTION | wxCLIP_CHILDREN |
              wxSTAY_ON_TOP);

      edWpLog = new wxTextCtrl(
          wxFrWpInfo, wxNewId(), wxEmptyString, wxDefaultPosition, wxSize(400, 150),
          wxTE_MULTILINE | wxTE_READONLY | wxTE_DONTWRAP | wxALWAYS_SHOW_SB, wxDefaultValidator,
          _T("ID_TEXTCTRL_WP"));
      edWpLog->SetMinSize(wxSize(190, 60));
      wxFont edLogFont(8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL);
      edWpLog->SetFont(edLogFont);
    }

    TWaypointStatusSequence wp_status;
    wp_nav->getWaypointNavStatus(wp_status);
    const std::string sWpLog = wp_status.getAsText();

    if (!wp_status.waypoints.empty())
      if (!wxFrWpInfo->IsShown()) wxFrWpInfo->Show();

    if (wxFrWpInfo->IsShown()) edWpLog->SetValue(sWpLog.c_str());

    // Plot waypoints being clicked by the user graphically:
    m_waypoints_clicked.getAsOpenglVisualization(*gl_waypoints_clicking);

    // Plot firmly set waypoints and their status:
    wp_status.getAsOpenglVisualization(*gl_waypoints_status);
  }

  WX_END_TRY
}

void reactive_navigator_demoframe::updateViewsDynamicObjects()
{
  gl_robot->setPose(m_robotSimul->getCurrentGTPose());

  // animate target:
  {
    const double TARGET_BOUNCE_MIN = 0.7;
    const double TARGET_BOUNCE_MAX = 1;

    const double TARGET_BOUNCE_PERIOD = 1.0;
    const double t = fmod(m_runtime.Tac(), TARGET_BOUNCE_PERIOD) / TARGET_BOUNCE_PERIOD;

    // Parabolic path
    const double s = 4 * t * (TARGET_BOUNCE_MAX - TARGET_BOUNCE_MIN) * (1 - t) + TARGET_BOUNCE_MIN;

    gl_target->setLocation(m_targetPoint.x, m_targetPoint.y, 0);
    gl_target->setScale(s);
  }

  // Labels:
  StatusBar1->SetStatusText(
      (mrpt::format(
           "Robot pose: %s vel: %s", m_robotSimul->getCurrentGTPose().asString().c_str(),
           m_robotSimul->getCurrentGTVel().asString().c_str())
           .c_str()),
      0);
  StatusBar1->SetStatusText(
      (mrpt::format(
           "Simul time: %.03f Target: (%.03f,%.03f)", m_robotSimul->getTime(), m_targetPoint.x,
           m_targetPoint.y)
           .c_str()),
      1);

  // Show/hide:
  gl_robot_sensor_range->setVisibility(mnuViewMaxRange->IsChecked());
  gl_robot_path->setVisibility(mnuViewRobotPath->IsChecked());

  // Refresh:
  m_plot3D->Refresh();
  m_plotLocalView->Refresh();
}

void reactive_navigator_demoframe::Onplot3DMouseMove(wxMouseEvent& event)
{
  int X, Y;
  event.GetPosition(&X, &Y);
  bool skip_normal_process = false;

  // Intersection of 3D ray with ground plane ====================
  TLine3D ray;
  m_plot3D->getOpenGLSceneRef()->getViewport("main")->get3DRayForPixelCoord(X, Y, ray);
  // Create a 3D plane, e.g. Z=0
  const TPlane ground_plane(TPoint3D(0, 0, 0), TPoint3D(1, 0, 0), TPoint3D(0, 1, 0));
  // Intersection of the line with the plane:
  TObject3D inters;
  intersect(ray, ground_plane, inters);
  // Interpret the intersection as a point, if there is an intersection:
  TPoint3D inters_pt;
  if (inters.getPoint(inters_pt))
  {
    m_curCursorPos.x = inters_pt.x;
    m_curCursorPos.y = inters_pt.y;

    switch (m_cursorPickState)
    {
      case cpsPickTarget:
      case cpsPickWaypoints:
      {
        m_gl_placing_nav_target->setVisibility(true);
        m_gl_placing_nav_target->setLocation(m_curCursorPos.x, m_curCursorPos.y, 0.01);
      }
      break;
      case cpsPlaceRobot:
      {
        m_gl_placing_robot->setVisibility(true);
        m_gl_placing_robot->setLocation(m_curCursorPos.x, m_curCursorPos.y, 0.01);
      }
      break;
      case cpsDrawObstacles:
      case cpsDrawClear:
      {
        m_gl_drawing_obs->setLocation(m_curCursorPos.x, m_curCursorPos.y, 0.01);
        if (event.ButtonIsDown(wxMOUSE_BTN_LEFT))
        {
          const int cx = m_gridMap.x2idx(m_curCursorPos.x);
          const int cy = m_gridMap.y2idx(m_curCursorPos.y);
          if (cx >= 0 && cy >= 0 && cx < (int)m_gridMap.getSizeX() &&
              cy < (int)m_gridMap.getSizeY())
          {
            m_gridMap.setCell(cx, cy, m_cursorPickState == cpsDrawObstacles ? 0.01f : 0.99f);
            updateMap3DView();
            m_plot3D->Refresh();
          }
          skip_normal_process = true;
        }
      }
      break;
      default:
        break;
    };

    StatusBar1->SetStatusText(
        wxString::Format(wxT("X=%.03f Y=%.04f Z=0"), m_curCursorPos.x, m_curCursorPos.y), 2);
  }

  if (!skip_normal_process)
  {
    // Do normal process in that class:
    m_plot3D->OnMouseMove(event);
  }
}

void reactive_navigator_demoframe::Onplot3DMouseClick(wxMouseEvent& event)
{
  WX_START_TRY

  bool skip_normal_process = false;

  switch (m_cursorPickState)
  {
    case cpsPickWaypoints:
    {
      if (event.ButtonIsDown(wxMOUSE_BTN_LEFT))
      {
        const bool allow_skip_wps = !event.ShiftDown();
        double heading = TWaypoint::INVALID_NUM;
        if (event.ControlDown())
        {
          this->edWayPtHeading->GetValue().ToCDouble(&heading);
          heading *= M_PI / 180;
        }
        m_waypoints_clicked.waypoints.emplace_back(
            m_curCursorPos.x, m_curCursorPos.y, 0.2 /* allowed dist */, allow_skip_wps, heading);
      }
      if (event.ButtonIsDown(wxMOUSE_BTN_RIGHT))
      {
        btnPlaceTarget->SetValue(false);
        btnPlaceTarget->Refresh();
        m_gl_placing_nav_target->setVisibility(false);

        auto* wp_nav = dynamic_cast<CWaypointsNavigator*>(m_navMethod.get());
        if (wp_nav) wp_nav->navigateWaypoints(m_waypoints_clicked);

        m_waypoints_clicked.clear();

        m_plot3D->SetCursor(*wxSTANDARD_CURSOR);  // End of cross cursor
        m_cursorPickState = cpsNone;              // end of mode
      }
    }
    break;

    case cpsPickTarget:
      if (event.ButtonIsDown(wxMOUSE_BTN_LEFT))
      {
        m_targetPoint = m_curCursorPos;

        btnPlaceTarget->SetValue(false);
        btnPlaceTarget->Refresh();
        m_gl_placing_nav_target->setVisibility(false);

        // Issue a new navigation cmd:
        CAbstractPTGBasedReactive::TNavigationParamsPTG navParams;
        navParams.target.target_coords.x = m_targetPoint.x;
        navParams.target.target_coords.y = m_targetPoint.y;
        navParams.target.targetAllowedDistance = 0.40f;
        navParams.target.targetIsRelative = false;

        // Optional: restrict the PTGs to use
        // navParams.restrict_PTG_indices.push_back(1);

        m_navMethod->navigate(&navParams);
        gl_target->setVisibility(true);

        m_plot3D->SetCursor(*wxSTANDARD_CURSOR);  // End of cross cursor
        m_cursorPickState = cpsNone;              // end of mode
      }
      break;
    case cpsPlaceRobot:
      if (event.ButtonIsDown(wxMOUSE_BTN_LEFT))
      {
        m_robotSimul->setCurrentGTPose(mrpt::math::TPose2D(m_curCursorPos.x, m_curCursorPos.y, .0));

        btnPlaceRobot->SetValue(false);
        btnPlaceRobot->Refresh();
        m_gl_placing_robot->setVisibility(false);
        m_plot3D->SetCursor(*wxSTANDARD_CURSOR);  // End of cross cursor
        m_cursorPickState = cpsNone;              // end of mode
      }
      break;
    case cpsDrawObstacles:
    case cpsDrawClear:
      if (event.ButtonIsDown(wxMOUSE_BTN_LEFT))
      {
        const int cx = m_gridMap.x2idx(m_curCursorPos.x);
        const int cy = m_gridMap.y2idx(m_curCursorPos.y);
        if (cx >= 0 && cy >= 0 && cx < (int)m_gridMap.getSizeX() && cy < (int)m_gridMap.getSizeY())
        {
          m_gridMap.setCell(cx, cy, m_cursorPickState == cpsDrawObstacles ? 0.01f : 0.99f);
          updateMap3DView();
          m_plot3D->Refresh();
        }
        skip_normal_process = true;
      }
      break;
    default:
      break;
  }

  if (!skip_normal_process)
  {
    // Do normal process in that class:
    m_plot3D->OnMouseDown(event);
  }
  WX_END_TRY
}

// ==== reactive_navigator_demoframe::TOptions ======
void reactive_navigator_demoframe::TOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
  MRPT_START

  // Load from config text:
  MRPT_LOAD_CONFIG_VAR(MAX_SENSOR_RADIUS, double, source, section);
  MRPT_LOAD_CONFIG_VAR_DEGREES(SENSOR_FOV, source, section);
  MRPT_LOAD_CONFIG_VAR(SENSOR_NUM_RANGES, uint64_t, source, section);
  MRPT_LOAD_CONFIG_VAR(SENSOR_RANGE_NOISE_STD, double, source, section);
  MRPT_LOAD_CONFIG_VAR(SENSOR_RATE, double, source, section);
  MRPT_LOAD_CONFIG_VAR(NAVIGATION_RATE, double, source, section);

  MRPT_END
}

void reactive_navigator_demoframe::TOptions::saveToConfigFile(
    mrpt::config::CConfigFileBase& cfg, const std::string& section) const
{
  MRPT_START
  const int WN = 40, WV = 20;

  cfg.write(
      section, "MAX_SENSOR_RADIUS", MAX_SENSOR_RADIUS, WN, WV,
      "Maximum range of the LiDAR sensor (meters)");
  cfg.write(
      section, "SENSOR_FOV", 180.0 / M_PI * SENSOR_FOV, WN, WV,
      "Horizontal Field of view of the LiDAR (deg)");
  cfg.write(
      section, "SENSOR_NUM_RANGES", SENSOR_NUM_RANGES, WN, WV,
      "Number of ranges in the 360deg sensor FOV");
  cfg.write(
      section, "SENSOR_RANGE_NOISE_STD", SENSOR_RANGE_NOISE_STD, WN, WV,
      "Sensor noise (one sigma, in meters)");
  cfg.write(section, "SENSOR_RATE", SENSOR_RATE, WN, WV, "Sensor rate (Hz)");
  cfg.write(section, "NAVIGATION_RATE", NAVIGATION_RATE, WN, WV, "Navigation algorithm rate (Hz)");

  MRPT_END
}

void reactive_navigator_demoframe::OnMenuItemChangeVisibleStuff(wxCommandEvent& event)
{
  updateViewsDynamicObjects();
}

void reactive_navigator_demoframe::OnMenuItemClearRobotPath(wxCommandEvent& event)
{
  gl_robot_path->clear();
  updateViewsDynamicObjects();
}

void reactive_navigator_demoframe::OnbtnLoadMapClick(wxCommandEvent& event)
{
  WX_START_TRY

  wxFileDialog dlg(
      this, _("Select grid map to load"), _("."), _("grid.png"),
      wxT("Image files (*.png,*.jpg,*.gif) or binary gridmap files "
          "|*.png;*.jpg;*.gif;*.gridmap;*.gridmap.gz|All files (*.*)|*.*"),
      wxFD_OPEN | wxFD_FILE_MUST_EXIST);

  if (dlg.ShowModal() != wxID_OK) return;

  const wxString sFil = dlg.GetPath();
  const std::string fil = std::string(sFil.mb_str());

  const std::string fil_ext = mrpt::system::extractFileExtension(fil, true);

  if (mrpt::system::lowerCase(fil_ext) == "gridmap")
  {
    CFileGZInputStream f(fil);
    archiveFrom(f) >> m_gridMap;
  }
  else
  {
    // Try loading the image:
    CImage img;
    if (!img.loadFromFile(fil, 0 /* force grayscale */))
    {
      wxMessageBox(_("Error"), _("Can't load the image file (check its format)."));
    }
    else
    {
      // We also need the size of each pixel:
      double cx = -1;
      double cy = -1;
      double cell_size = 0.05;

      const wxString sCellSize = wxGetTextFromUser(
          _("Enter the size (in meters) of each pixel:"), _("Grid parameters"), _("0.05"), this);
      const wxString sCX = wxGetTextFromUser(
          _("Enter the central pixel (x-coordinate), or -1 = the image "
            "center:"),
          _("Grid parameters"), _("-1"), this);
      const wxString sCY = wxGetTextFromUser(
          _("Enter the central pixel (y-coordinate), or -1 = the image "
            "center:"),
          _("Grid parameters"), _("-1"), this);

      if (sCellSize.ToCDouble(&cell_size) && sCX.ToCDouble(&cx) && sCY.ToCDouble(&cy))
      {
        if (!m_gridMap.loadFromBitmap(img, cell_size, {cx, cy}))
          wxMessageBox(_("Error"), _("Can't load the image file into the gridmap..."));
      }
      else
        wxMessageBox(_("Error"), _("Error parsing the numbers you entered..."));
    }
  }

  updateMap3DView();
  m_plot3D->Refresh();

  WX_END_TRY
}

void reactive_navigator_demoframe::OnNotebook1PageChanged(wxNotebookEvent& event) {}

void reactive_navigator_demoframe::OnNotebook1PageChanged1(wxNotebookEvent& event) {}

void reactive_navigator_demoframe::OnedManualKinRampsText(wxCommandEvent& event) {}

void reactive_navigator_demoframe::OnbtnQuitClick(wxCommandEvent& event) { Close(); }

void create_viz_robot_holo(mrpt::opengl::CSetOfObjects& objs)
{
  objs.clear();
  {
    mrpt::opengl::CSetOfObjects::Ptr gl_xyz =
        mrpt::opengl::stock_objects::CornerXYZSimple(1.0f, 2.0f);
    gl_xyz->setLocation(0.0, 0.0, 1.25);
    objs.insert(gl_xyz);
  }

  mrpt::opengl::CCylinder::Ptr obj = mrpt::opengl::CCylinder::Create(
      0.36f /*base radius*/, 0.20f /*top radius */, 1.2f /*height*/);
  obj->setColor_u8(TColor::red());
  objs.insert(obj);
}
void create_viz_robot_diff(mrpt::opengl::CSetOfObjects& objs)
{
  objs.clear();
  objs.insert(mrpt::opengl::stock_objects::RobotPioneer());
}

void reactive_navigator_demoframe::OnrbKinTypeSelect(wxCommandEvent& event)
{
  // We must first detach the simulator from the navigator object before
  // deleting it:
  m_navMethod.reset();

  // Delete old & build new simulator:
  m_robotSimul2NavInterface.reset();
  m_robotSimul.reset();

  if (gl_robot_path) gl_robot_path->clear();
  if (gl_target) gl_target->setVisibility(false);

  switch (rbKinType->GetSelection())
  {
    case 0:
    {
      auto* sim = new mrpt::kinematics::CVehicleSimul_DiffDriven();
      m_robotSimul.reset(sim);
      m_robotSimul2NavInterface =
          std::make_unique<MyRobot2NavInterface_Diff>(*sim, m_latest_obstacles);
      // Opengl viz:
      create_viz_robot_diff(*std::dynamic_pointer_cast<mrpt::opengl::CSetOfObjects>(
          gl_robot->getByName("robot_render")));
      create_viz_robot_diff(*gl_robot_local);
      gl_robot_local->setScale(1.0 / m_simul_options.MAX_SENSOR_RADIUS);
    }
    break;
    case 1:
    {
      auto* sim = new mrpt::kinematics::CVehicleSimul_Holo();
      m_robotSimul.reset(sim);
      m_robotSimul2NavInterface =
          std::make_unique<MyRobot2NavInterface_Holo>(*sim, m_latest_obstacles);
      // Opengl viz:
      create_viz_robot_holo(*std::dynamic_pointer_cast<mrpt::opengl::CSetOfObjects>(
          gl_robot->getByName("robot_render")));
      create_viz_robot_holo(*gl_robot_local);
      gl_robot_local->setScale(1.0 / m_simul_options.MAX_SENSOR_RADIUS);
    }
    break;
    default:
      throw std::runtime_error("Invalid kinematic model selected!");
  };
}

void reactive_navigator_demoframe::OnbtnEmptyMapClick(wxCommandEvent& event)
{
  WX_START_TRY

  double lx = 30., ly = 30.;
  double res = 0.25;
  wxString s;
  s = wxGetTextFromUser(_("Width (x) [meters]:"), _("New map"), _("40.0"), this);
  if (s.IsEmpty()) return;
  if (!s.ToCDouble(&lx))
  {
    wxMessageBox(_("Invalid number"));
    return;
  }

  s = wxGetTextFromUser(_("Height (y) [meters]:"), _("New map"), _("30.0"), this);
  if (s.IsEmpty()) return;
  if (!s.ToCDouble(&ly))
  {
    wxMessageBox(_("Invalid number"));
    return;
  }

  s = wxGetTextFromUser(_("Grid resolution [meters]:"), _("New map"), _("0.25"), this);
  if (s.IsEmpty()) return;
  if (!s.ToCDouble(&res))
  {
    wxMessageBox(_("Invalid number"));
    return;
  }

  m_gridMap.setSize(-.5 * lx, .5 * lx, -.5 * ly, .5 * ly, res, 0.99f);

  updateMap3DView();
  m_plot3D->Refresh();

  WX_END_TRY
}

void reactive_navigator_demoframe::OnbtnSaveMapClick(wxCommandEvent& event)
{
  WX_START_TRY

  wxFileDialog dlg(
      this, _("Save gridmap to file"), _("."), _("map.gridmap.gz"),
      wxT("Binary gridmap files "
          "(*.gridmap,*.gridmap.gz)|*.gridmap;*.gridmap.gz|All files "
          "(*.*)|*.*"),
      wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

  if (dlg.ShowModal() != wxID_OK) return;

  CFileGZOutputStream f(std::string(dlg.GetPath().mb_str()));
  archiveFrom(f) << m_gridMap;

  WX_END_TRY
}

void reactive_navigator_demoframe::OnbtnDrawMapObsClick(wxCommandEvent& event)
{
  if (m_cursorPickState != cpsDrawObstacles)
  {
    m_cursorPickState = cpsDrawObstacles;
    m_plot3D->SetCursor(*wxCROSS_CURSOR);
  }
  else
  {  // Cancel:
    m_cursorPickState = cpsNone;
    m_plot3D->SetCursor(*wxSTANDARD_CURSOR);
  }
  btnDrawMapObs->SetValue(m_cursorPickState == cpsDrawObstacles);
  btnDrawMapObs->Refresh();
  btnDrawEmpty->SetValue(m_cursorPickState == cpsDrawClear);
  btnDrawEmpty->Refresh();
  m_gl_drawing_obs->setVisibility(
      m_cursorPickState == cpsDrawObstacles || m_cursorPickState == cpsDrawClear);
  m_plot3D->Refresh();
}

void reactive_navigator_demoframe::OnbtnDrawEmptyClick(wxCommandEvent& event)
{
  if (m_cursorPickState != cpsDrawClear)
  {
    m_cursorPickState = cpsDrawClear;
    m_plot3D->SetCursor(*wxCROSS_CURSOR);
  }
  else
  {  // Cancel:
    m_cursorPickState = cpsNone;
    m_plot3D->SetCursor(*wxSTANDARD_CURSOR);
  }
  btnDrawMapObs->SetValue(m_cursorPickState == cpsDrawObstacles);
  btnDrawMapObs->Refresh();
  btnDrawEmpty->SetValue(m_cursorPickState == cpsDrawClear);
  btnDrawEmpty->Refresh();
  m_gl_drawing_obs->setVisibility(
      m_cursorPickState == cpsDrawObstacles || m_cursorPickState == cpsDrawClear);
  m_plot3D->Refresh();
}

void reactive_navigator_demoframe::OnbtnSetWaypointSeqClick(wxCommandEvent& event)
{
  auto* wp_nav = dynamic_cast<CWaypointsNavigator*>(m_navMethod.get());
  if (!wp_nav)
  {
    wxMessageBox(wxT("Navigator class does not support waypoints sequences!"));
    return;
  }

  if (m_cursorPickState != cpsPickWaypoints)
  {
    m_cursorPickState = cpsPickWaypoints;
    m_plot3D->SetCursor(*wxCROSS_CURSOR);
    m_waypoints_clicked.clear();
  }
  else
  {  // Cancel:
    m_cursorPickState = cpsNone;
    m_plot3D->SetCursor(*wxSTANDARD_CURSOR);
  }
  btnSetWaypointSeq->SetValue(m_cursorPickState == cpsPickWaypoints);
  btnSetWaypointSeq->Refresh();
  m_gl_placing_nav_target->setVisibility(m_cursorPickState == cpsPickWaypoints);
  m_plot3D->Refresh();
}

void reactive_navigator_demoframe::OnbtnGenerateTemplateClick(wxCommandEvent& event)
{
  WX_START_TRY;

  mrpt::nav::CReactiveNavigationSystem react(*m_robotSimul2NavInterface);

  mrpt::config::CConfigFileMemory cfgMem;
  react.saveConfigFile(cfgMem);
  edParamsReactive->SetValue(cfgMem.getContent().c_str());

  WX_END_TRY;
}
