#include "Behaviors/StateMachine.h"

$nodeclass Dominoes {
    // This code fragment finds dominoes that lie about 600 mm in front of the robot.

    $nodeclass FindDominoes : 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 DescribeDominoes : SpeechNode : doStart {
        NEW_SHAPEVEC(dominoes, DominoData, select_type<DominoData>(worldShS));

        if(dominoes.empty()) {
            cout << "I don't see any dominoes" << endl;
            return;
        }

        SHAPEVEC_ITERATE(dominoes, DominoData, domino) {
            cout << "I see a domino with " << domino->getLowValue() <<
                " and " << domino->getHighValue() << " dots." << endl;
        } END_ITERATE;
    }

    $setupmachine {
        FindDominoes =C=> DescribeDominoes
    }
}

REGISTER_BEHAVIOR(Dominoes);