#include "Behaviors/StateMachine.h"

$nodeclass Lab4Q4 : VisualRoutinesStateNode {
	$nodeclass GetLine : MapBuilderNode : doStart {
		mapreq.addObjectColor(lineDataType, "blue");
	}
	
	$nodeclass MapLine : MapBuilderNode(MapBuilderRequest :: worldMap) : doStart{
		mapreq.addObjectColor(lineDataType, "blue");
		mapreq.pursueShapes=true;
	}
	$nodeclass InOrOut : VisualRoutinesStateNode : doStart {
		int totLine = 0;
		int totOnRight = 0;
		int totOnLeft = 0;
		int cross = 0;
		bool hasLongLine = false;
		bool determined = false;
		NEW_SHAPE(vertiLine, LineData, new LineData(camShS, Point(320, 0), Point(320, 480)));
		vertiLine->setColor("red");
		//get lineds
		NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
		SHAPEVEC_ITERATE(lines, LineData, myLine)
			if(myLine->getColor() != rgb(200, 64, 64)){//not the artificial vertical mark line
				totLine ++;
				//check if this line is a long line
				if(myLine->getLength() > 150)
					hasLongLine = true;
				if(myLine->intersectsLine(vertiLine)){
					cross ++;
				}else if(myLine->getCentroid().coordX() < 320.0){
					totOnLeft ++;
				}else{
					totOnRight ++;
				}
			}
			cout<<"id is: "
				<<myLine->getId()
				<<" x:"
				<<myLine->getCentroid().coordX()
				<<" y:"
				<<myLine->getCentroid().coordY()
				<<" color: "
				<< myLine->getColor()
				<<endl;
		END_ITERATE;
		cout<<"total: "
			<<totLine
			<<endl;	
		//determine
		if(totLine > 0){
			if(!hasLongLine){
				cout << "inside" << endl;
				determined = true;
			}
			else if(totLine >= 5){//more than 5 lines, must be outside
				cout << "outside" << endl;
				determined = true;
			}else if(cross==0 && (totOnRight == 0 || totOnLeft == 0)){//all lines on the same side of the screen
				if(totLine >= 3){//more than 3 lines on the same side, outside
					cout << "outside" << endl;
					determined = true;
				}else if(totLine == 1){//see only one line, must be inside
					cout << "inside" << endl;
					determined = true;
				}else{//two lines, check the angle between them. if acute, then outside
					Shape<LineData> myLine1 = lines[0];
					Shape<LineData> myLine2 = lines[1];
					if(myLine1->intersectsLine(myLine2)){
						Point inter = myLine1->intersectionWithLine(myLine2);
						//define the mid-point connection as a new line
						NEW_SHAPE(midconn, LineData, new LineData(camShS, myLine1->getCentroid(), myLine2->getCentroid()));
						NEW_SHAPE(interconn, LineData, new LineData(camShS, inter, midconn->getCentroid()));
						if((interconn->getLength() > midconn->getLength()/2) && (interconn->getLength() < midconn->getLength())){
							//acute angle
							cout << "outside" << endl;
							determined = true;
						}
					}
				}
			}
		}
		if(!determined){//post a continue signal
			postStateSignal<bool> (determined);	
		}else{
			postStateCompletion();
		}
	}
	

	$setupmachine {
		GetLine =C=> InOrOut=S<bool>(false)=> Turn(mathutils::deg2rad(45.0))
			=C=> GetLine=C=> InOrOut=S<bool>(false)=> Turn(mathutils::deg2rad(45.0))
			=C=> GetLine=C=> InOrOut=S<bool>(false)=> Turn(mathutils::deg2rad(45.0))
			=C=> GetLine=C=> InOrOut=S<bool>(false)=> Turn(mathutils::deg2rad(45.0))
			=C=> GetLine=C=> InOrOut=S<bool>(false)=> Turn(mathutils::deg2rad(45.0))
			=C=> GetLine=C=> InOrOut=S<bool>(false)=> Turn(mathutils::deg2rad(45.0))
			=C=> GetLine=C=> InOrOut=S<bool>(false)=> Turn(mathutils::deg2rad(45.0))
			=C=> GetLine=C=> InOrOut=S<bool>(false)=> Turn(mathutils::deg2rad(45.0))
			=C=> GetLine=C=> InOrOut=S<bool>(false)=> SpeechNode("I guess it's inside.")
	}
}



REGISTER_BEHAVIOR(Lab4Q4);