#include "Behaviors/StateMachine.h"

$nodeclass LinkOrient {

  $nodeclass MoveArm : PostureNode : doStart {
    fmat::Column<3> target = fmat::pack(250,0,16);
    fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(-M_PI/2);
    fmat::Column<3> posOffset = fmat::pack(0,0,0);
    fmat::Quaternion oriOffset = fmat::Quaternion::identity();
    cout << "ArmElbow max speed=" << getMC()->getMaxSpeed(ArmElbowOffset) << endl;
    bool success = getMC()->solveLink(target, oriTgt, GripperFrameOffset, posOffset, oriOffset);
    cout << "success=" << success << endl;
  }

  $nodeclass ReportPosition : StateNode : doStart {
    fmat::Column<3> pos = kine->linkToBase(GripperFrameOffset).translation();
    cout << "Position = " << pos << endl;
  }

  $setupmachine{
    MoveArm =C=> StateNode =T(200)=> // add a little time for the arm to settle
      ReportPosition
      }

}

REGISTER_BEHAVIOR(LinkOrient);