/* +------------------------------------------------------------------------+
   |                     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/maps/CPointCloudFilterBase.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/math/filters.h>
#include <mrpt/nav/holonomic/CAbstractHolonomicReactiveMethod.h>
#include <mrpt/nav/holonomic/ClearanceDiagram.h>
#include <mrpt/nav/reactive/CLogFileRecord.h>
#include <mrpt/nav/reactive/CMultiObjectiveMotionOptimizerBase.h>
#include <mrpt/nav/reactive/CWaypointsNavigator.h>
#include <mrpt/nav/reactive/TCandidateMovementPTG.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/datetime.h>

#include <memory>  // unique_ptr

namespace mrpt::nav
{
/** Base class for reactive navigator systems based on TP-Space, with an
 * arbitrary holonomic
 * reactive method running on it and any number of PTGs for transforming the
 * navigation space.
 * Both, the holonomic method and the PTGs can be customized by the appropriate
 * user derived classes.
 *
 * How to use:
 *  - Instantiate a reactive navigation object (one of the derived classes of
 * this virtual class).
 *  - A class with callbacks must be defined by the user and provided to the
 * constructor (derived from CRobot2NavInterface)
 *  - loadConfigFile() must be called to set up the bunch of parameters from a
 * config file (could be a memory-based virtual config file).
 *  - 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.
 *
 * For working examples, refer to the source code of the apps:
 *  -
 * [ReactiveNavigationDemo](http://www.mrpt.org/list-of-mrpt-apps/application-reactivenavigationdemo/)
 *  -
 * [ReactiveNav3D-Demo](http://www.mrpt.org/list-of-mrpt-apps/application-reactivenav3d-demo/)
 *
 * Publications:
 *  - See derived classes for papers on each specific method.
 *
 * Available "variables" or "score names" for each motion candidate (these can
 * be used in runtime-compiled expressions
 * in the configuration files of motion deciders):
 *
 * - `clearance`: Clearance (larger means larger distances to obstacles) for
 * the path from "current pose" up to "end of trajectory".
 * - `collision_free_distance`: Normalized [0,1] collision-free distance in
 * selected path. For NOP candidates, the traveled distances is subtracted.
 * - `dist_eucl_final`: Euclidean distance (in the real-world WordSpace)
 * between "end of trajectory" and target.
 * - `eta`: Estimated Time of Arrival at "end of trajectory".
 * - `holo_stage_eval`: Final evaluation of the selected direction from inside
 * of the holonomic algorithm.
 * - `hysteresis`: Measure of similarity with previous command [0,1]
 * - `is_PTG_cont`: 1 (is "NOP" motion command), 0 otherwise
 * - `is_slowdown`: 1 if PTG returns true in
 * CParameterizedTrajectoryGenerator::supportSpeedAtTarget() for this step.
 * - `move_cur_d`: Normalized distance already traveled over the selected PTG.
 * Normally 0, unless in a "NOP motion".
 * - `move_k`: Motion candidate path 0-based index.
 * - `num_paths`: Number of paths in the PTG
 * - `original_col_free_dist`: Only for "NOP motions", the collision-free
 * distance when the motion command was originally issued.
 * - `ptg_idx`: PTG index (0-based)
 * - `ptg_priority`: Product of PTG getScorePriority() times PTG
 * evalPathRelativePriority()
 * - `ref_dist`: PTG ref distance [m]
 * - `robpose_x`, `robpose_y`, `robpose_phi`: Robot pose ([m] and [rad]) at the
 * "end of trajectory": at collision or at target distance.
 * - `target_d_norm`: Normalized target distance. Can be >1 if distance is
 * larger than ref_distance.
 * - `target_dir`: Angle of target in TP-Space [rad]
 * - `target_k`: Same as target_dir but in discrete path 0-based indices.
 * - `WS_target_x`, `WS_target_y`: Target coordinates in realworld [m]
 *
 * \sa CReactiveNavigationSystem, CReactiveNavigationSystem3D
 *  \ingroup nav_reactive
 */
class CAbstractPTGBasedReactive : public CWaypointsNavigator
{
 public:
  /** The struct for configuring navigation requests to
   * CAbstractPTGBasedReactive and derived classes. */
  struct TNavigationParamsPTG : public CWaypointsNavigator::TNavigationParamsWaypoints
  {
    /** (Default=empty) Optionally, a list of PTG indices can be sent such
     * that
     *  the navigator will restrict itself to only employ those PTGs. */
    std::vector<size_t> restrict_PTG_indices;

    std::string getAsText() const override;
    std::unique_ptr<TNavigationParams> clone() const override
    {
      return std::unique_ptr<TNavigationParams>(new TNavigationParamsPTG(*this));
    }

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

