#ifndef Included_MotionNodes_h_
#define Included_MotionNodes_h_

#include "Crew/PilotNode.h"
#include "Behaviors/Nodes/PIDNode.h"
#include "Behaviors/Nodes/PostureNode.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


// GripperPower
#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) : PostStateCompletion {}
#endif

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

#ifdef TGT_HAS_GRIPPER
$nodeclass OpenGripper(float percentage=0.5) : ArmNode : start {
  ArmNode::start();
#if defined(TGT_IS_CALLIOPE2)
  getMC()->setMaxSpeed(0.2);
  float gripperAngle = outputRanges[GripperOffset][MinRange] +
    percentage * (outputRanges[GripperOffset][MaxRange] - outputRanges[GripperOffset][MinRange]);
  getMC()->setJointValue(GripperOffset-ArmOffset, gripperAngle);
#elif defined(TGT_IS_CALLIOPE5)
  getMC()->setMaxSpeed(0.2);
  float gripperAngleL = outputRanges[LeftFingerOffset][MaxRange] +
    percentage * (outputRanges[LeftFingerOffset][MinRange] - outputRanges[LeftFingerOffset][MaxRange]);
  float gripperAngleR = outputRanges[RightFingerOffset][MinRange] +
    percentage * (outputRanges[RightFingerOffset][MaxRange] - outputRanges[RightFingerOffset][MinRange]);
  getMC()->setJointValue(LeftFingerOffset-ArmOffset, gripperAngleL);
  getMC()->setJointValue(RightFingerOffset-ArmOffset, gripperAngleR);
#endif
}
#else
  $nodeclass OpenGripper(float percentage=0.5) : PostStateCompletion {}
#endif

#if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE5)
$nodeclass ParkArm : StateNode {
  $setupmachine{
    PostureNode("park1.pos")[getMC()->defaultMaxSpeed(0.3)] =C=>
      PostureNode("park2.pos")[getMC()->defaultMaxSpeed(0.3)] =C=>
        ArmPower(0.0) =C=> PostMachineCompletion
  }
}
#else
$nodeclass ParkArm : PostStateCompletion {}
#endif

#endif