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

#include <mrpt/gui/about_box.h>
#include <wx/msgdlg.h>

//(*InternalHeaders(holonomic_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/gui/WxUtils.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/poses/CPoint2D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/serialization/CArchive.h>

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

// 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/gui/about_box.h>
#include <mrpt/nav.h>
#include <mrpt/opengl.h>
#include <mrpt/system/filesystem.h>

#include <memory>

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

//(*IdInit(holonomic_navigator_demoFrame)
const long holonomic_navigator_demoFrame::ID_BUTTON1 = wxNewId();
const long holonomic_navigator_demoFrame::ID_BUTTON2 = wxNewId();
const long holonomic_navigator_demoFrame::ID_BUTTON3 = wxNewId();
const long holonomic_navigator_demoFrame::ID_BUTTON6 = wxNewId();
const long holonomic_navigator_demoFrame::ID_BUTTON7 = wxNewId();
const long holonomic_navigator_demoFrame::ID_BUTTON4 = wxNewId();
const long holonomic_navigator_demoFrame::ID_BUTTON5 = wxNewId();
const long holonomic_navigator_demoFrame::ID_RADIOBOX1 = wxNewId();
const long holonomic_navigator_demoFrame::ID_TEXTCTRL1 = wxNewId();
const long holonomic_navigator_demoFrame::ID_PANEL1 = wxNewId();
const long holonomic_navigator_demoFrame::ID_PANEL2 = wxNewId();
const long holonomic_navigator_demoFrame::ID_NOTEBOOK1 = wxNewId();
const long holonomic_navigator_demoFrame::ID_STATICTEXT2 = wxNewId();
const long holonomic_navigator_demoFrame::ID_STATICTEXT1 = wxNewId();
const long holonomic_navigator_demoFrame::ID_XY_GLCANVAS = wxNewId();
const long holonomic_navigator_demoFrame::ID_CUSTOM1 = wxNewId();
const long holonomic_navigator_demoFrame::ID_TEXTCTRL2 = wxNewId();
const long holonomic_navigator_demoFrame::ID_MENUITEM4 = wxNewId();
const long holonomic_navigator_demoFrame::idMenuQuit = wxNewId();
const long holonomic_navigator_demoFrame::ID_MENUITEM1 = wxNewId();
const long holonomic_navigator_demoFrame::ID_MENUITEM2 = wxNewId();
const long holonomic_navigator_demoFrame::ID_MENUITEM3 = wxNewId();
const long holonomic_navigator_demoFrame::idMenuAbout = wxNewId();
const long holonomic_navigator_demoFrame::ID_STATUSBAR1 = wxNewId();
const long holonomic_navigator_demoFrame::ID_TIMER1 = wxNewId();
//*)

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