  /** Constructor.
   * \param[in] react_iterf_impl An instance of an object that implement all
   * the required interfaces to read from and control a robot.
   * \param[in] enableConsoleOutput Can be set to false to reduce verbosity.
   * \param[in] enableLogFile Set to true to enable creation of navigation
   * log files, useful for inspection and debugging.
   */
  CAbstractPTGBasedReactive(
      CRobot2NavInterface& react_iterf_impl,
      bool enableConsoleOutput = true,
      bool enableLogFile = false,
      const std::string& logFileDirectory = std::string("./reactivenav.logs"));

  ~CAbstractPTGBasedReactive() override;

  /** Must be called for loading collision grids, or the first navigation
   * command may last a long time to be executed.
   * Internally, it just calls STEP1_CollisionGridsBuilder().
   */
  void initialize() override;

  /** Selects which one from the set of available holonomic methods will be
   * used
   *  into transformed TP-Space, and sets its configuration from a
   * configuration file.
   * Available methods: class names of those derived from
   * CAbstractHolonomicReactiveMethod
   */
  void setHolonomicMethod(const std::string& method, const mrpt::config::CConfigFileBase& cfgBase);

  /** Provides a copy of the last log record with information about execution.
   * \param o An object where the log will be stored into.
   * \note Log records are not prepared unless either "enableLogFile" is
   * enabled in the constructor or "enableLogFile()" has been called.
   */
  void getLastLogRecord(CLogFileRecord& o);

  /** Enables keeping an internal registry of navigation logs that can be
   * queried with getLastLogRecord() */
  void enableKeepLogRecords(bool enable = true) { m_enableKeepLogRecords = enable; }

  /** Enables/disables saving log files. */
  void enableLogFile(bool enable);

  /** Changes the prefix for new log files. */
  void setLogFileDirectory(const std::string& sDir) { m_navlogfiles_dir = sDir; }
  std::string getLogFileDirectory() const { return m_navlogfiles_dir; }
  struct TAbstractPTGNavigatorParams : public mrpt::config::CLoadableOptions
  {
    /** C++ class name of the holonomic navigation method to run in the
     * transformed TP-Space */
    std::string holonomic_method;
    /** C++ class name of the motion chooser */
    std::string motion_decider_method;

    /** (Default: ".") */
    std::string ptg_cache_files_directory;
    /** Maximum distance up to obstacles will be considered (D_{max} in
     * papers). */
    double ref_distance{4.0};
    /** Time constant (in seconds) for the low-pass filter applied to
     * kinematic velocity commands (default=0: no filtering) */
    double speedfilter_tau{0.0};

    /** In normalized distances, the start and end of a ramp function that
     * scales the velocity
     *  output from the holonomic navigator:
     *
     * \code
     *  velocity scale
     *   ^
     *   |           _____________
     *   |          /
     * 1 |         /
     *   |        /
     * 0 +-------+---|----------------> normalized distance
     *         Start
     *              End
     * \endcode
     *
     */
    double secure_distance_start{0.05}, secure_distance_end{0.20};
    bool use_delays_model{false};
    /** Max distance [meters] to discard current PTG and issue a new vel cmd
     * (default= 0.05) */
    double max_distance_predicted_actual_path{0.15};
    /** Min normalized dist [0,1] after current pose in a PTG continuation
     * to allow it. */
    double min_normalized_free_space_for_ptg_continuation{0.2};

    /** Params related to speed limits. */
    mrpt::kinematics::CVehicleVelCmd::TVelCmdParams robot_absolute_speed_limits;
    bool enable_obstacle_filtering{true};
    /** Default: false */
    bool evaluate_clearance{false};
    /** Max dist [meters] to use time-based path prediction for NOP
     * evaluation. */
    double max_dist_for_timebased_path_prediction{2.0};

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

  TAbstractPTGNavigatorParams params_abstract_ptg_navigator;

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

  /** Enables/disables the detailed time logger (default:disabled upon
   * construction)
   *  When enabled, a report will be dumped to std::cout upon destruction.
   * \sa getTimeLogger
   */
  void enableTimeLog(bool enable = true) { m_timelogger.enable(enable); }
  /** Gives access to a const-ref to the internal time logger \sa
   * enableTimeLog */
  const mrpt::system::CTimeLogger& getTimeLogger() const { return m_timelogger; }

  /** Returns the number of different PTGs that have been setup */
  virtual size_t getPTG_count() const = 0;
  /** Gets the i'th PTG */
  virtual CParameterizedTrajectoryGenerator* getPTG(size_t i) = 0;
  /** Gets the i'th PTG */
  virtual const CParameterizedTrajectoryGenerator* getPTG(size_t i) const = 0;

