//-*-c++-*-

#ifndef Included_THREEGAITS_H_
#define Included_THREEGAITS_H_

#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<XWalkMC> 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)=>
      XWalkNode($, 0, 400, 0, 0, 0, 0)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, 0, 0.15, -M_PI/2)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, -400, 0, 0)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, 0, 0.15, M_PI/2)[setMC(walkID)] =C=>

      SpeechNode($, "Now I will switch to a new gait and move two legs at a time.") =T(8000)=>
      XWalkNode($, "ms/data/motion/xwalk2.plist")[setMC(walkID)] =T(5000)=>
      XWalkNode($, 0, 400, 0, 0, 0, 0)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, 0, 0.05, -M_PI/2)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, -400, 0, 0)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, 0, 0.05, M_PI/2)[setMC(walkID)] =C=>

      SpeechNode($, "The tripod gait moves three legs at a time.") =T(8000)=>
      XWalkNode($, "ms/data/motion/xwalk3.plist")[setMC(walkID)] =T(5000)=>
      XWalkNode($, 0, 400, 0, 0, 0, 0)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, 0, 0.05, -M_PI/2)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, -400, 0, 0)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, 0, 0.05, M_PI/2)[setMC(walkID)] =C=>

      SpeechNode($,"Done")
#endstatemachine
  }

  static std::string getClassDescription() {
    return "One, two, and three-legged walking gaits";
  }

};

#endif