#include "Behaviors/StateMachine.h"
#include "Motion/IKCalliope.h"
#include "Motion/IKThreeLink.h"

$nodeclass GripperPos : StateNode : doStart {
  cout << "Gripper is at " << kine->getPosition(GripperFrameOffset) << endl;

	KinematicJoint* effector = const_cast<KinematicJoint*>(kine->getKinematicJoint(GripperFrameOffset)); //->cloneBranch();
	IKSolver solver = effector->getIK();
	Point toPt(500,-100,50,egocentric);
	float thetaX = 0, thetaY = 0, thetaZ = 0;
	float midX = M_PI, midY = M_PI, midZ = M_PI;
	fmat::Quaternion ori = fmat::Quaternion::fromMatrix(fmat::rotationZ(midZ + thetaZ) * fmat::rotationY(midY + thetaY) * fmat::rotationX(midX + thetaX));
	IKSolver::Point ikpt(toPt.coords[0], toPt.coords[1], toPt.coords[2]);
	bool reached = 
		solver.solve(fmat::ZERO3, IKSolver::Rotation(), *effector, ikpt, 1, IKSolver::Rotation(ori), 0);
	cout << "reached = " << reached << endl;
  cout << "Gripper is at " << kine->getPosition(GripperFrameOffset) << endl;

	unsigned int numPlannerJoints = NumArmJoints-2;
	KinematicJoint* joint = effector;
	cout << "NumArmJoints=" << NumArmJoints << "   numPlannerJoints=" << numPlannerJoints << endl;
	for(unsigned int j = numPlannerJoints; j > 0; j--) {
		while (joint->isMobile() == false) joint = joint->getParent();
		// endSt[j-1] = joint->getQ();
		cout << "endSt[" << j-1 << "] = " << joint->getQ() << endl;
		joint = joint->getParent();
	}
	cout << endl;

	//	cout << "hasInverseSolution = " << solver.hasInverseSolution
	//			 << "   inverseKnee=" << solver.inverseKnee << endl;

}

REGISTER_BEHAVIOR(GripperPos);