/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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/nav/reactive/CRobot2NavInterface.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/poses/CPose2DInterpolator.h>
#include <mrpt/poses/FrameTransformer.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/typemeta/TEnumType.h>

#include <functional>
#include <list>
#include <memory>  // unique_ptr
#include <mutex>

namespace mrpt::nav
{
/** This is the base class for any reactive/planned navigation system. See
 * derived classes.
 *
 * How to use:
 *  - A class derived from `CRobot2NavInterface` with callbacks must be defined
 * by the user and provided to the constructor.
 *  - `navigationStep()` must be called periodically in order to effectively run
 * the navigation. This method will internally call the callbacks to gather
 * sensor data and robot positioning data.
 *
 * It implements the following state machine (see
 * CAbstractNavigator::getCurrentState() ), taking into account the extensions
 * described in CWaypointsNavigator
 *  \dot
 *  digraph CAbstractNavigator_States {
 *      IDLE; NAVIGATING; SUSPENDED; NAV_ERROR;
 *      IDLE -> NAVIGATING [ label="CAbstractNavigator::navigate()"];
 *      IDLE -> NAVIGATING [ label="CWaypointsNavigator::navigateWaypoints()" ];
 *      NAVIGATING -> IDLE [ label="Final target reached" ];
 *      NAVIGATING -> IDLE [ label="CAbstractNavigator::cancel()" ];
 *      NAVIGATING -> NAV_ERROR [ label="Upon sensor errors, timeout,..." ];
 *      NAVIGATING -> SUSPENDED [ label="CAbstractNavigator::suspend()" ];
 *      SUSPENDED -> NAVIGATING [ label="CAbstractNavigator::resume()" ];
 *      NAV_ERROR -> IDLE [ label="CAbstractNavigator::resetNavError()" ];
 *  }
 *  \enddot
 *
 * \sa CWaypointsNavigator, CReactiveNavigationSystem, CRobot2NavInterface, all
 * children classes
 *  \ingroup nav_reactive
 */
class CAbstractNavigator : public mrpt::system::COutputLogger
{
 public:
  /** ctor */
  CAbstractNavigator(CRobot2NavInterface& robot_interface_impl);
  /** dtor */
  ~CAbstractNavigator() override;

  /** Individual target info in CAbstractNavigator::TNavigationParamsBase and
   * derived classes */
  struct TargetInfo
  {
    /** Coordinates of desired target location. Heading may be ignored by
     * some reactive implementations. */
    mrpt::math::TPose2D target_coords;
    /** (Default="map") Frame ID in which target is given. Optional, use
     * only for submapping applications. */
    std::string target_frame_id;
    /** (Default=0.5 meters) Allowed distance to target in order to end the
     * navigation. */
    float targetAllowedDistance{0.5};
    /** (Default=false) Whether the \a target coordinates are in global
     * coordinates (false) or are relative to the current robot pose (true).
     */
    bool targetIsRelative{false};
    /** (Default=.05) Desired relative speed (wrt maximum speed), in range
     * [0,1], of the vehicle at target. Holonomic nav methods will perform
     * "slow down" approaching target only if this is "==.0". Intermediary
     * values will be honored only by the higher-level navigator, based on
     * straight-line Euclidean distances. */
    double targetDesiredRelSpeed{.05};
    bool targetIsIntermediaryWaypoint{false};  // !< (Default=false) If true, event
    // callback
    // `sendWaypointReachedEvent()` will
    // be called instead of
    // `sendNavigationEndEvent()`

    TargetInfo();
    /** Gets navigation params as a human-readable format */
    std::string getAsText() const;
    bool operator==(const TargetInfo& o) const;
    bool operator!=(const TargetInfo& o) const { return !(*this == o); }
  };

  /** Base for all high-level navigation commands. See derived classes */
  struct TNavigationParamsBase
  {
    virtual ~TNavigationParamsBase() = default;
    /** Gets navigation params as a human-readable format */
    virtual std::string getAsText() const = 0;

   protected:
    friend bool operator==(const TNavigationParamsBase&, const TNavigationParamsBase&);
    virtual bool isEqual(const TNavigationParamsBase& o) const = 0;
  };

  /** The struct for configuring navigation requests. Used in
   * CAbstractPTGBasedReactive::navigate() */
  struct TNavigationParams : public TNavigationParamsBase
  {
    /** Navigation target */
    TargetInfo target;

    /** Gets navigation params as a human-readable format */
    std::string getAsText() const override;
    virtual std::unique_ptr<TNavigationParams> clone() const
    {
      return std::make_unique<TNavigationParams>(*this);
    }

   protected:
    bool isEqual(const TNavigationParamsBase& o) const override;
  };

  /** \name Navigation control API
   * @{ */

