/* +------------------------------------------------------------------------+
   |                     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 <mrpt/gui/CMyRedirector.h>
#include <wx/busyinfo.h>
#include <wx/dirdlg.h>
#include <wx/filedlg.h>
#include <wx/image.h>
#include <wx/msgdlg.h>
#include <wx/progdlg.h>
#include <wx/textdlg.h>

#include "xRawLogViewerMain.h"

// General global variables:
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CTextFileLinesParser.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <mrpt/obs/CObservationGasSensors.h>
#include <mrpt/obs/CObservationIMU.h>
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CObservationRFID.h>
#include <mrpt/obs/CObservationWirelessPower.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/system/CDirectoryExplorer.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>

#include <Eigen/Dense>

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

extern TTimeStamp rawlog_first_timestamp;

void parseGeneralVector(
	const std::string& str, size_t idx, vector<float>& outVector);

void goToNextToken(char*& str);
void goToTheLastToken(char*& str);

void goToNextToken(char*& str)
{
	while (str[0] && str[0] != ' ')
		str++;
	if (str[0] == ' ') str++;
}

void goToTheLastToken(char*& str)
{
	while (str[0])
		str++;
	str--;
	while (str[0] && str[0] != ' ')
		str--;
	if (str[0] == ' ') str++;
}

void xRawLogViewerFrame::OnImportCARMEN(wxCommandEvent& event)
{
	WX_START_TRY

	wxString caption = wxT("Import a CARMEN log...");
	wxString wildcard =
		wxT("CARMEN log files (*.log)|*.log|All files (*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename = _("*.log");
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_OPEN | wxFD_FILE_MUST_EXIST);

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

	// Save the path
	WX_START_TRY
	iniFile->write(
		iniFileSect, "LastDir", string(dialog.GetDirectory().mb_str()));
	WX_END_TRY

	wxString fileName = dialog.GetPath();
	string fil(fileName.mbc_str());

	loadedFileName = fil + string(".rawlog");

	CActionRobotMovement2D actionMovement;
	CSensoryFrame sf;
	CObservation2DRangeScan::Ptr obsScan;
	CActionCollection actions;
	CPose2D frontLaserPose(0, 0, 0);
	CPose2D rearLaserPose(0, 0, M_PIf);
	CPose2D thisOdoReading, lastOdoReading, Apose;
	bool isTheFirstOdo = true;

	wxString strMaxLaserRange = wxGetTextFromUser(
		_("Maximum valid laser range:"), _("Enter parameter:"), _("30"));
	double maxValidLaserRange;
	strMaxLaserRange.ToCDouble(&maxValidLaserRange);

	float thisTimestamp;

	std::vector<std::string> sl;
	mrpt::io::loadTextFile(sl, fil);
	size_t n = sl.size();

	rawlog.clear();
	wxProgressDialog progDia(
		_("Importing rawlog..."), _("Processing..."),
		n,	// range
		this,  // parent
		wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
			wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);

	unsigned int i = 0;
	bool end = false;
	while (i < n && !end)
	{
		// Find the line type:
		string line(sl[i]);	 //= sl->Strings[i];

		// Find the line type:
		// ----------------------------------------------------
		bool isFrontLaser = line.find("FLASER ") == 0;
		bool isRearLaser = line.find("RLASER ") == 0;
		if (isFrontLaser || isRearLaser)
		{
			obsScan = std::make_shared<CObservation2DRangeScan>();
			obsScan->aperture = M_PIf;
			obsScan->rightToLeft = true;
			obsScan->maxRange = maxValidLaserRange;
			obsScan->sensorPose =
				isFrontLaser ? CPose3D(frontLaserPose) : CPose3D(rearLaserPose);

			// Load readings from the string:
			char* str = &line[7];
			int scanSize = atoi(str);
			goToNextToken(str);

			obsScan->resizeScanAndAssign(scanSize, 0, true);

			for (int q = 0; q < scanSize; q++)
			{
				const auto r = atof(str);
				obsScan->setScanRange(q, r);
				goToNextToken(str);
				obsScan->setScanRangeValidity(q, r < maxValidLaserRange);
			}

			// Read odometry:
			thisOdoReading.x(atof(str));
			goToNextToken(str);
			thisOdoReading.y(atof(str));
			goToNextToken(str);
			thisOdoReading.phi(atof(str));
			goToNextToken(str);

			if (isRearLaser) thisOdoReading.phi(-thisOdoReading.phi());

			// Read timestamp:
			goToTheLastToken(str);
			thisTimestamp = atof(str);
			// Insert timestamp from Carmen logfile, adds UNIX 1970-01-01 offset
			obsScan->timestamp =
				time_tToTimestamp(static_cast<double>(thisTimestamp));

			// thisOdoReading
			if (isTheFirstOdo)
			{
				isTheFirstOdo = false;
				Apose = CPose2D(0, 0, 0);
			}
			else
			{
				Apose = thisOdoReading - lastOdoReading;
			}

			// For the next step:
			// lastTimestamp = thisTimestamp;
			lastOdoReading = thisOdoReading;

			// Add the ODO to the log:
			CActionRobotMovement2D::TMotionModelOptions opts;

			actionMovement.computeFromOdometry(Apose, opts);

			actions.clear();
			actions.insert(actionMovement);

			rawlog.insert(actions);

			// Add the SCAN to the log:
			sf.clear();
			sf.push_back(obsScan);

			rawlog.insert(sf);
		}

		if ((i++ % 30) == 0)
		{
			if (!progDia.Update(i)) end = true;	 // Exit the loop
			wxTheApp->Yield();	// Let the app. process messages
		}
	}

	progDia.Update(n);

	// Time to erase the progress window.
	wxTheApp->Yield();

	// Update the views:
	rebuildTreeView();

	WX_END_TRY
}

// Import a sequence of images as a new rawlog
void xRawLogViewerFrame::OnImportSequenceOfImages(wxCommandEvent& event)
{
	WX_START_TRY

	if (rawlog.size())
		if (wxYES !=
			wxMessageBox(
				_("This will overwrite your currently loaded rawlog. "
				  "Proceed anyway?"),
				_("Import rawlog"), wxYES_NO, this))
			return;

	// Select directory:
	wxMessageBox(
		_("Please, select a directory which contain the images "
		  "(*.bmp,*.jpg,*.tif,*.png,...) to be imported\n They will be loaded "
		  "by filename ascending order."),
		_("Import rawlog"), wxOK, this);

	wxDirDialog dirDialog(
		this, _("Choose the directory that contains the images"), _("."), 0,
		wxDefaultPosition);

	if (dirDialog.ShowModal() != wxID_OK) return;
	string inDir(dirDialog.GetPath().mb_str());

	// Explore ir:
	CDirectoryExplorer::TFileInfoList lstFiles;
	CDirectoryExplorer::explore(inDir, FILE_ATTRIB_ARCHIVE, lstFiles);

	// Order by name:
	CDirectoryExplorer::sortByName(lstFiles);

	wxString strFPS = wxGetTextFromUser(
		_("Provide frame rate (fps, or Hz):"),
		_("To generate fake timestamps:"), _("10"));
	double FPS;
	strFPS.ToCDouble(&FPS);

	ASSERT_(FPS > 0);

	// Compute period:
	const auto At =
		std::chrono::microseconds(static_cast<uint64_t>((1e6 / FPS)));

	wxBusyCursor waitCursor;
	int nEntries = (int)lstFiles.size();
	size_t insertCount = 0;

	wxString auxStr;
	wxProgressDialog progDia(
		wxT("Progress"), wxT("Parsing files..."),
		nEntries,  // range
		this,  // parent
		wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
			wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);

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

	string errorMsg;
	rawlog.clear();

	TTimeStamp fakeTimeStamp = getCurrentTime();

	for (int countLoop = 0; countLoop < nEntries; countLoop++)
	{
		if (countLoop % 5 == 0)
		{
			auxStr.sprintf(wxT("Parsed %u files"), countLoop);
			if (!progDia.Update(countLoop, auxStr)) break;
			wxTheApp->Yield();	// Let the app. process messages
		}

		try
		{
			string filName = lstFiles[countLoop].wholePath;
			string filExt = extractFileExtension(filName);
			if (!os::_strcmpi("pgm", filExt.c_str()) ||
				!os::_strcmpi("jpg", filExt.c_str()) ||
				!os::_strcmpi("bmp", filExt.c_str()) ||
				!os::_strcmpi("png", filExt.c_str()) ||
				!os::_strcmpi("jpeg", filExt.c_str()) ||
				!os::_strcmpi("tif", filExt.c_str()) ||
				!os::_strcmpi("tiff", filExt.c_str()) ||
				!os::_strcmpi("dib", filExt.c_str()) ||
				!os::_strcmpi("jpe", filExt.c_str()) ||
				!os::_strcmpi("pbm", filExt.c_str()) ||
				!os::_strcmpi("ppm", filExt.c_str()) ||
				!os::_strcmpi("sr", filExt.c_str()) ||
				!os::_strcmpi("ras", filExt.c_str()))
			{
				// Add SF:
				CSensoryFrame::Ptr sf = std::make_shared<CSensoryFrame>();
				CObservationImage::Ptr im =
					std::make_shared<CObservationImage>();
				im->cameraPose = CPose3D(0, 0, 0);
				im->image.loadFromFile(filName);
				im->timestamp = fakeTimeStamp;

				// Default camera parameters:
				im->cameraParams.dist.fill(0);
				im->cameraParams.intrinsicParams.setZero();
				im->cameraParams.intrinsicParams(0, 0) = 300;  // fx
				im->cameraParams.intrinsicParams(1, 1) = 300;  // fy
				im->cameraParams.intrinsicParams(0, 2) =
					im->image.getWidth() / 2;
				im->cameraParams.intrinsicParams(1, 2) =
					im->image.getHeight() / 2;
				im->cameraParams.intrinsicParams(2, 2) = 1;

				sf->push_back(im);

				rawlog.insert(sf);

				// Add emppty action:
				CActionCollection::Ptr acts =
					std::make_shared<CActionCollection>();
				rawlog.insert(acts);

				// for the next step:
				fakeTimeStamp += At;
				insertCount++;
			}
		}
		catch (exception& e)
		{
			errorMsg = mrpt::exception_to_str(e);
			break;
		}
		catch (...)
		{
			break;
		}
	}  // end while keep loading

	progDia.Update(nEntries);

	wxMessageBox(
		(format(
			 "%u images have been imported to rawlog format",
			 (unsigned)insertCount)
			 .c_str()),
		_("Done"), wxOK, this);

	// Update the views:
	rebuildTreeView();

	WX_END_TRY
}

// Export rawlog as a MOOS' alog file:
void xRawLogViewerFrame::OnMenuExportALOG(wxCommandEvent& event)
{
	wxString caption = wxT("Export as ALOG...");
	wxString wildcard = wxT("ALOG files (*.alog)|*.alog|All files (*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename = (loadedFileName + string(".alog")).c_str();
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

	if (dialog.ShowModal() == wxID_OK)
	{
		// Ask for the robot name:
		wxString strRobot = wxGetTextFromUser(
			_("Enter the name of the robot to use in ALOG:"), _("Robot name:"),
			_("ROBOT"));

		string robot_name = string(strRobot.mb_str());

		wxString fil = dialog.GetPath();
		wxString filePath = dialog.GetDirectory();

		// Save the path
		WX_START_TRY

		iniFile->write(iniFileSect, "LastDir", string(filePath.mb_str()));

		// Save the file:
		string fileName = string(fil.mb_str());

		FILE* f = os::fopen(fileName.c_str(), "wt");
		if (!f) THROW_EXCEPTION("Cannot open output file for write.");

		int countLoop = 0, i, n = (int)rawlog.size();

		CPose2D globalOdometry(0, 0, 0);
		TTimeStamp firstTime = INVALID_TIMESTAMP;
		TTimeStamp lastTime = INVALID_TIMESTAMP;

		wxBusyCursor waitCursor;

		wxProgressDialog progDia(
			wxT("Exporting rawlog to ALOG"), wxT("Saving..."),
			n,	// range
			this,  // parent
			wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
				wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);

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

		for (i = 0; i < n; i++)
		{
			if (countLoop++ % 100 == 0)
			{
				if (!progDia.Update(i)) continue;  // Exit the loop
				wxTheApp->Yield();	// Let the app. process messages
			}

			// EXPORT
			// ---------------------
			if (rawlog.getType(i) == CRawlog::etActionCollection &&
				lastTime != INVALID_TIMESTAMP)
			{
				CActionCollection::Ptr acts = rawlog.getAsAction(i);

				// Have we odometry here?
				CActionRobotMovement2D::Ptr act =
					acts->getMovementEstimationByType(
						CActionRobotMovement2D::emOdometry);
				if (act)
				{
					globalOdometry =
						globalOdometry + act->poseChange->getMeanVal();

					// MOOS uses a different coordinate system:
					CPose2D MOOS_odometry(
						-globalOdometry.y(), globalOdometry.x(),
						globalOdometry.phi());

					double time_odo;
					if (act->timestamp != INVALID_TIMESTAMP)
						time_odo = timeDifference(firstTime, act->timestamp);
					else  // We must approximate it... :-S  ugly
						time_odo = timeDifference(firstTime, lastTime) + 0.01;

					::fprintf(
						f,
						"%f   %s_ODOMETRY   iPlatform    "
						"time=%f,Pose=[3x1]{%.03f,%.03f,%.03f}\n",
						time_odo, robot_name.c_str(), time_odo,
						MOOS_odometry.x(), MOOS_odometry.y(),
						MOOS_odometry.phi());
				}
			}
			else if (rawlog.getType(i) == CRawlog::etSensoryFrame)
			{
				CSensoryFrame::Ptr SF = rawlog.getAsObservations(i);

				// Any laser scan?
				CObservation2DRangeScan::Ptr obs =
					SF->getObservationByClass<CObservation2DRangeScan>();
				if (obs)
				{
					lastTime = obs->timestamp;
					if (firstTime == INVALID_TIMESTAMP) firstTime = lastTime;
					double time_obs = timeDifference(firstTime, lastTime);
					auto M_real = static_cast<unsigned int>(obs->getScanSize());
					ASSERT_(M_real == 361 || M_real == 181);

					::fprintf(
						f,
						"%f    LMS_LASER_2D     LMS2D  "
						"time=%f,elevation=0.000000,endElevation=0.000000,"
						"angRes=1.00,offset=0.00,subRange=0,minAngle=%.02f,"
						"maxAngle=%.02f,range=[%u]{",
						time_obs, time_obs, RAD2DEG(-obs->aperture / 2),
						RAD2DEG(obs->aperture / 2), 181);

					if (M_real == 361)
					{
						for (unsigned int j = 0; j < 181; j++)
						{
							unsigned int idx;
							if (j == 90) idx = 180;
							else if (j < 90)
								idx = 1 + 2 * j;
							else
								idx = 2 * j;

							float val = obs->getScanRangeValidity(idx)
								? obs->getScanRange(idx)
								: 0;
							if (j < (181 - 1)) ::fprintf(f, "%.03f,", val);
							else
								::fprintf(f, "%.03f}\n", val);
						}
					}
					else
					{
						for (unsigned int idx = 0; idx < 181; idx++)
						{
							float val = obs->getScanRangeValidity(idx)
								? obs->getScanRange(idx)
								: 0;
							if (idx < (181 - 1)) ::fprintf(f, "%.03f,", val);
							else
								::fprintf(f, "%.03f}\n", val);
						}
					}
				}
			}
			else
				THROW_EXCEPTION(
					"Export to ALOG only implemented for SF-based rawlogs.");

		}  // end for i

		progDia.Update(n);

		os::fclose(f);

		WX_END_TRY
	}
}

void xRawLogViewerFrame::OnImportRTL(wxCommandEvent& event)
{
	WX_START_TRY

	wxString caption = wxT("Import a RTL log file...");
	wxString wildcard = wxT("RTL files (*.rtl)|*.rtl|All files (*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename = _("*.rtl");
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_OPEN | wxFD_FILE_MUST_EXIST);

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

	// Save the path
	WX_START_TRY
	iniFile->write(
		iniFileSect, "LastDir", string(dialog.GetDirectory().mb_str()));
	WX_END_TRY

	wxString fileName = dialog.GetPath();
	string fil(fileName.mbc_str());

	// 1) Parse the entire file:
	size_t nLines = 0;
	size_t nLasers = 0;
	// Time -> data
	std::map<double, TAlogRecord> theAlog;

	{
		wxBusyCursor waitCursor;
		ifstream f(fil.c_str());
		string strLine;

		double init_timestamp = timestampTotime_t(getCurrentTime());
		// float	max_laser_range=49.5f;

		while (!f.eof())
		{
			std::getline(f, strLine);
			nLines++;

			double timestamp = 0;

			// Determine type:
			TAlogRecord newRecord;
			newRecord.type = -1;

			if (strLine.size() > 3)
			{
				if (strLine[0] == 'P')
				{
					//					size_t idx;
					//					if ((idx =
					// strLine.find("Max_scan_distance"))!=strLine.npos )
					//					{
					//						max_laser_range = atof(
					// strLine.c_str()
					//+
					// idx
					//+ strlen("Max_scan_distance") + 1 );
					//					}
					// else if
				}
				else if (strLine[0] == 'O')
				{
					vector<float> rawdata;
					parseGeneralVector(strLine, 2, rawdata);
					if (rawdata.size() == 4)
					{
						newRecord.type = 0;	 // OK, valid odometry

						timestamp = init_timestamp + rawdata[0];

						newRecord.data.resize(6);
						newRecord.data[0] = rawdata[1];
						newRecord.data[1] = rawdata[2];
						newRecord.data[2] = rawdata[3] - M_PI / 2;
					}
				}
				else if (strLine[0] == 'L')
				{
					newRecord.endElev = 0;
					newRecord.startElev = 0;

					vector<float> rawdata;
					parseGeneralVector(strLine, 2, rawdata);

					if (rawdata.size() > 10)
					{
						timestamp = init_timestamp + rawdata[0];

						size_t N = (rawdata.size() - 1) / 4;

						newRecord.data.clear();
						newRecord.data.reserve(N);

						for (size_t i = 0; i < N; i++)
						{
							float rng = sqrt(
								square(
									rawdata[1 + i * 4 + 2] -
									rawdata[1 + i * 4 + 0]) +
								square(
									rawdata[1 + i * 4 + 3] -
									rawdata[1 + i * 4 + 1]));
							newRecord.data.push_back(rng);
						}

						newRecord.type = 1;
						newRecord.label = "LASER_2D";

						nLasers++;
					}
				}

				// Add:
				if (newRecord.type >= 0)
				{
					// DO NOT OVERWRITE OTHER RECORDS! SHIFT TIMESTAMP A LITTLE
					auto itPrev = theAlog.begin();
					while (itPrev != theAlog.end())
					{
						timestamp += 0.0002;
						itPrev = theAlog.find(timestamp);
					}
					theAlog[timestamp] = newRecord;
				}
			}
		}
	}

	// Show summary:
	string summary;
	summary = format(
		"%u lines parsed, %u entries recognized\n", (unsigned)nLines,
		(unsigned)theAlog.size());
	//	summary += format("%u images\n", (unsigned) nImgs);
	summary += format("%u laser scans\n", (unsigned)nLasers);
	summary += string("The rawlog is being saved to a file now.");
	::wxMessageBox(summary.c_str());

	// Ask for target file:
	string target_filename;
	{
		caption = wxT("Save as...");
		wildcard =
			wxT("RawLog files (*.rawlog,*.rawlog.gz)|*.rawlog;*.rawlog.gz|All "
				"files (*.*)|*.*");
		defaultDir =
			(iniFile->read_string(iniFileSect, "LastDir", ".").c_str());
		defaultFilename = _("IMPORTED.rawlog");
		wxFileDialog dialog2(
			this, caption, defaultDir, defaultFilename, wildcard,
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

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

		target_filename = string(dialog2.GetPath().mbc_str());
	}

	string dir_for_images(".");

	// Create rawlog:
	saveImportedLogToRawlog(target_filename, theAlog, dir_for_images);

	WX_END_TRY
}

// [N]={X1,X2,...}
void parseMOOSVector(
	const std::string& str, size_t idx, vector<float>& outVector)
{
	ASSERT_(str[idx] == '[');

	unsigned N = atoi(str.c_str() + idx + 1);

	idx = str.find("]{", idx);
	ASSERT_(idx != string::npos);

	outVector.resize(N);

	idx += 2;  // idx points to the first number
	for (unsigned i = 0; i < N; i++)
	{
		outVector[i] = atof(str.c_str() + idx);

		if (i < (N - 1))
		{
			idx = str.find(",", idx + 1) + 1;
			ASSERT_(idx != string::npos);
		}
	}
}

// "X1,X2,..."
void parseGeneralVector(
	const std::string& str, size_t idx, vector<float>& outVector)
{
	outVector.clear();
	outVector.reserve(400);

	do
	{
		float v = atof(str.c_str() + idx);
		outVector.push_back(v);

		idx = str.find(" ", idx + 1);
		if (idx != str.npos) idx++;
	} while (idx != str.npos);
}

void xRawLogViewerFrame::OnMenuImportALOG(wxCommandEvent& event)
{
	WX_START_TRY

	wxString caption = wxT("Import a MOOS alog file...");
	wxString wildcard =
		wxT("MOOS alog files (*.alog)|*.alog|All files (*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename = _("*.alog");
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_OPEN | wxFD_FILE_MUST_EXIST);

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

	// Save the path
	WX_START_TRY
	iniFile->write(
		iniFileSect, "LastDir", string(dialog.GetDirectory().mb_str()));
	WX_END_TRY

	wxString fileName = dialog.GetPath();
	string fil(fileName.mbc_str());

	// 1) Parse the entire file:
	size_t nLines = 0;
	size_t nImgs = 0, nLasers = 0;
	// Time -> data
	std::map<double, TAlogRecord> theAlog;

	{
		wxBusyCursor waitCursor;
		ifstream f(fil.c_str());
		string strLine;

		while (!f.eof())
		{
			std::getline(f, strLine);
			nLines++;

			// Look for the real timestamp: "time=XXXX.XXX"
			double timestamp;
			size_t idx;
			idx = strLine.find("time=");
			// Determine type:
			TAlogRecord newRecord;
			newRecord.type = -1;

			if (idx != string::npos)
			{
				timestamp = atof(strLine.c_str() + idx + 5);

				if (string::npos != (idx = strLine.find("_ODOMETRY")))
				{
					if (string::npos != (idx = strLine.find("Pose=[3x1]{")))
					{
						newRecord.data.resize(6);
						if (3 ==
							sscanf(
								strLine.c_str() + idx + 11, "%f,%f,%f",
								&newRecord.data[0], &newRecord.data[1],
								&newRecord.data[2]))
						{
							if (string::npos !=
								(idx = strLine.find("Vel=[3x1]{")))
							{
								if (3 ==
									sscanf(
										strLine.c_str() + idx + 10, "%f,%f,%f",
										&newRecord.data[3], &newRecord.data[4],
										&newRecord.data[5]))
								{
									newRecord.type = 0;	 // OK, valid odometry
								}
							}
						}
					}
				}
				else if (
					string::npos != strLine.find("LMS_LASER_2D") ||
					string::npos != strLine.find("LMS_LASER_3D"))
				{
					if (string::npos != (idx = strLine.find("endElevation=")))
					{
						newRecord.endElev = atof(strLine.c_str() + idx + 13);

						if (string::npos != (idx = strLine.find("elevation=")))
						{
							newRecord.startElev =
								atof(strLine.c_str() + idx + 10);

							if (string::npos != (idx = strLine.find("range=")))
							{
								parseMOOSVector(
									strLine, idx + 6, newRecord.data);
								if (string::npos !=
									strLine.find("LMS_LASER_2D"))
								{
									newRecord.type = 1;
									newRecord.label = "LASER_2D";
								}
								else
								{
									newRecord.type = 2;
									newRecord.label = "LASER_3D";
								}
								nLasers++;
							}
						}
					}
				}

				// Add:
				if (newRecord.type >= 0)
				{
					// DO NOT OVERWRITE OTHER RECORDS! SHIFT TIMESTAMP A LITTLE
					auto itPrev = theAlog.begin();
					while (itPrev != theAlog.end())
					{
						timestamp += 0.0002;
						itPrev = theAlog.find(timestamp);
					}
					theAlog[timestamp] = newRecord;
				}
			}

			// Images are different:
			if (string::npos != strLine.find("CAMERA_GRAB"))
			{
				if (string::npos != (idx = strLine.find(".jpg")))
				{
					size_t idx_slash = strLine.rfind("/", idx);
					if (idx_slash != string::npos)
					{
						//  /XXXXXXX.jpg
						//  ^       ^
						newRecord.imgFile =
							strLine.substr(idx_slash + 1, idx + 3 - idx_slash);
						newRecord.type = 3;

						// Time:
						size_t idx_time = strLine.rfind("_", idx) + 1;
						if (string::npos != idx_time)
						{
							timestamp = atof(strLine.c_str() + idx_time);

							// Pan:
							size_t idx_pan = strLine.find("Pan=");
							if (idx_pan != string::npos)
							{
								newRecord.data.resize(1);
								newRecord.data[0] = DEG2RAD(
									atof(strLine.c_str() + idx_pan + 4));

								// DO NOT OVERWRITE OTHER RECORDS! SHIFT
								// TIMESTAMP A LITTLE
								auto itPrev = theAlog.begin();
								while (itPrev != theAlog.end())
								{
									timestamp += 0.0002;
									itPrev = theAlog.find(timestamp);
								}

								newRecord.label = "CAMERA";

								theAlog[timestamp] = newRecord;
								nImgs++;
							}
						}
					}
				}
			}
		}
	}

	// Show summary:
	string summary;
	summary = format(
		"%u lines parsed, %u entries recognized\n", (unsigned)nLines,
		(unsigned)theAlog.size());
	summary += format("%u images\n", (unsigned)nImgs);
	summary += format("%u laser scans\n", (unsigned)nLasers);
	summary += string("The rawlog is being saved to a file now.");
	::wxMessageBox(summary.c_str());

	// Ask for target file:
	string target_filename;
	{
		caption = wxT("Save as...");
		wildcard =
			wxT("RawLog files (*.rawlog,*.rawlog.gz)|*.rawlog;*.rawlog.gz|All "
				"files (*.*)|*.*");
		defaultDir =
			(iniFile->read_string(iniFileSect, "LastDir", ".").c_str());
		defaultFilename = _("IMPORTED.rawlog");
		wxFileDialog dialog2(
			this, caption, defaultDir, defaultFilename, wildcard,
			wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

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

		target_filename = string(dialog2.GetPath().mbc_str());
	}

	// Ask for the directory of images:
	string dir_for_images(".");
	if (nImgs)
	{
		::wxMessageBox(
			_("The alog contains images. Please pick next the directory "
			  "containing all the images."));
		wxDirDialog dirDialog(
			this,
			_("Choose the directory containing the image files from the log"),
			(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()), 0,
			wxDefaultPosition);

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

		dir_for_images = string(dirDialog.GetPath().mb_str());
	}
	else
	{
		// No images, but perhaps they are in a separate directory??
		if (wxYES ==
			wxMessageBox(
				_("No images found in the alog. Do you want to "
				  "provide an additional directory with image files?"),
				_("Additional images?"), wxYES_NO))
		{
			wxDirDialog dirDialog(
				this,
				_("Choose the directory containing the image files from "
				  "the log"),
				(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()), 0,
				wxDefaultPosition);

			if (dirDialog.ShowModal() == wxID_OK)
			{
				dir_for_images = string(dirDialog.GetPath().mb_str());

				// Look for image files:
				CDirectoryExplorer::TFileInfoList lstFiles;
				CDirectoryExplorer::explore(
					dir_for_images, FILE_ATTRIB_ARCHIVE, lstFiles);

				CDirectoryExplorer::filterByExtension(lstFiles, "jpg");
				CDirectoryExplorer::sortByName(lstFiles);

				for (auto& lstFile : lstFiles)
				{
					// string strLine( it->wholePath );
					TAlogRecord newRecord;
					newRecord.imgFile = lstFile.name;
					newRecord.type = 3;

					// Time:
					size_t idx_time = lstFile.name.rfind("_") + 1;
					if (string::npos != idx_time)
					{
						double timestamp =
							atof(lstFile.name.c_str() + idx_time);

						// Pan:
						size_t idx_pan = lstFile.name.find("Motion");
						if (idx_pan != string::npos)
						{
							newRecord.data.resize(1);
							newRecord.data[0] = DEG2RAD(
								atof(lstFile.name.c_str() + idx_pan + 6));

							// DO NOT OVERWRITE OTHER RECORDS! SHIFT TIMESTAMP A
							// LITTLE
							auto itPrev = theAlog.begin();
							while (itPrev != theAlog.end())
							{
								timestamp += 0.0002;
								itPrev = theAlog.find(timestamp);
							}

							newRecord.label = "CAMERA";

							theAlog[timestamp] = newRecord;
							nImgs++;
						}
					}
				}
			}
		}
	}

	// Create rawlog:
	saveImportedLogToRawlog(target_filename, theAlog, dir_for_images);

	WX_END_TRY
}

void xRawLogViewerFrame::saveImportedLogToRawlog(
	const std::string& target_filename,
	const std::map<double, TAlogRecord>& theAlog,
	const std::string& dir_for_images)
{
	WX_START_TRY

	wxBusyCursor waitCursor;

	CFileGZOutputStream filOut(target_filename);

	CPose2D lastOdometry;
	bool firstOdo = true;
	TTimeStamp firstSFtime = INVALID_TIMESTAMP;

	CActionRobotMovement2D::TMotionModelOptions odoOpts;

	CSensoryFrame sfAccum;

	size_t progress_i, progress_N = theAlog.size();

	wxProgressDialog progDia(
		wxT("Importing rawlog"), wxT("Importing..."),
		progress_N,	 // range
		this,  // parent
		wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
			wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);

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

	std::map<double, TAlogRecord>::const_iterator it;

	for (progress_i = 0, it = theAlog.begin(); it != theAlog.end(); it++)
	{
		if (progress_i++ % 50 == 0)
		{
			if (!progDia.Update((int)progress_i)) break;
			wxTheApp->Yield();	// Let the app. process messages
		}

		TTimeStamp tim = time_tToTimestamp(it->first);

		if (it->second.type != 0)
		{
			// is an observation:
			CObservation::Ptr newObs;

			// Create object:
			switch (it->second.type)
			{
				case 1:
				case 2:
				{
					// ----------------
					// 2D/3D LASER
					// ----------------
					CObservation2DRangeScan::Ptr obs =
						std::make_shared<CObservation2DRangeScan>();
					newObs = obs;

					obs->aperture = M_PIf;
					obs->rightToLeft = true;
					obs->resizeScan(it->second.data.size());
					for (size_t i = 0; i < it->second.data.size(); i++)
					{
						float v = it->second.data[i];
						bool valid = true;
						if (v < 0 || fabs(v - 81.19) < 0.01 ||
							fabs(v - 8.191) < 0.01)
						{
							v = .0f;
							valid = false;
						}
						obs->setScanRange(i, v);
						obs->setScanRangeValidity(i, valid);
					}
					obs->deltaPitch =
						-DEG2RAD(it->second.endElev - it->second.startElev);

					obs->sensorPose.setFromValues(
						0, 0, 0, 0, -DEG2RAD(it->second.startElev), 0);
				}
				break;

				case 3:
				{
					// ----------------
					//  Img:
					// ----------------
					string img_file =
						dir_for_images + string("/") + it->second.imgFile;

					if (fileExists(img_file))
					{
						CObservationImage::Ptr obs =
							std::make_shared<CObservationImage>();
						newObs = obs;

						obs->cameraPose.setFromValues(
							0, 0, 0, it->second.data[0], 0, 0);

						// obs->image.loadFromFile( img_file );
						obs->image.setExternalStorage(it->second.imgFile);
					}
					else
						cerr << "Image file does not exist: " << img_file
							 << endl;
				}
				break;
			};

			// Add it:
			if (newObs)
			{
				newObs->timestamp = tim;
				newObs->sensorLabel = it->second.label;

				if (!sfAccum.size()) firstSFtime = tim;

				sfAccum.insert(newObs);

				// Too many consecutive observations?
				if (firstSFtime != INVALID_TIMESTAMP &&
					timeDifference(firstSFtime, tim) > 0.3)
				{
					archiveFrom(filOut) << sfAccum;
					sfAccum.clear();
					firstSFtime = INVALID_TIMESTAMP;
				}
			}
		}
		else
		{
			// First, flush observations:
			if (sfAccum.size())
			{
				archiveFrom(filOut) << sfAccum;
				sfAccum.clear();
				firstSFtime = INVALID_TIMESTAMP;
			}

			// Is odometry
			CActionRobotMovement2D act;
			CPose2D Aodo(0, 0, 0);

			CPose2D curOdo(
				it->second.data[0], it->second.data[1],
				it->second.data[2] + M_PI / 2);

			if (firstOdo) { firstOdo = false; }
			else
			{
				Aodo = curOdo - lastOdometry;
			}

			act.computeFromOdometry(Aodo, odoOpts);
			act.timestamp = tim;

			CActionCollection acts;
			acts.insert(act);

			archiveFrom(filOut) << acts;

			lastOdometry = curOdo;
		}
	}  // end for each alog entry

	progDia.Update(progress_N);

	WX_END_TRY
}

void xRawLogViewerFrame::OnGenGasTxt(wxCommandEvent& event)
{
	WX_START_TRY

	wxString caption = wxT("Save as...");
	wxString wildcard = wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename =
		(loadedFileName + string("_gasSensors.txt")).c_str();
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

	if (dialog.ShowModal() == wxID_OK)
	{
		wxString fileName = dialog.GetPath();
		string fil(fileName.mbc_str());

		size_t i, M = 0, n = rawlog.size();
		FILE* f = os::fopen(fil.c_str(), "wt");
		if (!f) THROW_EXCEPTION("Cannot open output file for write.");

		bool genTimes = rawlog_first_timestamp != INVALID_TIMESTAMP;
		if (!genTimes)
			::wxMessageBox(
				_("It seems that there are no valid timestamps in the rawlog. "
				  "Timestamps will be zero."));

		for (i = 0; i < n; i++)
		{
			CObservationGasSensors::Ptr obs;

			switch (rawlog.getType(i))
			{
				case CRawlog::etSensoryFrame:
				{
					CSensoryFrame::Ptr sf = rawlog.getAsObservations(i);
					obs = sf->getObservationByClass<CObservationGasSensors>();
				}
				break;

				case CRawlog::etObservation:
				{
					CObservation::Ptr o =
						rawlog.getAsObservation(i);	 // get the observation
					if (IS_CLASS(*o, CObservationGasSensors))
					{
						obs = std::dynamic_pointer_cast<CObservationGasSensors>(
							o);	 // Get the GAS observation
					}
				}
				break;

				default: continue;
			}  // end-case

			// If we have a GAS obs, then process it:
			if (obs)
			{
				// Timestamp:
				double t = 0;
				if (genTimes)
				{
					if (obs->timestamp != INVALID_TIMESTAMP)
						t = mrpt::system::timeDifference(
							rawlog_first_timestamp, obs->timestamp);
				}

				::fprintf(f, "%f ", t);

				size_t lineCount = 0;

				// run each Enose (if more than one)
				for (auto& m_reading : obs->m_readings)
				{
					// Temperature
					if (m_reading.hasTemperature == true)
					{
						float temp = m_reading.temperature;
						::fprintf(f, "%f ", temp);
					}

					// Run each sensor on Enose
					for (auto it = m_reading.readingsVoltage.begin();
						 it != m_reading.readingsVoltage.end(); ++it)
					{
						::fprintf(f, "%f ", *it);
						lineCount++;
					}
				}

				// Fill:
				while (lineCount < 32)
				{
					::fprintf(f, "0 ");
					lineCount++;
				}

				::fprintf(f, "\n");
				M++;
			}  // end-if(obs)

		}  // end-for

		os::fclose(f);

		char auxStr[100];
		os::sprintf(auxStr, sizeof(auxStr), "%u entries saved!", (unsigned)M);
		wxMessageBox(auxStr, _("Done"), wxOK, this);
	}  // end-if

	WX_END_TRY
}

void xRawLogViewerFrame::OnGenWifiTxt(wxCommandEvent& event)
{
	WX_START_TRY

	wxString caption = wxT("Save as...");
	wxString wildcard = wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename =
		(loadedFileName + string("_wifiSensors.txt")).c_str();
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

	if (dialog.ShowModal() == wxID_OK)
	{
		wxString fileName = dialog.GetPath();
		string fil(fileName.mbc_str());

		size_t i, M = 0, n = rawlog.size();
		FILE* f = os::fopen(fil.c_str(), "wt");
		if (!f) THROW_EXCEPTION("Cannot open output file for write.");

		bool genTimes = rawlog_first_timestamp != INVALID_TIMESTAMP;
		if (!genTimes)
			::wxMessageBox(
				_("It seems that there are no valid timestamps in the rawlog. "
				  "Timestamps will be zero."));

		for (i = 0; i < n; i++)
		{
			CObservationWirelessPower::Ptr obs;

			switch (rawlog.getType(i))
			{
				case CRawlog::etSensoryFrame:
				{
					CSensoryFrame::Ptr sf = rawlog.getAsObservations(i);
					obs =
						sf->getObservationByClass<CObservationWirelessPower>();
				}
				break;

				case CRawlog::etObservation:
				{
					CObservation::Ptr o =
						rawlog.getAsObservation(i);	 // get the observation
					if (IS_CLASS(*o, CObservationWirelessPower))
					{
						obs = std::dynamic_pointer_cast<
							CObservationWirelessPower>(
							o);	 // Get the GAS observation
					}
				}
				break;

				default: continue;
			}  // end-case

			// If we have a WIFI obs, then process it:
			if (obs)
			{
				// Timestamp:
				double t = 0;
				if (genTimes)
				{
					if (obs->timestamp != INVALID_TIMESTAMP)
						t = mrpt::system::timeDifference(
							rawlog_first_timestamp, obs->timestamp);
				}

				::fprintf(f, "%f ", t);

				size_t lineCount = 0;

				// run each Enose (if more than one)
				/*	for (size_t j=0;j<obs->m_readings.size();j++)
					{
						//Temperature
						if (obs->m_readings[j].hasTemperature== true)
						{
							float temp=obs->m_readings[j].temperature;
							::fprintf(f,"%f ",temp);
						}

						//Run each sensor on Enose
						for (vector<float>::iterator
				   it=obs->m_readings[j].readingsVoltage.begin();it!=obs->m_readings[j].readingsVoltage.end();++it)
						{
							::fprintf(f,"%f ",*it);
							lineCount++;
						}
					}*/

				// Fill:
				while (lineCount < 32)
				{
					::fprintf(f, "0 ");
					lineCount++;
				}

				::fprintf(f, "\n");
				M++;
			}  // end-if(obs)

		}  // end-for

		os::fclose(f);

		char auxStr[100];
		os::sprintf(auxStr, sizeof(auxStr), "%u entries saved!", (unsigned)M);
		wxMessageBox(auxStr, _("Done"), wxOK, this);
	}  // end-if

	WX_END_TRY
}

