#include "Behaviors/StateMachine.h"

#ifdef TGT_HAS_GRIPPER

#define DEBUGPRINT true
#define MAX_NEG_LOAD -300
#define PERCENT_INCREMENT 0.10

$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 < MAX_NEG_LOAD)
			PostMachineCompletion();
		else{
			//*ID++;
#if DEBUGPRINT == true
			cout << "Performing event" << endl;
#endif
			
			GraspValue("My_GraspCommand", currentPercent);

#if DEBUGPRINT == true
			cout << "Finished event" << endl;
#endif

			currentPercent = currentPercent + PERCENT_INCREMENT;
			cout << "Current load " << currentLoad << "\tCurrent percent " << currentPercent << endl;
		}
	}
	
	$nodeclass GraspValue(float percent) : ArmNode : doStart {

#if DEBUGPRINT == true
		cout << "\tEntering GraspValue with percent = " << percent << endl;
#endif
		
		getMC()->setMaxSpeed(0.2);
		getMC()->openGripper(percent);

#if DEBUGPRINT == true		
		cout << "\tLeaving GraspValue" << endl;
#endif
	}

	
}

//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