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

#define DIST_THRESHOLD 100
#define SLOW_SPEED M_PI*1/8
#define FAST_SPEED M_PI*3/4

$nodeclass GripperTrackCamera2 : StateNode {

  $nodeclass ArmMover : PostureNode : doStart {
    fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    fmat::Column<3> Pgripper = Tgripper.translation();

    fmat::Column<3> targetCam = fmat::pack(0, 0, 100);
    fmat::Column<3> targetBase = kine->linkToBase(CameraFrameOffset) * targetCam;

    float dist = (Pgripper - targetBase).norm();
    std::cout << "dist:" << dist << std::endl;

    if (dist > DIST_THRESHOLD) {
        for(unsigned int i=ArmOffset; i<ArmOffset+NumArmJoints; ++i)
            getMC()->setMaxSpeed(i,(float)SLOW_SPEED);
    } else {
        for(unsigned int i=ArmOffset; i<ArmOffset+NumArmJoints; ++i)
            getMC()->setMaxSpeed(i,(float)FAST_SPEED);
    }

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

  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