#include "Behaviors/StateMachine.h"
#include "Shared/RobotInfo.h"

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

#include "Behaviors/Nodes/PostureNode.h"
#include "Motion/MotionPtr.h"

#define MAX_DIST 30
#define MAX_SPEED 0.4f

#ifdef TGT_IS_CALLIOPE5
#define LINK LeftFingerFrameOffset
#else
#define LINK GripperFrameOffset
#endif

$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);

	/* Calculate total distance */
	fmat::Column<3> sV = kine->getPosition(LINK);
	fmat::Column<3> dV = sV - targetBase; 
	float d = sqrt(dV[0]*dV[0] + dV[1]*dV[1] + dV[2]*dV[2]);

	/* Threshold of distance to slow everything down */
	if (d > MAX_DIST) {

		/* Must slow down ALL joints in the arm - each of them can snap really fast otherwise 
			Reference to joint names found in "CommonCalliopeInfo.h"
		*/
		getMC()->setMaxSpeed(ArmOffset, MAX_SPEED);
		getMC()->setMaxSpeed(ArmShoulderOffset, MAX_SPEED);
		getMC()->setMaxSpeed(ArmElbowOffset, MAX_SPEED);
		getMC()->setMaxSpeed(ArmWristOffset, MAX_SPEED);
		getMC()->setMaxSpeed(WristRotateOffset, MAX_SPEED);
//		cout << "TAKING IT EASY SO MY ARM DOESN'T MURDER YOU!! >:)" << endl;
	}

	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{
			movenode: ArmMover[setMC(posturemc)] =E(sensorEGID)=> movenode
		}
	}
}

REGISTER_BEHAVIOR(GripperTrackCamera2);

#endif