#include "impl/moveit_mediator.h"
#include <thread>
#include <mutex> 
#include <algorithm>
#include <gb_grasp/MapGenerator.h>
#include <regex>

#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/tree_node.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
#include "bt/planning_scene.h"
#include "bt/trajetory.h"
#include "bt/execution.h"
#include "bt/position_condition.h"
#include "bt/parallel_robot.h"
#include <actionlib_msgs/GoalStatusArray.h>




thread_local moveit::task_constructor::Task task__;
thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;

std::mutex task_writing;
//std::mutex publish;

void Moveit_mediator::connect_robots(Abstract_robot* robot) {
	robots_.push_back(robot);
	ROS_INFO("%s connected...", robot->name().c_str());

	Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
	acm_.insert(std::pair<std::string, std::vector<uint8_t>>(mr->map().at("base").c_str(), std::vector<uint8_t>()));
	for(auto name: mr->mgi()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
	for(auto name: mr->mgi_hand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
}

void Moveit_mediator::publish_tables(){
    ros::WallDuration sleep_time(1.0);
    for(long unsigned int i = 0; i < robots_.size();i++){
        Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);

		/*
		moveit_msgs::CollisionObject table;
		table.id = ceti->map().at("base").c_str();
		table.header.frame_id = "world";
		table.header.stamp = ros::Time::now();
		table.primitives.resize(1);
		table.primitives[0].type = table.primitives[0].BOX;
		table.primitives[0].dimensions.resize(3);
		table.primitives[0].dimensions[0] = ceti->size().getX();
		table.primitives[0].dimensions[1] = ceti->size().getY();
		table.primitives[0].dimensions[2] = ceti->tf().getOrigin().getZ() * 2;
		table.primitive_poses.resize(1);
		table.primitive_poses[0].position.x = ceti->tf().getOrigin().getX();
		table.primitive_poses[0].position.y = ceti->tf().getOrigin().getY();
		table.primitive_poses[0].position.z = ceti->tf().getOrigin().getZ();
		table.primitive_poses[0].orientation.x = ceti->tf().getRotation().getX();
		table.primitive_poses[0].orientation.y = ceti->tf().getRotation().getY();
		table.primitive_poses[0].orientation.z = ceti->tf().getRotation().getZ();
		table.primitive_poses[0].orientation.w = ceti->tf().getRotation().getW();
		table.operation = table.ADD;
		*/
		//psi_->applyCollisionObject(table);

        for (Abstract_robot_element* are : ceti->observers()) {
            Wing_moveit_decorator* wmd = dynamic_cast<Wing_moveit_decorator*>(are);
            psi_->applyCollisionObject(*wmd->markers());
			acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->markers()->id.c_str(), std::vector<uint8_t>()));
            sleep_time.sleep();
        }
    }
}

bool Moveit_mediator::check_collision(const int& robot){
    auto env = ps_->getCollisionEnvNonConst();
    Collision_helper* ch = static_cast<Collision_helper*>(env.get());
    return ch->collision_detector("object_A", "object_B"); //
}

void Moveit_mediator::mediate(){
    publish_tables();
	task_planner();
	setup_task();	
	ros::waitForShutdown();


};

void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
    std::bitset<3> result = robots_[robot]->observer_mask() & wing;
    Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);

    for (std::size_t i = 0; i < result.size(); i++){
        if (result[i]){
            ceti->register_observers(wings_[robot][i]);
        }
    }
}

void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp){
    for (int i =0; i < wbp.size(); i++){
        std::vector<Abstract_robot_element*> v;
        for (int j =0; j < wbp[i].first.size(); j++){
            Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i].first[j].name_, wbp[i].first[j].pose_,wbp[i].first[j].size_));
            v.push_back(are);
        }
        wings_.push_back(v);
    }
};

