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

$nodeclass ToFirstCylinder : PilotDemo
{
	$provide Point old_position;
	$provide Point new_position;
	$provide AngTwoPi old_angle;
	$provide AngTwoPi new_angle;

	$setupmachine {
		startdemo: OriginalPosition =C=> TurnCCW(30) =C=> GoForward(800) =C=> TurnCW(120) =C=> GoForward(600) =C=> TurnCW(110) =C=> RunDemo =C=> PostMachineCompletion
	}

	$nodeclass OriginalPosition : PilotNode(PilotTypes::walk) : doStart
	{
		$reference ToFirstCylinder::old_position, ToFirstCylinder::old_angle;
		old_position = theAgent->getCentroid();
		old_angle = theAgent->getOrientation();
		cout << "Robot starts out at " << old_position << " and oriented at " << float(old_angle)*180/M_PI << " degrees" << endl;
	}

	$nodeclass TurnCCW(float angle) : PilotNode(PilotTypes::walk) : doStart
	{
		pilotreq.collisionAction = collisionIgnore;
		pilotreq.da = deg2rad(angle);
	}

	$nodeclass TurnCW(float angle) : PilotNode(PilotTypes::walk) : doStart
	{
		pilotreq.collisionAction = collisionIgnore;
		pilotreq.da = -deg2rad(angle);
	}

	$nodeclass GoForward(float x) : PilotNode(PilotTypes::walk) : doStart
	{
		pilotreq.collisionAction = collisionIgnore;
		pilotreq.dx = x;
	}

	$nodeclass RunDemo : PilotDemo
	{
		$setupmachine {
			rundemo: FinalPosition =C=> PostMachineCompletion
		}

		$nodeclass FinalPosition : PilotNode(PilotTypes::walk) : doStart
		{
			$reference ToFirstCylinder::new_position, ToFirstCylinder::old_position;
			$reference ToFirstCylinder::new_angle;

			new_position = theAgent->getCentroid();
			new_angle = theAgent->getOrientation();
			float new_ang = float(new_angle) * 180 / M_PI;
			cout << "Robot ends at " << new_position << " and oriented at " << float(new_angle)*180/M_PI << " degrees" << endl;
			float move_pos = (new_position - old_position).xyNorm();
			float to_ang = rad2deg(atan(new_position.coordY() / new_position.coordX()));
			cout << "Robot must get to " << to_ang << " degrees" << endl;
			if(to_ang >= 0)
				to_ang = to_ang - 180;
			else
				to_ang = to_ang + 180;
			float move_ang = to_ang - new_ang;
			if(move_ang > 90)
				move_ang = 180 - move_ang;
			else if(move_ang < -90)
				move_ang = move_ang + 180;
			cout << "Robot must turn " << move_ang << " degrees and move " << move_pos << endl;
			pilotreq.collisionAction = collisionIgnore;
			pilotreq.da = deg2rad(move_ang);
			pilotreq.dx = move_pos;
		}
	}
}

REGISTER_BEHAVIOR(ToFirstCylinder);