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

$nodeclass Emaze: PilotDemo {
	
  virtual void buildMap() {
		
    cout << "Building map..." << endl;
		
    // world bounds
		
    vector<Point> pts;
    pts.push_back(Point(   -457.2,      457.2));
    pts.push_back(Point(   1981.2,      457.2));
    pts.push_back(Point(   1981.2,    -1524.0));
    pts.push_back(Point(   1219.2,    -1524.0));
    pts.push_back(Point(   1219.2,     -355.6));
    pts.push_back(Point(   1143.0,     -355.6));
    pts.push_back(Point(   1143.0,    -1524.0));
    pts.push_back(Point(    381.0,    -1524.0));
    pts.push_back(Point(    381.0,     -355.6));
    pts.push_back(Point(    304.8,     -355.6));
    pts.push_back(Point(    304.8,    -1524.0));
    pts.push_back(Point(   -457.2,    -1524.0));
		
    NEW_SHAPE(worldBounds, PolygonData, new PolygonData(worldShS, pts, true));
		
    // add landmarks
		
    NEW_SHAPE(tag0, AprilTagData,
	      new AprilTagData(worldShS, Point(-456.2,    50.8, 177.8),
			       AprilTags::TagDetection(0)));
		
    NEW_SHAPE(tag1, AprilTagData,
	      new AprilTagData(worldShS, Point( -76.2,   456.2, 177.8),
			       AprilTags::TagDetection(1)));

    NEW_SHAPE(tag2, AprilTagData,
	      new AprilTagData(worldShS, Point( 762.0,   456.2, 177.8),
			       AprilTags::TagDetection(2)));

    NEW_SHAPE(tag3, AprilTagData,
	      new AprilTagData(worldShS, Point(1600.2,   456.2, 177.8),
			       AprilTags::TagDetection(3)));

    NEW_SHAPE(tag4, AprilTagData,
	      new AprilTagData(worldShS, Point(1980.2,    50.8, 177.8),
			       AprilTags::TagDetection(4)));
		
    NEW_SHAPE(tag5, AprilTagData,
	      new AprilTagData(worldShS, Point(1980.2,  -508.0, 177.8),
			       AprilTags::TagDetection(5)));
		
    NEW_SHAPE(tag6, AprilTagData,
	      new AprilTagData(worldShS, Point(1980.2, -1371.6, 177.8),
			       AprilTags::TagDetection(6)));
		
    NEW_SHAPE(tag7, AprilTagData,
	      new AprilTagData(worldShS, Point(1600.2, -1523.0, 177.8),
			       AprilTags::TagDetection(7)));

    NEW_SHAPE(tag8, AprilTagData,
	      new AprilTagData(worldShS, Point(1220.2, -1371.6, 177.8),
			       AprilTags::TagDetection(8)));

    NEW_SHAPE(tag9, AprilTagData,
	      new AprilTagData(worldShS, Point(1220.2,  -508.0, 177.8),
			       AprilTags::TagDetection(9)));

    NEW_SHAPE(tag10, AprilTagData,
	      new AprilTagData(worldShS, Point(1142.0,  -508.0, 177.8),
			       AprilTags::TagDetection(10)));

    NEW_SHAPE(tag11, AprilTagData,
	      new AprilTagData(worldShS, Point(1142.0, -1371.6, 177.8),
			       AprilTags::TagDetection(11)));

    NEW_SHAPE(tag12, AprilTagData,
	      new AprilTagData(worldShS, Point( 762.0, -1523.0, 177.8),
			       AprilTags::TagDetection(12)));

    NEW_SHAPE(tag13, AprilTagData,
	      new AprilTagData(worldShS, Point( 382.0, -1371.6, 177.8),
			       AprilTags::TagDetection(13)));

    NEW_SHAPE(tag14, AprilTagData,
	      new AprilTagData(worldShS, Point( 382.0,  -508.0, 177.8),
			       AprilTags::TagDetection(14)));

    NEW_SHAPE(tag15, AprilTagData,
	      new AprilTagData(worldShS, Point( 303.8,  -508.0, 177.8),
			       AprilTags::TagDetection(15)));

    NEW_SHAPE(tag16, AprilTagData,
	      new AprilTagData(worldShS, Point( 303.8, -1371.6, 177.8),
			       AprilTags::TagDetection(16)));

    NEW_SHAPE(tag17, AprilTagData,
	      new AprilTagData(worldShS, Point( -76.2, -1523.0, 177.8),
			       AprilTags::TagDetection(17)));

    NEW_SHAPE(tag18, AprilTagData,
	      new AprilTagData(worldShS, Point(-456.2, -1371.6, 177.8),
			       AprilTags::TagDetection(18)));

    NEW_SHAPE(tag19, AprilTagData,
	      new AprilTagData(worldShS, Point(-456.2,  -508.0, 177.8),
			       AprilTags::TagDetection(19)));

    pilot->setWorldBounds(worldBounds);
  };
	
}