  /** Get the current, global (honored for all PTGs) robot speed limits */
  const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& getCurrentRobotSpeedLimits() const
  {
    return params_abstract_ptg_navigator.robot_absolute_speed_limits;
  }

  /** Changes the current, global (honored for all PTGs) robot speed limits,
   * via returning a reference to a structure that holds those limits */
  mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& changeCurrentRobotSpeedLimits()
  {
    return params_abstract_ptg_navigator.robot_absolute_speed_limits;
  }

  /** Changes this parameter in all inner holonomic navigator instances [m].
   */
  void setTargetApproachSlowDownDistance(const double dist);
  /** Returns this parameter for the first inner holonomic navigator instances
   * [m] (should be the same in all of them?) */
  double getTargetApproachSlowDownDistance() const;

 protected:
  /** The main method for the navigator */
  void performNavigationStep() override;

  virtual CAbstractHolonomicReactiveMethod* getHoloMethod(int idx);

  /** The holonomic navigation algorithm (one object per PTG, so internal
   * states are maintained) */
  std::vector<CAbstractHolonomicReactiveMethod::Ptr> m_holonomicMethod;
  std::unique_ptr<mrpt::io::CStream> m_logFile;
  /** The current log file stream, or nullptr if not being used */
  mrpt::io::CStream* m_prev_logfile{nullptr};
  /** See enableKeepLogRecords */
  bool m_enableKeepLogRecords{false};
  /** The last log */
  CLogFileRecord lastLogRecord;
  /** Last velocity commands */
  mrpt::kinematics::CVehicleVelCmd::Ptr m_last_vel_cmd;

  /** Critical zones */
  std::recursive_mutex m_critZoneLastLog;

  /** Enables / disables the console debug output. */
  bool m_enableConsoleOutput;
  /** Whether \a loadConfigFile() has been called or not. */
  bool m_init_done{false};
  /** Used to estimate the period at which we are being called */
  double m_lastExecutionTimestamp = 0;

  /** A complete time logger \sa enableTimeLog() */
  mrpt::system::CTimeLogger m_timelogger{false};  // default: disabled
  bool m_PTGsMustBeReInitialized{true};

  /** @name Variables for CReactiveNavigationSystem::performNavigationStep
    @{ */
  mrpt::system::CTicTac totalExecutionTime, executionTime, tictac;
  mrpt::math::LowPassFilter_IIR1 meanExecutionTime{0.7, 1};
  mrpt::math::LowPassFilter_IIR1 meanTotalExecutionTime{0.7, 1};
  /** Runtime estimation of execution period of the method. */
  mrpt::math::LowPassFilter_IIR1 meanExecutionPeriod{0.7, 1};
  mrpt::math::LowPassFilter_IIR1 tim_changeSpeed_avr{0.7}, timoff_obstacles_avr{0.7},
      timoff_curPoseAndSpeed_avr{0.7}, timoff_sendVelCmd_avr{0.7};
  /** @} */

  bool impl_waypoint_is_reachable(
      const mrpt::math::TPoint2D& wp_local_wrt_robot) const override;  // See docs in base class

  // Steps for the reactive navigation sytem.
  // ----------------------------------------------------------------------------
  virtual void STEP1_InitPTGs() = 0;

  /** Return false on any fatal error */
  virtual bool implementSenseObstacles(mrpt::system::TTimeStamp& obs_timestamp) = 0;
  bool STEP2_SenseObstacles();

  /** Builds TP-Obstacles from Workspace obstacles for the given PTG.
   * "out_TPObstacles" is already initialized to the proper length and
   * maximum collision-free distance for each "k" trajectory index.
   * Distances are in "pseudo-meters". They will be normalized automatically
   * to [0,1] upon return. */
  virtual void STEP3_WSpaceToTPSpace(
      const size_t ptg_idx,
      std::vector<double>& out_TPObstacles,
      mrpt::nav::ClearanceDiagram& out_clearance,
      const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense,
      const bool eval_clearance) = 0;

  /** Generates a pointcloud of obstacles, and the robot shape, to be saved in
   * the logging record for the current timestep */
  virtual void loggingGetWSObstaclesAndShape(CLogFileRecord& out_log) = 0;

  struct PTGTarget
  {
    PTGTarget() = default;

    /** For each PTG, whether target falls into the PTG domain. */
    bool valid_TP = false;

    /** The Target, in TP-Space (x,y), plust its actual relative heading (phi)
     *  wrt to the current robot pose. */
    mrpt::math::TPose2D TP_Target;

    /** TP-Target */
    double target_alpha = 0, target_dist = 0;

    /** The discrete version of target_alpha */
    int target_k = 0;
  };