  /** Loads all params from a file. To be called before initialize().
   * Each derived class *MUST* load its own parameters, and then call *ITS
   * PARENT'S* overridden method to ensure all params are loaded. */
  virtual void loadConfigFile(const mrpt::config::CConfigFileBase& c);
  /** Saves all current options to a config file.
   * Each derived class *MUST* save its own parameters, and then call *ITS
   * PARENT'S* overridden method to ensure all params are saved. */
  virtual void saveConfigFile(mrpt::config::CConfigFileBase& c) const;

  /**  Must be called before any other navigation command */
  virtual void initialize() = 0;
  /** This method must be called periodically in order to effectively run the
   * navigation */
  virtual void navigationStep();

  /** Navigation request to a single target location. It starts a new
   * navigation.
   * \param[in] params Pointer to structure with navigation info (its
   * contents will be copied, so the original can be freely destroyed upon
   * return if it was dynamically allocated.)
   * \note A pointer is used so the passed object can be polymorphic with
   * derived types.
   */
  virtual void navigate(const TNavigationParams* params);

  /** Cancel current navegation. */
  virtual void cancel();
  /** Continues with suspended navigation. \sa suspend */
  virtual void resume();
  /** Suspend current navegation. \sa resume */
  virtual void suspend();
  /** Resets a `NAV_ERROR` state back to `IDLE` */
  virtual void resetNavError();

  /** The different states for the navigation system. */
  enum TState
  {
    IDLE = 0,
    NAVIGATING,
    SUSPENDED,
    NAV_ERROR
  };

  /** Returns the current navigator state. */
  inline TState getCurrentState() const { return m_navigationState; }

  /** Explains the reason for the navigation error. */
  enum TErrorCode
  {
    ERR_NONE = 0,
    ERR_EMERGENCY_STOP,
    ERR_CANNOT_REACH_TARGET,
    ERR_OTHER
  };
  struct TErrorReason
  {
    TErrorReason() = default;

    TErrorCode error_code{ERR_NONE};
    /** Human friendly description of the error */
    std::string error_msg;
  };

  /** In case of state=NAV_ERROR, this returns the reason for the error.
   * Error state is reset every time a new navigation starts with
   * a call to navigate(), or when resetNavError() is called.
   */
  inline const TErrorReason& getErrorReason() const { return m_navErrorReason; }

  /** Sets a user-provided frame transformer object; used only if providing
   * targets in a frame ID
   * different than the one in which robot odometry is given (both IDs
   * default to `"map"`).
   */
  void setFrameTF(const std::weak_ptr<mrpt::poses::FrameTransformer<2>>& frame_tf);

  /** Get the current frame tf object (defaults to nullptr) \sa setFrameTF */
  std::weak_ptr<mrpt::poses::FrameTransformer<2>> getFrameTF() const { return m_frame_tf; }

  /** By default, error exceptions on navigationStep() will dump an error
   * message to the output logger interface. If rethrow is enabled
   * (default=false), the error message will be reported as well, but
   * exceptions will be re-thrown.
   */
  void enableRethrowNavExceptions(const bool enable) { m_rethrow_exceptions = enable; }
  bool isRethrowNavExceptionsEnabled() const { return m_rethrow_exceptions; }
  /** @}*/

  struct TAbstractNavigatorParams : public mrpt::config::CLoadableOptions
  {
    /** Default value=0, means use the "targetAllowedDistance" passed by the
     * user in the navigation request. */
    double dist_to_target_for_sending_event{0};
    /** navigator timeout (seconds) [Default=30 sec] */
    double alarm_seems_not_approaching_target_timeout{30};
    /** (Default value=0.6) When closer than this distance, check if the
     * target is blocked to abort navigation with an error. */
    double dist_check_target_is_blocked{0.6};
    /** (Default=3) How many steps should the condition for
     * dist_check_target_is_blocked be fulfilled to raise an event */
    int hysteresis_check_target_is_blocked{3};

    void loadFromConfigFile(const mrpt::config::CConfigFileBase& c, const std::string& s) override;
    void saveToConfigFile(mrpt::config::CConfigFileBase& c, const std::string& s) const override;
    TAbstractNavigatorParams();
  };

  TAbstractNavigatorParams params_abstract_navigator;

  /** Gives access to a const-ref to the internal time logger used to estimate
   * delays \sa getTimeLogger() in derived classes */
  const mrpt::system::CTimeLogger& getDelaysTimeLogger() const { return m_timlog_delays; }

  /** Publicly available time profiling object. Default: disabled */
  mrpt::system::CTimeLogger m_navProfiler{false, "mrpt::nav::CAbstractNavigator"};

 private:
  /** Last internal state of navigator: */
  TState m_lastNavigationState{IDLE};
  TErrorReason m_navErrorReason;
  /** Will be false until the navigation end is sent, and it is reset with
   * each new command */
  bool m_navigationEndEventSent{false};
  int m_counter_check_target_is_blocked{0};
  bool m_rethrow_exceptions{false};

