#include "Behaviors/StateMachine.h"
#include "Behaviors/Mon/ArmController.h"
#include "Behaviors/Controller.h"
#include "Motion/MMAccessor.h"
#include "Motion/PIDMC.h"
#include "Shared/RobotInfo.h"
#include "Motion/ArmMC.h"
#include "Motion/MotionPtr.h"

$nodeclass HoldHand : StateNode {

	$provide fmat::Column<3> gripperZero;
	//$provide float xDisp;
	//$provide float yDisp;

	$provide float xVel;
	$provide float yVel;

	$nodeclass ReachOut : PostureNode : doStart {	

		for (unsigned int joint = 0; joint < NumArmJoints; joint++)
			getMC()->setOutputCmd(ArmOffset + joint, 0);
	}

	$nodeclass MeasureStart : StateNode : doStart {
	
		fmat::Transform gripperBase = kine->linkToBase(GripperFrameOffset);
		$reference HoldHand::gripperZero;
		gripperZero = gripperBase.translation();
		
		postStateCompletion();
	}


	$nodeclass WalkRobot : WalkNode {
		
		virtual void doStart() {
			erouter->addListener(this, EventBase::sensorEGID);
		}

		virtual void doEvent() {
			// calculate the distance between the gripper zero position and its
			// current position
			fmat::Transform gripperBase = kine->linkToBase(GripperFrameOffset);
			fmat::Column<3> gripperPos = gripperBase.translation();

			$reference HoldHand::gripperZero;
			float xDisp;
			float yDisp;

			xDisp = gripperPos[0] - gripperZero[0];
			cout << "x displacement is " << xDisp << " mm." << endl;

			yDisp = gripperPos[1] - gripperZero[1];
			cout << "y displacement is " << yDisp << " mm." << endl;

			$reference HoldHand::xVel;
			$reference HoldHand::yVel;

			if (abs(xDisp) >= 2)
				xVel = xDisp * 10;
			else xVel = 0;

			if (abs(yDisp) >= 2)
				yVel = yDisp * 10;
			else yVel = 0;

			this->setVelocity(xVel, yVel, 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 posturemc = addMotion(MotionPtr<PostureMC>());

		$statemachine{
	  		startnode: ReachOut[setMC(posturemc)] =T(1000)=> MeasureStart =C=> walk
			walk: WalkRobot
		}
	}
}

REGISTER_BEHAVIOR(HoldHand);