#include "Behaviors/StateMachine.h"

#define DIST_FROM_BOARD 500

$nodeclass TicTacToe : VisualRoutinesStateNode {
	$provide Shape<PolygonData> boardOutline; 
	$provide vector<Shape<PolygonData> > squares;

	$provide string const lineColor("blue");
	$provide string const xColor("green");
	$provide string const oColor("red");

	$provide int lineColorIndex(ProjectInterface::getColorIndex("blue"));
	$provide int xColorIndex(ProjectInterface::getColorIndex("green"));
	$provide int oColorIndex(ProjectInterface::getColorIndex("red"));

	$provide string myColor(xColor);
	$provide int myColorIndex(xColorIndex);

	$provide vector<int> positions;
	$provide vector<int> prevState;

	$provide vector<Shape<CylinderData> > unplayedPieces;
	$provide vector<Shape<CylinderData> > playedPieces;
	$provide bool clearBoard(true);

	/* getGazePoints()

	Get a set of gaze points for looking at the tic tac toe board.
	Looks at the ground close to the robot, 45 deg. to the left and right.
	Not using groundSearchPoints() because that looks behind, as well, 
	which was causing problems.
	*/
	static Shape<PolygonData> getGazePoints(vector<Shape<PolygonData> > squares) {
		vector<Point> gazePoints;
		// look front first to hopefully see the break in the board line 
		// caused by occluding pieces, as opposed to thinking the line just ends.

		// look front
		gazePoints.push_back(Point( 500, 0, 0, egocentric));
		gazePoints.push_back(Point( 1000, 0, 0, egocentric));
		gazePoints.push_back(Point( 2000, 0, 0, egocentric));

		/*

		// look left
		gazePoints.push_back(Point( 500, -500, 0, egocentric));
		gazePoints.push_back(Point( 750, -250, 0, egocentric));
		gazePoints.push_back(Point( 1000, -1000, 0, egocentric));

		// look right
		gazePoints.push_back(Point( 1000, 1000, 0, egocentric));
		gazePoints.push_back(Point( 500, 500, 0, egocentric));
		gazePoints.push_back(Point( 750, 250, 0, egocentric));

		// look at each board square (if we have any)
		// commenting out to reduce wasted head motion. May yet be necessary.
		//        SHAPEVEC_ITERATE(squares, PolygonData, square) {
		//            gazePoints.push_back(square->getCentroid());
		//        } END_ITERATE;

		*/
		NEW_SHAPE(gazePoly, PolygonData, new PolygonData(VRmixin::localShS, gazePoints, false));
		return gazePoly;
	}

	// clear board for safety when running the behavior multiple times
	$nodeclass InitializeNode : doStart {
		$reference TicTacToe::positions, TicTacToe::prevState,
			TicTacToe::unplayedPieces, TicTacToe::playedPieces, TicTacToe::clearBoard;

		positions.clear();
		prevState.clear();
		unplayedPieces.clear();
		playedPieces.clear();
		clearBoard = true;
		postStateCompletion();
	}

	$nodeclass FindBoardLines : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		$reference TicTacToe::lineColor, TicTacToe::xColor, TicTacToe::oColor;

		mapreq.addObjectColor(lineDataType, lineColor);
		mapreq.addOccluderColor(lineDataType, xColor);
		mapreq.addOccluderColor(lineDataType, oColor);
		// mapreq.addOccluderColor(lineDataType, "black");
		// mapreq.addOccluderColor(lineDataType, "white");
		mapreq.addAllMinBlobAreas(500);

		vector<Shape<PolygonData> > noSquares;
		mapreq.searchArea = TicTacToe::getGazePoints(noSquares);
		mapreq.pursueShapes = false;
		mapreq.setVerbosity = -1u; // Turn on all mapbuilder warnings
	}

	$nodeclass AlignWithBoard : PilotNode(PilotTypes::goToShape) {
		virtual void doStart() {
			pair<Point, float> dest = target();
			NEW_SHAPE(destination, PointData, new PointData(worldShS, dest.first));
			destination->setObstacle(false);
			pilotreq.targetShape = destination;
			pilotreq.targetHeading = dest.second;

			pilotreq.executePath = true;
		}

		pair<Point, float> target() {
			NEW_SHAPEVEC(lines, LineData, select_type<LineData>(worldShS));
			lines = stable_sort(lines,not2(LineData::LengthLessThan()));
			if ( lines.size() < 4 ) {
				cout << "Found " << lines.size() << " lines in the image; needed 4." << endl;
				return make_pair(Point(), 0.0);
			}

			Shape<LineData> l1, l2;

			vector<pair<Shape<LineData>, Shape<LineData > > > linePairs = vector<pair<Shape<LineData>, Shape<LineData> > >();
			SHAPEVEC_ITERATE(lines, LineData, line1) {
				line1->setObstacle(false);
				SHAPENEXT_ITERATE(lines, LineData, line1, line2) { 
					if ( LineData::ParallelTest()(line1,line2) ) {
						linePairs.push_back(make_pair(line1, line2));
					}
				} END_ITERATE;
			} END_ITERATE;

			// get center of board
			Point center = 
				(linePairs[0].first->getCentroid()
				 + linePairs[0].second->getCentroid()
				 + linePairs[1].first->getCentroid()
				 + linePairs[1].second->getCentroid()) / 4;

			Point wp1 = getWP(linePairs[0].first, linePairs[0].second);
			Point wp2 = getWP(linePairs[1].first, linePairs[1].second);

			Point p, endpt;

			// choose the closer of the 2 waypoints
			p = Point(0,0,0);
			endpt = (p.distanceFrom(wp1) < p.distanceFrom(wp2) ? wp1 : wp2);

			Point dir = center - endpt;
			float heading = atan2(dir.coordY(), dir.coordX());
			cout << "wp1 = " << wp1 << "   wp2 = " << wp2 << endl;
			cout << "Center: " << center << "; Endpt: " << endpt << "; heading: " << heading << endl;

			return make_pair(endpt, heading);
		}

		Point getWP(Shape<LineData> l1, Shape<LineData> l2) {
			/* 
				 rotate averaged endpoints of these two lines
				 offset by DIST_FROM_BOARD
				 rotate back
			*/
			float orientation = l1->getOrientation();

			Point end1, end2;
			// bottom point should be closer to 0 (in x), which is where we are.
			end1 = l1->bottomPt();
			end2 = l2->bottomPt();
			if (orientation < deg2rad(90.0)) {
				//orient toward the bottom point instead of the top
				orientation += deg2rad(180.0);
			}
			Point wp = (end1 + end2) / 2;

			fmat::Column<3> rotated = fmat::rotationZ(-orientation) * wp.getCoords();
			rotated[0] += DIST_FROM_BOARD;
			fmat::Column<3> unrotated = fmat::rotationZ(orientation) * rotated;

			Point extended = Point(unrotated[0], unrotated[1], unrotated[2]);
			return extended;
		}
	}

	$nodeclass LookAtBoard : MapBuilderNode(MapBuilderRequest::worldMap) {
		virtual void doStart() {
			$reference TicTacToe::lineColor, TicTacToe::xColor, TicTacToe::oColor,
				TicTacToe::clearBoard, TicTacToe::squares;

			mapreq.addObjectColor(lineDataType, lineColor);
			mapreq.addOccluderColor(lineDataType, xColor);
			mapreq.addOccluderColor(lineDataType, oColor);

			// hopefully detect AprilTags as occluders.
			mapreq.addOccluderColor(lineDataType, "black");
			// mapreq.addOccluderColor(lineDataType, "white");
			// mapreq.addOccluderColor(lineDataType, "gray");

			mapreq.addObjectColor(cylinderDataType, xColor);
			mapreq.addObjectColor(cylinderDataType, oColor);

			mapreq.addAllMinBlobAreas(500);

			mapreq.searchArea = TicTacToe::getGazePoints(squares);
			//mapreq.setVerbosity = -1u; // Turn on all mapbuilder warnings

			mapreq.pursueShapes = false;

			if (clearBoard) {
				// reset world to be aligned with the board
				mapreq.clearWorld = true;
				pilot->setAgent(Point(0,0,0), 0.0, false, false);
				clearBoard = false;
			} else {
				mapreq.clearWorld = false;
			}
		}
	}

	$nodeclass ParseBoard : VisualRoutinesStateNode {

		virtual void doStart() {
			if ( parseImage() )
				postStateSuccess();
			else
				postStateFailure();
		}

		bool parseImage() {
			NEW_SHAPEVEC(lines, LineData, parseLines());
			if ( lines.size() < 4 )
				return false;
			return true;
		}


		bool isNorthOf(Shape<LineData> ln1, Shape<LineData> ln2) {
			return ln1->getCentroid().coordX() > ln2->getCentroid().coordX();
		}
		bool isWestOf(Shape<LineData> ln1, Shape<LineData> ln2) {
			return ln1->getCentroid().coordY() > ln2->getCentroid().coordY();
		}

		/*! 1. sort lines by length
		 *  2. select longest horizontal line and next longest line parallel to it
		 *  3. select two longest lines not parallel to either line from previous step */
		class IsLong : public UnaryShapePred<LineData> {
		public: 
			IsLong() {};
			bool operator() (Shape<LineData> l) const {
				return l->getLength() > 2000.0f;
			};
		};

		vector<Shape<LineData> > parseLines() {
			NEW_SHAPEVEC(boardLines, LineData, vector<Shape<LineData> >());

			// 1. sort by length
			NEW_SHAPEVEC(allLines, LineData, select_type<LineData>(worldShS));

			// delete lines that are too long.
			NEW_SHAPEVEC(longLines, LineData, subset(allLines, IsLong()));
			SHAPEVEC_ITERATE(longLines, LineData, l) {
				l.deleteShape();
			} END_ITERATE;

			// long lines are now deleted from worldShS
			NEW_SHAPEVEC(lines, LineData, select_type<LineData>(worldShS));

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

			if ( lines.size() < 4 ) {
				cout << "Found " << lines.size() << " lines in the image; needed 4." << endl;
				return boardLines;
			}

			// 2. Find the north and south horizontal lines
			NEW_SHAPE(topLine, LineData, Shape<LineData>());
			NEW_SHAPE(bottomLine, LineData, Shape<LineData>());
			SHAPEVEC_ITERATE(lines, LineData, ln1) {
				if ( LineData::IsVertical()(ln1) ) { 
					//TODO check IsVertical's behavior in allocentric coords
					SHAPENEXT_ITERATE(lines, LineData, ln1, ln2) {
						if ( LineData::ParallelTest()(ln1,ln2) ) {
							topLine = isNorthOf(ln1,ln2) ? ln1 : ln2;
							bottomLine = isNorthOf(ln1,ln2) ? ln2 : ln1;
							break;
						}
						END_ITERATE }
				}
				if ( bottomLine )
					break;
				END_ITERATE }
			if ( ! bottomLine ) {
				cout << "Couldn't find top or bottom line" << endl;
				return boardLines;
			}

			// 3. Find the left and right sort-of-vertical lines
			NEW_SHAPE(leftLine, LineData, Shape<LineData>());
			NEW_SHAPE(rightLine, LineData, Shape<LineData>());
			SHAPEVEC_ITERATE(lines, LineData, ln1) {
				if ( !LineData::ParallelTest()(topLine,ln1) ) {
					SHAPENEXT_ITERATE(lines, LineData, ln1, ln2) {
						if ( !LineData::ParallelTest()(topLine,ln2) ) {
							leftLine = isWestOf(ln1, ln2) ? ln1 : ln2;
							rightLine = isWestOf(ln1, ln2) ? ln2 : ln1;
							break;
						}
						END_ITERATE }
				}
				if ( rightLine )
					break;
				END_ITERATE }
			if ( ! rightLine ) {
				cout << "Couldn't find left or right line" << endl;
				return boardLines;
			}

			// rename and return lines in specified order
      topLine->setName("topLine");
      bottomLine->setName("bottomLine");
      leftLine->setName("leftLine");
      rightLine->setName("rightLine");

			boardLines.push_back(topLine);
			boardLines.push_back(bottomLine);
			boardLines.push_back(leftLine);
			boardLines.push_back(rightLine);
			parseBoundaries(topLine, bottomLine, leftLine, rightLine);

			SHAPEVEC_ITERATE(boardLines, LineData, line) {
				line->setLandmark(true);
				line->setObstacle(false);
			} END_ITERATE;

			return boardLines;
		}

		void parseBoundaries(const Shape<LineData>& topLine, const Shape<LineData>& bottomLine,
												 const Shape<LineData>& leftLine, const Shape<LineData>& rightLine) {

			$reference TicTacToe::boardOutline, TicTacToe::squares;
			// Construct board boundary lines
      const Point& tl = topLine->leftPt();
			const Point& tr = topLine->rightPt();
			const Point& bl = bottomLine->leftPt();
			const Point& br = bottomLine->rightPt();
			const Point& lt = leftLine->topPt();
			const Point& lb = leftLine->bottomPt();
			const Point& rt = rightLine->topPt();
			const Point& rb = rightLine->bottomPt();

			Point top = lt.coordX() > rt.coordX() ? lt : rt;
			Point left = tl.coordY() > bl.coordY() ? tl : bl;
			Point right = tr.coordY() < br.coordY() ? tr : br;
			Point bottom = lb.coordX() < rb.coordX() ? lb : rb;

			NEW_SHAPE(topBoundary, LineData, new LineData(worldShS, top, topLine->getOrientation()));
			NEW_SHAPE(leftBoundary, LineData, new LineData(worldShS, left, leftLine->getOrientation()));
			NEW_SHAPE(rightBoundary, LineData, new LineData(worldShS, right, leftLine->getOrientation()));
			NEW_SHAPE(bottomBoundary, LineData, new LineData(worldShS, bottom, topLine->getOrientation()));

			// outer corners
			Point otlc = leftBoundary->intersectionWithLine(topBoundary);
			Point otrc = topBoundary->intersectionWithLine(rightBoundary);
			Point oblc = bottomBoundary->intersectionWithLine(leftBoundary);
			Point obrc = rightBoundary->intersectionWithLine(bottomBoundary);

			// center square points
			Point itlc = topLine->intersectionWithLine(leftLine);
			Point itrc = topLine->intersectionWithLine(rightLine);
			Point iblc = bottomLine->intersectionWithLine(leftLine);
			Point ibrc = bottomLine->intersectionWithLine(rightLine);

      // edge points not on the corners of the board
      Point etlc = topLine->intersectionWithLine(leftBoundary);
      Point eblc = bottomLine->intersectionWithLine(leftBoundary);
      Point etrc = topLine->intersectionWithLine(rightBoundary);
      Point ebrc = bottomLine->intersectionWithLine(rightBoundary);

      Point eltc = leftLine->intersectionWithLine(topBoundary);
      Point elbc = leftLine->intersectionWithLine(bottomBoundary);
      Point ertc = rightLine->intersectionWithLine(topBoundary);
      Point erbc = rightLine->intersectionWithLine(bottomBoundary);

			boardOutline = makeSquare(otlc, otrc, obrc, oblc);
			squares.push_back(makeSquare(otlc, eltc, itlc, etlc));
			squares.push_back(makeSquare(eltc, ertc, itrc, itlc));
			squares.push_back(makeSquare(ertc, otrc, etrc, itrc));
			squares.push_back(makeSquare(etlc, itlc, iblc, eblc));
			squares.push_back(makeSquare(itlc, itrc, ibrc, iblc));
			squares.push_back(makeSquare(itrc, etrc, ebrc, ibrc));
			squares.push_back(makeSquare(eblc, iblc, elbc, oblc));
			squares.push_back(makeSquare(iblc, ibrc, erbc, elbc));
			squares.push_back(makeSquare(ibrc, ebrc, obrc, erbc));

			topBoundary.deleteShape();
			leftBoundary.deleteShape();
			rightBoundary.deleteShape();
			bottomBoundary.deleteShape();
		}

		Shape<PolygonData> makeSquare(Point tlc, Point trc, Point brc, Point blc) {
			vector<Point> points;
			points.push_back(tlc);
			points.push_back(trc);
			points.push_back(brc);
			points.push_back(blc);

			NEW_SHAPE(square, PolygonData, new PolygonData(worldShS, points, true));
      square->setObstacle(false);
			return square;
		}


	}

	$nodeclass GetPieces : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		$reference TicTacToe::xColor, TicTacToe::oColor, TicTacToe::squares; 

		mapreq.addObjectColor(cylinderDataType, xColor);
		mapreq.addObjectColor(cylinderDataType, oColor);

		mapreq.addAllMinBlobAreas(500);
		mapreq.searchArea = TicTacToe::getGazePoints(squares);

		mapreq.pursueShapes = false;
	}

	$nodeclass ParsePieces {
		void doStart() {
			$reference TicTacToe::positions;
			positions = parsePieces();
			postStateCompletion();
		}

		class IsFar : public UnaryShapePred<CylinderData> {
		public: 
			IsFar() {};
			bool operator() (const Shape<CylinderData> cyl) const {
				Point centroid = cyl->getCentroid();
				float x = centroid.coordX();
				float y = centroid.coordY();

				return sqrt(pow(x,2) + pow(y,2)) >= 3000; 
			};
		};

		vector<int> parsePieces() {
			$reference TicTacToe::xColor, TicTacToe::oColor,
				TicTacToe::xColorIndex, TicTacToe::oColorIndex,
				TicTacToe::boardOutline, TicTacToe::squares,
				TicTacToe::unplayedPieces, TicTacToe::playedPieces;

			unplayedPieces = vector<Shape<CylinderData> >();
			playedPieces = vector<Shape<CylinderData> >();

			NEW_SHAPEVEC(allCylinders, CylinderData, select_type<CylinderData>(worldShS));
			NEW_SHAPEVEC(far, CylinderData, subset(allCylinders, IsFar()));
			SHAPEVEC_ITERATE(far, CylinderData, cyl) {
				cyl.deleteShape();
			} END_ITERATE;

			// distant cylinders have been removed
			NEW_SHAPEVEC(cylinders, CylinderData, select_type<CylinderData>(worldShS));
			NEW_SHAPEVEC(x_pieces, CylinderData, subset(cylinders, IsColor(xColor)));
			NEW_SHAPEVEC(o_pieces, CylinderData, subset(cylinders, IsColor(oColor)));

			vector<int> squareValues(9,0);
			SHAPEVEC_ITERATE(x_pieces, CylinderData, piece) {
				if (boardOutline->isInside(piece->getCentroid())) {
					for (int i=0; i<9; i++){
						if (squares[i]->isInside(piece->getCentroid())){
							piece->setObstacle(true);
							piece->setLandmark(false);
							squareValues[i] = xColorIndex;
						}
					}
				} else {
					unplayedPieces.push_back(piece);
				}
			} END_ITERATE;
			SHAPEVEC_ITERATE(o_pieces, CylinderData, piece) {
				if (boardOutline->isInside(piece->getCentroid())) {
					playedPieces.push_back(piece);
					for (int i=0; i<9; i++){
						if (squares[i]->isInside(piece->getCentroid())){
							piece->setObstacle(true);
							piece->setLandmark(false);
							squareValues[i] = oColorIndex;
						}
					}
				} else {
					unplayedPieces.push_back(piece);
				}
			} END_ITERATE;

			return squareValues;
		}

	}

	$nodeclass DisplayBoard : StateNode {
		char outputPosition(int i) {
			$reference TicTacToe::positions, TicTacToe::xColorIndex, TicTacToe::oColorIndex;
			if ( positions[i] == xColorIndex )
				return 'x';
			else if ( positions[i] == oColorIndex )
				return 'o';
			else
				return ' ';
		}

		virtual void doStart() {
			cout << " " << outputPosition(0) << " | " << outputPosition(1) << " | " << outputPosition(2) << endl;
			cout << "-----------\n";
			cout << " " << outputPosition(3) << " | " << outputPosition(4) << " | " << outputPosition(5) << endl;
			cout << "-----------\n";
			cout << " " << outputPosition(6) << " | " << outputPosition(7) << " | " << outputPosition(8) << endl;
		}
	}

	$nodeclass MakeMove : GrasperNode(GrasperRequest::moveTo) {

		virtual void doStart() {
			$reference TicTacToe::unplayedPieces, TicTacToe::playedPieces, TicTacToe::myColor;

			Point dest = getMove();
			NEW_SHAPE(destination, PointData, new PointData(worldShS, dest));
			destination->setObstacle(false);

			NEW_SHAPE(piece, CylinderData, find_if (unplayedPieces, IsColor(myColor)));
			if (!piece.isValid()) {
				cout << "Couldn't find a piece to play" << endl;
				cancelThisRequest();
			} else { 
				playedPieces.push_back(piece);
				graspreq.object = piece;
				graspreq.targetLocation = destination;
			}
		}

		// pseudo-AI (choose a random square...)
		// TODO don't suck
		virtual Point getMove() {
			$reference TicTacToe::positions, TicTacToe::squares, TicTacToe::myColorIndex;

			int move;
			int safety = 50;
			while ( --safety > 0 && positions[move = rand() % 9] != 0) { }
			positions[move] = myColorIndex;
			return squares[move]->getCentroid();
		}
	}

	$nodeclass LeaveBoard : PilotNode(PilotTypes::goToShape) {
		void doStart() {
			cout << "leaving board --------------------------------" << endl;
			$reference TicTacToe::playedPieces;
			SHAPEVEC_ITERATE(playedPieces, CylinderData, piece) {
				piece->setObstacle(true);
				piece->setLandmark(false);
			} END_ITERATE;


			NEW_SHAPE(target, PointData, new PointData(worldShS, Point(0,0,0)));
			target->setObstacle(false);
        
			pilotreq.targetShape = target;
			pilotreq.targetHeading = 0;
		}
	}

	$nodeclass WaitForTurn {

		$nodeclass IsGameOver {
			void doStart() {
				$reference TicTacToe::positions, TicTacToe::myColorIndex;

				int winStates[8][3] = {
					/* horizontal */
					{ 0, 1, 2 },
					{ 3, 4, 5 },
					{ 6, 7, 8 },
					/* vertical */
					{ 0, 3, 6 },
					{ 1, 4, 7 },
					{ 2, 5, 8 },
					/* diagonal */
					{ 0, 4, 8 },
					{ 6, 4, 2 }
				};


				for (int i=0; i<8; i++) {
					int *trip = winStates[i];
					if (positions[trip[0]] != 0
							&& positions[trip[0]] == positions[trip[1]]
							&& positions[trip[1]] == positions[trip[2]]) {
						positions[trip[0]] == myColorIndex ?
							postStateSuccess() : 
							postStateFailure();
						return;
					}
				}
				postStateCompletion();
			}
		}

		$nodeclass IsMyTurn {
			virtual void doStart() {
				$reference TicTacToe::positions, TicTacToe::xColorIndex, 
					TicTacToe::oColorIndex, TicTacToe::myColorIndex;

				// see if it's our turn
				int myPieces = 0, otherPieces = 0;
				for (unsigned int i=0; i<positions.size(); i++) {
					if (positions[i] == xColorIndex || positions[i] == oColorIndex) {
						positions[i] == myColorIndex ?
							myPieces++ : otherPieces++;
					}
				}
							
				if (myColorIndex == xColorIndex && myPieces <= otherPieces) {
					cout << "It is our (x's) turn" << endl;
					postStateSuccess();
				} else if (myColorIndex == oColorIndex && myPieces < otherPieces) {
					cout << "It is our (o's) turn" << endl;
					postStateSuccess();
				} else {
					cout << "Not our turn." << endl;
					postStateFailure();
				}
			}
		}

		$nodeclass IsStable {
			void doStart() {
				$reference TicTacToe::prevState, TicTacToe::positions;
				if (prevState.size() != positions.size()) {
					prevState = positions;
					postStateFailure();
					return;
				}
				for (size_t i=0; i<prevState.size(); i++) {
					if (prevState[i] != positions[i]) {
						cout << "Positions do not match: "
								 << prevState[i] << " is now " << positions[i] << endl;
						prevState = positions;
						postStateFailure();
						return;
					}
				}
						
				prevState = positions;
				postStateSuccess();
				return;
			}
		}

		$setupmachine{
		isGameOver: IsGameOver
				isGameOver =C=> isMyTurn
				isGameOver =S=> PostMachineSuccess()
				isGameOver =F=> PostMachineFailure()

				isMyTurn: IsMyTurn
				isMyTurn =S=> PostMachineCompletion()
				isMyTurn =F=> wait

				wait: StateNode
				wait =T(5000)=> SpeechNode("Waiting for my turn.") =C=> 
				GetPieces =C=> ParsePieces =C=> isStable

				isStable: IsStable
				isStable =S=> isGameOver
				isStable =F=> SpeechNode("Board is not stable.") =C=> wait
				}
	}

	//$nodeclass Localize : PilotNode(PilotTypes::localize) { }

	$nodeclass SetMyColor(string color) : doStart {
		$reference TicTacToe::myColor, TicTacToe::xColor, TicTacToe::oColor, 
			TicTacToe::myColorIndex, TicTacToe::xColorIndex, 
			TicTacToe::oColorIndex;

		if (color == "x") {
			myColor = xColor;
			myColorIndex = xColorIndex;
		} else if (color == "o") {
			myColor = oColor;
			myColorIndex = oColorIndex;
		} else {
			cout << "Invalid piece type " << color << endl;
		}
		postStateCompletion();
	}

    $setupmachine{

		init: InitializeNode =C=> ParkArm =C=> listener

        listener: SpeechNode("Choose x or o")
        listener =TM("x")=> SetMyColor("x") =C=> findboard
				listener =TM("o")=> SetMyColor("o") =C=> findboard
				listener =TM=> listener

        findboard: FindBoardLines =C=> align

        align: AlignWithBoard
        align =C=> lookatboard
        align =F=> SpeechNode("Pilot couldn't align robot with board") =N=> lookatboard

				lookatboard: LookAtBoard =C=> parse

        parse: ParseBoard
				parse =S=> ParsePieces =C=> loop
				parse =F=> SpeechNode("failure")

        loop : StateNode =N=> waitTurn

        waitTurn: WaitForTurn
        waitTurn =S=> SpeechNode("I win!")
        waitTurn =F=> SpeechNode("You win")
        waitTurn =C=> DisplayBoard =N=> makeMove

        makeMove: MakeMove
				makeMove =C=> ParkArm =C=> LeaveBoard =C=> loop
        makeMove =F=> SpeechNode("Failure.  Message to continue.") =TM=> makeMove
				}

}

REGISTER_BEHAVIOR(TicTacToe);