void Moveit_mediator::setup_task(){
	ROS_INFO("=> write Task Constructor Objects");
	for(auto& a: acm_) a.second.resize(acm_.size(), 0);
	/*
	
	bool coop = false;

	for(int i = 0; i < objects_.size(); i++){
        if (i+1 < objects_.size()){
            for (long unsigned int j = objects_[i].size()-1; j > 0; j--){
                if(objects_[i][j].getOrigin().distance(objects_[i+1].back().getOrigin()) == 0) {
					objects_[i+1].pop_back(); 
					coop= true;
				}
            }
        }
    }

	if(coop){
		moveit_msgs::CollisionObject bottle;
		bottle.id = "bottle";
		bottle.header.frame_id = "world";
		bottle.header.stamp = ros::Time::now();
		bottle.primitives.resize(1);
		bottle.primitives[0].type = bottle.primitives[0].BOX;
		bottle.primitives[0].dimensions.resize(3);
		bottle.primitives[0].dimensions[0] = box_size.getX();
		bottle.primitives[0].dimensions[1] = box_size.getY();
		bottle.primitives[0].dimensions[2] = box_size.getZ();

		bottle.primitive_poses.resize(1);
		bottle.primitive_poses[0].position.x = -0.3f;
		bottle.primitive_poses[0].position.y = -0.6f;
		bottle.primitive_poses[0].position.z = 0.9355f;
		bottle.primitive_poses[0].orientation.x = 0;
		bottle.primitive_poses[0].orientation.y = 0;
		bottle.primitive_poses[0].orientation.z = 0;
		bottle.primitive_poses[0].orientation.w = 1;
		bottle.operation = bottle.ADD;

		psi_->applyCollisionObject(bottle);

		
		for( int i = 0; i < objects_.size();i++){
			for(int j =0; j < objects_[i].size();j++){
				Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
				moveit::task_constructor::Task task = create_Task(mr, psi_->getObjects().at("bottle"), objects_[i][j]);
				if (task.plan(1)){
					task.execute(*task.solutions().front());
				}
				
			}
		}
	} else {
		acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle0", std::vector<uint8_t>()));
		acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle1", std::vector<uint8_t>()));

		moveit_msgs::CollisionObject bottle1;
		bottle1.id = "bottle0";
		bottle1.header.frame_id = "world";
		bottle1.header.stamp = ros::Time::now();
		bottle1.primitives.resize(1);
		bottle1.primitives[0].type = bottle1.primitives[0].BOX;
		bottle1.primitives[0].dimensions.resize(3);
		bottle1.primitives[0].dimensions[0] = box_size.getX();
		bottle1.primitives[0].dimensions[1] = box_size.getY();
		bottle1.primitives[0].dimensions[2] = box_size.getZ();

		bottle1.primitive_poses.resize(1);
		bottle1.primitive_poses[0].position.x = objects_[0][0].getOrigin().getX();
		bottle1.primitive_poses[0].position.y = objects_[0][0].getOrigin().getY();
		bottle1.primitive_poses[0].position.z = 0.9355f;
		bottle1.primitive_poses[0].orientation.x = 0;
		bottle1.primitive_poses[0].orientation.y = 0;
		bottle1.primitive_poses[0].orientation.z = 0;
		bottle1.primitive_poses[0].orientation.w = 1;
		bottle1.operation = bottle1.ADD;

		moveit_msgs::CollisionObject bottle2;
		bottle2.id = "bottle1";
		bottle2.header.frame_id = "world";
		bottle2.header.stamp = ros::Time::now();
		bottle2.primitives.resize(1);
		bottle2.primitives[0].type = bottle2.primitives[0].BOX;
		bottle2.primitives[0].dimensions.resize(3);
		bottle2.primitives[0].dimensions[0] = box_size.getX();
		bottle2.primitives[0].dimensions[1] = box_size.getY();
		bottle2.primitives[0].dimensions[2] = box_size.getZ();

		bottle2.primitive_poses.resize(1);
		bottle2.primitive_poses[0].position.x = objects_[1][0].getOrigin().getX(); 
		bottle2.primitive_poses[0].position.y = objects_[1][0].getOrigin().getY();
		bottle2.primitive_poses[0].position.z = 0.9355f;
		bottle2.primitive_poses[0].orientation.x = 0;
		bottle2.primitive_poses[0].orientation.y = 0;
		bottle2.primitive_poses[0].orientation.z = 0;
		bottle2.primitive_poses[0].orientation.w = 1;
		bottle2.operation = bottle2.ADD;

		psi_->applyCollisionObject(bottle1);
		psi_->applyCollisionObject(bottle2);

		std::vector<std::thread> ths;
		std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> tasks;

		for (int i = 0 ; i < objects_.size(); i++){
			std::stringstream ss;
			ss << "bottle" << std::to_string(i);
			Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
			std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot;
			for (int j = 1; j < objects_[i].size(); j++){
				moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(ss.str()), objects_[i][j]);
				if(mgt.plan(1)) {
					moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
					mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
					tasks_per_robot.push(e);

					moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
					temp.operation = temp.MOVE;
					temp.pose.position.x = objects_[i][j].getOrigin().getX();
					temp.pose.position.y = objects_[i][j].getOrigin().getY();
					psi_->applyCollisionObject(temp);
				}		
			}

			moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
			temp.operation = temp.MOVE;
			temp.pose.position.x = objects_[i][0].getOrigin().getX();
			temp.pose.position.y = objects_[i][0].getOrigin().getY();
			psi_->applyCollisionObject(temp);
			tasks.push_back(tasks_per_robot);
			//ths.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), psi_->getObjects().at(ss.str()), objects_[i]));
		}

		for(auto& a: acm_) a.second.resize(acm_.size(), 0);
		
		while (!tasks[0].empty() && !tasks[1].empty()){
			for (int i =0; i < std::max(tasks[0].front().solution.sub_trajectory.size(), tasks[1].front().solution.sub_trajectory.size()); i++){
				moveit_msgs::RobotTrajectory* t1 = nullptr;
				moveit_msgs::RobotTrajectory* t2 = nullptr;
				moveit_msgs::PlanningScene* ps1 = nullptr;
				moveit_msgs::PlanningScene* ps2 = nullptr;
				moveit_msgs::PlanningScene ps_m;
				std::vector<moveit_msgs::PlanningScene::_allowed_collision_matrix_type> acm_m;

				//for(auto entry: acm_) ROS_INFO("%s", entry.first.c_str());

				if (i < tasks[0].front().solution.sub_trajectory.size()){
					t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
					ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff;
				} 

				if (i < tasks[1].front().solution.sub_trajectory.size()){
					t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory;
					ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
				}

				std::vector<std::thread> th;

				if (t1){
					Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));

					// First find ID from panda to start with
					std::regex rx_panda("panda_arm([0-9]+)");
    				std::smatch match;
        			std::regex_match(mr->name(), match, rx_panda);

					// build panda link regex
					std::stringstream ss;
					ss << "panda_" << match[1] << "_.*";
					std::regex rx_panda_links(ss.str());
					std::regex rx_box("bottle.*");
					std::regex rx_table("table.*");

					// Iterate task collsion matrix
					for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){
						if( mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){
							 In case an entry matches an robot spezific link
							ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
								 For that specific entry, loop over values								
								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}				

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}	

								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}
							}
						}

						if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
							 In case an entry matches an robot spezific link
							ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
								 For that specific entry, loop over values							
								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}				

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}	

								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}
							}
						}

						if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box)){
							 In case an entry matches an robot spezific link
							ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
								 For that specific entry, loop over values							
								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}				

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}	

								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}
							}
						}

						if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table)){
							 In case an entry matches an robot spezific link
							ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j], j);
							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
								 For that specific entry, loop over values									
								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}				

								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}	

								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
								}
							}
						}
					}
					merge_ps(ps_m, ps1, mr);
				}

				if (t2){
					Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]);
					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));		

					std::regex rx_panda("panda_arm([0-9]+)");
    				std::smatch match;
        			std::regex_match(mr->name(), match, rx_panda);

					// build panda link regex
					std::stringstream ss;
					ss << "panda_" << match[1] << "_.*";
					std::regex rx_panda_links(ss.str());
					std::regex rx_box("bottle.*");
					std::regex rx_table("table.*");

					// Iterate task collsion matrix
					for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){
						if( mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
							 In case an entry matches an robot spezific link
							ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
								 For that specific entry, loop over values						
								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}				

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}	

								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}
							}
						}

						if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
							In case an entry matches an robot spezific link
							ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
								For that specific entry, loop over values								
								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}				

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}	

								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}
							}
						}

						if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box)){
							In case an entry matches an robot spezific link
							ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
								For that specific entry, loop over values		
								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}				

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}	

								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}
							}
						}

						if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table)){
							 In case an entry matches an robot spezific link
							ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j], j);
							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
								 For that specific entry, loop over values							
								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}				

								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}	

								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
								}
							}
						}
					}
					merge_ps(ps_m, ps2, mr);
				}


				for(int i = 0; i < th.size(); i++){
					th[i].join();
				}
				merge_acm(ps_m);
				planning_scene_diff_publisher_->publish(ps_m);

			}
			tasks[0].pop();
			tasks[1].pop();
		}
	}
	*/

}

