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

$nodeclass Question7 : VeeTags{
	$nodeclass PrintLocation : PilotNode(PilotTypes::localize){
		virtual void doStart(){
    		cout << "Not Localized"<< endl;
		cout << "Robot is at x=" << theAgent->getCentroid().coordX(); 
               	cout << " y="<< theAgent->getCentroid().coordY() << endl;
             	cout << "Robot heading is "<< theAgent->getOrientation() << endl;
		}
      		virtual void doStop(){
  		cout << "Localize" << endl;
                cout << "Robot is at x=" << theAgent->getCentroid().coordX();
                cout << " y="<< theAgent->getCentroid().coordY() << endl;
                cout << "Robot heading is "<< theAgent->getOrientation() << endl;
		}
	}

	$setupmachine{
		rundemo: Turn(M_PI/2)=C=>WalkForward(1500)=C=>Turn(-M_PI/2)=C=>WalkForward(1500)=C=>Turn(-M_PI/2)=C=>WalkForward(3000)=C=>Turn(-M_PI/2)=C=>WalkForward(1500)=C=>Turn(-M_PI/2)=C=>WalkForward(1500)=C=>Turn(-M_PI/2)=C=>PrintLocation=C=>StateNode
	}
}
REGISTER_BEHAVIOR(Question7);