#include "Shared/RobotInfo.h"
#if defined(TGT_HAS_ARMS) && defined(TGT_HAS_HEAD)

#include "Behaviors/StateNode.h"
#include "IPC/SharedObject.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/Kinematics.h"
#include "Motion/MotionPtr.h"
#include "Motion/PIDMC.h"

//! Relaxes the arm and keeps the camera pointed at the gripper as the user moves the arm around
$nodeclass CameraTrackGripper : StateNode : headMover(),
					    armRelaxer(SharedObject<PIDMC>(ArmOffset, ArmOffset+NumArmJoints, 0.f)) {

  MotionPtr<HeadPointerMC> headMover;
  MotionPtr<PIDMC> armRelaxer;

  virtual void doStart() {
    addMotion(armRelaxer);
    addMotion(headMover);
    erouter->addListener(this, EventBase::sensorEGID);
  }
	
  virtual void doEvent() {
    fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    std::cout << "Gripper to base transform:\n" << Tgripper.fmt("%7.3f") << std::endl;
    fmat::Column<3> target = Tgripper.translation();
    // If you just want the joint origin and not the full
    // transformation matrix, you could also do: 
    //    target = kine->getPosition(GripperFrameOffset);
    headMover->lookAtPoint(target[0],target[1],target[2]);
  }
}

REGISTER_BEHAVIOR_MENU(CameraTrackGripper,DEFAULT_TK_MENU"/Kinematics Demos");

#endif