void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScene& ps){
	// First find ID from panda to start with
	std::regex rx_panda("panda_arm([0-9]+)");
	std::smatch match;
	std::regex_match(mr->name(), match, rx_panda);

	// build panda link regex
	std::stringstream ss;
	ss << "panda_" << match[1] << "_.*";
	std::regex rx_panda_links(ss.str());
	std::regex rx_box("box.*");
	std::regex rx_table("table.*");

	// Iterate task collsion matrix
	for(int j = 0; j < ps.allowed_collision_matrix.entry_names.size(); j++ ){
		if( mr->map().at("base") == ps.allowed_collision_matrix.entry_names[j]){
			//In case an entry matches an robot spezific link
			ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);

			for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
				// For that specific entry, loop over values								
				int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));								

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
					ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
					ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}				

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
					ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}	

				if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
					ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}
			}
		}

		if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
			//In case an entry matches an robot spezific link
			ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
			for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
					//For that specific entry, loop over values							
				int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));								

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
					ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
					ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}				

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
					ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}	

				if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
					ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}
			}
		}

		if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_box)){
				//In case an entry matches an robot spezific link
			ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
			for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
				//For that specific entry, loop over values							
				int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));								

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
					ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
					ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}				

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
					ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}	

				if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
					ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}
			}
		}

		if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_table)){
			//In case an entry matches an robot spezific link
			ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j], j);
			for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
				//For that specific entry, loop over values									
				int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));								

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
					ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
					ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}				

				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
					ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}	

				if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
					ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
				}
			}
		}
	}
}


