#include "Behaviors/StateMachine.h"

#ifdef TGT_HAS_GRIPPER

$nodeclass GripObject(float initialPercent = 0.01) {
//The parameter initialPercent indicates where to start (in percent closed-ness) when gripping an object
		
//The joint to watch for is GripperOffset when using CALLIOPE2SP
int currentLoad;
float currentPercent;

	virtual void doStart(){
		//Subscribe to sensor events, and set the current percentage
		erouter->addListener(this, EventBase::sensorEGID);
		
		currentPercent = initialPercent;
	}

	virtual void doEvent(){
		char *ID = "a";
		
		//Get initial load on the gripper
		currentLoad = int(state->pidduties[GripperOffset]*1023);
		
		//Progressively close the gripper until the load is safely and sufficiently closed
		//if(currentLoad < -300)
		//	PostMachineCompletion();
		//else{
			*ID++;
			GraspValue(ID, currentPercent);
			currentPercent = currentPercent + 1.01;
			cout << "Current load " << currentLoad << "\tCurrent percent " << currentPercent << endl;
		//}
	}
	
	$nodeclass GraspValue(float percent) : ArmNode : doStart {
		getMC()->setMaxSpeed(0.2);
		getMC()->openGripper(percent);
	}

	
}

REGISTER_BEHAVIOR(GripObject);

/*
class GripObject : public ArmNode {
public:
GripObject(const std::string &nodename="GripObject", float _percentage=0.01) : ArmNode(nodename), percentage(_percentage) {}
float percentage;  // Percentage is the default start angle for gripping
virtual void start() {
	ArmNode::start();
#if defined(TGT_IS_CALLIOPE)
 getMC()->setMaxSpeed(0.2);
 getMC()->openGripper(percentage);
#endif
  }
};
#else
class OpenGripper : public PostStateCompletion {
public:
  OpenGripper(const std::string &nodename="OpenGripper", float _percentage=0.5) : PostStateCompletion(nodename), percentage(_percentage) {}
  float percentage;  // cache the constructor's parameter
};
*/

#endif