#include "Behaviors/Demos/Navigation/PilotDemo.h"


$nodeclass GrasperDemoWorld : PilotDemo {

virtual void postStart() {
    PilotDemo::postStart();
    particleFilter->displayParticles(100);
}

    virtual void buildMap() {
    
    //walls containing the three cylinders
    vector<Point> wallPts;
    //these pts are offset
    wallPts.push_back(Point(200, 225, 0, allocentric));
    wallPts.push_back(Point(200, 1075,0, allocentric));
    wallPts.push_back(Point(750, 1075, 0, allocentric));
    wallPts.push_back(Point(750, 225, 0, allocentric));

    NEW_SHAPE(wall, PolygonData, new PolygonData(worldShS, wallPts, false));

    //cylinder positions
    
    fmat::QuaternionT<float> identity = fmat::Quaternion();

    float radius = 70;

    NEW_SHAPE(redCylinder, CylinderData, new CylinderData(worldShS, Point(485,885, 100, allocentric), 200, radius, identity));

    NEW_SHAPE(greenCylinder, CylinderData, new CylinderData(worldShS, Point(485,685, 100, allocentric), 200, radius, identity));

    NEW_SHAPE(blueCylinder, CylinderData, new CylinderData(worldShS, Point(485,385, 100, allocentric), 200, radius, identity));

    //target locations -- they appear as circles, but in fact are spheres with collision = false

    NEW_SHAPE(targetForRed, PointData, new PointData(worldShS, Point(300, -500 , 0 , allocentric)));

    NEW_SHAPE(targetForGreen, PointData, new PointData(worldShS, Point(500, -500, 0 , allocentric)));

    NEW_SHAPE(targetForBlue, PointData, new PointData(worldShS, Point(700, -500, 0, allocentric)));

    //turn off obstacle attribute -- speeds up path planning
    targetForRed->setObstacle(false);
    targetForGreen->setObstacle(false);
    targetForBlue->setObstacle(false);

    PostStateCompletion();
    }


}