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

#include "LocalizationNode.h"
#include "Motion/MotionPtr.h"

using namespace DualCoding;
using namespace std;

class LocalizationNodeTest : public LocalizationNode {
private:

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

  virtual void setup() {
    MotionPtr<XWalkMC> walker;
    MotionManager::MC_ID walkID = addMotion(walker);
#statemachine
  startnode: SpeechNode($,"Move the robot!")
#endstatemachine
  }

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

    // world bounds
    vector<Point> pts;
    pts.push_back(Point(0,0));
    pts.push_back(Point(1560,0));
    pts.push_back(Point(1560,-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(910,-50,203.2),
				  ProjectInterface::getColorRGB("orange"),
				  ProjectInterface::getColorRGB("blue")));

    NEW_SHAPE(marker2, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1510,-2700,203.2),
				  ProjectInterface::getColorRGB("pink"),
				  ProjectInterface::getColorRGB("orange")));

    NEW_SHAPE(marker3, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1510,-910,203.2),
				  ProjectInterface::getColorRGB("blue"),
				  ProjectInterface::getColorRGB("pink")));

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

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

};

#endif