void Moveit_mediator::task_planner(){
	/* There are 2 ways to interprete a Task 
	1. A box position in acm is also the first entry in task, 
	2. A box position in acm is not the first entry in task, in that case we can might try for each position
	*/
	auto jq = job_reader_->robot_data();
	auto cd = cuboid_reader_->cuboid_bin();

	//std::vector<std::string> objs =  {"bottle1", "bottle2"};

	for (int i = 0; i < cd.size(); i ++){
		std::stringstream ss;
		ss << "box_" << i;

		moveit_msgs::CollisionObject item;
		item.id = ss.str();
		item.header.frame_id = "world";
		item.header.stamp = ros::Time::now();
		item.primitives.resize(1);
		item.primitives[0].type = item.primitives[0].BOX;
		item.primitives[0].dimensions.resize(3);
		item.primitives[0].dimensions[0] = cd[i].x_depth;
		item.primitives[0].dimensions[1] = cd[i].y_width;
		item.primitives[0].dimensions[2] = cd[i].z_heigth;

		item.primitive_poses.resize(1);
		item.primitive_poses[0].position.x = cd[i].Pose.position.x;
		item.primitive_poses[0].position.y = cd[i].Pose.position.y;
		item.primitive_poses[0].position.z = cd[i].Pose.position.z;
		item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
		item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
		item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
		item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
		item.operation = item.ADD;

		psi_->applyCollisionObject(item);
		acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));

		// Could also safe id's somewhere
	}
	
	// Setup shared ACM
	for(auto& a: acm_) a.second.resize(acm_.size(), 0);

	std::regex item("box_([0-9]+)");
    std::smatch match;

	// ----------------------------------
	// GROOT Strategie => erst FROM TEXT
	// ----------------------------------

	BT::BehaviorTreeFactory factory;
	factory.registerNodeType<Planning_scene>("PS");
	factory.registerNodeType<Trajetory>("Trajetory");
	factory.registerNodeType<Execution>("Execution");
	factory.registerNodeType<Position_condition>("Position_condition");
	factory.registerNodeType<Parallel_robot>("Parallel_robot");

	std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;

	while(!jq.empty()){
		for(auto& s_co : psi_->getObjects()){
			if(!std::regex_match(s_co.first, match, item)) continue;
			
			std::pair<std::string, job_data> temp = jq.front();
			ROS_INFO("1. job entry %f %f %f", temp.second.jobs_.front().getOrigin().getX(), temp.second.jobs_.front().getOrigin().getY(), temp.second.jobs_.front().getOrigin().getZ());
			ROS_INFO("object position %f %f %f", s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z);
			if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z )) == 0) {
				std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> bt_list;
				
				Moveit_robot* mr;
				for(auto* r: robots_) if (r->name() == temp.first) mr = dynamic_cast<Moveit_robot*>(r);

				// loop jobs
				for (int k = 1; k < temp.second.jobs_.size(); k++){
					moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(s_co.first), temp.second.jobs_[k]);
					if(mgt.plan(1)) {
						moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
						mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
						bt_list.push_back(e);

						moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first);
						temp_co.operation = temp_co.MOVE;
						temp_co.pose.position.x = temp.second.jobs_[k].getOrigin().getX();
						temp_co.pose.position.y = temp.second.jobs_[k].getOrigin().getY();
						temp_co.pose.position.z = temp.second.jobs_[k].getOrigin().getZ();
						temp_co.pose.orientation.x = temp.second.jobs_[k].getRotation().getX();
						temp_co.pose.orientation.y = temp.second.jobs_[k].getRotation().getY();
						temp_co.pose.orientation.z = temp.second.jobs_[k].getRotation().getZ();
						temp_co.pose.orientation.w = temp.second.jobs_[k].getRotation().getW();
						psi_->applyCollisionObject(temp_co);
					}
				}

				/*
				if(auto condition = dynamic_cast<Position_condition*>(node_it->get())) {ROS_INFO("om");condition->init(s_co.first, temp.second.jobs_.front().getOrigin(), psi_.get()); ++node_it;}

				for(auto& ets : bt_list)
					if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("tr");execution->init(planning_scene_diff_publisher_.get(), mr->mgi().get(), ets); ++node_it;}
				*/

				for (int i = 0; i < cd.size(); i ++){
					std::stringstream ss;
					ss << "box_" << i;

					moveit_msgs::CollisionObject item;
					item.id = ss.str();
					item.header.frame_id = "world";
					item.header.stamp = ros::Time::now();
					item.primitives.resize(1);
					item.primitives[0].type = item.primitives[0].BOX;
					item.primitives[0].dimensions.resize(3);
					item.primitives[0].dimensions[0] = cd[i].x_depth;
					item.primitives[0].dimensions[1] = cd[i].y_width;
					item.primitives[0].dimensions[2] = cd[i].z_heigth;

					item.pose.position.x = cd[i].Pose.position.x;
					item.pose.position.y = cd[i].Pose.position.y;
					item.pose.position.z = cd[i].Pose.position.z;
					item.pose.orientation.x = cd[i].Pose.orientation.x;
					item.pose.orientation.y = cd[i].Pose.orientation.y;
					item.pose.orientation.z = cd[i].Pose.orientation.z;
					item.pose.orientation.w = cd[i].Pose.orientation.w;
					item.operation = item.MOVE;

					psi_->applyCollisionObject(item);
					// Could also safe id's somewhere
				}

				
				try{
					task_but_different.at(mr->name()).push_back(std::tuple<std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list));
				} catch(std::out_of_range &oor) {
					std::tuple<const std::string&, tf2::Vector3&, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>&> tuple(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list);
					task_but_different.insert(std::pair<std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>>(mr->name(),{tuple}));
				}
				jq.pop_front();
			} else {jq.push_back(temp);}
		}
	}

	std::stringstream ss;
	ss << "<root main_tree_to_execute = \"MainTree\">\n";
	ss << "<BehaviorTree ID=\"MainTree\">\n";
	ss << "<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\""<< robots_.size() << "\" failure_threshold=\""<< robots_.size() << "\">\n";

	for(int i =0; i < robots_.size();i++){
		try{
			ss << "<Control ID=\"Parallel_robot\" name=\""<< robots_[i]->name() << "\" success_threshold=\""<< task_but_different.at(robots_[i]->name()).size() << "\" failure_threshold=\""<< task_but_different.at(robots_[i]->name()).size() << "\">\n";
			for (auto& p_obj: task_but_different.at(robots_[i]->name())){
				ss << "<SequenceStar name=\"root_sequence\">\n";
				ss << "<Condition ID=\"Position_condition\" name=\"Position_condition\"/>\n";
				for(int j = 0; j < std::get<2>(p_obj).size(); j++){
					ss << "<Action ID=\"Execution\" name=\"Execution\"/>\n";
				}
				ss << "</SequenceStar>\n";
			}
			ss << "</Control>\n";
		} catch(std::out_of_range& oor){}
	}
	ss << "</Control>\n";
	ss << "</BehaviorTree>\n";
	ss << "</root>\n";

	std::cout << ss.str();
	auto tree = factory.createTreeFromText(ss.str());
	auto node_it = tree.nodes.begin();

	

	for(int i =0; i < robots_.size();i++){
		try{
			for (auto& p_obj: task_but_different.at(robots_[i]->name())){
				while (node_it->get()->type() == BT::NodeType::CONTROL) ++node_it;
				if(node_it->get()->type() == BT::NodeType::ACTION) ROS_INFO("Action");
				if(node_it->get()->type() == BT::NodeType::CONDITION) ROS_INFO("Condition");
				if(node_it->get()->type() == BT::NodeType::CONTROL) ROS_INFO("Control");
				if(node_it->get()->type() == BT::NodeType::DECORATOR) ROS_INFO("Decorator");

				if(auto condition = dynamic_cast<Position_condition*>(node_it->get())) {ROS_INFO("init Condition");condition->init(std::get<0>(p_obj), std::get<1>(p_obj), psi_.get());++node_it;}
				for(int j = 0; j < std::get<2>(p_obj).size(); j++){
					auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
					if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("init Action");execution->init(&executions_, planning_scene_diff_publisher_.get(), mr, std::get<2>(p_obj)[j]);++node_it;}
				}
				ss << "</SequenceStar>\n";
			}
			ss << "</Control>\n";
		} catch(std::out_of_range& oor){}
	}


	BT::PublisherZMQ zmq(tree);
	ros::Duration sleep(1);
	BT::NodeStatus status = BT::NodeStatus::RUNNING;
	while( status == BT::NodeStatus::RUNNING){
		status = tree.tickRoot();

		std::vector<std::thread> th;
		moveit_msgs::PlanningScene ps_m;
		
		for(auto& exec : executions_){
			for (int i = 0; i < robots_.size(); i++){
				if (exec.first == robots_[i]->name()){
					auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), exec.second.first));
					//mr->mgi()->execute(exec.second.first);
					//manipulate_acm(mr, exec.second.second);
					//merge_ps(ps_m, exec.second.second, mr);
				}
			}
		}
		
		for(auto& t : th){
			t.join();
		}

		for(auto& exec : executions_){
		 	for (int i = 0; i < robots_.size(); i++){
		 		if (exec.first == robots_[i]->name()){
		 			auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
	 				manipulate_acm(mr, exec.second.second);
		 			merge_ps(ps_m, exec.second.second, mr);
		 		}
			}
		}

		merge_acm(ps_m);
		planning_scene_diff_publisher_->publish(ps_m);
	}


	// clean up
	for (int i = 0; i < cd.size(); i ++){
		std::stringstream ss;
		ss << "box_" << i;

		moveit_msgs::CollisionObject item;
		item.id = ss.str();
		item.header.frame_id = "world";
		item.header.stamp = ros::Time::now();
		item.primitives.resize(1);
		item.primitives[0].type = item.primitives[0].BOX;
		item.primitives[0].dimensions.resize(3);
		item.primitives[0].dimensions[0] = cd[i].x_depth;
		item.primitives[0].dimensions[1] = cd[i].y_width;
		item.primitives[0].dimensions[2] = cd[i].z_heigth;

		item.pose.position.x = cd[i].Pose.position.x;
		item.pose.position.y = cd[i].Pose.position.y;
		item.pose.position.z = cd[i].Pose.position.z;
		item.pose.orientation.x = cd[i].Pose.orientation.x;
		item.pose.orientation.y = cd[i].Pose.orientation.y;
		item.pose.orientation.z = cd[i].Pose.orientation.z;
		item.pose.orientation.w = cd[i].Pose.orientation.w;
		item.operation = item.MOVE;

		psi_->applyCollisionObject(item);
		// Could also safe id's somewhere
	}
	
}


