#include "Shared/RobotInfo.h"
#ifdef TGT_CHIARA

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

using namespace std;

//! Demonstrates one, two, and three-legged walking gaits
class ThreeGaits : public StateNode {
public:
	ThreeGaits() : StateNode("ThreeGaits") {}
	
	virtual void setup() {
		MotionPtr<WalkMC> walker;
		MotionManager::MC_ID walkID = addMotion(walker);
#statemachine
		startnode:  SpeechNode($, "Chiara walk demo.") =T(3000)=>
		
		SpeechNode($, "This is a ripple gait.  I move one leg at a time.") =T(6000)=>
		WalkNode($, 400, 0, 0, 0, WalkNode::DISP)[setMC(walkID)] =C=>
		WalkNode($, 0, 0, -M_PI/2, 3, WalkNode::DISP)[setMC(walkID)] =C=>
		WalkNode($, 0, -400, 0, 0, WalkNode::DISP)[setMC(walkID)] =C=>
		WalkNode($, 0, 0, M_PI/2, 3, WalkNode::DISP)[setMC(walkID)] =C=>
		
		SpeechNode($, "Now I will switch to a new gait and move two legs at a time.") =T(8000)=>
		WalkNode($, "xwalk2.plist")[setMC(walkID)] =T(5000)=>
		WalkNode($, 400, 0, 0, 0, WalkNode::DISP)[setMC(walkID)] =C=>
		WalkNode($, 0, 0, -M_PI/2, 5, WalkNode::DISP)[setMC(walkID)] =C=>
		WalkNode($, 0, -400, 0, 0, WalkNode::DISP)[setMC(walkID)] =C=>
		WalkNode($, 0, 0, M_PI/2, 5, WalkNode::DISP)[setMC(walkID)] =C=>
		
		SpeechNode($, "The tripod gait moves three legs at a time.") =T(8000)=>
		WalkNode($, "xwalk3.plist")[setMC(walkID)] =T(5000)=>
		WalkNode($, 400, 0, 0, 0, WalkNode::DISP)[setMC(walkID)] =C=>
		WalkNode($, 0, 0, -M_PI/2, 5, WalkNode::DISP)[setMC(walkID)] =C=>
		WalkNode($, 0, -400, 0, 0, WalkNode::DISP)[setMC(walkID)] =C=>
		WalkNode($, 0, 0, M_PI/2, 5, WalkNode::DISP)[setMC(walkID)] =C=>
		
		SpeechNode($,"Done")
#endstatemachine
	}
	
	static std::string getClassDescription() {
		return "One, two, and three-legged walking gaits";
	}
	
};

REGISTER_BEHAVIOR_MENU(ThreeGaits,DEFAULT_TK_MENU"/Chiara Demos");

#endif