#include "Behaviors/StateMachine.h"
#include "Motion/IKSolver.h"

$nodeclass GraspRequest : VisualRoutinesStateNode {

	$nodeclass FindCylinders : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		mapreq.addObjectColor(cylinderDataType, "red");
		mapreq.pursueShapes = true;
	}

	$nodeclass MoveArm : DynamicMotionSequenceNode : doStart {
		NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(localShS));
		if (cyls.empty())
			return;
		cout << "NumArmJoints = " << NumArmJoints << endl;
		Point end;
		end = cyls[0]->getCentroid();
		cout << "moving arm" << endl;

		KinematicJoint *gripperFrameKJ = kine->getKinematicJoint(GripperFrameOffset)->cloneBranch();
		KinematicJoint* KJjoints[NumArmJoints];
		for (uint i = 0; i < NumArmJoints; i++)
			KJjoints[i] = NULL;
		gripperFrameKJ->getRoot()->buildChildMap(KJjoints, ArmOffset, NumArmJoints);

		float speed = 0.4f;
		int oriPri = 0;
		int orientationIndex = 1;
		fmat::Quaternion orientation[2];
		orientation[0] = fmat::Quaternion::aboutY(M_PI_2);
		orientation[1] = fmat::Quaternion::aboutY(M_PI);
		fmat::Column<3> target = fmat::packT(end.coordX(), end.coordY(), end.coordZ());

		// calculate difference between current pose and target pose
		fmat::Column<3> from = kine->getPosition(GripperFrameOffset);
		fmat::Column<3> diff = target - from;

		// time to get to the point (ms)
		const int time = 8 * diff.norm() / speed;
		getMC()->advanceTime(time);
		cout << "solving for target: " << target << endl;
			
		bool succ = gripperFrameKJ->getIK().solve(fmat::ZERO3,
					  *gripperFrameKJ,
					  IKSolver::Point(target));
		cout << "success: " << succ << endl;
		
		cout << "NumArmJoints = " << NumArmJoints << endl;
		for (unsigned int joint = 0; joint < NumArmJoints-2; joint++) {
			cout << "   joint = " << joint << "  NumArmJoints=" << NumArmJoints <<
				"  KJjoints[joint]=" << (void*)KJjoints[joint] << endl;
			// angle, mapped from 0 - 1, based on joint limits
			float normJointVal;
				
			// set the new angles
			float q = KJjoints[joint]->getQ();
			cout << "      q = " << q << endl;
				
			getMC()->setOutputCmd(ArmOffset+joint, q);
		}
	}

	

	$setupmachine {
		FindCylinders =C=> MoveArm
	}
}

REGISTER_BEHAVIOR(GraspRequest);