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

$nodeclass Lab6_Playpen: PilotDemo {
	
  virtual void buildMap() {
		
    cout << "Building map..." << endl;
		
    // world bounds
		
    vector<Point> pts;
    pts.push_back(Point(-500, 500));

    pts.push_back(Point(-500+2750, 500));
    pts.push_back(Point(-500+2750, 500-1680));

    pts.push_back(Point(-500+2750-770, 500-1680));
    pts.push_back(Point(-500+2750-770, 500-1680+920));

    pts.push_back(Point(-500+2750-770-300, 500-1680+920));
    pts.push_back(Point(-500+2750-770-300, 500-1680-100));

    pts.push_back(Point(-500+2750-770-300-610, 500-1680-100));
    pts.push_back(Point(-500+2750-770-300-610, 500-1680+920));
    
    pts.push_back(Point(-500+2750-770-300-610-300, 500-1680+920));
    pts.push_back(Point(-500+2750-770-300-610-300, 500-1680));
    
    pts.push_back(Point(-500+2750-770-300-610-300-770, 500-1680));

    NEW_SHAPE(worldBounds, PolygonData, new PolygonData(worldShS, pts, true));
		
    // add landmarks
		
    NEW_SHAPE(tag0, AprilTagData,
	      new AprilTagData(worldShS, Point(-500, 500-770/2.0, 177.8),
			       AprilTags::TagDetection(0)));
		
    NEW_SHAPE(tag1, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+770/2.0, 500, 177.8),
			       AprilTags::TagDetection(1)));

    NEW_SHAPE(tag2, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750/2.0, 500, 177.8),
			       AprilTags::TagDetection(2)));

    NEW_SHAPE(tag3, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770/2.0, 500, 177.8),
			       AprilTags::TagDetection(3)));

    NEW_SHAPE(tag4, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750, 500-770/2.0, 177.8),
			       AprilTags::TagDetection(4)));
		
    NEW_SHAPE(tag5, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750, 500-1680+300+300, 177.8),
			       AprilTags::TagDetection(5)));
		
    NEW_SHAPE(tag6, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750, 500-1680+300, 177.8),
			       AprilTags::TagDetection(6)));
		
    NEW_SHAPE(tag7, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770/2.0, 500-1680, 177.8),
			       AprilTags::TagDetection(7)));

    NEW_SHAPE(tag8, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770, 500-1680+300, 177.8),
			       AprilTags::TagDetection(8)));

    NEW_SHAPE(tag9, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770, 500-1680+300+300, 177.8),
			       AprilTags::TagDetection(9)));

    NEW_SHAPE(tag10, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300, 500-1680-100+340+340, 177.8),
			       AprilTags::TagDetection(10)));

    NEW_SHAPE(tag11, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300, 500-1680-100+340, 177.8),
			       AprilTags::TagDetection(11)));

    NEW_SHAPE(tag12, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300-610/2.0, 500-1680-100, 177.8),
			       AprilTags::TagDetection(12)));

    NEW_SHAPE(tag13, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300-610, 500-1680-100+340, 177.8),
			       AprilTags::TagDetection(13)));

    NEW_SHAPE(tag14, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300-610, 500-1680-100+340+340, 177.8),
			       AprilTags::TagDetection(14)));

    NEW_SHAPE(tag15, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300-610-300, 500-1680+300+300, 177.8),
			       AprilTags::TagDetection(15)));

    NEW_SHAPE(tag16, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300-610-300, 500-1680+300, 177.8),
			       AprilTags::TagDetection(16)));

    NEW_SHAPE(tag17, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300-610-300-770/2.0, 500-1680, 177.8),
			       AprilTags::TagDetection(17)));

    NEW_SHAPE(tag18, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300-610-300-770, 500-1680+300, 177.8),
			       AprilTags::TagDetection(18)));

    NEW_SHAPE(tag19, AprilTagData,
	      new AprilTagData(worldShS, Point(-500+2750-770-300-610-300-770, 500-1680+300+300, 177.8),
			       AprilTags::TagDetection(19)));

    pilot->setWorldBounds(worldBounds);
  };
	
}

REGISTER_BEHAVIOR(Lab6_Playpen);