#include "Behaviors/Demos/Navigation/PilotDemo.h"

$nodeclass Box : PilotDemo {

	$nodeclass FindLines : MapBuilderNode : doStart {
		mapreq.addObjectColor(lineDataType, "blue");
		mapreq.pursueShapes = false;
	}

	$nodeclass InsideOrOutside : VisualRoutinesStateNode : doStart {

		NEW_SHAPEVEC(line_shapes, LineData, select_type<LineData>(camShS));

		cout << "Found " << line_shapes.size() << " lines." << endl;


		NEW_SHAPE(verticalMidline, LineData, new LineData(camShS, Point(320,0), Point(320,480)));
		verticalMidline->setColor("red");

		NEW_SHAPE(horizontalMidline, LineData, new LineData(camShS, Point(0,240), Point(0,640)));
		horizontalMidline->setColor("red");

		NEW_SHAPEVEC(blue_lines, LineData, subset(line_shapes, IsColor("blue")));

		// If no lines seen, post failure then rotate to try again
		if (blue_lines.size() == 0) {
			postStateFailure();
		} 
		
		// If one line seen, make judgement based on whether line is horizontal
		else if (blue_lines.size() == 1) {
				
			if (LineData::IsHorizontal() (blue_lines[0])) {
				cout << "The robot is inside the box." << endl;
			} else {
				cout << "The robot is outside the box." << endl;
			}
		}

		// If two lines seen, make judgement based on how lines cross the midline
		else if (blue_lines.size() == 2) {
			cout << "Line 0 crosses vertical midline: " << verticalMidline->intersectsLine(blue_lines[0]) << endl;
			cout << "Line 1 crosses veritcal midline: " << verticalMidline->intersectsLine(blue_lines[1]) << endl;
			cout << "Line 0 crosses horizontal midline: " << verticalMidline->intersectsLine(blue_lines[0]) << endl;
			cout << "Line 1 crosses horizontal midline: " << verticalMidline->intersectsLine(blue_lines[1]) << endl;
			if (verticalMidline->intersectsLine(blue_lines[0]) ||
				 verticalMidline->intersectsLine(blue_lines[1])) {

				if (horizontalMidline->intersectsLine(blue_lines[0]) ||
					horizontalMidline->intersectsLine(blue_lines[1])) {
					cout << "The robot is outside the box" << endl;
				} else {
					cout << "The robot is inside the box" << endl;
				}
			} else {
				cout << "The robot is outside the box" << endl;
			}
		}
		
		// If three lines are seen, try rotating a little to get a different view
		else if (blue_lines.size() == 3) {
			postStateFailure();
		}
			
		// All lines seen, the robot is outside the box
		else if (blue_lines.size() == 4) {
			cout << "The robot is outside the box." << endl;
		}
		
		// Try rotating to get a new view
		else {
			postStateFailure();
		}
	}

	$setupmachine {

		finder: FindLines
		inout: InsideOrOutside

		rundemo: StateNode =N=> finder =C=> inout =F=> Turn(M_PI/6) =C=> finder
	}
}

REGISTER_BEHAVIOR(Box);