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

$nodeclass ThreePuzzle : PilotDemo {
  virtual void buildMap() {
    int const ellipseSize = 50;
    NEW_SHAPE(orange, EllipseData,
	      new EllipseData(worldShS, Point(1000,0,0,allocentric), ellipseSize, ellipseSize));
    NEW_SHAPE(pink, EllipseData,
	      new EllipseData(worldShS, Point(1600,600,0,allocentric), ellipseSize, ellipseSize));
    NEW_SHAPE(green, EllipseData,
	      new EllipseData(worldShS, Point(1600,-600,0,allocentric), ellipseSize, ellipseSize));
    NEW_SHAPE(blue, EllipseData,
	      new EllipseData(worldShS, Point(400,-600,0,allocentric), ellipseSize, ellipseSize));
    NEW_SHAPE(yellow, EllipseData,
	      new EllipseData(worldShS, Point(400,600,0,allocentric), ellipseSize, ellipseSize));
    NEW_SHAPE(vline, LineData,
	      new LineData(worldShS, Point(400,0,0,allocentric), Point(1600,0,0,allocentric)));
    NEW_SHAPE(hline, LineData,
	      new LineData(worldShS, Point(1000,-600,0,allocentric), Point(1000,600,0,allocentric)));

    orange->setColor("orange");
    pink->setColor("pink");
    green->setColor("green");
    blue->setColor("blue");
    yellow->setColor("yellow");
    vline->setColor(rgb(0,0,0));
    hline->setColor(rgb(0,0,0));

    vector<ShapeRoot> landmarks;
    landmarks.push_back(orange);
    landmarks.push_back(pink);
    landmarks.push_back(green);
    landmarks.push_back(blue);
    landmarks.push_back(yellow);
    pilot->setDefaultLandmarks(landmarks);

    MapBuilderRequest mapreq(MapBuilderRequest::localMap);
    mapreq.addAllObjectColors(ellipseDataType);
    pilot->setDefaultLandmarkExtractor(mapreq);
  }

  $nodeclass Push_3_4 : StateNode {
    $nodeclass LineUp : PilotNode($,PilotTypes::goToShape) : doStart {
      worldShS.deleteShapes<PointData>();
      NEW_SHAPE(destination, PointData, new PointData(worldShS, Point(700,800,0,allocentric)));
      pilotreq.targetShape = destination;
    }

    $nodeclass Turn : PilotNode($,PilotTypes::walk) : doStart {
      pilotreq.da = -M_PI/2 - theAgent->getOrientation();
    }

    $nodeclass PushIt : PilotNode($,PilotTypes::walk) : doStart {
      worldShS.deleteShapes<PointData>();
      //NEW_SHAPE(destination, PointData, new PointData(worldShS, Point(700,-150,0,allocentric)));
      //pilotreq.targetShape = destination;
      pilotreq.dx = 800;
      pilotreq.forwardSpeed = 10;
      pilotreq.collisionAction = collisionIgnore;
    }

    $setupmachine{
      LineUp =PILOT=> Turn =PILOT=> PushIt =PILOT=> PostMachineCompletion
    }
  }

  $setupmachine{
    Push_3_4 =C=> SpeechNode($,"done pushing")
  }

}

REGISTER_BEHAVIOR(ThreePuzzle);