#include "Behaviors/StateMachine.h"

#define TARGET_LOAD 300
#define TARGET_RANGE 30
#define LOAD_DELTA 0.04 
#define LOWER_LOAD (TARGET_LOAD - TARGET_RANGE)
#define UPPER_LOAD (TARGET_LOAD + TARGET_RANGE)


$nodeclass ApplyLoad {
  $nodeclass ChangeLoad : PostureNode  {
	virtual void doStart(){ 
		//Subscribe to sensor EGID events	
		erouter->addListener(this, EventBase::sensorEGID);
	}

	virtual void doStop(){		
		motman->addPrunableMotion(SharedObject<PIDMC>(0));
	}
	
	virtual void doEvent(){
		unsigned int joint = HeadPanOffset;
		int load = int(state->pidduties[joint]*1023);
		//cout << "Servo " << joint+1 << " load is " << load << std::endl;
		float difference = 0;
		if (load < LOWER_LOAD) {
			cout << "TOO LOW " << load << std::endl;
			difference = LOAD_DELTA;
		} else if (load > UPPER_LOAD) {
			cout << "TOO HIGH " << load << std::endl;
			difference = -LOAD_DELTA;
		} else {
			cout << "JUUUUUUUUST RIIIIIIGHT " << load << std::endl;
		}
		getMC()->setOutputCmd(HeadPanOffset, state->outputs[HeadPanOffset] + difference);  

	}
  }


  $setupmachine{
	startnode: ChangeLoad
  }

}

REGISTER_BEHAVIOR(ApplyLoad);