#ifndef MOVEIT_MEDIATOR_
#define MOVEIT_MEDIATOR_

#include <ros/ros.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/ApplyPlanningScene.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_visual_tools/moveit_visual_tools.h>


#include "impl/abstract_mediator.h"
#include "impl/abstract_robot.h"
#include "impl/robot.h"
#include "impl/moveit_robot.h"

#include "impl/wing_moveit_decorator.h"
#include "impl/collision_helper.h"

#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/generate_place_pose.h>
#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/predicate_filter.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/stages/fixed_state.h>

#include <eigen_conversions/eigen_msg.h>
#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
#include <moveit/planning_scene_monitor/current_state_monitor.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include "reader/job_reader.h"
#include "reader/cuboid_reader.h"

#include <stdint.h>


class Moveit_mediator : public Abstract_mediator{
    protected:
    std::shared_ptr<ros::NodeHandle> nh_;
    std::shared_ptr<moveit::core::RobotModel> robot_model_;
    std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;

    // planning Interfaces and planing scene 
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_;
    std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_;
    std::shared_ptr<planning_scene::PlanningScene> ps_;
    std::shared_ptr<ros::Publisher> planning_scene_diff_publisher_;
    std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;


    // task constructor
    std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_;
    std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
    std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;

    std::unique_ptr<Job_reader> job_reader_; 
    std::unique_ptr<Cuboid_reader> cuboid_reader_;

    std::map<std::string, std::vector<uint8_t>> acm_;
    std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_;





    public:
    Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);

    void setup_task();
    inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;};


    bool check_collision(const int& robot) override;
    void mediate() override;
    void build_wings(std::bitset<3>& wing,int& robot) override;
    void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override;
    void connect_robots(Abstract_robot* robot) override;


    void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
    void merge_acm(moveit_msgs::PlanningScene& in);

    void task_planner();
    void publish_tables();
    void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
    void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);
    moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
    inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
    inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;};



};

#endif