#include "Shared/RobotInfo.h"

#if defined(TGT_HAS_ARMS) && defined(TGT_HAS_HEAD)

#include "Behaviors/StateMachine.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"

$nodeclass GripperToEgg : VisualRoutinesStateNode {
    $provide fmat::Column<3> target;

    $nodeclass LookEgg : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
        mapreq.addObjectColor(ellipseDataType, "red");
    }

    $nodeclass FindEgg : doStart {
        $reference GripperToEgg::target;

        NEW_SHAPEVEC(ellipses, EllipseData,
            select_type<EllipseData>(localShS));

        if (ellipses.size() < 1) {
            postStateFailure();
            return;
        }

        // The point is relative to our base position
        target = fmat::Column<3>((ellipses[0]->getCentroid()).getCoords());
        postStateSuccess();
    }

  $nodeclass ArmMover : PostureNode : doStart {
    $reference GripperToEgg::target;
	fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
#ifdef TGT_IS_CALLIOPE5
		unsigned int link = LeftFingerFrameOffset;
#else
		unsigned int link = GripperFrameOffset;
#endif
    getMC()->solveLinkPosition(target, link, noOffset);
  }

  virtual void setup() {
		// We make the posture motion command here and supply it to the
		// PostureNode via setMC() so that it only has to be registered
		// with the MotionManager once. Registering and deregistering a
		// PostureMC inside a tight sensor loop is inefficient and leads
		// to crashes.
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

    $statemachine{
  		startnode: LookEgg =MAP=> findegg
        findegg: FindEgg
        findegg =S=> movearm
        findegg =F=> SpeechNode("No eggs found") =C=> startnode
        movearm: ArmMover[setMC(posturemc)] =E(sensorEGID)=> startnode
    }
  }

}

REGISTER_BEHAVIOR(GripperToEgg);

#endif