#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"

$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
        fmat::Transform disp = kine->linkToLink(GripperFrameOffset, CameraFrameOffset);
        if (disp.translation().norm() > 200) {
            float max_speed = 0.5;
            getMC()->setMaxSpeed(ArmBaseOffset, max_speed);
            getMC()->setMaxSpeed(ArmShoulderOffset, max_speed);
            getMC()->setMaxSpeed(ArmElbowOffset, max_speed);
            getMC()->setMaxSpeed(ArmWristOffset, max_speed);
            getMC()->setMaxSpeed(WristRotateOffset, max_speed);
            getMC()->setMaxSpeed(LeftFingerOffset, max_speed);
            getMC()->setMaxSpeed(RightFingerOffset, max_speed);
        } else {
            getMC()->noMaxSpeed();
        }
        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_MENU(GripperTrackCamera2,DEFAULT_TK_MENU"/Kinematics Demos");

#endif