//-*-c++-*-

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"

#define NUM_SQUARES 9

using namespace DualCoding;

class BuildMap : public VisualRoutinesStateNode {
public:
    BuildMap() : VisualRoutinesStateNode("BuildMap") {}
  
    virtual void DoStart() {
    
        // set up map builder request
        const int pink_index = ProjectInterface::getColorIndex("pink");
        const int blue_index = ProjectInterface::getColorIndex("blue");

        MapBuilderRequest req(MapBuilderRequest::cameraMap);
        
        req.objectColors[ellipseDataType].insert(blue_index);
        req.objectColors[ellipseDataType].insert(pink_index);

        // execute request
        unsigned int mapreq_id = mapBuilder.executeRequest(req);
        erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::statusETID);
    }

    virtual void processEvent(const EventBase &) {
        postStateCompletion();
    }

};

class ParseCircle : public VisualRoutinesStateNode {
public:
    ParseCircle() : VisualRoutinesStateNode("ParseCircle") {}

    virtual void DoStart() {
        
        NEW_SHAPEVEC(ellipses,EllipseData,select_type<EllipseData>(camShS));
        NEW_SHAPEVEC(blue_ellipses,EllipseData,subset(ellipses,IsColor("blue")));
        NEW_SHAPEVEC(pink_ellipses,EllipseData,subset(ellipses,IsColor("pink")));

        NEW_SKETCH(blue_sketch, bool, visops::zeros(camSkS));

        SHAPEVEC_ITERATE(blue_ellipses, EllipseData, e)
            blue_sketch |= e->getRendering();
        END_ITERATE

        NEW_SHAPE(hull,PolygonData,PolygonData::convexHull(blue_sketch));
        NEW_SKETCH(hull_sketch, bool, hull->getRendering());
        hull_sketch = visops::fillInterior(hull_sketch);
        NEW_SKETCH(result, bool, visops::zeros(camSkS));

        SHAPEVEC_ITERATE(pink_ellipses, EllipseData, e)

            NEW_SKETCH(bottom, bool, visops::zeros(camSkS));
            NEW_SKETCH(current_shell, bool, e->getRendering());
            bottom += current_shell & ! current_shell[*camSkS.idxS];
            
            if(!((bottom & hull_sketch)->empty()))
                result |= current_shell;

        END_ITERATE
    }
};

class CircleBehavior : public VisualRoutinesStateNode {
public:
    CircleBehavior(): VisualRoutinesStateNode("CircleBehavior") {}
  
	virtual void setup() {
        #statemachine
        startnode: BuildMap() =C=> ParseCircle()
        #endstatemachine
    }
};