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

//using namespace DualCoding;

$nodeclass Playpen : PilotDemo {
	virtual void buildMap() {
	std::cout<< "Building map..." << endl;
  
  // world bounds
		
    vector<Point> pts;
    pts.push_back(Point(   -500.0,      500.0));
    pts.push_back(Point(   2760.0,      500.0));
    pts.push_back(Point(   2760.0,    -1215.0));
    pts.push_back(Point(   1970.0,    -1215.0));
	pts.push_back(Point(  1970, -285));
    pts.push_back(Point(   1450.0,     -285.6));
    pts.push_back(Point(   1450.0,    -1315.0));
    pts.push_back(Point(    800.0,    -1315.0));
    pts.push_back(Point(    800.0,     -285.0));
    pts.push_back(Point(    300.0,     -285.0));
    pts.push_back(Point(    300.0,    -1215.0));
    pts.push_back(Point(   -500.0,    -1215.0));
		
    NEW_SHAPE(worldBounds, PolygonData, new PolygonData(worldShS, pts, true));
		
    // add landmarks
		
    NEW_SHAPE(tag0, AprilTagData,
	      new AprilTagData(worldShS, Point(-500.0,    107.5, 177.8),
			       AprilTags::TagDetection(0)));
		
    NEW_SHAPE(tag1, AprilTagData,
	      new AprilTagData(worldShS, Point( -100.0,   500.0, 177.8),
			       AprilTags::TagDetection(1)));

    NEW_SHAPE(tag2, AprilTagData,
	      new AprilTagData(worldShS, Point( 1125.0,   500.0, 177.8),
			       AprilTags::TagDetection(2)));

    NEW_SHAPE(tag3, AprilTagData,
	      new AprilTagData(worldShS, Point(2365.0,   500.0, 177.8),
			       AprilTags::TagDetection(3)));

    NEW_SHAPE(tag4, AprilTagData,
	      new AprilTagData(worldShS, Point(2760.0,    107.5, 177.8),
			       AprilTags::TagDetection(4)));
		
    NEW_SHAPE(tag5, AprilTagData,
	      new AprilTagData(worldShS, Point(2760.0,  -500.0, 177.8),
			       AprilTags::TagDetection(5)));
		
    NEW_SHAPE(tag6, AprilTagData,
	      new AprilTagData(worldShS, Point(2760.0, -1000.0, 177.8),
			       AprilTags::TagDetection(6)));
		
    NEW_SHAPE(tag7, AprilTagData,
	      new AprilTagData(worldShS, Point(2365.0, -1215.0, 177.8),
			       AprilTags::TagDetection(7)));

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

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

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

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

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

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

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

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

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

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

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

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

    pilot->setWorldBounds(worldBounds);
  };
	
}

REGISTER_BEHAVIOR(Playpen);