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

$nodeclass PushCanister : PilotDemo {

	$nodeclass MoveXHome: PilotNode(PilotTypes::walk): doStart {
		pilotreq.collisionAction = collisionIgnore;
		
		//Turn to face southward or northward depending on position
		double curX = theAgent->getCentroid().coordX();
		AngTwoPi orient =  theAgent->getOrientation();
		
		//Move home in x-coord
		if(curX >= 0) {
			pilotreq.da = 3.14 - orient;
			pilotreq.dx = curX;
		}
		else  {
			pilotreq.da = -orient;
			pilotreq.dx = -curX;
		}
		postStateCompletion();

	}

	$nodeclass MoveYHome: PilotNode(PilotTypes::walk): doStart {
		pilotreq.collisionAction = collisionIgnore;

		//Turn to face eastward or westward depending on position
		double curY = theAgent->getCentroid().coordY();
		AngTwoPi orient =  theAgent->getOrientation();

		//Move home in y-coord
		if(curY >= 0 && (orient < 1 || orient > 5)) {
			pilotreq.da = -1.57;
			pilotreq.dx = curY;
		}
		else if (curY < 0 && (orient < 1 || orient > 5)) {
		 	pilotreq.da = 1.57;
			pilotreq.dx = -curY;
		}
		else if (curY >= 0 && (orient >= 1 && orient < 5)) { 
			pilotreq.da = 1.57;
			pilotreq.dx = curY;
		}
		else {
			pilotreq.da = -1.57;
			pilotreq.dx = -curY;
		}
	}

	$setupmachine {
		rundemo: MoveXHome =PILOT=> MoveYHome
	}
}
REGISTER_BEHAVIOR(PushCanister);