#include <robot/obstacles.h>
#include <geometry_msgs/Point.h>

//param pose_x,pose_y other robot current pose
vector< vector<geometry_msgs::Point> > obstacles::getObstacleArray(double pose_x, double pose_y)
{
    vector<geometry_msgs::Point> obstaclePoint;
    geometry_msgs::Point point;

    //first point, left-top point
    point.x = pose_x-0.5104;
    point.y = pose_y+0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);

    //second point, left-down point
    point.x = pose_x-0.5104;
    point.y = pose_y-0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);

    //third point, right-down point
    point.x = pose_x+0.5104;
    point.y = pose_y-0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);

    //fourth point, right-top point
    point.x = pose_x+0.5104;
    point.y = pose_y+0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);

    //first point again to complete the box
    point.x = pose_x-0.5104;
    point.y = pose_y+0.5104;
    point.z = 0;
    obstaclePoint.push_back(point);
    obstacleArray.push_back(obstaclePoint);

    //------------------------------------//
    //first point, left-top point
    point.x = 4.66;
    point.y = 3.673;
    point.z = 0;
    obstaclePoint.push_back(point);

    //second point, left-down point
    point.x = 4.725;
    point.y = 5.218;
    point.z = 0;
    obstaclePoint.push_back(point);

    //third point, right-down point
    point.x = 0.215;
    point.y = 5.226;
    point.z = 0;
    obstaclePoint.push_back(point);

    //fourth point, right-top point
    point.x = 0.24;
    point.y = 3.71;
    point.z = 0;
    obstaclePoint.push_back(point);

    //first point again to complete the box
    point.x = 4.66;
    point.y = 3.673;
    point.z = 0;
    obstaclePoint.push_back(point);
    obstacleArray.push_back(obstaclePoint);

    //-------------------------------------------//
    //first point, left-top point
    point.x = 5.63;
    point.y = 4.177;
    point.z = 0;
    obstaclePoint.push_back(point);

    //second point, left-down point
    point.x = 5.78;
    point.y = 3.66;
    point.z = 0;
    obstaclePoint.push_back(point);

    //third point, right-down point
    point.x = 6.415;
    point.y = 3.641;
    point.z = 0;
    obstaclePoint.push_back(point);

    //fourth point, right-top point
    point.x = 6.371;
    point.y = 4.20;
    point.z = 0;
    obstaclePoint.push_back(point);

    //first point again to complete the box
    point.x = 5.63;
    point.y = 4.177;
    point.z = 0;
    obstaclePoint.push_back(point);
    obstacleArray.push_back(obstaclePoint);

    //--------------------------------------------------//
    //first point, left-top point
    point.x = 0.308;
    point.y = 11;
    point.z = 0;
    obstaclePoint.push_back(point);

    //second point, left-down point
    point.x = 0.251;
    point.y = 9.00;
    point.z = 0;
    obstaclePoint.push_back(point);

    //third point, right-down point
    point.x = 3.87;
    point.y = 9.02;
    point.z = 0;
    obstaclePoint.push_back(point);

    //fourth point, right-top point
    point.x = 3.88;
    point.y = 11;
    point.z = 0;
    obstaclePoint.push_back(point);

    //first point again to complete the box
    point.x = 0.308;
    point.y = 11;
    point.z = 0;
    obstaclePoint.push_back(point);
    obstacleArray.push_back(obstaclePoint);

    return obstacleArray;
}
