#ifndef Included_MotionNodes_h_
#define Included_MotionNodes_h_

#include "Crew/PilotNode.h"
#include "Behaviors/Nodes/PIDNode.h"
#include "Behaviors/Nodes/PostMachine.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"

$nodeclass WalkForward(float xdist=0) : PilotNode(PilotTypes::walk) : preStart {
  PilotNode::preStart();
  pilotreq.dx = xdist;
}

$nodeclass WalkSideways(float ydist=0) : PilotNode(PilotTypes::walk) : preStart {
  PilotNode::preStart();
  pilotreq.dy = ydist;
}

$nodeclass Turn(float angle=0) : PilotNode(PilotTypes::walk) : preStart {
  PilotNode::preStart();
  pilotreq.da = angle;
}	

#ifdef TGT_HAS_ARMS
$nodeclass ArmPower(float level) : PIDNode(ArmOffset,ArmOffset+NumArmJoints,_level) {}

$nodeclass ResetArm : StateNode {
  $setupmachine{
    ArmPower(0.0) =T(500)=> ArmPower(1.0) =N=> PostMachineCompletion
  }
}
#else
$nodeclass ArmPower(float level) : StateNode {}
$nodeclass ResetArm : StateNode {}
#endif

#if defined(TGT_IS_CALLIOPE2)
$nodeclass GripperPower(float level) : PIDNode(GripperOffset,GripperOffset+1,_level) {}
#elif defined(TGT_IS_CALLIOPE5)
$nodeclass GripperPower(float level) : PIDNode(LeftFingerOffset,RightFingerOffset+1,_level) {}
#else
$nodeclass GripperPower(float level) : StateNode {}
#endif

#if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE5)
$nodeclass ResetGripper : StateNode {
  $setupmachine{
    GripperPower(0.0) =T(500)=> GripperPower(1.0) =C=> PostMachineCompletion
  }
}
#else
$nodeclass ResetGripper : StateNode {}
#endif


#endif