#include "Shared/RobotInfo.h"
#ifdef TGT_HAS_CAMERA

#include "Behaviors/StateMachine.h"

$nodeclass VirtualReality : VisualRoutinesStateNode {

    $nodeclass BuildMap : MapBuilderNode($,MapBuilderRequest::worldMap) : doStart {
        mapreq.addAllObjectColors(lineDataType);
    }
	
    $nodeclass ExamineMap : SpeechNode : doStart {
        NEW_SHAPEVEC(lines, LineData, select_type<LineData>(worldShS));
        int const numLines = lines.size();
        if ( numLines == 0 )
            textstream << "I did not see any lines. Try again.";
        else
            textstream << "I saw " << numLines
                       << " line" << ( numLines != 1 ? "s" : "")
                       << ". Look in raw cam.";
        SHAPEVEC_ITERATE(lines, LineData, e) {
            rgb ecol = e->getColor();
            e->setColor(rgb(~ecol.red, ~ecol.green, ~ecol.blue));
            VRmixin::drawShapes.push_back(e);
            NEW_SHAPE(ellipse1, EllipseData, new EllipseData(worldShS, e->leftPt(), 10, 10, 0));
            NEW_SHAPE(ellipse2, EllipseData, new EllipseData(worldShS, e->rightPt(), 10, 10, 0));
            VRmixin::drawShapes.push_back(ellipse1);
            VRmixin::drawShapes.push_back(ellipse2);
        } END_ITERATE;
    }

    $setupmachine{
        BuildMap =MAP=> ExamineMap
    }
}

REGISTER_BEHAVIOR(VirtualReality);

#endif