#include "Behaviors/StateMachine.h"
#include "Shared/RobotInfo.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Motion/MotionPtr.h"

#define LEAD_RATIO 12.0f
#define MAX_SPEED 0.6f
#define TURN_SPEED 0.15f
#define ARM_ANGLE 1.1f
#define TARGET_POS fmat::pack(430, -73, 494)
#define DISPLACEMENT_THRESHOLD 10
#define INVALID_THRESHOLD 60

$nodeclass HoldingHands : StateNode {

$nodeclass ResetPosition : PostureNode : doStart {

	getMC()->setMaxSpeed(ArmOffset, MAX_SPEED);
	getMC()->setOutputCmd(ArmOffset,0);
	getMC()->setOutputCmd(ArmShoulderOffset,ARM_ANGLE);
	getMC()->setOutputCmd(ArmElbowOffset,0);
	getMC()->setOutputCmd(ArmWristOffset,0);
	getMC()->setOutputCmd(WristRotateOffset,0);
	getMC()->setOutputCmd(LeftFingerOffset,0);
	getMC()->setOutputCmd(RightFingerOffset,0);

}

$nodeclass Lead : WalkNode : doStart {

	// Calculate gripper position
	fmat::Column<3> gP = kine->getPosition(GripperFrameOffset);

	// Remember target position
	fmat::Column<3> dV = (gP - TARGET_POS);
	float d = dV.norm();

	cout << "claw: " << gP << " -- " << TARGET_POS << " difference of: " << d;
	if (d > DISPLACEMENT_THRESHOLD && d < INVALID_THRESHOLD) {
		cout << " MOVING!";
		float x = dV[0] * LEAD_RATIO;
		float a = dV[1] * TURN_SPEED;
		getMC()->setTargetVelocity(x, 0, a);
	}
	cout << endl;
}

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

		$statemachine{
			pos: ResetPosition[setMC(posturemc)] =E(sensorEGID)=> Lead =E(sensorEGID)=> pos
		}
	}
}

REGISTER_BEHAVIOR(HoldingHands);