/**
 * @author Matt Dee (medee)
 * @author Alex Franchuk (afranchu)
 */
#include "Behaviors/StateMachine.h"

#define ELLIPSE_COLOR "red"
#define LINE_COLOR "blue"

$nodeclass Lines {

    $nodeclass GetShape : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
        mapreq.addObjectColor(ellipseDataType, ELLIPSE_COLOR);
        mapreq.addObjectColor(lineDataType, LINE_COLOR);
    }

    $nodeclass CountShapes : doStart {
        NEW_SHAPEVEC(ellipses, EllipseData,
            select_type<EllipseData>(camShS));
        NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));

        if (lines.size() < 1) {
            cout << "No " LINE_COLOR " lines visible." << endl;
            return;
        }

        lines[0]->setInfinite();

        unsigned left = 0;
        for (unsigned int i = 0; i < ellipses.size(); i++) {
            if (lines[0]->pointIsLeftOf(ellipses[i]->getCentroid())) {
                left++;
            }
        }
        cout << "Number left: " << left << ", number right: " << ellipses.size()-left << endl;
    }

    $setupmachine {
        GetShape =C=> CountShapes =N=> SpeechNode("Done!")
    }

}

REGISTER_BEHAVIOR(Lines);