#include "Behaviors/StateMachine.h"

$nodeclass Lines {

    $nodeclass Find : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
        mapreq.addObjectColor(ellipseDataType, "red");
        mapreq.addObjectColor(ellipseDataType, "blue");
        mapreq.addObjectColor(ellipseDataType, "green");
        mapreq.addObjectColor(ellipseDataType, "black");
        mapreq.addObjectColor(lineDataType, "red");
    }


    $nodeclass Report : doStart {
        NEW_SHAPEVEC(ellipse_shapes, EllipseData, 
                    select_type<EllipseData>(camShS));

        NEW_SHAPEVEC(line_shapes, LineData, select_type<LineData>(camShS));

        int leftCount = 0;
        int rightCount = 0;
        int lineCount = 0;
        SHAPEVEC_ITERATE(line_shapes, LineData, myline) {
            myline->setInfinite();
            lineCount++;
            SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myellipse) {
                cout << "check\n";
                if (myline->pointIsLeftOf(myellipse->centerPt())) {
                    leftCount++;
                }
                else {
                    rightCount++;
                }
            } END_ITERATE;
            break;
        } END_ITERATE;
        
        if (lineCount == 0) {
            cout << "No lines detected\n";
        }
        else {
            cout << "There are " << leftCount << " ellipses to the left \n";
            cout << "There are " << rightCount << " ellipses to the right \n";
        }
    }
    
    $setupmachine {
        Find =C=> Report =N=> SpeechNode("done")
    }

}

REGISTER_BEHAVIOR(Lines);