/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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
