/* +------------------------------------------------------------------------+
   |                     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/config/CConfigFile.h>
#include <mrpt/math/wrap2pi.h>
#include <wx/image.h>
#include <wx/msgdlg.h>

#include <fstream>

#include "slamdemoApp.h"
#include "slamdemoMain.h"

using namespace std;
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::slam;
using namespace mrpt::system;
using namespace mrpt::config;
using namespace mrpt::io;
using namespace mrpt::serialization;

// Do batch experiments as indicated by the command line arguments
//  and the command file "cfgFil".
void slamdemoApp::DoBatchExperiments(const std::string& cfgFil)
{
	CConfigFile cf(cfgFil);

	win->options.loadFromConfigFile(cf, "");
	win->m_SLAM.options.loadFromConfigFile(cf, "");
	win->m_SLAM.KF_options.loadFromConfigFile(cf, "");

	win->resetSimulator(win->options.map_generator);

	const size_t nSteps = round(
		6 * win->options.path_square_len /
		double(win->options.robot_step_length));

	CTicTac tim;
	tim.Tic();

	if (!m_option_norun)
		for (size_t i = 0; i < nSteps; i++)
			win->executeOneStep();

	double mT = tim.Tac();

	// Stats:
	const size_t N = win->m_historicData.size();
	double totalFP = 0;
	double totalFN = 0;
	double totalJCBBiters = 0;
	double err_xy = 0;
	double err_phi = 0;
	double err_D2 = 0;

	for (size_t i = 0; i < N; i++)
	{
		totalFP += win->m_historicData[i].da_false_pos;
		totalFN += win->m_historicData[i].da_false_neg;
		totalJCBBiters += win->m_historicData[i].jcbb_iters;
		err_xy += win->m_historicData[i].GT_robot_pose.distanceTo(
			win->m_historicData[i].estimate_robot_pose.getMeanVal());

		CPosePDFGaussian p(
			win->m_historicData[i].GT_robot_pose,
			CMatrixDouble33() /* cov= zero */);
		double dm2 = 1000;
		try
		{
			dm2 = win->m_historicData[i]
					  .estimate_robot_pose.mahalanobisDistanceTo(p);
		}
		catch (...)
		{
		}
		err_D2 += dm2;
		err_phi += fabs(wrapToPi(
			win->m_historicData[i].GT_robot_pose.phi() -
			win->m_historicData[i].estimate_robot_pose.getMeanVal().phi()));
	}

	if (N)
	{
		totalFP /= N;
		totalFN /= N;
		totalJCBBiters /= N;
		err_xy /= N;
		err_phi /= N;
		err_D2 /= N;
		mT /= N;
	}

	cout << "FP: " << totalFP << " FN: " << totalFN
		 << " JCBB iters: " << totalJCBBiters << " Run iters: " << N << endl;
	cout << "Mean err_xy: " << err_xy << " mean tim: " << mT << endl;

	ofstream f("slam_out.txt", ios::out | ios::app);

	f << mT << " " << err_xy << " " << err_phi << " " << err_D2 << " "
	  << totalJCBBiters << " " << totalFP << " " << totalFN << " " << endl;

	win->updateAllGraphs();
}
