#include "Shared/RobotInfo.h"
#if defined(TGT_HAS_LEGS) && !defined(TGT_IS_AIBO)

#include "Localization/LocalizationNode.h"
#include "Planners/Navigation/FollowPathNode.h"
#ifdef TGT_HAS_HEAD
#  include "Localization/LookAtMarkers.h"
#endif
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"
#include "Behaviors/Demos/Tapia/TapiaMarkerData.h"

using namespace DualCoding;
using namespace std;

$nodeclass* PathFollowing : VisualRoutinesStateNode : localizeNode(NULL) {

  $nodeclass DummyStart : StateNode : doStart {
    Point target(350,-380);
    postStateSignal<Point>(target);
  }

  $nodeclass DummyFinish : StateNode : doStart {
    getParent()->stop();
  }

#ifdef TGT_HAS_HEAD
  $nodeclass MaybeLookAtMarkers : LookAtMarkers {}
#else
  $nodeclass MaybeLookAtMarkers : StateNode {}
#endif

  virtual void setup() {
    MapBuilderRequest mapreq(MapBuilderRequest::cameraMap);
    mapreq.rawY = true;
    mapreq.addMarkerType(BiColorMarkerData::biColorMarkerType);
    mapreq.addAllObjectColors(markerDataType);
    mapreq.immediateRequest = true;
    $statemachine{
      startnode: StateNode =N=> {look, localize, dummystart}

      look: MaybeLookAtMarkers

      localize: LocalizationNode($,mapreq)

      dummystart: DummyStart =S<Point>=> FollowPathNode =C=>
        SpeechNode($, "Done") =T(3000)=> DummyFinish

    }
    localizeNode = localize;
  }

  virtual void doStart() {
    buildMap();
  }

  virtual void buildMap() {

    cout << "Building map..." << endl;
    
    const float s = 770;  // scale of the maze in mm
    
    // world bounds
    vector<Point> pts;
    pts.push_back(Point(   0,    0));
    pts.push_back(Point( 2*s,    0));
    // bump 1
    pts.push_back(Point( 2*s,   -s));
    pts.push_back(Point( 3*s,   -s));
    pts.push_back(Point( 3*s, -2*s));
    pts.push_back(Point( 2*s, -2*s));
    // bump 2
    pts.push_back(Point( 2*s, -3*s));
    pts.push_back(Point( 3*s, -3*s));
    pts.push_back(Point( 3*s, -4*s));
    pts.push_back(Point( 2*s, -4*s));
    // bump 3
    pts.push_back(Point( 2*s, -5*s));
    pts.push_back(Point(   0, -5*s));
    pts.push_back(Point(   0, -4*s));
    pts.push_back(Point(   s, -4*s));
    // bump 4
    pts.push_back(Point(   s, -3*s));
    pts.push_back(Point(   0, -3*s));
    pts.push_back(Point(   0, -2*s));
    pts.push_back(Point(   s, -2*s));
    // bump 5
    pts.push_back(Point(   s,   -s));
    pts.push_back(Point(   0,   -s));
    //		pts.push_back(Point(   0,    0));
    //		pts.push_back(Point(   s,    0));
    NEW_SHAPE(worldBounds, PolygonData, new PolygonData(worldShS, pts, true));
    
      const float HEIGHT = 177.8f;
      
    NEW_SHAPE(marker1, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1.5f*s,-3,HEIGHT),
				  ProjectInterface::getColorRGB("blue"),
				  ProjectInterface::getColorRGB("yellow")));
    
    NEW_SHAPE(marker2, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(2*s-3,-0.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("green"),
				  ProjectInterface::getColorRGB("yellow")));
    
    NEW_SHAPE(marker3, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(3*s-3,-1.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("green"),
				  ProjectInterface::getColorRGB("blue")));
    
    NEW_SHAPE(marker4, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(2*s-3,-2.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("orange"),
				  ProjectInterface::getColorRGB("green")));
    
    NEW_SHAPE(marker5, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(3*s-3,-3.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("orange"),
				  ProjectInterface::getColorRGB("blue")));
    
    NEW_SHAPE(marker6, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(2*s-3,-4.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("pink"),
				  ProjectInterface::getColorRGB("orange")));
    
    NEW_SHAPE(marker7, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1.5f*s,-5*s+3,HEIGHT),
				  ProjectInterface::getColorRGB("green"),
				  ProjectInterface::getColorRGB("pink")));
    
    NEW_SHAPE(marker8, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(3,-4.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("yellow"),
				  ProjectInterface::getColorRGB("orange")));
    
    NEW_SHAPE(marker9, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1*s+3,-3.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("blue"),
				  ProjectInterface::getColorRGB("pink")));
    
    NEW_SHAPE(marker10, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(0*s+3,-2.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("pink"),
				  ProjectInterface::getColorRGB("yellow")));
    
    NEW_SHAPE(marker11, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(1*s+3,-1.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("pink"),
				  ProjectInterface::getColorRGB("blue")));
    
    NEW_SHAPE(marker12, TapiaMarkerData,
	      new TapiaMarkerData(worldShS, Point(0*s+3,-0.5f*s,HEIGHT),
				  ProjectInterface::getColorRGB("pink"),
				  ProjectInterface::getColorRGB("green")));
    
    // set up the starting point
    localizeNode->setPosition(300,-4.5f*s, 0);
  }
  
  static std::string getClassDescription() {
    return "A path following node.";
  }

private:
  LocalizationNode *localizeNode;
  
}

REGISTER_BEHAVIOR_MENU(PathFollowing,DEFAULT_TK_MENU"/Navigation Demos");

#endif