/* +------------------------------------------------------------------------+
   |                     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/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/stock_observations.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/random.h>

#include "common.h"

using namespace mrpt;
using namespace mrpt::tfest;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace std;

// ------------------------------------------------------
//				BenchmarkGridmaps
// ------------------------------------------------------
double grid_test_1(int a1, int a2)
{
	COccupancyGridMap2D gridMap(-20, 20, -20, 20, 0.05f);

	// test 1: getcell
	// ----------------------------------------
	const long N = 10000000;
	float p = 0;

	CTicTac tictac;
	for (long i = 0; i < N; i++)
	{
		p += gridMap.getCell(0, 0);
	}
	return tictac.Tac() / N;
}

double grid_test_2(int a1, int a2)
{
	COccupancyGridMap2D gridMap(-20, 20, -20, 20, 0.05f);
	// test 2: setcell
	// ----------------------------------------
	const long N = 10000000;

	float p = 0.8f;

	CTicTac tictac;
	for (long i = 0; i < N; i++)
	{
		gridMap.setCell(0, 0, p);
	}
	return tictac.Tac() / N;
}

double grid_test_3(int a1, int a2)
{
	COccupancyGridMap2D gridMap(-20, 20, -20, 20, 0.05f);
	const long N = 1000000;
	float p = 0.57f;

	CTicTac tictac;
	for (long i = 0; i < N; i++)
	{
		gridMap.updateCell(0, 0, p);
	}
	return tictac.Tac() / N;
}

double grid_test_4(int a1, int a2)
{
	COccupancyGridMap2D gridMap(-20, 20, -20, 20, 0.05f);

	// test 4: updateCell_fast
	// ----------------------------------------
	const long N = 10000000;

	float p = 0.57f;
	COccupancyGridMap2D::cellType logodd_obs = COccupancyGridMap2D::p2l(p);
	COccupancyGridMap2D::cellType* theMapArray = gridMap.getRow(0);
	unsigned theMapSize_x = gridMap.getSizeX();
	COccupancyGridMap2D::cellType logodd_thres_occupied =
		COccupancyGridMap2D::OCCGRID_CELLTYPE_MIN + logodd_obs;

	CTicTac tictac;
	for (long i = 0; i < N; i++)
	{
		COccupancyGridMap2D::updateCell_fast_occupied(
			2, 2, logodd_obs, logodd_thres_occupied, theMapArray, theMapSize_x);
	}
	return tictac.Tac() / N;
}

double grid_test_5_6(int a1, int a2)
{
	getRandomGenerator().randomize(333);

	// prepare the laser scan:
	CObservation2DRangeScan scan1;
	stock_observations::example2DRangeScan(scan1);

	COccupancyGridMap2D gridmap(-20, 20, -20, 20, 0.05f);
	gridmap.insertionOptions.wideningBeamsWithDistance = a1 != 0;
	const long N = 3000;
	CTicTac tictac;
	for (long i = 0; i < N; i++)
	{
		CPose2D pose(
			getRandomGenerator().drawUniform(-1.0, 1.0),
			getRandomGenerator().drawUniform(-1.0, 1.0),
			getRandomGenerator().drawUniform(-M_PI, M_PI));
		CPose3D pose3D(pose);

		gridmap.insertObservation(scan1, pose3D);
	}
	return tictac.Tac() / N;
}

double grid_test_7(int a1, int a2)
{
	COccupancyGridMap2D gridmap(-20, 20, -20, 20, 0.05f);
	CTicTac tictac;

	gridmap.resizeGrid(-30, 30, -40, 40);

	return tictac.Tac();
}

double grid_test_8(int a1, int a2)
{
	getRandomGenerator().randomize(333);

	// prepare the laser scan:
	CObservation2DRangeScan scan1;
	stock_observations::example2DRangeScan(scan1);

	COccupancyGridMap2D gridmap(-20, 20, -20, 20, 0.05f);

	// test 8: Likelihood computation
	const long N = 5000;

	CPose3D pose3D(0, 0, 0);
	gridmap.insertObservation(scan1, pose3D);

	double R = 0;
	CTicTac tictac;
	for (long i = 0; i < N; i++)
	{
		CPose3D pose(
			getRandomGenerator().drawUniform(-1.0, 1.0),
			getRandomGenerator().drawUniform(-1.0, 1.0), .0,
			getRandomGenerator().drawUniform(-M_PI, M_PI), .0, .0);
		R += gridmap.computeObservationLikelihood(scan1, pose);
	}
	return tictac.Tac() / N;
}

double grid_test_9(int a1, int a2)
{
	// test 9: computeMatchingWith2D
	// ----------------------------------------

	// prepare the laser scan:
	CObservation2DRangeScan scan1;
	stock_observations::example2DRangeScan(scan1);

	CSimplePointsMap gridmap;
	CSimplePointsMap pt_map2;
	pt_map2.insertionOptions.minDistBetweenLaserPoints = 0.03f;

	CPose3D pose;
	gridmap.insertObservation(scan1, pose);

	CPose3D pose2(0.05, 0.04, 0, 4.0_deg, 0, 0);
	pt_map2.insertObservation(scan1, pose2);

	const CPose2D nullPose(0, 0, 0);
	TMatchingPairList correspondences;

	TMatchingParams matchParams;
	TMatchingExtraResults matchExtraResults;
	matchParams.maxDistForCorrespondence = 0.10f;
	matchParams.maxAngularDistForCorrespondence = 0;

	CTicTac tictac;
	for (long i = 0; i < a1; i++)
	{
		gridmap.determineMatching2D(
			&pt_map2,  // The other map
			nullPose,  // The other map's pose
			correspondences, matchParams, matchExtraResults);
	}
	return tictac.Tac() / a1;
}

// ------------------------------------------------------
// register_tests_grids
// ------------------------------------------------------
void register_tests_grids()
{
	lstTests.emplace_back("gridmap2D: getCell", grid_test_1);
	lstTests.emplace_back("gridmap2D: setCell", grid_test_2);
	lstTests.emplace_back("gridmap2D: updateCell", grid_test_3);
	lstTests.emplace_back("gridmap2D: updateCell_fast_occupied", grid_test_4);
	lstTests.emplace_back(
		"gridmap2D: insert scan w/o widening", grid_test_5_6, 0);
	lstTests.emplace_back(
		"gridmap2D: insert scan with widening", grid_test_5_6, 1);
	lstTests.emplace_back("gridmap2D: resize", grid_test_7);
	lstTests.emplace_back("gridmap2D: computeLikelihood", grid_test_8);
	lstTests.emplace_back("gridmap2D: determineMatching2D", grid_test_9, 5000);
}
