#include "Behaviors/StateMachine.h"

$nodeclass MyGrabStuff : VisualRoutinesStateNode {

 $provide fmat::Column<3> ellipseCntr;

$nodeclass Look() : HeadPointerNode : doStart{
 getMC()->lookAtPoint(200, 0, 0);
}


$nodeclass FindEllipse() : MapBuilderNode(MapBuilderRequest::localMap) :
doStart{
 mapreq.addObjectColor(blobDataType, "blue");
 mapreq.addObjectColor(blobDataType, "red");
}

$nodeclass ReportEllipse() : VisualRoutinesStateNode : doStart{
        NEW_SHAPEVEC(blobs,BlobData,select_type<BlobData>(localShS));
NEW_SHAPE(ellipse, BlobData, max_element(blobs, BlobData::AreaLessThan()));
 if (ellipse.isValid()) {
$reference MyGrabStuff::ellipseCntr;
ellipseCntr = ellipse->getCentroid().getCoords();
 postStateSuccess();
} else {
postStateFailure();
 }
}

$nodeclass PutAboveFood : DynamicMotionSequenceNode : doStart{
  $reference MyGrabStuff::ellipseCntr;
          cout << "ellipse " << ellipseCntr << endl;
  fmat::Transform gripperStart =
  kine->linkToBase(GripperFrameOffset);
  fmat::Column<3> gripperStartPos = gripperStart.translation();
//          cout << "initial conditions: " << gripperStartPos << ":"
//                   << ( ellipseCntr[0] - gripperStartPos[0] ) << endl;
  fmat::Column<3> finalPos = ellipseCntr;
	finalPos[2] = 160;
//  fmat::Column<3> offset = fmat::pack(diff[0] / 20, 0, 0);

//  cout << "diff: " << diff << endl;
  fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
  PostureEngine movearm;

          fmat::Quaternion oriTgt =
fmat::Quaternion::fromMatrix(gripperStart);
          fmat::Quaternion oriOffset = fmat::Quaternion::identity();

          cout << "offset: " << oriTgt << endl;

//          for (int i = 0; i < 20; i++) {
//  gripperStartPos = gripperStartPos + offset;
//	fmat::Quaternion aboutY = fmat::Quaternion::aboutY(-M_PI/2);
	bool fromSolveLink = movearm.solveLink(finalPos, fmat::Quaternion::aboutY(-M_PI/2),
 GripperFrameOffset,
noOffset, oriOffset);
	cout << "solved link: " << fromSolveLink << endl;
  getMC()->advanceTime(1000);
  getMC()->setPose(movearm);
  cout << "gripper: " << gripperStartPos << endl;
//  }
}

$nodeclass DownToFood : DynamicMotionSequenceNode : doStart{
  $reference MyGrabStuff::ellipseCntr;
          cout << "ellipse " << ellipseCntr << endl;
  fmat::Transform gripperStart =
  kine->linkToBase(GripperFrameOffset);
  fmat::Column<3> gripperStartPos = gripperStart.translation();
          cout << "initial conditions: " << gripperStartPos << ":"
                   << ( ellipseCntr[0] - gripperStartPos[0] ) << endl;
  fmat::Column<3> finalPos = ellipseCntr;
	finalPos[2] = 60;
//  fmat::Column<3> offset = fmat::pack(diff[0] / 20, 0, 0);

//  cout << "diff: " << diff << endl;
  fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
  PostureEngine movearm;

          fmat::Quaternion oriTgt =
fmat::Quaternion::fromMatrix(gripperStart);
          fmat::Quaternion oriOffset = fmat::Quaternion::identity();

          cout << "offset: " << oriTgt << endl;

//          for (int i = 0; i < 20; i++) {
//  gripperStartPos = gripperStartPos + offset;
//	fmat::Quaternion aboutY = fmat::Quaternion::aboutY(-M_PI/2);
	bool fromSolveLink = movearm.solveLink(finalPos, fmat::Quaternion::aboutY(-M_PI/2),
 GripperFrameOffset,
noOffset, oriOffset);
	cout << "solved link: " << fromSolveLink << endl;
  getMC()->advanceTime(1000);
  getMC()->setPose(movearm);
  cout << "gripper: " << gripperStartPos << endl;
//  }
}


$nodeclass EndPosition : DynamicMotionSequenceNode : doStart{
  fmat::Transform gripperEnd = kine->linkToBase(GripperFrameOffset);
  fmat::Column<3> gripperEndPos = gripperEnd.translation();
          cout << "gripper above object: " << gripperEndPos << endl;
	postStateCompletion();
}

    $setupmachine {
//       startnode: Look =C=> FindEllipse =C=> ReportEllipse =S=> DynamicMotionSequenceNode("grabpos.mot") =T(3000)=> Rotate =T(3000)=> EndPosition =C=> DynamicMotionSequenceNode("grip_close_crackers.mot") =T(3000)=> DynamicMotionSequenceNode("relaxarm.mot")
       startnode: Look =C=> FindEllipse =C=> ReportEllipse =S=> DynamicMotionSequenceNode("grabpos.mot") =C=> PutAboveFood =C=> DownToFood =C=> EndPosition =C=> DynamicMotionSequenceNode("grip_close_crackers.mot") =C=> DynamicMotionSequenceNode("relaxarm.mot")

    }
}

REGISTER_BEHAVIOR(MyGrabStuff);