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

#include "Events/EventRouter.h"
#include "Events/VisionObjectEvent.h"
#include "Shared/WorldState.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/MMAccessor.h"
#include "Shared/ProjectInterface.h"
#include "Shared/ChiaraInfo.h"
#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.h"
#include "Motion/MotionManager.h"
#include "DualCoding/MapBuilderNode.h"
#include "DualCoding/DualCoding.h"
#include "Motion/MotionSequenceMC.h"

class GoThisDistance : public StateNode{
  class GoThisDistanceNode : public StateNode {
  public:
    MotionPtr<XWalkMC> walker;  // cache the constructor's parameter
    GoThisDistanceNode(const std::string &nodename, MotionPtr<XWalkMC> _walker) : StateNode(nodename), walker(_walker) {}
    virtual void DoStartEvent(const EventBase &event) {
      const DataEvent<Point>& ptr = dynamic_cast<const DataEvent<Point>&>(event);
      walker->setTargetDisplacement(ptr.getData().coordX(), ptr.getData().coordY(), 0);
    }
  };
    class Box : public StateNode{
    public:
      Box(const std::string &nodename): StateNode(nodename){}
      virtual void DoStart(){
	Point box = Point(200, 0);
	postStateSignal<Point>(box);
      }
    };
    class Box2 : public StateNode{
    public:
       Box2(const std::string &nodename): StateNode(nodename){}
      virtual void DoStart(){
	Point box = Point(0, 45);
	postStateSignal<Point>(box);
      }
    };
    class Box3 : public StateNode{
    public:
      Box3(const std::string &nodename): StateNode(nodename){}
      virtual void DoStart(){
	Point box = Point(-200, 0);
	postStateSignal<Point>(box);
      }
    };
    class Box4 : public StateNode{
    public:
      Box4(const std::string &nodename): StateNode(nodename){}
      virtual void DoStart(){
	Point box = Point(0, -45);
	postStateSignal<Point>(box);
      }
    };

    virtual void Setup(){
#statemachine
    startnode: SpeechNode($, "Going to the next point") =N=>
	move1: Box=S<Point>=>walk: GoThisDistanceNode =T(2000) =>
	move2: Box2 =S<Point>=>walk2: GoThisDistanceNode =T(1000)=>
	move3: Box3 =S<Point>=>walk3: GoThisDistanceNode =T(2000)=>
	move4: Box4=S<Point>=>walk4: GoThisDistanceNode =T(2000)=>startnode
#endstatemachine
	}
};
#endif