//-*-c++-*-
#ifndef INCLUDED_CameraTrackGripper_h_
#define INCLUDED_CameraTrackGripper_h_

#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"
#include "Motion/Kinematics.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/PIDMC.h"
#include "Motion/MotionPtr.h"
#include "Shared/fmat.h"
#include "Shared/RobotInfo.h"

using namespace std;

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

  virtual void DoStart() {
    addMotion(armRelaxer);
    addMotion(headMover);
    erouter->addListener(this, EventBase::sensorEGID);
  }

  virtual void processEvent(const EventBase&) {
    fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    cout << "Transform:\n" << Tgripper.fmt("%7.3f") << endl;
    fmat::Column<3> target = fmat::SubVector<3>(&Tgripper(0,3));
    headMover->lookAtPoint(target[0],target[1],target[2]);
  }

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

private:
  CameraTrackGripper(const CameraTrackGripper&);
  CameraTrackGripper& operator=(const CameraTrackGripper&);
};

#endif