#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

    // get distance
    fmat::Transform curr = kine->linkToBase(link);
    fmat::Column<3> currPos = curr.translation();
    float dist = (targetBase-currPos).norm();

    // set speed, hopefully we are using a calliope 5
    // perhaps this should placed be in the above macro?
    float threshold = 50;
    float speed = 0.5;
    if (dist > threshold) {
      getMC()->setMaxSpeed(ArmBaseOffset,speed);
      getMC()->setMaxSpeed(ArmShoulderOffset,speed);
      getMC()->setMaxSpeed(ArmElbowOffset,speed);
      getMC()->setMaxSpeed(ArmWristOffset,speed);
      getMC()->setMaxSpeed(WristRotateOffset,speed);
      getMC()->setMaxSpeed(LeftFingerOffset,speed);
      getMC()->setMaxSpeed(RightFingerOffset,speed);
    }

    // solve
    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