/* +------------------------------------------------------------------------+
   |                     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 <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
