#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 HoldingHands : StateNode {

  $provide fmat::Column<3> startGripper;
 
  $provide float xVel;
  $provide float yVel;
 
 //set arm pos to straight out
  $nodeclass StartPos : PostureNode : doStart {
		for (unsigned int joint = 0; joint < NumArmJoints; joint++) {
			getMC()->setOutputCmd(ArmOffset + joint, 0);
		}
  }
  
  //calculate start pos
  $nodeclass CalcStart : StateNode : doStart {
  		fmat::Transform gripperBase = kine->linkToBase(GripperFrameOffset);
		$reference HoldingHands::startGripper;
		startGripper= gripperBase.translation();
		postStateCompletion();
  }
  
  
  $nodeclass SuspendArm : PostureNode : doStart {
  
  		$reference HoldingHands::startGripper;
  		
  		//arm is suspended
 	 	for (unsigned int joint = 0; joint < NumArmJoints; joint++) {
			getMC()->setOutputCmd(ArmOffset + joint, 0);
		}
		
		fmat::Transform gripBase = kine->linkToBase(GripperFrameOffset);
		fmat::Column<3> gripPos = gripBase.translation();
		
		float xDispl;
		xDispl = gripPos[0] - startGripper[0];
		float yDispl;
		yDispl = gripPos[1] - startGripper[1];
		cout << "xDispl = " << xDispl << " , yDispl = " << yDispl << "." << endl;
		
		$reference HoldingHands::xVel;
		$reference HoldingHands::yVel;
		
		if (abs(xDispl) >= 10) {
			xVel = xDispl/10;
		}
		else {
			xVel = 0;
		}

		if (abs(yDispl) >= 10) {
			yVel = yDispl/10);
		}
		else {
			yVel = 0;
		}
		
  } //end SuspendArm
  
  
  
  virtual void setup() {
  		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

		$statemachine{
	  		startnode: StartPos[setMC(posturemc)] =T(1000)=> calc
	  		calc: CalcStart =C=> walk
			walking: SuspendArm[setMC(posturemc)] =E(sensorEGID)=> WalkNode(xVel, yVel, 0) =E(sensorEGID)=> walking 
		}
	}
	
  
  
  
REGISTER_BEHAVIOR(HoldingHands);

#endif