//-*-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() {
      std::cout << "Sending WalkRequest signal" << std::endl;
      WalkRequest req(20,500,20,500,0,0);
      postStateSignal<WalkRequest>(req);
    }
  };

  XWalkTest() : StateNode("XWalkTest") {}

  virtual void setup() {
    MotionPtr<XWalkMC> walker;
    addMotion(walker);
    MotionManager::MC_ID walkID = walker->getID();
    std::cout << "setup:  walker is " << (void*)((walker.operator->()).operator->()) << ":id=" << walkID << std::endl;
		
#statemachine

  startnode: 
    // walk forward at 20 mm/sec for a while, then pause for 3 secs
    XWalkNode($,20,0,0)[setMC(walkID);] =T(10000)=> StateNode =T(3000)=>

    // now send a signal to the next walk node to walk diagonally
    TalkToWalker() =S<WalkRequest>=> XWalkNode =C=>

    // now walk forward at 20 mm/sec for 200 mm 
    XWalkNode($,20,200,0,0,0,0)[setMC(walkID);] =C=>

    // now turn at 0.1 rad/sec to the right by 1 radians
    XWalkNode($,0,0,0,0,0.1,-1)[setMC(walkID);]

#endstatemachine
      }
};

#endif