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

$nodeclass SnackRobot : PilotDemo {

	//***********************************************************************
	// Variables for setting things that maybe variable
	//***********************************************************************

	//***********************************************************************
	// buildMap
	// Create world representation for robot when behavior starts up
	//***********************************************************************
	virtual void buildMap() {
		//Cobot object
		NEW_SHAPE(cobot, EllipseData,
			new EllipseData(worldShS, Point(780/2-390, 800+305/2-400, 500/2,allocentric), 60, 60));
		cobot->setColor("black");

		//Snacks
		NEW_SHAPE(redSnack1, EllipseData,
			new EllipseData(worldShS, Point(780+300+100-390, 800-400, 50/2,allocentric), 60, 60));
		redSnack1->setColor("red");
		NEW_SHAPE(redSnack2, EllipseData,
			new EllipseData(worldShS, Point(780+300+200-390, 800-400, 50/2,allocentric), 60, 60));
		redSnack2->setColor("red");
		NEW_SHAPE(greenSnack1, EllipseData,
			new EllipseData(worldShS, Point(780+300+400-390, 800-400, 50/2,allocentric), 60, 60));
		greenSnack1->setColor("green");
		NEW_SHAPE(greenSnack2, EllipseData,
			new EllipseData(worldShS, Point(780+300+500-390, 800-400,allocentric), 60, 60));
		greenSnack2->setColor("green");
		
	}
	
	//***********************************************************************
	// CobotToSnack
	// State representing the robot moving from the cobot to the snacks
	//***********************************************************************
	$nodeclass CobotToSnack : StateNode {
		$setupmachine {
			walk: WalkNode(500,0,0,0,WalkNode::DISP) =C=> Turn(M_PI/2) =C=> PostMachineCompletion
		}
	}

	//***********************************************************************
	// SnackToCobot
	// State representing the robot moving from cobot to snacks
	//***********************************************************************
	$nodeclass SnackToCobot : StateNode  {
		$setupmachine {
			walk: Turn(M_PI/2) =C=> WalkNode(500,0,0,0,WalkNode::DISP) =C=> PostMachineCompletion
		}
	}


	//***********************************************************************
	// Top level state machine for executing all lower level tasks
	//***********************************************************************
	$setupmachine {
		startdemo: CobotToSnack =C=> SnackToCobot
	}
}
REGISTER_BEHAVIOR(SnackRobot);