#include "Behaviors/StateMachine.h"

enum BoardFillType
{
	Empty,
	X,
	O
};

#define X_COLOR "blue"
#define O_COLOR "pink"

$nodeclass BoardParser: VisualRoutinesStateNode("BoardParser")
{
		$nodeclass BuildMap: MapBuilderNode($, MapBuilderRequest::cameraMap): doStart {
			NEW_SHAPE(gazePt, PointData, new PointData(camShS,Point(400,0,-100,egocentric)));
			mapreq.maxDist = 1500;
			mapreq.motionSettleTime = 2000;

			mapreq.addObjectColor(lineDataType, "yellow");
			mapreq.addOccluderColor(lineDataType, X_COLOR);
			mapreq.addOccluderColor(lineDataType, O_COLOR);

			mapreq.addObjectColor(ellipseDataType, X_COLOR);
			mapreq.addObjectColor(ellipseDataType, O_COLOR);

		}


		$nodeclass ExamineMap: SoundNode($, "barklow.wav") : doStart {
			// Get all the horizontal lines in the image
			NEW_SHAPEVEC(horizontalLines, LineData, select_type<LineData>(camShS.allShapes()));
			horizontalLines = subset(horizontalLines, LineData::IsHorizontal());

			// Sort them by length;
			horizontalLines = stable_sort(horizontalLines,not2(LineData::LengthLessThan()));


			// Longest and second longest are the first in the array
			Shape<LineData>* firstHorizontalShape = &(horizontalLines[0]);
			Shape<LineData>* secondHorizontalShape = &(horizontalLines[1]);
			LineData* firstHorizontal = &(firstHorizontalShape->getData());
			LineData* secondHorizontal = &(secondHorizontalShape->getData());


			// We want the first horizontal to be the top one.
			if(firstHorizontal->topPt().isBelow(secondHorizontal->topPt()))
			{
				secondHorizontalShape = &(horizontalLines[0]);
				firstHorizontalShape = &(horizontalLines[1]);
				firstHorizontal = &(firstHorizontalShape->getData());
				secondHorizontal = &(secondHorizontalShape->getData());

			}


			std::vector<DualCoding::Shape<LineData> > verticalLines;

			NEW_SHAPEVEC(lines2, LineData, select_type<LineData>(camShS.allShapes()));

			// Get all the lines not parallel to the two longest lines
			SHAPEVEC_ITERATE(lines2, LineData, line)
			{
				if(LineData::linesParallel(line, *firstHorizontal) || LineData::linesParallel(line, *secondHorizontal))
				{
					cout<<"Line was parallel.\n";
					continue;
				}
				else
				{
					cout<<"Line was not parallel.\n";
					verticalLines.push_back(line);
				}
			}
			END_ITERATE;


			// Sort them.

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

			// Take the first two largest
			Shape<LineData>* firstVerticalShape = &(verticalLines[0]);
			Shape<LineData>* secondVerticalShape = &(verticalLines[1]);
			LineData* firstVertical = &(firstVerticalShape->getData());
			LineData* secondVertical = &(secondVerticalShape->getData());

			// We want the first vertical line to be to the left
			if(firstVertical->topPt().isRightOf(secondVertical->topPt()))
			{
				secondVerticalShape = &(verticalLines[0]);
				firstVerticalShape = &(verticalLines[1]);
				firstVertical = &(firstVerticalShape->getData());
				secondVertical = &(secondVerticalShape->getData());
			}



			// Create the border lines:
			LineData leftBorder(camShS, leftMost(leftMost(firstHorizontal->firstPt(secondHorizontal), secondHorizontal->firstPt(firstHorizontal)),leftMost(firstHorizontal->secondPt(secondHorizontal), secondHorizontal->secondPt(firstHorizontal))),firstVertical->getOrientation());
			LineData rightBorder(camShS,rightMost(rightMost(firstHorizontal->firstPt(secondHorizontal), secondHorizontal->firstPt(firstHorizontal)),rightMost(firstHorizontal->secondPt(secondHorizontal), secondHorizontal->secondPt(firstHorizontal))),secondVertical->getOrientation());
			LineData topBorder(camShS, topMost(firstVertical->topPt(), secondVertical->topPt()), firstHorizontal->getOrientation());
			LineData bottomBorder(camShS, bottomMost(firstVertical->bottomPt(), secondVertical->bottomPt()), secondHorizontal->getOrientation());

			//Now create masks for each region:
			//  ____________
			// | A | B | C |
			// |---+---+---|
			// | D | E | F |
			// |---+---+---|
			// | G | H | I |
			//  ------------

			// Half-plane masking
			NEW_SKETCH(L_v1, bool, visops::leftHalfPlane(*firstVertical));
			NEW_SKETCH(R_v1, bool, visops::rightHalfPlane(*firstVertical));
			NEW_SKETCH(L_v2, bool, visops::leftHalfPlane(*secondVertical));
			NEW_SKETCH(R_v2, bool, visops::rightHalfPlane(*secondVertical));
			NEW_SKETCH(R_bl, bool, visops::rightHalfPlane(leftBorder));
			NEW_SKETCH(L_br, bool, visops::leftHalfPlane(rightBorder));
			NEW_SKETCH(B_bt, bool, visops::bottomHalfPlane(topBorder));
			NEW_SKETCH(T_h1, bool, visops::topHalfPlane(*firstHorizontal));
			NEW_SKETCH(B_h1, bool, visops::bottomHalfPlane(*firstHorizontal));
			NEW_SKETCH(T_h2, bool, visops::topHalfPlane(*secondHorizontal));
			NEW_SKETCH(B_h2, bool, visops::bottomHalfPlane(*secondHorizontal));
			NEW_SKETCH(T_bb, bool, visops::topHalfPlane(bottomBorder));

			// Mask for every region
			NEW_SKETCH(A, bool, L_v1 & T_h1 & R_bl & B_bt);
			NEW_SKETCH(B, bool, B_bt & T_h1 & R_v1 & L_v2);
			NEW_SKETCH(C, bool, T_h1 & L_br & R_v2 & B_bt);
			NEW_SKETCH(D, bool, B_h1 & R_bl & L_v1 & T_h2);
			NEW_SKETCH(E, bool, B_h1 & R_v1 & L_v2 & T_h2);
			NEW_SKETCH(F, bool, B_h1 & R_v2 & L_br & T_h2);
			NEW_SKETCH(G, bool, B_h2 & L_v1 & R_bl & T_bb);
			NEW_SKETCH(H, bool, B_h2 & L_v2 & R_v1 & T_bb);
			NEW_SKETCH(I, bool, B_h2 & L_br & R_v2 & T_bb);

			//Now get every ellipse
			NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS.allShapes()));

			// For each ellipse, calculate its bottom pixels and put it into a sketch
			NEW_SKETCH(bottomPixels, bool, visops::zeros(camSkS));
			NEW_SKETCH(ellipsePixels, uchar, visops::zeros(camSkS));
			SHAPEVEC_ITERATE(ellipses,EllipseData, ellipse)
				bottomPixels |= ellipse.getData().getRendering()&(R_bl & L_br & B_bt & T_bb);
				ellipsePixels += ellipse.getData().getRendering()*ProjectInterface::getColorIndex(ellipse.getData().getColor());
			END_ITERATE;

			// Get the bottom pixels inside the grid
			bottomPixels = bottomPixels & !bottomPixels[*camSkS.idxS];

			// Now, parse the way the board is constructed.
			BoardFillType boardStatus[3][3];

			// Initialize board to empty
			for(int r = 0; r<3; r++)
			{
				for(int c = 0; c<3; c++)
				{
					boardStatus[r][c] = Empty;
				}
			}

			// For convenience, put each mask into an array.
			Sketch<uchar> boardMasks[3][3];
			boardMasks[0][0] = A;
			boardMasks[0][1] = B;
			boardMasks[0][2] = C;
			boardMasks[1][0] = D;
			boardMasks[1][1] = E;
			boardMasks[1][2] = F;
			boardMasks[2][0] = G;
			boardMasks[2][1] = H;
			boardMasks[2][2] = I;
			NEW_SKETCH(coloredMask,uchar,visops::zeros(camSkS));
			NEW_SKETCH(xMask, bool, visops::zeros(camSkS));
			NEW_SKETCH(oMask, bool, visops::zeros(camSkS));

			// Now parse each segment of the board
			for(int r = 0; r<3; r++)
			{
				for(int c = 0; c<3; c++)
				{
					// If the board mask contains some bottom pixels..
					if(!(boardMasks[r][c]*bottomPixels)->empty())
					{
						//Figure out what the color of those pixels were
						coloredMask = boardMasks[r][c] * ellipsePixels * bottomPixels;
						xMask*=0;
						oMask*=0;
						xMask = visops::colormask(coloredMask,X_COLOR);
						oMask = visops::colormask(coloredMask,O_COLOR);


						if(!xMask->empty())
						{
							cout<< r << "," << c << " X!"<<endl;
							boardStatus[r][c] = X;
							boardMasks[r][c]*=(int)ProjectInterface::getColorIndex(X_COLOR);
						}

						else if(!oMask->empty())
						{
							cout << r << "," << c << " O!"<<endl;
							boardStatus[r][c] = O;
							boardMasks[r][c]*=(int)ProjectInterface::getColorIndex(O_COLOR);
						}
						else
						{
							cout<< r <<","<< c <<" Empty!"<<endl;
							boardMasks[r][c]*=0;
						}

					}
				}
			}

			Sketch<uchar> boardMaskFinal = visops::zeros(camSkS);
			for(int r = 0; r<3; r++)
			{
				for(int c = 0; c<3; c++)
				{
					boardMaskFinal += boardMasks[r][c];
				}
			}

			NEW_SKETCH(parsed, uchar ,visops::copy(boardMaskFinal));
		}

		$setupmachine{
			BuildMap =MAP=> ExamineMap
		}

}
REGISTER_BEHAVIOR(BoardParser);