#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 Rotate : 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> diff = ellipseCntr;
	diff[2] = 70;
  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(diff, fmat::Quaternion::aboutY(-M_PI/2),
 GripperFrameOffset,
noOffset, oriOffset);
	cout << "solved link: " << fromSolveLink << endl;
  getMC()->advanceTime(1000);
  getMC()->setPose(movearm);
  cout << "gripper: " << gripperStartPos << endl;
//  }

  //for (int i = -280; i < -110; i++) {
// float theta = deg2rad((float) 10);
// fmat::Column<3> target = ellipseCntr;
// fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(theta);
// fmat::Column<3> posOffset = fmat::pack(-100,0,0);
// fmat::Quaternion oriOffset = fmat::Quaternion::identity();
// movearm.solveLink(target, oriTgt, GripperFrameOffset, posOffset,oriOffset);
// getMC()->advanceTime(1000);
// getMC()->setPose(movearm);
  //}
    }

$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=> Rotate =C=> EndPosition =C=> DynamicMotionSequenceNode("grip_close_crackers.mot") =C=> DynamicMotionSequenceNode("relaxarm.mot")

    }
}

REGISTER_BEHAVIOR(MyGrabStuff);