#include "Behaviors/StateMachine.h"

#ifdef TGT_HAS_GRIPPER

#define DEBUGPRINT false
#define MAX_NEG_LOAD -280
#define ANGLE_INCREMENT 0.05

$nodeclass GripObject2 {
	
	$nodeclass GripSetter : PostureNode {
		int currentLoad;
		int previousLoad;
		
		virtual void doStart(){
			//Set the initial position of the gripper to be widely open
			getMC()->setOutputCmd(GripperOffset, 1.70);
			previousLoad = 0;
			
			//Subscribe to sensor events, and set the current percentage
			erouter->addListener(this, EventBase::sensorEGID);
		}
		
		virtual void doEvent(){
			static float progressiveIncrement = 0.0;
			
			//Get the robot's load on its gripper
			currentLoad = int(state->pidduties[GripperOffset]*1023);
			
			//If the current load of the robot's gripper is at or past maximum, end the grasp command
			if(currentLoad < MAX_NEG_LOAD){

#if DEBUGPRINT == true
				cout << "Reached maximum load" << endl;
#endif
				cout << "Grasp complete" << endl;
				
				postStateCompletion();
			}
			else{
				//Otherwise, get the current angle of the gripper, and increment it
				float currentGripperAngle = state->outputs[GripperOffset];

#if DEBUGPRINT == true
				cout << "Gripper is now at " << currentGripperAngle << " with load " << currentLoad << endl;
#endif
				if(previousLoad == currentLoad && currentLoad != 0){
						progressiveIncrement += ANGLE_INCREMENT;
				} else {
					previousLoad = currentLoad;
					progressiveIncrement = 0;
				}
				
				getMC()->setOutputCmd(GripperOffset, currentGripperAngle - ANGLE_INCREMENT - progressiveIncrement);
			}

		}//END DOEVENT METHOD

	}//END GRIPSETTER CLASS
	
	$setupmachine{
		startnode: GripSetter =C=> end
		end: PostMachineCompletion
	}
}

#endif