  /** Scores \a holonomicMovement */
  void calc_move_candidate_scores(
      TCandidateMovementPTG& holonomicMovement,
      const std::vector<double>& in_TPObstacles,
      const mrpt::nav::ClearanceDiagram& in_clearance,
      const std::vector<mrpt::math::TPose2D>& WS_Targets,
      const std::vector<PTGTarget>& TP_Targets,
      CLogFileRecord::TInfoPerPTG& log,
      CLogFileRecord& newLogRec,
      const bool this_is_PTG_continuation,
      const mrpt::math::TPose2D& relPoseVelCmd_NOP,
      const unsigned int ptg_idx4weights,
      const mrpt::system::TTimeStamp tim_start_iteration,
      const mrpt::nav::CHolonomicLogFileRecord::Ptr& hlfr);

  /** Return the [0,1] velocity scale of raw PTG cmd_vel */
  virtual double generate_vel_cmd(
      const TCandidateMovementPTG& in_movement, mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd);
  void STEP8_GenerateLogRecord(
      CLogFileRecord& newLogRec,
      const std::vector<mrpt::math::TPose2D>& relTargets,
      int nSelectedPTG,
      const mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd,
      int nPTGs,
      const bool best_is_NOP_cmdvel,
      const math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP,
      const math::TPose2D& rel_pose_PTG_origin_wrt_sense_NOP,
      const double executionTimeValue,
      const double tim_changeSpeed,
      const mrpt::system::TTimeStamp& tim_start_iteration);

  /** To be called during children destructors to assure thread-safe
   * destruction, and free of shared objects. */
  void preDestructor();
  void onStartNewNavigation() override;

  /** Signal that the destructor has been called, so no more calls are
   * accepted from other threads */
  bool m_closing_navigator{false};

  mrpt::system::TTimeStamp m_WS_Obstacles_timestamp{INVALID_TIMESTAMP};
  /** Default: none */
  mrpt::maps::CPointCloudFilterBase::Ptr m_WS_filter;

  mrpt::nav::CMultiObjectiveMotionOptimizerBase::Ptr m_multiobjopt;

  struct TInfoPerPTG
  {
    std::vector<PTGTarget> targets;
    /** One distance per discretized alpha value, describing the "polar
     * plot" of TP obstacles. */
    std::vector<double> TP_Obstacles;
    /** Clearance for each path */
    ClearanceDiagram clearance;
  };

  /** Temporary buffers for working with each PTG during a navigationStep() */
  std::vector<TInfoPerPTG> m_infoPerPTG;
  mrpt::system::TTimeStamp m_infoPerPTG_timestamp{INVALID_TIMESTAMP};

  void build_movement_candidate(
      CParameterizedTrajectoryGenerator* ptg,
      const size_t indexPTG,
      const std::vector<mrpt::math::TPose2D>& relTargets,
      const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense,
      TInfoPerPTG& ipf,
      TCandidateMovementPTG& holonomicMovement,
      CLogFileRecord& newLogRec,
      const bool this_is_PTG_continuation,
      mrpt::nav::CAbstractHolonomicReactiveMethod& holoMethod,
      const mrpt::system::TTimeStamp tim_start_iteration,
      const TNavigationParams& navp = TNavigationParams(),
      const mrpt::math::TPose2D& relPoseVelCmd_NOP = mrpt::math::TPose2D(0, 0, 0));

  struct TSentVelCmd
  {
    TSentVelCmd() = default;

    void reset() { *this = TSentVelCmd(); }
    bool isValid() const { return this->poseVel.timestamp != INVALID_TIMESTAMP; }
    /** 0-based index of used PTG */
    int ptg_index = -1;
    /** Path index for selected PTG */
    int ptg_alpha_index = -1;
    /** Timestamp of when the cmd was sent */
    mrpt::system::TTimeStamp tim_send_cmd_vel = INVALID_TIMESTAMP;
    /** Robot pose & velocities and timestamp of when it was queried */
    TRobotPoseVel poseVel;
    /** TP-Obstacles in the move direction at the instant of picking this
     * movement */
    double colfreedist_move_k = .0;
    bool was_slowdown = false;
    /** [0,1] scale of the raw cmd_vel as generated by the PTG */
    double speed_scale = 1.0;
    double original_holo_eval = .0;
    CParameterizedTrajectoryGenerator::TNavDynamicState ptg_dynState;
  };

  TSentVelCmd m_lastSentVelCmd;

 private:
  /** Delete m_holonomicMethod */
  void deleteHolonomicObjects();

  /** Default: "./reactivenav.logs" */
  std::string m_navlogfiles_dir;

  double m_expr_var_k, m_expr_var_k_target, m_expr_var_num_paths;
  /** A copy of last-iteration navparams, used to detect changes */
  std::unique_ptr<TNavigationParams> m_copy_prev_navParams;

};  // end of CAbstractPTGBasedReactive
}  // namespace mrpt::nav
