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

$nodeclass MirageReasoning : PilotDemo {
	$nodeclass FindLines : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		mapreq.addObjectColor(lineDataType,"blue");
	}

	$nodeclass DetermineInsideOutSide :  VisualRoutinesStateNode : doStart {
		//Get all lines
		NEW_SHAPEVEC(line_shape, LineData, select_type<LineData>(camShS));
	
		//Filter out noise
		SHAPEVEC_ITERATE(line_shape, LineData, myLine)
			if(myLine->getLength() < 150) {
				camShS.deleteShape(myLine);
			}
		END_ITERATE;

		//*******************************************************
		// Zero line case
		//*******************************************************

		//If there is no line, say robot position cannot be determined
		int found = 0;
		if(line_shape.size() == 0) {
			cout << "There is no line in view. Position cannot be determined!" << endl;
		}

		//*******************************************************
		// One line case
		//*******************************************************

		//If there is one horizontal line, say robot is inside
		LineData::IsHorizontal horizonTest;
		LineData::IsVertical verticalTest;
		if (line_shape.size() == 1 && horizonTest(line_shape[0])) {
			found = 1;
			cout << "inside!" << endl;
		}

		int cHorizontal = 0;
		int cVertical = 0;
		SHAPEVEC_ITERATE(line_shape, LineData, myLine)
			if(horizonTest(myLine)) cHorizontal++;
			else if (verticalTest(myLine)) cVertical++;
		END_ITERATE;

		//*******************************************************
		// Two lines case
		//*******************************************************
	
		//If there are 2 lines pointing away from the robot /\, say robot is inside
		//Otherwise, if line is pointing away \/, say robot is outside
		Shape<LineData> left,right;
		if(line_shape.size() == 2 && line_shape[0]->intersectsLine(line_shape[1])) {
			found = 1;
			if(line_shape[0]->getCentroid().coordX() < line_shape[1]->getCentroid().coordX()){
				if(line_shape[0]->getOrientation() > line_shape[1]->getOrientation()) {
					cout << "inside!" << endl;
				} else {
					cout << "outside!" << endl;
				}
			} else {
				if(line_shape[0]->getOrientation() < line_shape[1]->getOrientation()) {
					cout << "inside!" << endl;
				} else {
					cout << "outside!" << endl;
				}
			}
		}

		//*******************************************************
		// More than 2 lines case
		//*******************************************************
	
		//If there is one or more horizontal line with at least 2 vertical lines, say robot is outside
		if(cHorizontal >= 1 && cVertical >= 2) {
			found = 1;
			cout << "outside!" << endl;
		}

		//*******************************************************
		// Not found
		//*******************************************************

		//If not caught by any case, say position cannot be determined
		if (!found) cout << "Position cannot be determined from information" << endl;
	}

	$nodeclass PursueLines : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		mapreq.addObjectColor(lineDataType,"blue");
		mapreq.pursueShapes = true;
	}

	$setupmachine {
		rundemo: FindLines =C=> DetermineInsideOutSide
	}
}
REGISTER_BEHAVIOR (MirageReasoning);