//-*-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