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

#include "Behaviors/StateMachine.h"

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

class XWalkTest : public StateNode {
public:
	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 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