#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() {
        camSkS.clear();
        camShS.clear();
        
        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();
    }
};

#ifndef _ParsedColors_

#define _ParsedColors_

#define PINK -1
#define EMPTY 0
#define BLUE 1

#endif

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

    void printParsedBoard(int board[9]) { 
        
        /* Printing the parsed board in ASCII */
        for (int i = 0; i < 3; i++) {
            
            for (int j = 0; j < 3; j++) {
                switch(board[3*i+j]) {
                    case PINK: 
                        printf(" X ");
                        break;
                    case BLUE:
                        printf(" O ");
                        break;
                    default:
                        printf("   ");
                }

                if (j < 2)
                    printf("|");
                else
                    printf("\n");
            }

            if (i < 2)
                    printf("---+---+---\n");
        }

        return;
    }

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

        NEW_SHAPE(h1, LineData, horizontalLines[0]->copy());
        NEW_SHAPE(h2, LineData, horizontalLines[1]->copy());

        vector<Shape<LineData> > verticalLines;
        
        SHAPEVEC_ITERATE(lines, LineData, ln) 
            if (! (LineData::ParallelTest()(ln, h1) 
                   || LineData::ParallelTest()(ln, h2)))
                verticalLines.push_back(ln);
        END_ITERATE;

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

        NEW_SHAPE(v1, LineData, verticalLines[0]->copy());
        NEW_SHAPE(v2, LineData, verticalLines[1]->copy());
        
        cout << "Found " << horizontalLines.size() 
            << " horizontal lines and " << verticalLines.size() 
            << " lines not parallel to the longest two horizontal lines" << endl;

        /*
         * Take Endpoints of the lines chosen
         */
        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 points to create the borders
         */
        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
         */
        NEW_SHAPE(top, LineData, h1->leftPt().coordY() > h2->leftPt().coordY() 
                                ? new LineData(h2) : new LineData(h1));
        NEW_SHAPE(bottom, LineData, h1->leftPt().coordY() > h2->leftPt().coordY() 
                                ? new LineData(h1) : new LineData(h2));
        NEW_SHAPE(left, LineData, v1->topPt().coordX() > v2->bottomPt().coordX() 
                                ? new LineData(v2) : new LineData(v1));
        NEW_SHAPE(right, LineData, v1->topPt().coordX() > v2->bottomPt().coordX() 
                                ? new LineData(v1) : new LineData(v2));
        
 
        /*
         * Create Boundaries
         */
        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 //   /* Board indexing */
        // --|---|-- //
        // 3 | 4 | 5 //
        // --|---|-- //
        // 6 | 7 | 8 //
        ///////////////

        /* 
         * Construct all the 9 regions and put them into a vector
         * to save tedious repetitions of code.
         */
        NEW_SKETCH(wholeBoard, bool, visops::bottomHalfPlane(topBound) &
                                     visops::topHalfPlane(bottomBound) &
                                     visops::leftHalfPlane(rightBound) &
                                     visops::rightHalfPlane(leftBound));
  
        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));

        vector<Sketch<bool> > positions;
        positions.push_back(board0);
        positions.push_back(board1);
        positions.push_back(board2);
        positions.push_back(board3);
        positions.push_back(board4);
        positions.push_back(board5);
        positions.push_back(board6);
        positions.push_back(board7);
        positions.push_back(board8);
        
        /*
         * Take all the ellipses and separate pink ones and blue ones 
         */
        NEW_SHAPEVEC(blobs, EllipseData, select_type<EllipseData>(camShS));

        IsColor pinkTest("pink");
        IsColor blueTest("blue");
        
        NEW_SKETCH_N(pink_stuff, bool, visops::colormask(wholeBoard,"pink"));
        NEW_SKETCH_N(blue_stuff, bool, visops::colormask(wholeBoard,"blue"));

        NEW_SKETCH(pinkEggs, bool, visops::zeros(pink_stuff));
        NEW_SKETCH(blueEggs, bool, visops::zeros(blue_stuff));

        SHAPEVEC_ITERATE(blobs, EllipseData, b)
            if (pinkTest(b))
                pinkEggs |= b->getRendering();
            if (blueTest(b))
                blueEggs |= b->getRendering();    
        END_ITERATE;

        // Limit the egg pixels within the board
        pinkEggs &= wholeBoard;
        blueEggs &= wholeBoard;

        NEW_SKETCH(pinkEggsSouthPts, bool, pinkEggs & !pinkEggs[*camSkS.idxS]);
        NEW_SKETCH(blueEggsSouthPts, bool, blueEggs & !blueEggs[*camSkS.idxS]);
                
        /*
         * For each board position, intersect the position with pink and blue south points
         * and figure out it any egg is occupying the position.
         */

        int board[9]; // -1 represents pink, 1 represents blue, and 0 empty (defined above)

        NEW_SKETCH_N(pink_positions, bool, visops::zeros(pink_stuff));
        NEW_SKETCH_N(blue_positions, bool, visops::zeros(blue_stuff));

        for (int i = 0; i < 9; i++) {
            board[i] = EMPTY;

            /*
             * For each board position, intersect it with pink & blue blobs
             * and add the position to the appropriate sketch if there's any pink / blue blob
             */

            /*
             * I don't know why, but there was some 'touching the edge' case
             * in one of the examples (third image on the website, the upper-right pink blob)
             * The problematic pink blob at the upper right corner of the board has
             * very few pixels in the position (1,3) so it double counts in both (1,3) and (2,3).
             * 
             * Just to make this work correctly on the examples, I set a threshold to the number of pixels
             * Hope this doesn't happen in the test set or if it does you can accept our solution too
             * (you can click and see 'pinkInThePosition' and 'blueInThePosition' for each board position
             * and see if there's a south neighbor pixel in the position.)
             *
             * I suppose there's been a change to extractLines() since the date you created this assignment,
             * since the lines extracted from the last image is totally different from the one from mine.
             * Hope you can consider this difference!
             */

            NEW_SKETCH(pinkInThePosition, bool, positions[i] & pinkEggsSouthPts);
            NEW_SKETCH_N(PITP_area, uint, visops::areacc(pinkInThePosition));
            if (!pinkInThePosition->empty() && PITP_area->max() > 2) {
                board[i] = PINK;
                pink_positions |= positions[i];
            }
            
            NEW_SKETCH(blueInThePosition, bool, positions[i] & blueEggsSouthPts);
            NEW_SKETCH_N(BITP_area, uint, visops::areacc(blueInThePosition));
            if (!blueInThePosition->empty() && BITP_area->max() > 2) {
                board[i] = BLUE;            
                blue_positions |= positions[i];
            }
        }

        /*
         * Merge all the parsed board positions
         */

        pink_positions->setColor("pink");
        blue_positions->setColor("blue");

        NEW_SKETCH(parsed_positions, uchar, 
                    (pink_positions * ProjectInterface::getColorIndex("pink")) 
                    + (blue_positions * ProjectInterface::getColorIndex("blue")));

        printParsedBoard(board);

        cout << "Done parsing the board" << endl;
    }
};

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