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

$nodeclass Part1 : PilotDemo {
  
  virtual void buildMap() {
    NEW_SHAPE(marker_1_r,EllipseData,new EllipseData(worldShS, Point(2000,300,0,allocentric), 150, 150));
    marker_1_r->setColor("red");

    NEW_SHAPE(marker_1_b,EllipseData,new EllipseData(worldShS, Point(2000,-300,0,allocentric), 150, 150));
    marker_1_b->setColor("blue");

    NEW_SHAPE(marker_2_r,EllipseData,new EllipseData(worldShS, Point(0,2000,0,allocentric), 150, 150));
    marker_2_r->setColor("red");

    NEW_SHAPE(marker_2_b,EllipseData,new EllipseData(worldShS, Point(600,2000,0,allocentric), 150, 150));
    marker_2_b->setColor("blue");
  }

  $nodeclass Scramble : PilotNode(PilotTypes::noRequest) : doStart {
    particleFilter->resetFilter();
  }

  $nodeclass Localize : PilotNode(PilotTypes::localize) : doStart {
    MapBuilderRequest* lex = new MapBuilderRequest(MapBuilderRequest::worldMap);
    lex->addObjectColor(ellipseDataType,"blue");
    lex->addObjectColor(ellipseDataType,"red");

    pilotreq.landmarkExtractor = lex;
  }

  $nodeclass Move : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.da = 1.57; // forgot the name for the variable pi
  }

  $setupmachine {
  startdemo: Scramble =TM("next")=> Localize =TM("next")=> Move =TM("next")=> Localize
  }

}

REGISTER_BEHAVIOR(Part1);