#include <unistd.h>
#include "Shared/CalliopeInfo.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"
#include "Events/EventRouter.h"

$nodeclass HoldHands {

    $nodeclass SetGripper : PostureNode {
	virtual void doStart(){
	    getMC()->setOutputCmd(ArmBaseOffset, -0.0742); 
	    getMC()->setOutputCmd(ArmShoulderOffset, 2.6180);
	    getMC()->setOutputCmd(ArmElbowOffset, -2.6180);
	    getMC()->setOutputCmd(ArmWristOffset, -0.0384);
	}

	static fmat::Column<3> getPosition() {
	    cout << "getPosition()" << endl;
	    return kine->getPosition(LeftFingerFrameOffset);	
	}
    }

    $nodeclass Walk : WalkNode {
	fmat::Column<3> permanentPosit;

	virtual void doStart() {
	    //subscribe to sensorEGID events
	    cout << "Walk->doStart()" << endl;
	    erouter->addListener(this,
		EventBase::sensorEGID);
	    cout << "Subscribed?" << endl;
	    permanentPosit = SetGripper().getPosition();
	    cout << "permanentPosit: " << permanentPosit << endl;
	}

  	virtual void doEvent() {
	    cout << "Event!" << endl;
	    fmat::Column<3> gripperPosit = kine->getPosition(LeftFingerFrameOffset);
	    fmat::Column<3> diff = gripperPosit - permanentPosit;

	    cout << "Displacement: " << diff << endl;

	    float y_threshold = 5;
	    float z_threshold = 1;
	    int x=0,y=0;
	    if(abs(diff[2]) > z_threshold) {
		x = 1;
		cout << "Z displaced" << endl;
	    }
	    if(abs(diff[1]) > y_threshold) {
		y = 1;
		cout << "Y displaced" << endl;
	    }

	    getMC()->setTargetVelocity(x*4*diff[2], 0, y * diff[1] / 8);
  	}

    }

    $setupmachine{
	launch: SpeechNode("I am ready to be freaking sweet.") =C=> StateNode =N=> {gripperNode, wait}

	wait: StateNode
	walk: Walk
	gripperNode : SetGripper

	wait =T(2000)=> walk
    }

}

REGISTER_BEHAVIOR(HoldHands);