#include "Behaviors/StateMachine.h"
#include "Crew/PilotNode.h"
#include "Behaviors/Demos/Navigation/VeeTags.h"

$nodeclass Lab3Q2 : VeeTags{
	$nodeclass Report : VisualRoutinesStateNode : doStart {
    		Point finalPosition = theAgent->getCentroid();
    		AngTwoPi finalOrientation = theAgent->getOrientation();
    		cout << "Robot stops at " << finalPosition << endl;
    		cout << "Orientation drift = "
         		<<float( finalOrientation)*180/M_PI << " degrees" << endl;
		postStateCompletion();
  	}
	$setupmachine{	
		rundemo:PilotNode(PilotTypes::localize) =C=> Turn(mathutils::deg2rad(180.0)) =C=> 
			WalkForward(250) =C=> Turn(mathutils::deg2rad(90.0)) =C=>
			WalkForward(1100) =C=> Turn(mathutils::deg2rad(90.0)) =C=>
			WalkForward(1000) =C=> Turn(mathutils::deg2rad(90.0)) =C=>
			WalkForward(2200) =C=> Turn(mathutils::deg2rad(90.0)) =C=>
			WalkForward(1250) =C=> Turn(mathutils::deg2rad(90.0)) =C=>
			WalkForward(1100) =C=> Turn(mathutils::deg2rad(90.0)) =C=>
			WalkForward(500) =C=> SpeechNode("Before:") =C=> Report =C=>
			PilotNode(PilotTypes::localize) =C=> SpeechNode("After:") =C=> Report
	}
}

REGISTER_BEHAVIOR (Lab3Q2);