#include "Behaviors/StateMachine.h"

$nodeclass CamCoordTest {
    $nodeclass FindTags : MapBuilderNode(MapBuilderRequest::cameraMap) : doStart {
        mapreq.setAprilTagFamily();
    }

    $nodeclass EchoCentroids : doStart {
        cout << camSkS.getWidth() << ", " << camSkS.getHeight() << endl;
        NEW_SHAPEVEC(april_tags, AprilTagData, select_type<AprilTagData>(camShS));
        SHAPEVEC_ITERATE(april_tags, AprilTagData, tag) {
            cout << "Saw tag at " << tag->getCentroid() << endl;
        } END_ITERATE;
    }

    $setupmachine {
        FindTags =C=> EchoCentroids
    }
}

REGISTER_BEHAVIOR(CamCoordTest);