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

$nodeclass GradientTest : StateNode {

  $nodeclass Mover : PostureNode : doStart {
    typedef ShapeSpacePlanner3DR<NumArmJoints-2>::NodeValue_t NodeValue_t;
    int numPlannerJoints = NumArmJoints - 2;  // subtract the fingers
    IKSolver::Point pTgt(433.041, 27.63, 52.7496);
    cout << "Target is " << pTgt << endl;
    IKGradientSolver solver(750, 0.5f, 0.001f, 0.2/5000);
    KinematicJoint *gripper = kine->getKinematicJoint(GripperFrameOffset)->cloneBranch();
    bool reached =
      solver.solve(IKSolver::Point(fmat::ZERO3), IKSolver::Rotation(fmat::Quaternion::aboutX(M_PI/2)), *gripper,
		   pTgt, 1, IKSolver::Parallel(0,0,1), 1);
    cout << "reached = " << reached << endl;
    NodeValue_t endSt;
    KinematicJoint *joint = gripper;
    for (int j = numPlannerJoints-1; j >= 0; j--) {
      while (joint->isMobile() == false)
	joint = joint->getParent();
      endSt[j] = joint->getQ();
      joint = joint->getParent();
    }
    for (int j=0; j<numPlannerJoints; j++) {
      cout << "Joint " << j << ": " << endSt[j] << endl;
      getMC()->setMaxSpeed(ArmOffset+j, 0.5);
      getMC()->setOutputCmd(ArmOffset+j, float(endSt[j]));
    }
  delete gripper;
  }

  $nodeclass Reporter : StateNode : doStart {
    cout << "Gripper is now at " << kine->getPosition(GripperFrameOffset) << endl;
    //cout << "Gripper transform is " << kine->getKinematicJoint(GripperFrameOffset)->getFullT() << endl;
    cout << "Gripper quaternion is " << kine->getKinematicJoint(GripperFrameOffset)->getWorldQuaternion() << endl;
  }

  $setupmachine{
    Mover =C=> Reporter
  }
}

REGISTER_BEHAVIOR(GradientTest);