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

$nodeclass PushBox2 : PilotDemo {

	virtual void buildMap() {
        // Objects
		NEW_SHAPE(object1, EllipseData,
							new EllipseData(worldShS, Point(1500,-200,0,allocentric), 80, 80));
		object1->setColor("red");
//        object1->setObstacle(false);
		
        NEW_SHAPE(object2, EllipseData,
							new EllipseData(worldShS, Point(700,800,0,allocentric), 80, 80));
		object2->setColor("red");
//        object2->setObstacle(false);
        
        NEW_SHAPE(object3, EllipseData,
							new EllipseData(worldShS, Point(800,-850,0,allocentric), 80, 80));
		object3->setColor("red");
//        object3->setObstacle(false);
        
        NEW_SHAPE(object4, EllipseData,
							new EllipseData(worldShS, Point(1300,500,0,allocentric), 80, 80));
		object4->setColor("red");
//        object4->setObstacle(false);
        
        NEW_SHAPE(object5, EllipseData,
							new EllipseData(worldShS, Point(350,250,0,allocentric), 80, 80));
		object5->setColor("red");
//        object5->setObstacle(false);

        // Target
        std::vector<Point> boxpts;
        float boxSide = 750;
        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);


        // Obstacles
		NEW_SHAPE(obstacle1, EllipseData,
							new EllipseData(worldShS, Point(1300,1200,0,allocentric), 70, 70, -M_PI/4));
		
        NEW_SHAPE(obstacle2, EllipseData,
							new EllipseData(worldShS, Point(650,-300,0,allocentric), 70, 70, -M_PI/4));
		
        NEW_SHAPE(obstacle3, EllipseData,
							new EllipseData(worldShS, Point(400,-600,0,allocentric), 70, 70, -M_PI/4));

		
	}

	$nodeclass PushIt5 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(object5, EllipseData, worldShS);
		GET_SHAPE(box, PolygonData, worldShS);
		pilotreq.objectShape = object5;
		pilotreq.targetShape = box;
	}
    
    $nodeclass PushIt1 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(object1, EllipseData, worldShS);
		GET_SHAPE(object3, EllipseData, worldShS);
		GET_SHAPE(box, PolygonData, worldShS);
        object3->setObstacle(false);
		pilotreq.objectShape = object1;
		pilotreq.targetShape = box;
	}
	
    $nodeclass PushIt2 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(object2, EllipseData, worldShS);
		GET_SHAPE(object5, EllipseData, worldShS);
		GET_SHAPE(box, PolygonData, worldShS);
        object5->setObstacle(false);
		pilotreq.objectShape = object2;
		pilotreq.targetShape = box;
	}
	
    $nodeclass PushIt3 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(object4, EllipseData, worldShS);
		GET_SHAPE(object3, EllipseData, worldShS);
		GET_SHAPE(box, PolygonData, worldShS);
        object4->setObstacle(false);
		pilotreq.objectShape = object3;
		pilotreq.targetShape = box;
	}
    
    $nodeclass PushIt4 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(object2, EllipseData, worldShS);
		GET_SHAPE(object4, EllipseData, worldShS);
		GET_SHAPE(box, PolygonData, worldShS);
        object2->setObstacle(false);
		pilotreq.objectShape = object4;
		pilotreq.targetShape = box;
	}

	$setupmachine{
  	startdemo: PushIt5 =C=> PushIt2 =C=> PushIt4 =C=> PushIt3 =C=> PushIt1 =C=> SpeechNode("done")
  }

}

REGISTER_BEHAVIOR(PushBox2);