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

$nodeclass Circumnavigate : VeeTags {
	$nodeclass RandParticles : PilotNode(PilotTypes::localize) : doStart {
		particleFilter->resetFilter();	
	}

	$nodeclass PrintLoc : PilotNode(PilotTypes::localize) : doStart {
		cout << "Robot is at x="
		     << theAgent->getCentroid().coordX()
		     << " y=" 
		     << theAgent->getCentroid().coordY() << endl;
		cout << "Robot heading is "
		     << theAgent->getOrientation() << endl;	
	}


	$setupmachine{
		rundemo : RandParticles =C=> Turn(-90*M_PI/180) =C=> WalkForward(1500) =C=> Turn(90*M_PI/180) =C=> WalkForward(1500) =C=> Turn(90*M_PI/180) =C=>WalkForward(3000) =C=> Turn(90*M_PI/180) =C=> WalkForward(1500) =C=> Turn(90*M_PI/180) =C=> WalkForward(1500) =C=>Turn(90*M_PI/180) =C=> PrintLoc
	}
}


REGISTER_BEHAVIOR(Circumnavigate);