#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 << endl;
  fmat::Column<3> finalPos = ellipseCntr;
  finalPos[2] = 110;
  fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
  PostureEngine movearm;
  fmat::Quaternion oriOffset = fmat::Quaternion::identity();
  cout << "target: " << finalPos << endl;
  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);
}

$nodeclass DownToFood : DynamicMotionSequenceNode : doStart{
  $reference MyGrabStuff::ellipseCntr;
  fmat::Transform gripperStart = kine->linkToBase(GripperFrameOffset);
  fmat::Column<3> gripperStartPos = gripperStart.translation();
  fmat::Column<3> finalPos = ellipseCntr;
  finalPos[2] = 60;
  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 << "target: " << finalPos << endl;
  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);
}


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

    $setupmachine {
       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);