void xRawLogViewerFrame::OnGenRFIDTxt(wxCommandEvent& event)
{
	WX_START_TRY

	wxString caption = wxT("Save as...");
	wxString wildcard = wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename =
		(loadedFileName + string("_RFIDSensors.txt")).c_str();
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

	if (dialog.ShowModal() == wxID_OK)
	{
		wxString fileName = dialog.GetPath();
		string fil(fileName.mbc_str());

		size_t i, M = 0, n = rawlog.size();
		FILE* f = os::fopen(fil.c_str(), "wt");
		if (!f) THROW_EXCEPTION("Cannot open output file for write.");

		bool genTimes = rawlog_first_timestamp != INVALID_TIMESTAMP;
		if (!genTimes)
			::wxMessageBox(
				_("It seems that there are no valid timestamps in the rawlog. "
				  "Timestamps will be zero."));

		for (i = 0; i < n; i++)
		{
			CObservationRFID::Ptr obs;

			switch (rawlog.getType(i))
			{
				case CRawlog::etSensoryFrame:
				{
					CSensoryFrame::Ptr sf = rawlog.getAsObservations(i);
					obs = sf->getObservationByClass<CObservationRFID>();
				}
				break;

				case CRawlog::etObservation:
				{
					CObservation::Ptr o =
						rawlog.getAsObservation(i);	 // get the observation
					if (IS_CLASS(*o, CObservationRFID))
					{
						obs = std::dynamic_pointer_cast<CObservationRFID>(
							o);	 // Get the GAS observation
					}
				}
				break;

				default: continue;
			}  // end-case

			// If we have a RFID obs, then process it:
			if (obs)
			{
				// Timestamp:
				double t = 0;
				if (genTimes)
				{
					if (obs->timestamp != INVALID_TIMESTAMP)
						t = mrpt::system::timeDifference(
							rawlog_first_timestamp, obs->timestamp);
				}

				::fprintf(f, "%f ", t);

				size_t lineCount = 0;

				// run each Enose (if more than one)
				/*	for (size_t j=0;j<obs->m_readings.size();j++)
					{
						//Temperature
						if (obs->m_readings[j].hasTemperature== true)
						{
							float temp=obs->m_readings[j].temperature;
							::fprintf(f,"%f ",temp);
						}

						//Run each sensor on Enose
						for (vector<float>::iterator
				   it=obs->m_readings[j].readingsVoltage.begin();it!=obs->m_readings[j].readingsVoltage.end();++it)
						{
							::fprintf(f,"%f ",*it);
							lineCount++;
						}
					}*/

				// Fill:
				while (lineCount < 32)
				{
					::fprintf(f, "0 ");
					lineCount++;
				}

				::fprintf(f, "\n");
				M++;
			}  // end-if(obs)

		}  // end-for

		os::fclose(f);

		char auxStr[100];
		os::sprintf(auxStr, sizeof(auxStr), "%u entries saved!", (unsigned)M);
		wxMessageBox(auxStr, _("Done"), wxOK, this);
	}  // end-if

	WX_END_TRY
}

