/* +------------------------------------------------------------------------+
   |                     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/graphs/TNodeID.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/opengl/Scene.h>
#include <mrpt/system/CTimeLogger.h>

#include <cstdlib>  // size_t
#include <string>

namespace mrpt::nav
{
/** \addtogroup nav_planners Path planning
 * \ingroup mrpt_nav_grp
 * @{ */

template <typename node_pose_t, typename world_limits_t>
struct TPlannerInputTempl
{
  node_pose_t start_pose, goal_pose;
  /** Bounding box of the world, used to draw uniform random pose samples */
  world_limits_t world_bbox_min, world_bbox_max;
  /** World obstacles, as a point cloud */
  mrpt::maps::CSimplePointsMap obstacles_points;
};

template <typename tree_t>
struct TPlannerResultTempl
{
  /** Whether the target was reached or not */
  bool success{false};
  /** Time spent (in secs) */
  double computation_time{0};
  /** Distance from best found path to goal */
  double goal_distance;
  /** Total cost of the best found path (cost ~~ Euclidean distance) */
  double path_cost;
  /** The ID of the best target node in the tree */
  mrpt::graphs::TNodeID best_goal_node_id;
  /** The set of target nodes within an acceptable distance to target
   * (including `best_goal_node_id` and others) */
  std::set<mrpt::graphs::TNodeID> acceptable_goal_node_ids;
  /** The generated motion tree that explores free space starting at "start"
   */
  tree_t move_tree;

  TPlannerResultTempl() :
      goal_distance(std::numeric_limits<double>::max()),
      path_cost(std::numeric_limits<double>::max()),
      best_goal_node_id(mrpt::graphs::INVALID_NODEID)
  {
  }
};

struct RRTEndCriteria
{
  /** Maximum distance from a pose to target to accept it as a valid solution
   * (meters). (Both acceptedDistToTarget & acceptedAngToTarget must be
   * satisfied) */
  double acceptedDistToTarget{0.1};
  /** Maximum angle from a pose to target to accept it as a valid solution
   * (rad).  (Both acceptedDistToTarget & acceptedAngToTarget must be
   * satisfied) */
  double acceptedAngToTarget{mrpt::DEG2RAD(180.0)};

  /** In seconds. 0 means no limit until a solution is found. */
  double maxComputationTime{0.0};
  /** In seconds. 0 means the first valid path will be returned. Otherwise,
   * the algorithm will try to refine and find a better one. */
  double minComputationTime{0.0};

  RRTEndCriteria() = default;
};

struct RRTAlgorithmParams
{
  /** The robot shape used when computing collisions; it's loaded from the
   *  config file/text as a single 2xN matrix in MATLAB format, first row are
   * Xs, second are Ys, e.g:
   *  \code
   *   robot_shape = [-0.2 0.2 0.2 -0.2; -0.1 -0.1 0.1 0.1]
   *  \endcode
   * \note PTGs will use only one of either `robot_shape` or
   * `robot_shape_circular_radius`
   */
  mrpt::math::TPolygon2D robot_shape;
  /** The radius of a circular-shape-model of the robot shape.
   * \note PTGs will use only one of either `robot_shape` or
   * `robot_shape_circular_radius`
   */
  double robot_shape_circular_radius{0.30};

  /** (Default: ".") */
  std::string ptg_cache_files_directory;

  /** Probabily of picking the goal as random target (in [0,1], default=0.05)
   */
  double goalBias{0.05};
  /** (Very sensitive parameter!) Max length of each edge path (in meters,
   * default=1.0) */
  double maxLength{1.0};
  /** Minimum distance [meters] to nearest node to accept creating a new one
   * (default=0.10). (Any of minDistanceBetweenNewNodes and
   * minAngBetweenNewNodes must be satisfied) */
  double minDistanceBetweenNewNodes{0.10};
  /** Minimum angle [rad] to nearest node to accept creating a new one
   * (default=15 deg) (Any of minDistanceBetweenNewNodes and
   * minAngBetweenNewNodes must be satisfied) */
  double minAngBetweenNewNodes;
  /** Display PTG construction info (default=true) */
  bool ptg_verbose{true};

  /** Frequency (in iters) of saving tree state to debug log files viewable in
   * SceneViewer3D (default=0, disabled) */
  size_t save_3d_log_freq{0};

  RRTAlgorithmParams();
};

/** Virtual base class for TP-Space-based path planners */
class PlannerTPS_VirtualBase
{
 public:
  RRTEndCriteria end_criteria;
  /** Parameters specific to this path solver algorithm */
  RRTAlgorithmParams params;

  /** ctor */
  PlannerTPS_VirtualBase();

  mrpt::system::CTimeLogger& getProfiler() { return m_timelogger; }
  const mrpt::nav::TListPTGPtr& getPTGs() const { return m_PTGs; }
  /** Options for renderMoveTree()  */
  struct TRenderPlannedPathOptions
  {
    /** Highlight the path from root towards this node (usually, the target)
     */
    mrpt::graphs::TNodeID highlight_path_to_node_id = mrpt::graphs::INVALID_NODEID;

