//-*-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 yellow_index = ProjectInterface::getColorIndex("yellow");
        const int blue_index = ProjectInterface::getColorIndex("blue");

        MapBuilderRequest req(MapBuilderRequest::cameraMap);
        
        req.occluderColors[lineDataType].insert(blue_index);
        req.occluderColors[lineDataType].insert(pink_index);
        req.objectColors[lineDataType].insert(yellow_index);

        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 ParseBoard : public VisualRoutinesStateNode {
public:
	ParseBoard() : VisualRoutinesStateNode("ParseBoard") {}

	virtual void DoStart() {
        
        const int pink_index = ProjectInterface::getColorIndex("pink");
        const int blue_index = ProjectInterface::getColorIndex("blue");

        NEW_SHAPEVEC(lines,LineData,select_type<LineData>(camShS));
        NEW_SHAPEVEC(ellipses,EllipseData,select_type<EllipseData>(camShS));

        lines = stable_sort(lines,not2(LineData::LengthLessThan()));

        Shape<LineData> top_hor;
        Shape<LineData> bot_hor;
        Shape<LineData> left_vert;
        Shape<LineData> right_vert;

        // find boundary lines
        SHAPEVEC_ITERATE(lines, LineData, ln1)
            SHAPENEXT_ITERATE(lines, LineData, ln1, ln2)
                if(LineData::IsHorizontal()(ln1) && LineData::IsHorizontal()(ln2)) {
                    if(IsAbove()(ln1,ln2)) {
                        top_hor = ln1->copy();
                        bot_hor = ln2->copy();
                    }
                    else {
                        top_hor = ln2->copy();
                        bot_hor = ln1->copy();
                    }
                }
                else if(LineData::IsVertical()(ln1) && LineData::IsVertical()(ln2)){
                    if(IsLeftOf()(ln1,ln2)) {
                        left_vert = ln1->copy();
                        right_vert = ln2->copy();
                    }
                    else {
                        left_vert = ln2->copy();
                        right_vert = ln1->copy();
                    }
                }
            END_ITERATE;
        END_ITERATE;
				
        // draw boundary lines
        NEW_SHAPE(left_vert_bound, LineData, new LineData(camShS,leftMost(top_hor->leftPt(),bot_hor->leftPt()), left_vert->getOrientation()));
        NEW_SHAPE(right_vert_bound, LineData, new LineData(camShS,rightMost(top_hor->rightPt(),bot_hor->rightPt()), right_vert->getOrientation()));
        NEW_SHAPE(top_hor_bound, LineData, new LineData(camShS,topMost(left_vert->topPt(),right_vert->topPt()), top_hor->getOrientation()));
        NEW_SHAPE(bot_hor_bound, LineData, new LineData(camShS,bottomMost(left_vert->bottomPt(),right_vert->bottomPt()), bot_hor->getOrientation()));

        left_vert_bound->firstPt().setActive(false);
        left_vert_bound->secondPt().setActive(false);
        right_vert_bound->firstPt().setActive(false);
        right_vert_bound->secondPt().setActive(false);
        top_hor_bound->firstPt().setActive(false);
        top_hor_bound->secondPt().setActive(false);
        bot_hor_bound->firstPt().setActive(false);
        bot_hor_bound->secondPt().setActive(false);

        // draw squares	
        NEW_SKETCH(top_left_sq, bool, visops::leftHalfPlane(left_vert)&visops::rightHalfPlane(left_vert_bound)&visops::topHalfPlane(top_hor)&visops::bottomHalfPlane(top_hor_bound));
        NEW_SKETCH(top_mid_sq, bool, visops::leftHalfPlane(right_vert)&visops::rightHalfPlane(left_vert)&visops::topHalfPlane(top_hor)&visops::bottomHalfPlane(top_hor_bound));
        NEW_SKETCH(top_right_sq, bool, visops::leftHalfPlane(right_vert_bound)&visops::rightHalfPlane(right_vert)&visops::topHalfPlane(top_hor)&visops::bottomHalfPlane(top_hor_bound));
        NEW_SKETCH(mid_left_sq, bool, visops::leftHalfPlane(left_vert)&visops::rightHalfPlane(left_vert_bound)&visops::topHalfPlane(bot_hor)&visops::bottomHalfPlane(top_hor));
        NEW_SKETCH(mid_mid_sq, bool, visops::leftHalfPlane(right_vert)&visops::rightHalfPlane(left_vert)&visops::topHalfPlane(bot_hor)&visops::bottomHalfPlane(top_hor));
        NEW_SKETCH(mid_right_sq, bool, visops::leftHalfPlane(right_vert_bound)&visops::rightHalfPlane(right_vert)&visops::topHalfPlane(bot_hor)&visops::bottomHalfPlane(top_hor));
        NEW_SKETCH(bot_left_sq, bool, visops::leftHalfPlane(left_vert)&visops::rightHalfPlane(left_vert_bound)&visops::topHalfPlane(bot_hor_bound)&visops::bottomHalfPlane(bot_hor));
        NEW_SKETCH(bot_mid_sq, bool, visops::leftHalfPlane(right_vert)&visops::rightHalfPlane(left_vert)&visops::topHalfPlane(bot_hor_bound)&visops::bottomHalfPlane(bot_hor));
        NEW_SKETCH(bot_right_sq, bool, visops::leftHalfPlane(right_vert_bound)&visops::rightHalfPlane(right_vert)&visops::topHalfPlane(bot_hor_bound)&visops::bottomHalfPlane(bot_hor));

        // find ellipse bottoms
        NEW_SKETCH(camFrame, uchar, sketchFromSeg());   // may not need this
        NEW_SKETCH(blue_bottoms, bool, visops::zeros(camFrame));
        NEW_SKETCH(pink_bottoms, bool, visops::zeros(camFrame));

        SHAPEVEC_ITERATE(ellipses, EllipseData, e)
            NEW_SKETCH(current_shell, bool, e->getRendering());
            if(IsColor("blue")(e)) {
                blue_bottoms += current_shell & ! current_shell[*camSkS.idxS];
            }
            else {
                pink_bottoms += current_shell & ! current_shell[*camSkS.idxS];
            }
        END_ITERATE;

        // draw the resulting board

        // draw grid lines
        NEW_SKETCH(result, uchar, top_hor_bound->getRendering());
        result +=  bot_hor_bound->getRendering();
        result +=  right_vert_bound->getRendering();
        result +=  left_vert_bound->getRendering();
        SHAPEVEC_ITERATE(lines, LineData, ln)
            result += ln->getRendering();
        END_ITERATE;

        // fill squares
        Sketch<bool> squares[] = {top_left_sq, top_mid_sq, top_right_sq, mid_left_sq, mid_mid_sq, mid_right_sq, bot_left_sq, bot_mid_sq, bot_right_sq};
        for(int i = 0; i < NUM_SQUARES; i++) {
            if(!(squares[i] & blue_bottoms)->empty())  result += squares[i] * blue_index;
                else if(!(squares[i] & pink_bottoms)->empty())  result += squares[i] * pink_index;
        }

    }
};

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