#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, 300);
    fmat::Column<3> targetBase = kine->linkToBase(CameraFrameOffset) * targetCam;
    fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
    unsigned int link = LeftFingerFrameOffset;

    PostureEngine *copy = new PostureEngine();
    copy->takeSnapshot();
    copy->solveLinkPosition(targetBase, link, noOffset);
    copy->setWeights(1);
    getMC()->setWeights(1);
    float dist = copy->diff(*getMC());
    cout << dist << endl;

    if (dist > 2.8) {
        getMC()->defaultMaxSpeed(.1);
    } else {
        getMC()->defaultMaxSpeed(1);
    }

    getMC()->solveLinkPosition(targetBase, link, noOffset);
    delete copy;
  }

  virtual void setup() {
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

    $statemachine{
  		startnode: ArmMover[setMC(posturemc)] =E(sensorEGID)=> startnode
    }
  }

}

REGISTER_BEHAVIOR(GripperTrackCamera2);

#endif