#include "Behaviors/StateMachine.h"

$nodeclass GrabStuff2 : 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 GrabStuff2::ellipseCntr;
ellipseCntr = ellipse->getCentroid().getCoords();
 postStateSuccess();
} else {
postStateFailure();
 }
}

$nodeclass Rotate : DynamicMotionSequenceNode : doStart{
  fmat::Transform gripperStart = kine->linkToBase(GripperFrameOffset);
  cout << "actually used: " << gripperStart << endl;
  fmat::Transform gripperStart2 = kine->linkToBase(WristRotateOffset);
  cout << "just randomly output: " << gripperStart2 << endl;

  fmat::Column<3> gripperStartPos = gripperStart.translation();
  fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
  PostureEngine movearm;
  fmat::Quaternion oriTgt = fmat::Quaternion::fromMatrix(gripperStart2.rotation());
  fmat::Quaternion oriOffset = fmat::Quaternion::identity();
//  bool fromSolveLink = movearm.solveLink(gripperStartPos, oriTgt, GripperFrameOffset, noOffset, oriOffset);
//	bool fromSolveLink = movearm.solveLink(gripperStartPos, oriTgt, GripperFrameOffset, fmat::ZERO3, fmat::Quaternion::IDENTITY);
	bool fromSolveLink = movearm.solveLink(gripperStartPos, fmat::Quaternion::aboutX(-M_PI/2), GripperFrameOffset, fmat::ZERO3, fmat::Quaternion::IDENTITY);
	cout << fromSolveLink << endl;
  getMC()->advanceTime(1000);
  getMC()->setPose(movearm);
 }


    $setupmachine {
       startnode: Look =C=> FindEllipse =C=> ReportEllipse =S=> DynamicMotionSequenceNode("grabpos.mot") =T(10000)=> Rotate
    }
}

REGISTER_BEHAVIOR(GrabStuff2);