holonomic_navigator_demoFrame::holonomic_navigator_demoFrame(wxWindow* parent, wxWindowID id) :
    m_gridMap(),
    m_targetPose(-5, -7, 0),
    m_robotPose(0, 0, 0),
    m_curCursorPos(0, 0),
    m_cursorPickState(cpsNone)
{
  // Load my custom icons:
  wxArtProvider::Push(new MyArtProvider);

  //(*Initialize(holonomic_navigator_demoFrame)
  wxFlexGridSizer* FlexGridSizer4;
  wxMenuItem* MenuItem2;
  wxFlexGridSizer* FlexGridSizer3;
  wxMenuItem* MenuItem1;
  wxFlexGridSizer* FlexGridSizer5;
  wxFlexGridSizer* FlexGridSizer2;
  wxBoxSizer* BoxSizer2;
  wxMenu* Menu1;
  wxFlexGridSizer* FlexGridSizer7;
  wxBoxSizer* BoxSizer1;
  wxMenuBar* MenuBar1;
  wxFlexGridSizer* FlexGridSizer1;
  wxMenu* Menu2;

  Create(
      parent, id, _("Holonomic Navigation Demo - Part of MRPT"), wxDefaultPosition, wxDefaultSize,
      wxDEFAULT_FRAME_STYLE, _T("id"));
  FlexGridSizer1 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer1->AddGrowableCol(0);
  FlexGridSizer1->AddGrowableRow(1);
  FlexGridSizer2 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer2->AddGrowableCol(1);
  FlexGridSizer4 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer4->AddGrowableCol(0);
  FlexGridSizer4->AddGrowableRow(0);
  FlexGridSizer4->AddGrowableRow(1);
  BoxSizer1 = new wxBoxSizer(wxHORIZONTAL);
  btnLoadMap = new wxCustomButton(
      this, ID_BUTTON1, _("Load map..."),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FOLDER")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON1"));
  btnLoadMap->SetBitmapDisabled(btnLoadMap->CreateBitmapDisabled(btnLoadMap->GetBitmapLabel()));
  btnLoadMap->SetBitmapMargin(wxSize(2, 4));
  BoxSizer1->Add(btnLoadMap, 1, wxEXPAND, 1);
  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, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON2"));
  btnHelp->SetBitmapDisabled(btnHelp->CreateBitmapDisabled(btnHelp->GetBitmapLabel()));
  btnHelp->SetBitmapMargin(wxSize(5, 4));
  BoxSizer1->Add(btnHelp, 1, wxEXPAND, 1);
  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, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON3"));
  btnQuit->SetBitmapDisabled(btnQuit->CreateBitmapDisabled(btnQuit->GetBitmapLabel()));
  BoxSizer1->Add(btnQuit, 1, wxEXPAND, 1);
  FlexGridSizer4->Add(BoxSizer1, 1, wxEXPAND, 0);
  BoxSizer2 = new wxBoxSizer(wxHORIZONTAL);
  btnPlaceRobot = new wxCustomButton(
      this, ID_BUTTON6, _("Place robot..."),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FIND")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON6"));
  btnPlaceRobot->SetBitmapDisabled(
      btnPlaceRobot->CreateBitmapDisabled(btnPlaceRobot->GetBitmapLabel()));
  btnPlaceRobot->SetBitmapMargin(wxSize(2, 4));
  BoxSizer2->Add(btnPlaceRobot, 1, wxEXPAND, 2);
  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, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON7"));
  btnPlaceTarget->SetBitmapDisabled(
      btnPlaceTarget->CreateBitmapDisabled(btnPlaceTarget->GetBitmapLabel()));
  btnPlaceTarget->SetBitmapMargin(wxSize(2, 4));
  BoxSizer2->Add(btnPlaceTarget, 1, wxEXPAND, 2);
  btnStart = new wxCustomButton(
      this, ID_BUTTON4, _("START"),
      wxArtProvider::GetBitmap(
          wxART_MAKE_ART_ID_FROM_STR(_T("wxART_GO_FORWARD")),
          wxART_MAKE_CLIENT_ID_FROM_STR(wxString(wxEmptyString))),
      wxDefaultPosition, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON4"));
  btnStart->SetBitmapDisabled(btnStart->CreateBitmapDisabled(btnStart->GetBitmapLabel()));
  btnStart->SetBitmapMargin(wxSize(2, 4));
  BoxSizer2->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, wxDefaultSize, wxCUSTBUT_BUTTON | wxCUSTBUT_BOTTOM, wxDefaultValidator,
      _T("ID_BUTTON5"));
  btnStop->SetBitmapDisabled(btnStop->CreateBitmapDisabled(btnStop->GetBitmapLabel()));
  btnStop->SetBitmapMargin(wxSize(2, 4));
  BoxSizer2->Add(btnStop, 1, wxEXPAND, 2);
  FlexGridSizer4->Add(BoxSizer2, 1, wxEXPAND, 0);
  FlexGridSizer2->Add(FlexGridSizer4, 1, wxEXPAND, 0);
  Notebook1 =
      new wxNotebook(this, ID_NOTEBOOK1, wxDefaultPosition, wxDefaultSize, 0, _T("ID_NOTEBOOK1"));
  Panel1 = new wxPanel(
      Notebook1, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL1"));
  FlexGridSizer7 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer7->AddGrowableCol(1);
  FlexGridSizer7->AddGrowableRow(0);
  wxString __wxRadioBoxChoices_1[2] = {_("VFF (Virtual Force Field)"), _("ND (Nearness  Diagram)")};
  rbHoloMethod = new wxRadioBox(
      Panel1, ID_RADIOBOX1, _(" Holonomic method "), wxDefaultPosition, wxDefaultSize, 2,
      __wxRadioBoxChoices_1, 1, wxRA_HORIZONTAL, wxDefaultValidator, _T("ID_RADIOBOX1"));
  FlexGridSizer7->Add(
      rbHoloMethod, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  edHoloParams = new wxTextCtrl(
      Panel1, ID_TEXTCTRL1, wxEmptyString, wxDefaultPosition, wxDefaultSize,
      wxTE_PROCESS_ENTER | wxTE_PROCESS_TAB | wxTE_MULTILINE | wxVSCROLL | wxHSCROLL |
          wxALWAYS_SHOW_SB,
      wxDefaultValidator, _T("ID_TEXTCTRL1"));
  edHoloParams->SetMinSize(wxSize(-1, 100));
  wxFont edHoloParamsFont(8, wxFONTFAMILY_TELETYPE, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL);
  edHoloParams->SetFont(edHoloParamsFont);
  FlexGridSizer7->Add(edHoloParams, 1, wxEXPAND, 2);
  Panel1->SetSizer(FlexGridSizer7);
  FlexGridSizer7->Fit(Panel1);
  FlexGridSizer7->SetSizeHints(Panel1);
  Panel2 = new wxPanel(
      Notebook1, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL2"));
  Notebook1->AddPage(Panel1, _("Configuration"), true);
  Notebook1->AddPage(Panel2, _("Stats"), false);
  FlexGridSizer2->Add(Notebook1, 1, wxEXPAND, 2);
  FlexGridSizer1->Add(FlexGridSizer2, 1, wxEXPAND, 0);
  FlexGridSizer3 = new wxFlexGridSizer(2, 2, 0, 0);
  FlexGridSizer3->AddGrowableCol(0);
  FlexGridSizer3->AddGrowableCol(1);
  FlexGridSizer3->AddGrowableRow(1);
  StaticText2 = new wxStaticText(
      this, ID_STATICTEXT2, _("[3D view of the simulated world]"), wxDefaultPosition, wxDefaultSize,
      0, _T("ID_STATICTEXT2"));
  FlexGridSizer3->Add(
      StaticText2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  StaticText1 = new wxStaticText(
      this, ID_STATICTEXT1, _("[Local view]"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT1"));
  FlexGridSizer3->Add(
      StaticText1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  m_plot3D = new CMyGLCanvas(
      this, ID_XY_GLCANVAS, wxDefaultPosition, wxSize(450, 350), wxTAB_TRAVERSAL,
      _T("ID_XY_GLCANVAS"));
  FlexGridSizer3->Add(
      m_plot3D, 1, wxALL | wxEXPAND | wxFIXED_MINSIZE | wxALIGN_LEFT | wxALIGN_TOP, 1);
  FlexGridSizer5 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer5->AddGrowableCol(0);
  FlexGridSizer5->AddGrowableRow(0);
  m_plotScan = new CMyGLCanvas(
      this, ID_CUSTOM1, wxDefaultPosition, wxSize(150, 150), wxTAB_TRAVERSAL, _T("ID_CUSTOM1"));
  FlexGridSizer5->Add(m_plotScan, 1, wxEXPAND, 1);
  edInfoLocalView = new wxTextCtrl(
      this, 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);
  edInfoLocalView->SetFont(edInfoLocalViewFont);
  FlexGridSizer5->Add(edInfoLocalView, 1, wxEXPAND, 2);
  FlexGridSizer3->Add(FlexGridSizer5, 1, wxEXPAND, 0);
  FlexGridSizer1->Add(FlexGridSizer3, 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->Fit(this);
  FlexGridSizer1->SetSizeHints(this);
  Center();

  using hndf = holonomic_navigator_demoFrame;

  Bind(wxEVT_BUTTON, &hndf::OnbtnLoadMapClick, this, ID_BUTTON1);
  Bind(wxEVT_BUTTON, &hndf::OnAbout, this, ID_BUTTON2);
  Bind(wxEVT_BUTTON, &hndf::OnQuit, this, ID_BUTTON3);
  Bind(wxEVT_BUTTON, &hndf::OnbtnPlaceRobotClick, this, ID_BUTTON6);
  Bind(wxEVT_BUTTON, &hndf::OnbtnPlaceTargetClick, this, ID_BUTTON7);
  Bind(wxEVT_BUTTON, &hndf::OnbtnStartClick, this, ID_BUTTON4);
  Bind(wxEVT_BUTTON, &hndf::OnbtnStopClick, this, ID_BUTTON5);
  Bind(wxEVT_MENU, &hndf::OnbtnLoadMapClick, this, ID_MENUITEM4);
  Bind(wxEVT_MENU, &hndf::OnQuit, this, idMenuQuit);
  Bind(wxEVT_MENU, &hndf::OnMenuItemChangeVisibleStuff, this, ID_MENUITEM1);
  Bind(wxEVT_MENU, &hndf::OnMenuItemChangeVisibleStuff, this, ID_MENUITEM2);
  Bind(wxEVT_MENU, &hndf::OnMenuItemClearRobotPath, this, ID_MENUITEM3);
  Bind(wxEVT_MENU, &hndf::OnAbout, this, idMenuAbout);
  Bind(wxEVT_TIMER, &hndf::OntimRunSimulTrigger, this, ID_TIMER1);
  //*)

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

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

  btnStart->Enable(true);
  btnStop->Enable(false);

  WX_START_TRY

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

  auto openGLSceneRef = m_plot3D->getOpenGLSceneRef();
  // Populate 3D views:
  // -------------------------------
  {
    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();

  gl_robot = mrpt::opengl::CSetOfObjects::Create();
  {
    mrpt::opengl::CCylinder::Ptr obj = mrpt::opengl::CCylinder::Create(0.2f, 0.1f, 0.9f);
    obj->setColor_u8(TColor::red());
    gl_robot->insert(obj);
  }
  openGLSceneRef->insert(gl_robot);

  gl_scan3D = mrpt::opengl::CPlanarLaserScan::Create();
  gl_scan3D->enableLine(false);
  gl_scan3D->setPointSize(2.0);
  gl_scan3D->setLocation(0, 0, 0.01);  // to avoid z-fighting in rendering
  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));
  openGLSceneRef->insert(gl_robot_path);

  gl_target = mrpt::opengl::CSetOfObjects::Create();
  {
    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);
  }

  {  // 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::CCylinder::Ptr obj = mrpt::opengl::CCylinder::Create(0.2f, 0.1f, 0.9f);
    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);
  }

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

  // 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);

  // 2D view ==============
  auto openGLScanRef = m_plotScan->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));
    openGLScanRef->insert(obj);
  }

  gl_scan2D = mrpt::opengl::CPlanarLaserScan::Create();
  gl_scan2D->enableLine(false);
  gl_scan2D->enableSurface(false);
  gl_scan2D->setPointSize(2.0);
  openGLScanRef->insert(gl_scan2D);

  gl_line_direction = mrpt::opengl::CSimpleLine::Create();
  gl_line_direction->setLineWidth(4);
  gl_line_direction->setColor_u8(TColor(0, 0, 0));
  openGLScanRef->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));
  gl_rel_target->insertPoint(0, 0, 0);
  openGLScanRef->insert(gl_rel_target);

  openGLScanRef->insert(mrpt::opengl::stock_objects::CornerXYSimple(0.1f, 2));

  gl_nd_gaps = mrpt::opengl::CSetOfLines::Create();
  gl_nd_gaps->setLineWidth(2);
  gl_nd_gaps->setColor_u8(TColor(204, 102, 51));
  openGLScanRef->insert(gl_nd_gaps);

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

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

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

    m_simul_options.saveToConfigFile(cfg, "SIMULATOR");

    mrpt::nav::CHolonomicVFF holo_VFF;
    holo_VFF.options.saveToConfigFile(cfg, "VFF_CONFIG");

    mrpt::nav::CHolonomicND holo_ND;
    holo_ND.options.saveToConfigFile(cfg, "ND_CONFIG");

    this->edHoloParams->SetValue(cfg.getContent().c_str());
  }

  WX_END_TRY

  this->Maximize();
}

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

