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

#include <wx/msgdlg.h>

//(*InternalHeaders(gridmapSimulFrame)
#include <wx/artprov.h>
#include <wx/bitmap.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/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPlanarLaserScan.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/filesystem.h>
#include <wx/artprov.h>
#include <wx/busyinfo.h>
#include <wx/colordlg.h>
#include <wx/dcmemory.h>
#include <wx/dirdlg.h>
#include <wx/filedlg.h>
#include <wx/image.h>
#include <wx/imaglist.h>
#include <wx/log.h>
#include <wx/msgdlg.h>
#include <wx/progdlg.h>
#include <wx/textdlg.h>

#include "../wx-common/mrpt_logo.xpm"
#include "imgs/app_icon_gridmapsimul.xpm"

//#define DO_SCAN_LIKELIHOOD_DEBUG

#ifdef DO_SCAN_LIKELIHOOD_DEBUG
#include <mrpt/gui/CDisplayWindowPlots.h>
#include <mrpt/math/data_utils.h>
#endif

#if !wxUSE_GLCANVAS
#error "OpenGL required: set wxUSE_GLCANVAS to 1 and rebuild wxWidgets"
#endif

#define TIMER_MS 10

#include "virtual_map1.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;
};

#define RETURN_BITMAP(artid, xpm)                                              \
	if (id == artid)                                                           \
	{                                                                          \
		if (client == wxART_MENU)                                              \
		{                                                                      \
			wxBitmap b(xpm);                                                   \
			return wxBitmap(                                                   \
				b.ConvertToImage().Scale(16, 16, wxIMAGE_QUALITY_HIGH));       \
		}                                                                      \
		else                                                                   \
		{                                                                      \
			return wxBitmap(xpm);                                              \
		}                                                                      \
	}

