//-*-c++-*-
#ifndef _FollowPathNodeTest_h_
#define _FollowPathNodeTest_h_

#include "LocalizationNode.h"
#include "FollowPathNode.h"
#include "LookAtMarkers.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"
#include "Behaviors/Demos/Tapia/TapiaMarkerData.h"

using namespace DualCoding;
using namespace std;

class FollowPathNodeTest : public LocalizationNode {
private:

public:
  FollowPathNodeTest() : LocalizationNode("FollowPathNodeTest")
  {
  }

  class DummyStart : public StateNode {
  public:
    DummyStart(const std::string &nodename): StateNode(nodename) {}
    virtual void DoStart(){
      Point target(350,-380);
      postStateSignal<Point>(target);
    }
  };

#shortnodeclass DummyFinish : StateNode : DoStart
  getParent()->stop();

  virtual void setup() {
#statemachine
  startnode: StateNode =N=> {look,dummy}
  dummy: DummyStart =S<Point>=> FollowPathNode($, this) =C=>
      SpeechNode($, "Done") =T(3000)=> DummyFinish

      look: LookAtMarkers($, this)
#endstatemachine
  }

  virtual void buildMap()
  {
    cout << "Building map..." << endl;

    // world bounds
    vector<Point> pts;
    pts.push_back(Point(0,0));
    pts.push_back(Point(1680,0));
    pts.push_back(Point(1680,-2750));
    pts.push_back(Point(0,-2750));
    pts.push_back(Point(0,-1990));
    pts.push_back(Point(910,-1990));
    pts.push_back(Point(910,-1680));
    pts.push_back(Point(-110,-1680));
    pts.push_back(Point(-110,-1070));
    pts.push_back(Point(910,-1070));
    pts.push_back(Point(910,-760));
    pts.push_back(Point(0,-760));
    NEW_SHAPE(bounds, PolygonData, new PolygonData(worldShS, pts, true));
    
    setWorldBounds(bounds);

    NEW_SHAPE(marker1, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(500,-2749,203.2),
				  ProjectInterface::getColorRGB("green"),
				  ProjectInterface::getColorRGB("red")));

    NEW_SHAPE(marker2, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1000,-2749,203.2),
				  ProjectInterface::getColorRGB("yellow"),
				  ProjectInterface::getColorRGB("blue")));

    NEW_SHAPE(marker3, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1500,-2749,203.2),
				  ProjectInterface::getColorRGB("pink"),
				  ProjectInterface::getColorRGB("yellow")));

    NEW_SHAPE(marker4, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1679,-2450,203.2),
				  ProjectInterface::getColorRGB("blue"),
				  ProjectInterface::getColorRGB("green")));

    NEW_SHAPE(marker5, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1679,-1950,203.2),
				  ProjectInterface::getColorRGB("orange"),
				  ProjectInterface::getColorRGB("blue")));

    // set up the starting point
    setPosition(350,-2370, 0.0);
  }

  static std::string getClassDescription() {
    return "A test of the localization node.";
  }

};

#endif