void holonomic_navigator_demoFrame::OnQuit(wxCommandEvent&) { Close(); }
void holonomic_navigator_demoFrame::OnAbout(wxCommandEvent&)
{
  mrpt::gui::show_mrpt_about_box_wxWidgets(this, "holonomic-navigator-demo");
}

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

void holonomic_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 holonomic_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 holonomic_navigator_demoFrame::OnbtnStartClick(wxCommandEvent& event)
{
  reinitSimulator();

  btnStart->Enable(false);
  btnStart->Refresh();
  btnStop->Enable(true);
  btnStop->Refresh();
  edHoloParams->Enable(false);
  rbHoloMethod->Enable(false);
}

void holonomic_navigator_demoFrame::OnbtnStopClick(wxCommandEvent& event)
{
  btnStart->Enable(true);
  btnStart->Refresh();
  btnStop->Enable(false);
  btnStop->Refresh();
  edHoloParams->Enable(true);
  rbHoloMethod->Enable(true);
}

// Run simulator (when "running"):
void holonomic_navigator_demoFrame::OntimRunSimulTrigger(wxTimerEvent& event)
{
  try
  {
    if (btnStop->IsEnabled())
    {
      simulateOneStep(event.GetInterval() * 1e-3);
    }
    updateViewsDynamicObjects();
  }
  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:
void holonomic_navigator_demoFrame::reinitSimulator()
{
  WX_START_TRY

  // Delete old & build new navigator:
  m_holonomicMethod.reset();
  switch (rbHoloMethod->GetSelection())
  {
    case 0:
      m_holonomicMethod = std::make_unique<mrpt::nav::CHolonomicVFF>();
      break;
    case 1:
      m_holonomicMethod = std::make_unique<mrpt::nav::CHolonomicND>();
      break;
    default:
      throw std::runtime_error("Invalid holonomic method selected!");
  };

  // Load params:
  {
    CConfigFileMemory cfg;
    cfg.setContent(std::string(edHoloParams->GetValue().mb_str()));
    m_holonomicMethod->initialize(cfg);

    m_simul_options.loadFromConfigFile(cfg, "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);

  WX_END_TRY
}

void holonomic_navigator_demoFrame::simulateOneStep(double time_step)
{
  // Simulate 360deg range scan:
  CObservation2DRangeScan simulatedScan;

  simulatedScan.aperture = M_2PIf;
  simulatedScan.rightToLeft = true;
  simulatedScan.maxRange = m_simul_options.MAX_SENSOR_RADIUS;
  simulatedScan.sensorPose = CPose3D();

  m_gridMap.laserScanSimulator(
      simulatedScan, mrpt::poses::CPose2D(m_robotPose), 0.5, m_simul_options.SENSOR_NUM_RANGES,
      m_simul_options.SENSOR_RANGE_NOISE_STD);

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

  // Normalize:
  for (size_t j = 0; j < simulatedScan.getScanSize(); j++)
    simulatedScan.setScanRange(j, simulatedScan.getScanRange(j) / simulatedScan.maxRange);

  gl_scan2D->setScan(simulatedScan);  // Draw scaled scan in right-hand view

  // Navigate:
  mrpt::math::TPose2D relTargetPose = m_targetPose - m_robotPose;
  // Normalized relative target:
  relTargetPose.x *= 1.0 / simulatedScan.maxRange;
  relTargetPose.y *= 1.0 / simulatedScan.maxRange;

  // tictac.Tic();
  CAbstractHolonomicReactiveMethod::NavOutput no;
  CAbstractHolonomicReactiveMethod::NavInput ni;
  ni.targets.push_back(relTargetPose);

  ni.obstacles.resize(simulatedScan.getScanSize());
  for (unsigned int i = 0; i < ni.obstacles.size(); i++)
    ni.obstacles[i] = simulatedScan.getScanRange(i);

  ni.maxRobotSpeed = m_simul_options.ROBOT_MAX_SPEED;
  ni.maxObstacleDist = m_simul_options.MAX_SENSOR_RADIUS;

  this->m_holonomicMethod->navigate(ni, no);

  mrpt::nav::CHolonomicLogFileRecord::Ptr out_log = no.logRecord;

  // Move robot:
  m_robotPose.x += cos(no.desiredDirection) * no.desiredSpeed * time_step;
  m_robotPose.y += sin(no.desiredDirection) * no.desiredSpeed * time_step;

  // Update path graph:
  const TPoint3D cur_pt(m_robotPose.x, m_robotPose.y, 0.01);

  if (gl_robot_path->empty())
    gl_robot_path->appendLine(cur_pt, cur_pt);
  else
    gl_robot_path->appendLineStrip(cur_pt);

  gl_rel_target->setLocation(relTargetPose.x, relTargetPose.y, 0);

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

  // Update 2D view graphs:
  if (out_log && IS_CLASS(*out_log, CLogFileRecord_ND))
  {
    CLogFileRecord_ND::Ptr log = std::dynamic_pointer_cast<CLogFileRecord_ND>(out_log);
    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 + 2 * sec / ((float)simulatedScan.getScanSize()));
        const double d = simulatedScan.getScanRange(sec) - 0.05;
        gl_nd_gaps->appendLineStrip(d * cos(ang), d * sin(ang), 0);
      }
      gl_nd_gaps->appendLineStrip(0, 0, 0);
    }
  }

  // Movement direction:
  const double d = no.desiredSpeed / m_simul_options.ROBOT_MAX_SPEED;
  gl_line_direction->setLineCoords(
      0, 0, 0, cos(no.desiredDirection) * d, sin(no.desiredDirection) * d, 0);
}