// --------------------------------------------------------------------------------
// Datasets and docs from these nice guys:
//  http://www.informatik.uni-bremen.de/agebv/en/DlrSpatialCognitionDataSet
// --------------------------------------------------------------------------------
void xRawLogViewerFrame::OnMenuItemImportBremenDLRLog(wxCommandEvent& event)
{
	WX_START_TRY

	if (rawlog.size())
		if (wxYES !=
			wxMessageBox(
				_("This will overwrite your currently loaded rawlog. "
				  "Proceed anyway?"),
				_("Import rawlog"), wxYES_NO, this))
			return;

	wxString caption = wxT("Import a uni-bremen DLR dataset...");
	wxString wildcard =
		wxT("Uni-bremen DLR circles dataset (*.circles)|*.circles|All files "
			"(*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename = _("*.circles");
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_OPEN | wxFD_FILE_MUST_EXIST);
	if (dialog.ShowModal() != wxID_OK) return;
	// Save the path
	WX_START_TRY
	iniFile->write(
		iniFileSect, "LastDir", string(dialog.GetDirectory().mb_str()));
	WX_END_TRY

	// File to import:
	const wxString sFile = dialog.GetPath();
	string import_filename(sFile.mbc_str());

	/* --------------------------------------------------------------------------------
	   As I say above, see:
	http://www.informatik.uni-bremen.de/agebv/en/DlrSpatialCognitionDataSet

	   Sample lines:

	  CIRCLEMARKDETECTOR dlr-spatial_cognition-c.cam nohalf 0.10000 0.50000
	  CIRCLEMARKDETECTOR <cameraCalibrationFile> <useHalfImages> <radius>
	<threshold>

	Defines the parameters for the circular fiducial detector.
	   <cameraCalibrationFile> is the file containing the camera calibration
	   with respect to the floor plane that is used for finding the fiducials
	   and converting the image location to a metrical location.

	   <useHalfImages> If it is 'half' the camera calibration refers to a full
	   frame image and it's y resolution must be divided by two to match the
	images.

	   <radius> metrical radius of the fiducials (meter)

	   <threshold> the visual fiducial detector assigns a quality measure
	between [0..1]
	   to every fiducial found. Only those above <threshold> are passed to the
	SLAM algorithm.

	  STEP dlr-spatial_cognition-c.0000 -0.03752 -0.10467 -0.10807
	0.000014396673 -0.000000184001 0.000013237216 0.000015157225 -0.000001814284
	0.000246181200
	  STEP <image> <dX> <dY> <dTheta>, <cXX>, <cXY>, <cYY>, <cXTheta>,
	<cYTheta>, <cThetaTheta>

	Defines a single step of the robot trajectory, that is an odometry
	measurement plus an image. Such
	a line defines, that the robot first moved by <dX>, <dY>, <dTheta> relative
	to it's previous pose and
	then took the image saved in <image>. <cXX>...<cThetaTheta> are the
	covariances of <dX>, <dY>, <dTheta>.
	Units are m and radian.

	   --------------------------------------------------------------------------------
	*/

	// Create a new dataset:
	CRawlog newRawlog;

	const bool use_SF_format =
		(wxYES ==
		 wxMessageBox(
			 _("Use Actions-SensoryFrames format (YES) or the "
			   "Observation-only format (NO)?"),
			 _("Import rawlog"), wxYES_NO, this));

	const bool use_ground_truth_IDs =
		(wxYES ==
		 wxMessageBox(
			 _("Employ landmark IDs in file (YES) or make the sensor "
			   "unable to identify any landmarks (NO)?"),
			 _("Import rawlog"), wxYES_NO, this));

	// Parse line by line:
	mrpt::io::CTextFileLinesParser fileParser(import_filename);
	std::string line;

	mrpt::system::TTimeStamp cur_timestamp = mrpt::system::now();
	using namespace std::chrono_literals;
	const auto time_steps = 250ms;

	loadedFileName =
		import_filename + string(use_SF_format ? ".rawlog" : ".obs.rawlog");

	wxBusyCursor wait;

	CSensoryFrame set_of_obs;  // Set of observations after the last odometry;
	// saved BEFORE processing the next odometry.

	// For use only in Observations-only format:
	CObservationOdometry obs_odo;
	obs_odo.sensorLabel = "ODOMETRY";
	obs_odo.odometry = CPose2D(0, 0, 0);

	// For stats on covariance transforms:
	CVectorDouble stats_stdRanges, stats_stdYaw;
	CVectorDouble stats_stdXs, stats_stdYs;

	int32_t next_outlier_ID = 10000;  // In DLR datasets no real landmark has
	// such a high ID... use these numbers for
	// outliers.

	while (fileParser.getNextLine(line))
	{
		std::vector<std::string> words;
		mrpt::system::tokenize(line, " \t", words);
		if (words.empty()) continue;

		if (words[0] == "STEP")
		{
			cur_timestamp += time_steps;  // Increment time counter

			// First, do we have some queued obs?
			if (!set_of_obs.empty())
			{
				if (use_SF_format) { newRawlog.insert(set_of_obs); }
				else
				{
					for (size_t i = 0; i < set_of_obs.size(); i++)
						newRawlog.insert(set_of_obs.getObservationByIndex(i));
				}
				set_of_obs.clear();
			}

			if (set_of_obs.empty())
			{
				// Always create an range-bearing observation, for the cases of
				// images without any detected landmark
				//  so we have the observation, even if it's empty:
				// Create upon first landmark:
				CObservationBearingRange::Ptr obs =
					std::make_shared<CObservationBearingRange>();
				obs->sensorLabel = "CIRCLE_DETECTOR";
				obs->timestamp = cur_timestamp;
				obs->minSensorDistance = 0;
				obs->maxSensorDistance = 100;
				obs->sensor_std_yaw = 1e-4f;
				obs->sensor_std_range = 1e-2f;
				obs->sensor_std_pitch = 0;	// Is a 2D sensor
				obs->fieldOfView_pitch = 0;
				obs->fieldOfView_yaw = 180.0_deg;
				obs->validCovariances =
					true;  // Each observation has its own cov. matrix.

				set_of_obs.insert(obs);
			}

			ASSERT_GE_(words.size(), 11);
			// Process STEP entries (odometry increments)
			//  STEP dlr-spatial_cognition-c.0000 -0.03752 -0.10467 -0.10807
			//  0.000014396673 -0.000000184001 0.000013237216 0.000015157225
			//  -0.000001814284 0.000246181200
			//  STEP <image> <dX> <dY> <dTheta>, <cXX>, <cXY>, <cYY>, <cXTheta>,
			//  <cYTheta>, <cThetaTheta>

			// Add the image to be inserted before the next odometry entry:
			CObservationImage::Ptr newImg =
				std::make_shared<CObservationImage>();
			newImg->timestamp = cur_timestamp;
			newImg->sensorLabel = "CAMERA";
			newImg->image.setExternalStorage(words[1] + std::string(".jpg"));
			// MRPT_TODO("Camera params, ...")
			// newImg->cameraParams
			set_of_obs.insert(newImg);

			const double odo_dx = atof(words[2].c_str());
			const double odo_dy = atof(words[3].c_str());
			const double odo_dphi = atof(words[4].c_str());

			const CPose2D odoIncr(-odo_dy, odo_dx, odo_dphi);

			CActionRobotMovement2D act_mov;
			CActionRobotMovement2D::TMotionModelOptions odoParams;
			odoParams.modelSelection = CActionRobotMovement2D::mmGaussian;
			odoParams.gaussianModel.a1 = odoParams.gaussianModel.a2 =
				odoParams.gaussianModel.a3 = odoParams.gaussianModel.a4 = 0;
			odoParams.gaussianModel.minStdXY =
				std::sqrt(atof(words[5].c_str()) + atof(words[7].c_str()));
			odoParams.gaussianModel.minStdPHI =
				std::sqrt(atof(words[10].c_str()));

			act_mov.computeFromOdometry(odoIncr, odoParams);
			act_mov.timestamp = cur_timestamp;

			if (use_SF_format)
			{
				CActionCollection acts;
				acts.insert(act_mov);
				newRawlog.insert(acts);
			}
			else
			{
				obs_odo.timestamp = cur_timestamp;
				obs_odo.odometry += odoIncr;
				newRawlog.insert(
					std::make_shared<CObservationOdometry>(obs_odo));
			}
		}
		else if (words[0] == "LANDMARK_C")
		{
			ASSERT_GE_(words.size(), 8);
			// LANDMARK_C 0.67972 -2.87676 0.600235 0.00176143 -0.000314459
			// 0.00334762 -1
			// #  LANDMARK <pX> <pY> <quality> <cXX> <cXY> <cYY> <ID>

			// Just append this measure to the observation within "set_of_obs":

			CObservationBearingRange::Ptr obs =
				set_of_obs.getObservationByClass<CObservationBearingRange>();
			ASSERT_(obs);

			// <pX> <pY> <quality> <cXX> <cXY> <cYY> <ID>
			const double lm_x =
				-atof(words[2].c_str());  // Ey! Yes, I flipped the coord.
			// system to match MRPT's standard
			const double lm_y = atof(
				words[1].c_str());	//  of +X pointing fordward, +Y to the left.

			const double r = std::sqrt(square(lm_x) + square(lm_y));
			const double a = atan2(lm_y, lm_x);

			/* Transform covariance from (x,y) -> (r,a):
			 *   Jacobian:
			 *   [ x/(x^2 + y^2)^(1/2), y/(x^2 + y^2)^(1/2)]  // range
			 *   [      -y/(x^2 + y^2),       x/(x^2 + y^2)]  // yaw
			 */
			const double Cxy_xx = atof(words[4].c_str());
			const double Cxy_xy = atof(words[5].c_str());
			const double Cxy_yy = atof(words[6].c_str());

			const double dat_C_xy[3 * 3] = {Cxy_xx, Cxy_xy, 0, Cxy_xy, Cxy_yy,
											0,		0,		0, 0};
			const CMatrixDouble33 C_xy(dat_C_xy);

			// Jacobian:
			const double dat_H[3 * 3] = {
				lm_x / r, lm_y / r, 0, -lm_y / (r * r), lm_x / (r * r), 0,
				0,		  0,		0};
			const CMatrixDouble33 H(dat_H);

			// Transform covariance with Jacobian:
			const CMatrixDouble33 C_ypr = mrpt::math::multiply_HCHt(H, C_xy);

			// Create obs:
			CObservationBearingRange::TMeasurement meas;
			if (use_ground_truth_IDs)
			{
				const int32_t ID_in_file = atoi(words[7].c_str());
				if (ID_in_file < 0)
				{
					// If the ground-truth says this landmark is an outlier
					// (ID=-1), then
					//  assign a unique ID in a range that will not collide with
					//  real landmarks:
					meas.landmarkID = next_outlier_ID++;
				}
				else
				{
					meas.landmarkID = ID_in_file;
				}
			}
			else
			{
				meas.landmarkID = INVALID_LANDMARK_ID;	// Unknown IDs
			}

			meas.range = r;
			meas.yaw = a;
			meas.pitch = 0;
			meas.covariance = C_ypr;

			obs->sensedData.push_back(meas);

			// gather stats:
			stats_stdRanges.push_back(std::sqrt(C_ypr(0, 0)));
			stats_stdYaw.push_back(std::sqrt(C_ypr(1, 1)));
			stats_stdXs.push_back(std::sqrt(C_xy(0, 0)));
			stats_stdYs.push_back(std::sqrt(C_xy(1, 1)));
		}
	};

	// finally, we have to repeat this part not to lost the last few
	// observations:
	if (!set_of_obs.empty())
	{
		if (use_SF_format) { newRawlog.insert(set_of_obs); }
		else
		{
			for (size_t i = 0; i < set_of_obs.size(); i++)
				newRawlog.insert(set_of_obs.getObservationByIndex(i));
		}
	}

	wxTheApp->Yield();
	// Update the views:
	rawlog = newRawlog;	 // Load into GUI
	rebuildTreeView();

	// Extra info, to the console:
	if (stats_stdRanges.size() != 0)
	{
		cout << "[ImportBremenDLRLog] Stats for: " << import_filename << endl;

		double stdRange_mean, stdRange_std;
		double stdYaw_mean, stdYaw_std;
		mrpt::math::meanAndStd(stats_stdRanges, stdRange_mean, stdRange_std);
		mrpt::math::meanAndStd(stats_stdYaw, stdYaw_mean, stdYaw_std);
		cout << "[ImportBremenDLRLog] Computed stdRange (m): mean= "
			 << stdRange_mean << " std= " << stdRange_std << endl;
		cout << "[ImportBremenDLRLog] Computed stdYaw (deg): mean= "
			 << RAD2DEG(stdYaw_mean) << " std= " << RAD2DEG(stdYaw_std) << endl;

		double stdX_mean, stdX_std;
		double stdY_mean, stdY_std;
		mrpt::math::meanAndStd(stats_stdXs, stdX_mean, stdX_std);
		mrpt::math::meanAndStd(stats_stdYs, stdY_mean, stdY_std);
		cout << "[ImportBremenDLRLog] Computed stdX (m): mean= " << stdX_mean
			 << " std= " << stdX_std << endl;
		cout << "[ImportBremenDLRLog] Computed stdY (m): mean= " << stdY_mean
			 << " std= " << stdY_std << endl;
	}

	WX_END_TRY
}

