#include "Shared/RobotInfo.h"

#if defined(TGT_HAS_ARMS) && defined(TGT_HAS_HEAD)

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

$nodeclass GripperTrackCamera2 : StateNode {

  $nodeclass ArmMover : PostureNode : doStart {
    fmat::Column<3> targetCam = fmat::pack(0, 0, 100);
    fmat::Column<3> targetBase = kine->linkToBase(CameraFrameOffset) * targetCam;
		fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
#ifdef TGT_IS_CALLIOPE5
		unsigned int link = LeftFingerFrameOffset;
#else
		unsigned int link = GripperFrameOffset;
#endif

		/* Calculate the distance that the arm is going to travel when moving from
		   its current position to the target position. If this distance is greater
       than some threshold, slow the arm down */

		// Get the arm position
		fmat::Transform arm = kine->linkToBase(link);
		fmat::Column<3> armPos = arm.translation();
		// Get the distance between the arm position and the camera
		float dist = (armPos - targetBase).norm();
		// Slow down the arm if it is moving more than 150 mm
		if (dist > 150) {
			// Slow down each joint
      for (unsigned int i = 0; i < NumArmJoints; i++) {
				getMC()->setMaxSpeed(ArmBaseOffset + i, 0.3);
			}
		}
		// Otherwise allow the arm to move faster
		else {
			for (unsigned int i = 0; i < NumArmJoints; i++) {
				getMC()->setMaxSpeed(ArmBaseOffset + i, 1);
			}
		}

    getMC()->solveLinkPosition(targetBase, link, noOffset);
  }

  virtual void setup() {
		// We make the posture motion command here and supply it to the
		// PostureNode via setMC() so that it only has to be registered
		// with the MotionManager once. Registering and deregistering a
		// PostureMC inside a tight sensor loop is inefficient and leads
		// to crashes.
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

    $statemachine{
  		startnode: ArmMover[setMC(posturemc)] =E(sensorEGID)=> startnode
    }
  }

}

REGISTER_BEHAVIOR(GripperTrackCamera2);

#endif