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

$nodeclass GetAround : VeeTags {

        $setupmachine{
		rundemo : StateNode =N=>  right : RightNode =C=> halfForward : HalfForward =C=> left2 : LeftNode =C=> halfforward2 : HalfForward 
		halfforward2 =C=> left3 : LeftNode =C=> forward2 : Forward =C=> left4 : LeftNode =C=> halfForward3 : HalfForward
	halfForward3 =C=> left5 : LeftNode =C=> halfFoward4 : HalfForward =C=> left6 : LeftNode =C=>  printLoc : PrintPreLoc =C=> printloc2 : LocAndPrint
		
		        }


        $nodeclass Forward : PilotNode(PilotTypes::walk) : doStart {
                pilotreq.dx = 2800;
        }

		 $nodeclass HalfForward : PilotNode(PilotTypes::walk) : doStart 		{
	                pilotreq.dx = 1400;
	        }

        $nodeclass RightNode : PilotNode(PilotTypes::walk) :doStart {
                pilotreq.da = 1.57;
        }

		$nodeclass LeftNode : PilotNode(PilotTypes::walk) :doStart {
                pilotreq.da = 4.71238;
        }


       $nodeclass PrintPreLoc : PilotNode(PilotTypes::walk) :doStart {
		cout << "Pre-loc-- robot thinks it is at" <<  theAgent->getCentroid().coordX() << "," <<theAgent->getCentroid().coordY()<< endl;
		
	} 

	$nodeclass LocAndPrint : PilotNode(PilotTypes::localize) :doStop {
		cout << "Pre-loc-- robot thinks it is at" <<  theAgent->getCentroid().coordX() << "," <<theAgent->getCentroid().coordY()<< endl;
	}
}

REGISTER_BEHAVIOR(GetAround);