void xRawLogViewerFrame::OnGenerateIMUTextFile(wxCommandEvent& event)
{
	WX_START_TRY

	wxString caption = wxT("Save as...");
	wxString wildcard = wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename = (loadedFileName + string("_IMU.txt")).c_str();
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

	if (dialog.ShowModal() == wxID_OK)
	{
		wxString fileName = dialog.GetPath();
		string fil(fileName.mbc_str());

		int i, M = 0, n = (int)rawlog.size();
		FILE* f = os::fopen(fil.c_str(), "wt");
		if (!f) THROW_EXCEPTION("Cannot open output file for write.");

		::fprintf(
			f,
			"%% TIMESTAMP IMU_X_ACC IMU_Y_ACC IMU_Z_ACC IMU_YAW_VEL "
			"IMU_PITCH_VEL IMU_ROLL_VEL IMU_X_VEL IMU_Y_VEL IMU_Z_VEL IMU_YAW "
			"IMU_PITCH IMU_ROLL IMU_X IMU_Y IMU_Z MAG_X MAG_Y MAG_Z PRESS "
			"ALTIT TEMP\n"
			"%% "
			"------------------------------------------------------------------"
			"------------------------------------------------------------------"
			"---------------------\n");

		for (i = 0; i < n; i++)
		{
			switch (rawlog.getType(i))
			{
				default: break;

				case CRawlog::etSensoryFrame:
				{
					CSensoryFrame::Ptr sf = rawlog.getAsObservations(i);
					size_t k = 0;
					CObservationIMU::Ptr obs;
					do
					{
						obs = sf->getObservationByClass<CObservationIMU>(k++);
						if (obs)
						{
							// For each entry in this sequence:
							if (obs->rawMeasurements.size() > 0)
							{
								ASSERT_(
									obs->dataIsPresent.size() ==
									obs->rawMeasurements.size());
								size_t nValuesPerRow =
									obs->dataIsPresent.size();

								// For each entry in this sequence: Compute the
								// timestamp and save all 15 values:
								ASSERT_(obs->timestamp != INVALID_TIMESTAMP);
								TTimeStamp t = obs->timestamp;

								// double 	sampleTime = timeDifference(
								// rawlog_first_timestamp ,t);
								double sampleTime = timestampTotime_t(t);

								// Time:
								::fprintf(f, "%f ", sampleTime);
								ASSERT_(
									obs->rawMeasurements.size() ==
									obs->rawMeasurements.size());
								for (size_t idx = 0; idx < nValuesPerRow; idx++)
									::fprintf(
										f, "%f ",
										obs->dataIsPresent[idx]
											? obs->rawMeasurements[idx]
											: 0);
								::fprintf(f, "\n");
								M++;
							}  // end if
						}  // end if
					} while (obs);
				}
				break;

				case CRawlog::etObservation:
				{
					CObservation::Ptr oo = rawlog.getAsObservation(i);

					if (oo->GetRuntimeClass() == CLASS_ID(CObservationIMU))
					{
						CObservationIMU::Ptr obs =
							std::dynamic_pointer_cast<CObservationIMU>(oo);
						// For each entry in this sequence:
						if (obs->rawMeasurements.size() > 0)
						{
							ASSERT_(
								obs->dataIsPresent.size() ==
								obs->rawMeasurements.size());
							size_t nValuesPerRow = obs->dataIsPresent.size();

							// For each entry in this sequence: Compute the
							// timestamp and save all 15 values:
							ASSERT_(obs->timestamp != INVALID_TIMESTAMP);
							TTimeStamp t = obs->timestamp;

							double sampleTime = timestampTotime_t(
								t);	 // timeDifference(rawlog_first_timestamp,t);

							// Time:
							::fprintf(f, "%f ", sampleTime);
							ASSERT_(
								obs->rawMeasurements.size() ==
								obs->rawMeasurements.size());
							for (size_t idx = 0; idx < nValuesPerRow; idx++)
								::fprintf(
									f, "%f ",
									obs->dataIsPresent[idx]
										? obs->rawMeasurements[idx]
										: 0);
							::fprintf(f, "\n");
							M++;
						}  // end if
					}
				}
				break;

			}  // end switch.
		}

		os::fclose(f);

		wxMessageBox(
			(format("%u IMU data entries saved!", M).c_str()), _("Done"), wxOK,
			this);
	}

	WX_END_TRY
}

