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

#include "slamdemoMain.h"

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

#include "CDlgParams.h"
#include "CLogView.h"
#include "slamdemoApp.h"

//(*InternalHeaders(slamdemoFrame)
#include <wx/artprov.h>
#include <wx/bitmap.h>
#include <wx/font.h>
#include <wx/icon.h>
#include <wx/image.h>
#include <wx/intl.h>
#include <wx/string.h>
//*)
#include <mrpt/gui/about_box.h>
#include <mrpt/gui/wx28-fixes.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/math/ops_matrices.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/obs/CObservationComment.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>

#include <memory>

using namespace std;
using namespace mrpt;
using namespace mrpt::io;
using namespace mrpt::bayes;
using namespace mrpt::random;
using namespace mrpt::serialization;
using namespace mrpt::system;
using namespace mrpt::slam;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::poses;

#include "../wx-common/mrpt_logo.xpm"
#include "imgs/icon_batch.xpm"
#include "imgs/icon_config.xpm"
#include "imgs/icon_exit.xpm"
#include "imgs/icon_help.xpm"
#include "imgs/icon_play.xpm"
#include "imgs/icon_reset.xpm"
#include "imgs/icon_step.xpm"
#include "imgs/icon_stop.xpm"
#include "imgs/icono_main.xpm"

static const double RAD2DEGSQ = square(180 / M_PI);

