/* +------------------------------------------------------------------------+
   |                     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 <mrpt/system/COutputLogger.h>
#include <mrpt/system/CRateTimer.h>
#include <mrpt/system/CTicTac.h>

namespace mrpt::system
{
/** A class for calling sleep() in a loop, such that the amount of sleep time
 * will be computed to make the loop run at the desired rate (in Hz).
 * This class implements a PI controller on top of a vanilla CRateTimer object,
 * ensuring a high accuracy in achieved execution rates. Note that this is done
 * by setting a slightly-higher rate ("control action") to the internal
 * CRateTimer, such that the error between the user-provided expected rate and
 * the actual measured rate (low-pass filtered) is decreased by means of a PI
 * controller.
 *
 * Note that rates higher than a few kHz are not attainable in all CPUs and/or
 * kernel versions. Find below some graphs illustrating how this class tries to
 * achieve a constant setpoint rate (given by the user), reacting to changes in
 * the setpoint values:
 *
 *  ![controlled rate timer plots](CControlledRateTimer_example.png)
 *
 * This graphs is generated with the example:
 *
 * `system_control_rate_timer_example --rate1 2000.0 --rate2 4000.0`
 *
 * All the parameters for the PI controller and low-pass filter (rate estimator)
 * are settable by the user to adapt them to specific needs.
 *
 * \note Control law by [Francisco Jose Mañas
 * Alvarez](https://github.com/FranciscoJManasAlvarez)
 *
 * \note [New in MRPT 2.0.4]
 * \ingroup mrpt_system_grp
 */
class CControlledRateTimer : public mrpt::system::COutputLogger
{
 public:
  /** @name Main API
   *  @{ */

  /** Ctor: specifies the desired rate (Hz) */
  CControlledRateTimer(const double rate_hz = 1.0);
  /** Dtor */
  virtual ~CControlledRateTimer() = default;

  /** Changes the object loop rate (Hz) */
  void setRate(const double rate_hz);

  /** Sleeps for some time, such as the return of this method is 1/rate
   * (seconds)
   * after the return of the previous call.
   * \return false if the rate could not be achieved ("we are already late"),
   * true if all went right. */
  bool sleep();

  /** @} */

  /** @name PI control parameters
   *  @{ */

  /** PI controller Kp parameter [default=1.0] */
  double controllerParam_Kp() const { return m_Kp; }
  void controllerParam_Kp(double v)
  {
    ASSERT_GT_(v, .0);
    m_Kp = v;
  }

  /** PI controller Ti parameter [default=0.0194] */
  double controllerParam_Ti() const { return m_Ti; }
  void controllerParam_Ti(double v)
  {
    ASSERT_GE_(v, .0);
    m_Ti = v;
  }

  /** Low-pass filter a0 value [default=0.9]:
   * estimation = a0*input + (1-a0)*former_estimation */
  double lowPassParam_a0() const { return m_lowPass_a0; }
  void lowPassParam_a0(double v)
  {
    ASSERT_GT_(v, .0);
    ASSERT_LE_(v, 1.0);
    m_lowPass_a0 = v;
  }

  /** Get/set ratio threshold for issuing a warning (via COutputLogger
   * interface) if the achieved rate is not this close to the set-point
   * [Default=0.2, =20%]
   */
  double followErrorRatioToRaiseWarning() const { return m_followErrorRatioForWarning; }
  void followErrorRatioToRaiseWarning(double v)
  {
    ASSERT_GT_(v, .0);
    ASSERT_LE_(v, 1.0);
    m_followErrorRatioForWarning = v;
  }

  /** Gets the actual controller output: the rate (Hz) of the internal
   * CRateTimer object. */
  double actualControlledRate() const { return m_ratetimer.rate(); }

  /** Gets the latest estimated run rate (Hz), which comes from actual period
   * measurement, low-pass filtered. */
  double estimatedRate() const { return m_currentEstimatedRate; }

  /** Last actual execution rate measured (Hz), without low-pass filtering */
  double estimatedRateRaw() const { return m_lastRawRate; }

  /** @} */
 private:
  double m_rate_hz = 1.0;
  mrpt::system::CRateTimer m_ratetimer;  //!< the one control acts on

  double m_lowPass_a0 = 0.99;
  double m_Kp = 1.0;
  double m_Ti = 0.1;

  double m_followErrorRatioForWarning = 0.20;
  double m_lastControlError = .0;

  bool internalUpdateRateEstimate();
  double m_currentEstimatedRate = 1.0, m_lastRawRate = 1.0;
  double m_lastTic = 0;
  mrpt::system::CTicTac m_tic;

};  // End of class def.

}  // namespace mrpt::system
