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

$nodeclass Circumnavigate : VeeTags {

	$nodeclass WalkAndTurn : StateNode {
	  $setupmachine{

		forward =C=> turn =C=> complete

		forward : WalkForward(2000)
		turn : Turn(1.570796)
		complete : PostMachineCompletion
	  }
	}

	$nodeclass WalkFarAndTurn : StateNode {
		$setupmachine{

			forward_much =C=> turn =C=> complete

			forward_much: WalkForward(4000)
			turn : Turn(1.570796)
			complete : PostMachineCompletion
		}
	}

	$nodeclass TurnLeft : StateNode {
		$setupmachine{
			turn =C=> complete

			turn : Turn(-1.570796)
			complete : PostMachineCompletion
		}
	}
	
	$nodeclass PrintLoc : PilotNode(PilotTypes::localize) {
		virtual void doStart() {
			cout << "Robot is at (" << theAgent->getCentroid().coordX()
				 << "," << theAgent->getCentroid().coordY() << ")" << endl;
			cout << "Robot heading is " << theAgent->getOrientation() << endl;
		}
		virtual void doStop() {
			cout << "Robot is at (" << theAgent->getCentroid().coordX()
				 << "," << theAgent->getCentroid().coordY() << ")" << endl;
			cout << "Robot heading is " << theAgent->getOrientation() << endl;
		}
	}

	$setupmachine{
		rundemo: TurnLeft
					=C=> WalkAndTurn
						=C=> WalkAndTurn
							=C=> WalkFarAndTurn
								=C=> WalkAndTurn
									=C=> WalkAndTurn
										=C=> PrintLoc
											=C=> StateNode
	}
}

REGISTER_BEHAVIOR(Circumnavigate);