void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
	moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
	acmt.entry_values.resize(acm_.size());

	int i = 0;
	for (auto& a : acm_){
		acmt.entry_names.push_back(a.first);
		acmt.entry_values[i].enabled = a.second;
		i++;
	}

	ps_m.allowed_collision_matrix = acmt;
}

void Moveit_mediator::status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status){
 if (!status->status_list.empty()){
	ROS_INFO("%i", status->status_list.size());
   }
}

void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, Moveit_robot* mr){
	// get full mr link list 
	std::vector<std::string> links;
	for (auto link : mr->mgi()->getLinkNames())links.push_back(link);
	links.push_back(mr->map()["left_finger"]);
	links.push_back(mr->map()["right_finger"]);
	links.push_back(mr->map()["hand_link"]);
	links.push_back(mr->map()["base"]);


	for (auto ao : in.robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
	if (in.robot_state.is_diff) out.robot_state.is_diff = in.robot_state.is_diff;
	if (in.is_diff) out.is_diff = in.is_diff;
	out.robot_state.joint_state.header = in.robot_state.joint_state.header;
	out.robot_model_name = "panda";



	for (auto link : links) {
		for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){
			if(link.c_str() == in.robot_state.joint_state.name[i].c_str()){
				out.robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
				out.robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
				out.robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
				ROS_INFO("check");
			}
		}

		for(int i = 0; i < in.link_padding.size(); i++){
			if(link.c_str() == in.link_padding[i].link_name.c_str()){
				out.link_padding.push_back(in.link_padding[i]);
			}
		}

		for(int i = 0; i < in.link_scale.size(); i++){
			if(link.c_str() == in.link_scale[i].link_name.c_str()){
				out.link_scale.push_back(in.link_scale[i]);
			}
		}
	}
	
}


