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

#include "Behaviors/BehaviorBase.h"
#include "Motion/Kinematics.h"
#include "Motion/MotionPtr.h"
#include "Motion/PIDMC.h"
#include "Shared/fmat.h"

using namespace std;

class FootToGripper : public BehaviorBase {
public:
  FootToGripper() : BehaviorBase("FootToGripper"), 
		    armRelaxer(SharedObject<PIDMC>(ArmOffset, ArmOffset+JointsPerArm, 0.f)),
		    legRelaxer(SharedObject<PIDMC>(RFrRotatorOffset, RFrRotatorOffset+JointsPerLeg+1, 0.f))
  {}

  virtual void DoStart() {
    cout << "The arm and right front leg are relaxed so you can move them." << endl;
    addMotion(armRelaxer);
    addMotion(legRelaxer);
    erouter->addTimer(this, 0, 500, true);
  }

  virtual void processEvent(const EventBase&) {
    fmat::Transform rfrFoot = kine->linkToBase(FootFrameOffset+RFrLegOrder);
    fmat::Column<3> rfrFootPos = rfrFoot.translation();

    fmat::Transform gripper = kine->linkToBase(GripperFrameOffset);
    fmat::Column<3> gripperPos = gripper.translation();

    float dist = (rfrFootPos-gripperPos).norm();
    cout << "Foot=" << rfrFootPos.fmt("%5.1f")
	 << " Gripper=" << gripperPos.fmt("%5.1f")
	 << " Distance=" << setw(5) << dist << " mm." << endl;
  }

private:
  MotionPtr<PIDMC> armRelaxer, legRelaxer;

};

#endif