#include "Behaviors/StateMachine.h"
#include "DualCoding/ShapeTypes.h"

$nodeclass InsideOrOutside : VisualRoutinesStateNode {
	$nodeclass BuildMap : MapBuilderNode($,MapBuilderRequest::localMap) : doStart {
		mapreq.addObjectColor(lineDataType, "blue");
	}

	$setupmachine {
		BuildMap =MAP=> Decide
	}

	$nodeclass Decide : VisualRoutinesStateNode : doStart {
		cout << "There were " << localShS.allShapes().size() << " lines." << endl;

		NEW_SHAPEVEC(lines, LineData, select_type<LineData>(localShS));

		int numLines = 0;
		
		SHAPEVEC_ITERATE(lines, LineData, myLine)
			numLines++;
			if (myLine->getLength() < 50) {
				myLine.deleteShape();
				numLines--;
			}
			SHAPEVEC_ITERATE(lines, LineData, otherLine) 
				if (LineData::ColinearTest(M_PI/5)(myLine, otherLine) && !(myLine == otherLine)) {
					cout << "fuck" << endl;
					if (LineData::LengthLessThan()(myLine, otherLine)) {
						myLine.deleteShape();
						numLines--;
					}
				}
			END_ITERATE
		END_ITERATE;

		SHAPEVEC_ITERATE(lines, LineData, myLine) 
			if (myLine->perpendicularDistanceFrom(theAgent->getCentroid()) > 750 ) {
				cout << "Outside (more than 750 from some line)" << endl;
				return;
			}
		END_ITERATE

		switch (numLines) {
			case 0:
				cout << "Not enough info. (no lines)" << endl;
				break;
			case 1:
				cout << "Not enough info. (one line)" << endl;
				break;
			case 2: {
				// Can't see a "V" because of the field of view, which leaves only "^", "<", and ">"

				bool isHoriz = false;
				bool crossesCenter=false;
				SHAPEVEC_ITERATE(lines, LineData, myLine)
					if (LineData::IsHorizontal()(myLine)) {
						isHoriz=true;
					} else {
						if (myLine->getOrientation()<M_PI) {
							crossesCenter = myLine->pointIsRightOf(theAgent->getCentroid());
							
						} else {
							crossesCenter = myLine->pointIsLeftOf(theAgent->getCentroid());
						}
					}
                          	END_ITERATE;

				if (isHoriz && crossesCenter) {
					cout << "inside (left or right carrot)" << endl;
				}  else if (isHoriz) {
					cout << "outside (left or right carrot)" << endl;
				} else {
					cout << "inside (up carrot)" << endl;
				}
				
				break;
			}
			case 3:
				cout << "Inside (I can see 3 lines less than 750 away)" << endl;
				break;
			case 4:
				cout << "Outisde (I can see all four lines)" << endl;
				break;
			default:
				cout << "I'm seeing " << numLines << " lines, you messed up." << endl;
				break;
		}
	}
}

REGISTER_BEHAVIOR(InsideOrOutside);