/* +------------------------------------------------------------------------+
   |                     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/containers/CDynamicGrid.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/typemeta/TEnumType.h>

namespace mrpt
{
namespace nav
{
/** \addtogroup nav_tpspace
 * @{  */

/** Trajectory points in C-Space for non-holonomic robots \sa
 * CPTG_DiffDrive_CollisionGridBased */
struct TCPoint
{
	TCPoint() = default;
	TCPoint(
		const float x_, const float y_, const float phi_, const float t_,
		const float dist_, const float v_, const float w_)
		: x(x_), y(y_), phi(phi_), t(t_), dist(dist_), v(v_), w(w_)
	{
	}
	float x, y, phi, t, dist, v, w;
};
using TCPointVector = std::vector<TCPoint>;
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& o, const mrpt::nav::TCPoint& p);
mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& i, mrpt::nav::TCPoint& p);

/** Base class for all PTGs suitable to non-holonomic, differentially-driven (or
 * Ackermann) vehicles
 * based on numerical integration of the trajectories and collision
 * look-up-table.
 * Regarding `initialize()`: in this this family of PTGs, the method builds the
 * collision grid or load it from a cache file.
 * Collision grids must be calculated before calling getTPObstacle(). Robot
 * shape must be set before initializing with setRobotShape().
 * The rest of PTG parameters should have been set at the constructor.
 */
class CPTG_DiffDrive_CollisionGridBased : public CPTG_RobotShape_Polygonal
{
   public:
	/** The main method to be implemented in derived classes: it defines the
	 * differential-driven differential equation */
	virtual void ptgDiffDriveSteeringFunction(
		float alpha, float t, float x, float y, float phi, float& v,
		float& w) const = 0;

	/** @name Virtual interface of each PTG implementation
	 *  @{ */
	// getDescription(): remains to be defined in derived classes.
	// setParams() to be defined in derived classses.

	/** The default implementation in this class relies on a look-up-table.
	 * Derived classes may redefine this to closed-form expressions, when they
	 * exist.
	 * See full docs in base class
	 * CParameterizedTrajectoryGenerator::inverseMap_WS2TP() */
	bool inverseMap_WS2TP(
		double x, double y, int& out_k, double& out_d,
		double tolerance_dist = 0.10) const override;

	/** In this class, `out_action_cmd` contains: [0]: linear velocity (m/s),
	 * [1]: angular velocity (rad/s).
	 * See more docs in
	 * CParameterizedTrajectoryGenerator::directionToMotionCommand() */
	mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(
		uint16_t k) const override;
	mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand()
		const override;

	/** Launches an exception in this class: it is not allowed in numerical
	 * integration-based PTGs to change the reference distance
	 * after initialization. */
	void setRefDistance(const double refDist) override;

	// Access to PTG paths (see docs in base class)
	size_t getPathStepCount(uint16_t k) const override;
	mrpt::math::TPose2D getPathPose(uint16_t k, uint32_t step) const override;
	double getPathDist(uint16_t k, uint32_t step) const override;
	bool getPathStepForDist(
		uint16_t k, double dist, uint32_t& out_step) const override;
	double getPathStepDuration() const override;
	double getMaxLinVel() const override { return V_MAX; }
	double getMaxAngVel() const override { return W_MAX; }
	void updateTPObstacle(
		double ox, double oy, std::vector<double>& tp_obstacles) const override;
	void updateTPObstacleSingle(
		double ox, double oy, uint16_t k, double& tp_obstacle_k) const override;

	/** This family of PTGs ignores the dynamic states */
	void onNewNavDynamicState() override
	{
		// Do nothing.
	}

	/** @} */  // --- end of virtual methods

	double getMax_V() const { return V_MAX; }
	double getMax_W() const { return W_MAX; }

   protected:
	CPTG_DiffDrive_CollisionGridBased();

	void internal_processNewRobotShape() override;
	void internal_initialize(
		const std::string& cacheFilename = std::string(),
		const bool verbose = true) override;
	void internal_deinitialize() override;

	/** Possible values in "params" (those in CParameterizedTrajectoryGenerator,
	 * which is called internally, plus):
	 *   - `${sKeyPrefix}resolution`: The cell size
	 *   - `${sKeyPrefix}v_max`, ``${sKeyPrefix}w_max`: Maximum robot speeds.
	 *   - `${sKeyPrefix}shape_x{0,1,2..}`, ``${sKeyPrefix}shape_y{0,1,2..}`:
	 * Polygonal robot shape [Optional, can be also set via
	 * `setRobotPolygonShape()`]
	 *
	 * See docs of derived classes for additional parameters in setParams()
	 */
	void loadFromConfigFile(
		const mrpt::config::CConfigFileBase& cfg,
		const std::string& sSection) override;
	void saveToConfigFile(
		mrpt::config::CConfigFileBase& cfg,
		const std::string& sSection) const override;

