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

$nodeclass PushCanisters : PilotDemo
{
	virtual void buildMap()
	{
	   	std::vector<DualCoding::Point> boxpts;
	   	float boxSide = 750;  // millimeters
	   	float offset = 500 + boxSide/2;
	   	boxpts.push_back(Point(offset-boxSide/2, boxSide/2,0,allocentric));
	   	boxpts.push_back(Point(offset+boxSide/2, boxSide/2,0,allocentric));
	   	boxpts.push_back(Point(offset+boxSide/2,-boxSide/2,0,allocentric));
	   	boxpts.push_back(Point(offset-boxSide/2,-boxSide/2,0,allocentric));
	   	NEW_SHAPE(box, PolygonData, new PolygonData(worldShS, boxpts, true));
	   	box->setColor("blue");
	   	box->setObstacle(false);

        NEW_SHAPE(source_one, EllipseData,
                       new EllipseData(worldShS, Point(350,250,66,allocentric), 100, 100));
        source_one->setColor("red");

        NEW_SHAPE(source_two, EllipseData,
                       new EllipseData(worldShS, Point(700,800,66,allocentric), 100, 100));
        source_two->setColor("red");
		
        NEW_SHAPE(source_three, EllipseData,
                       new EllipseData(worldShS, Point(1300,500,66,allocentric), 100, 100));
        source_three->setColor("red");

        NEW_SHAPE(source_four, EllipseData,
                       new EllipseData(worldShS, Point(1500,-200,66,allocentric), 100, 100));
        source_four->setColor("red");

        NEW_SHAPE(source_five, EllipseData,
                       new EllipseData(worldShS, Point(800,-850,66,allocentric), 100, 100));
        source_five->setColor("red");

        NEW_SHAPE(obstacle_one, EllipseData,
                       new EllipseData(worldShS, Point(1300,1200,0,allocentric), 100, 100));
        obstacle_one->setColor("green");

        NEW_SHAPE(obstacle_two, EllipseData,
                       new EllipseData(worldShS, Point(650,-300,0,allocentric), 100, 100));
        obstacle_two->setColor("green");

        NEW_SHAPE(obstacle_three, EllipseData,
                       new EllipseData(worldShS, Point(400,-600,0,allocentric), 100, 100));
        obstacle_three->setColor("green");


	}

   	$nodeclass PushOne : PilotNode(PilotTypes::pushObject) : doStart
	{
        GET_SHAPE(source_one, EllipseData, worldShS);
        GET_SHAPE(box, PolygonData, worldShS);
        pilotreq.objectShape = source_one;
        pilotreq.targetShape = box;
    }

   	$nodeclass PushTwo : PilotNode(PilotTypes::pushObject) : doStart
	{
		GET_SHAPE(source_one, EllipseData, worldShS);
  		source_one->setObstacle(false);
	    GET_SHAPE(source_two, EllipseData, worldShS);
        GET_SHAPE(box, PolygonData, worldShS);
        pilotreq.objectShape = source_two;
        pilotreq.targetShape = box;
		source_two->setObstacle(false);
    }

   	$nodeclass PushThree : PilotNode(PilotTypes::pushObject) : doStart
	{
		GET_SHAPE(source_two, EllipseData, worldShS);
		source_two->setObstacle(false);
        GET_SHAPE(source_three, EllipseData, worldShS);
        GET_SHAPE(box, PolygonData, worldShS);
        pilotreq.objectShape = source_three;
        pilotreq.targetShape = box;
    }

   	$nodeclass PushFour : PilotNode(PilotTypes::pushObject) : doStart
	{
		GET_SHAPE(source_three, EllipseData, worldShS);
		source_three->setObstacle(false);
        GET_SHAPE(source_four, EllipseData, worldShS);
        GET_SHAPE(box, PolygonData, worldShS);
        pilotreq.objectShape = source_four;
        pilotreq.targetShape = box;
    }

   	$nodeclass PushFive : PilotNode(PilotTypes::pushObject) : doStart
	{
		GET_SHAPE(source_four, EllipseData, worldShS);
		source_four->setObstacle(false);
        GET_SHAPE(source_five, EllipseData, worldShS);
        GET_SHAPE(box, PolygonData, worldShS);
        pilotreq.objectShape = source_five;
        pilotreq.targetShape = box;
    }

    $setupmachine{
    	startdemo: PushOne =C=> PushTwo =C=> PushThree =C=> PushFour =C=> PushFive =C=> SpeechNode("done")
  	}

}

REGISTER_BEHAVIOR(PushCanisters);