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

#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.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;

  #nodemethod DoStart
    addMotion(armRelaxer);
    addMotion(headMover);
    erouter->addListener(this, EventBase::sensorEGID);
	
  #nodemethod processEvent
    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]);

#endnodeclass

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

#endif