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

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

using namespace DualCoding;

#nodeclass SwingAndSway : StateNode

  static MotionPtr<XWalkMC> walker;
  static MotionPtr<HeadPointerMC> headpointer;

  #shortnodeclass ArmAhead : ArmNode : DoStart
      getMC()->moveToPoint(200, 0, 41);

  #shortnodeclass Rock(float const offset, Point const &point) : StateNode : DoStart
      walker->offsetX = offset;
      headpointer->lookAtPoint(point.coordX(), point.coordY(), point.coordZ());

  #shortnodeclass Sway(float const offset) : StateNode : DoStart
      walker->offsetY = offset;

  #shortnodeclass Twist(float const angle) : StateNode : DoStart
      walker->offsetA = angle;

  #shortnodeclass LegsUp(float const groundOffset=60) : StateNode : DoStart
      walker->groundPlane[3] = groundOffset;

  #shortnodeclass Finish : StateNode : DoStart
      getParent()->stop();

  #nodemethod setup
    addMotion(walker);
    addMotion(headpointer);
    headpointer->setMaxSpeed(0, 0.5);
    headpointer->setMaxSpeed(1, 0.5);

      #statemachine
      startnode: SpeechNode("Swing and sway demo") =T(5000)=> ArmAhead() =C=>
        SpeechNode("Rock forward and back") =T(1000)=>
        Rock(25,Point(1000,0,1000)) =T(3000)=> Rock(-25,Point(1000,0,-1000)) =T(3000)=> 
        Rock(0,Point(1000,0,0)) =T(3000)=>

        SpeechNode("Sway side to side") =T(1000)=>
        Sway(25) =T(3000)=> Sway(-25) =T(3000)=> Sway(0) =T(3000)=>

        SpeechNode("Twist left and right") =T(1000)=>
        Twist(0.2) =T(3000)=> Twist(-0.2) =T(3000)=> Twist(0) =T(3000)=>

        SpeechNode("Resting now") =T(1000)=>
        LegsUp() =T(2000)=> Finish()
      #endstatemachine
  #endnodemethod

#endnodeclass

#endif