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

$nodeclass TurnToCenterAndDrive : PilotDemo {

	$setupmachine {
		rundemo: TurnAndDrive 
	}

	$nodeclass TurnAndDrive : StateNode {
		$setupmachine {
		
		launch: StateNode =N=> turn

		turn: Turn((atan(theAgent->getCentroid().coordY() / theAgent->getCentroid().coordX()) + M_PI - theAgent->getOrientation()))
		turn =PILOT=> Forward
		Forward =PILOT=> PostMachineCompletion()
	}

	$nodeclass Forward: PilotNode(PilotTypes::walk) : doStart {
			pilotreq.dx = sqrt(theAgent->getCentroid().coordX() * theAgent->getCentroid().coordX() + theAgent->getCentroid().coordY() * theAgent->getCentroid().coordY());
			pilotreq.collisionAction = collisionIgnore;
		}
	}

}

REGISTER_BEHAVIOR(TurnToCenterAndDrive);