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

$nodeclass ParticleTest1 : PilotDemo {

  void buildMap() {

    NEW_SHAPE(w_green1, EllipseData,
              new EllipseData(worldShS, Point(1000, 300,0,allocentric), 100, 100));
    w_green1->setColor("green");
    NEW_SHAPE(w_red1, EllipseData,
              new EllipseData(worldShS, Point(1000,-300,0,allocentric), 100, 100));
    w_red1->setColor("red");

    NEW_SHAPE(w_green2, EllipseData,
              new EllipseData(worldShS, Point(1000,-2700,0,allocentric), 100, 100));
    w_green2->setColor("green");
    NEW_SHAPE(w_red2, EllipseData,
              new EllipseData(worldShS, Point(1000,-3300,0,allocentric), 100, 100));
    w_red2->setColor("red");

    NEW_SHAPE(w_blue1, EllipseData,
              new EllipseData(worldShS, Point(1000,-5700,0,allocentric), 100, 100));
    w_blue1->setColor("blue");
    NEW_SHAPE(w_blue2, EllipseData,
              new EllipseData(worldShS, Point(1000,-6300,0,allocentric), 100, 100));
    w_blue2->setColor("blue");

    // Mark all the ellipses as landmarks to be used by the Pilot
    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(worldShS));
    SHAPEVEC_ITERATE(ellipses, EllipseData, e) {
      e->setLandmark(true);
    } END_ITERATE;

    pilot->randomizeParticles(2000,2000);
    particleFilter->displayParticles(0.5);
  }

  $nodeclass TestLocalize : PilotNode(PilotTypes::localize) : doStart {
    pilotreq.displayParticles = 0.5;
  }

  $setupmachine{
    startdemo: HeadPointerNode[getMC()->lookAtPoint(1000,0,0)] =T(2000)=> loop

    loop: SpeechNode("Type msg 'go' to proceed") =TM("go")=> test

    test: TestLocalize
    test =PILOT(noError)=> SpeechNode("Success") =C=> loop
    test =PILOT=> SpeechNode("Failure") =C=> loop
  }

}

REGISTER_BEHAVIOR(ParticleTest1);