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

$nodeclass VeeTags : PilotDemo {

  virtual void doStart() {
    particleFilter->resetFilter();  // randomize the particles since we don't know where we are
  }

  virtual void buildMap() {
    cout << "Building map..." << endl;
    // vee-shaped wall
    vector<Point> pts;
    pts.push_back(Point(  0,  866, 0, allocentric));
    pts.push_back(Point(500,    0, 0, allocentric));
    pts.push_back(Point(  0, -866, 0, allocentric));
    NEW_SHAPE(wall, PolygonData, new PolygonData(worldShS, pts, false));

    NEW_SHAPE(tag0, AprilTagData,
	      new AprilTagData(worldShS, Point( 92,  704, 177.8),
			       AprilTags::TagDetection(0)));

    NEW_SHAPE(tag1, AprilTagData,
	      new AprilTagData(worldShS, Point(245,  440, 177.8),
			       AprilTags::TagDetection(1)));

    NEW_SHAPE(tag2, AprilTagData,
	      new AprilTagData(worldShS, Point(397,  176, 177.8),
			       AprilTags::TagDetection(2)));

    NEW_SHAPE(tag3, AprilTagData,
	      new AprilTagData(worldShS, Point(397, -176, 177.8),
			       AprilTags::TagDetection(3)));

    NEW_SHAPE(tag4, AprilTagData,
	      new AprilTagData(worldShS, Point(245, -440, 177.8),
			       AprilTags::TagDetection(4)));

    NEW_SHAPE(tag5, AprilTagData,
	      new AprilTagData(worldShS, Point( 92, -704, 177.8),
			       AprilTags::TagDetection(5)));

  }

}