#include "Shared/RobotInfo.h"

#if defined(TGT_HAS_ARMS)

#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"

$nodeclass HandGuide : StateNode {
	$provide fmat::Column<3> target;

	/** Slowly move the robot's arm to an extended pose and hold it there. **/
	$nodeclass InitialPose : PostureNode : doStart {
		// Assign an output to each of the joints
		for (unsigned int i = 0; i < NumArmJoints; i++) {
			getMC()->setMaxSpeed(ArmBaseOffset + i, 0.3);
		  getMC()->setOutputCmd(ArmBaseOffset + i, 0);
		}		
		getMC()->setOutputCmd(ArmElbowOffset, 1.75);
		getMC()->setOutputCmd(ArmWristOffset, -1.1);	
		// Hold the position
		getMC()->setHold(true);
	}

	/** This node also computes the translation matrix which describes how
			to move from the robot base to the wrist, and stores it in the parent
			variable target **/
	$nodeclass ComputePose : StateNode : doStart {
		// Compute the translation matrix from the base to the arm and store
		// it in the parent variable
		$reference HandGuide::target;
		target = kine->linkToBase(WristRotateOffset).translation();

		cout << "The arm is at position: " << target << endl;
	}

	/** Whenever the arm is moved, compute how far it has moved and move the
			robot in a compliant direction **/
	$nodeclass WalkWithGrip : WalkNode {
		/** Subscribe to sensorEGID events and set the velocity to 0 **/		
		virtual void doStart() {
			erouter->addListener(this, EventBase::sensorEGID);
			setVelocity(0, 0, 0);
		}
		
		/** Measure the new position of the arm. If the arm's displacement is
				above a certain threshold, move the robot to reduce the
				displacement **/
		virtual void doEvent() {
			// Get the old and new positions and compute the difference			
			$reference HandGuide::target;
			fmat::Column<3> newPos = kine->linkToBase(WristRotateOffset).translation();
			fmat::Column<3> diff = (newPos - target);
			// The difference is biased towards reporting the x difference at around
			// -3 when the arm is at rest. We compensate for this by adding three to
			// the x difference.
			float x = diff[0] + 3;
			// Also, it is harder to pull the arm forwards than to push it backwards.
			// To make it easier, we double the detected displacement if it is
			// positive (if the arm is being pulled forwards).
			if (x > 0) {
				x *= 2;
			}
			float y = diff[1];

			cout << "Diff: (" << x << "," << y << ")" << endl;

			float xThreshold = 3.5;
			float angThreshold = 5.0;
			// Compute the new velocity of the robot. If the arm is pushed or pulled,
			// to a point above the threshold, then the robot will either move
			// forwards or backwards compliantly. The velocity is set in the
			// direction to correct the displacement. The robot's velocity is
			// proportional to the displacement of the arm. The same logic applies to
			// when the arm is pulled left or right, except that the robot turns.
			float xVel = 0.0;
			float angVel = 0.0;
			if (fabs(x) > xThreshold) {
				// As the arm's position approaches the threshold, the velocity of the
				// robot should get arbitrarily close to zero. This prevents a jerky
				// movement.
				if (x >= 0) {	
					xVel = 100.0 * (x - xThreshold);
				} else {
					xVel = 100.0 * (x + xThreshold);
				}
			}
			if (fabs(y) > angThreshold) {
				if (y >= 0) {
					angVel = 0.1 * (y - angThreshold);
				} else {
					angVel = 0.1 * (y + angThreshold);
				}
			}
			setVelocity(xVel, 0, angVel);
		}
	}

  virtual void setup() {
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

    $statemachine{
			startnode : InitialPose[setMC(posturemc)] =C=>
					ComputePose =N=> WalkWithGrip
    }
  }

}

REGISTER_BEHAVIOR(HandGuide);

#endif