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

///////////////////
// Part V - Star5
///////////////////

$nodeclass Star5 : PilotDemo {
	$nodeclass StarLeg : StateNode {
		$setupmachine {
			WalkForward(750)=C=>Turn(-2.513)=C=>PostMachineCompletion
		}
	}

	$setupmachine {
		rundemo: StarLeg
		rundemo=C=>StarLeg=C=>StarLeg=C=>StarLeg=C=>StarLeg
	}
}

REGISTER_BEHAVIOR(Star5);

///////////////////
// Part V - Vee navigation
///////////////////

$nodeclass VeeNav : PilotDemo {

	virtual void postStart() {
		PilotDemo::postStart();
		particleFilter->displayParticles(100);
	}

	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));

		float rot = atan2(500, 866);
		NEW_SHAPE(wall, PolygonData, new PolygonData(worldShS, pts, false));

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

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

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

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

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

		NEW_SHAPE(tag5, AprilTagData,
			  new AprilTagData(worldShS, AprilTags::TagDetection(5),
								   Point( 92, -704, 177.8), fmat::Quaternion::aboutZ(-rot)));
	}

	$nodeclass PrintLocalize : PilotNode(PilotTypes::localize) {
		virtual void doStart() {
			cout << "Robot's pose estimate (pre-loc):  ("
				 << theAgent->getCentroid().coordX() << ", "
				 << theAgent->getCentroid().coordY() << ", " 
				 << theAgent->getOrientation() << ")" 
				 << endl;
		}

		virtual void doStop() {
			cout << "Robot's pose estimate (post-loc): ("
				 << theAgent->getCentroid().coordX() << ", "
				 << theAgent->getCentroid().coordY() << ", " 
				 << theAgent->getOrientation() << ")" 
				 << endl;
		}
	}

	$setupmachine {
		rundemo: Turn(1.5708)
			   =C=>WalkForward(1500)
			   =C=>Turn(-1.5708)
			   =C=>WalkForward(1500)
			   =C=>Turn(-1.5708)
			   =C=>WalkForward(3000)
			   =C=>Turn(-1.5708)
			   =C=>WalkForward(1500)
			   =C=>Turn(-1.5708)
			   =C=>WalkForward(1500)
			   =C=>PrintLocalize
			   =C=>StateNode // do this so doStop of the previous state gets called
	}
}

REGISTER_BEHAVIOR(VeeNav);