#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
    getMC()->solveLinkPosition(targetBase, link, noOffset);
  }

/////////////////////////////////////////////////////////////////////////////

  $nodeclass MoveBegin : PostureNode : doStart {

	//Determine robot type
#ifdef TGT_IS_CALLIOPE5
		unsigned int link = LeftFingerFrameOffset;
#else
		unsigned int link = GripperFrameOffset;
#endif

	//Get matrix for target location
	 fmat::Column<3> targetCam = fmat::pack(0, 0, 100);
    	 fmat::Column<3> targetBase = kine->linkToBase(CameraFrameOffset) * targetCam;

 	//Get matrix for current gripper location
	 fmat::Column<3> currentLoc = kine->linkToBase(link).translation();

	//Get distance between target and current location
	float dist = (currentLoc - targetBase).norm();

	//If distance is above certain threshold, slow down speed
	float threshold = 300;
	float speed = 0.3;
	if (dist > threshold) {
		getMC()->setMaxSpeed(ArmOffset,speed);
		getMC()->setMaxSpeed(ArmBaseOffset,speed);
		getMC()->setMaxSpeed(ArmShoulderOffset,speed);
		getMC()->setMaxSpeed(ArmWristOffset,speed);
		getMC()->setMaxSpeed(link,speed);
	}

	//Move to position
	fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
	getMC()->solveLinkPosition(targetBase, link, noOffset);
	postStateCompletion();
  }

/////////////////////////////////////////////////////////////////////////////
  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: MoveBegin[setMC(posturemc)] =C=> armmove
	armmove: ArmMover[setMC(posturemc)] =E(sensorEGID)=> armmove
    }
  }

/////////////////////////////////////////////////////////////////////////////
}

REGISTER_BEHAVIOR_MENU(GripperTrackCamera2,DEFAULT_TK_MENU"/Kinematics Demos");

#endif