void holonomic_navigator_demoFrame::updateViewsDynamicObjects()
{
  gl_robot->setLocation(m_robotPose.x, m_robotPose.y, 0);

  // 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(mrpt::math::TPoint3D(m_targetPose.translation()));
    gl_target->setScale(s);
  }

  // Labels:
  StatusBar1->SetStatusText(mrpt::format("Robot: (%.03f,%.03f)", m_robotPose.x, m_robotPose.y), 0);
  StatusBar1->SetStatusText(mrpt::format("Target: %s", m_targetPose.asString().c_str()));

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

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

void holonomic_navigator_demoFrame::Onplot3DMouseMove(wxMouseEvent& event)
{
  int X, Y;
  event.GetPosition(&X, &Y);

  // 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;

    if (m_cursorPickState == cpsPickTarget)
    {
      m_gl_placing_nav_target->setVisibility(true);
      m_gl_placing_nav_target->setLocation(m_curCursorPos.x, m_curCursorPos.y, 0.05);
    }
    else if (m_cursorPickState == cpsPlaceRobot)
    {
      m_gl_placing_robot->setVisibility(true);
      m_gl_placing_robot->setLocation(m_curCursorPos.x, m_curCursorPos.y, 0.05);
    }

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

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

void holonomic_navigator_demoFrame::Onplot3DMouseClick(wxMouseEvent& event)
{
  switch (m_cursorPickState)
  {
    case cpsPickTarget:
    {
      m_targetPose = {m_curCursorPos.x, m_curCursorPos.y, .0};

      btnPlaceTarget->SetValue(false);
      btnPlaceTarget->Refresh();
      m_gl_placing_nav_target->setVisibility(false);
      break;
    }
    case cpsPlaceRobot:
    {
      m_robotPose.x = m_curCursorPos.x;
      m_robotPose.y = m_curCursorPos.y;

      btnPlaceRobot->SetValue(false);
      btnPlaceRobot->Refresh();
      m_gl_placing_robot->setVisibility(false);
      break;
    }
    default:
      break;
  }

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

  // Do normal process in that class:
  m_plot3D->OnMouseDown(event);
}

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

  // Load from config text:
  MRPT_LOAD_CONFIG_VAR(ROBOT_MAX_SPEED, double, source, section);
  MRPT_LOAD_CONFIG_VAR(MAX_SENSOR_RADIUS, double, 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_END
}

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

  cfg.write(
      section, "ROBOT_MAX_SPEED", ROBOT_MAX_SPEED, WN, WV, "Maximum speed for the robot (m/s)");
  cfg.write(
      section, "MAX_SENSOR_RADIUS", MAX_SENSOR_RADIUS, WN, WV,
      "Maximum range of the 360deg sensor (meters)");
  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)");

  MRPT_END
}

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

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

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

  wxFileDialog dlg(
      this, _("Select grid map to load"), _("."), _("grid.png"),
      wxT("Image files (*.png,*.jpg,*.gif)|*.png;*.jpg;*.gif|Binary gridmap "
          "files (*.gridmap,*.gridmap.gz)|*.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
}
