/* +------------------------------------------------------------------------+
   |                     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/kinematics/CVehicleVelCmd.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TTwist2D.h>
#include <mrpt/obs/obs_frwds.h>	 // CSimplePointsMap
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/datetime.h>

namespace mrpt::nav
{
/** The pure virtual interface between a real or simulated robot and any
 * `CAbstractNavigator`-derived class.
 *
 *  The user must define a new class derived from `CRobot2NavInterface` and
 * reimplement
 *   all pure virtual and the desired virtual methods according to the
 * documentation in this class.
 *
 * [New in MRPT 1.5.0] This class does not make assumptions about the kinematic
 * model of the robot, so it can work with either
 *  Ackermann, differential-driven or holonomic robots. It will depend on the
 * used PTGs, so checkout
 *  each PTG documentation for the lenght and meaning of velocity commands.
 *
 * If used for a simulator, users may prefer to inherit from one of these
 * classes, which already provide partial implementations:
 *  - mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven
 *  - mrpt::nav::CRobot2NavInterfaceForSimulator_Holo
 *
 * \sa CReactiveNavigationSystem, CAbstractNavigator
 *  \ingroup nav_reactive
 */
class CRobot2NavInterface : public mrpt::system::COutputLogger
{
   public:
	CRobot2NavInterface();
	~CRobot2NavInterface() override;

	/** Get the current pose and velocity of the robot. The implementation
	 * should not take too much time to return,
	 *   so if it might take more than ~10ms to ask the robot for the
	 * instantaneous data, it may be good enough to
	 *   return the latest values from a cache which is updated in a parallel
	 * thread.
	 * \return false on any error retrieving these values from the robot.
	 * \callergraph */
	virtual bool getCurrentPoseAndSpeeds(
		/** (output) The latest robot pose (typically from a
		   mapping/localization module), in world coordinates. (x,y: meters,
		   phi: radians) */
		mrpt::math::TPose2D& curPose,
		/** (output) The latest robot velocity vector, in world coordinates.
		   (vx,vy: m/s, omega: rad/s) */
		mrpt::math::TTwist2D& curVelGlobal,
		/** (output) The timestamp for the read pose and velocity values. Use
		   mrpt::system::now() unless you have something more accurate. */
		mrpt::system::TTimeStamp& timestamp,
		/** (output) The latest robot raw odometry pose; may have long-time
		   drift should be more locally consistent than curPose (x,y: meters,
		   phi: radians) */
		mrpt::math::TPose2D& curOdometry,
		/** (output) ID of the coordinate frame for curPose. Default is not
		   modified is "map". [Only for future support to submapping,etc.] */
		std::string& frame_id) = 0;

	/** Sends a velocity command to the robot.
	 * The number components in each command depends on children classes of
	 * mrpt::kinematics::CVehicleVelCmd.
	 * One robot may accept one or more different CVehicleVelCmd classes.
	 * This method resets the watchdog timer (that may be or may be not
	 * implemented in a particular robotic platform) started with
	 * startWatchdog()
	 * \return false on any error.
	 * \sa startWatchdog
	 * \callergraph
	 */
	virtual bool changeSpeeds(
		const mrpt::kinematics::CVehicleVelCmd& vel_cmd) = 0;

	/** Just like changeSpeeds(), but will be called when the last velocity
	 * command is still the preferred solution,
	 * so there is no need to change that past command. The unique effect of
	 * this callback would be resetting the watchdog timer.
	 * \return false on any error.
	 * \sa changeSpeeds(), startWatchdog()
	 * \callergraph */
	virtual bool changeSpeedsNOP();

	/** Stop the robot right now.
	 *  \param[in] isEmergencyStop true if stop is due to some unexpected error.
	 * false if "stop" happens as part of a normal operation (e.g. target
	 * reached).
	 * \return false on any error.
	 */
	virtual bool stop(bool isEmergencyStop = true) = 0;

	/** Gets the emergency stop command for the current robot
	 * \return the emergency stop command
	 */
	virtual mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd() = 0;

	/** Gets the emergency stop command for the current robot
	 * \return the emergency stop command
	 */
	virtual mrpt::kinematics::CVehicleVelCmd::Ptr getStopCmd() = 0;

	/** Gets a motion command to make the robot to align with a given *relative*
	 * heading, without translating.
	 * Only for circular robots that can rotate in place; otherwise, return an
	 * empty smart pointer to indicate
	 * that the operation is not possible (this is what the default
	 * implementation does). */
	virtual mrpt::kinematics::CVehicleVelCmd::Ptr getAlignCmd(
		const double relative_heading_radians);

	/** Start the watchdog timer of the robot platform, if any, for maximum
	 * expected delay between consecutive calls to changeSpeeds().
	 * \param T_ms Period, in ms.
	 * \return false on any error. */
	virtual bool startWatchdog(float T_ms);

	/** Stop the watchdog timer.
	 * \return false on any error. \sa startWatchdog */
	virtual bool stopWatchdog();

	/** Return the current set of obstacle points, as seen from the local
	 * coordinate frame of the robot.
	 * \return false on any error.
	 * \param[out] obstacles  A representation of obstacles in robot-centric
	 * coordinates.
	 * \param[out] timestamp  The timestamp for the read obstacles. Use
	 * mrpt::system::now() unless you have something more accurate.
	 */
	virtual bool senseObstacles(
		mrpt::maps::CSimplePointsMap& obstacles,
		mrpt::system::TTimeStamp& timestamp) = 0;

	/** @name Navigation event callbacks
	 * @{ */
	/** Callback: Start of navigation command */
	virtual void sendNavigationStartEvent();

	/** Callback: End of navigation command (reach of single goal, or final
	 * waypoint of waypoint list) */
	virtual void sendNavigationEndEvent();

	/** Callback: Reached an intermediary waypoint in waypoint list navigation.
	 * reached_nSkipped will be `true` if the waypoint was physically reached;
	 * `false` if it was actually "skipped".
	 */
	virtual void sendWaypointReachedEvent(
		int waypoint_index, bool reached_nSkipped);

	/** Callback: Heading towards a new intermediary/final waypoint in waypoint
	 * list navigation */
	virtual void sendNewWaypointTargetEvent(int waypoint_index);

	/** Callback: Error asking sensory data from robot or sending motor
	 * commands. */
	virtual void sendNavigationEndDueToErrorEvent();

	/** Callback: No progression made towards target for a predefined period of
	 * time. */
	virtual void sendWaySeemsBlockedEvent();

	/** Callback: Apparent collision event (i.e. there is at least one obstacle
	 * point inside the robot shape) */
	virtual void sendApparentCollisionEvent();

	/** Callback: Target seems to be blocked by an obstacle. */
	virtual void sendCannotGetCloserToBlockedTargetEvent();

	/** @} */

	/** Returns the number of seconds ellapsed since the constructor of this
	 * class was invoked, or since
	 * the last call of resetNavigationTimer(). This will be normally
	 * wall-clock time, except in simulators where this method will return
	 * simulation time. */
	virtual double getNavigationTime();
	/** see getNavigationTime() */
	virtual void resetNavigationTimer();

   private:
	/** For getNavigationTime */
	mrpt::system::CTicTac m_navtime;
};
}  // namespace mrpt::nav
