//
// Created by Sebastian Ebert
//

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

class SafetyEnvironmentCreator {

    private:
        double safety_zone_width;
        double safety_zone_height;
        double safety_zone_depth;
        double zone_orientation_w;
        double zone_orientation_x;
        double zone_orientation_y;
        double zone_orientation_z;
        double zone_position_x;
        double zone_position_y;
        double zone_position_z;

    public:
        /**
         * ctor
         * @param shape describes the inner dimensions of the safety zone
         * @param pose describes position and orientation of the safety zone
         * note: current zone-threshold is 0.1
         */
        SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
            if(shape.type != shape.BOX){
                ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
                safety_zone_depth = 0.0;
                safety_zone_width = 0.0;
                safety_zone_height = 0.0;
                zone_orientation_w = 1.0;
                zone_orientation_x = 1.0;
                zone_orientation_y = 1.0;
                zone_orientation_z = 1.0;
                            }else {
                safety_zone_depth = shape.dimensions[0];
                safety_zone_width = shape.dimensions[1];
                safety_zone_height = shape.dimensions[2];
                zone_orientation_w = pose.orientation.w;
                zone_orientation_x = pose.orientation.x;
                zone_orientation_y = pose.orientation.y;
                zone_orientation_z = pose.orientation.z;
                zone_position_x = pose.position.x;
                zone_position_y = pose.position.y;
                zone_position_z = pose.position.z;
            }
        }

        double getZoneWidth() const {
            return safety_zone_width;
        }

        double getZoneHeight() const {
            return safety_zone_height;
        }

        double getZoneDepth() const {
            return safety_zone_depth;
        }

        double getZoneOrientationW() const {
            return zone_orientation_w;
        }

        double getZoneOrientationX() const {
            return zone_orientation_x;
        }

        double getZoneOrientationY() const {
            return zone_orientation_y;
        }

        double getZoneOrientationZ() const {
            return zone_orientation_z;
        }

         std::vector<moveit_msgs::CollisionObject> createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group) {

            std::vector<moveit_msgs::CollisionObject> collision_objects;

            // OBJECT 1 ( left box / -y )
            // ^^^^^^^^
            // Define a collision object ROS message.
            moveit_msgs::CollisionObject collision_object;
            collision_object.header.frame_id = move_group.getPlanningFrame();

            // The id of the object is used to identify it.
            collision_object.id = "box1";

            // Define a box to add to the world.
            shape_msgs::SolidPrimitive primitive;
            primitive.type = primitive.BOX;
            primitive.dimensions.resize(3);

            primitive.dimensions[0] = getZoneDepth() + 0.2; // x
            primitive.dimensions[1] = 0.1; // y
            primitive.dimensions[2] = getZoneHeight(); // z

            // Define a pose for the box (specified relative to frame_id)
            geometry_msgs::Pose box_pose;
            box_pose.orientation.w = 1.0;
            box_pose.position.x = zone_position_x;
            box_pose.position.y = zone_position_y + (-1 * ((getZoneWidth() / 2) + 0.05));
            box_pose.position.z = zone_position_z + (primitive.dimensions[2] / 2);

            collision_object.primitives.push_back(primitive);
            collision_object.primitive_poses.push_back(box_pose);
            collision_object.operation = collision_object.ADD;

            collision_objects.push_back(collision_object);

            // OBJECT 2  right box / +y )
            // ^^^^^^^^
            moveit_msgs::CollisionObject collision_object_2;
            collision_object_2.header.frame_id = move_group.getPlanningFrame();

            // The id of the object is used to identify it.
            collision_object_2.id = "box2";

            // Define a box to add to the world.
            shape_msgs::SolidPrimitive primitive_2;
            primitive_2.type = primitive.BOX;
            primitive_2.dimensions.resize(3);
            primitive_2.dimensions[0] = getZoneDepth() + 0.2;
            primitive_2.dimensions[1] = 0.1;
            primitive_2.dimensions[2] = getZoneHeight();

            // Define a pose for the box (specified relative to frame_id)
            geometry_msgs::Pose box_pose_2;
            box_pose_2.orientation.w = 1.0;
            box_pose_2.position.x = zone_position_x;
            box_pose_2.position.y = zone_position_y + ((getZoneWidth() / 2) + 0.05);
            box_pose_2.position.z = zone_position_z + (primitive_2.dimensions[2] / 2);

            collision_object_2.primitives.push_back(primitive_2);
            collision_object_2.primitive_poses.push_back(box_pose_2);
            collision_object_2.operation = collision_object_2.ADD;

            collision_objects.push_back(collision_object_2);

            // OBJECT 3 (front box / +x)
            // ^^^^^^^^
            moveit_msgs::CollisionObject collision_object_3;
            collision_object_3.header.frame_id = move_group.getPlanningFrame();

            // The id of the object is used to identify it.
            collision_object_3.id = "box3";

            // Define a box to add to the world.
            shape_msgs::SolidPrimitive primitive_3;
            primitive_3.type = primitive.BOX;
            primitive_3.dimensions.resize(3);
            primitive_3.dimensions[0] = 0.1;
            primitive_3.dimensions[1] = getZoneWidth() + 0.2;
            primitive_3.dimensions[2] = getZoneHeight();

            // Define a pose for the box (specified relative to frame_id)
            geometry_msgs::Pose box_pose_3;
            box_pose_3.orientation.w = 1.0;
            box_pose_3.position.x = zone_position_x + ((getZoneDepth() / 2) + 0.05);
            box_pose_3.position.y = zone_position_y;
            box_pose_3.position.z = zone_position_z + (primitive_3.dimensions[2] / 2);

            collision_object_3.primitives.push_back(primitive_3);
            collision_object_3.primitive_poses.push_back(box_pose_3);
            collision_object_3.operation = collision_object_3.ADD;

            collision_objects.push_back(collision_object_3);

            // OBJECT 4 (back box / -x)
            // ^^^^^^^^
            moveit_msgs::CollisionObject collision_object_4;
            collision_object_4.header.frame_id = move_group.getPlanningFrame();

            // The id of the object is used to identify it.
            collision_object_4.id = "box4";

            // Define a box to add to the world.
            shape_msgs::SolidPrimitive primitive_4;
            primitive_4.type = primitive.BOX;
            primitive_4.dimensions.resize(3);
            primitive_4.dimensions[0] = 0.1;
            primitive_4.dimensions[1] = getZoneWidth() + 0.2;
            primitive_4.dimensions[2] = getZoneHeight();

            // Define a pose for the box (specified relative to frame_id)
            geometry_msgs::Pose box_pose_4;
            box_pose_4.orientation.w = 1.0;
            box_pose_4.position.x = zone_position_x + (-1 * ((getZoneDepth() / 2) + 0.05));
            box_pose_4.position.y = zone_position_y;
            box_pose_4.position.z = zone_position_z + (primitive_4.dimensions[2] / 2);

            collision_object_4.primitives.push_back(primitive_4);
            collision_object_4.primitive_poses.push_back(box_pose_4);
            collision_object_4.operation = collision_object_4.ADD;

            collision_objects.push_back(collision_object_4);

            // OBJECT 5 (floor / -z)
            // ^^^^^^^^
            moveit_msgs::CollisionObject collision_object_5;
            collision_object_5.header.frame_id = move_group.getPlanningFrame();

            // The id of the object is used to identify it.
            collision_object_5.id = "box5";

            // Define a box to add to the world.
            shape_msgs::SolidPrimitive primitive_5;
            primitive_5.type = primitive.BOX;
            primitive_5.dimensions.resize(3);
            primitive_5.dimensions[0] = getZoneDepth() + 0.2;
            primitive_5.dimensions[1] = getZoneWidth() + 0.2;
            primitive_5.dimensions[2] = 0.1;

            // Define a pose for the box (specified relative to frame_id)
            geometry_msgs::Pose box_pose_5;
            box_pose_5.orientation.w = 1.0;
            box_pose_5.position.x = zone_position_x;
            box_pose_5.position.y = zone_position_y;
            box_pose_5.position.z = zone_position_z - 0.05;

            collision_object_5.primitives.push_back(primitive_5);
            collision_object_5.primitive_poses.push_back(box_pose_5);
            collision_object_5.operation = collision_object_5.ADD;

            collision_objects.push_back(collision_object_5);

             // OBJECT 6 (top / +z)
             // ^^^^^^^^
             moveit_msgs::CollisionObject collision_object_6;
             collision_object_6.header.frame_id = move_group.getPlanningFrame();

             // The id of the object is used to identify it.
             collision_object_6.id = "box6";

             // Define a box to add to the world.
             shape_msgs::SolidPrimitive primitive_6;
             primitive_6.type = primitive.BOX;
             primitive_6.dimensions.resize(3);
             primitive_6.dimensions[0] = getZoneDepth() + 0.2;
             primitive_6.dimensions[1] = getZoneWidth() + 0.2;
             primitive_6.dimensions[2] = 0.1;

             // Define a pose for the box (specified relative to frame_id)
             geometry_msgs::Pose box_pose_6;
             box_pose_6.orientation.w = 1.0;
             box_pose_6.position.x = zone_position_x;
             box_pose_6.position.y = zone_position_y;
             box_pose_6.position.z = zone_position_z + getZoneHeight() + 0.05;

             collision_object_6.primitives.push_back(primitive_6);
             collision_object_6.primitive_poses.push_back(box_pose_6);
             collision_object_6.operation = collision_object_6.ADD;
             collision_objects.push_back(collision_object_6);

             /*for(std::size_t i=0; i<collision_objects.size(); ++i) {
                 collision_objects[i].primitive_poses[0].orientation.w = getZoneOrientationW();
                 collision_objects[i].primitive_poses[0].orientation.x = getZoneOrientationX();
                 collision_objects[i].primitive_poses[0].orientation.y = getZoneOrientationY();
                 collision_objects[i].primitive_poses[0].orientation.z = getZoneOrientationZ();
             }*/

            return collision_objects;
        }
};