#ifndef __PARSE_TIC_TAC_H
#define __PARSE_TIC_TAC_H

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

class BuildBoard : public VisualRoutinesStateNode {
public:
    BuildBoard() : VisualRoutinesStateNode("BuildBoard") {}
  
    virtual void DoStart() {
    
        cout << "Begin building board\n";
        //**************** 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.maxDist = 1500;
        req.pursueShapes = false;

        req.objectColors[lineDataType].insert(yellow_index);
        req.objectColors[ellipseDataType].insert(blue_index);
        req.objectColors[ellipseDataType].insert(pink_index);
        
        req.occluderColors[ellipseDataType].insert(blue_index);
        req.occluderColors[ellipseDataType].insert(pink_index);
        
        req.immediateRequest = true;

        mapBuilder.executeRequest(req);        

        postStateCompletion();
    }
};

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

	virtual void DoStart() {
        cout << "Begin parsing board\n";
        NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
        //get all horizontal and vertical lines.
        NEW_SHAPEVEC(horizontalLines, LineData, 
            subset(lines, LineData::IsHorizontal()));
        horizontalLines = 
            stable_sort(horizontalLines, not2(LineData::LengthLessThan()));

        NEW_SHAPEVEC(verticalLines, LineData, 
            subset(lines, LineData::IsVertical()));
        verticalLines = 
            stable_sort(verticalLines, not2(LineData::LengthLessThan()));

        //get the shape vec for the 2 longest horizontal / vertical lines.
        LineData *h1 = NULL;
        LineData *h2 = NULL;
        LineData *v1 = NULL;
        LineData *v2 = NULL;

        int ctr = 0;
        SHAPEVEC_ITERATE(horizontalLines, LineData, ln)
            if (ctr == 0)
                h1 = new LineData(ln);
            if (ctr == 1)
                h2 = new LineData(ln);                
            ctr++;
        END_ITERATE;
        ctr = 0;

        SHAPEVEC_ITERATE(verticalLines, LineData, ln)
            if (ctr == 0)
                v1 = new LineData(ln);
            if (ctr == 1)
                v2 = new LineData(ln);                
            ctr++;
        END_ITERATE;
        
        NEW_SHAPE(leftPt1, PointData, h1->leftPtShape());
        NEW_SHAPE(rightPt1, PointData, h1->rightPtShape());

        NEW_SHAPE(topPt1, PointData, v1->topPtShape());
        NEW_SHAPE(bottomPt1, PointData, v1->bottomPtShape());

        NEW_SHAPE(leftPt2, PointData, h2->leftPtShape());
        NEW_SHAPE(rightPt2, PointData, h2->rightPtShape());

        NEW_SHAPE(topPt2, PointData, v2->topPtShape());
        NEW_SHAPE(bottomPt2, PointData, v2->bottomPtShape());
   
        //get the end points
        leftPt1 = leftPt1->getCentroid().coordX() < 
            leftPt2->getCentroid().coordX() ? leftPt1 : leftPt2;
        rightPt1 = rightPt1->getCentroid().coordX() > 
            rightPt2->getCentroid().coordX() ? rightPt1 : rightPt2;
        topPt1 = topPt1->getCentroid().coordY() < 
            topPt2->getCentroid().coordY() ? topPt1 : topPt2;
        bottomPt1 = bottomPt1->getCentroid().coordY() > 
            bottomPt2->getCentroid().coordY() ? bottomPt1 : bottomPt2;
        
        //get the lines of board
        LineData *top = h1;
        LineData *bottom = h2;
        LineData *left = v1;
        LineData *right = v2;

        if (h1->leftPt().coordY() > h2->leftPt().coordY()) {
            top = h2;
            bottom = h1;
        }
        
        if (v1->topPt().coordX() > v2->topPt().coordX()) {
            left = v2;
            right = v1;
        }

        NEW_SHAPE(bottomBound, LineData, 
            new LineData(camShS, bottomPt1, bottom->getOrientation()));
        NEW_SHAPE(topBound, LineData, 
            new LineData(camShS, topPt1, top->getOrientation()));
        
        NEW_SHAPE(leftBound, LineData, 
            new LineData(camShS, leftPt1, left->getOrientation()));
        NEW_SHAPE(rightBound, LineData, 
            new LineData(camShS, rightPt1, right->getOrientation()));

        ///////////////
        // 0 | 1 | 2 //
        // --|---|-- //
        // 3 | 4 | 5 //
        // --|---|-- //
        // 6 | 7 | 8 //
        ///////////////
        NEW_SKETCH(wholeBoard, bool, visops::bottomHalfPlane(topBound) &
                                     visops::topHalfPlane(bottomBound) &
                                     visops::leftHalfPlane(rightBound) &
                                     visops::rightHalfPlane(leftBound));
        //construct all the 9 regions
        NEW_SKETCH(board0, bool, wholeBoard & 
                                 visops::leftHalfPlane(left) & 
                                 visops::topHalfPlane(top));
        NEW_SKETCH(board1, bool, wholeBoard & 
                                 visops::rightHalfPlane(left) &
                                 visops::leftHalfPlane(right) & 
                                 visops::topHalfPlane(top));
        NEW_SKETCH(board2, bool, wholeBoard & 
                                 visops::rightHalfPlane(right) & 
                                 visops::topHalfPlane(top));

        NEW_SKETCH(board3, bool, wholeBoard & 
                                 visops::leftHalfPlane(left) & 
                                 visops::bottomHalfPlane(top) &
                                 visops::topHalfPlane(bottom));
        NEW_SKETCH(board4, bool, wholeBoard & 
                                 visops::rightHalfPlane(left) & 
                                 visops::leftHalfPlane(right) & 
                                 visops::bottomHalfPlane(top) &
                                 visops::topHalfPlane(bottom));
        NEW_SKETCH(board5, bool, wholeBoard & 
                                 visops::rightHalfPlane(right) & 
                                 visops::bottomHalfPlane(top) &
                                 visops::topHalfPlane(bottom));

        NEW_SKETCH(board6, bool, wholeBoard & 
                                 visops::leftHalfPlane(left) & 
                                 visops::bottomHalfPlane(bottom));
        NEW_SKETCH(board7, bool, wholeBoard & 
                                 visops::rightHalfPlane(left) &
                                 visops::leftHalfPlane(right) & 
                                 visops::bottomHalfPlane(bottom));
        NEW_SKETCH(board8, bool, wholeBoard & 
                                 visops::rightHalfPlane(right) & 
                                 visops::bottomHalfPlane(bottom));
    }
};

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