#ifndef _PLANNER_MANAGER_H_
#define _PLANNER_MANAGER_H_
#include <torch/torch.h>
#include <torch/script.h>
#include <stdlib.h>

#include <optimizer/poly_traj_optimizer.h>
#include <traj_utils/DataDisp.h>
#include <plan_env/grid_map.h>
#include <traj_utils/plan_container.hpp>
#include <ros/ros.h>
#include <traj_utils/planning_visualization.h>
#include <plan_env/local_map.hpp>
#include <optimizer/poly_traj_utils.hpp>
#include <optimizer/global_traj_opt.hpp>
#include <plan_env/global_map.hpp>
#include <path_searching/global_astar.h>
#include <path_searching/hybrida.h>
#include <cv_bridge/cv_bridge.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include "plan_manage/minco_qp.hpp"
#include <ros/package.h>
#include <OsqpEigen/OsqpEigen.h>
#include <unsupported/Eigen/CXX11/Tensor>
#include <c10/cuda/CUDAStream.h>
#include <ATen/cuda/CUDAContext.h>
#include <plan_manage/minco_nlp.hpp>
#include <mutex>
#include "ego_planner/NNDebug.h"

namespace ego_planner
{

  // Fast Planner Manager
  // Key algorithms of mapping and planning are called

  class EGOPlannerManager
  {
    // SECTION stable
  public:
    EGOPlannerManager();
    ~EGOPlannerManager();

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /* main planning interface */
    void initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis = NULL);
    bool computeInitState(
        const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel,
        const Eigen::Vector3d &start_acc, const Eigen::Vector3d &local_target_pt,
        const Eigen::Vector3d &local_target_vel, const bool flag_polyInit,
        const bool flag_randomPolyTraj, const double &ts, poly_traj::MinJerkOpt &initMJO);
    bool reboundReplan(
        const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel,
        const Eigen::Vector3d &start_acc, const Eigen::Vector3d &end_pt,
        const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc, const bool flag_polyInit,
        const bool flag_randomPolyTraj, const bool touch_goal);
    bool reboundReplanNN(
        const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel,
        const Eigen::Vector3d &start_acc, const Eigen::Vector3d &end_pt,
        const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_ac);

    bool reboundReplanIplanner(
          const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel,
          const Eigen::Vector3d &start_acc, const Eigen::Vector3d &end_pt,
          const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc);


    bool reboundReplanNN_Local(
    const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel,
    const Eigen::Vector3d &start_acc, const Eigen::Vector3d &end_pt,
    const Eigen::Vector3d &end_vel,  const Eigen::Vector3d &end_acc, Eigen::MatrixXf local_traj);
    bool planGlobalTrajWaypoints(
        const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel,
        const Eigen::Vector3d &start_acc, const std::vector<Eigen::Vector3d> &waypoints,
        const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc);
    void getLocalTarget(
        const double planning_horizen,
        const Eigen::Vector3d &start_pt, const Eigen::Vector3d &global_end_pt,
        Eigen::Vector3d &local_target_pos, Eigen::Vector3d &local_target_vel,
        Eigen::Vector3d &local_target_acc,
        bool &touch_goal);
    void getLocalTarget_me(
        const double planning_horizen,
        const Eigen::Vector3d &start_pt, const Eigen::Vector3d &global_end_pt,
        Eigen::Vector3d &local_target_pos, Eigen::Vector3d &local_target_vel,
        Eigen::Vector3d &local_target_acc,
        bool &touch_goal);
    bool EmergencyStop(Eigen::Vector3d stop_pos);
    bool checkCollision(int drone_id);
    bool setLocalTrajFromOpt(const poly_traj::MinJerkOpt &opt, const bool touch_goal);
    inline double getSwarmClearance(void) { return ploy_traj_opt_->get_swarm_clearance_(); }
    inline int getCpsNumPrePiece(void) { return ploy_traj_opt_->get_cps_num_prePiece_(); }
    // inline PtsChk_t getPtsCheck(void) { return ploy_traj_opt_->get_pts_check_(); }

    PlanParameters pp_;
    GridMap::Ptr grid_map_;
    TrajContainer traj_;

    // net utils
    MincoNLP minco_nlp_;
    torch::jit::script::Module nn_planner, nn_planner_consistency, iplanner;
    ros::Publisher nn_depth_debug_pub_;
    ros::Publisher nn_traj_vis_pub_;
    ros::Publisher nn_corridor_pub_;
    ros::Publisher nn_debug_pub_;
    visualization_msgs::MarkerArray corridor_marker_array_;
    visualization_msgs::Marker qp_traj;
    visualization_msgs::Marker corridor_marker;
    std::mutex mutex_depth;
    ros::Timer cudaWarmTimer;
    void warm(const ros::TimerEvent &);
    map_util::VoxelMapUtil global_map_;

  private:
    PlanningVisualization::Ptr visualization_;
    int velocity_type = 0;
    int planner_type_ = 0;

    PolyTrajOptimizer::Ptr ploy_traj_opt_;

    std::unique_ptr<GraphSearch::Astar> astar_path_finder_;
    global_traj::GlobalTrajOptimizer<map_util::VoxelMapUtil> global_traj_opt_;
    int continous_failures_count_{0};
    double start_global_time_, end_global_time_;
    ros::NodeHandle prv_nh_;
    ros::NodeHandle nh_;
    
    local_map_util::VoxelMapUtil local_map_;
    global_traj::GlobalTrajOptimizer<local_map_util::VoxelMapUtil> local_traj_opt_;
    std::unique_ptr<hybrid::KinodynamicAstar<local_map_util::VoxelMapUtil>> hybrid_path_finder_;


    

  public:
    typedef unique_ptr<EGOPlannerManager> Ptr;
    Eigen::MatrixXf last_traj;
    // !SECTION
  };
} // namespace ego_planner

#endif