void xRawLogViewerFrame::OnGenerateTextFileRangeBearing(wxCommandEvent& event)
{
	WX_START_TRY

	wxString caption = wxT("Save as...");
	wxString wildcard = wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*");
	wxString defaultDir(
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
	wxString defaultFilename =
		(loadedFileName + string("_RANGE_BEARING.txt")).c_str();
	wxFileDialog dialog(
		this, caption, defaultDir, defaultFilename, wildcard,
		wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

	if (dialog.ShowModal() == wxID_OK)
	{
		wxString fileName = dialog.GetPath();
		string fil(fileName.mbc_str());

		int i, M = 0, n = (int)rawlog.size();
		FILE* f = os::fopen(fil.c_str(), "wt");
		if (!f) THROW_EXCEPTION("Cannot open output file for write.");

		for (i = 0; i < n; i++)
		{
			if (rawlog.getType(i) == CRawlog::etSensoryFrame)
			{
				CSensoryFrame::Ptr sf = rawlog.getAsObservations(i);
				CObservationBearingRange::Ptr obs =
					sf->getObservationByClass<CObservationBearingRange>();
				if (obs)
				{
					// For each entry in this sequence:
					for (auto& q : obs->sensedData)
					{
						M++;
						::fprintf(
							f, "%u %i %f %f %f\n", i, q.landmarkID, q.range,
							q.yaw, q.pitch);
					}
				}
			}
			else if (rawlog.getType(i) == CRawlog::etObservation)
			{
				CObservation::Ptr o = rawlog.getAsObservation(i);

				if (IS_CLASS(*o, CObservationBearingRange))
				{
					CObservationBearingRange::Ptr obs =
						std::dynamic_pointer_cast<CObservationBearingRange>(o);
					// For each entry in this sequence:
					for (auto& q : obs->sensedData)
					{
						M++;
						::fprintf(
							f, "%u %i %f %f %f\n", i, q.landmarkID, q.range,
							q.yaw, q.pitch);
					}
				}
			}
		}

		os::fclose(f);

		wxMessageBox(
			(format("%u bearing-range data entries saved!", M).c_str()),
			_("Done"), wxOK, this);
	}

	WX_END_TRY
}
