// This code finds dominoes that lie about 600 mm in front of the robot.
#include "Behaviors/StateMachine.h"

$nodeclass FindDominoes : VisualRoutinesStateNode {

  $nodeclass BuildMap : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    std::vector<Point> gazePoints;
    gazePoints.push_back(Point(600, -300, 0, egocentric));
    gazePoints.push_back(Point(600,  300, 0, egocentric));
    NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, gazePoints, false));

    mapreq.searchArea = gazePoly;
    mapreq.addObjectColor(dominoDataType,"red");
    mapreq.addSecondColor(dominoDataType,"blue");
  }

  $nodeclass ExamineMap : SpeechNode : doStart {
    NEW_SHAPEVEC(dominoes, DominoData, select_type<DominoData>(localShS));
    if (dominoes.size() == 0)
      textstream << "No dominoes found.\n";
    SHAPEVEC_ITERATE(dominoes, DominoData, dom) {
      textstream << "I see a domino with " << dom->getLowValue() << " and "
                 << dom->getHighValue() << " dots.\n";
    } END_ITERATE;
  }

  $setupmachine{
    BuildMap =MAP=> ExamineMap
  }
}

REGISTER_BEHAVIOR(FindDominoes);