#include "Behaviors/StateMachine.h"

#define TARGET_LOAD 250
#define DELTA_MAX 0.60
#define LOAD_DELTA 0.01

$nodeclass ApplyLoad {
  $provide int runningLoad;
  $provide float runningDifference;
  $provide float commandedAngle;

  $nodeclass ChangeLoad : PostureNode  {
	virtual void doStart(){ 
		//Subscribe to sensor EGID events
		$reference ApplyLoad::runningLoad;
		$reference ApplyLoad::commandedAngle;
		runningLoad = 0;
		commandedAngle = state->outputs[HeadPanOffset];
		erouter->addListener(this, EventBase::sensorEGID);
	}

	virtual void doStop(){		
		motman->addPrunableMotion(SharedObject<PIDMC>(0));
	}
	
	virtual void doEvent(){
		$reference ApplyLoad::runningLoad;
		$reference ApplyLoad::runningDifference;
		$reference ApplyLoad::commandedAngle;
		
		unsigned int joint = HeadPanOffset;
		int load = int(state->pidduties[joint]*1023);
		float distanceToTarget = float(abs(TARGET_LOAD - runningLoad)) / TARGET_LOAD;
		float difference = abs(LOAD_DELTA * distanceToTarget);
		if (difference > DELTA_MAX) difference = DELTA_MAX;
		if (runningLoad <= TARGET_LOAD) {
			difference = difference;
		} else if (runningLoad > TARGET_LOAD) {
			difference = -difference;
		} 
		
		runningLoad += (load - runningLoad)/4;
		runningDifference = (difference * 1.0/4.0) + (runningDifference * 3.0/4.0);
		commandedAngle += runningDifference;
		getMC()->setOutputCmd(HeadPanOffset, commandedAngle);  
	}
  }


  $setupmachine{
	startnode: ChangeLoad
  }

}

REGISTER_BEHAVIOR(ApplyLoad);