  /** Called before starting a new navigation. Internally, it calls to
   * child-implemented onStartNewNavigation() */
  void internal_onStartNewNavigation();

 protected:
  /** Events generated during navigationStep(), enqueued to be called at the
   * end of the method execution to avoid user code to change the navigator
   * state. */
  std::list<std::function<void(void)>> m_pending_events;

  void dispatchPendingNavEvents();

  /** To be implemented in derived classes */
  virtual void performNavigationStep() = 0;

  /** Called whenever a new navigation has been started. Can be used to reset
   * state variables, etc. */
  virtual void onStartNewNavigation() = 0;

  /** Called after each call to CAbstractNavigator::navigate() */
  virtual void onNavigateCommandReceived();

  /** Call to the robot getCurrentPoseAndSpeeds() and updates members
   * m_curPoseVel accordingly.
   * If an error is returned by the user callback, first, it calls
   * robot.stop() ,then throws an std::runtime_error exception. */
  virtual void updateCurrentPoseAndSpeeds();

  /** Factorization of the part inside navigationStep(), for the case of state
   * being NAVIGATING.
   * Performs house-hold tasks like raising events in case of starting/ending
   * navigation, timeout reaching destination, etc.
   * `call_virtual_nav_method` can be set to false to avoid calling the
   * virtual method performNavigationStep()
   */
  virtual void performNavigationStepNavigating(bool call_virtual_nav_method = true);

  /** Does the job of navigate(), except the call to
   * onNavigateCommandReceived() */
  virtual void processNavigateCommand(const TNavigationParams* params);

  /** Stops the robot and set navigation state to error */
  virtual void doEmergencyStop(const std::string& msg);

  /** Default: forward call to m_robot.changeSpeed(). Can be overridden. */
  virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd& vel_cmd);
  /** Default: forward call to m_robot.changeSpeedsNOP(). Can be overridden. */
  virtual bool changeSpeedsNOP();
  /** Default: forward call to m_robot.stop(). Can be overridden. */
  virtual bool stop(bool isEmergencyStop);

  /** Default implementation: check if target_dist is below the accepted
   * distance.
   * If true is returned here, the end-of-navigation event will be sent out
   * (only for non-intermediary targets).
   */
  virtual bool checkHasReachedTarget(const double targetDist) const;

  /** Checks whether the robot shape, when placed at the given pose (relative
   * to the current pose),
   * is colliding with any of the latest known obstacles.
   * Default implementation: always returns false. */
  virtual bool checkCollisionWithLatestObstacles(
      const mrpt::math::TPose2D& relative_robot_pose) const;

  /** Current internal state of navigator: */
  TState m_navigationState{IDLE};
  /** Current navigation parameters */
  std::unique_ptr<TNavigationParams> m_navigationParams;

  /** The navigator-robot interface. */
  CRobot2NavInterface& m_robot;

  /** Optional, user-provided frame transformer. */
  std::weak_ptr<mrpt::poses::FrameTransformer<2>> m_frame_tf;

  /** mutex for all navigation methods */
  std::recursive_mutex m_nav_cs;

  struct TRobotPoseVel
  {
    mrpt::math::TPose2D pose;
    mrpt::math::TTwist2D velGlobal, velLocal;
    /** raw odometry (frame does not match to "pose", but is expected to be
     * smoother in the short term). */
    mrpt::math::TPose2D rawOdometry;
    mrpt::system::TTimeStamp timestamp;
    /** map frame ID for `pose` */
    std::string pose_frame_id;
    TRobotPoseVel();
  };

  /** Current robot pose (updated in CAbstractNavigator::navigationStep() ) */
  TRobotPoseVel m_curPoseVel;
  double m_last_curPoseVelUpdate_robot_time{-1e9};
  std::string m_last_curPoseVelUpdate_pose_frame_id;
  /** Latest robot poses (updated in CAbstractNavigator::navigationStep() ) */
  mrpt::poses::CPose2DInterpolator m_latestPoses, m_latestOdomPoses;

  /** Time logger to collect delay-related stats */
  mrpt::system::CTimeLogger m_timlog_delays{true, "CAbstractNavigator::m_timlog_delays"};

  /** For sending an alarm (error event) when it seems that we are not
   * approaching toward the target in a while... */
  double m_badNavAlarm_minDistTarget;
  mrpt::system::TTimeStamp m_badNavAlarm_lastMinDistTime;

 public:
};

bool operator==(
    const CAbstractNavigator::TNavigationParamsBase&,
    const CAbstractNavigator::TNavigationParamsBase&);
}  // namespace mrpt::nav
MRPT_ENUM_TYPE_BEGIN(mrpt::nav::CAbstractNavigator::TState)
using namespace mrpt::nav;
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, IDLE);
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, NAVIGATING);
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, SUSPENDED);
MRPT_FILL_ENUM_MEMBER(CAbstractNavigator, NAV_ERROR);
MRPT_ENUM_TYPE_END()
