/* +------------------------------------------------------------------------+
   |                     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/bayes/CParticleFilterCapable.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/math/ops_vectors.h>
#include <mrpt/math/utils.h>
#include <mrpt/random.h>
#include <mrpt/system/filesystem.h>

#include <iostream>
#include <map>

using namespace mrpt::bayes;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace mrpt::io;
using namespace std;

double MIN_LOG_WEIG = -1.0;

unsigned int N_TESTS = 500;
int N_PARTICLES = 100;

// For batch experiment:
CVectorDouble min_log_ws;
map<string, CVectorDouble> results;

// vectorToTextFile( out_indxs, #ALGOR, true, true); /* By rows, append */

#define TEST_RESAMPLING(ALGOR)                                                 \
	mrpt::system::deleteFile(#ALGOR);                                          \
	/*printf(#ALGOR);*/                                                        \
	/*printf("\n");*/                                                          \
	ERR_MEANs.clear();                                                         \
	ERR_STDs.clear();                                                          \
	for (size_t i = 0; i < N_TESTS; i++)                                       \
	{                                                                          \
		mrpt::random::getRandomGenerator().drawUniformVector(                  \
			log_ws, MIN_LOG_WEIG, 0.0);                                        \
		CParticleFilterCapable::log2linearWeights(log_ws, lin_ws);             \
		CParticleFilterCapable::computeResampling(                             \
			CParticleFilter::ALGOR, log_ws, out_indxs);                        \
		hist_parts = mrpt::math::histogram(out_indxs, 0, M - 1, M, true);      \
		vector<double> errs_hist = lin_ws - hist_parts;                        \
		ERR_MEANs.push_back(mrpt::math::mean(errs_hist));                      \
		ERR_STDs.push_back(mrpt::math::stddev(errs_hist));                     \
	}                                                                          \
	printf("%s: ERR_MEAN %e\n", #ALGOR, mrpt::math::mean(ERR_MEANs));          \
	printf("%s: ERR_STD %f\n", #ALGOR, mrpt::math::mean(ERR_STDs));            \
	results[#ALGOR].push_back(mrpt::math::mean(ERR_STDs));

// ------------------------------------------------------
//                  TestResampling
// ------------------------------------------------------
void TestResampling()
{
	vector<double> log_ws;
	std::vector<size_t> out_indxs;

	const size_t M = N_PARTICLES;

	log_ws.resize(M);
	// vectorToTextFile( log_ws, "log_ws.txt");

	// Compute normalized linear weights:
	vector<double> lin_ws;
	vector<double> hist_parts;
	vector<double> ERR_MEANs;
	vector<double> ERR_STDs;

	// prMultinomial
	TEST_RESAMPLING(prMultinomial)
	// prResidual
	TEST_RESAMPLING(prResidual)
	// prStratified
	TEST_RESAMPLING(prStratified)
	// prSystematic
	TEST_RESAMPLING(prSystematic)
}

void TestBatch()
{
	for (double LL = -2; LL <= 2.01; LL += 0.08)
	{
		double L = pow(10.0, LL);

		min_log_ws.push_back(L);
		printf("MIN_LOG_W=%f\n", L);

		MIN_LOG_WEIG = L;
		TestResampling();
	}

	// Save results to files:
	CVectorDouble R;

	vectorToTextFile(min_log_ws, "min_log_ws.txt");

	R = results["prMultinomial"];
	vectorToTextFile(R, "prMultinomial.txt");
	R = results["prResidual"];
	vectorToTextFile(R, "prResidual.txt");
	R = results["prStratified"];
	vectorToTextFile(R, "prStratified.txt");
	R = results["prSystematic"];
	vectorToTextFile(R, "prSystematic.txt");
}

// ------------------------------------------------------
//                        MAIN
// ------------------------------------------------------
int main(int argc, char** argv)
{
	try
	{
		getRandomGenerator().randomize();

		if (argc > 1) N_PARTICLES = atoi(argv[1]);

		// TestResampling();
		TestBatch();

		return 0;
	}
	catch (exception& e)
	{
		std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
		return -1;
	}
	catch (...)
	{
		cerr << "Untyped excepcion!!";
		return -1;
	}
}
