#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"
#include <cmath>

using namespace std;

$nodeclass HoldingHands : StateNode {

  $nodeclass ArmPos : PostureNode : doStart {
	float postures[NumArmJoints] = {-0.12, 1.2, -2.1, 1.17, -1.55};

	for (uint i = ArmBaseOffset; i < ArmBaseOffset + NumArmJoints - 2; i++)
		getMC()->setOutputCmd(i, postures[i - ArmBaseOffset]);
  }

  $nodeclass UpdateWalk : WalkNode {
	
	virtual void doStart() {
		erouter->addListener(this, EventBase::sensorEGID);
	}

	virtual void doEvent() {

		float x = 502.5;
		float y = -118.0;

		if (event->getGeneratorID() != EventBase::sensorEGID)
			return;

		float velocity_factor = 20.5;
		fmat::Column<3> current_pos = kine->linkToBase(GripperFrameOffset).translation();
		cout << "cur pos: (" << current_pos[0] << ", " << current_pos[1]
				<< ", " << current_pos[2] << ")" << endl;

		float diff[2] = {current_pos[0] - x, current_pos[1] - y};
		float magnitude = sqrt(pow(diff[0], 2) + pow(diff[1], 2));
		cout << "offset by: " << magnitude << endl;

		if (fabs(diff[0]) > 1 || fabs(diff[1] > 3)) {
			setVelocity(diff[0] * velocity_factor, 0, deg2rad(1.5) * diff[1]);
		}
		else {
			setVelocity(0, 0, 0);
		}
		
	}
  }

  virtual void setup() {
		// We make the posture motion command here and supply it to the
		// PostureNode via setMC() so that it only has to be registered
		// with the MotionManager once. Registering and deregistering a
		// PostureMC inside a tight sensor loop is inefficient and leads
		// to crashes.
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

    $statemachine{
		arm : ArmPos[setMC(posturemc)]
		walk : UpdateWalk
  		startnode: StateNode =N=> {arm, walk}
    }
  }

}

REGISTER_BEHAVIOR(HoldingHands);