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

$nodeclass Part2 : PilotDemo {
    
  virtual void buildMap() {

    // pushable canisters
    
    NEW_SHAPE(can1,EllipseData,new EllipseData(worldShS, Point(1500,-200,0,allocentric), 100, 100));
    can1->setColor("red");
    NEW_SHAPE(can2,EllipseData,new EllipseData(worldShS, Point(700,800,0,allocentric), 100, 100));
    can2->setColor("red");
    NEW_SHAPE(can3,EllipseData,new EllipseData(worldShS, Point(800,-850,0,allocentric), 100, 100));
    can3->setColor("red");
    NEW_SHAPE(can4,EllipseData,new EllipseData(worldShS, Point(1300,500,0,allocentric), 100, 100));
    can4->setColor("red");
    NEW_SHAPE(can5,EllipseData,new EllipseData(worldShS, Point(350,250,0,allocentric), 100, 100));
    can5->setColor("red");
    

    // obstacles
    NEW_SHAPE(obstacle1,EllipseData,new EllipseData(worldShS, Point(1300,1200,0,allocentric), 100, 100));
    obstacle1->setColor("green");
    NEW_SHAPE(obstacle2,EllipseData,new EllipseData(worldShS, Point(650,-300,0,allocentric), 100, 100));
    obstacle2->setColor("green");
    NEW_SHAPE(obstacle3,EllipseData,new EllipseData(worldShS, Point(400,-600,0,allocentric), 100, 100));
    obstacle3->setColor("green");
        
    // target square
    std::vector<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);
    
  }

  // lots of boilderplate code here
  $nodeclass Push1 : PilotNode(PilotTypes::pushObject) : doStart {

    // strange event of the day: robot collides with the origin
    theAgent->setObstacle(false);

    GET_SHAPE(can1,EllipseData,worldShS);
    can1->setObstacle(false);
    pilotreq.objectShape = can1;
    
    GET_SHAPE(box,PolygonData,worldShS);
    pilotreq.targetShape = box;

    //pilotreq.displayObstacles = true;
    pilotreq.obstacleInflation = 25;

    pilotreq.collisionAction = PilotTypes::collisionReplan;
  }


  $nodeclass Push2 : PilotNode(PilotTypes::pushObject) : doStart {
    // strange event of the day: robot collides with the origin
    theAgent->setObstacle(false);

    GET_SHAPE(can1,EllipseData,worldShS);
    can1->setObstacle(false);
    GET_SHAPE(can2,EllipseData,worldShS);
    can2->setObstacle(false);
    pilotreq.objectShape = can2;
    
    GET_SHAPE(box,PolygonData,worldShS);
    pilotreq.targetShape = box;

    //pilotreq.displayObstacles = true;
    pilotreq.obstacleInflation = 25;

    pilotreq.collisionAction = PilotTypes::collisionReplan;
  }


  $nodeclass Push3 : PilotNode(PilotTypes::pushObject) : doStart {
    // strange event of the day: robot collides with the origin
    theAgent->setObstacle(false);

    GET_SHAPE(can1,EllipseData,worldShS);
    can1->setObstacle(false);
    GET_SHAPE(can2,EllipseData,worldShS);
    can2->setObstacle(false);
    GET_SHAPE(can3,EllipseData,worldShS);
    can3->setObstacle(false);
    pilotreq.objectShape = can3;
    
    GET_SHAPE(box,PolygonData,worldShS);
    pilotreq.targetShape = box;

    //pilotreq.displayObstacles = true;
    pilotreq.obstacleInflation = 25;

    pilotreq.collisionAction = PilotTypes::collisionReplan;
  }


  $nodeclass Push4 : PilotNode(PilotTypes::pushObject) : doStart {
    // strange event of the day: robot collides with the origin
    theAgent->setObstacle(false);

    GET_SHAPE(can1,EllipseData,worldShS);
    can1->setObstacle(false);
    GET_SHAPE(can2,EllipseData,worldShS);
    can2->setObstacle(false);
    GET_SHAPE(can3,EllipseData,worldShS);
    can3->setObstacle(false);
    GET_SHAPE(can4,EllipseData,worldShS);
    can4->setObstacle(false);
    pilotreq.objectShape = can4;
    
    GET_SHAPE(box,PolygonData,worldShS);
    pilotreq.targetShape = box;

    //pilotreq.displayObstacles = true;
    pilotreq.obstacleInflation = 25;

    pilotreq.collisionAction = PilotTypes::collisionReplan;
  }


  $nodeclass Push5 : PilotNode(PilotTypes::pushObject) : doStart {
    // strange event of the day: robot collides with the origin
    theAgent->setObstacle(false);

    GET_SHAPE(can1,EllipseData,worldShS);
    can1->setObstacle(false);
    GET_SHAPE(can2,EllipseData,worldShS);
    can2->setObstacle(false);
    GET_SHAPE(can3,EllipseData,worldShS);
    can3->setObstacle(false);
    GET_SHAPE(can4,EllipseData,worldShS);
    can4->setObstacle(false);
    GET_SHAPE(can5,EllipseData,worldShS);
    can5->setObstacle(false);
    pilotreq.objectShape = can5;
    
    GET_SHAPE(box,PolygonData,worldShS);
    pilotreq.targetShape = box;

    //pilotreq.displayObstacles = true;
    pilotreq.obstacleInflation = 25;

    pilotreq.collisionAction = PilotTypes::collisionReplan;
  }


  $setupmachine {
    startdemo:  Push1 =C=> Push2 =C=> Push3 =C=> Push4 =C=> Push5
  }

}

REGISTER_BEHAVIOR(Part2);