#include "Behaviors/StateMachine.h"

#define TARGET_LOAD 250
#define DELTA_MAX 0.60
#define LOAD_DELTA 0.006

$nodeclass ApplyLoad {
  $provide int runningLoadLeft;
  $provide float runningDifferenceLeft;
  $provide float commandedAngleLeft;

  $provide int runningLoadRight;
  $provide float runningDifferenceRight;
  $provide float commandedAngleRight;

  $nodeclass ChangeLoad : PostureNode  {
	virtual void doStart(){ 
		//Subscribe to sensor EGID events
		$reference ApplyLoad::runningLoadLeft;
		$reference ApplyLoad::commandedAngleLeft;

		$reference ApplyLoad::runningLoadLeft;
		$reference ApplyLoad::commandedAngleLeft;

		runningLoadLeft = 0;
		commandedAngleLeft = state->outputs[LeftFingerOffset];
		erouter->addListener(this, EventBase::sensorEGID);
	}

	virtual void doStop(){		
		motman->addPrunableMotion(SharedObject<PIDMC>(0));
	}
	
	virtual void doEvent(){
		$reference ApplyLoad::runningLoadLeft;
		$reference ApplyLoad::runningDifferenceLeft;
		$reference ApplyLoad::commandedAngleLeft;
		
		unsigned int joint = LeftFingerOffset;
		int load = int(state->pidduties[joint]*1023);
		float distanceToTarget = float(abs(TARGET_LOAD - runningLoadLeft)) / TARGET_LOAD;
		float difference = LOAD_DELTA * distanceToTarget;
		if (difference > DELTA_MAX) difference = DELTA_MAX;
		cout << "INSTANTANEOUS LOAD: " << load << std::endl;
		cout << "LOAD DIFFERENCE " << difference << std::endl;
		if (runningLoadLeft <= TARGET_LOAD) {
			cout << "TOO LOW " << runningLoadLeft << std::endl;
			difference = difference;
		} else if (runningLoadLeft > TARGET_LOAD) {
			cout << "TOO HIGH " << runningLoadLeft << std::endl;
			difference = -difference;
		} else {
			cout << "JUUUUUUUUST RIIIIIIGHT " << runningLoadLeft << std::endl;
		}
		
		cout << "Running difference: " << runningDifferenceLeft << std::endl;
		runningLoadLeft += (load - runningLoadLeft)/4;
		runningDifferenceLeft = (difference * 1.0/4.0) + (runningDifferenceLeft * 3.0/4.0);
		commandedAngleLeft += runningDifferenceLeft;
		getMC()->setOutputCmd(LeftFingerOffset, commandedAngleLeft);
		getMC()->setOutputCmd(RightFingerOffset, -commandedAngleLeft);
	}
  }

  $setupmachine{
	startnode: ChangeLoad
  }

}

REGISTER_BEHAVIOR(ApplyLoad);