#include <stdio.h>
#include <math.h>

#include "Behaviors/StateMachine.h"
#include "Behaviors/Demos/Navigation/PilotDemo.h"
$nodeclass Lab3Q3 : PilotDemo{
	$nodeclass Turner : PilotNode(PilotTypes::walk) : doStart{
		pilotreq.collisionAction = collisionIgnore;
		Point position = theAgent->getCentroid();
		float currOrient =(float)theAgent->getOrientation();
		float x = (float)position.coordX();
		float y = (float)position.coordY();
		//cout << x << endl;
		//cout << y << endl;
		float targetOrient = atan(y/x);
		if(x > 0){
			targetOrient += M_PI;
		}
		pilotreq.da = targetOrient - currOrient;
		//cout << currOrient << endl;
		//cout << targetOrient << endl;
		//cout << pilotreq.da << endl;
	}
	$nodeclass Pusher : PilotNode(PilotTypes::walk) : doStart{
		Point position = theAgent->getCentroid();
		float x = (float)position.coordX();
		float y = (float)position.coordY();
		pilotreq.collisionAction = collisionIgnore;
		float distance = sqrt(pow(x,2) + pow(y,2));
		pilotreq.dx = distance;
		//cout << distance << endl;

	}	
	$setupmachine{
		rundemo: Turner =PILOT=> Pusher
	}
}
REGISTER_BEHAVIOR (Lab3Q3);