    /** (Default=1) Draw one out of N vehicle shapes along the highlighted
     * path */
    size_t draw_shape_decimation{1};

    const mrpt::poses::CPose2D* x_rand_pose = nullptr;
    const mrpt::poses::CPose2D* x_nearest_pose = nullptr;
    const mrpt::maps::CPointsMap* local_obs_from_nearest_pose = nullptr;
    const mrpt::poses::CPose2D* new_state = nullptr;

    /** A scale factor to all XYZ corners (default=0, means auto determien
     * from vehicle shape) */
    double xyzcorners_scale{0};
    /** (Default=false) */
    bool highlight_last_added_edge{false};
    /** (Default=10 meters) Set to 0 to disable */
    double ground_xy_grid_frequency{10.0};

    /** Robot color  */
    mrpt::img::TColor color_vehicle;
    /** obstacles color  */
    mrpt::img::TColor color_obstacles;
    /** local obstacles color  */
    mrpt::img::TColor color_local_obstacles;
    /** START indication color  */
    mrpt::img::TColor color_start;
    /** END indication color  */
    mrpt::img::TColor color_goal;
    mrpt::img::TColor color_ground_xy_grid;
    mrpt::img::TColor color_normal_edge;
    mrpt::img::TColor color_last_edge;
    mrpt::img::TColor color_optimal_edge;
    float width_last_edge{3.f};
    float width_normal_edge{1.f};
    float width_optimal_edge{4.f};
    int point_size_obstacles{5};
    int point_size_local_obstacles{5};

    /** (Default=0.01) Height (Z coordinate) for the vehicle shapes. Helps
     * making it in the "first plane" */
    double vehicle_shape_z{0.01};
    /** Robot line width for visualization - default 2.0 */
    double vehicle_line_width{2.0};
    /** (Default=true) */
    bool draw_obstacles{true};

    std::string log_msg;
    mrpt::math::TPoint3D log_msg_position;
    double log_msg_scale{0.2};

    TRenderPlannedPathOptions() :
        color_vehicle(0xFF, 0x00, 0x00, 0xFF),
        color_obstacles(0x00, 0x00, 0xFF, 0x40),
        color_local_obstacles(0x00, 0x00, 0xFF),
        color_start(0x00, 0x00, 0x00, 0x00),
        color_goal(0x00, 0x00, 0x00, 0x00),
        color_ground_xy_grid(0xFF, 0xFF, 0xFF),
        color_normal_edge(0x22, 0x22, 0x22, 0x40),
        color_last_edge(0xff, 0xff, 0x00),
        color_optimal_edge(0x00, 0x00, 0x00),
        log_msg_position(0, 0, 0)
    {
    }

    virtual ~TRenderPlannedPathOptions() = default;
  };

  template <typename node_pose_t, typename world_limits_t, typename tree_t>
  void renderMoveTree(
      mrpt::opengl::Scene& scene,
      const TPlannerInputTempl<node_pose_t, world_limits_t>& pi,
      const TPlannerResultTempl<tree_t>& result,
      const TRenderPlannedPathOptions& options);

 protected:
  mrpt::system::CTimeLogger m_timelogger;
  bool m_initialized_PTG{false};
  mrpt::nav::TListPTGPtr m_PTGs;
  mrpt::maps::CSimplePointsMap m_local_obs;  // Temporary map. Defined as a
  // member to save realloc time
  // between calls

  /** Load all PTG params from a config file source */
  void internal_loadConfig_PTG(
      const mrpt::config::CConfigFileBase& cfgSource,
      const std::string& sSectionName = std::string("PTG_CONFIG"));

  /** Must be called after setting all params (see
   * `internal_loadConfig_PTG()`) and before calling `solve()` */
  void internal_initialize_PTG();

  static void transformPointcloudWithSquareClipping(
      const mrpt::maps::CPointsMap& in_map,
      mrpt::maps::CPointsMap& out_map,
      const mrpt::poses::CPose2D& asSeenFrom,
      const double MAX_DIST_XY);

  void spaceTransformer(
      const mrpt::maps::CSimplePointsMap& in_obstacles,
      const mrpt::nav::CParameterizedTrajectoryGenerator* in_PTG,
      const double MAX_DIST,
      std::vector<double>& out_TPObstacles);

  void spaceTransformerOneDirectionOnly(
      const int tp_space_k_direction,
      const mrpt::maps::CSimplePointsMap& in_obstacles,
      const mrpt::nav::CParameterizedTrajectoryGenerator* in_PTG,
      const double MAX_DIST,
      double& out_TPObstacle_k);

};  // end class PlannerTPS_VirtualBase
/** @} */
}  // namespace mrpt::nav
#include "impl_renderMoveTree.h"
