/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/exceptions.h>

#include <functional>

namespace mrpt::math
{
/** Estimate the Jacobian of a multi-dimensional function around a point "x",
 * using finite differences of a given size in each input dimension.
 *  The template argument USERPARAM is for the data can be passed to the
 * functor.
 *   If it is not required, set to "int" or any other basic type.
 *
 *  This is a generic template which works with:
 *    VECTORLIKE: vector_float, CVectorDouble, CVectorFixed<>, double [N],
 * ...
 *    MATRIXLIKE: CMatrixDynamic, CMatrixFixed
 *  \ingroup mrpt_math_grp
 */
template <
	class VECTORLIKE, class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE,
	class USERPARAM>
void estimateJacobian(
	const VECTORLIKE& x,
	const std::function<void(
		const VECTORLIKE& x, const USERPARAM& y, VECTORLIKE3& out)>& functor,
	const VECTORLIKE2& increments, const USERPARAM& userParam,
	MATRIXLIKE& out_Jacobian)
{
	MRPT_START
	ASSERT_(x.size() > 0 && increments.size() == x.size());

	size_t m = 0;  // will determine automatically on the first call to "f":
	const size_t n = x.size();

	for (size_t j = 0; j < n; j++)
	{
		ASSERT_(increments[j] > 0);
	}  // Who knows...

	VECTORLIKE3 f_minus, f_plus;
	VECTORLIKE x_mod(x);

	// Evaluate the function "i" with increments in the "j" input x variable:
	for (size_t j = 0; j < n; j++)
	{
		// Create the modified "x" vector:
		x_mod[j] = x[j] + increments[j];
		functor(x_mod, userParam, f_plus);

		x_mod[j] = x[j] - increments[j];
		functor(x_mod, userParam, f_minus);

		x_mod[j] = x[j];  // Leave as original
		const double Ax_2_inv = 0.5 / increments[j];

		// The first time?
		if (j == 0)
		{
			m = f_plus.size();
			out_Jacobian.resize(m, n);
		}

		for (size_t i = 0; i < m; i++)
			out_Jacobian(i, j) = Ax_2_inv * (f_plus[i] - f_minus[i]);

	}  // end for j

	MRPT_END
}
}  // namespace mrpt::math
