#include "Behaviors/StateMachine.h"

$nodeclass DetectSquare : VisualRoutinesStateNode
{
	$provide int num_of_turns;

	$nodeclass FindStuff : MapBuilderNode : doStart
	{
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(lineDataType, "blue");
		mapreq.addOccluderColor(lineDataType, "red");
	}

	$nodeclass ReportShapes : VisualRoutinesStateNode : doStart
	{
//		needs to turn if sees four lines all below a threshold in image, also if
//		sees just one diagonal line. If it sees one vertical or horizontal
//		line, it's inside box. Just one vertical line means its outside.

		$reference DetectSquare::num_of_turns;
		if(num_of_turns == 8)
		{
			cout << "num_of_turns: inside box" << endl;
			postStateSuccess();
			return;
		}
		Point centerPoint = Point(320, 240, 0);
		vector<Shape<LineData> > old_lines;
		NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
		int index = 0;
		SHAPEVEC_ITERATE(lines, LineData, line)
		index++;
		if(index == 1)
			old_lines.push_back(line);
		else
		{
			int insert = 1;
			for(unsigned int count = 0; count < old_lines.size(); count++)
			{
//				cout << abs(old_lines.at(count)->getOrientation() - line->getOrientation()) << endl;
				LineData line1 = old_lines.at(count);
				LineData line2 = line;
				line1.setInfinite();
				line2.setInfinite();
				float x1 = old_lines.at(count)->getCentroid().coordX();
				float x2 = line->getCentroid().coordX();
				float y1 = old_lines.at(count)->getCentroid().coordY();
				float y2 = line->getCentroid().coordY();
				float dist = distance(x1, y1, x2, y2);
				if(line2.intersectsLine(line1) || dist < 50)
				{
   		        	if(abs(old_lines.at(count)->getOrientation() - line->getOrientation()) < M_PI/9)
					{
						if(line->getLength() > old_lines.at(count)->getLength())
							old_lines.erase(old_lines.begin() + count);
						else
							insert = 0;
						break;
					}
   		        	if(abs(old_lines.at(count)->getOrientation() - line->getOrientation()) > 8*M_PI/9)
   		        		if(abs(old_lines.at(count)->getOrientation() - line->getOrientation()) < 10*M_PI/9)
						{
							if(line->getLength() > old_lines.at(count)->getLength())
							old_lines.erase(old_lines.begin() + count);
							else
								insert = 0;
							break;
						}
				}
			}
			if(insert == 1)
				old_lines.push_back(line);
		}
       	END_ITERATE;

		int count_above = 0;
		for(unsigned int count_new = 0; count_new < old_lines.size(); count_new++)
		{
			if(old_lines.at(count_new)->getLength() < 20)
				old_lines.erase(old_lines.begin() + count_new);
			else
			{
				if(old_lines.at(count_new)->topPtShape()->coordY() < 380)
					count_above = 1;
				cout << "orientation: " << old_lines.at(count_new)->getOrientation() << ", length: " << old_lines.at(count_new)->getLength() << ", endpoint1: " << old_lines.at(count_new)->leftPt() << ", endpoint2: " << old_lines.at(count_new)->rightPt() << endl;
			}
		}

		if(count_above == 1)
		{
			cout << "count_above: definitely outside box" << endl;
			postStateSuccess();
			return;
		}
		if(old_lines.size() > 4)
		{
			cout << "impossible: seeing " << old_lines.size() << " lines" << endl;
			postStateSuccess();
			return;
		}

		if(old_lines.size() == 4)
		{			
			cout << "four_lines: must turn for more info" << endl;
			postStateFailure();
			return;
		}

		if(old_lines.size() == 3)
		{
//			check if there are two short lines (if so, we're in box)
			int count = 0;
			for(unsigned int count_old = 0; count_old < old_lines.size(); count_old++)
			{
				if(old_lines.at(count_old)->getLength() < 90)
					count++;
			}
			if(count > 2)
			{
				cout << "impossible: seeing three edges, all long" << endl;
				postStateFailure();
				return;
			}
			if(count == 2)
			{
				cout << "three_lines: inside box" << endl;
				postStateSuccess();
				return;
			}
			else
			{
				cout << "three_lines: outside box" << endl;
				postStateSuccess();
				return;
			}
		}
		
		if(old_lines.size() == 2)
		{
			int count = 0, count_left = 0, count_right = 0;
			int count_left_off_screen = 0, count_right_off_screen = 0;
			for(unsigned int count_old = 0; count_old < old_lines.size(); count_old++)
			{
				if(old_lines.at(count_old)->bottomPtShape()->coordY() < 440)
					count++;
				if(old_lines.at(count_old)->leftPtShape()->coordX() > centerPoint.coordX())
					count_left++;
				if(old_lines.at(count_old)->rightPtShape()->coordX() < centerPoint.coordX())
					count_right++;
				if(old_lines.at(count_old)->leftPtShape()->coordX() < 15)
					if(old_lines.at(count_old)->leftPtShape()->coordY() < 450)
						count_left_off_screen++;
				if(old_lines.at(count_old)->rightPtShape()->coordX() > 625)
					if(old_lines.at(count_old)->rightPtShape()->coordY() < 450)
						count_right_off_screen++;
			}
			if(count == 2)
			{
				cout << "count_for_two_lines: outside box" << endl;
				postStateSuccess();
				return;
			}
			if(count_left == 2 && count_left_off_screen >= 1)
			{
				cout << "count_left and count_left_off_screen: outside box" << endl;
				postStateSuccess();
				return;
			}
			if(count_right == 2 && count_right_off_screen >= 1)
			{
				cout << "count_right and count_right_off_screen: outside box" << endl;
				postStateSuccess();
				return;
			}
			else
			{
				cout << "two_lines: inside box" << endl;
				postStateSuccess();
				return;
			}
		}

//			if(old_lines.size() == 1)
//			{
//				if(old_lines.at(0).isHorizontal())
//				{
//					if(old_lines.at(0)->getCentroid()->coordY() > 440)
//					{
//						cout << "one_line: horizontal so inside box" << endl;
//						postStateSuccess();
//					}
//				}
//				else if(old_lines.at(0).isVertical())
//				{
//					if(old_lines.at(0)->getCentroid()->coordX() < 20
//						|| old_lines.at(0)->getCentroid()->coordX() > 620)
//					{
//						cout << "one_line: vertical so outside box" << endl;
//						postStateSuccess();
//					}
//				}
//				else
//					turn_head = 1;
//			}
		postStateFailure();
		return;
	}

	$nodeclass TurnHead : PilotNode(PilotTypes::walk) : doStart
	{
		$reference DetectSquare::num_of_turns;
 		pilotreq.da = -deg2rad((double)45);
		num_of_turns++;
	}

	$setupmachine {
		FindStuff =C=> report
		report: ReportShapes
		report =F=> turn
		report =S=> PostMachineCompletion
		turn: TurnHead =T(3000)=> report
	}
}

REGISTER_BEHAVIOR(DetectSquare);