	void loadDefaultParams() override;

	double V_MAX{.0}, W_MAX{.0};
	double turningRadiusReference{.10};
	std::vector<TCPointVector> m_trajectory;
	double m_resolution{0.05};
	double m_stepTimeDuration{0.01};

	void internal_readFromStream(mrpt::serialization::CArchive& in) override;
	void internal_writeToStream(
		mrpt::serialization::CArchive& out) const override;

	/** Numerically solve the diferential equations to generate a family of
	 * trajectories */
	void simulateTrajectories(
		float max_time, float max_dist, unsigned int max_n, float diferencial_t,
		float min_dist, float* out_max_acc_v = nullptr,
		float* out_max_acc_w = nullptr);

	mrpt::math::TTwist2D getPathTwist(uint16_t k, uint32_t step) const override;

	/**  A list of all the pairs (alpha,distance) such as the robot collides at
	 *that cell.
	 *  - map key   (uint16_t) -> alpha value (k)
	 *	 - map value (float)    -> the MINIMUM distance (d), in meters,
	 *associated with that "k".
	 */
	using TCollisionCell = std::vector<std::pair<uint16_t, float>>;

	/** An internal class for storing the collision grid  */
	class CCollisionGrid : public mrpt::containers::CDynamicGrid<TCollisionCell>
	{
	   private:
		CPTG_DiffDrive_CollisionGridBased const* m_parent;

	   public:
		CCollisionGrid(
			float x_min, float x_max, float y_min, float y_max,
			float resolution, CPTG_DiffDrive_CollisionGridBased* parent)
			: mrpt::containers::CDynamicGrid<TCollisionCell>(
				  x_min, x_max, y_min, y_max, resolution),
			  m_parent(parent)
		{
		}
		~CCollisionGrid() override = default;
		/** Save to file, true = OK */
		bool saveToFile(
			mrpt::serialization::CArchive* fil,
			const mrpt::math::CPolygon& computed_robotShape) const;
		/** Load from file,  true = OK */
		bool loadFromFile(
			mrpt::serialization::CArchive* fil,
			const mrpt::math::CPolygon& current_robotShape);

		/** For an obstacle (x,y), returns a vector with all the pairs (a,d)
		 * such as the robot collides */
		const TCollisionCell& getTPObstacle(
			const float obsX, const float obsY) const;

		/** Updates the info into a cell: It updates the cell only if the
		 *distance d for the path k is lower than the previous value:
		 *	\param cellInfo The index of the cell
		 * \param k The path index (alpha discreet value)
		 * \param d The distance (in TP-Space, range 0..1) to collision.
		 */
		void updateCellInfo(
			const unsigned int icx, const unsigned int icy, const uint16_t k,
			const float dist);

	};	// end of class CCollisionGrid

	// Save/Load from files.
	bool saveColGridsToFile(
		const std::string& filename,
		const mrpt::math::CPolygon& computed_robotShape) const;	 // true = OK
	bool loadColGridsFromFile(
		const std::string& filename,
		const mrpt::math::CPolygon& current_robotShape);  // true = OK

	/** The collision grid */
	CCollisionGrid m_collisionGrid;

	/** Specifies the min/max values for "k" and "n", respectively.
	 * \sa m_lambdaFunctionOptimizer
	 */
	struct TCellForLambdaFunction
	{
		TCellForLambdaFunction()
			: k_min(std::numeric_limits<uint16_t>::max()),
			  k_max(std::numeric_limits<uint16_t>::min()),
			  n_min(std::numeric_limits<uint32_t>::max()),
			  n_max(std::numeric_limits<uint32_t>::min())
		{
		}

		uint16_t k_min, k_max;
		uint32_t n_min, n_max;

		bool isEmpty() const
		{
			return k_min == std::numeric_limits<uint16_t>::max();
		}
	};

	/** This grid will contain indexes data for speeding-up the default,
	 * brute-force lambda function */
	mrpt::containers::CDynamicGrid<TCellForLambdaFunction>
		m_lambdaFunctionOptimizer;
};

/** @} */
}  // namespace nav
namespace typemeta
{
// Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_NAMESPACE(TCPoint, mrpt::nav)
}  // namespace typemeta
}  // namespace mrpt
