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

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

//(*InternalHeaders(ptgConfiguratorframe)
#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 <mrpt/gui/WxUtils.h>
#include <mrpt/gui/wx28-fixes.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>

#include <fstream>

#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/config/CConfigFileMemory.h>
#include <mrpt/config/CConfigFilePrefixer.h>
#include <mrpt/math/geometry.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>

mrpt::nav::CParameterizedTrajectoryGenerator::Ptr ptg;

//(*IdInit(ptgConfiguratorframe)
const long ptgConfiguratorframe::ID_STATICTEXT1 = wxNewId();
const long ptgConfiguratorframe::ID_CHOICE1 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT5 = wxNewId();
const long ptgConfiguratorframe::ID_BUTTON1 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT2 = wxNewId();
const long ptgConfiguratorframe::ID_SPINCTRL1 = wxNewId();
const long ptgConfiguratorframe::ID_CHECKBOX1 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT4 = wxNewId();
const long ptgConfiguratorframe::ID_TEXTCTRL5 = wxNewId();
const long ptgConfiguratorframe::ID_BUTTON5 = wxNewId();
const long ptgConfiguratorframe::ID_CHECKBOX3 = wxNewId();
const long ptgConfiguratorframe::ID_SLIDER1 = wxNewId();
const long ptgConfiguratorframe::ID_SPINCTRL2 = wxNewId();
const long ptgConfiguratorframe::ID_CHECKBOX4 = wxNewId();
const long ptgConfiguratorframe::ID_CHECKBOX2 = wxNewId();
const long ptgConfiguratorframe::ID_TEXTCTRL3 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT3 = wxNewId();
const long ptgConfiguratorframe::ID_TEXTCTRL4 = wxNewId();
const long ptgConfiguratorframe::ID_BUTTON3 = wxNewId();
const long ptgConfiguratorframe::ID_BUTTON2 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT6 = wxNewId();
const long ptgConfiguratorframe::ID_TEXTCTRL6 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT7 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT7b = wxNewId();
const long ptgConfiguratorframe::ID_TEXTCTRL7 = wxNewId();
const long ptgConfiguratorframe::ID_TEXTCTRL7b = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT17 = wxNewId();
const long ptgConfiguratorframe::ID_TEXTCTRL8 = wxNewId();
const long ptgConfiguratorframe::ID_BUTTON4 = wxNewId();
const long ptgConfiguratorframe::ID_TEXTCTRL1 = wxNewId();
const long ptgConfiguratorframe::ID_TEXTCTRL2 = wxNewId();
const long ptgConfiguratorframe::ID_PANEL1 = wxNewId();
const long ptgConfiguratorframe::ID_XY_GLCANVAS = wxNewId();
const long ptgConfiguratorframe::ID_CHECKBOX5 = wxNewId();
const long ptgConfiguratorframe::ID_CHECKBOX6 = wxNewId();
const long ptgConfiguratorframe::ID_CHECKBOX7 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM2 = wxNewId();
const long ptgConfiguratorframe::ID_PANEL2 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM1 = wxNewId();
const long ptgConfiguratorframe::ID_PANEL3 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT8 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM3 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT9 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM4 = wxNewId();
const long ptgConfiguratorframe::ID_PANEL4 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT10 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM5 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT11 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM6 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT12 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM7 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT16 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM11 = wxNewId();
const long ptgConfiguratorframe::ID_PANEL5 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT13 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM8 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT14 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM9 = wxNewId();
const long ptgConfiguratorframe::ID_STATICTEXT15 = wxNewId();
const long ptgConfiguratorframe::ID_CUSTOM10 = wxNewId();
const long ptgConfiguratorframe::ID_PANEL6 = wxNewId();
const long ptgConfiguratorframe::ID_NOTEBOOK1 = wxNewId();
const long ptgConfiguratorframe::idMenuQuit = wxNewId();
const long ptgConfiguratorframe::idMenuAbout = wxNewId();
const long ptgConfiguratorframe::ID_STATUSBAR1 = wxNewId();
//*)
const long ID_TEXTCTRL_SEL_TRAJ = wxNewId();
const long idMenuExportPath = wxNewId();

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

