#include "Behaviors/StateMachine.h"

$nodeclass NavigateMirage : VisualRoutinesStateNode {

	//Create VeeTags so robot can localize
 	static 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)));

  }

	//Variables used by class
  	$provide Point finalPosition;
  	$provide AngTwoPi finalOrientation;
	
	//Inner class used to report position
	$nodeclass PositionNode : VisualRoutinesStateNode : doStart {
		Point finalPosition = theAgent->getCentroid();
		AngTwoPi finalOrientation = theAgent->getOrientation();
		cout << "Robot's position is " << finalPosition << ". Its orientation is " << finalOrientation << endl;
		postStateCompletion();
	}

	//Inner class used to localize
	$nodeclass LocalizeNode : PilotNode(PilotTypes::localize) : doStart {
		NavigateMirage::buildMap();
	}

	//Setup machine
	$setupmachine{
		Turn(2.094) =C=> WalkForward(1000) =C=> Turn(-1.57) =C=> WalkForward(650) =C=> Turn(-1.396) =C=> WalkForward(1000) =C=> Turn(-0.349) =C=> WalkForward(1000) =C=> Turn(-1.22) =C=> WalkForward(1300) =C=> Turn(-1.57) =C=> WalkForward(1200) =C=> Turn(-2.094) =C=> WalkForward(800) =C=> Turn(1.57) =C=> WalkForward(250) =C=> Turn(-1.9) =C=> PositionNode =C=>  LocalizeNode =C=> PositionNode
	}
}
REGISTER_BEHAVIOR(NavigateMirage);