// 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;
};
wxBitmap MyArtProvider::CreateBitmap(
	const wxArtID& id, const wxArtClient& client, const wxSize& size)
{
	if (id == wxART_MAKE_ART_ID(MAIN_ICON)) return wxBitmap(icono_main_xpm);
	if (id == wxART_MAKE_ART_ID(IMG_MRPT_LOGO)) return wxBitmap(mrpt_logo_xpm);

	if (id == wxART_MAKE_ART_ID(ICON_BATCH)) return wxBitmap(icon_batch_xpm);
	if (id == wxART_MAKE_ART_ID(ICON_CONFIG)) return wxBitmap(icon_config_xpm);
	if (id == wxART_MAKE_ART_ID(ICON_HELP)) return wxBitmap(icon_help_xpm);
	if (id == wxART_MAKE_ART_ID(ICON_EXIT)) return wxBitmap(icon_exit_xpm);
	if (id == wxART_MAKE_ART_ID(ICON_PLAY)) return wxBitmap(icon_play_xpm);
	if (id == wxART_MAKE_ART_ID(ICON_RESET)) return wxBitmap(icon_reset_xpm);
	if (id == wxART_MAKE_ART_ID(ICON_STEP)) return wxBitmap(icon_step_xpm);
	if (id == wxART_MAKE_ART_ID(ICON_STOP)) return wxBitmap(icon_stop_xpm);

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

//(*IdInit(slamdemoFrame)
const long slamdemoFrame::ID_STATICTEXT1 = wxNewId();
const long slamdemoFrame::ID_PANEL3 = wxNewId();
const long slamdemoFrame::ID_CUSTOM1 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT2 = wxNewId();
const long slamdemoFrame::ID_PANEL4 = wxNewId();
const long slamdemoFrame::ID_CUSTOM2 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT10 = wxNewId();
const long slamdemoFrame::ID_PANEL5 = wxNewId();
const long slamdemoFrame::ID_CUSTOM3 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT3 = wxNewId();
const long slamdemoFrame::ID_PANEL6 = wxNewId();
const long slamdemoFrame::ID_CUSTOM4 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT4 = wxNewId();
const long slamdemoFrame::ID_PANEL7 = wxNewId();
const long slamdemoFrame::ID_GRID1 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT5 = wxNewId();
const long slamdemoFrame::ID_PANEL9 = wxNewId();
const long slamdemoFrame::ID_CUSTOM7 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT6 = wxNewId();
const long slamdemoFrame::ID_PANEL10 = wxNewId();
const long slamdemoFrame::ID_CUSTOM8 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT7 = wxNewId();
const long slamdemoFrame::ID_PANEL11 = wxNewId();
const long slamdemoFrame::ID_CUSTOM9 = wxNewId();
const long slamdemoFrame::ID_PANEL1 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT9 = wxNewId();
const long slamdemoFrame::ID_PANEL8 = wxNewId();
const long slamdemoFrame::ID_CUSTOM5 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT11 = wxNewId();
const long slamdemoFrame::ID_PANEL14 = wxNewId();
const long slamdemoFrame::ID_CUSTOM6 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT12 = wxNewId();
const long slamdemoFrame::ID_PANEL15 = wxNewId();
const long slamdemoFrame::ID_CUSTOM11 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT13 = wxNewId();
const long slamdemoFrame::ID_PANEL16 = wxNewId();
const long slamdemoFrame::ID_CUSTOM12 = wxNewId();
const long slamdemoFrame::ID_PANEL2 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT8 = wxNewId();
const long slamdemoFrame::ID_PANEL13 = wxNewId();
const long slamdemoFrame::ID_CUSTOM10 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT14 = wxNewId();
const long slamdemoFrame::ID_PANEL17 = wxNewId();
const long slamdemoFrame::ID_CUSTOM13 = wxNewId();
const long slamdemoFrame::ID_PANEL12 = wxNewId();
const long slamdemoFrame::ID_NOTEBOOK1 = wxNewId();
const long slamdemoFrame::ID_MENUITEM1 = wxNewId();
const long slamdemoFrame::ID_MENUITEM2 = wxNewId();
const long slamdemoFrame::ID_MENUITEM3 = wxNewId();
const long slamdemoFrame::ID_MENUITEM6 = wxNewId();
const long slamdemoFrame::ID_MENUITEM4 = wxNewId();
const long slamdemoFrame::ID_MENUITEM5 = wxNewId();
const long slamdemoFrame::idMenuQuit = wxNewId();
const long slamdemoFrame::ID_MENUITEM8 = wxNewId();
const long slamdemoFrame::ID_MENUITEM11 = wxNewId();
const long slamdemoFrame::ID_MENUITEM_SAVE_RAWLOG = wxNewId();
const long slamdemoFrame::ID_MENUITEM9 = wxNewId();
const long slamdemoFrame::ID_MENUITEM10 = wxNewId();
const long slamdemoFrame::ID_MENUITEM7 = wxNewId();
const long slamdemoFrame::idMenuAbout = wxNewId();
const long slamdemoFrame::ID_STATUSBAR1 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM1 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM2 = wxNewId();
const long slamdemoFrame::ID_BTNRUN = wxNewId();
const long slamdemoFrame::ID_BTNSTOP = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM4 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM3 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM6 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM7 = wxNewId();
const long slamdemoFrame::ID_TOOLBAR1 = wxNewId();
const long slamdemoFrame::ID_TIMER1 = wxNewId();
//*)

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

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

	//(*Initialize(slamdemoFrame)
	wxMenuItem* MenuItem2;
	wxGridSizer* GridSizer13;
	wxMenuItem* MenuItem1;
	wxFlexGridSizer* FlexGridSizer8;
	wxGridSizer* GridSizer1;
	wxFlexGridSizer* FlexGridSizer1;
	wxFlexGridSizer* FlexGridSizer2;
	wxMenu* Menu1;
	wxGridSizer* GridSizer4;
	wxGridSizer* GridSizer7;
	wxGridSizer* GridSizer3;
	wxGridSizer* GridSizer2;
	wxGridSizer* GridSizer6;
	wxFlexGridSizer* FlexGridSizer7;
	wxFlexGridSizer* FlexGridSizer4;
	wxFlexGridSizer* FlexGridSizer9;
	wxFlexGridSizer* FlexGridSizer6;
	wxGridSizer* GridSizer11;
	wxGridSizer* GridSizer5;
	wxFlexGridSizer* FlexGridSizer3;
	wxGridSizer* GridSizer14;
	wxGridSizer* GridSizer10;
	wxGridSizer* GridSizer8;
	wxMenuBar* MenuBar1;
	wxGridSizer* GridSizer9;
	wxMenuItem* MenuItem7;
	wxMenu* Menu2;
	wxGridSizer* GridSizer12;
	wxFlexGridSizer* FlexGridSizer5;

	Create(
		parent, wxID_ANY, _("2D SLAM Demo - 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_FRAME_ICON));
		SetIcon(FrameIcon);
	}
	FlexGridSizer1 = new wxFlexGridSizer(2, 1, 0, 0);
	FlexGridSizer1->AddGrowableCol(0);
	FlexGridSizer1->AddGrowableRow(0);
	FlexGridSizer3 = new wxFlexGridSizer(1, 4, 0, 0);
	FlexGridSizer3->AddGrowableCol(0);
	FlexGridSizer3->AddGrowableCol(1);
	FlexGridSizer3->AddGrowableCol(2);
	FlexGridSizer3->AddGrowableCol(3);
	FlexGridSizer3->AddGrowableRow(0);
	FlexGridSizer4 = new wxFlexGridSizer(4, 1, 0, 0);
	FlexGridSizer4->AddGrowableCol(0);
	FlexGridSizer4->AddGrowableRow(1);
	FlexGridSizer4->AddGrowableRow(3);
	Panel3 = new wxPanel(
		this, ID_PANEL3, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL3"));
	Panel3->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer1 = new wxGridSizer(0, 1, 0, 0);
	lbGT = new wxStaticText(
		Panel3, ID_STATICTEXT1, _("Ground truth"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT1"));
	wxFont lbGTFont(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	lbGT->SetFont(lbGTFont);
	GridSizer1->Add(
		lbGT, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	Panel3->SetSizer(GridSizer1);
	GridSizer1->Fit(Panel3);
	GridSizer1->SetSizeHints(Panel3);
	FlexGridSizer4->Add(
		Panel3, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	plotGT =
		new mpWindow(this, ID_CUSTOM1, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer4->Add(
		plotGT, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 3);
	Panel4 = new wxPanel(
		this, ID_PANEL4, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL4"));
	Panel4->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer2 = new wxGridSizer(0, 1, 0, 0);
	lbObs = new wxStaticText(
		Panel4, ID_STATICTEXT2, _("Observation"), wxDefaultPosition,
		wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT2"));
	wxFont lbObsFont(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	lbObs->SetFont(lbObsFont);
	GridSizer2->Add(
		lbObs, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	Panel4->SetSizer(GridSizer2);
	GridSizer2->Fit(Panel4);
	GridSizer2->SetSizeHints(Panel4);
	FlexGridSizer4->Add(
		Panel4, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	plotObs =
		new mpWindow(this, ID_CUSTOM2, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer4->Add(
		plotObs, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 3);
	FlexGridSizer3->Add(
		FlexGridSizer4, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	FlexGridSizer5 = new wxFlexGridSizer(4, 1, 0, 0);
	FlexGridSizer5->AddGrowableCol(0);
	FlexGridSizer5->AddGrowableRow(1);
	FlexGridSizer5->AddGrowableRow(3);
	Panel5 = new wxPanel(
		this, ID_PANEL5, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL5"));
	Panel5->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer3 = new wxGridSizer(0, 1, 0, 0);
	lbMap = new wxStaticText(
		Panel5, ID_STATICTEXT10, _("Map"), wxDefaultPosition, wxDefaultSize,
		wxALIGN_CENTRE, _T("ID_STATICTEXT10"));
	wxFont lbMapFont(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	lbMap->SetFont(lbMapFont);
	GridSizer3->Add(
		lbMap, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	Panel5->SetSizer(GridSizer3);
	GridSizer3->Fit(Panel5);
	GridSizer3->SetSizeHints(Panel5);
	FlexGridSizer5->Add(
		Panel5, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	plotMap =
		new mpWindow(this, ID_CUSTOM3, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer5->Add(
		plotMap, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 3);
	Panel6 = new wxPanel(
		this, ID_PANEL6, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL6"));
	Panel6->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer4 = new wxGridSizer(0, 1, 0, 0);
	lmIndCompat = new wxStaticText(
		Panel6, ID_STATICTEXT3, _("Indiv. compat (95% ellips.)"),
		wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT3"));
	wxFont lmIndCompatFont(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	lmIndCompat->SetFont(lmIndCompatFont);
	GridSizer4->Add(
		lmIndCompat, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	Panel6->SetSizer(GridSizer4);
	GridSizer4->Fit(Panel6);
	GridSizer4->SetSizeHints(Panel6);
	FlexGridSizer5->Add(
		Panel6, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	plotIndivCompat =
		new mpWindow(this, ID_CUSTOM4, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer5->Add(
		plotIndivCompat, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 3);
	FlexGridSizer3->Add(
		FlexGridSizer5, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	FlexGridSizer6 = new wxFlexGridSizer(2, 1, 0, 0);
	FlexGridSizer6->AddGrowableCol(0);
	FlexGridSizer6->AddGrowableRow(1);
	Panel7 = new wxPanel(
		this, ID_PANEL7, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL7"));
	Panel7->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer5 = new wxGridSizer(0, 1, 0, 0);
	lbDatAssoc = new wxStaticText(
		Panel7, ID_STATICTEXT4, _("Dat assoc"), wxDefaultPosition,
		wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT4"));
	wxFont lbDatAssocFont(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	lbDatAssoc->SetFont(lbDatAssocFont);
	GridSizer5->Add(
		lbDatAssoc, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	Panel7->SetSizer(GridSizer5);
	GridSizer5->Fit(Panel7);
	GridSizer5->SetSizeHints(Panel7);
	FlexGridSizer6->Add(
		Panel7, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	gridDA = new wxGrid(
		this, ID_GRID1, wxDefaultPosition, wxDefaultSize, 0, _T("ID_GRID1"));
	gridDA->CreateGrid(3, 3);
	gridDA->EnableEditing(false);
	gridDA->EnableGridLines(true);
	gridDA->SetColLabelSize(20);
	gridDA->SetRowLabelSize(40);
	gridDA->SetDefaultColSize(40, true);
	gridDA->SetDefaultCellFont(gridDA->GetFont());
	gridDA->SetDefaultCellTextColour(gridDA->GetForegroundColour());
	FlexGridSizer6->Add(
		gridDA, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	FlexGridSizer3->Add(
		FlexGridSizer6, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	FlexGridSizer7 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer7->AddGrowableCol(0);
	FlexGridSizer7->AddGrowableRow(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"));
	FlexGridSizer8 = new wxFlexGridSizer(6, 1, 0, 0);
	FlexGridSizer8->AddGrowableCol(0);
	FlexGridSizer8->AddGrowableRow(1);
	FlexGridSizer8->AddGrowableRow(3);
	FlexGridSizer8->AddGrowableRow(5);
	Panel8 = new wxPanel(
		Panel1, ID_PANEL9, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL9"));
	Panel8->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer7 = new wxGridSizer(0, 1, 0, 0);
	StaticText2 = new wxStaticText(
		Panel8, ID_STATICTEXT5, _("Vehicle X (err,99% bounds)"),
		wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT5"));
	wxFont StaticText2Font(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	StaticText2->SetFont(StaticText2Font);
	GridSizer7->Add(
		StaticText2, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	Panel8->SetSizer(GridSizer7);
	GridSizer7->Fit(Panel8);
	GridSizer7->SetSizeHints(Panel8);
	FlexGridSizer8->Add(
		Panel8, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	plotErrorX =
		new mpWindow(Panel1, ID_CUSTOM7, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer8->Add(
		plotErrorX, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 3);
	Panel9 = new wxPanel(
		Panel1, ID_PANEL10, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL10"));
	Panel9->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer8 = new wxGridSizer(0, 1, 0, 0);
	StaticText3 = new wxStaticText(
		Panel9, ID_STATICTEXT6, _("Vehicle Y (err,99% bounds)"),
		wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT6"));
	wxFont StaticText3Font(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	StaticText3->SetFont(StaticText3Font);
	GridSizer8->Add(
		StaticText3, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	Panel9->SetSizer(GridSizer8);
	GridSizer8->Fit(Panel9);
	GridSizer8->SetSizeHints(Panel9);
	FlexGridSizer8->Add(
		Panel9, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	plotErrorY =
		new mpWindow(Panel1, ID_CUSTOM8, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer8->Add(
		plotErrorY, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 3);
	Panel10 = new wxPanel(
		Panel1, ID_PANEL11, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL11"));
	Panel10->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer9 = new wxGridSizer(0, 1, 0, 0);
	StaticText4 = new wxStaticText(
		Panel10, ID_STATICTEXT7, _("Vehicle Phi (err,99% bounds)"),
		wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT7"));
	wxFont StaticText4Font(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	StaticText4->SetFont(StaticText4Font);
	GridSizer9->Add(
		StaticText4, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	Panel10->SetSizer(GridSizer9);
	GridSizer9->Fit(Panel10);
	GridSizer9->SetSizeHints(Panel10);
	FlexGridSizer8->Add(
		Panel10, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	plotErrorPhi =
		new mpWindow(Panel1, ID_CUSTOM9, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer8->Add(
		plotErrorPhi, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 3);
	Panel1->SetSizer(FlexGridSizer8);
	FlexGridSizer8->Fit(Panel1);
	FlexGridSizer8->SetSizeHints(Panel1);
	Panel2 = new wxPanel(
		Notebook1, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL2"));
	FlexGridSizer9 = new wxFlexGridSizer(8, 1, 0, 0);
	FlexGridSizer9->AddGrowableCol(0);
	FlexGridSizer9->AddGrowableRow(1);
	FlexGridSizer9->AddGrowableRow(3);
	FlexGridSizer9->AddGrowableRow(5);
	FlexGridSizer9->AddGrowableRow(7);
	Panel13 = new wxPanel(
		Panel2, ID_PANEL8, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL8"));
	Panel13->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer6 = new wxGridSizer(0, 1, 0, 0);
	lbDaTP = new wxStaticText(
		Panel13, ID_STATICTEXT9, _("True positives:"), wxDefaultPosition,
		wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT9"));
	wxFont lbDaTPFont(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	lbDaTP->SetFont(lbDaTPFont);
	GridSizer6->Add(
		lbDaTP, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	Panel13->SetSizer(GridSizer6);
	GridSizer6->Fit(Panel13);
	GridSizer6->SetSizeHints(Panel13);
	FlexGridSizer9->Add(
		Panel13, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	plotDaTP =
		new mpWindow(Panel2, ID_CUSTOM5, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer9->Add(
		plotDaTP, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 3);
	Panel14 = new wxPanel(
		Panel2, ID_PANEL14, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL14"));
	Panel14->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer11 = new wxGridSizer(0, 1, 0, 0);
	lbDaTN = new wxStaticText(
		Panel14, ID_STATICTEXT11, _("True negatives:"), wxDefaultPosition,
		wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT11"));
	wxFont lbDaTNFont(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	lbDaTN->SetFont(lbDaTNFont);
	GridSizer11->Add(
		lbDaTN, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	Panel14->SetSizer(GridSizer11);
	GridSizer11->Fit(Panel14);
	GridSizer11->SetSizeHints(Panel14);
	FlexGridSizer9->Add(
		Panel14, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	plotDaTN =
		new mpWindow(Panel2, ID_CUSTOM6, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer9->Add(
		plotDaTN, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 5);
	Panel15 = new wxPanel(
		Panel2, ID_PANEL15, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL15"));
	Panel15->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer12 = new wxGridSizer(0, 1, 0, 0);
	StaticText6 = new wxStaticText(
		Panel15, ID_STATICTEXT12, _("False positives:"), wxDefaultPosition,
		wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT12"));
	wxFont StaticText6Font(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	StaticText6->SetFont(StaticText6Font);
	GridSizer12->Add(
		StaticText6, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	Panel15->SetSizer(GridSizer12);
	GridSizer12->Fit(Panel15);
	GridSizer12->SetSizeHints(Panel15);
	FlexGridSizer9->Add(
		Panel15, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	plotDaFP =
		new mpWindow(Panel2, ID_CUSTOM11, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer9->Add(
		plotDaFP, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 5);
	Panel16 = new wxPanel(
		Panel2, ID_PANEL16, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL16"));
	Panel16->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer13 = new wxGridSizer(0, 1, 0, 0);
	StaticText7 = new wxStaticText(
		Panel16, ID_STATICTEXT13, _("False negatives:"), wxDefaultPosition,
		wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT13"));
	wxFont StaticText7Font(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	StaticText7->SetFont(StaticText7Font);
	GridSizer13->Add(
		StaticText7, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	Panel16->SetSizer(GridSizer13);
	GridSizer13->Fit(Panel16);
	GridSizer13->SetSizeHints(Panel16);
	FlexGridSizer9->Add(
		Panel16, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	plotDaFN =
		new mpWindow(Panel2, ID_CUSTOM12, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer9->Add(
		plotDaFN, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 5);
	Panel2->SetSizer(FlexGridSizer9);
	FlexGridSizer9->Fit(Panel2);
	FlexGridSizer9->SetSizeHints(Panel2);
	Panel11 = new wxPanel(
		Notebook1, ID_PANEL12, wxDefaultPosition, wxDefaultSize,
		wxTAB_TRAVERSAL, _T("ID_PANEL12"));
	FlexGridSizer2 = new wxFlexGridSizer(4, 1, 0, 0);
	FlexGridSizer2->AddGrowableCol(0);
	FlexGridSizer2->AddGrowableRow(1);
	Panel12 = new wxPanel(
		Panel11, ID_PANEL13, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL13"));
	Panel12->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer10 = new wxGridSizer(0, 1, 0, 0);
	StaticText1 = new wxStaticText(
		Panel12, ID_STATICTEXT8, _("Computation time (ms)"), wxDefaultPosition,
		wxDefaultSize, wxALIGN_CENTRE, _T("ID_STATICTEXT8"));
	wxFont StaticText1Font(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	StaticText1->SetFont(StaticText1Font);
	GridSizer10->Add(
		StaticText1, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	Panel12->SetSizer(GridSizer10);
	GridSizer10->Fit(Panel12);
	GridSizer10->SetSizeHints(Panel12);
	FlexGridSizer2->Add(
		Panel12, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 1);
	plotStatTime =
		new mpWindow(Panel11, ID_CUSTOM10, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer2->Add(
		plotStatTime, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 3);
	Panel17 = new wxPanel(
		Panel11, ID_PANEL17, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL17"));
	Panel17->SetBackgroundColour(wxColour(255, 255, 0));
	GridSizer14 = new wxGridSizer(0, 1, 0, 0);
	StaticText5 = new wxStaticText(
		Panel17, ID_STATICTEXT14, _("JCBB iterations (if used)"),
		wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE,
		_T("ID_STATICTEXT14"));
	wxFont StaticText5Font(
		-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
	StaticText5->SetFont(StaticText5Font);
	GridSizer14->Add(
		StaticText5, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	Panel17->SetSizer(GridSizer14);
	GridSizer14->Fit(Panel17);
	GridSizer14->SetSizeHints(Panel17);
	FlexGridSizer2->Add(
		Panel17, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_BOTTOM, 0);
	plotDaJCBB =
		new mpWindow(Panel11, ID_CUSTOM13, wxDefaultPosition, wxDefaultSize, 0);
	FlexGridSizer2->Add(
		plotDaJCBB, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_BOTTOM, 3);
	Panel11->SetSizer(FlexGridSizer2);
	FlexGridSizer2->Fit(Panel11);
	FlexGridSizer2->SetSizeHints(Panel11);
	Notebook1->AddPage(Panel1, _("Errors"), true);
	Notebook1->AddPage(Panel2, _("Data assoc."), false);
	Notebook1->AddPage(Panel11, _("Stats"), false);
	FlexGridSizer7->Add(
		Notebook1, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	FlexGridSizer3->Add(
		FlexGridSizer7, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	FlexGridSizer1->Add(
		FlexGridSizer3, 1, wxALL | wxEXPAND | wxALIGN_LEFT | wxALIGN_TOP, 0);
	SetSizer(FlexGridSizer1);
	MenuBar1 = new wxMenuBar();
	Menu1 = new wxMenu();
	MenuItem3 = new wxMenuItem(
		Menu1, ID_MENUITEM1, _("Reset simulation\tCtrl+F2"), wxEmptyString,
		wxITEM_NORMAL);
	Menu1->Append(MenuItem3);
	Menu1->AppendSeparator();
	mnuOneStep = new wxMenuItem(
		Menu1, ID_MENUITEM2, _("Run one step\tF11"), wxEmptyString,
		wxITEM_NORMAL);
	Menu1->Append(mnuOneStep);
	mnuRun = new wxMenuItem(
		Menu1, ID_MENUITEM3, _("Run (interactive)...\tF5"), wxEmptyString,
		wxITEM_NORMAL);
	Menu1->Append(mnuRun);
	mnuStop = new wxMenuItem(
		Menu1, ID_MENUITEM6, _("Stop\tF2"), wxEmptyString, wxITEM_NORMAL);
	Menu1->Append(mnuStop);
	mnuRunBatch = new wxMenuItem(
		Menu1, ID_MENUITEM4, _("Run (non-interactive)...\tCtrl-F5"),
		wxEmptyString, wxITEM_NORMAL);
	Menu1->Append(mnuRunBatch);
	Menu1->AppendSeparator();
	MenuItem7 = new wxMenuItem(
		Menu1, ID_MENUITEM5, _("Parameters..."), wxEmptyString, wxITEM_NORMAL);
	Menu1->Append(MenuItem7);
	Menu1->AppendSeparator();
	MenuItem1 = new wxMenuItem(
		Menu1, idMenuQuit, _("Quit\tAlt-F4"), _("Quit the application"),
		wxITEM_NORMAL);
	Menu1->Append(MenuItem1);
	MenuBar1->Append(Menu1, _("&Simulation"));
	Menu3 = new wxMenu();
	MenuItem5 = new wxMenuItem(
		Menu3, ID_MENUITEM8, _("Save filter state..."), wxEmptyString,
		wxITEM_NORMAL);
	Menu3->Append(MenuItem5);
	mnuSaveLastDA = new wxMenuItem(
		Menu3, ID_MENUITEM11, _("Save last data association state..."),
		wxEmptyString, wxITEM_NORMAL);
	Menu3->Append(mnuSaveLastDA);
	Menu3->AppendSeparator();
	mnuItemSaveRawlog = new wxMenuItem(
		Menu3, ID_MENUITEM_SAVE_RAWLOG, _("Enable save rawlog..."),
		wxEmptyString, wxITEM_CHECK);
	Menu3->Append(mnuItemSaveRawlog);
	Menu3->AppendSeparator();
	MenuItem4 = new wxMenu();
	MenuItem6 = new wxMenuItem(
		MenuItem4, ID_MENUITEM9, _("View stats"), wxEmptyString, wxITEM_NORMAL);
	MenuItem4->Append(MenuItem6);
	MenuItem8 = new wxMenuItem(
		MenuItem4, ID_MENUITEM10, _("Reset stats"), wxEmptyString,
		wxITEM_NORMAL);
	MenuItem4->Append(MenuItem8);
	Menu3->Append(ID_MENUITEM7, _("Profiler"), MenuItem4, wxEmptyString);
	MenuBar1->Append(Menu3, _("&Tools"));
	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[1] = {-1};
	int __wxStatusBarStyles_1[1] = {wxSB_NORMAL};
	StatusBar1->SetFieldsCount(1, __wxStatusBarWidths_1);
	StatusBar1->SetStatusStyles(1, __wxStatusBarStyles_1);
	SetStatusBar(StatusBar1);
	ToolBar1 = new wxToolBar(
		this, ID_TOOLBAR1, wxDefaultPosition, wxDefaultSize,
		wxTB_FLAT | wxTB_HORIZONTAL | wxTB_TEXT | wxNO_BORDER,
		_T("ID_TOOLBAR1"));
	ToolBarItem1 = ToolBar1->AddTool(
		ID_TOOLBARITEM1, _("Reset"),
		wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("ICON_RESET")), wxART_TOOLBAR),
		wxNullBitmap, wxITEM_NORMAL, _("Reset the simulation"),
		_("Reset the simulation"));
	ToolBar1->AddSeparator();
	ToolBarItem2 = ToolBar1->AddTool(
		ID_TOOLBARITEM2, _("One step"),
		wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("ICON_STEP")), wxART_TOOLBAR),
		wxNullBitmap, wxITEM_NORMAL, _("Run one step"), _("Run one step"));
	ToolBarItem3 = ToolBar1->AddTool(
		ID_BTNRUN, _("Run..."),
		wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("ICON_PLAY")), wxART_TOOLBAR),
		wxNullBitmap, wxITEM_CHECK, _("Continuous run with animations..."),
		_("Continuous run with animations..."));
	ToolBarItem4 = ToolBar1->AddTool(
		ID_BTNSTOP, _("Stop"),
		wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("ICON_STOP")), wxART_TOOLBAR),
		wxNullBitmap, wxITEM_NORMAL, _("Stop continuous run"),
		_("Stop continuous run"));
	ToolBarItem5 = ToolBar1->AddTool(
		ID_TOOLBARITEM4, _("Run (batch)"),
		wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("ICON_BATCH")), wxART_TOOLBAR),
		wxNullBitmap, wxITEM_NORMAL,
		_("Run the whole simulation without animate"),
		_("Run the whole simulation without animate"));
	ToolBar1->AddSeparator();
	ToolBarItem6 = ToolBar1->AddTool(
		ID_TOOLBARITEM3, _("Config..."),
		wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("ICON_CONFIG")), wxART_TOOLBAR),
		wxNullBitmap, wxITEM_NORMAL, _("Change simulation parameters..."),
		_("Change simulation parameters..."));
	ToolBar1->AddSeparator();
	ToolBarItem7 = ToolBar1->AddTool(
		ID_TOOLBARITEM6, _("About..."),
		wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("ICON_HELP")), wxART_TOOLBAR),
		wxNullBitmap, wxITEM_NORMAL, _("About this program"),
		_("About this program"));
	ToolBarItem8 = ToolBar1->AddTool(
		ID_TOOLBARITEM7, _("Quit"),
		wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("ICON_EXIT")), wxART_TOOLBAR),
		wxNullBitmap, wxITEM_NORMAL, _("Exit the application"),
		_("Exit the application"));
	ToolBar1->Realize();
	SetToolBar(ToolBar1);
	timSimul.SetOwner(this, ID_TIMER1);
	FlexGridSizer1->Fit(this);
	FlexGridSizer1->SetSizeHints(this);

	Bind(wxEVT_MENU, &slamdemoFrame::OnbtnResetClicked, this, ID_MENUITEM1);
	Bind(wxEVT_MENU, &slamdemoFrame::OnbtnOneStepClicked, this, ID_MENUITEM2);
	Bind(wxEVT_MENU, &slamdemoFrame::OnbtnRunClicked, this, ID_MENUITEM3);
	Bind(wxEVT_MENU, &slamdemoFrame::OnbtnStopClicked, this, ID_MENUITEM6);
	Bind(wxEVT_MENU, &slamdemoFrame::OnbtnRunBatchClicked, this, ID_MENUITEM4);
	Bind(wxEVT_MENU, &slamdemoFrame::OnConfigClicked, this, ID_MENUITEM5);
	Bind(wxEVT_MENU, &slamdemoFrame::OnQuit, this, idMenuQuit);
	Bind(wxEVT_MENU, &slamdemoFrame::OnMenuSaveFilterState, this, ID_MENUITEM8);
	Bind(
		wxEVT_MENU, &slamdemoFrame::OnmnuSaveLastDASelected, this,
		ID_MENUITEM11);
	Bind(
		wxEVT_MENU, &slamdemoFrame::OnmnuItemSaveRawlogSelected, this,
		ID_MENUITEM_SAVE_RAWLOG);
	Bind(
		wxEVT_MENU, &slamdemoFrame::OnMenuProfilerViewStats, this,
		ID_MENUITEM9);
	Bind(wxEVT_MENU, &slamdemoFrame::OnMenuProfilerReset, this, ID_MENUITEM10);
	Bind(wxEVT_MENU, &slamdemoFrame::OnAbout, this, idMenuAbout);
	Bind(wxEVT_TOOL, &slamdemoFrame::OnbtnResetClicked, this, ID_TOOLBARITEM1);
	Bind(
		wxEVT_TOOL, &slamdemoFrame::OnbtnOneStepClicked, this, ID_TOOLBARITEM2);
	Bind(wxEVT_TOOL, &slamdemoFrame::OnbtnRunClicked, this, ID_BTNRUN);
	Bind(wxEVT_TOOL, &slamdemoFrame::OnbtnStopClicked, this, ID_BTNSTOP);
	Bind(
		wxEVT_TOOL, &slamdemoFrame::OnbtnRunBatchClicked, this,
		ID_TOOLBARITEM4);
	Bind(wxEVT_TOOL, &slamdemoFrame::OnConfigClicked, this, ID_TOOLBARITEM3);
	Bind(wxEVT_TOOL, &slamdemoFrame::OnAbout, this, ID_TOOLBARITEM6);
	Bind(wxEVT_TOOL, &slamdemoFrame::OnQuit, this, ID_TOOLBARITEM7);
	Bind(wxEVT_TIMER, &slamdemoFrame::OntimSimulTrigger, this, ID_TIMER1);
	//*)

	gridDA->SetSelectionMode(wxGrid::wxGridSelectCells);

	// buttons ------------
	this->ToolBar1->EnableTool(ID_BTNSTOP, false);
	mnuStop->Enable(false);

// Init graphs:
#define INIT_PLOT_LABELS(_PL, _LBX, _LBY)                                      \
	_PL->AddLayer(new mpScaleX(wxT(_LBX)));                                    \
	_PL->AddLayer(new mpScaleY(wxT(_LBY)));                                    \
	_PL->LockAspect(true);                                                     \
	_PL->Fit(-10, 10, -10, 10);

#define INIT_PLOT(_PL) INIT_PLOT_LABELS(_PL, "x", "y")
#define INIT_PLOT_TIME(_PL) INIT_PLOT_LABELS(_PL, "time", "")

	INIT_PLOT(plotGT)
	INIT_PLOT(plotMap)
	INIT_PLOT_TIME(plotErrorX)
	INIT_PLOT_TIME(plotErrorY)
	INIT_PLOT_TIME(plotErrorPhi)
	INIT_PLOT_LABELS(plotIndivCompat, "Bearing", "Range")
	INIT_PLOT(plotObs)

	INIT_PLOT_TIME(plotStatTime)

	INIT_PLOT_TIME(plotDaFN)
	INIT_PLOT_TIME(plotDaFP)
	INIT_PLOT_TIME(plotDaTN)
	INIT_PLOT_TIME(plotDaTP)
	INIT_PLOT_TIME(plotDaJCBB)

	vector<float> robot_shape_xs(3);
	vector<float> robot_shape_ys(3);
	robot_shape_xs[0] = 0;
	robot_shape_ys[0] = -0.10f;
	robot_shape_xs[1] = 0;
	robot_shape_ys[1] = 0.10f;
	robot_shape_xs[2] = 0.2f;
	robot_shape_ys[2] = 0;

	// GT plot ------------
	m_lyGTMap = new mpFXYVector();
	m_lyGTMap->SetPen(wxPen(wxColour(0, 0, 255), 5));
	m_lyGTMap->SetContinuity(false);
	plotGT->AddLayer(m_lyGTMap);

	m_lyGTRobot = new mpPolygon();
	m_lyGTRobot->SetPen(wxPen(wxColour(255, 0, 0), 2));
	m_lyGTRobot->SetContinuity(true);
	m_lyGTRobot->setPoints(robot_shape_xs, robot_shape_ys);
	m_lyGTRobot->SetCoordinateBase(0, 0, 0);
	plotGT->AddLayer(m_lyGTRobot);

	m_lyGTvisibleRange = new mpPolygon();
	m_lyGTvisibleRange->SetPen(wxPen(wxColour(0, 0, 0), 1));
	m_lyGTvisibleRange->SetContinuity(true);
	m_lyGTvisibleRange->SetCoordinateBase(0, 0, 0);
	plotGT->AddLayer(m_lyGTvisibleRange);

	plotGT->LockAspect();

	// Map plot ------------
	m_lyMapRobot = new mpPolygon();
	m_lyMapRobot->SetPen(wxPen(wxColour(255, 0, 0), 2));
	m_lyMapRobot->SetContinuity(true);
	m_lyMapRobot->setPoints(robot_shape_xs, robot_shape_ys);
	m_lyMapRobot->SetCoordinateBase(0, 0, 0);
	plotMap->AddLayer(m_lyMapRobot);
	plotMap->LockAspect();

	// Observations plot ------------
	m_lyObsRobot = new mpPolygon();
	m_lyObsRobot->SetPen(wxPen(wxColour(255, 0, 0), 2));
	m_lyObsRobot->SetContinuity(true);
	m_lyObsRobot->setPoints(robot_shape_xs, robot_shape_ys);
	m_lyObsRobot->SetCoordinateBase(0, 0, 0);
	plotObs->AddLayer(m_lyObsRobot);

	m_lyObsvisibleRange = new mpPolygon();
	m_lyObsvisibleRange->SetPen(wxPen(wxColour(0, 0, 0), 1));
	m_lyObsvisibleRange->SetContinuity(true);
	m_lyObsvisibleRange->SetCoordinateBase(0, 0, 0);
	plotObs->AddLayer(m_lyObsvisibleRange);

	plotObs->LockAspect();

	// IC plot ------------
	m_lyICvisibleRange = new mpPolygon();
	m_lyICvisibleRange->SetPen(wxPen(wxColour(0, 0, 0), 1, wxLONG_DASH));
	m_lyICvisibleRange->SetContinuity(true);
	m_lyICvisibleRange->SetCoordinateBase(0, 0, 0);
	plotIndivCompat->AddLayer(m_lyICvisibleRange);

	plotIndivCompat->LockAspect(false);

	// X ERROR plot ------------
	m_lyERRX_err = new mpFXYVector();
	m_lyERRX_err->SetPen(wxPen(wxColour(0, 0, 0), 4));
	m_lyERRX_err->SetContinuity(true);
	plotErrorX->AddLayer(m_lyERRX_err);

	m_lyERRX_boundUp = new mpFXYVector();
	m_lyERRX_boundUp->SetPen(wxPen(wxColour(0, 255, 0), 2));
	m_lyERRX_boundUp->SetContinuity(true);
	plotErrorX->AddLayer(m_lyERRX_boundUp);

	m_lyERRX_boundDown = new mpFXYVector();
	m_lyERRX_boundDown->SetPen(wxPen(wxColour(0, 255, 0), 2));
	m_lyERRX_boundDown->SetContinuity(true);
	plotErrorX->AddLayer(m_lyERRX_boundDown);

	plotErrorX->LockAspect(false);

	// Y ERROR plot ------------
	m_lyERRY_err = new mpFXYVector();
	m_lyERRY_err->SetPen(wxPen(wxColour(0, 0, 0), 4));
	m_lyERRY_err->SetContinuity(true);
	plotErrorY->AddLayer(m_lyERRY_err);

	m_lyERRY_boundUp = new mpFXYVector();
	m_lyERRY_boundUp->SetPen(wxPen(wxColour(0, 255, 0), 2));
	m_lyERRY_boundUp->SetContinuity(true);
	plotErrorY->AddLayer(m_lyERRY_boundUp);

	m_lyERRY_boundDown = new mpFXYVector();
	m_lyERRY_boundDown->SetPen(wxPen(wxColour(0, 255, 0), 2));
	m_lyERRY_boundDown->SetContinuity(true);
	plotErrorY->AddLayer(m_lyERRY_boundDown);

	plotErrorY->LockAspect(false);

	// Phi ERROR plot ------------
	m_lyERRPHI_err = new mpFXYVector();
	m_lyERRPHI_err->SetPen(wxPen(wxColour(0, 0, 0), 4));
	m_lyERRPHI_err->SetContinuity(true);
	plotErrorPhi->AddLayer(m_lyERRPHI_err);

	m_lyERRPHI_boundUp = new mpFXYVector();
	m_lyERRPHI_boundUp->SetPen(wxPen(wxColour(0, 255, 0), 2));
	m_lyERRPHI_boundUp->SetContinuity(true);
	plotErrorPhi->AddLayer(m_lyERRPHI_boundUp);

	m_lyERRPHI_boundDown = new mpFXYVector();
	m_lyERRPHI_boundDown->SetPen(wxPen(wxColour(0, 255, 0), 2));
	m_lyERRPHI_boundDown->SetContinuity(true);
	plotErrorPhi->AddLayer(m_lyERRPHI_boundDown);

	plotErrorPhi->LockAspect(false);

	// Stats Time plot ------------
	m_lyStatTimes = new mpFXYVector();
	m_lyStatTimes->SetPen(wxPen(wxColour(0, 0, 255), 3));
	m_lyStatTimes->SetContinuity(true);
	plotStatTime->AddLayer(m_lyStatTimes);

	plotStatTime->LockAspect(false);

// DA Stats plots ------------
#define INIT_DA_PLOT(CODE)                                                     \
	m_lyDa##CODE = new mpFXYVector();                                          \
	m_lyDa##CODE->SetPen(wxPen(wxColour(0, 0, 0), 5));                         \
	m_lyDa##CODE->SetContinuity(false);                                        \
	plotDa##CODE->AddLayer(m_lyDa##CODE);                                      \
	plotDa##CODE->LockAspect(false);

	INIT_DA_PLOT(FP);
	INIT_DA_PLOT(FN);
	INIT_DA_PLOT(TP);
	INIT_DA_PLOT(TN);

	INIT_DA_PLOT(JCBB);

	// Set some default params:
	m_SLAM.options.std_sensor_range = 0.03f;
	m_SLAM.options.std_sensor_yaw = DEG2RAD(0.5f);

	options.sensor_max_range = 5;
	options.sensor_min_range = 0.50;
	options.sensor_fov = 140.0_deg;

	options.sensorDistingishesLandmarks = false;

	m_SLAM.KF_options.method = kfEKFNaive;
	//	m_SLAM.KF_options.method = kfEKFAlaDavison;
	//	m_SLAM.KF_options.method = kfIKFFull;
	//	m_SLAM.KF_options.IKF_iterations = 4;

	m_SLAM.KF_options.enable_profiler = true;
	// m_SLAM.KF_options.debug_verify_analytic_jacobians = true;

	// Init everything:
	resetSimulator(options.map_generator);
	updateAllGraphs();

	Maximize();
}

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

/*---------------------------------------------------------------
						Quit
  ---------------------------------------------------------------*/
void slamdemoFrame::OnQuit(wxCommandEvent& event)
{
	// Stop simulation
	wxCommandEvent dumm;
	OnbtnStopClicked(dumm);

	Close();
}

/*---------------------------------------------------------------
						About
  ---------------------------------------------------------------*/
void slamdemoFrame::OnAbout(wxCommandEvent&)
{
	mrpt::gui::show_mrpt_about_box_wxWidgets(
		this, "2d-slam-demo",
		"Invoke with --help to see command line arguments.\n"
		"This program was inspired by a MATLAB version written by J. "
		"Neira and J. Tardos.\n");
}

/*---------------------------------------------------------------
						Reset btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnResetClicked(wxCommandEvent& event)
{
	resetSimulator(options.map_generator);
	updateAllGraphs();
}

/*---------------------------------------------------------------
						1 step btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnOneStepClicked(wxCommandEvent& event)
{
	CTicTac tictac;
	tictac.Tic();
	executeOneStep();
	const double T = tictac.Tac();
	StatusBar1->SetStatusText(wxString::Format(
		_("Step %u done in %.03fms"), (unsigned)m_historicData.size(),
		1e3 * T));
	updateAllGraphs();
}

/*---------------------------------------------------------------
						Run btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnRunClicked(wxCommandEvent& event)
{
	this->ToolBar1->EnableTool(ID_BTNSTOP, true);
	mnuStop->Enable(true);

	this->ToolBar1->ToggleTool(ID_BTNRUN, true);
	this->ToolBar1->EnableTool(ID_BTNRUN, false);
	mnuRun->Enable(false);

	// Prepare next step:
	timSimul.Start(10, true);
}

/*---------------------------------------------------------------
						Stop btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnStopClicked(wxCommandEvent& event)
{
	this->ToolBar1->EnableTool(ID_BTNSTOP, false);
	mnuStop->Enable(false);

	this->ToolBar1->ToggleTool(ID_BTNRUN, false);
	this->ToolBar1->EnableTool(ID_BTNRUN, true);
	mnuRun->Enable(true);
}

/*---------------------------------------------------------------
						batch run btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnRunBatchClicked(wxCommandEvent& event)
{
	static CTicTac tictac;

	wxBusyCursor info;
	wxTheApp->Yield();	// Let the app. process messages

	tictac.Tic();
	const size_t N =
		(options.path_square_len / options.robot_step_length) * 4 + 50;
	for (size_t i = 0; i < N; i++)
		executeOneStep();

	const double T = tictac.Tac();

	updateAllGraphs();

	StatusBar1->SetStatusText(
		wxString::Format(_("%u steps done in %.03f secs"), (unsigned)N, T));
}

/*---------------------------------------------------------------
						Reset simulator
  ---------------------------------------------------------------*/
void slamdemoFrame::resetSimulator(const std::string& map_type)
{
	if (options.random_seed >= 0)
		getRandomGenerator().randomize(options.random_seed);
	else
		getRandomGenerator().randomize();

	m_SLAM.reset();
	m_historicData.clear();
	m_estimatedIDX2realIDX.clear();
	m_realIDX_already_mapped.clear();

	m_lastObservation.sensedData.clear();
	m_lastObservation_GT_indices.clear();

	m_GT_pose = CPose2D(0, 0, 0);

	// The map:
	// -----------------
	m_GT_map.clear();

	if (map_type == "1")
	{
		// Default map:
		size_t ID = 0;
		CLandmark newLM;
		newLM.pose_mean.z = 0;

		for (int i = 0; i <= mrpt::round((options.path_square_len) / 2.0); i++)
		{
			// Bottom & top  corridors:
			newLM.pose_mean.x = 1 + 2 * i;
			newLM.pose_mean.y = -0.35;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.y += options.path_square_len;  // Top corridor
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x = 1 + 2 * i;
			newLM.pose_mean.y = -0.6;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.y += options.path_square_len;  // Top corridor
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x = 2 * (1 + i);
			newLM.pose_mean.y = 0.35;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x = 2 * i;
			newLM.pose_mean.y += options.path_square_len;  // Top corridor
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x = 2 * (1 + i);
			newLM.pose_mean.y = 0.6;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x = 2 * i;
			newLM.pose_mean.y += options.path_square_len;  // Top corridor
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			// Left & right corridor:
			newLM.pose_mean.x = -0.35;
			newLM.pose_mean.y = 2 * i;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x += options.path_square_len;  // Right corridor
			// newLM.pose_mean.y -=1;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x = -0.6;
			newLM.pose_mean.y = 2 * i;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x += options.path_square_len;  // Right corridor
			// newLM.pose_mean.y -=1;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x = 0.35;
			newLM.pose_mean.y = 2 * i - 1;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x += options.path_square_len;  // Right  corridor
			newLM.pose_mean.y += 1 + 1;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x = 0.6;
			newLM.pose_mean.y = 2 * i - 1;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);

			newLM.pose_mean.x += options.path_square_len;  // Right  corridor
			newLM.pose_mean.y += 1 + 1;
			newLM.ID = ID++;
			m_GT_map.landmarks.push_back(newLM);
		}
	}
	else if (map_type == "2")
	{
		const double extra_width = 5;
		// Random map:
		for (size_t i = 0; i < options.randomMap_nLMs; i++)
		{
			CLandmark newLM;
			newLM.pose_mean.x = getRandomGenerator().drawUniform(
				-extra_width, options.path_square_len + extra_width);
			newLM.pose_mean.y = getRandomGenerator().drawUniform(
				-extra_width, options.path_square_len + extra_width);
			newLM.pose_mean.z = 0;
			newLM.ID = i;
			m_GT_map.landmarks.push_back(newLM);
		}
	}

	// Refresh all:
	updateAllGraphs(true);
}

/*---------------------------------------------------------------
						updateAllGraphs
  ---------------------------------------------------------------*/
void slamdemoFrame::updateAllGraphs(bool alsoGTMap)
{
	// The area visible by the sensor (used in multiple graphs):
	const size_t AREA_SEGS = 100;
	vector<float> xs_area(AREA_SEGS + 1);
	vector<float> ys_area(AREA_SEGS + 1);
	for (size_t i = 0; i < AREA_SEGS; i++)
	{
		double a = options.sensor_fov * (-0.5 + double(i) / (AREA_SEGS - 1));
		xs_area[i] = options.sensorOnTheRobot.x() +
			options.sensor_max_range * cos(a + options.sensorOnTheRobot.phi());
		ys_area[i] = options.sensorOnTheRobot.y() +
			options.sensor_max_range * sin(a + options.sensorOnTheRobot.phi());
	}
	xs_area[AREA_SEGS] = options.sensorOnTheRobot.x();
	ys_area[AREA_SEGS] = options.sensorOnTheRobot.y();

	// GT Map ----------------------
	if (alsoGTMap)
	{
		vector<float> xs, ys;
		for (auto& landmark : m_GT_map.landmarks)
		{
			xs.push_back(landmark.pose_mean.x);
			ys.push_back(landmark.pose_mean.y);
		}

		lbGT->SetLabel(format(
						   "Ground truth (%u landmarks",
						   (unsigned)m_GT_map.landmarks.size())
						   .c_str());

		m_lyGTMap->Clear();
		m_lyGTMap->SetData(xs, ys);

		plotGT->Fit();
	}

	m_lyGTvisibleRange->setPoints(xs_area, ys_area);

	m_lyGTRobot->SetCoordinateBase(
		m_GT_pose.x(), m_GT_pose.y(), m_GT_pose.phi());
	m_lyGTvisibleRange->SetCoordinateBase(
		m_GT_pose.x(), m_GT_pose.y(), m_GT_pose.phi());
	plotGT->Refresh();

	// Observation ----------------------
	m_lyObsvisibleRange->setPoints(xs_area, ys_area);

	lbObs->SetLabel(format(
						"Observation (%u landmarks",
						(unsigned)m_lastObservation.sensedData.size())
						.c_str());

	for (auto& m_lyObsLM : m_lyObsLMs)
		plotObs->DelLayer(m_lyObsLM, true);
	m_lyObsLMs.clear();

	CMatrixDouble22 NOISE;
	NOISE(0, 0) = square(m_SLAM.options.std_sensor_range);
	NOISE(1, 1) = square(m_SLAM.options.std_sensor_yaw);

	// Create an ellipse for each observed landmark:
	for (auto& i : m_lastObservation.sensedData)
	{
		mpCovarianceEllipse* cov = new mpCovarianceEllipse();
		cov->SetPen(wxPen(wxColour(255, 0, 0), 2));
		if (i.landmarkID != INVALID_LANDMARK_ID)
			cov->SetName(wxString::Format(_("#%u"), (unsigned)i.landmarkID));
		else
			cov->SetName(_("?"));

		// Compute mean & cov:
		const double hr = i.range;
		const double ha = i.yaw;
		const double cphi_0sa = cos(options.sensorOnTheRobot.phi() + ha);
		const double sphi_0sa = sin(options.sensorOnTheRobot.phi() + ha);

		const CPoint2D lm_xy =
			options.sensorOnTheRobot + CPoint2D(hr * cos(ha), hr * sin(ha));

		// Jacobian wrt hn:
		CMatrixDouble22 dyn_dhn;
		dyn_dhn(0, 0) = cphi_0sa;
		dyn_dhn(0, 1) = -hr * sphi_0sa;
		dyn_dhn(1, 0) = sphi_0sa;
		dyn_dhn(1, 1) = hr * cphi_0sa;

		const CMatrixDouble22 COV = mrpt::math::multiply_HCHt(dyn_dhn, NOISE);

		cov->SetQuantiles(3);
		cov->SetCoordinateBase(lm_xy.x(), lm_xy.y());
		cov->SetCovarianceMatrix(COV(0, 0), COV(0, 1), COV(1, 1));

		plotObs->AddLayer(cov);
		m_lyObsLMs.push_back(cov);
	}

	plotObs->Fit();
	plotObs->Refresh();

	// Map estimated by the SLAM filter ----------------------
	{
		CPosePDFGaussian estRobotPose;
		vector<TPoint2D> LMs;
		map<unsigned int, CLandmark::TLandmarkID> landmarkIDs;

		CVectorDouble Xkk;	// Full mean & cov
		CMatrixDouble Pkk;

		m_SLAM.getCurrentState(estRobotPose, LMs, landmarkIDs, Xkk, Pkk);

		lbMap->SetLabel(wxString::Format(
			_("Estimated map (%u landmarks)"), (unsigned)LMs.size()));

		// mean robot pose:
		m_lyMapRobot->SetCoordinateBase(
			estRobotPose.mean.x(), estRobotPose.mean.y(),
			estRobotPose.mean.phi());

		// Delete old covs:
		for (auto& m_lyMapEllipse : m_lyMapEllipses)
			plotMap->DelLayer(m_lyMapEllipse, true);
		m_lyMapEllipses.clear();

		// Robot ellipse:
		{
			mpCovarianceEllipse* cov = new mpCovarianceEllipse();
			cov->SetPen(wxPen(wxColour(255, 0, 0), 2));
			cov->SetName(_("Robot"));
			cov->SetQuantiles(3);

			cov->SetCoordinateBase(
				estRobotPose.mean.x(), estRobotPose.mean.y(),
				estRobotPose.mean.phi());
			cov->SetCovarianceMatrix(
				estRobotPose.cov(0, 0), estRobotPose.cov(0, 1),
				estRobotPose.cov(1, 1));

			plotMap->AddLayer(cov);
			m_lyMapEllipses.push_back(cov);
		}

		// Landmarks:
		for (size_t i = 0; i < LMs.size(); i++)
		{
			const size_t idx_in_real_map =
				m_estimatedIDX2realIDX.find(i)->second;

			mpCovarianceEllipse* cov = new mpCovarianceEllipse();
			cov->SetPen(wxPen(wxColour(0, 0, 255), 2));

			if (options.show_map_real_correspondences)
				cov->SetName(wxString::Format(
					_("#%u->%u"), (unsigned)i, (unsigned)idx_in_real_map));
			else
				cov->SetName(wxString::Format(_("#%u"), (unsigned)i));

			cov->SetQuantiles(3);

			cov->SetCoordinateBase(LMs[i].x, LMs[i].y);
			const size_t idx = 3 + 2 * i;
			cov->SetCovarianceMatrix(
				Pkk(idx + 0, idx + 0), Pkk(idx + 0, idx + 1),
				Pkk(idx + 1, idx + 1));

			plotMap->AddLayer(cov);
			m_lyMapEllipses.push_back(cov);
		}
	}

	plotMap->Fit();
	plotMap->Refresh();

	// Error plots -----------
	if (!m_historicData.empty())
	{
		const size_t N = m_historicData.size();

		// ERRORS IN X --------------
		if (m_lyERRX_err->GetDataLength() >= N)
		{
			m_lyERRX_err->Clear();
			m_lyERRX_boundUp->Clear();
			m_lyERRX_boundDown->Clear();
		}
		for (size_t i = m_lyERRX_err->GetDataLength(); i < N; i++)
		{
			THistoric& h = m_historicData[i];
			m_lyERRX_err->AppendDataPoint(
				i, h.GT_robot_pose.x() - h.estimate_robot_pose.mean.x());
			const double std_x = sqrt(h.estimate_robot_pose.cov(0, 0));
			m_lyERRX_boundUp->AppendDataPoint(i, 3 * std_x);
			m_lyERRX_boundDown->AppendDataPoint(i, -3 * std_x);
		}

		// ERRORS IN Y --------------
		if (m_lyERRY_err->GetDataLength() >= N)
		{
			m_lyERRY_err->Clear();
			m_lyERRY_boundUp->Clear();
			m_lyERRY_boundDown->Clear();
		}
		for (size_t i = m_lyERRY_err->GetDataLength(); i < N; i++)
		{
			THistoric& h = m_historicData[i];
			m_lyERRY_err->AppendDataPoint(
				i, h.GT_robot_pose.y() - h.estimate_robot_pose.mean.y());
			const double std_y = sqrt(h.estimate_robot_pose.cov(1, 1));
			m_lyERRY_boundUp->AppendDataPoint(i, 3 * std_y);
			m_lyERRY_boundDown->AppendDataPoint(i, -3 * std_y);
		}

		// ERRORS IN PHI --------------
		if (m_lyERRPHI_err->GetDataLength() >= N)
		{
			m_lyERRPHI_err->Clear();
			m_lyERRPHI_boundUp->Clear();
			m_lyERRPHI_boundDown->Clear();
		}
		for (size_t i = m_lyERRPHI_err->GetDataLength(); i < N; i++)
		{
			THistoric& h = m_historicData[i];
			m_lyERRPHI_err->AppendDataPoint(
				i,
				RAD2DEG(mrpt::math::wrapToPi(
					h.GT_robot_pose.phi() - h.estimate_robot_pose.mean.phi())));
			const double std_p = sqrt(h.estimate_robot_pose.cov(2, 2));
			m_lyERRPHI_boundUp->AppendDataPoint(i, RAD2DEG(3 * std_p));
			m_lyERRPHI_boundDown->AppendDataPoint(i, RAD2DEG(-3 * std_p));
		}

		plotErrorX->Fit(
			-0.05 * N, N * 1.05, m_lyERRX_boundDown->GetMinY() * 1.05,
			m_lyERRX_boundUp->GetMaxY() * 1.05);
		plotErrorX->Refresh();
		plotErrorY->Fit(
			-0.05 * N, N * 1.05, m_lyERRY_boundDown->GetMinY() * 1.05,
			m_lyERRY_boundUp->GetMaxY() * 1.05);
		plotErrorY->Refresh();
		plotErrorPhi->Fit(
			-0.05 * N, N * 1.05, m_lyERRPHI_boundDown->GetMinY() * 1.05,
			m_lyERRPHI_boundUp->GetMaxY() * 1.05);
		plotErrorPhi->Refresh();

		// Execution times: --------------
		if (m_lyStatTimes->GetDataLength() >= N) { m_lyStatTimes->Clear(); }
		for (size_t i = m_lyStatTimes->GetDataLength(); i < N; i++)
		{
			THistoric& h = m_historicData[i];
			m_lyStatTimes->AppendDataPoint(i, 1e3 * h.run_time);
		}
		plotStatTime->Fit(
			-0.15 * N, N * 1.05, -0.10 * m_lyStatTimes->GetMaxY(),
			m_lyStatTimes->GetMaxY() * 1.05);
		plotStatTime->Refresh();
	}

	// Data association graphs ------------------------
	const CRangeBearingKFSLAM2D::TDataAssocInfo& da =
		m_SLAM.getLastDataAssociation();

	// IC graph ------------------------------------------
	{
		// Draw sensor ranges in the R-B plane:
		vector<float> xs_area_RG(5);
		vector<float> ys_area_RG(5);
		xs_area_RG[0] = -RAD2DEG(options.sensor_fov) * 0.5;
		ys_area_RG[0] = 0;
		xs_area_RG[1] = RAD2DEG(options.sensor_fov) * 0.5;
		ys_area_RG[1] = 0;
		xs_area_RG[2] = RAD2DEG(options.sensor_fov) * 0.5;
		ys_area_RG[2] = options.sensor_max_range;
		xs_area_RG[3] = -RAD2DEG(options.sensor_fov) * 0.5;
		ys_area_RG[3] = options.sensor_max_range;
		xs_area_RG[4] = xs_area_RG[0];
		ys_area_RG[4] = ys_area_RG[0];

		m_lyICvisibleRange->setPoints(xs_area_RG, ys_area_RG);

		// Delete old ellipses:
		for (auto& m_lyIC_LM : m_lyIC_LMs)
			plotIndivCompat->DelLayer(m_lyIC_LM, true);
		m_lyIC_LMs.clear();

		// Create an ellipse for each observed landmark, in
		//  the RANGE-BEARING plane:
		for (size_t i = 0; i < m_lastObservation.sensedData.size(); i++)
		{
			mpCovarianceEllipse* cov = new mpCovarianceEllipse();
			cov->SetPen(wxPen(wxColour(255, 0, 0), 2));
			if (m_lastObservation.sensedData[i].landmarkID !=
				INVALID_LANDMARK_ID)
				cov->SetName(wxString::Format(
					_("O(%u)"),
					(unsigned)m_lastObservation.sensedData[i].landmarkID));
			else
				cov->SetName(wxString::Format(_("O%u"), (unsigned)i));

			// Compute mean & cov:
			const double hr = m_lastObservation.sensedData[i].range;
			const double ha = m_lastObservation.sensedData[i].yaw;

			cov->SetQuantiles(2);
			cov->SetCoordinateBase(RAD2DEG(ha), hr);
			cov->SetCovarianceMatrix(RAD2DEGSQ * NOISE(1, 1), 0, NOISE(0, 0));

			plotIndivCompat->AddLayer(cov);
			m_lyIC_LMs.push_back(cov);
		}

		const size_t obs_size = m_SLAM.get_observation_size();

		// Create an ellipse for each PREDICTED map landmark, in
		//  the RANGE-BEARING plane:
		for (size_t i = 0; i < da.predictions_IDs.size(); i++)
		{
			mpCovarianceEllipse* cov = new mpCovarianceEllipse();
			cov->SetPen(wxPen(wxColour(0, 0, 255), 2));
			cov->SetName(
				wxString::Format(_("P%u"), (unsigned)da.predictions_IDs[i]));

			const double hr = da.Y_pred_means(i, 0);
			const double ha = da.Y_pred_means(i, 1);

			cov->SetQuantiles(2);
			cov->SetCoordinateBase(RAD2DEG(ha), hr);
			if (size_t(da.Y_pred_covs.cols()) == obs_size)
			{  // Independent predictions:
				ASSERT_(
					size_t(da.Y_pred_covs.rows()) ==
					obs_size * da.predictions_IDs.size());
				cov->SetCovarianceMatrix(
					RAD2DEGSQ * da.Y_pred_covs(obs_size * i + 1, 1),
					RAD2DEG(da.Y_pred_covs(obs_size * i + 1, 0)),
					da.Y_pred_covs(obs_size * i + 0, 0));
			}
			else
			{  // Full cov. predictions:
				ASSERT_(
					da.Y_pred_covs.isSquare() &&
					size_t(da.Y_pred_covs.cols()) ==
						obs_size * da.predictions_IDs.size());
				cov->SetCovarianceMatrix(
					RAD2DEGSQ *
						da.Y_pred_covs(obs_size * i + 1, obs_size * i + 1),
					RAD2DEG(da.Y_pred_covs(obs_size * i + 1, obs_size * i + 0)),
					da.Y_pred_covs(obs_size * i + 0, obs_size * i + 0));
			}

			plotIndivCompat->AddLayer(cov);
			m_lyIC_LMs.push_back(cov);
		}

		// Draw lines between individually compatible covs:
		for (unsigned int o = 0; o < m_lastObservation.sensedData.size(); o++)
		{
			const double hr_o = m_lastObservation.sensedData[o].range;
			const double ha_o = m_lastObservation.sensedData[o].yaw;

			for (unsigned int p = 0; p < da.predictions_IDs.size(); p++)
			{
				bool is_final_assoc =
					(da.results.associations.find(o) !=
						 da.results.associations.end() &&
					 da.results.associations.find(o)->second ==
						 da.predictions_IDs[p]);

				if (da.results.indiv_compatibility(p, o))
				{
					mpFXYVector* v = new mpFXYVector(wxEmptyString, mpALIGN_SW);
					v->SetPen(wxPen(wxColour(0, 0, 0), is_final_assoc ? 4 : 2));
					v->SetName(wxString::Format(
						_("%f"), da.results.indiv_distances(p, o)));

					const double hr = da.Y_pred_means(p, 0);
					const double ha = da.Y_pred_means(p, 1);

					vector<float> xs(2);
					vector<float> ys(2);
					xs[0] = RAD2DEG(ha);
					ys[0] = hr;
					xs[1] = RAD2DEG(ha_o);
					ys[1] = hr_o;

					v->SetData(xs, ys);
					v->SetContinuity(true);

					plotIndivCompat->AddLayer(v);
					m_lyIC_LMs.push_back(v);
				}
				// Set background if it's a final association:
				if (is_final_assoc)
				{
					gridDA->SetCellBackgroundColour(p, o, wxColor(255, 255, 0));
				}
			}
		}

		plotIndivCompat->Fit(
			-RAD2DEG(options.sensor_fov) * 0.5 - 10,
			RAD2DEG(options.sensor_fov) * 0.5 + 10, -0.10,
			options.sensor_max_range + 0.5);

		plotIndivCompat->Refresh();
	}

	// DA distances ------------------------------------------
	{
		gridDA->BeginBatch();

		// Rows: predictions; Cols: observations
		if (gridDA->GetNumberCols())
			gridDA->DeleteCols(0, gridDA->GetNumberCols());
		gridDA->AppendCols(m_lastObservation.sensedData.size());

		for (unsigned int i = 0; i < m_lastObservation.sensedData.size(); i++)
			gridDA->SetColLabelValue(i, wxString::Format(wxT("O%u"), i));

		if (gridDA->GetNumberRows())
			gridDA->DeleteRows(0, gridDA->GetNumberRows());
		gridDA->AppendRows(da.predictions_IDs.size());

		for (unsigned int i = 0; i < da.predictions_IDs.size(); i++)
			gridDA->SetRowLabelValue(
				i, wxString::Format(_("P%u"), (unsigned)da.predictions_IDs[i]));

		for (unsigned int o = 0; o < m_lastObservation.sensedData.size(); o++)
			for (unsigned int p = 0; p < da.predictions_IDs.size(); p++)
			{
				gridDA->SetCellAlignment(p, o, wxALIGN_RIGHT, wxALIGN_CENTRE);

				const double v = da.results.indiv_distances(p, o);
				if (v > 500)
					gridDA->SetCellValue(p, o, wxT("\u221E"));	// Infinity
				else if (v < -950)
					gridDA->SetCellValue(p, o, wxT("-\u221E"));	 // -Infinity
				else
					gridDA->SetCellValue(
						p, o, wxString::Format(wxT("%.02f"), v));

				// Set background if it's a final association:
				if (da.results.associations.find(o) !=
						da.results.associations.end() &&
					da.results.associations.find(o)->second ==
						da.predictions_IDs[p])
				{
					gridDA->SetCellBackgroundColour(p, o, wxColor(255, 255, 0));
				}
			}

		gridDA->EndBatch();
	}

	// DA stats ------------------------------------------
	if (!m_historicData.empty())
	{
		const size_t N = m_historicData.size();

		// DA True positives --------------
		if (m_lyDaTP->GetDataLength() >= N) m_lyDaTP->Clear();
		for (size_t i = m_lyDaTP->GetDataLength(); i < N; i++)
			m_lyDaTP->AppendDataPoint(i, m_historicData[i].da_true_pos);

		unsigned int totalTP = 0;
		for (size_t i = 0; i < N; i++)
			totalTP += m_historicData[i].da_true_pos;
		lbDaTP->SetLabel(wxString::Format(wxT("True positives: %u"), totalTP));

		// DA True negatives --------------
		if (m_lyDaTN->GetDataLength() >= N) m_lyDaTN->Clear();
		for (size_t i = m_lyDaTN->GetDataLength(); i < N; i++)
			m_lyDaTN->AppendDataPoint(i, m_historicData[i].da_true_neg);

		unsigned int totalTN = 0;
		for (size_t i = 0; i < N; i++)
			totalTN += m_historicData[i].da_true_neg;
		lbDaTN->SetLabel(wxString::Format(wxT("True negatives: %u"), totalTN));

		// DA false positives --------------
		if (m_lyDaFP->GetDataLength() >= N) m_lyDaFP->Clear();
		for (size_t i = m_lyDaFP->GetDataLength(); i < N; i++)
			m_lyDaFP->AppendDataPoint(i, m_historicData[i].da_false_pos);

		unsigned int totalFP = 0;
		for (size_t i = 0; i < N; i++)
			totalFP += m_historicData[i].da_false_pos;
		StaticText6->SetLabel(
			wxString::Format(wxT("False positives: %u"), totalFP));

		// DA false negatives --------------
		if (m_lyDaFN->GetDataLength() >= N) m_lyDaFN->Clear();
		for (size_t i = m_lyDaFN->GetDataLength(); i < N; i++)
			m_lyDaFN->AppendDataPoint(i, m_historicData[i].da_false_neg);

		unsigned int totalFN = 0;
		for (size_t i = 0; i < N; i++)
			totalFN += m_historicData[i].da_false_neg;
		StaticText7->SetLabel(
			wxString::Format(wxT("False negatives: %u"), totalFN));

		// JCBB iterations --------------
		if (m_lyDaJCBB->GetDataLength() >= N) m_lyDaJCBB->Clear();
		for (size_t i = m_lyDaJCBB->GetDataLength(); i < N; i++)
			m_lyDaJCBB->AppendDataPoint(i, m_historicData[i].jcbb_iters);

		plotDaFN->Fit();
		plotDaFN->Refresh();
		plotDaFP->Fit();
		plotDaFP->Refresh();
		plotDaTN->Fit();
		plotDaTN->Refresh();
		plotDaTP->Fit();
		plotDaTP->Refresh();

		plotDaJCBB->Fit();
		plotDaJCBB->Refresh();
	}
}

void slamdemoFrame::TSimulationOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& f, const std::string& c)
{
	MRPT_LOAD_CONFIG_VAR(random_seed, int, f, c)
	MRPT_LOAD_CONFIG_VAR(map_generator, string, f, c)
	MRPT_LOAD_CONFIG_VAR(randomMap_nLMs, int, f, c)

	sensorOnTheRobot.x(
		f.read_double(c, "sensorOnTheRobot.x", sensorOnTheRobot.x()));
	sensorOnTheRobot.y(
		f.read_double(c, "sensorOnTheRobot.y", sensorOnTheRobot.y()));
	sensorOnTheRobot.phi(DEG2RAD(f.read_double(
		c, "sensorOnTheRobot.phi", RAD2DEG(sensorOnTheRobot.phi()))));

	MRPT_LOAD_CONFIG_VAR(sensor_max_range, double, f, c)
	MRPT_LOAD_CONFIG_VAR(sensor_min_range, double, f, c)
	MRPT_LOAD_CONFIG_VAR_DEGREES(sensor_fov, f, c)

	MRPT_LOAD_CONFIG_VAR(sensorDistingishesLandmarks, bool, f, c)

	MRPT_LOAD_CONFIG_VAR(path_square_len, double, f, c)
	MRPT_LOAD_CONFIG_VAR(robot_step_length, double, f, c)

	MRPT_LOAD_CONFIG_VAR(odometry_noise_std_xy, double, f, c)
	MRPT_LOAD_CONFIG_VAR_DEGREES(odometry_noise_std_phi, f, c)

	MRPT_LOAD_CONFIG_VAR(uncert_overestim_odom, double, f, c)
	MRPT_LOAD_CONFIG_VAR(uncert_overestim_sensor, double, f, c)

	MRPT_LOAD_CONFIG_VAR(show_map_real_correspondences, bool, f, c)

	MRPT_LOAD_CONFIG_VAR(spurious_count_mean, double, f, c)
	MRPT_LOAD_CONFIG_VAR(spurious_count_std, double, f, c)
}

void slamdemoFrame::TSimulationOptions::saveToConfigFile(
	mrpt::config::CConfigFileBase& f, const std::string& c) const
{
	MRPT_SAVE_CONFIG_VAR(random_seed, f, c)
	MRPT_SAVE_CONFIG_VAR(map_generator, f, c)
	MRPT_SAVE_CONFIG_VAR(randomMap_nLMs, f, c)

	f.write(c, "sensorOnTheRobot.x", sensorOnTheRobot.x());
	f.write(c, "sensorOnTheRobot.y", sensorOnTheRobot.y());
	f.write(c, "sensorOnTheRobot.phi", RAD2DEG(sensorOnTheRobot.phi()));

	MRPT_SAVE_CONFIG_VAR(sensor_max_range, f, c)
	MRPT_SAVE_CONFIG_VAR(sensor_min_range, f, c)
	MRPT_SAVE_CONFIG_VAR_DEGREES(sensor_fov, f, c)

	MRPT_SAVE_CONFIG_VAR(sensorDistingishesLandmarks, f, c)

	MRPT_SAVE_CONFIG_VAR(path_square_len, f, c)
	MRPT_SAVE_CONFIG_VAR(robot_step_length, f, c)

	MRPT_SAVE_CONFIG_VAR(odometry_noise_std_xy, f, c)
	MRPT_SAVE_CONFIG_VAR_DEGREES(odometry_noise_std_phi, f, c)

	MRPT_SAVE_CONFIG_VAR(uncert_overestim_odom, f, c)
	MRPT_SAVE_CONFIG_VAR(uncert_overestim_sensor, f, c)

	MRPT_SAVE_CONFIG_VAR(show_map_real_correspondences, f, c)

	MRPT_SAVE_CONFIG_VAR(spurious_count_mean, f, c)
	MRPT_SAVE_CONFIG_VAR(spurious_count_std, f, c)
}

void slamdemoFrame::TSimulationOptions::dumpToTextStream(
	std::ostream& out) const
{
}
/*---------------------------------------------------------------
						executeOneStep
  ---------------------------------------------------------------*/
void slamdemoFrame::executeOneStep()
{
	try
	{
		// Move the robot   ------------------------
		// Skip in the first step:
		CPose2D poseIncr(0, 0, 0);

		const int turnSteps = 10;
		const double Aphi = DEG2RAD(90.0 / turnSteps);

		if (!m_historicData.empty())
		{  // Ok, move:
			const double PATH_SQUARE_LEN = options.path_square_len;

			if (fabs(fmod(m_GT_pose.phi(), 90.0_deg)) < 1e-2)
			{
				int dir = mrpt::round(m_GT_pose.phi() / Aphi);

				// Continue in a straight line, unless we reach a corner:
				if ((m_GT_pose.x() > PATH_SQUARE_LEN &&
					 m_GT_pose.y() < PATH_SQUARE_LEN && dir == 0) ||
					(m_GT_pose.x() > PATH_SQUARE_LEN &&
					 m_GT_pose.y() > PATH_SQUARE_LEN && dir == turnSteps) ||
					(m_GT_pose.x() <= 0 && std::abs(dir) == 2 * turnSteps) ||
					(m_GT_pose.y() <= 0 && dir == -turnSteps))
				{  // Turn
					poseIncr = CPose2D(0, 0, Aphi);
				}
				else
				{  // Straight:
					poseIncr = CPose2D(options.robot_step_length, 0, 0);
				}
			}
			else
			{  // Continue turning:
				poseIncr = CPose2D(0, 0, Aphi);
			}
		}

		m_GT_pose = m_GT_pose + poseIncr;

		// Round phi so we have always perfect square paths:
		m_GT_pose.phi(mrpt::round(m_GT_pose.phi() / Aphi) * Aphi);

		// Simulate observation ------------------------
		{
			const CPose3D robotPose = CPose3D(this->m_GT_pose);
			const CPose3D sensorOnRobot = CPose3D(options.sensorOnTheRobot);

			m_lastObservation.fieldOfView_yaw = options.sensor_fov;
			m_lastObservation.maxSensorDistance = options.sensor_max_range;
			m_lastObservation.minSensorDistance = options.sensor_min_range;

			m_GT_map.simulateRangeBearingReadings(
				robotPose, sensorOnRobot, m_lastObservation,
				options.sensorDistingishesLandmarks,
				m_SLAM.options.std_sensor_range /
					options.uncert_overestim_sensor,
				m_SLAM.options.std_sensor_yaw / options.uncert_overestim_sensor,
				0,	// sigma_pitch: we are in 2D
				&m_lastObservation_GT_indices, options.spurious_count_mean,
				options.spurious_count_std);
		}

		// before processing the observation:
		const std::set<size_t> old_realIDX_already_mapped =
			m_realIDX_already_mapped;

		// SLAM ------------------------
		double executionTime;
		static CTicTac tictac;
		{
			auto act = CActionCollection::Create();
			CActionRobotMovement2D actmov;
			CActionRobotMovement2D::TMotionModelOptions odo_opts;
			odo_opts.modelSelection = CActionRobotMovement2D::mmGaussian;

			// Model as a constant noise in X,Y,PHI:
			odo_opts.gaussianModel.a1 = 0;	// 0.01f;
			odo_opts.gaussianModel.a2 = 0;	// RAD2DEG( 0.0001f );
			odo_opts.gaussianModel.a3 = 0;	// DEG2RAD( 0.1f );
			odo_opts.gaussianModel.a4 = 0;	// 0.01; //0.05f;
			odo_opts.gaussianModel.minStdXY = options.odometry_noise_std_xy;
			odo_opts.gaussianModel.minStdPHI = options.odometry_noise_std_phi;

			// Add noise:
			CPose2D noisyPoseIncr = poseIncr;
			noisyPoseIncr.x_incr(getRandomGenerator().drawGaussian1D(
				0,
				options.odometry_noise_std_xy / options.uncert_overestim_odom));
			noisyPoseIncr.y_incr(getRandomGenerator().drawGaussian1D(
				0,
				options.odometry_noise_std_xy / options.uncert_overestim_odom));
			noisyPoseIncr.phi_incr(getRandomGenerator().drawGaussian1D(
				0,
				options.odometry_noise_std_phi /
					options.uncert_overestim_odom));

			actmov.computeFromOdometry(noisyPoseIncr, odo_opts);
			actmov.timestamp = mrpt::system::now();
			act->insert(actmov);

			CSensoryFrame::Ptr sf = CSensoryFrame::Create();
			m_lastObservation.timestamp = mrpt::system::now();
			m_lastObservation.sensorLabel = "SIMUL_2D_RB";

			sf->insert(CObservationBearingRange::Create(m_lastObservation));

			tictac.Tic();

			m_SLAM.processActionObservation(act, sf);

			executionTime = tictac.Tac();

			// Save dataset to file?
			if (m_rawlog_out_file.fileOpenCorrectly())
			{ archiveFrom(m_rawlog_out_file) << act << sf; }
		}

		// For the case of doing D.A., save the correspondences REAL_MAP <->
		// ESTIMATED_MAP ----------
		const CRangeBearingKFSLAM2D::TDataAssocInfo& da =
			m_SLAM.getLastDataAssociation();

		for (auto newly_inserted_landmark : da.newly_inserted_landmarks)
		{
			const size_t obs_idx = newly_inserted_landmark.first;
			const size_t est_map_idx = newly_inserted_landmark.second;
			const size_t real_map_idx = m_lastObservation_GT_indices[obs_idx];

			m_estimatedIDX2realIDX[est_map_idx] = real_map_idx;

			// Do NOT remember all spureous readings, since they all look alike:
			if (real_map_idx != std::string::npos)
				m_realIDX_already_mapped.insert(real_map_idx);
		}

		// Save historic data ------------------------
		m_historicData.emplace_back();
		THistoric& hist = m_historicData.back();

		hist.GT_robot_pose = m_GT_pose;
		m_SLAM.getCurrentRobotPose(hist.estimate_robot_pose);
		hist.run_time = executionTime;
		hist.jcbb_iters = da.results.nNodesExploredInJCBB;

		// D.A. history data:
		if (!options.sensorDistingishesLandmarks)
		{
			ASSERT_(
				m_lastObservation.sensedData.size() ==
				m_lastObservation_GT_indices.size());

			for (unsigned int o = 0; o < m_lastObservation.sensedData.size();
				 o++)
			{
				const bool o_has_assoc =
					(da.results.associations.find(o) !=
					 da.results.associations.end());
				const size_t o_realmap_idx =
					m_lastObservation_GT_indices[o];  // Note: This can be "-1"
				// for spurious readings!
				const bool o_was_mapped = (o_realmap_idx == std::string::npos)
					? false
					: old_realIDX_already_mapped.find(o_realmap_idx) !=
						old_realIDX_already_mapped.end();
				const bool o_has_been_just_inserted =
					da.newly_inserted_landmarks.find(o) !=
					da.newly_inserted_landmarks.end();

				if (o_was_mapped)
				{  // ALREADY OBSERVED IN THE PAST

					if (o_has_assoc)
					{
						if (m_estimatedIDX2realIDX
								[da.results.associations.find(o)->second] ==
							o_realmap_idx)
						{
							// True positive: If the LM was already known and
							// it's been recognized as such...
							hist.da_true_pos++;
						}
						else
						{
							// False positive:
							hist.da_false_pos++;
						}
					}
					else
					{
						ASSERT_(o_has_been_just_inserted);
						// False negative: It was an already known LM but has
						// been wrongly classified as new:
						hist.da_false_neg++;
					}
				}
				else
				{  // OBSERVED FOR THE FIRST TIME

					if (o_has_assoc)
					{
						// False positive:
						hist.da_false_pos++;
					}
					else
					{
						ASSERT_(o_has_been_just_inserted);
						// True negative: If the LM is a new one and it's been
						// inserted as such...
						hist.da_true_neg++;
					}
				}
			}
		}
	}
	catch (const std::exception& e)
	{
		try
		{
			wxCommandEvent dum;
			this->OnbtnStopClicked(dum);
		}
		catch (...)
		{
		}
		cerr << endl << mrpt::exception_to_str(e) << endl;
		wxMessageBox(mrpt::exception_to_str(e), _("Exception"));
	}
}

/*---------------------------------------------------------------
						OntimSimulTrigger
  ---------------------------------------------------------------*/
void slamdemoFrame::OntimSimulTrigger(wxTimerEvent& event)
{
	static CTicTac tictac;

	if (!ToolBar1->GetToolState(ID_BTNRUN)) return;

	// Simulate one step:
	tictac.Tic();

	executeOneStep();

	const double T = tictac.Tac();

	updateAllGraphs();

	StatusBar1->SetStatusText(wxString::Format(
		_("Step %u done in %.03fms"), (unsigned)m_historicData.size(),
		1e3 * T));

	wxTheApp->Yield(true);	// Let the app. process messages

	// Prepare next step:
	timSimul.Start(20, true);
}

/*---------------------------------------------------------------
						OnConfigClicked
  ---------------------------------------------------------------*/
void slamdemoFrame::OnConfigClicked(wxCommandEvent& event)
{
	CDlgParams dlg(this);

	// Put all params
	// ---------------------------------------
	dlg.rbKFnaiv->SetValue(m_SLAM.KF_options.method == kfEKFNaive);
	dlg.rbKFdavison->SetValue(m_SLAM.KF_options.method == kfEKFAlaDavison);
	dlg.rbIKF->SetValue(m_SLAM.KF_options.method == kfIKFFull);
	dlg.rbIKFdavison->SetValue(m_SLAM.KF_options.method == kfIKF);
	dlg.edIKFiters->SetValue(m_SLAM.KF_options.IKF_iterations);

	dlg.cbJacobTran->SetValue(
		!m_SLAM.KF_options.use_analytic_transition_jacobian);
	dlg.cbJacobObs->SetValue(
		!m_SLAM.KF_options.use_analytic_observation_jacobian);

	// dlg.rbFusion->SetSelection( (int)m_SLAM.KF_options.fusion_strategy );

	dlg.rbMapCorridor->SetValue(options.map_generator == "1");
	dlg.rbMapRandom->SetValue(options.map_generator == "2");

	if (options.map_generator != "1" && options.map_generator != "2")
		dlg.rbMapFile->SetValue(true);

	dlg.edLMs->SetValue(options.randomMap_nLMs);

	dlg.edPathLen->SetValue(wxString::Format(_("%f"), options.path_square_len));
	dlg.edPathStepSize->SetValue(
		wxString::Format(_("%f"), options.robot_step_length));
	dlg.edOdomStdXY->SetValue(
		wxString::Format(_("%f"), options.odometry_noise_std_xy));
	dlg.edStdOdomPhi->SetValue(
		wxString::Format(_("%f"), RAD2DEG(options.odometry_noise_std_phi)));

	dlg.cbSensorDistin->SetValue(options.sensorDistingishesLandmarks);
	dlg.edSeed->SetValue(options.random_seed);

	dlg.edStdRange->SetValue(
		wxString::Format(_("%f"), m_SLAM.options.std_sensor_range));
	dlg.edStdAngle->SetValue(
		wxString::Format(_("%f"), RAD2DEG(m_SLAM.options.std_sensor_yaw)));

	dlg.edSpuriousMean->SetValue(
		wxString::Format(_("%f"), options.spurious_count_mean));
	dlg.edSpuriousStd->SetValue(
		wxString::Format(_("%f"), options.spurious_count_std));

	dlg.edSenX->SetValue(
		wxString::Format(_("%f"), options.sensorOnTheRobot.x()));
	dlg.edSenY->SetValue(
		wxString::Format(_("%f"), options.sensorOnTheRobot.y()));
	dlg.edSenPhi->SetValue(
		wxString::Format(_("%f"), RAD2DEG(options.sensorOnTheRobot.phi())));

	dlg.edMaxR->SetValue(wxString::Format(_("%f"), options.sensor_max_range));
	dlg.edMinR->SetValue(wxString::Format(_("%f"), options.sensor_min_range));
	dlg.edFOV->SetValue(wxString::Format(_("%f"), RAD2DEG(options.sensor_fov)));

	dlg.edChi2->SetValue(
		wxString::Format(_("%.04f"), m_SLAM.options.data_assoc_IC_chi2_thres));
	dlg.rbDAMethod->SetSelection(int(m_SLAM.options.data_assoc_method));
	dlg.rbDAMetric->SetSelection(int(m_SLAM.options.data_assoc_metric));
	dlg.rbICmetric->SetSelection(int(m_SLAM.options.data_assoc_IC_metric));
	dlg.edICMLrefDist->SetValue(wxString::Format(
		_("%.04f"), m_SLAM.options.data_assoc_IC_ml_threshold));

	dlg.edOverOdom->SetValue(options.uncert_overestim_odom * 100);
	dlg.edOverSensor->SetValue(options.uncert_overestim_sensor * 100);

	{
		wxCommandEvent dum;
		dlg.OnUpdateControlsState(dum);
	}

	if (dlg.ShowModal() == wxID_OK)
	{
		// Get all params
		// ---------------------------------------
		if (dlg.rbKFnaiv->GetValue()) m_SLAM.KF_options.method = kfEKFNaive;
		if (dlg.rbKFdavison->GetValue())
			m_SLAM.KF_options.method = kfEKFAlaDavison;
		if (dlg.rbIKF->GetValue()) m_SLAM.KF_options.method = kfIKFFull;
		if (dlg.rbIKFdavison->GetValue()) m_SLAM.KF_options.method = kfIKF;
		m_SLAM.KF_options.IKF_iterations = dlg.edIKFiters->GetValue();

		m_SLAM.KF_options.use_analytic_transition_jacobian =
			!dlg.cbJacobTran->GetValue();
		m_SLAM.KF_options.use_analytic_observation_jacobian =
			!dlg.cbJacobObs->GetValue();

		//		m_SLAM.KF_options.fusion_strategy =
		// TKFFusionMethod(dlg.rbFusion->GetSelection());

		if (dlg.rbMapCorridor->GetValue()) options.map_generator = "1";
		else if (dlg.rbMapRandom->GetValue())
			options.map_generator = "2";
		else
			options.map_generator = string(dlg.edMapFile->GetValue().mb_str());

		options.randomMap_nLMs = dlg.edLMs->GetValue();
		options.random_seed = dlg.edSeed->GetValue();

		options.path_square_len = atof(dlg.edPathLen->GetValue().mb_str());
		options.robot_step_length =
			atof(dlg.edPathStepSize->GetValue().mb_str());
		options.odometry_noise_std_xy =
			atof(dlg.edOdomStdXY->GetValue().mb_str());
		options.odometry_noise_std_phi =
			DEG2RAD(atof(dlg.edStdOdomPhi->GetValue().mb_str()));

		options.sensorDistingishesLandmarks = dlg.cbSensorDistin->GetValue();

		m_SLAM.options.std_sensor_range =
			atof(dlg.edStdRange->GetValue().mb_str());
		m_SLAM.options.std_sensor_yaw =
			DEG2RAD(atof(dlg.edStdAngle->GetValue().mb_str()));

		options.spurious_count_mean =
			atof(dlg.edSpuriousMean->GetValue().mb_str());
		options.spurious_count_std =
			atof(dlg.edSpuriousStd->GetValue().mb_str());

		options.sensorOnTheRobot.x(atof(dlg.edSenX->GetValue().mb_str()));
		options.sensorOnTheRobot.y(atof(dlg.edSenY->GetValue().mb_str()));
		options.sensorOnTheRobot.phi(
			DEG2RAD(atof(dlg.edSenPhi->GetValue().mb_str())));

		options.sensor_max_range = atof(dlg.edMaxR->GetValue().mb_str());
		options.sensor_min_range = atof(dlg.edMinR->GetValue().mb_str());
		options.sensor_fov = DEG2RAD(atof(dlg.edFOV->GetValue().mb_str()));

		m_SLAM.options.data_assoc_IC_chi2_thres =
			atof(dlg.edChi2->GetValue().mb_str());
		m_SLAM.options.data_assoc_method =
			TDataAssociationMethod(dlg.rbDAMethod->GetSelection());
		m_SLAM.options.data_assoc_metric =
			TDataAssociationMetric(dlg.rbDAMetric->GetSelection());
		m_SLAM.options.data_assoc_IC_metric =
			TDataAssociationMetric(dlg.rbICmetric->GetSelection());
		m_SLAM.options.data_assoc_IC_ml_threshold =
			atof(dlg.edICMLrefDist->GetValue().mb_str());

		options.uncert_overestim_odom = 0.01 * dlg.edOverOdom->GetValue();
		options.uncert_overestim_sensor = 0.01 * dlg.edOverSensor->GetValue();
	}
}

void slamdemoFrame::OnMenuSaveFilterState(wxCommandEvent& event)
{
	CPosePDFGaussian estRobotPose;
	vector<TPoint2D> LMs;
	map<unsigned int, CLandmark::TLandmarkID> landmarkIDs;

	CVectorDouble Xkk;	// Full mean & cov
	CMatrixDouble Pkk;

	m_SLAM.getCurrentState(estRobotPose, LMs, landmarkIDs, Xkk, Pkk);

	{
		wxFileDialog dialog(
			this, _("Save state vector...") /*caption*/,
			_(".") /* defaultDir */, _("kf_mean.txt") /* defaultFilename */,
			_("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

		if (dialog.ShowModal() != wxID_OK) return;
		string filName(dialog.GetPath().mb_str());

		Xkk.saveToTextFile(filName);
	}
	{
		wxFileDialog dialog(
			this, _("Save covariance matrix...") /*caption*/,
			_(".") /* defaultDir */, _("kf_cov.txt") /* defaultFilename */,
			_("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

		if (dialog.ShowModal() != wxID_OK) return;
		string filName(dialog.GetPath().mb_str());

		Pkk.saveToTextFile(filName);
	}

	{
		wxFileDialog dialog(
			this, _("Save as 3D opengl scene...") /*caption*/,
			_(".") /* defaultDir */, _("slam.3Dscene") /* defaultFilename */,
			_("MRPT 3D scenes "
			  "(*.3Dscene)|*."
			  "3Dscene|All files "
			  "(*.*)|*.*") /* wildcard */,
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

		if (dialog.ShowModal() != wxID_OK) return;
		string filName(dialog.GetPath().mb_str());

		// Save as 3D objects:
		auto obj3D = mrpt::opengl::CSetOfObjects::Create();
		m_SLAM.getAs3DObject(obj3D);

		mrpt::opengl::COpenGLScene scene;
		scene.insert(obj3D);

		scene.saveToFile(filName);
	}
}

void slamdemoFrame::OnMenuProfilerViewStats(wxCommandEvent& event)
{
	string profStats = m_SLAM.getProfiler().getStatsAsText();
	CLogView dlg(this);
	dlg.edLog->SetValue(profStats.c_str());
	dlg.ShowModal();
}

void slamdemoFrame::OnMenuProfilerReset(wxCommandEvent& event)
{
	m_SLAM.getProfiler().clear();
}

void slamdemoFrame::OnmnuSaveLastDASelected(wxCommandEvent& event)
{
	// Data association graphs ------------------------
	const CRangeBearingKFSLAM2D::TDataAssocInfo& da =
		m_SLAM.getLastDataAssociation();

	{
		wxFileDialog dialog(
			this, _("Save prediction landmark IDs...") /*caption*/,
			_(".") /* defaultDir */,
			_("prediction_IDs.txt") /* defaultFilename */,
			_("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

		if (dialog.ShowModal() != wxID_OK) return;
		string filName(dialog.GetPath().mb_str());

		mrpt::io::vectorToTextFile(da.predictions_IDs, filName);
	}
	{
		wxFileDialog dialog(
			this, _("Save prediction means...") /*caption*/,
			_(".") /* defaultDir */,
			_("prediction_means.txt") /* defaultFilename */,
			_("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

		if (dialog.ShowModal() != wxID_OK) return;
		string filName(dialog.GetPath().mb_str());

		da.Y_pred_means.saveToTextFile(filName);
	}
	{
		wxFileDialog dialog(
			this, _("Save prediction covariance...") /*caption*/,
			_(".") /* defaultDir */,
			_("prediction_cov.txt") /* defaultFilename */,
			_("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

		if (dialog.ShowModal() != wxID_OK) return;
		string filName(dialog.GetPath().mb_str());

		da.Y_pred_covs.saveToTextFile(filName);
	}
}

void slamdemoFrame::OnmnuItemSaveRawlogSelected(wxCommandEvent& event)
{
	const bool saveMnuChkd = mnuItemSaveRawlog->IsChecked();

	if (m_rawlog_out_file.fileOpenCorrectly()) m_rawlog_out_file.close();

	if (saveMnuChkd)
	{
		// Start saving:
		wxFileDialog dialog(
			this, _("Save data set...") /*caption*/, _(".") /* defaultDir */,
			_("simul-range-bearing_dataset.rawlog") /* defaultFilename */,
			_("Rawlogs (*.rawlog)|*.rawlog|All files (*.*)|*.*") /* wildcard */,
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

		if (dialog.ShowModal() != wxID_OK) return;
		const string filName(dialog.GetPath().mb_str());

		if (!m_rawlog_out_file.open(filName))
		{
			mnuItemSaveRawlog->Check(false);
			wxMessageBox(_("Cannot open output file..."), _("Error"));
			return;
		}

		// Save a first "observation" with historic data:
		CObservationComment obs;
		obs.text = std::string(
					   "Rawlog generated by 2d-slam-demo\n"
					   " MRPT version: ") +
			mrpt::system::MRPT_getVersion() +
			std::string(
					   "\n"
					   " Creation date: ") +
			mrpt::system::dateTimeLocalToString(mrpt::system::now()) +
			std::string("\n");

		mrpt::serialization::archiveFrom(m_rawlog_out_file) << obs;
	}
}