// CreateBitmap function
wxBitmap MyArtProvider::CreateBitmap(
	const wxArtID& id, const wxArtClient& client, const wxSize& size)
{
	RETURN_BITMAP(wxART_MAKE_ART_ID(IMG_MRPT_LOGO), mrpt_logo_xpm);
	RETURN_BITMAP(wxART_MAKE_ART_ID(MAIN_ICON), app_icon_gridmapsimul_xpm);

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

#include <mrpt/gui/CWxGLCanvasBase.h>
#include <mrpt/gui/WxUtils.h>
#include <mrpt/hwdrivers/CJoystick.h>
#include <mrpt/kinematics/CVehicleSimul_DiffDriven.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/CTicTac.h>

#include <vector>

using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::opengl;
using namespace mrpt::io;
using namespace mrpt::img;
using namespace mrpt::serialization;
using namespace mrpt::gui;
using namespace mrpt::system;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace std;

mrpt::kinematics::CVehicleSimul_DiffDriven the_robot;
COccupancyGridMap2D the_grid;
CJoystick joystick;

std::vector<mrpt::poses::CPose2D> robot_path_GT, robot_path_ODO;
CPose2D lastOdo, pose_start;
bool we_are_closing = false;
long decimation = 1;

mrpt::opengl::CSetOfObjects::Ptr gl_grid;
mrpt::opengl::CSetOfObjects::Ptr gl_robot;
mrpt::opengl::CPlanarLaserScan::Ptr gl_scan;
mrpt::opengl::CPointCloud::Ptr gl_path_GT;
mrpt::opengl::CPointCloud::Ptr gl_path_ODO;

int last_pressed_key = 0;

long LASER_N_RANGES = 361;
double LASER_APERTURE = M_PI;
double LASER_STD_ERROR = 0.01;
double LASER_BEARING_STD_ERROR = 0.05_deg;

class CMyGLCanvas : public CWxGLCanvasBase
{
   public:
	CMyGLCanvas(
		wxWindow* parent, wxWindowID id = wxID_ANY,
		const wxPoint& pos = wxDefaultPosition,
		const wxSize& size = wxDefaultSize, long style = 0,
		const wxString& name = _T("CWxGLCanvasBase"))
		: CWxGLCanvasBase(parent, id, pos, size, style, name)
	{
	}

	~CMyGLCanvas() override = default;
	void OnCharCustom(wxKeyEvent& event) override;

	void OnPreRender() override;
	void OnPostRender() override;
	void OnPostRenderSwapBuffers(double At, wxPaintDC& dc) override;
	void OnRenderError(const wxString& str) override;
};

void CMyGLCanvas::OnRenderError(const wxString& str) {}
void CMyGLCanvas::OnPreRender() {}
void CMyGLCanvas::OnPostRenderSwapBuffers(double At, wxPaintDC& dc) {}
void CMyGLCanvas::OnPostRender() {}

void CMyGLCanvas::OnCharCustom(wxKeyEvent& event)
{
	last_pressed_key = event.GetKeyCode();
}

//(*IdInit(gridmapSimulFrame)
const long gridmapSimulFrame::ID_STATICTEXT1 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL1 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT2 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL2 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT3 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL3 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT8 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL5 = wxNewId();
const long gridmapSimulFrame::ID_BUTTON5 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT16 = wxNewId();
const long gridmapSimulFrame::ID_SPINCTRL1 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT9 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL6 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT13 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL7 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT10 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL8 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT11 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL9 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT12 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL10 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT14 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL11 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT4 = wxNewId();
const long gridmapSimulFrame::ID_BUTTON4 = wxNewId();
const long gridmapSimulFrame::ID_CHECKBOX1 = wxNewId();
const long gridmapSimulFrame::ID_CHECKBOX_RAWLOG_FORMAT = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT6 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT7 = wxNewId();
const long gridmapSimulFrame::ID_BUTTON6 = wxNewId();
const long gridmapSimulFrame::ID_PANEL3 = wxNewId();
const long gridmapSimulFrame::ID_BUTTON1 = wxNewId();
const long gridmapSimulFrame::ID_BUTTON2 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT5 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL4 = wxNewId();
const long gridmapSimulFrame::ID_BUTTON3 = wxNewId();
const long gridmapSimulFrame::ID_STATICTEXT15 = wxNewId();
const long gridmapSimulFrame::ID_TEXTCTRL12 = wxNewId();
const long gridmapSimulFrame::ID_PANEL6 = wxNewId();
const long gridmapSimulFrame::ID_PANEL4 = wxNewId();
const long gridmapSimulFrame::ID_PANEL2 = wxNewId();
const long gridmapSimulFrame::ID_PANEL1 = wxNewId();
const long gridmapSimulFrame::ID_PANEL5 = wxNewId();
const long gridmapSimulFrame::ID_SPLITTERWINDOW1 = wxNewId();
const long gridmapSimulFrame::ID_TIMER1 = wxNewId();
const long gridmapSimulFrame::ID_MENUITEM1 = wxNewId();
const long gridmapSimulFrame::ID_MENUITEM_LOADMAP = wxNewId();
const long gridmapSimulFrame::ID_MENUITEM2 = wxNewId();
const long gridmapSimulFrame::ID_MENUITEM3 = wxNewId();
//*)
const long gridmapSimulFrame::ID_TEXTCTRL_INPUT = wxNewId();

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

#ifdef _MSC_VER
#define _MYTT(X) L##X
#else
#define _MYTT(X) _T(X)
#endif

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

	//(*Initialize(gridmapSimulFrame)
	wxMenuItem* MenuItem2;
	wxStaticBoxSizer* StaticBoxSizer2;
	wxFlexGridSizer* FlexGridSizer8;
	wxFlexGridSizer* FlexGridSizer1;
	wxFlexGridSizer* FlexGridSizer2;
	wxStaticBoxSizer* StaticBoxSizer5;
	wxFlexGridSizer* FlexGridSizer11;
	wxFlexGridSizer* FlexGridSizer7;
	wxFlexGridSizer* FlexGridSizer4;
	wxFlexGridSizer* FlexGridSizer9;
	wxStaticBoxSizer* StaticBoxSizer3;
	wxFlexGridSizer* FlexGridSizer6;
	wxMenuItem* MenuItem3;
	wxFlexGridSizer* FlexGridSizer3;
	wxFlexGridSizer* flexGL;
	wxStaticBoxSizer* StaticBoxSizer4;
	wxStaticBoxSizer* StaticBoxSizer6;
	wxFlexGridSizer* FlexGridSizer10;
	wxFlexGridSizer* FlexGridSizer13;
	wxFlexGridSizer* FlexGridSizer12;
	wxFlexGridSizer* FlexGridSizer5;
	wxStaticBoxSizer* StaticBoxSizer1;

	Create(
		parent, wxID_ANY,
		_("Gridmap navigation simulator - Part of the MRPT project"),
		wxDefaultPosition, wxDefaultSize, wxDEFAULT_FRAME_STYLE,
		_T("wxID_ANY"));
	SetMinSize(wxSize(400, 400));
	{
		wxIcon FrameIcon;
		FrameIcon.CopyFromBitmap(wxArtProvider::GetBitmap(
			wxART_MAKE_ART_ID_FROM_STR(_T("MAIN_ICON")), wxART_FRAME_ICON));
		SetIcon(FrameIcon);
	}
	FlexGridSizer1 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer1->AddGrowableCol(0);
	FlexGridSizer1->AddGrowableRow(0);
	SplitterWindow1 = new wxSplitterWindow(
		this, ID_SPLITTERWINDOW1, wxDefaultPosition, wxDefaultSize, wxSP_3D,
		_T("ID_SPLITTERWINDOW1"));
	SplitterWindow1->SetMinSize(wxSize(200, 200));
	SplitterWindow1->SetMinimumPaneSize(200);
	Panel1 = new wxPanel(
		SplitterWindow1, ID_PANEL1, wxDefaultPosition, wxSize(983, 291),
		wxTAB_TRAVERSAL, _T("ID_PANEL1"));
	Panel1->SetMinSize(wxSize(400, 250));
	FlexGridSizer2 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer2->AddGrowableCol(0);
	// FlexGridSizer2->AddGrowableRow();
	Panel2 = new wxPanel(
		Panel1, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL2"));
	FlexGridSizer3 = new wxFlexGridSizer(2, 1, 0, 0);
	FlexGridSizer3->AddGrowableCol(0);
	Panel3 = new wxPanel(
		Panel2, ID_PANEL3, wxDefaultPosition, wxSize(622, 67), wxTAB_TRAVERSAL,
		_T("ID_PANEL3"));
	FlexGridSizer4 = new wxFlexGridSizer(1, 4, 0, 0);
	FlexGridSizer4->AddGrowableCol(2);
	FlexGridSizer12 = new wxFlexGridSizer(0, 1, 0, 0);
	StaticBoxSizer1 =
		new wxStaticBoxSizer(wxHORIZONTAL, Panel3, _("Laser sensor"));
	FlexGridSizer5 = new wxFlexGridSizer(0, 2, 0, 0);
	StaticText1 = new wxStaticText(
		Panel3, ID_STATICTEXT1, _("Range count:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT1"));
	FlexGridSizer5->Add(
		StaticText1, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edCount = new wxTextCtrl(
		Panel3, ID_TEXTCTRL1, _("361"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL1"));
	FlexGridSizer5->Add(
		edCount, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText2 = new wxStaticText(
		Panel3, ID_STATICTEXT2, _("Span (deg):"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT2"));
	FlexGridSizer5->Add(
		StaticText2, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edSpan = new wxTextCtrl(
		Panel3, ID_TEXTCTRL2, _("180"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL2"));
	FlexGridSizer5->Add(
		edSpan, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText3 = new wxStaticText(
		Panel3, ID_STATICTEXT3, _("Range noise std. (m):"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT3"));
	FlexGridSizer5->Add(
		StaticText3, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edStdNoise = new wxTextCtrl(
		Panel3, ID_TEXTCTRL3, _("0.01"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL3"));
	FlexGridSizer5->Add(
		edStdNoise, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticText8 = new wxStaticText(
		Panel3, ID_STATICTEXT8, _("Bearing noise std. (deg):"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT8"));
	FlexGridSizer5->Add(
		StaticText8, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edStdNoiseAng = new wxTextCtrl(
		Panel3, ID_TEXTCTRL5, _("0.05"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL5"));
	FlexGridSizer5->Add(
		edStdNoiseAng, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	btnSetLaser = new wxButton(
		Panel3, ID_BUTTON5, _("Apply"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_BUTTON5"));
	FlexGridSizer5->Add(
		btnSetLaser, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	StaticBoxSizer1->Add(FlexGridSizer5, 1, wxEXPAND, 0);
	FlexGridSizer12->Add(
		StaticBoxSizer1, 1,
		wxALL | wxEXPAND | wxALIGN_TOP | wxALIGN_CENTER_HORIZONTAL, 5);
	StaticBoxSizer6 =
		new wxStaticBoxSizer(wxHORIZONTAL, Panel3, _(" Decimation: "));
	FlexGridSizer13 = new wxFlexGridSizer(0, 2, 0, 0);
	FlexGridSizer13->AddGrowableCol(0);
	StaticText16 = new wxStaticText(
		Panel3, ID_STATICTEXT16, _("Decimation:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT16"));
	FlexGridSizer13->Add(
		StaticText16, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edDecimate = new wxSpinCtrl(
		Panel3, ID_SPINCTRL1, _T("1"), wxDefaultPosition, wxDefaultSize, 0, 1,
		100, 1, _T("ID_SPINCTRL1"));
	edDecimate->SetValue(_T("1"));
	FlexGridSizer13->Add(
		edDecimate, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticBoxSizer6->Add(FlexGridSizer13, 1, wxEXPAND, 0);
	FlexGridSizer12->Add(StaticBoxSizer6, 1, wxEXPAND, 5);
	FlexGridSizer4->Add(FlexGridSizer12, 1, wxEXPAND, 0);
	FlexGridSizer7 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer7->AddGrowableCol(0);
	StaticBoxSizer3 =
		new wxStaticBoxSizer(wxHORIZONTAL, Panel3, _("Odometry errors"));
	FlexGridSizer9 = new wxFlexGridSizer(0, 4, 0, 0);
	StaticText9 = new wxStaticText(
		Panel3, ID_STATICTEXT9, _MYTT("\u0394x bias:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT9"));
	FlexGridSizer9->Add(
		StaticText9, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edAxb = new wxTextCtrl(
		Panel3, ID_TEXTCTRL6, _("1e-2"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL6"));
	FlexGridSizer9->Add(
		edAxb, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText13 = new wxStaticText(
		Panel3, ID_STATICTEXT13, _MYTT("\u0394x std:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT13"));
	FlexGridSizer9->Add(
		StaticText13, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edAxs = new wxTextCtrl(
		Panel3, ID_TEXTCTRL7, _("0.01"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL7"));
	FlexGridSizer9->Add(
		edAxs, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText10 = new wxStaticText(
		Panel3, ID_STATICTEXT10, _MYTT("\u0394y bias:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT10"));
	FlexGridSizer9->Add(
		StaticText10, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edAyb = new wxTextCtrl(
		Panel3, ID_TEXTCTRL8, _("0.01"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL8"));
	FlexGridSizer9->Add(
		edAyb, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText11 = new wxStaticText(
		Panel3, ID_STATICTEXT11, _MYTT("\u0394y std:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT11"));
	FlexGridSizer9->Add(
		StaticText11, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edAys = new wxTextCtrl(
		Panel3, ID_TEXTCTRL9, _("0.10"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL9"));
	FlexGridSizer9->Add(
		edAys, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText12 = new wxStaticText(
		Panel3, ID_STATICTEXT12, _MYTT("\u0394\u03C6 bias:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT12"));
	FlexGridSizer9->Add(
		StaticText12, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edApb = new wxTextCtrl(
		Panel3, ID_TEXTCTRL10, _("0.01"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL10"));
	FlexGridSizer9->Add(
		edApb, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText14 = new wxStaticText(
		Panel3, ID_STATICTEXT14, _MYTT("\u0394\u03C6 std:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT14"));
	FlexGridSizer9->Add(
		StaticText14, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edAps = new wxTextCtrl(
		Panel3, ID_TEXTCTRL11, _("0.01"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL11"));
	FlexGridSizer9->Add(
		edAps, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticBoxSizer3->Add(FlexGridSizer9, 1, wxEXPAND, 0);
	FlexGridSizer7->Add(
		StaticBoxSizer3, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticBoxSizer5 =
		new wxStaticBoxSizer(wxHORIZONTAL, Panel3, _(" Re-simulate "));
	FlexGridSizer11 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer11->AddGrowableCol(0);
	StaticText4 = new wxStaticText(
		Panel3, ID_STATICTEXT4,
		_("Select an existing ground truth (GT) file and \nthe corresponding "
		  "scans will be simulated \nagain with the current laser sensor "
		  "errors."),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT4"));
	FlexGridSizer11->Add(StaticText4, 1, wxEXPAND, 5);
	btnResimulate = new wxButton(
		Panel3, ID_BUTTON4, _("Re-simulate..."), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON4"));
	FlexGridSizer11->Add(
		btnResimulate, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticBoxSizer5->Add(FlexGridSizer11, 1, wxEXPAND, 0);
	FlexGridSizer7->Add(StaticBoxSizer5, 1, wxEXPAND, 5);
	FlexGridSizer4->Add(FlexGridSizer7, 1, wxEXPAND, 0);
	StaticBoxSizer2 = new wxStaticBoxSizer(wxHORIZONTAL, Panel3, _("Controls"));
	FlexGridSizer8 = new wxFlexGridSizer(0, 1, 0, 0);
	cbJoy = new wxCheckBox(
		Panel3, ID_CHECKBOX1, _("Use joystick"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX1"));
	cbJoy->SetValue(true);
	FlexGridSizer8->Add(
		cbJoy, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
	StaticText6 = new wxStaticText(
		Panel3, ID_STATICTEXT6,
		_("Use arrow keys for inc/decrement speed and turn."),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT6"));
	FlexGridSizer8->Add(
		StaticText6, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
	StaticText7 = new wxStaticText(
		Panel3, ID_STATICTEXT7, _("Brake: Space or joystick\'s first button."),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT7"));
	FlexGridSizer8->Add(
		StaticText7, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);

	FlexGridSizer8->Add(
		new wxStaticText(
			Panel3, ID_STATICTEXT6, _("Enter keystrokes in this box:"),
			wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT6")),
		1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
	wxTextCtrl* edInput = new wxTextCtrl(
		Panel3, ID_TEXTCTRL_INPUT, _(""), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL_INPUT"));
	FlexGridSizer8->Add(
		edInput, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);

	StaticBoxSizer2->Add(FlexGridSizer8, 1, wxEXPAND, 0);
	FlexGridSizer4->Add(
		StaticBoxSizer2, 1, wxALL | wxALIGN_LEFT | wxALIGN_TOP, 5);
	btnExit = new wxButton(
		Panel3, ID_BUTTON6, _("Exit"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_BUTTON6"));
	FlexGridSizer4->Add(btnExit, 1, wxALL | wxALIGN_LEFT | wxALIGN_TOP, 5);
	Panel3->SetSizer(FlexGridSizer4);
	FlexGridSizer4->SetSizeHints(Panel3);
	FlexGridSizer3->Add(Panel3, 1, wxEXPAND, 0);
	Panel4 = new wxPanel(
		Panel2, ID_PANEL4, wxDefaultPosition, wxSize(622, 55), wxTAB_TRAVERSAL,
		_T("ID_PANEL4"));
	FlexGridSizer6 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer6->AddGrowableCol(0);
	StaticBoxSizer4 =
		new wxStaticBoxSizer(wxHORIZONTAL, Panel4, _("Rawlog generation"));
	Panel5 = new wxPanel(
		Panel4, ID_PANEL6, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL6"));
	FlexGridSizer10 = new wxFlexGridSizer(0, 5, 0, 0);
	FlexGridSizer10->AddGrowableCol(3);
	btnStart = new wxButton(
		Panel5, ID_BUTTON1, _("START"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_BUTTON1"));
	FlexGridSizer10->Add(
		btnStart, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	btnEnd = new wxButton(
		Panel5, ID_BUTTON2, _("STOP"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_BUTTON2"));
	btnEnd->Disable();
	FlexGridSizer10->Add(
		btnEnd, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText5 = new wxStaticText(
		Panel5, ID_STATICTEXT5, _("Output rawlog:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT5"));
	FlexGridSizer10->Add(
		StaticText5, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edOutFile = new wxTextCtrl(
		Panel5, ID_TEXTCTRL4, _("./simul.rawlog"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_TEXTCTRL4"));
	FlexGridSizer10->Add(edOutFile, 1, wxEXPAND, 5);
	btnExplore = new wxButton(
		Panel5, ID_BUTTON3, _("Browse..."), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_BUTTON3"));
	FlexGridSizer10->Add(
		btnExplore, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	// FlexGridSizer10->Add(-1,-1,1,
	// wxALL|wxALIGN_CENTER_HORIZONTAL|wxALIGN_CENTER_VERTICAL, 5);
	cbRawlogSFformat = new wxCheckBox(
		Panel5, ID_CHECKBOX_RAWLOG_FORMAT, _("Act/SF format"),
		wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
		_T("ID_CHECKBOX_RAWLOG_FORMAT"));
	cbRawlogSFformat->SetValue(false);
	FlexGridSizer10->Add(
		cbRawlogSFformat, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);

	FlexGridSizer10->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText15 = new wxStaticText(
		Panel5, ID_STATICTEXT15, _("Output groundtruth:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT15"));
	FlexGridSizer10->Add(
		StaticText15, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edOutGT = new wxTextCtrl(
		Panel5, ID_TEXTCTRL12, _("./simul.rawlog.GT.txt"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_TEXTCTRL12"));
	edOutGT->Disable();
	FlexGridSizer10->Add(edOutGT, 1, wxEXPAND, 5);
	Panel5->SetSizer(FlexGridSizer10);
	FlexGridSizer10->Fit(Panel5);
	FlexGridSizer10->SetSizeHints(Panel5);
	StaticBoxSizer4->Add(Panel5, 1, wxEXPAND, 0);
	FlexGridSizer6->Add(StaticBoxSizer4, 1, wxEXPAND, 5);
	Panel4->SetSizer(FlexGridSizer6);
	FlexGridSizer6->SetSizeHints(Panel4);
	FlexGridSizer3->Add(Panel4, 1, wxEXPAND, 0);
	Panel2->SetSizer(FlexGridSizer3);
	FlexGridSizer3->Fit(Panel2);
	FlexGridSizer3->SetSizeHints(Panel2);
	FlexGridSizer2->Add(Panel2, 1, wxEXPAND, 0);
	Panel1->SetSizer(FlexGridSizer2);
	FlexGridSizer2->SetSizeHints(Panel1);
	panelGL = new wxPanel(
		SplitterWindow1, ID_PANEL5, wxDefaultPosition, wxDefaultSize,
		wxTAB_TRAVERSAL, _T("ID_PANEL5"));
	flexGL = new wxFlexGridSizer(0, 1, 0, 0);
	flexGL->AddGrowableCol(0);
	flexGL->AddGrowableRow(0);
	panelGL->SetSizer(flexGL);
	flexGL->Fit(panelGL);
	flexGL->SetSizeHints(panelGL);
	SplitterWindow1->SplitHorizontally(Panel1, panelGL);
	FlexGridSizer1->Add(SplitterWindow1, 1, wxEXPAND, 0);
	SetSizer(FlexGridSizer1);
	timRun.SetOwner(this, ID_TIMER1);
	timRun.Start(10, true);
	MenuBar1 = new wxMenuBar();
	Menu1 = new wxMenu();
	MenuItem1 = new wxMenuItem(
		Menu1, ID_MENUITEM1, _("Set &output rawlog file..."), wxEmptyString,
		wxITEM_NORMAL);
	MenuItem1->SetBitmap(wxArtProvider::GetBitmap(
		wxART_MAKE_ART_ID_FROM_STR(_T("wxART_FILE_SAVE")), wxART_MENU));
	Menu1->Append(MenuItem1);

	MenuItemLoadMap = new wxMenuItem(
		Menu1, ID_MENUITEM_LOADMAP, _("Load &gridmap..."), wxEmptyString,
		wxITEM_NORMAL);
	Menu1->Append(MenuItemLoadMap);

	MenuItem2 = new wxMenuItem(
		Menu1, ID_MENUITEM2, _("&Quit"), wxEmptyString, wxITEM_NORMAL);
	MenuItem2->SetBitmap(wxArtProvider::GetBitmap(
		wxART_MAKE_ART_ID_FROM_STR(_T("wxART_QUIT")), wxART_MENU));
	Menu1->Append(MenuItem2);
	MenuBar1->Append(Menu1, _("&File"));
	Menu2 = new wxMenu();
	MenuItem3 = new wxMenuItem(
		Menu2, ID_MENUITEM3, _("&About..."), wxEmptyString, wxITEM_NORMAL);
	MenuItem3->SetBitmap(wxArtProvider::GetBitmap(
		wxART_MAKE_ART_ID_FROM_STR(_T("wxART_HELP_BOOK")), wxART_MENU));
	Menu2->Append(MenuItem3);
	MenuBar1->Append(Menu2, _("&Help"));
	SetMenuBar(MenuBar1);
	FlexGridSizer1->Fit(this);
	FlexGridSizer1->SetSizeHints(this);

	Bind(
		wxEVT_BUTTON, &gridmapSimulFrame::OnbtnSetLaserClick, this, ID_BUTTON5);
	Bind(
		wxEVT_BUTTON, &gridmapSimulFrame::OnbtnResimulateClick, this,
		ID_BUTTON4);
	Bind(wxEVT_BUTTON, &gridmapSimulFrame::OnbtnQuitClick, this, ID_BUTTON6);
	Bind(wxEVT_BUTTON, &gridmapSimulFrame::OnbtnStartClick, this, ID_BUTTON1);
	Bind(wxEVT_BUTTON, &gridmapSimulFrame::OnbtnEndClick, this, ID_BUTTON2);
	Bind(wxEVT_BUTTON, &gridmapSimulFrame::OnbtnExploreClick, this, ID_BUTTON3);
	Bind(wxEVT_TIMER, &gridmapSimulFrame::OntimRunTrigger, this, ID_TIMER1);
	Bind(wxEVT_MENU, &gridmapSimulFrame::OnbtnExploreClick, this, ID_MENUITEM1);
	Bind(
		wxEVT_MENU, &gridmapSimulFrame::OnMenuLoadMap, this,
		ID_MENUITEM_LOADMAP);
	Bind(wxEVT_MENU, &gridmapSimulFrame::OnbtnQuitClick, this, ID_MENUITEM2);
	Bind(wxEVT_MENU, &gridmapSimulFrame::OnAbout, this, ID_MENUITEM3);
	//*)

	// Import grid map:
	{
		wxImage img(virtual_map1_xpm);
		CImage* myImg = wxImage2MRPTImage(img);
		the_grid.loadFromBitmap(*myImg, 0.03f);
		delete myImg;
	}

	// Create GL canvas:
	// -------------------------------
	m_canvas =
		new CMyGLCanvas(panelGL, wxID_ANY, wxDefaultPosition, wxDefaultSize);
	panelGL->SetMinSize(wxSize(200, 200));
	m_canvas->SetMinSize(wxSize(200, 200));
	flexGL->Add(m_canvas, 1, wxEXPAND, 0);

	m_canvas->setCameraPointing(0.0f, 0.0f, 0.0f);
	m_canvas->setElevationDegrees(60.0f);
	m_canvas->setZoomDistance(25.0f);

	// Populate scene:
	auto openGLSceneRef = m_canvas->getOpenGLSceneRef();
	openGLSceneRef->insert(
		mrpt::opengl::CGridPlaneXY::Create(-100, 100, -100, 100, 0, 5));

	update_grid_map_3d();
	openGLSceneRef->insert(gl_grid);

	// paths:
	gl_path_GT = mrpt::opengl::CPointCloud::Create();
	gl_path_GT->setColor(0, 0, 0, 0.7);
	gl_path_GT->setLocation(0, 0, 0.01);
	gl_path_GT->setPointSize(3);

	openGLSceneRef->insert(gl_path_GT);

	gl_path_ODO = mrpt::opengl::CPointCloud::Create();
	gl_path_ODO->setColor(0, 1, 0, 0.7);
	gl_path_ODO->setLocation(0, 0, 0.01);
	gl_path_ODO->setPointSize(2);

	openGLSceneRef->insert(gl_path_ODO);

	// Robot & scan:
	gl_robot = mrpt::opengl::stock_objects::RobotPioneer();
	openGLSceneRef->insert(gl_robot);

	gl_scan = mrpt::opengl::CPlanarLaserScan::Create();
	gl_robot->insert(gl_scan);

	// Redirect all keystrokes in this box to the gl canvas:
	edInput->Bind(wxEVT_CHAR, &CMyGLCanvas::OnCharCustom, m_canvas);

	// fix sizes:
	SplitterWindow1->SetMinSize(wxSize(200, 200));
	SplitterWindow1->SetSashPosition(200);
	Fit();
	Maximize();
}

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

void gridmapSimulFrame::update_grid_map_3d()
{
	gl_grid = the_grid.getVisualization();
}

void gridmapSimulFrame::OnbtnQuitClick(wxCommandEvent& event)
{
	we_are_closing = true;
	Close();
}

void gridmapSimulFrame::OntimRunTrigger(wxTimerEvent& event)
{
	static CTicTac tictac;

	if (we_are_closing) return;

	double At = tictac.Tac();
	tictac.Tic();

	// Simul robot:
	the_robot.simulateOneTimeStep(At);

	try
	{
		// Use joystick:
		if (cbJoy->GetValue())
		{
			float x, y, z;
			std::vector<bool> btns;

			if (joystick.getJoystickPosition(0, x, y, z, btns))
			{
				const float V_MAX_ACC = 1;
				const float W_MAX_ACC = DEG2RAD(45.0f);

				const mrpt::math::TTwist2D vel_local =
					the_robot.getCurrentGTVelLocal();
				double v = vel_local.vx, w = vel_local.omega;

				v -= y * V_MAX_ACC * At;
				w -= x * W_MAX_ACC * At;

				if (!btns.empty() && btns[0])
				{
					v *= 0.95;
					w *= 0.95;
				}

				if (btns.size() >= 2 && btns[1])
					m_canvas->setZoomDistance(
						m_canvas->getZoomDistance() * 0.99f);

				if (btns.size() >= 3 && btns[2])
					m_canvas->setZoomDistance(
						m_canvas->getZoomDistance() * 1.01f);

				the_robot.movementCommand(v, w);
			}
		}

		// Use keyboard:
		{
			const float V_MAX_ACC = 1;
			const float W_MAX_ACC = DEG2RAD(45.0f);

			const mrpt::math::TTwist2D vel_local =
				the_robot.getCurrentGTVelLocal();
			double v = vel_local.vx, w = vel_local.omega;

			float x = 0, y = 0;

			switch (last_pressed_key)
			{
				case WXK_UP: y = -1; break;
				case WXK_DOWN: y = 1; break;
				case WXK_LEFT: x = -1; break;
				case WXK_RIGHT: x = 1; break;
			}

			if (x != 0 || y != 0)
			{
				v -= y * V_MAX_ACC * At;
				w -= x * W_MAX_ACC * At;
				the_robot.movementCommand(v, w);
			}
			else if (last_pressed_key == WXK_SPACE)
			{
				v *= 0.95;
				w *= 0.95;
				the_robot.movementCommand(v, w);
			}

			last_pressed_key = 0;
		}

		const mrpt::math::TPose2D p = the_robot.getCurrentGTPose();

		// Simulate scan:
		mrpt::obs::CObservation2DRangeScan the_scan;
		the_scan.sensorLabel = "LASER_SIM";
		the_scan.sensorPose.setFromValues(0.20, 0, 0.10);
		the_scan.maxRange = 80;	 // LASER_MAX_RANGE;
		the_scan.aperture = LASER_APERTURE;
		the_scan.stdError = LASER_STD_ERROR;

		static mrpt::system::CTimeLogger timlog;

		timlog.enter("laserScanSimulator");
		the_grid.laserScanSimulator(
			the_scan, mrpt::poses::CPose2D(p), 0.6f, LASER_N_RANGES,
			LASER_STD_ERROR, 1, LASER_BEARING_STD_ERROR);
		timlog.leave("laserScanSimulator");

#ifdef DO_SCAN_LIKELIHOOD_DEBUG
		{
			timlog.enter("laserScanSimulatorWithUncertainty");
			COccupancyGridMap2D::TLaserSimulUncertaintyParams ssu_params;
			COccupancyGridMap2D::TLaserSimulUncertaintyResult ssu_out;

			ssu_params.robotPose.mean = p;
			ssu_params.robotPose.cov << square(0.25), 0.0, 0.0, 0.0,
				square(0.25), 0.0, 0.0, 0.0, square(5.0_deg);

			the_grid.laserScanSimulatorWithUncertainty(ssu_params, ssu_out);
			timlog.leave("laserScanSimulatorWithUncertainty");

#if 1
			static mrpt::gui::CDisplayWindowPlots win;

			win.plot(ssu_out.scanWithUncert.rangeScan.scan, "3k-", "mean");
			win.plot(the_scan.scan, "r-", "obs");

			Eigen::VectorXd ci1 = ssu_out.scanWithUncert.rangesMean +
				3 *
					ssu_out.scanWithUncert.rangesCovar.diagonal()
						.array()
						.sqrt()
						.matrix();
			Eigen::VectorXd ci2 = ssu_out.scanWithUncert.rangesMean -
				3 *
					ssu_out.scanWithUncert.rangesCovar.diagonal()
						.array()
						.sqrt()
						.matrix();
			win.plot(ci1, "k-", "CI+");
			win.plot(ci2, "k-", "CI-");

			const double LIK =
				ssu_out.scanWithUncert.evaluateScanLikelihood(the_scan);
			win.setWindowTitle(mrpt::format("LIK: %.05f", LIK));

			win.axis_fit();
#endif
		}
#endif

		// Save rawlog?
		// ----------------------------
		static CFileGZOutputStream outs;
		static CFileOutputStream out_GT;

		if (!btnStart->IsEnabled())
		{
			// Recording
			if (!outs.fileOpenCorrectly())
			{
				if (!outs.open(edOutFile->GetValue().ToStdString()))
				{
					wxCommandEvent dum;
					OnbtnEndClick(dum);
					wxMessageBox(
						_("Cannot open output rawlog file."), _("Error"), wxOK,
						this);
				}

				if (!out_GT.open(
						edOutGT->GetValue().ToStdString(),
						mrpt::io::OpenMode::TRUNCATE))
				{
					outs.close();
					wxCommandEvent dum;
					OnbtnEndClick(dum);
					wxMessageBox(
						_("Cannot open output GT file."), _("Error"), wxOK,
						this);
				}

				// Save also the gridmap:
				CFileGZOutputStream out_grid;
				string grid_file = edOutFile->GetValue().ToStdString() +
					string("_grid.gridmap.gz");
				if (!out_grid.open(grid_file))
				{
					outs.close();
					out_GT.close();
					wxCommandEvent dum;
					OnbtnEndClick(dum);
					wxMessageBox(
						_("Cannot open output file for the reference "
						  "gridmap!."),
						_("Error"), wxOK, this);
				}

				archiveFrom(out_grid) << the_grid;	// save it
			}

			static long decimation_count = 0;

			if (outs.fileOpenCorrectly() &&
				0 == (decimation_count++ % decimation))
			{
				// Desired rawlog format?
				const bool is_sf_format = cbRawlogSFformat->GetValue();

				const TTimeStamp tim_now = mrpt::system::now();
				const mrpt::poses::CPose2D odo_now =
					CPose2D(the_robot.getCurrentOdometricPose());

				if (is_sf_format)
				{
					// Action:
					CActionCollection acts;
					CActionRobotMovement2D act;
					CActionRobotMovement2D::TMotionModelOptions opts;

					opts.modelSelection = CActionRobotMovement2D::mmGaussian;

					CPose2D Aodom = odo_now - lastOdo;
					lastOdo = odo_now;

					act.computeFromOdometry(Aodom, opts);
					act.timestamp = tim_now;

					acts.insert(act);
					archiveFrom(outs) << acts;

					// Observation:
					CSensoryFrame sf;
					the_scan.timestamp = act.timestamp;
					sf.insert(CObservation2DRangeScan::Create(the_scan));
					archiveFrom(outs) << sf;
				}
				else
				{
					// Observations only format:
					mrpt::obs::CObservationOdometry odo_obs;
					odo_obs.timestamp = tim_now;
					odo_obs.sensorLabel = "odometry";

					odo_obs.odometry = odo_now;

					odo_obs.hasVelocities = true;
					odo_obs.velocityLocal = the_robot.getCurrentGTVelLocal();

					archiveFrom(outs) << odo_obs << the_scan;
				}

				// And save to a text file the GT robot pose:
				CPose2D cur_pose_relative = CPose2D(p) - pose_start;

				out_GT.printf(
					"%f %.03f %.03f %.03f\n",
					mrpt::system::timestampTotime_t(tim_now),
					cur_pose_relative.x(), cur_pose_relative.y(),
					cur_pose_relative.phi());

				// save to lists:
				robot_path_GT.push_back(mrpt::poses::CPose2D(p));
				robot_path_ODO.push_back(odo_now);
			}
		}
		else
		{
			// Not recording
			if (outs.fileOpenCorrectly())
			{
				// Close it:
				outs.close();
				out_GT.close();
			}
		}

		// Update scan 3D:
		gl_scan->setScan(the_scan);

		// Update robot pose in the 3D scene:
		gl_robot->setPose(CPose3D(p));

		// Update robot path GT/odo:
		if (!robot_path_GT.empty())
		{
			gl_path_GT->insertPoint(
				robot_path_GT.rbegin()->x(), robot_path_GT.rbegin()->y(), 0);
			CPose2D this_odo = pose_start + *robot_path_ODO.rbegin();
			gl_path_ODO->insertPoint(this_odo.x(), this_odo.y(), 0);
		}

		{
			auto scene = m_canvas->getOpenGLSceneRef();

			scene->getViewport()->addTextMessage(
				20, 20,
				string("Pose: ") + the_robot.getCurrentGTPose().asString(), 0);

			const mrpt::math::TTwist2D vel_local =
				the_robot.getCurrentGTVelLocal();
			scene->getViewport()->addTextMessage(
				20, 45, string("Twist local: ") + vel_local.asString(), 1);
		}

		m_canvas->setCameraPointing(p.x, p.y, m_canvas->getCameraPointingZ());
		m_canvas->Refresh();

		// Prepare next interval
		if (!we_are_closing) timRun.Start(TIMER_MS, true);
	}
	catch (const std::exception& e)
	{
		wxCommandEvent dum;
		OnbtnEndClick(dum);
		wxMessageBox(mrpt::exception_to_str(e), wxT("Exception"), wxOK, this);
	}
	catch (...)
	{
		wxCommandEvent dum;
		OnbtnEndClick(dum);
		wxMessageBox(_("Untyped exception!"), _("Exception"), wxOK, this);
	}
}

void gridmapSimulFrame::OnbtnStartClick(wxCommandEvent& event)
{
	btnStart->Enable(false);
	btnEnd->Enable(true);

	robot_path_GT.clear();
	robot_path_ODO.clear();

	gl_path_GT->clear();
	gl_path_ODO->clear();

	pose_start = CPose2D(the_robot.getCurrentGTPose());
	the_robot.setCurrentOdometricPose(TPose2D(0, 0, 0));
	lastOdo = CPose2D(0, 0, 0);

	// odo errors:
	double Ax_err_bias;
	double Ax_err_std;
	double Ay_err_bias;
	double Ay_err_std;
	double Aphi_err_bias;
	double Aphi_err_std;

	edAxb->GetValue().ToCDouble(&Ax_err_bias);
	edAxs->GetValue().ToCDouble(&Ax_err_std);

	edAyb->GetValue().ToCDouble(&Ay_err_bias);
	edAys->GetValue().ToCDouble(&Ay_err_std);

	edApb->GetValue().ToCDouble(&Aphi_err_bias);
	edAps->GetValue().ToCDouble(&Aphi_err_std);
	Aphi_err_bias = DEG2RAD(Aphi_err_bias);
	Aphi_err_std = DEG2RAD(Aphi_err_std);

	the_robot.setOdometryErrors(
		true, Ax_err_bias, Ax_err_std, Ay_err_bias, Ay_err_std, Aphi_err_bias,
		Aphi_err_std);

	// Get decimation:
	decimation = edDecimate->GetValue();
}

void gridmapSimulFrame::OnbtnEndClick(wxCommandEvent& event)
{
	btnStart->Enable(true);
	btnEnd->Enable(false);
}

void gridmapSimulFrame::OnMenuLoadMap(wxCommandEvent& event)
{
	WX_START_TRY

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

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

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

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

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

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

			if (sCellSize.ToCDouble(&cell_size) && sCX.ToCDouble(&cx) &&
				sCY.ToCDouble(&cy))
			{
				if (the_grid.loadFromBitmap(
						img, cell_size, mrpt::math::TPoint2D(cx, cy)))
				{
					update_grid_map_3d();
					wxMessageBox(_("OK"), _("Map loaded!"));
				}
				else
					wxMessageBox(
						_("Error"),
						_("Can't load the image file into the gridmap..."));
			}
			else
				wxMessageBox(
					_("Error"), _("Error parsing the numbers you entered..."));
		}
	}

	WX_END_TRY
}

void gridmapSimulFrame::OnbtnExploreClick(wxCommandEvent& event)
{
	wxFileDialog dialog(
		this, _("Rawlog to create.."), _("."), _("simul.rawlog"),
		wxT("RawLog files (*.rawlog)|*.rawlog|All files (*.*)|*.*"),
		wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

	if (dialog.ShowModal() == wxID_OK)
	{
		edOutFile->SetValue(dialog.GetPath());
		edOutGT->SetValue(dialog.GetPath() + _(".GT.txt"));
	}
}

void gridmapSimulFrame::OnbtnSetLaserClick(wxCommandEvent& event)
{
	edSpan->GetValue().ToCDouble(&LASER_APERTURE);
	LASER_APERTURE = DEG2RAD(LASER_APERTURE);

	edCount->GetValue().ToLong(&LASER_N_RANGES);

	edStdNoise->GetValue().ToCDouble(&LASER_STD_ERROR);

	edStdNoiseAng->GetValue().ToCDouble(&LASER_BEARING_STD_ERROR);
	LASER_BEARING_STD_ERROR = DEG2RAD(LASER_BEARING_STD_ERROR);
}

void gridmapSimulFrame::OnAbout(wxCommandEvent& event)
{
	mrpt::gui::show_mrpt_about_box_wxWidgets(this, "GripmapNavSimul");
}

void gridmapSimulFrame::OnbtnResimulateClick(wxCommandEvent& event)
{
	WX_START_TRY

	string gt_file;
	{
		wxFileDialog dialog(
			this, _("Existing GT file..."), _("."), _("GT.txt"),
			wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*"),
			wxFD_OPEN | wxFD_FILE_MUST_EXIST);
		if (dialog.ShowModal() != wxID_OK) return;
		gt_file = string(dialog.GetPath().mb_str());
	}

	string raw_file;
	{
		wxFileDialog dialog(
			this, _("Corresponding rawlog..."),
			mrpt::system::extractFileDirectory(gt_file).c_str(),
			_("simul.rawlog"),
			wxT("Rawlog files (*.rawlog)|*.rawlog|All files (*.*)|*.*"),
			wxFD_OPEN | wxFD_FILE_MUST_EXIST);
		if (dialog.ShowModal() != wxID_OK) return;
		raw_file = string(dialog.GetPath().mb_str());
	}

	// GT file rows are:
	//  time x y z
	CMatrixDouble GT;
	GT.loadFromTextFile(gt_file);

	// Rawlog:
	CRawlog rawlog;
	rawlog.loadFromRawLogFile(raw_file);

	// cout << "rawlog entries: " << rawlog.size() << endl;

	// Assert sizes:
	ASSERT_(rawlog.size() > 0);
	ASSERT_(GT.cols() >= 4);
	ASSERT_(rawlog.getType(0) == CRawlog::etActionCollection);
	ASSERT_(rawlog.size() / 2 == (size_t)GT.rows());

	// Ask for the output:
	string out_raw_file, out_GT_file;
	{
		wxFileDialog dialog(
			this, _("New rawlog to create..."),
			mrpt::system::extractFileDirectory(gt_file).c_str(),
			_("simul_new.rawlog"),
			wxT("Rawlog files (*.rawlog)|*.rawlog|All files (*.*)|*.*"),
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);
		if (dialog.ShowModal() != wxID_OK) return;
		out_raw_file = string(dialog.GetPath().mb_str());
		out_GT_file = out_raw_file + string(".GT.txt");
	}

	// ------------------------------------------
	// Resimulate scans:
	// ------------------------------------------
	wxCommandEvent ee;
	OnbtnSetLaserClick(ee);

	for (size_t i = 1; i < rawlog.size(); i += 2)
	{
		ASSERT_(rawlog.getType(i) == CRawlog::etSensoryFrame);

		CSensoryFrame::Ptr sf = rawlog.getAsObservations(i);
		CPose2D gt_pose(GT(i / 2, 1), GT(i / 2, 2), GT(i / 2, 3));

		CObservation2DRangeScan::Ptr the_scan =
			sf->getObservationByClass<CObservation2DRangeScan>();
		the_grid.laserScanSimulator(
			*the_scan, gt_pose, 0.5f, LASER_N_RANGES, LASER_STD_ERROR, 1,
			LASER_BEARING_STD_ERROR);
	}

	// Save the new rawlog:
	CFileGZOutputStream f(out_raw_file);
	archiveFrom(f) << rawlog;
	// rawlog.saveToRawLogFile(out_raw_file);

	// The GT file is the same:
	GT.saveToTextFile(out_GT_file, MATRIX_FORMAT_FIXED);

	WX_END_TRY
}
