/* +------------------------------------------------------------------------+
   |                     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/config/CLoadableOptions.h>

#include "CAbstractHolonomicReactiveMethod.h"
#include "CHolonomicLogFileRecord.h"

namespace mrpt::nav
{
/** \addtogroup nav_holo Holonomic navigation methods
 * \ingroup mrpt_nav_grp
 * @{ */

/** A class for storing extra information about the execution of
 *    CHolonomicVFF navigation.
 * \sa CHolonomicVFF, CHolonomicLogFileRecord
 */
class CLogFileRecord_VFF : public CHolonomicLogFileRecord
{
	DEFINE_SERIALIZABLE(CLogFileRecord_VFF, mrpt::nav)
   public:
};

/** A holonomic reactive navigation method, based on Virtual Force Fields (VFF).
 *
 * These are the optional parameters of the method which can be set by means of
 * a configuration file passed to the constructor or to CHolonomicND::initialize
 * (see also the field CHolonomicVFF::options).
 *
 * \code
 * # Section name can be changed via setConfigFileSectionName()
 * [VFF_CONFIG]
 * TARGET_SLOW_APPROACHING_DISTANCE = 0.10  // For stopping gradually
 * TARGET_ATTRACTIVE_FORCE          = 20    // Dimension-less (may have to be
 * tuned depending on the density of obstacle sampling)
 * \endcode
 *
 *  \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
 */
class CHolonomicVFF : public CAbstractHolonomicReactiveMethod
{
	DEFINE_SERIALIZABLE(CHolonomicVFF, mrpt::nav)
   public:
	/**  Initialize the parameters of the navigator, from some configuration
	 * file, or default values if set to NULL. */
	CHolonomicVFF(const mrpt::config::CConfigFileBase* INI_FILE = nullptr);

	// See base class docs
	void navigate(const NavInput& ni, NavOutput& no) override;

	void initialize(const mrpt::config::CConfigFileBase& INI_FILE)
		override;  // See base class docs
	void saveConfigFile(mrpt::config::CConfigFileBase& c)
		const override;	 // See base class docs

	/** Algorithm options */
	struct TOptions : public mrpt::config::CLoadableOptions
	{
		/** For stopping gradually (Default: 0.10) */
		double TARGET_SLOW_APPROACHING_DISTANCE{0.10};
		/** Dimension-less (may have to be tuned depending on the density of
		 * obstacle sampling) (Default: 20) */
		double TARGET_ATTRACTIVE_FORCE{20.0};

		TOptions();
		void loadFromConfigFile(
			const mrpt::config::CConfigFileBase& source,
			const std::string& section) override;  // See base docs
		void saveToConfigFile(
			mrpt::config::CConfigFileBase& cfg,
			const std::string& section) const override;	 // See base docs
	};

	/** Parameters of the algorithm (can be set manually or loaded from
	 * CHolonomicVFF::initialize or options.loadFromConfigFile(), etc.) */
	TOptions options;

	double getTargetApproachSlowDownDistance() const override
	{
		return options.TARGET_SLOW_APPROACHING_DISTANCE;
	}
	void setTargetApproachSlowDownDistance(const double dist) override
	{
		options.TARGET_SLOW_APPROACHING_DISTANCE = dist;
	}
};
/** @} */
}  // namespace mrpt::nav
