/* +------------------------------------------------------------------------+
   |                     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/math/CAtan2LookUpTable.h>
#include <mrpt/random.h>

#include <cmath>

#include "common.h"

double atan2_lut_test_build(int, int)
{
	const unsigned int step = 100;
	mrpt::system::CTicTac tictac;
	mrpt::math::CAtan2LookUpTable lut;
	tictac.Tic();
	for (unsigned int i = 0; i < step; i++)
	{
		lut.resize(-10.0, 10.0, -10.0, 10.0, 0.01);
	}
	return tictac.Tac() / step;
}

double atan2_lut_test_query(int, int)
{
	const unsigned int step = 1000000;
	mrpt::system::CTicTac tictac;
	mrpt::math::CAtan2LookUpTable lut;
	lut.resize(-10.0, 10.0, -10.0, 10.0, 0.01);
	tictac.Tic();
	double atan2val;
	double x = -9.0, y = -8.0;
	const double dx = 18.0 / step, dy = 12.5 / step;
	for (unsigned int i = 0; i < step; i++)
	{
		x += dx;
		y += dy;
		// bool valid =
		lut.atan2(y, x, atan2val);
	}
	return tictac.Tac() / step;
}

double atan2_lut_multires_test_build(int, int)
{
	const unsigned int step = 10;

	mrpt::math::CAtan2LookUpTableMultiRes atan2lut;

	std::map<double, double> res2extension;
	res2extension[0.001] = 0.8;	 // 0.1 cm resolution
	res2extension[0.01] = 2.0;	// 1.0 cm resolution
	res2extension[0.02] = 5.0;	// 2.0 cm resolution
	res2extension[0.05] = 11.0;	 // 5.0 cm resolution

	mrpt::system::CTicTac tictac;
	tictac.Tic();
	for (unsigned int i = 0; i < step; i++)
	{
		atan2lut.resize(res2extension);
	}
	return tictac.Tac() / step;
}

double atan2_lut_multires_test_query(int, int)
{
	const unsigned int step = 1000000;
	mrpt::system::CTicTac tictac;
	mrpt::math::CAtan2LookUpTableMultiRes atan2lut;

	std::map<double, double> res2extension;
	res2extension[0.001] = 0.8;	 // 0.1 cm resolution
	res2extension[0.01] = 2.0;	// 1.0 cm resolution
	res2extension[0.02] = 5.0;	// 2.0 cm resolution
	res2extension[0.05] = 11.0;	 // 5.0 cm resolution
	atan2lut.resize(res2extension);

	tictac.Tic();
	double atan2val;
	double x = -9.0, y = -8.0;
	const double dx = 18.0 / step, dy = 12.5 / step;
	for (unsigned int i = 0; i < step; i++)
	{
		x += dx;
		y += dy;
		// bool valid =
		atan2lut.atan2(y, x, atan2val);
	}
	return tictac.Tac() / step;
}

double atan2_raw_test_query(int, int)
{
	const unsigned int step = 1000000;
	mrpt::system::CTicTac tictac;

	double atan2val = .0;
	double x = -9.0, y = -8.0;
	const double dx = 18.0 / step, dy = 12.5 / step;
	for (unsigned int i = 0; i < step; i++)
	{
		x += dx;
		y += dy;
		atan2val += ::atan2(y, x);
	}
	const double t = tictac.Tac() / step;
	{
		std::stringstream ss;
		ss << atan2val;
	}
	return t;
}

// ------------------------------------------------------
// register_tests_atan2lut
// ------------------------------------------------------
void register_tests_atan2lut()
{
	lstTests.emplace_back(
		"CAtan2LUT: 20x20m,1cm cells,build", atan2_lut_test_build);
	lstTests.emplace_back(
		"CAtan2LUT: 20x20m,1cm cells,query", atan2_lut_test_query);

	lstTests.emplace_back(
		"CAtan2LUTMultiRes: 22x22m,build", atan2_lut_multires_test_build);
	lstTests.emplace_back(
		"CAtan2LUTMultiRes: 22x22m,query", atan2_lut_multires_test_query);

	lstTests.emplace_back(
		"CAtan2LUT: raw ::atan2() call", atan2_raw_test_query);
}