void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt){
	mr.mgi()->execute(rt);
}




moveit::task_constructor::Task Moveit_mediator::create_Task(Moveit_robot* mr, moveit_msgs::CollisionObject& source, tf2::Transform& target){
    tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
	std::string support_surface1 = "nichts";
    std::string support_surface2 = "nichts";


    for (Abstract_robot* ar : robots_){
        std::string str;
        if(ar->check_single_object_collision(t, str)) support_surface1 = str;
        if(ar->check_single_object_collision(target, str)) support_surface2= str;
    }

	ROS_INFO("ss1 %s", support_surface1.c_str());
	ROS_INFO("ss2 %s", support_surface2.c_str());


	const std::string object = source.id;
    moveit::task_constructor::Task task_;

	std::string name = "Pick&Place";
	task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
	task_.loadRobotModel();
	task_.setRobotModel(mr->mgi()->getRobotModel());

	// Set task properties
	task_.setProperty("group", mr->name());
	task_.setProperty("eef", mr->map()["eef_name"]);
	task_.setProperty("hand", mr->map()["hand_group_name"]);
	task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]);
	task_.setProperty("ik_frame", mr->map()["hand_frame"]);

	moveit::task_constructor::Stage* current_state_ptr = nullptr;  
	{
		auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
		auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
		applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
			if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
				comment = "object with id '" + object + "' is already attached and cannot be picked";
				return false;
			}
			return true;
		});

		current_state_ptr = applicability_filter.get();
		task_.add(std::move(applicability_filter));
	}

	{  // Open Hand
		auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
		stage->setGroup(mr->map()["eef_name"]);
		stage->setGoal("open");
		task_.add(std::move(stage));
	}

	{  // Move-to pre-grasp
		auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
		    "move to pick",  moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(),  sampling_planner_} });
		stage->setTimeout(5.0);
		stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
		task_.add(std::move(stage));
	}

	 moveit::task_constructor::Stage* attach_object_stage = nullptr;  // Forward attach_object_stage to place pose generator
	{
		auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
		task_.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
		grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });


		{	// Approach Obj
			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner_);
			stage->properties().set("marker_ns", "approach_object");
			stage->properties().set("link", mr->map()["hand_frame"]);
			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
			stage->setMinMaxDistance(0.07, 0.2);

			// Set hand forward direction
			geometry_msgs::Vector3Stamped vec;
			vec.header.frame_id = mr->map()["hand_frame"];
			vec.vector.z = 1.0;
			stage->setDirection(vec);
			grasp->insert(std::move(stage));
		}

		{
			// Sample grasp pose
			auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
			stage->properties().set("marker_ns", "grasp_pose");
			stage->setPreGraspPose("open");
			stage->setObject(object);
			stage->setAngleDelta(M_PI / 12);
			stage->setMonitoredStage(current_state_ptr);  // Hook into current state

			// Compute IK
			Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
                                   Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
                                   Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
			Eigen::Translation3d trans(0.1f, 0, 0);
			Eigen::Isometry3d ik = eigen * trans;

			auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
			wrapper->setMaxIKSolutions(8);
			wrapper->setMinSolutionDistance(1.0);
			wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
			grasp->insert(std::move(wrapper));
		}

		{
			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
			stage->allowCollisions(
			    object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
			    true);
			grasp->insert(std::move(stage));
		}

		{
			auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
			stage->setGroup(mr->map()["eef_name"]);
			stage->setGoal("close");
			grasp->insert(std::move(stage));
		}

		{	// Attach obj
			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
			stage->attachObject(object, mr->map()["hand_frame"]);
			attach_object_stage = stage.get();
			grasp->insert(std::move(stage));
		}

		{	// Allow Collision obj table
			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
			stage->allowCollisions({ object }, support_surface1, true);
			grasp->insert(std::move(stage));
		}

		{
			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner_);
			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
			stage->setMinMaxDistance(0.1, 0.2);
			stage->setIKFrame(mr->map()["hand_frame"]);
			stage->properties().set("marker_ns", "lift_object");

			// Set upward direction
			geometry_msgs::Vector3Stamped vec;
			vec.header.frame_id = "world";
			vec.vector.z = 1.0;
			stage->setDirection(vec);
			grasp->insert(std::move(stage));
		}

		{	// forbid collision
			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
			stage->allowCollisions({ object }, support_surface1, false);
			grasp->insert(std::move(stage));
		}

		// Add grasp container to task
		task_.add(std::move(grasp));
	}

	{
		auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
		    "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_ } });
		stage->setTimeout(5.0);
		stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
		task_.add(std::move(stage));
	}

	{
		auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object");
		task_.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
		place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" });

		{
		auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow cokbkmomsurface)");
		stage->allowCollisions( {object} , support_surface2, true);
		place->insert(std::move(stage));
		}


		{
			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner_);
			stage->properties().set("marker_ns", "lower_object");
			stage->properties().set("link", mr->map()["hand_frame"]);
			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
			stage->setMinMaxDistance(.03, .13);

			// Set downward direction
			geometry_msgs::Vector3Stamped vec;
			vec.header.frame_id = "world";
			vec.vector.z = -1.0;
			stage->setDirection(vec);
			place->insert(std::move(stage));
		}

		{
			// Generate Place Pose
			auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose");
			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" });
			stage->properties().set("marker_ns", "place_pose");
			stage->setObject(object);

			// Set target pose
			geometry_msgs::PoseStamped p;
			p.header.frame_id = "world";
			p.pose.position.x = target.getOrigin().getX();
			p.pose.position.y = target.getOrigin().getY();
			p.pose.position.z = target.getOrigin().getZ();
			p.pose.orientation.x = target.getRotation().getX();
			p.pose.orientation.y = target.getRotation().getY();
			p.pose.orientation.z = target.getRotation().getZ();
			p.pose.orientation.w = target.getRotation().getW();



			stage->setPose(p);
			stage->setMonitoredStage(attach_object_stage);  // Hook into attach_object_stage

			// Compute IK
			Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
                                   Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
                                   Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
			Eigen::Translation3d trans(0.1f, 0, 0);
			Eigen::Isometry3d ik = eigen * trans;
			auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage));
			wrapper->setMaxIKSolutions(2);
			wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
			place->insert(std::move(wrapper));
		}

		{
			auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
			stage->setGroup(mr->map()["eef_name"]);
			stage->setGoal("open");
			place->insert(std::move(stage));
		}

		{
			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
			stage->allowCollisions(
			    object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
			    false);
			place->insert(std::move(stage));
		}

		{
			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object");
			stage->detachObject(object, mr->map()["hand_frame"]);
			place->insert(std::move(stage));
		}

		{
			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner_);
			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
			stage->setMinMaxDistance(.1, .2);
			stage->setIKFrame(mr->map()["hand_frame"]);
			stage->properties().set("marker_ns", "retreat");
			geometry_msgs::Vector3Stamped vec;
			vec.header.frame_id = mr->map()["hand_frame"];
			vec.vector.z = -1.0;
			stage->setDirection(vec);
			place->insert(std::move(stage));
		}

		{
			auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
			stage->setGroup(mr->map()["eef_name"]);
			stage->setGoal("close");
			place->insert(std::move(stage));
		}


		// Add place container to task
		task_.add(std::move(place));
	}

	{
		auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner_);
		stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
		stage->setGoal("ready");
		stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
		task_.add(std::move(stage));
	}

	return task_;
}


Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
    : Abstract_mediator(objects, pub), nh_(nh)
    , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
    , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
    , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
    , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
    , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
    , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
	, job_reader_(std::make_unique<Job_reader>(nh_))
    , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {

        if (planning_scene_monitor_->getPlanningScene()){
            planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                                                              "planning_scene");
            planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
        } else {
            ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
            return;
        }

        robot_model_loader::RobotModelLoaderPtr robot_model_loader;
        robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
        robot_model_ = robot_model_loader->getModel();
        ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);

        // planner 
        sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);

        // cartesian
	    cartesian_planner_->setMaxVelocityScaling(1.0);
	    cartesian_planner_->setMaxAccelerationScaling(1.0);
	    cartesian_planner_->setStepSize(.01);

    };