#include "Shared/RobotInfo.h"

#if defined(TGT_HAS_ARMS) && defined(TGT_HAS_HEAD)

#include "Behaviors/StateMachine.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Crew/Grasper.h"
#include "Motion/MotionPtr.h"
#include "Events/EventRouter.h"

using namespace fmat;
const float rest_z = 138.488;
const float rest_y = -111.633;
$nodeclass HoldHands {

  $nodeclass StraightArm : PostureNode : doStart {
      getMC()->setOutputCmd(ArmOffset+0, 0.0);
      getMC()->setOutputCmd(ArmOffset+1, 0.0);
  }

  $nodeclass ArmMover : PostureNode : doStart {
	fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
#ifdef TGT_IS_CALLIOPE5
		unsigned int link = LeftFingerFrameOffset;
#else
		unsigned int link = GripperFrameOffset;
#endif
    //getMC()->solveLinkPosition(target, link, noOffset);
  }

  $nodeclass Follower : WalkNode {
        virtual void doStart() {
            erouter->addListener(this, EventBase::sensorEGID);
        }
        virtual void doEvent() {
            Transform t = kine->baseToLink(GripperFrameOffset);
            cout << t.fmt("%8.3f") << endl;
            SubVector<3> trans = t.translation();
            float zdiff = trans[2] - rest_z;
            float ydiff = trans[1] - rest_y;
            printf("zdiff=%f, ydiff,%f\n", zdiff, ydiff);
            float threshold = 2.0;
            if (abs(zdiff) < threshold) zdiff = 0;
            if (abs(ydiff) < threshold) ydiff = 0;
            setVelocity(-zdiff*50, 0, -ydiff/10.0);
        }


  }


  virtual void setup() {
		// We make the posture motion command here and supply it to the
		// PostureNode via setMC() so that it only has to be registered
		// with the MotionManager once. Registering and deregistering a
		// PostureMC inside a tight sensor loop is inefficient and leads
		// to crashes.
		MotionManager::MC_ID armmc = addMotion(MotionPtr<PostureMC>());

    $statemachine{
  		startnode: StateNode =N=> {straight, follow}
        straight: StraightArm[setMC(armmc)]
        follow: Follower
        straight =E(sensorEGID)=> straight
    }
  }

}

REGISTER_BEHAVIOR(HoldHands);

#endif