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

#include "Behaviors/StateMachine.h"
#include "Motion/XWalkMC.h"
#include "Motion/MotionPtr.h"

// This behavior demonstrates how to smoothly chain XWalkNodes together
// by using a shared XWalkMC.

class XWalkTest : public StateNode {
public:

  class TalkToWalker : public StateNode {
  public:
    TalkToWalker() : StateNode("TalkToWalker") {}
    virtual void DoStart() {
      // walk forward by 200 mm and to the right by 50 mm; speed is 20 mm/sec
      WalkRequest req(20, 200, 20, -50, 0, 0);
      postStateSignal<WalkRequest>(req);
    }
  };

  XWalkTest() : StateNode("XWalkTest") {}
  virtual void setup() {
    MotionManager::MC_ID walkID = addMotion(MotionPtr<XWalkMC>());
#statemachine
  startnode: 
    // walk forward at 20 mm/sec for 200 mm 
    XWalkNode($,20,200,0,0,0,0)[setMC(walkID);] =C=> SpeechNode("One") =T(1000)=>

    // now send a signal to the next walk node to walk diagonally
    TalkToWalker() =S<WalkRequest>=> XWalkNode[setMC(walkID)] =C=> SpeechNode("Two") =T(1000)=>

    // now turn at 0.05 rad/sec to the right by 1 radians
    XWalkNode($, 0, 0, 0, 0, 0.05, -1)[setMC(walkID)] =C=> SpeechNode("Three")
#endstatemachine
      }
};

#endif