//-*-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->jointToBase(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