#include "Behaviors/StateMachine.h"

$nodeclass AprilTest : VisualRoutinesStateNode {

  $nodeclass Look : MapBuilderNode($,MapBuilderRequest::worldMap) : constructor {
    cout << "Point the camera at an April Tag before running this demo." << endl;
    cout << "Type \"msg <Enter>\" to take another image and run again." << endl;
    mapreq.setAprilTagFamily();  // Use the default tag family
  }

  $nodeclass Report : SpeechNode : doStart {
    NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(camShS));
    // Put "good" before "April" to prevent MARY from speaking "1" as "first"
    textstream << "I saw " << tags.size() << " good april tag"
	       << ( tags.size() == 1 ? "" : "s") << ".";
    SHAPEVEC_ITERATE(tags, AprilTagData, t) {
      textstream << "  Tag " << t->getTagID() << " distance " << int(t->getCentroid().coordZ()) << " millimeters.\n";
    } END_ITERATE;
  }

  $setupmachine{
    look: Look =MAP=> Report =TM=> look
  }

}

REGISTER_BEHAVIOR_MENU(AprilTest,DEFAULT_TK_MENU"/Vision Demos");