#include "Shared/RobotInfo.h"

#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"

$nodeclass Part4ComplexMotion : StateNode {

  // like for the simpler version, keeping only x as variable results in diagonal motion for some reason
  $nodeclass MoveAbout : DynamicMotionSequenceNode : doStart {
    int i;
    for (i=50; i<=450; i+=4) {
      PostureEngine pos = PostureEngine();

      pos.solveLinkPosition(fmat::pack(i,-100,350),LeftFingerFrameOffset,fmat::pack(0,0,0));

      getMC()->setPose(pos);
    }
  }

  virtual void setup() {
    $statemachine{
      MoveAbout
    }
  }

}

REGISTER_BEHAVIOR(Part4ComplexMotion);