/* +------------------------------------------------------------------------+
   |                     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.h>
#include <mrpt/poses/CPoint2D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>

#include <iostream>

using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::poses;
using namespace mrpt::random;
using namespace mrpt::system;

double SIGMA = 0.05;

// The custom class:
class CMyRejectionSampling : public CRejectionSamplingCapable<CPose2D>
{
   protected:
	void RS_drawFromProposal(CPose2D& outSample) override
	{
		double ang = getRandomGenerator().drawUniform(-M_PI, M_PI);
		double R = getRandomGenerator().drawGaussian1D(1.0, SIGMA);
		outSample.x(1.0f - cos(ang) * R);
		outSample.y(sin(ang) * R);
		outSample.phi(getRandomGenerator().drawUniform(-M_PI, M_PI));
	}

	/** Returns the NORMALIZED observation likelihood at a given point of the
	 * state space (values in the range [0,1]).
	 */
	double RS_observationLikelihood(const CPose2D& x) override
	{
		return exp(
			-0.5 * square((x.distanceTo(CPoint2D(0, 0)) - 1.0f) / SIGMA));
	}
};

// ------------------------------------------------------
//				TestRS
// ------------------------------------------------------
void TestRS()
{
	CMyRejectionSampling RS;
	std::vector<CMyRejectionSampling::TParticle> samples;
	CTicTac tictac;

	tictac.Tic();
	printf("Computing...");

	RS.rejectionSampling(1000, samples);

	printf("Ok! %fms\n", 1000 * tictac.Tac());

	FILE* f = os::fopen("_out_samples.txt", "wt");
	std::vector<CMyRejectionSampling::TParticle>::iterator it;
	for (it = samples.begin(); it != samples.end(); it++)
		os::fprintf(
			f, "%f %f %f %e\n", it->d->x(), it->d->y(), it->d->phi(),
			it->log_w);

	os::fclose(f);
}

// ------------------------------------------------------
//						MAIN
// ------------------------------------------------------
int main()
{
	try
	{
		TestRS();

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