#include "Behaviors/StateMachine.h"

#define TIME_INCREMENT 50
#define LOAD_TOLERANCE 1.5
#define DEBUGPrint false

$nodeclass ShowLoad {
	//These are global variables
	$provide unsigned long time(0), int previousLoad(0);
	
  $nodeclass ShowLoad1 : doStart {
		$reference ShowLoad::time, ShowLoad::previousLoad;
		int currentLoad;
		
#ifdef TGT_HAS_FINGERS
		const unsigned int joint = LeftFingerOffset;
#elif TGT_HAS_GRIPPER
		const unsigned int joint = GripperOffset;
#elif TGT_MANTIS
		const unsigned int joint = 22; // left middle elevator
#else
		const unsigned int joint = 0;
#endif
		
		currentLoad = int(state->pidduties[joint]*1023);

#if DEBUGPrint == true
		cout << "Servo " << joint+1 << " ";
		cout << "\tLoad Difference " << ((currentLoad-previousLoad)/LOAD_TOLERANCE) << endl;
#endif
		
		//If everything is okay, increment time
		if(abs(currentLoad-previousLoad)/LOAD_TOLERANCE < 100)
				time += TIME_INCREMENT;
		else{	//Otherwise, output the values:

				cout << "Load " << previousLoad << " at time " << time << std::endl;
				
				//Reset the timer
				time = 0;
		}
		
		if(time % 1000 == 0)
			cout << "Load " << currentLoad << " at time " << time << std::endl;

		//Update the load
		previousLoad = currentLoad;
	}

  $setupmachine{
	  startnode: ShowLoad1 =T(TIME_INCREMENT)=> startnode
  }

}

REGISTER_BEHAVIOR(ShowLoad);