#include "Shared/RobotInfo.h"

#ifdef TGT_CALLIOPE5KP

#include "Behaviors/StateMachine.h"

$nodeclass LinkOrient : VisualRoutinesStateNode {

  $nodeclass FindBigBlob : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
    NEW_SHAPE(gazeTarget, PointData, new PointData(localShS, Point(300,0,0,egocentric)));
    mapreq.searchArea = gazeTarget;
    mapreq.addObjectColor(blobDataType, "red");
    mapreq.addMinBlobArea("red", 400);
  }

  $nodeclass MoveArm : PostureNode : doStart {
    NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
    if ( blobs.size() == 0 ) {
      cout << "\n\n *** Cannot see any large red blobs! ***" << endl;
      return;
    }
    fmat::Column<3> target = blobs[0]->getCentroid().getCoords();
    target[2] = 30;
    // fmat::Quaternion oriTgt = fmat::Quaternion::aboutZ(-M_PI/2);
    fmat::Quaternion oriTgt = fmat::Quaternion::aboutY(-M_PI/2);
    fmat::Column<3> posOffset = fmat::ZERO3;
    fmat::Quaternion oriOffset = fmat::Quaternion::identity();
    cout << "ArmElbow max speed=" << getMC()->getMaxSpeed(ArmElbowOffset) << endl;
    cout << "Target is at " << target << endl;
    bool success = getMC()->solveLink(target, oriTgt, GripperFrameOffset, posOffset, oriOffset);
    cout << "success=" << success << endl;
  }

  $nodeclass ReportPosition : StateNode : doStart {
    fmat::Column<3> pos = kine->linkToBase(GripperFrameOffset).translation();
    cout << "Position = " << pos << endl;
  }

  $setupmachine{
    FindBigBlob =C=> MoveArm =C=> StateNode =T(200)=> // add a little time for the arm to settle
     ReportPosition
  }

}

REGISTER_BEHAVIOR(LinkOrient);

#endif