ptgConfiguratorframe::ptgConfiguratorframe(wxWindow* parent, wxWindowID id)
	: m_cursorPickState(cpsNone)
{
	// Load my custom icons:
	wxArtProvider::Push(new MyArtProvider);

	//(*Initialize(ptgConfiguratorframe)
	wxFlexGridSizer* FlexGridSizer4;
	wxMenuItem* MenuItem2;
	wxFlexGridSizer* FlexGridSizer10;
	wxFlexGridSizer* FlexGridSizer3;
	wxMenuItem* MenuItem1;
	wxMenuItem* mnuItemExportPath;
	wxFlexGridSizer* FlexGridSizer9;
	wxFlexGridSizer* FlexGridSizer2;
	wxMenu* Menu1;
	wxFlexGridSizer* FlexGridSizer7;
	wxFlexGridSizer* FlexGridSizer15;
	wxFlexGridSizer* FlexGridSizer8;
	wxFlexGridSizer* FlexGridSizer14;
	wxFlexGridSizer* FlexGridSizer13;
	wxFlexGridSizer* FlexGridSizer12;
	wxMenuBar* MenuBar1;
	wxFlexGridSizer* FlexGridSizer6;
	wxStaticBoxSizer* StaticBoxSizer1;
	wxFlexGridSizer* FlexGridSizer1;
	wxFlexGridSizer* FlexGridSizer11;
	wxMenu* Menu2;

	Create(
		parent, wxID_ANY, _("PTG configurator - Part of the MRPT project"),
		wxDefaultPosition, wxDefaultSize, wxDEFAULT_FRAME_STYLE,
		_T("wxID_ANY"));

	{
		wxIcon FrameIcon;
		FrameIcon.CopyFromBitmap(wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("MAIN_ICON")), wxART_OTHER));
		SetIcon(FrameIcon);
	}
	FlexGridSizer1 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer1->AddGrowableCol(0);
	FlexGridSizer1->AddGrowableRow(1);
	Panel1 = new wxPanel(
		this, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL1"));
	FlexGridSizer2 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer2->AddGrowableCol(0);
	FlexGridSizer2->AddGrowableRow(0);
	FlexGridSizer3 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer3->AddGrowableCol(0);
	FlexGridSizer3->AddGrowableRow(3);
	FlexGridSizer7 = new wxFlexGridSizer(1, 0, 0, 0);
	StaticText1 = new wxStaticText(
		Panel1, ID_STATICTEXT1, _("Pick a PTG:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT1"));
	FlexGridSizer7->Add(
		StaticText1, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	cbPTGClass = new wxChoice(
		Panel1, ID_CHOICE1, wxDefaultPosition, wxDefaultSize, 0, nullptr,
		wxCB_SORT, wxDefaultValidator, _T("ID_CHOICE1"));
	FlexGridSizer7->Add(cbPTGClass, 1, wxEXPAND, 5);
	StaticText5 = new wxStaticText(
		Panel1, ID_STATICTEXT5, _(", change params, click: "),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT5"));
	FlexGridSizer7->Add(
		StaticText5, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	btnReloadParams = new wxButton(
		Panel1, ID_BUTTON1, _("Initialize PTG"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON1"));
	wxFont btnReloadParamsFont(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxBOLD);
	btnReloadParams->SetFont(btnReloadParamsFont);
	FlexGridSizer7->Add(
		btnReloadParams, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	btnLoadPlugin = new wxButton(Panel1, ID_BUTTON5, _("Load Plugin"));
	FlexGridSizer7->Add(
		btnLoadPlugin, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);

	FlexGridSizer3->Add(FlexGridSizer7, 1, wxEXPAND, 0);

	FlexGridSizer4 = new wxFlexGridSizer(0, 7, 0, 0);
	StaticText2 =
		new wxStaticText(Panel1, ID_STATICTEXT2, _("PTG index for cfg file:"));
	FlexGridSizer4->Add(
		StaticText2, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edPTGIndex = new wxSpinCtrl(
		Panel1, ID_SPINCTRL1, _T("0"), wxDefaultPosition, wxDefaultSize, 0, 0,
		100, 0, _T("ID_SPINCTRL1"));
	edPTGIndex->SetValue(_T("0"));
	FlexGridSizer4->Add(edPTGIndex, 1, wxEXPAND, 5);
	cbDrawShapePath = new wxCheckBox(
		Panel1, ID_CHECKBOX1, _("Draw robot shape"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX1"));
	cbDrawShapePath->SetValue(true);
	FlexGridSizer4->Add(
		cbDrawShapePath, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	StaticText4 =
		new wxStaticText(Panel1, ID_STATICTEXT4, _("Dist. btw. robot shapes:"));
	FlexGridSizer4->Add(
		StaticText4, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edMinDistBtwShapes = new wxTextCtrl(
		Panel1, ID_TEXTCTRL5, _("0.5"), wxDefaultPosition, wxSize(50, -1), 0,
		wxDefaultValidator, _T("ID_TEXTCTRL5"));
	FlexGridSizer4->Add(
		edMinDistBtwShapes, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	FlexGridSizer4->Add(
		new wxStaticText(Panel1, wxNewId(), "Draw max. time [s]:"), 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	edDrawMaxTime = new wxTextCtrl(Panel1, wxNewId(), "100.0");
	FlexGridSizer4->Add(
		edDrawMaxTime, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	FlexGridSizer3->Add(FlexGridSizer4, 1, wxEXPAND, 0);

	// ---
	auto FlexGridSizer4b = new wxFlexGridSizer(0, 5, 0, 0);

	cbHighlightOnePath = new wxCheckBox(
		Panel1, ID_CHECKBOX3, _("Highlight one trajectory:"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX3"));
	cbHighlightOnePath->SetValue(true);
	FlexGridSizer4b->Add(
		cbHighlightOnePath, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	slidPathHighlight = new wxSlider(Panel1, ID_SLIDER1, 0, 0, 100);
	FlexGridSizer4b->Add(
		slidPathHighlight, 1,
		wxALL | wxEXPAND | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	edIndexHighlightPath = new wxSpinCtrl(
		Panel1, ID_SPINCTRL2, _T("0"), wxDefaultPosition, wxDefaultSize, 0, 0,
		100, 0, _T("ID_SPINCTRL2"));
	edIndexHighlightPath->SetValue(_T("0"));
	FlexGridSizer4b->Add(
		edIndexHighlightPath, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	cbShowOnlySelectedTraj =
		new wxCheckBox(Panel1, ID_CHECKBOX4, _("Show only selected traj."));
	cbShowOnlySelectedTraj->SetValue(false);
	FlexGridSizer4b->Add(
		cbShowOnlySelectedTraj, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	edSelectedTrajCmd = new wxTextCtrl(
		Panel1, ID_TEXTCTRL_SEL_TRAJ, _("Cmd: (none)"), wxDefaultPosition,
		wxSize(50, -1), wxTE_READONLY, wxDefaultValidator,
		_T("ID_TEXTCTRL_SEL_TRAJ"));
	FlexGridSizer4b->Add(
		edSelectedTrajCmd, 1,
		wxALL | wxEXPAND | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);

	FlexGridSizer4b->AddGrowableCol(1);
	FlexGridSizer4b->AddGrowableCol(4);

	FlexGridSizer3->Add(FlexGridSizer4b, 1, wxEXPAND, 0);
	// ---
	FlexGridSizer8 = new wxFlexGridSizer(1, 0, 0, 0);
	cbBuildTPObs = new wxCheckBox(
		Panel1, ID_CHECKBOX2, _("Obstacle point: x="), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX2"));
	cbBuildTPObs->SetValue(true);
	FlexGridSizer8->Add(
		cbBuildTPObs, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edObsX = new wxTextCtrl(
		Panel1, ID_TEXTCTRL3, _("9.0"), wxDefaultPosition, wxSize(50, -1), 0,
		wxDefaultValidator, _T("ID_TEXTCTRL3"));
	FlexGridSizer8->Add(
		edObsX, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText3 = new wxStaticText(
		Panel1, ID_STATICTEXT3, _("y="), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT3"));
	FlexGridSizer8->Add(
		StaticText3, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edObsY = new wxTextCtrl(
		Panel1, ID_TEXTCTRL4, _("3.0"), wxDefaultPosition, wxSize(50, -1), 0,
		wxDefaultValidator, _T("ID_TEXTCTRL4"));
	FlexGridSizer8->Add(
		edObsY, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	btnPlaceObs = new wxButton(
		Panel1, ID_BUTTON3, _("Click to place obstacle..."), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON3"));
	FlexGridSizer8->Add(
		btnPlaceObs, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	btnRebuildTPObs = new wxButton(
		Panel1, ID_BUTTON2, _("Rebuild TP-Obs now"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON2"));
	FlexGridSizer8->Add(
		btnRebuildTPObs, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	StaticText6 = new wxStaticText(
		Panel1, ID_STATICTEXT6, _("Target (x,y,phi_deg):"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT6"));
	FlexGridSizer8->Add(
		StaticText6, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edTargetX = new wxTextCtrl(
		Panel1, ID_TEXTCTRL6, _("5.0"), wxDefaultPosition, wxSize(31, -1), 0,
		wxDefaultValidator, _T("ID_TEXTCTRL6"));
	FlexGridSizer8->Add(
		edTargetX, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	edTargetY = new wxTextCtrl(
		Panel1, ID_TEXTCTRL7, _("1.0"), wxDefaultPosition, wxSize(35, -1), 0,
		wxDefaultValidator, _T("ID_TEXTCTRL7"));
	FlexGridSizer8->Add(
		edTargetY, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	edTargetPhiDeg = new wxTextCtrl(
		Panel1, ID_TEXTCTRL7b, _("0.0"), wxDefaultPosition, wxSize(35, -1), 0,
		wxDefaultValidator, _T("ID_TEXTCTRL7b"));
	FlexGridSizer8->Add(
		edTargetPhiDeg, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	StaticText17 = new wxStaticText(
		Panel1, ID_STATICTEXT17, _("RelSpeed="), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT17"));
	FlexGridSizer8->Add(
		StaticText17, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edRelSpeedAtTarget = new wxTextCtrl(
		Panel1, ID_TEXTCTRL8, _("0.0"), wxDefaultPosition, wxSize(34, -1), 0,
		wxDefaultValidator, _T("ID_TEXTCTRL8"));
	FlexGridSizer8->Add(
		edRelSpeedAtTarget, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	btnPlaceTarget = new wxButton(
		Panel1, ID_BUTTON4, _("Click to place target..."), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON4"));
	FlexGridSizer8->Add(
		btnPlaceTarget, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	FlexGridSizer3->Add(FlexGridSizer8, 1, wxEXPAND, 0);
	FlexGridSizer6 = new wxFlexGridSizer(1, 2, 0, 0);
	FlexGridSizer6->AddGrowableCol(0);
	FlexGridSizer6->AddGrowableCol(1);
	FlexGridSizer6->AddGrowableRow(0);
	edCfg = new wxTextCtrl(
		Panel1, ID_TEXTCTRL1, wxEmptyString, wxDefaultPosition, wxSize(-1, 100),
		wxTE_PROCESS_ENTER | wxTE_PROCESS_TAB | wxTE_MULTILINE | wxHSCROLL |
			wxTE_DONTWRAP | wxALWAYS_SHOW_SB,
		wxDefaultValidator, _T("ID_TEXTCTRL1"));
	wxFont edCfgFont = wxSystemSettings::GetFont(wxSYS_OEM_FIXED_FONT);
	if (!edCfgFont.Ok())
		edCfgFont = wxSystemSettings::GetFont(wxSYS_DEFAULT_GUI_FONT);
	edCfgFont.SetPointSize(8);
	edCfgFont.SetFamily(wxFONTFAMILY_TELETYPE);
	edCfg->SetFont(edCfgFont);
	FlexGridSizer6->Add(edCfg, 1, wxEXPAND, 2);
	edLog = new wxTextCtrl(
		Panel1, ID_TEXTCTRL2, wxEmptyString, wxDefaultPosition, wxSize(-1, 100),
		wxTE_PROCESS_ENTER | wxTE_PROCESS_TAB | wxTE_MULTILINE | wxTE_READONLY |
			wxHSCROLL | wxTE_DONTWRAP | wxALWAYS_SHOW_SB,
		wxDefaultValidator, _T("ID_TEXTCTRL2"));
	wxFont edLogFont = wxSystemSettings::GetFont(wxSYS_OEM_FIXED_FONT);
	if (!edLogFont.Ok())
		edLogFont = wxSystemSettings::GetFont(wxSYS_DEFAULT_GUI_FONT);
	edLogFont.SetPointSize(8);
	edLogFont.SetFamily(wxFONTFAMILY_TELETYPE);
	edLog->SetFont(edLogFont);
	FlexGridSizer6->Add(edLog, 1, wxEXPAND, 0);
	FlexGridSizer3->Add(FlexGridSizer6, 1, wxEXPAND, 0);
	FlexGridSizer2->Add(FlexGridSizer3, 1, wxEXPAND, 0);
	Panel1->SetSizer(FlexGridSizer2);
	FlexGridSizer2->Fit(Panel1);
	FlexGridSizer2->SetSizeHints(Panel1);
	FlexGridSizer1->Add(Panel1, 1, wxEXPAND, 0);
	FlexGridSizer9 = new wxFlexGridSizer(1, 2, 0, 0);
	FlexGridSizer9->AddGrowableCol(0);
	FlexGridSizer9->AddGrowableCol(1);
	FlexGridSizer9->AddGrowableRow(0);
	m_plot = new CMyGLCanvas(
		this, ID_XY_GLCANVAS, wxDefaultPosition, wxSize(150, 200),
		wxTAB_TRAVERSAL, _T("ID_XY_GLCANVAS"));
	FlexGridSizer9->Add(m_plot, 1, wxEXPAND, 0);
	Notebook1 = new wxNotebook(
		this, ID_NOTEBOOK1, wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_NOTEBOOK1"));
	Panel2 = new wxPanel(
		Notebook1, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL2"));
	FlexGridSizer10 = new wxFlexGridSizer(2, 1, 0, 0);
	FlexGridSizer10->AddGrowableCol(0);
	FlexGridSizer10->AddGrowableRow(1);
	StaticBoxSizer1 = new wxStaticBoxSizer(wxHORIZONTAL, Panel2, _("Show:"));
	FlexGridSizer15 = new wxFlexGridSizer(0, 4, 0, 0);
	cbShowTPObs = new wxCheckBox(
		Panel2, ID_CHECKBOX5, _("TP-Obstacles"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX5"));
	cbShowTPObs->SetValue(true);
	FlexGridSizer15->Add(
		cbShowTPObs, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	cbShowClearance = new wxCheckBox(
		Panel2, ID_CHECKBOX6, _("Clearance diagram"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX6"));
	cbShowClearance->SetValue(false);
	FlexGridSizer15->Add(
		cbShowClearance, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	cbClearanceInterp = new wxCheckBox(
		Panel2, ID_CHECKBOX7, _("Interpolate clearance diagram"),
		wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
		_T("ID_CHECKBOX7"));
	cbClearanceInterp->SetValue(false);
	FlexGridSizer15->Add(
		cbClearanceInterp, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticBoxSizer1->Add(FlexGridSizer15, 1, wxEXPAND, 0);
	FlexGridSizer10->Add(StaticBoxSizer1, 1, wxEXPAND, 2);
	m_plotTPSpace = new CMyGLCanvas(
		Panel2, ID_CUSTOM2, wxDefaultPosition, wxSize(150, 200),
		wxTAB_TRAVERSAL, _T("ID_CUSTOM2"));
	FlexGridSizer10->Add(m_plotTPSpace, 1, wxEXPAND, 0);
	Panel2->SetSizer(FlexGridSizer10);
	FlexGridSizer10->Fit(Panel2);
	FlexGridSizer10->SetSizeHints(Panel2);
	Panel3 = new wxPanel(
		Notebook1, ID_PANEL3, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL3"));
	FlexGridSizer11 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer11->AddGrowableCol(0);
	FlexGridSizer11->AddGrowableRow(0);
	m_plotVelCmds =
		new mpWindow(Panel3, ID_CUSTOM1, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer11->Add(m_plotVelCmds, 1, wxEXPAND, 0);
	Panel3->SetSizer(FlexGridSizer11);
	FlexGridSizer11->Fit(Panel3);
	FlexGridSizer11->SetSizeHints(Panel3);
	Panel4 = new wxPanel(
		Notebook1, ID_PANEL4, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL4"));
	FlexGridSizer12 = new wxFlexGridSizer(4, 1, 0, 0);
	FlexGridSizer12->AddGrowableCol(0);
	FlexGridSizer12->AddGrowableRow(1);
	FlexGridSizer12->AddGrowableRow(3);
	StaticText8 = new wxStaticText(
		Panel4, ID_STATICTEXT8,
		_("Motion direction to robot heading angle: "
		  "for selected PTG over time [deg]"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT8"));
	FlexGridSizer12->Add(StaticText8, 1, wxALL | wxALIGN_LEFT | wxALIGN_TOP, 5);
	m_plotHeadAngIndiv =
		new mpWindow(Panel4, ID_CUSTOM3, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer12->Add(m_plotHeadAngIndiv, 1, wxEXPAND, 0);
	StaticText9 = new wxStaticText(
		Panel4, ID_STATICTEXT9,
		_("Motion direction to robot heading angle: "
		  "maximum value for each trajectory [deg]"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT9"));
	FlexGridSizer12->Add(StaticText9, 1, wxALL | wxALIGN_LEFT | wxALIGN_TOP, 5);
	m_plotHeadAngAll =
		new mpWindow(Panel4, ID_CUSTOM4, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer12->Add(m_plotHeadAngAll, 1, wxEXPAND, 0);
	Panel4->SetSizer(FlexGridSizer12);
	FlexGridSizer12->Fit(Panel4);
	FlexGridSizer12->SetSizeHints(Panel4);
	Panel5 = new wxPanel(
		Notebook1, ID_PANEL5, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL5"));
	FlexGridSizer13 = new wxFlexGridSizer(8, 1, 0, 0);
	FlexGridSizer13->AddGrowableCol(0);
	FlexGridSizer13->AddGrowableRow(1);
	FlexGridSizer13->AddGrowableRow(3);
	FlexGridSizer13->AddGrowableRow(5);
	FlexGridSizer13->AddGrowableRow(7);
	StaticText10 = new wxStaticText(
		Panel5, ID_STATICTEXT10, _("Selected path trajectory: X [m]"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT10"));
	FlexGridSizer13->Add(
		StaticText10, 1, wxALL | wxALIGN_LEFT | wxALIGN_TOP, 5);
	m_plotPathX =
		new mpWindow(Panel5, ID_CUSTOM5, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer13->Add(m_plotPathX, 1, wxEXPAND, 0);
	StaticText11 = new wxStaticText(
		Panel5, ID_STATICTEXT11, _("Selected path trajectory: Y [m]"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT11"));
	FlexGridSizer13->Add(
		StaticText11, 1, wxALL | wxALIGN_LEFT | wxALIGN_TOP, 5);
	m_plotPathY =
		new mpWindow(Panel5, ID_CUSTOM6, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer13->Add(m_plotPathY, 1, wxEXPAND, 0);
	StaticText12 = new wxStaticText(
		Panel5, ID_STATICTEXT12, _("Selected path trajectory: Phi [deg]"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT12"));
	FlexGridSizer13->Add(StaticText12, 1, wxEXPAND, 5);
	m_plotPathPhi =
		new mpWindow(Panel5, ID_CUSTOM7, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer13->Add(m_plotPathPhi, 1, wxEXPAND, 0);
	StaticText16 = new wxStaticText(
		Panel5, ID_STATICTEXT16,
		_("Selected path trajectory: traversed distance [m]"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT16"));
	FlexGridSizer13->Add(StaticText16, 1, wxEXPAND, 5);
	m_plotPathDist =
		new mpWindow(Panel5, ID_CUSTOM11, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer13->Add(m_plotPathDist, 1, wxEXPAND, 0);
	Panel5->SetSizer(FlexGridSizer13);
	FlexGridSizer13->Fit(Panel5);
	FlexGridSizer13->SetSizeHints(Panel5);
	Panel6 = new wxPanel(
		Notebook1, ID_PANEL6, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL6"));
	FlexGridSizer14 = new wxFlexGridSizer(6, 1, 0, 0);
	FlexGridSizer14->AddGrowableCol(0);
	FlexGridSizer14->AddGrowableRow(1);
	FlexGridSizer14->AddGrowableRow(3);
	FlexGridSizer14->AddGrowableRow(5);
	StaticText13 = new wxStaticText(
		Panel6, ID_STATICTEXT13, _("Selected path trajectory: dX/dt [m/s]"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT13"));
	FlexGridSizer14->Add(
		StaticText13, 1, wxALL | wxALIGN_LEFT | wxALIGN_TOP, 5);
	m_plotPathXp =
		new mpWindow(Panel6, ID_CUSTOM8, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer14->Add(m_plotPathXp, 1, wxEXPAND, 0);
	StaticText14 = new wxStaticText(
		Panel6, ID_STATICTEXT14, _("Selected path trajectory: dY/dt [m/s]"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT14"));
	FlexGridSizer14->Add(
		StaticText14, 1, wxALL | wxALIGN_LEFT | wxALIGN_TOP, 5);
	m_plotPathYp =
		new mpWindow(Panel6, ID_CUSTOM9, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer14->Add(m_plotPathYp, 1, wxEXPAND, 0);
	StaticText15 = new wxStaticText(
		Panel6, ID_STATICTEXT15, _("Selected path trajectory: w [deg/s]"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT15"));
	FlexGridSizer14->Add(StaticText15, 1, wxEXPAND, 5);
	m_plotPathW =
		new mpWindow(Panel6, ID_CUSTOM10, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer14->Add(m_plotPathW, 1, wxEXPAND, 0);
	Panel6->SetSizer(FlexGridSizer14);
	FlexGridSizer14->Fit(Panel6);
	FlexGridSizer14->SetSizeHints(Panel6);
	Notebook1->AddPage(Panel2, _("TP-Space"), true);
	Notebook1->AddPage(Panel3, _("VelCmds@t=0"), false);
	Notebook1->AddPage(Panel4, _("Head angle"), false);
	Notebook1->AddPage(Panel5, _("Robot path"), false);
	Notebook1->AddPage(Panel6, _("Robot velocity components"), false);
	FlexGridSizer9->Add(Notebook1, 1, wxEXPAND, 0);
	FlexGridSizer1->Add(FlexGridSizer9, 1, wxEXPAND, 0);
	SetSizer(FlexGridSizer1);
	MenuBar1 = new wxMenuBar();
	Menu1 = new wxMenu();

	mnuItemExportPath = new wxMenuItem(
		Menu1, idMenuExportPath, _("Export path to .m"),
		_("Export selected path"), wxITEM_NORMAL);
	Menu1->Append(mnuItemExportPath);

	MenuItem1 = new wxMenuItem(
		Menu1, idMenuQuit, _("Quit\tAlt-F4"), _("Quit the application"),
		wxITEM_NORMAL);
	Menu1->Append(MenuItem1);

	MenuBar1->Append(Menu1, _("&File"));
	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[4] = {-2, -5, -3, -3};
	int __wxStatusBarStyles_1[4] = {
		wxSB_NORMAL, wxSB_NORMAL, wxSB_NORMAL, wxSB_NORMAL};
	StatusBar1->SetFieldsCount(4, __wxStatusBarWidths_1);
	StatusBar1->SetStatusStyles(4, __wxStatusBarStyles_1);
	SetStatusBar(StatusBar1);
	FlexGridSizer1->SetSizeHints(this);
	Center();

	using pcf = ptgConfiguratorframe;

	Bind(wxEVT_CHOICE, &pcf::OncbPTGClassSelect, this, ID_CHOICE1);
	Bind(wxEVT_BUTTON, &pcf::OnbtnReloadParamsClick, this, ID_BUTTON1);
	Bind(
		wxEVT_COMMAND_SPINCTRL_UPDATED, &pcf::OnedPTGIndexChange, this,
		ID_SPINCTRL1);
	Bind(wxEVT_CHECKBOX, &pcf::OncbDrawShapePathClick, this, ID_CHECKBOX1);
	Bind(wxEVT_BUTTON, &pcf::OnButton1Click, this, ID_BUTTON5);
	Bind(wxEVT_CHECKBOX, &pcf::OncbHighlightOnePathClick, this, ID_CHECKBOX3);
	Bind(wxEVT_SLIDER, &pcf::OnslidPathHighlightCmdScroll, this, ID_SLIDER1);
	Bind(
		wxEVT_COMMAND_SPINCTRL_UPDATED, &pcf::OnedIndexHighlightPathChange,
		this, ID_SPINCTRL2);
	Bind(wxEVT_CHECKBOX, &pcf::OncbHighlightOnePathClick, this, ID_CHECKBOX4);
	Bind(wxEVT_CHECKBOX, &pcf::OncbBuildTPObsClick, this, ID_CHECKBOX2);
	Bind(wxEVT_BUTTON, &pcf::OnbtnPlaceObsClick, this, ID_BUTTON3);
	Bind(wxEVT_BUTTON, &pcf::OnbtnRebuildTPObsClick, this, ID_BUTTON2);
	Bind(wxEVT_BUTTON, &pcf::OnbtnPlaceTargetClick, this, ID_BUTTON4);
	Bind(wxEVT_CHECKBOX, &pcf::OnrbShowTPSelectSelect, this, ID_CHECKBOX5);
	Bind(wxEVT_CHECKBOX, &pcf::OnrbShowTPSelectSelect, this, ID_CHECKBOX6);
	Bind(wxEVT_CHECKBOX, &pcf::OnrbShowTPSelectSelect, this, ID_CHECKBOX7);
	Bind(wxEVT_MENU, &pcf::OnQuit, this, idMenuQuit);
	Bind(wxEVT_MENU, &pcf::OnAbout, this, idMenuAbout);
	Bind(wxEVT_MENU, &pcf::OnExportSelectedPath, this, idMenuExportPath);
	//*)

	m_plot->Bind(wxEVT_MOTION, &pcf::Onplot3DMouseMove, this);
	m_plot->Bind(wxEVT_LEFT_DOWN, &pcf::Onplot3DMouseClick, this);

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

	WX_START_TRY

	// 2D plots:
	prepareRobotPathPlot(m_plotHeadAngAll, &m_graph_head_all, "head_all");
	prepareRobotPathPlot(m_plotHeadAngIndiv, &m_graph_head_indiv, "head_indiv");

	//
	prepareRobotPathPlot(m_plotPathX, &m_graph_path_x, "x");
	prepareRobotPathPlot(m_plotPathY, &m_graph_path_y, "y");
	prepareRobotPathPlot(m_plotPathPhi, &m_graph_path_phi, "phi");
	prepareRobotPathPlot(m_plotPathDist, &m_graph_path_dist, "dist");
	//
	prepareRobotPathPlot(m_plotPathXp, &m_graph_path_vx, "vx");
	prepareRobotPathPlot(m_plotPathYp, &m_graph_path_vy, "vy");
	prepareRobotPathPlot(m_plotPathW, &m_graph_path_omega, "omega");

	// Populate 3D views:
	// ---------------------------
	gl_view_WS = m_plot->getOpenGLSceneRef()->getViewport();
	gl_view_TPSpace = m_plotTPSpace->getOpenGLSceneRef()->getViewport();

	gl_TPSpace_TP_obstacles = mrpt::opengl::CSetOfObjects::Create();
	gl_TPSpace_clearance =
		mrpt::opengl::CMesh::Create(true, -5.0f, 5.0f, -5.0f, 5.0f);
	gl_TPSpace_clearance->setVisibility(false);
	gl_TPSpace_clearance_interp =
		mrpt::opengl::CMesh::Create(true, -5.0f, 5.0f, -5.0f, 5.0f);
	gl_TPSpace_clearance_interp->setVisibility(false);

	gl_view_TPSpace->insert(gl_TPSpace_TP_obstacles);
	gl_view_TPSpace->insert(gl_TPSpace_clearance);
	gl_view_TPSpace->insert(gl_TPSpace_clearance_interp);

	m_plot->getOpenGLSceneRef()->getViewport()->addTextMessage(
		0.01, 5, "Workspace", 1);
	m_plotTPSpace->getOpenGLSceneRef()->getViewport()->addTextMessage(
		0.01, 5, "TP-Space", 2);

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

	gl_robot_ptg_prediction_highlight = mrpt::opengl::CSetOfLines::Create();
	gl_robot_ptg_prediction_highlight->setName("ptg_prediction_highlight");
	gl_robot_ptg_prediction_highlight->setLineWidth(3.0);
	gl_robot_ptg_prediction_highlight->setColor_u8(
		mrpt::img::TColor(0xff, 0x00, 0x00, 0xff));
	gl_view_WS->insert(gl_robot_ptg_prediction_highlight);

	gl_WS_obs = mrpt::opengl::CPointCloud::Create();
	gl_WS_obs->setPointSize(7.0);
	gl_WS_obs->setColor_u8(0, 0, 0);
	gl_view_WS->insert(gl_WS_obs);

	gl_WS_target = mrpt::opengl::CSetOfObjects::Create();
	{
		auto glShape = mrpt::opengl::CSetOfLines::Create();
		glShape->setColor_u8(0xff, 0, 0);
		mrpt::nav::CPTG_RobotShape_Circular::
			static_add_robotShape_to_setOfLines(*glShape, {}, 0.2);
		gl_WS_target->insert(glShape);
	}
	gl_WS_target->setName("WS-target");
	gl_WS_target->enableShowName(true);
	gl_view_WS->insert(gl_WS_target);

	gl_WS_target_reprojected = mrpt::opengl::CSetOfObjects::Create();
	{
		auto glShape = mrpt::opengl::CSetOfLines::Create();
		glShape->setColor_u8(0xff, 0xff, 0x00, 0xe0);
		mrpt::nav::CPTG_RobotShape_Circular::
			static_add_robotShape_to_setOfLines(*glShape, {}, 0.15);
		gl_WS_target_reprojected->insert(glShape);
	}
	gl_WS_target_reprojected->setName("WS-target-reproj");
	gl_WS_target_reprojected->enableShowName(true);
	gl_view_WS->insert(gl_WS_target_reprojected);

	gl_TP_target = mrpt::opengl::CPointCloud::Create();
	gl_TP_target->setPointSize(7.0);
	gl_TP_target->setColor_u8(0xff, 0, 0);
	gl_TP_target->insertPoint(0, 0, 0);
	gl_TP_target->setName("TP-target");
	gl_TP_target->enableShowName(true);
	gl_view_TPSpace->insert(gl_TP_target);

	{
		gl_axis_WS = mrpt::opengl::CAxis::Create(
			-10.0, -10.0, 0, 10.0, 10.0, 0.0, 1.0, 2.0);
		gl_axis_WS->setTextScale(0.10f);
		gl_axis_WS->enableTickMarks(true, true, true);
		gl_axis_WS->setColor_u8(mrpt::img::TColor(30, 30, 30, 50));
		gl_axis_WS->setTextLabelOrientation(0, 0, 0, 0);
		gl_axis_WS->setTextLabelOrientation(1, 0, 0, 0);

		gl_view_WS->insert(gl_axis_WS);
	}
	{
		gl_axis_TPS = mrpt::opengl::CAxis::Create(
			-1.0, -1.0, 0, 1.0, 1.0, 0.0, 0.25, 2.0);
		gl_axis_TPS->setTextScale(0.025f);
		gl_axis_TPS->enableTickMarks(true, true, false);
		gl_axis_TPS->setColor_u8(mrpt::img::TColor(30, 30, 30, 50));
		gl_axis_TPS->setTextLabelOrientation(0, 0, 0, 0);
		gl_axis_TPS->setTextLabelOrientation(1, 0, 0, 0);
		gl_view_TPSpace->insert(gl_axis_TPS);
	}

	gl_tp_obstacles = mrpt::opengl::CSetOfLines::Create();
	gl_tp_obstacles->setName("tp_obstacles");
	gl_tp_obstacles->setLineWidth(2.0f);
	gl_tp_obstacles->setColor_u8(mrpt::img::TColor(0x00, 0x00, 0x00, 0xff));

	gl_TPSpace_TP_obstacles->insert(gl_tp_obstacles);

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

#if 0
	// Fixed camera:
	gl_view_TPSpace_cam = mrpt::opengl::CCamera::Create();
	gl_view_TPSpace->insert ( gl_view_TPSpace_cam );
	gl_view_TPSpace_cam->setAzimuthDegrees( -90 );
	gl_view_TPSpace_cam->setElevationDegrees(90);
	gl_view_TPSpace_cam->setProjectiveModel( false );
	gl_view_TPSpace_cam->setZoomDistance(2.1f);
#else
	// User can rotate view:
	m_plotTPSpace->setCameraPointing(0.0f, 0.0f, 0.0f);
	m_plotTPSpace->setZoomDistance(2.1f);
	m_plotTPSpace->setElevationDegrees(90.0f);
	m_plotTPSpace->setAzimuthDegrees(-90.0f);
	m_plotTPSpace->setCameraProjective(false);
#endif

	// Populate list of existing PTGs:
	{
		// mrpt::nav::registerAllNavigationClasses();
		const std::vector<const mrpt::rtti::TRuntimeClassId*>& lstClasses =
			mrpt::rtti::getAllRegisteredClasses();
		for (auto lstClasse : lstClasses)
		{
			if (!lstClasse->derivedFrom(
					"mrpt::nav::CParameterizedTrajectoryGenerator") ||
				!mrpt::system::os::_strcmpi(
					lstClasse->className,
					"mrpt::nav::CParameterizedTrajectoryGenerator"))
				continue;
			cbPTGClass->AppendString(lstClasse->className);
		}
		if (cbPTGClass->GetCount() > 0) cbPTGClass->SetSelection(0);
	}
	{
		wxCommandEvent e;
		OncbPTGClassSelect(e);
	}

	this->Maximize();

	WX_END_TRY
}

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

void ptgConfiguratorframe::prepareRobotPathPlot(
	mpWindow* plot, mpFXYVector** graph, const std::string& name)
{
	plot->AddLayer(new mpScaleX(wxT("t [s]"), mpALIGN_CENTER, false /*grid*/));
	plot->AddLayer(new mpScaleY(wxT("y"), mpALIGN_CENTER, false /*grid*/));

	*graph = new mpFXYVector(name.c_str());
	(*graph)->SetPen(wxPen(wxColour(0, 0, 255), 5));
	(*graph)->SetContinuity(false);
	plot->AddLayer(*graph);
}

void ptgConfiguratorframe::OnAbout(wxCommandEvent&)
{
	mrpt::gui::show_mrpt_about_box_wxWidgets(this, "ptg-configurator");
}
void ptgConfiguratorframe::OnQuit(wxCommandEvent&) { Close(); }
void ptgConfiguratorframe::OnbtnReloadParamsClick(wxCommandEvent& event)
{
	WX_START_TRY;
	if (!ptg) return;

	ptg->deinitialize();

	const std::string sKeyPrefix =
		mrpt::format("PTG%d_", (int)edPTGIndex->GetValue());

	mrpt::config::CConfigFileMemory cfg;
	mrpt::config::CConfigFilePrefixer cfp;
	cfp.bind(cfg);
	cfp.setPrefixes("", sKeyPrefix);

	cfg.setContent(std::string(edCfg->GetValue().mb_str()));

	ptg->loadFromConfigFile(cfp, m_cfgFileSection);

	ptg->initialize();

	// PTG: Realize updated dynamical state (not from a real robot in this app,
	// but already loaded from a config file in this app for debugging):
	ptg->updateNavDynamicState(ptg->getCurrentNavDynamicState(), true);

	// one-time GUI init for each PTG settings:
	edIndexHighlightPath->SetRange(0, ptg->getPathCount() - 1);
	slidPathHighlight->SetRange(0, ptg->getPathCount() - 1);

	// first time full GUI refresh:
	rebuild3Dview();

	WX_END_TRY;
}

void ptgConfiguratorframe::OncbPTGClassSelect(wxCommandEvent& event)
{
	WX_START_TRY;

	const int sel = cbPTGClass->GetSelection();
	if (sel < 0) return;

	const std::string sSelPTG =
		std::string(cbPTGClass->GetString(sel).mb_str());

	ptg.reset();

	// Factory:
	const mrpt::rtti::TRuntimeClassId* classId =
		mrpt::rtti::findRegisteredClass(sSelPTG);
	if (!classId)
	{
		THROW_EXCEPTION_FMT(
			"[CreatePTG] No PTG named `%s` is registered!", sSelPTG.c_str());
	}

	ptg = mrpt::ptr_cast<mrpt::nav::CParameterizedTrajectoryGenerator>::from(
		classId->createObject());
	if (!ptg)
	{
		THROW_EXCEPTION_FMT(
			"[CreatePTG] Object of type `%s` seems not to be a PTG!",
			sSelPTG.c_str());
	}

	// Set some common defaults:
	ptg->loadDefaultParams();

	if (!m_disableLoadDefaultParams) dumpPTGcfgToTextBox();

	WX_END_TRY;

	// Update graphs:
	rebuild3Dview();
}

void ptgConfiguratorframe::rebuild3Dview()
{
	WX_START_TRY;
	static mrpt::system::CTicTac timer;
	const double refDist = ptg ? ptg->getRefDistance() : 10.0;
	ASSERT_(refDist > 0);

	// Limits:
	gl_axis_WS->setAxisLimits(-refDist, -refDist, .0f, refDist, refDist, .0f);

	double tx = 10.0, ty = .0, tphi_deg = .0;  // Target in WS
	{
		bool ok_x = edTargetX->GetValue().ToCDouble(&tx);
		bool ok_y = edTargetY->GetValue().ToCDouble(&ty);
		bool ok_phi = edTargetPhiDeg->GetValue().ToCDouble(&tphi_deg);
		if (ok_x && ok_y && ok_phi)
		{
			gl_WS_target->setPose(
				mrpt::math::TPose3D(tx, ty, 0, mrpt::DEG2RAD(tphi_deg), 0, 0));
		}
	}

	if (ptg && ptg->isInitialized())
	{
		// Update PTG dynamic state with new target:
		{
			mrpt::nav::CParameterizedTrajectoryGenerator::TNavDynamicState
				navdyn = ptg->getCurrentNavDynamicState();
			navdyn.relTarget.x = tx;
			navdyn.relTarget.y = ty;
			navdyn.relTarget.phi = mrpt::DEG2RAD(tphi_deg);
			edRelSpeedAtTarget->GetValue().ToCDouble(&navdyn.targetRelSpeed);
			ptg->updateNavDynamicState(navdyn);
		}

		// TP-Obstacles:
		std::vector<double> TP_Obstacles;
		ptg->initTPObstacles(TP_Obstacles);

		gl_WS_obs->clear();
		{
			double ox, oy;
			bool ok_x = edObsX->GetValue().ToCDouble(&ox);
			bool ok_y = edObsY->GetValue().ToCDouble(&oy);
			if (ok_x && ok_y)
			{
				if (cbBuildTPObs->IsChecked())
				{
					gl_WS_obs->insertPoint(ox, oy, 0);
					timer.Tic();
					ptg->updateTPObstacle(ox, oy, TP_Obstacles);
					const double t = timer.Tac();
					StatusBar1->SetStatusText(
						wxString::Format(
							wxT("TP-Obstacle build time: %ss"),
							mrpt::system::unitsFormat(t, 2).c_str()),
						2);
				}

				// Clearance diagram:
				mrpt::nav::ClearanceDiagram cd;
				ptg->initClearanceDiagram(cd);
				double tim_build_cd = .0;
				{
					timer.Tic();
					ptg->updateClearance(ox, oy, cd);
					ptg->updateClearancePost(cd, TP_Obstacles);
					tim_build_cd = timer.Tac();
				}

				timer.Tic();
				cd.renderAs3DObject(
					*gl_TPSpace_clearance, -1.0, 1.0, -1.0, 1.0, 0.05,
					false /*interpolate*/);
				cd.renderAs3DObject(
					*gl_TPSpace_clearance_interp, -1.0, 1.0, -1.0, 1.0, 0.05,
					true /*interpolate*/);
				const double tim_render_cd = timer.Tac();

				StatusBar1->SetStatusText(
					wxString::Format(
						wxT("Clearance-diagram time: build=%ss render=%ss"),
						mrpt::system::unitsFormat(tim_build_cd, 2).c_str(),
						mrpt::system::unitsFormat(tim_render_cd, 2).c_str()),
					3);
			}
		}

		try
		{
			const double ptg_alpha =
				ptg->index2alpha(edIndexHighlightPath->GetValue());
			StaticText12->SetLabel(wxString::Format(
				_("Selected path trajectory: Phi [deg]. PTG alpha=%.03f "
				  "[deg]"),
				mrpt::RAD2DEG(ptg_alpha)));
		}
		catch (...)
		{
		}

		double max_draw_ptg_time = 100.0;
		edDrawMaxTime->GetValue().ToCDouble(&max_draw_ptg_time);

		const size_t nPTGPaths = ptg->getPathCount();

		const double stepTime = ptg->getPathStepDuration();
		ASSERT_(stepTime > 0);

		// All paths:
		gl_robot_ptg_prediction->clear();
		gl_robot_ptg_prediction_highlight->clear();
		for (size_t k = 0; k < nPTGPaths; k++)
		{
			if (cbShowOnlySelectedTraj->IsChecked() &&
				k != size_t(edIndexHighlightPath->GetValue()))
				continue;

			double max_dist = TP_Obstacles[k];

			{
				const uint32_t maxStepByTime = max_draw_ptg_time / stepTime;
				double maxRenderDist = ptg->getPathDist(k, maxStepByTime);
				mrpt::keep_min(max_dist, maxRenderDist);
			}

			mrpt::opengl::CSetOfLines& sol = cbHighlightOnePath->IsChecked() &&
					k == size_t(edIndexHighlightPath->GetValue())
				? *gl_robot_ptg_prediction_highlight
				: *gl_robot_ptg_prediction;

			ptg->renderPathAsSimpleLine(k, sol, 0.10f, max_dist);

			// Overlay a sequence of robot shapes:
			if (cbDrawShapePath->IsChecked())
			{
				double min_shape_dists = 1.0;
				edMinDistBtwShapes->GetValue().ToCDouble(&min_shape_dists);
				bool done = false;
				for (double d = max_dist; !done; d -= min_shape_dists)
				{
					if (d < 0)
					{
						d = 0;
						done = true;
					}
					uint32_t step;
					if (!ptg->getPathStepForDist(k, d, step)) continue;
					const auto p = ptg->getPathPose(k, step);
					ptg->add_robotShape_to_setOfLines(
						sol, mrpt::poses::CPose2D(p));
				}
			}
		}

		// selected PTG trajectory motion cmd:
		std::string strMotionCmd;
		try
		{
			const auto cmd =
				ptg->directionToMotionCommand(edIndexHighlightPath->GetValue());
			strMotionCmd = cmd->asString();
		}
		catch (const std::exception& e)
		{
			strMotionCmd = e.what();
		}
		edSelectedTrajCmd->SetValue(strMotionCmd);

		// 2D angle to robot head plots:
		std::vector<double> robotHeadAng_x, robotHeadAng_y,
			robotHeadAngAll_x(nPTGPaths), robotHeadAngAll_y(nPTGPaths);
		std::vector<double> robotPath_x, robotPath_y, robotPath_phi,
			robotPath_dist;
		std::vector<double> robotPath_vx, robotPath_vy, robotPath_w;
		const double dt = ptg->getPathStepDuration();

		for (size_t k = 0; k < nPTGPaths; k++)
		{
			robotHeadAngAll_x[k] = k;
			bool is_selected_path =
				(k == size_t(edIndexHighlightPath->GetValue()));

			size_t nSteps = ptg->getPathStepCount(k);

			if (is_selected_path)
			{
				robotHeadAng_x.resize(nSteps);
				robotHeadAng_y.resize(nSteps);
				robotPath_x.resize(nSteps);
				robotPath_y.resize(nSteps);
				robotPath_phi.resize(nSteps);
				robotPath_dist.resize(nSteps);
				robotPath_vx.resize(nSteps);
				robotPath_vy.resize(nSteps);
				robotPath_w.resize(nSteps);
			}

			double maxRobotHeadErr = .0;
			for (size_t j = 0; j < nSteps; j++)
			{
				const mrpt::math::TPose2D curPose = ptg->getPathPose(k, j);
				const mrpt::math::TTwist2D curVel = ptg->getPathTwist(k, j);

				// Head calc:
				const double head2dir = (curVel.vy != 0 || curVel.vx != 0)
					? mrpt::math::angDistance(
						  ::atan2(curVel.vy, curVel.vx), curPose.phi)
					: .0;

				if (is_selected_path)
				{
					robotHeadAng_x[j] = j * dt;
					robotHeadAng_y[j] = mrpt::RAD2DEG(head2dir);
					robotPath_x[j] = curPose.x;
					robotPath_y[j] = curPose.y;
					robotPath_phi[j] = mrpt::RAD2DEG(curPose.phi);
					robotPath_dist[j] = ptg->getPathDist(k, j);

					robotPath_vx[j] = curVel.vx;
					robotPath_vy[j] = curVel.vy;
					robotPath_w[j] = mrpt::RAD2DEG(curVel.omega);
				}

				mrpt::keep_max(maxRobotHeadErr, std::abs(head2dir));
			}

			robotHeadAngAll_y[k] = mrpt::RAD2DEG(maxRobotHeadErr);
		}
		m_graph_head_all->SetData(robotHeadAngAll_x, robotHeadAngAll_y);
		m_graph_head_indiv->SetData(robotHeadAng_x, robotHeadAng_y);

		m_graph_path_x->SetData(robotHeadAng_x, robotPath_x);
		m_graph_path_y->SetData(robotHeadAng_x, robotPath_y);
		m_graph_path_phi->SetData(robotHeadAng_x, robotPath_phi);
		m_graph_path_dist->SetData(robotHeadAng_x, robotPath_dist);

		m_graph_path_vx->SetData(robotHeadAng_x, robotPath_vx);
		m_graph_path_vy->SetData(robotHeadAng_x, robotPath_vy);
		m_graph_path_omega->SetData(robotHeadAng_x, robotPath_w);

		// TP-Obstacles:
		gl_tp_obstacles->clear();
		const size_t nObs = TP_Obstacles.size();
		if (nObs > 1)
		{
			for (size_t i = 0; i <= nObs; i++)
			{
				const double d0 = TP_Obstacles[i % nObs] / refDist;
				const double a0 =
					M_PI * (-1.0 + 2.0 * ((i % nObs) + 0.5) / nObs);
				const double d1 = TP_Obstacles[(i + 1) % nObs] / refDist;
				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);
			}
		}
	}  // if ptg init

	// Target:
	{
		if (ptg && ptg->isInitialized())
		{
			int k = 0;
			double norm_d = 0;
			bool is_exact = ptg->inverseMap_WS2TP(tx, ty, k, norm_d);
			const double dir = ptg->index2alpha(k);
			if (is_exact)
			{
				gl_TP_target->setLocation(
					cos(dir) * norm_d, sin(dir) * norm_d, .0);
				gl_TP_target->setName("TP-target");
			}
			else
			{
				gl_TP_target->setName("TP-target (NOT EXACT)");
			}
			StatusBar1->SetStatusText(
				wxString::Format(
					wxT("TP-Target: k=%i (alpha=%.03f deg) norm_d=%.03f "
						"is_exact:%s"),
					k, dir * 180 / M_PI, norm_d, (is_exact ? "yes" : "NO")),
				1);

			// Sanity check: reproject TP_target back to WS:
			uint32_t check_step;
			if (ptg->getPathStepForDist(
					k, norm_d * ptg->getRefDistance(), check_step))
			{
				gl_WS_target_reprojected->setPose(
					ptg->getPathPose(k, check_step));
				gl_WS_target_reprojected->setName("WS-Target-reproj");
			}
			else
			{
				gl_WS_target_reprojected->setName(
					"WS-Target-reproj (not exact!)");
			}
		}
	}

	m_plotHeadAngAll->Fit();
	m_plotHeadAngIndiv->Fit();
	m_plotPathX->Fit();
	m_plotPathY->Fit();
	m_plotPathPhi->Fit();
	m_plotPathXp->Fit();
	m_plotPathYp->Fit();
	m_plotPathW->Fit();
	m_plotPathDist->Fit();

	m_plotHeadAngAll->UpdateAll();
	m_plotHeadAngIndiv->UpdateAll();
	m_plotPathX->UpdateAll();
	m_plotPathY->UpdateAll();
	m_plotPathPhi->UpdateAll();
	m_plotPathXp->UpdateAll();
	m_plotPathYp->UpdateAll();
	m_plotPathW->UpdateAll();
	m_plotPathDist->UpdateAll();

	m_plot->Refresh();
	m_plotTPSpace->Refresh();
	WX_END_TRY;
}

void ptgConfiguratorframe::loadPlugin()
{
	wxFileDialog openFileDialog(
		this, _("Open library"), wxT(""), wxT(""),
		wxT("so files (*.so)|*.so|so files (*.so.*)|*.so.*|*.dll"),
		wxFD_OPEN | wxFD_FILE_MUST_EXIST);

	if (openFileDialog.ShowModal() == wxID_CANCEL) return;

	const std::string sLib = std::string(openFileDialog.GetPath().mb_str());
	mrpt::system::loadPluginModule(sLib);

	// Populate list of existing PTGs:
	{
		cbPTGClass->Clear();
		const std::vector<const mrpt::rtti::TRuntimeClassId*>& lstClasses =
			mrpt::rtti::getAllRegisteredClasses();
		for (auto lstClasse : lstClasses)
		{
			if (!lstClasse->derivedFrom(
					"mrpt::nav::CParameterizedTrajectoryGenerator") ||
				!mrpt::system::os::_strcmpi(
					lstClasse->className,
					"mrpt::nav::CParameterizedTrajectoryGenerator"))
				continue;
			cbPTGClass->AppendString(lstClasse->className);
		}
		if (cbPTGClass->GetCount() > 0) cbPTGClass->SetSelection(0);
	}
	{
		wxCommandEvent e;
		OncbPTGClassSelect(e);
	}
}

void ptgConfiguratorframe::OnedPTGIndexChange(wxSpinEvent& event)
{
	dumpPTGcfgToTextBox();
}

void ptgConfiguratorframe::dumpPTGcfgToTextBox()
{
	if (!ptg) return;

	// Wrapper to transparently add prefixes to all config keys:
	const std::string sKeyPrefix =
		mrpt::format("PTG%d_", (int)edPTGIndex->GetValue());
	const std::string sSection = m_cfgFileSection;

	mrpt::config::CConfigFileMemory cfg;
	mrpt::config::CConfigFilePrefixer cfp;
	cfp.bind(cfg);
	cfp.setPrefixes("", sKeyPrefix);

	const int WN = 25, WV = 30;
	cfp.write(
		sSection, "Type", ptg->GetRuntimeClass()->className, WN, WV,
		"PTG C++ class name");

	// Dump default params:
	ptg->saveToConfigFile(cfp, sSection);

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

void ptgConfiguratorframe::OncbDrawShapePathClick(wxCommandEvent& event)
{
	rebuild3Dview();
}

void ptgConfiguratorframe::OncbBuildTPObsClick(wxCommandEvent& event)
{
	rebuild3Dview();
}

void ptgConfiguratorframe::OnbtnRebuildTPObsClick(wxCommandEvent& event)
{
	rebuild3Dview();
}

void ptgConfiguratorframe::Onplot3DMouseMove(wxMouseEvent& event)
{
	using namespace mrpt::math;

	int X, Y;
	event.GetPosition(&X, &Y);

	// Intersection of 3D ray with ground plane ====================
	TLine3D ray;
	m_plot->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 cpsPickObstacle:
			{
				edObsX->SetValue(
					(mrpt::format("%.03f", m_curCursorPos.x).c_str()));
				edObsY->SetValue(
					(mrpt::format("%.03f", m_curCursorPos.y).c_str()));
				rebuild3Dview();
			}
			break;
			case cpsPickTarget:
			{
				edTargetX->SetValue(
					(mrpt::format("%.03f", m_curCursorPos.x).c_str()));
				edTargetY->SetValue(
					(mrpt::format("%.03f", m_curCursorPos.y).c_str()));
				rebuild3Dview();
			}
			break;
			default: break;
		};
		StatusBar1->SetStatusText(
			wxString::Format(
				wxT("Cursor: X=%.03f Y=%.04f"), m_curCursorPos.x,
				m_curCursorPos.y),
			0);
	}

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

void ptgConfiguratorframe::Onplot3DMouseClick(wxMouseEvent& event)
{
	m_plot->SetCursor(*wxSTANDARD_CURSOR);	// End of cross cursor
	m_cursorPickState = cpsNone;  // end of mode

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

void ptgConfiguratorframe::OnbtnPlaceObsClick(wxCommandEvent& event)
{
	m_plot->SetCursor(*wxCROSS_CURSOR);
	m_cursorPickState = cpsPickObstacle;
}

void ptgConfiguratorframe::OnbtnPlaceTargetClick(wxCommandEvent& event)
{
	m_plot->SetCursor(*wxCROSS_CURSOR);
	m_cursorPickState = cpsPickTarget;
}

void ptgConfiguratorframe::OnslidPathHighlightCmdScroll(wxCommandEvent&)
{
	edIndexHighlightPath->SetValue(slidPathHighlight->GetValue());
	wxSpinEvent dm;
	OnedIndexHighlightPathChange(dm);
}

void ptgConfiguratorframe::OncbHighlightOnePathClick(wxCommandEvent& event)
{
	// slidPathHighlight->Enable( cbHighlightOnePath->IsChecked() );
	// edIndexHighlightPath->Enable( cbHighlightOnePath->IsChecked() );
	rebuild3Dview();
}

void ptgConfiguratorframe::OnedIndexHighlightPathChange(wxSpinEvent& event)
{
	rebuild3Dview();
}

void ptgConfiguratorframe::OnButton1Click(wxCommandEvent& event)
{
	loadPlugin();
}

void ptgConfiguratorframe::OnrbShowTPSelectSelect(wxCommandEvent& event)
{
	WX_START_TRY;

	gl_TPSpace_TP_obstacles->setVisibility(cbShowTPObs->IsChecked());
	gl_TPSpace_clearance->setVisibility(
		cbShowClearance->IsChecked() && !cbClearanceInterp->IsChecked());
	gl_TPSpace_clearance_interp->setVisibility(
		cbShowClearance->IsChecked() && cbClearanceInterp->IsChecked());

	m_plotTPSpace->Refresh();
	WX_END_TRY;
}

static std::string vectorToMatlab(
	const std::string& varName, const std::vector<double>& v)
{
	std::string s;
	s = varName;
	s += "=[";

	for (size_t i = 0; i < v.size(); i++)
	{
		s += mrpt::format("%.04g", v[i]);
		if (i + 1 != v.size()) s += ", ";
		if ((i % 20) == 19) s += " ...\n  ";
	}

	s += "];\n";

	return s;
}

void ptgConfiguratorframe::OnExportSelectedPath(wxCommandEvent&)
{
	WX_START_TRY

	if (!ptg) return;

	wxFileDialog openFileDialog(
		this, _("Save selected path to .m"), wxT(""), wxT(""),
		wxT("m files (*.m)|*.m"), wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

	if (openFileDialog.ShowModal() == wxID_CANCEL) return;

	const std::string sFil = openFileDialog.GetPath().ToStdString();

	std::ofstream f(sFil.c_str());
	if (!f.is_open())
		THROW_EXCEPTION_FMT("Cannot open file for writing: '%s'", sFil.c_str());

	f << "%\n";
	f << "% File generated automatically by ptg-configurator, MRPT project.\n";
	f << "%\n";
	f << "% For PTG path 0-based index " << edIndexHighlightPath->GetValue()
	  << " out of " << ptg->getPathCount() << " alpha="
	  << mrpt::RAD2DEG(ptg->index2alpha(edIndexHighlightPath->GetValue()))
	  << " [deg]\n";
	f << "% PTG details:\n%{\n";

	const std::string sKeyPrefix =
		mrpt::format("PTG%d_", (int)edPTGIndex->GetValue());
	const std::string sSection = m_cfgFileSection;
	mrpt::config::CConfigFileMemory cfg;
	mrpt::config::CConfigFilePrefixer cfp;
	cfp.bind(cfg);
	cfp.setPrefixes("", sKeyPrefix);
	const int WN = 25, WV = 30;
	cfp.write(
		sSection, "Type", ptg->GetRuntimeClass()->className, WN, WV,
		"PTG C++ class name");
	ptg->saveToConfigFile(cfp, sSection);

	f << cfg.getContent();
	f << "%}\n";

	const auto& ts = m_graph_path_x->GetDataX();
	const auto& xs = m_graph_path_x->GetDataY();
	const auto& ys = m_graph_path_y->GetDataY();
	const auto& phis = m_graph_path_phi->GetDataY();

	const auto& vxs = m_graph_path_vx->GetDataY();
	const auto& vys = m_graph_path_vy->GetDataY();
	const auto& ws = m_graph_path_omega->GetDataY();

	f << vectorToMatlab("t", ts);
	f << vectorToMatlab("x", xs);
	f << vectorToMatlab("y", ys);
	f << vectorToMatlab("phi", phis);
	f << vectorToMatlab("vx", vxs);
	f << vectorToMatlab("vy", vys);
	f << vectorToMatlab("w", ws);

	f <<
		R"XX(
figure();
plot(x,y,'.');
axis equal; grid minor;
title('XY path');

figure();
subplot(3,1,1); plot(t,x,'.'); xlabel('t [s]');title('x(t) [m]'); grid minor;
subplot(3,1,2); plot(t,y,'.'); xlabel('t [s]');title('y(t) [m]'); grid minor;
subplot(3,1,3); plot(t,phi,'.'); xlabel('t [s]');title('phi(t) [deg]'); grid minor;

figure();
subplot(3,1,1); plot(t,vx,'.'); xlabel('t [s]');title('vx(t) [m/s]'); grid minor;
subplot(3,1,2); plot(t,vy,'.'); xlabel('t [s]');title('vy(t) [m/s]'); grid minor;
subplot(3,1,3); plot(t,w,'.'); xlabel('t [s]');title('w(t) [deg/s]'); grid minor;
)XX";

	WX_END_TRY
}
