/* +------------------------------------------------------------------------+
   |                     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 <mrpt/obs/CSinCosLookUpTableFor2DScans.h>

#include "CAbstractHolonomicReactiveMethod.h"

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

/** Full evaluation of all possible directions within the discrete set of input
 * directions.
 *
 * These are the optional parameters of the method which can be set by means of
 * a configuration file passed to the constructor or to
 * CHolonomicFullEval::initialize() or directly in \a
 * CHolonomicFullEval::options
 *
 * \code
 * # Section name can be changed via setConfigFileSectionName()
 * [FULL_EVAL_CONFIG]
 * factorWeights        = 1.0 1.0 1.0 0.05 1.0
 * factorNormalizeOrNot =   0   0   0    0   1
 * // 0: Clearness in direction
 * // 1: Closest approach to target along straight line (Euclidean)
 * // 2: Distance of end collision-free point to target (Euclidean)
 * // 3: Hysteresis
 * // 4: Clearness to nearest obstacle along path
 * // 5: Like 2, but without being decimated if path to target is obstructed
 * TARGET_SLOW_APPROACHING_DISTANCE = 0.20   // Start to reduce speed when
 * closer than this to target [m]
 * TOO_CLOSE_OBSTACLE               = 0.02   // Directions with collision-free
 * distances below this threshold are not elegible.
 * HYSTERESIS_SECTOR_COUNT          = 5      // Range of "sectors" (directions)
 * for hysteresis over successive timesteps
 * PHASE1_FACTORS   = 0 1 2                  // Indices of the factors above to
 * be considered in phase 1
 * PHASE2_FACTORS   = 3 4                    // Indices of the factors above to
 * be considered in phase 2
 * PHASE1_THRESHOLD = 0.75                   // Phase1 scores must be above this
 * relative range threshold [0,1] to be considered in phase 2 (Default:`0.75`)
 * \endcode
 *
 *  \sa CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem
 */
class CHolonomicFullEval : public CAbstractHolonomicReactiveMethod
{
	DEFINE_SERIALIZABLE(CHolonomicFullEval, mrpt::nav)
   public:
	/**  Initialize the parameters of the navigator, from some configuration
	 * file, or default values if set to nullptr */
	CHolonomicFullEval(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
	{
		/** Directions with collision-free distances below this threshold are
		 * not elegible. */
		double TOO_CLOSE_OBSTACLE{0.15};
		/** Start to reduce speed when closer than this to target  [m] */
		double TARGET_SLOW_APPROACHING_DISTANCE{0.60};
		/** Start to reduce speed when clearance is below this value ([0,1]
		 * ratio wrt obstacle reference/max distance) */
		double OBSTACLE_SLOW_DOWN_DISTANCE{0.15};
		/** Range of "sectors" (directions) for hysteresis over successive
		 * timesteps */
		double HYSTERESIS_SECTOR_COUNT{5};
		/** See docs above */
		std::vector<double> factorWeights;
		/** 0/1 to normalize factors. */
		std::vector<int32_t> factorNormalizeOrNot;
		/** Factor indices [0,4] for the factors to consider in each phase
		 * 1,2,...N of the movement decision (Defaults: `PHASE1_FACTORS=0 1 2`,
		 * `PHASE2_FACTORS=`3 4`) */
		std::vector<std::vector<int32_t>> PHASE_FACTORS;
		/** Phase 1,2,N-1... scores must be above this relative range threshold
		 * [0,1] to be considered in phase 2 (Default:`0.75`) */
		std::vector<double> PHASE_THRESHOLDS;

		/** (default:false, to save space) */
		bool LOG_SCORE_MATRIX{false};

		/**  Ratio [0,1], times path_count, gives the minimum number of paths at
		 * each side of a target direction to be accepted as desired direction
		 */
		double clearance_threshold_ratio{0.05};
		/**  Ratio [0,1], times path_count, gives the minimum gap width to
		 * accept a direct motion towards target. */
		double gap_width_ratio_threshold{0.25};

		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
	 * CHolonomicFullEval::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;
	}

   private:
	unsigned int m_last_selected_sector;
	unsigned int direction2sector(const double a, const unsigned int N);
	/** Individual scores for each direction: (i,j), i (row) are directions, j
	 * (cols) are scores. Not all directions may have evaluations, in which case
	 * a "-1" value will be found. */
	mrpt::math::CMatrixD m_dirs_scores;

	/** If desired, override in a derived class to manipulate the final
	 * evaluations of each directions */
	virtual void postProcessDirectionEvaluations(
		std::vector<double>& dir_evals, const NavInput& ni,
		unsigned int trg_idx);

	struct EvalOutput
	{
		std::vector<std::vector<double>> phase_scores;
		EvalOutput() {}
	};

	/** Evals one single target of the potentially many of them in NavInput */
	void evalSingleTarget(
		unsigned int target_idx, const NavInput& ni, EvalOutput& eo);

	mrpt::obs::CSinCosLookUpTableFor2DScans m_sincos_lut;
};	// end of CHolonomicFullEval

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

	/** Member data */
	int32_t selectedSector{0};
	double evaluation{.0};
	/** Individual scores for each direction: (i,j), i (row) are directions, j
	 * (cols) are scores. Not all directions may have evaluations, in which case
	 * a "-1" value will be found. */
	mrpt::math::CMatrixD dirs_scores;
	/** Normally = 0. Can be >0 if multiple targets passed simultaneously. */
	int32_t selectedTarget{0};

	const mrpt::math::CMatrixD* getDirectionScores() const override
	{
		return &dirs_scores;
	}
};

/** @} */
}  // namespace mrpt::nav
