#include "Behaviors/StateMachine.h"
#include "Domino.h"

$nodeclass DominoProject : VisualRoutinesStateNode {
	$provide vector<Shape<DominoData> > domino_shapes;

	$nodeclass Look : MapBuilderNode($,MapBuilderRequest::localMap) : constructor {
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(lineDataType, "blue");

		mapreq.minLineLength = 50;
		mapreq.minEllipseSemiMajor = 7;
//		mapreq.minRayLength ?
		mapreq.groundPlaneAssumption = MapBuilderRequest::custom;
		mapreq.customGroundPlane = PlaneEquation (fmat::pack(0, 0, 1), 75);
	}

	$nodeclass Report : doStart {
		$reference DominoProject::domino_shapes;

		vector<Point> points1;
		vector<Point> points2;

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

		if (lines.size() > 0 && ellipses_shapes.size() > 0) {
			SHAPEVEC_ITERATE(lines, LineData, myLine) {
				cout << "Line ID = " << myLine->getId()
					<< ", length = " << myLine->getLength()
					<< ", centroid " << myLine->getCentroid()
					<< ", orientation "<< myLine->getOrientation()
					<< endl;
				if(myLine->getLength() > 65 && myLine->getLength() < 85) { //
					NEW_SHAPE (myBrick, BrickData, new BrickData(localShS,
						fmat::pack(myLine->getCentroid().coordX(), myLine->getCentroid().coordY(), 75./2), //Centroid
						fmat::pack(200./2, 75./2, 75./2), //extents
						fmat::rotationZ(myLine->getOrientation() + (AngPi)(M_PI/2)))); //orientation

					//cout << "orientation = " << fmat::rotationZ(myLine->getOrientation() + (AngPi)(M_PI/2)) << endl;
					//cout << "GFL " << myBrick->getGFL()
					//<< ", GFR " << myBrick->getGFR()
					//<< ", GBL " << myBrick->getGBL()
					//<< ", GBR " << myBrick->getGBR()
		/*			cout << "TFL " << myBrick->getTFL()
					<< ", TFR " << myBrick->getTFR()
					<< ", TBL " << myBrick->getTBL()
					<< ", TBR " << myBrick->getTBR()
					<< endl;
			*/		
					//cout << "myLine->leftPt = " << myLine->leftPt () << ", myLine->rightPt = " << myLine->rightPt () << endl;

					LineData line1(localShS,myBrick->getTFR(),myBrick->getTFL());
					LineData line2(localShS,myBrick->getTBR(),myBrick->getTBL());

					points1.clear();
					points2.clear();

					points1.push_back (line1.getCentroid());
					points1.push_back (myBrick->getTFR());
					points1.push_back (myBrick->getTBR());
					points1.push_back (line2.getCentroid());

					points2.push_back (line1.getCentroid());
					points2.push_back (myBrick->getTFL());
					points2.push_back (myBrick->getTBL());
					points2.push_back (line2.getCentroid());

					NEW_SHAPE(polygon_shape1, PolygonData, new PolygonData(localShS, points1, true));
					NEW_SHAPE(polygon_shape2, PolygonData, new PolygonData(localShS, points2, true));

					int counter1 = 0;
					int counter2 = 0;

					SHAPEVEC_ITERATE(ellipses_shapes, EllipseData, myEllipse) {
						if(polygon_shape1->isInside(myEllipse->getCentroid()))
							counter1++;
						if(polygon_shape2->isInside(myEllipse->getCentroid()))
							counter2++;						
					} END_ITERATE;

					if (counter1 > 0 && counter2 > 0 && counter1 < 7 && counter2 < 7) {
						AngTwoPi dominoOrientation;
						double atan2Result;
						LineData ray(localShS,polygon_shape2->getCentroid(), polygon_shape1->getCentroid());

						//AngTwoPi brickOrientation); //extended orientation for Domino
							//(AngTwoPi)
						if(counter1 >= counter2) {
							atan2Result = atan2(polygon_shape2->getCentroid().coordY() - polygon_shape1->getCentroid().coordY(),
								polygon_shape2->getCentroid().coordX() - polygon_shape1->getCentroid().coordX());
						} else {
							atan2Result = atan2(polygon_shape1->getCentroid().coordY() - polygon_shape2->getCentroid().coordY(),
								polygon_shape1->getCentroid().coordX() - polygon_shape2->getCentroid().coordX());
							ray.setEndPts(polygon_shape1->getCentroid(), polygon_shape2->getCentroid());
						}

						cout << "Ray orientation = " << ray.getOrientation() << endl;
						cout << "atan2Result = " << atan2Result << endl;
						if(atan2Result >= -(M_PI/2) && atan2Result < M_PI/2)
							dominoOrientation = (AngTwoPi) ray.getOrientation() + (AngTwoPi) (M_PI);
						else
							dominoOrientation = (AngTwoPi) ray.getOrientation();
						cout << "dominoOrientation = " << dominoOrientation << endl;

					  domino_shapes.push_back(Shape<DominoData>(localShS,
							min(counter1, counter2),
							max(counter1, counter2),
							fmat::pack(myLine->getCentroid().coordX(),myLine->getCentroid().coordY(), 75./2), //Centroid
							fmat::pack(200./2, 75./2, 75./2), //extents
							fmat::rotationZ(myLine->getOrientation() + (AngPi)(M_PI/2)),
							dominoOrientation));//orientation for brick
//dominoOrientation
//						Shape<PolygonData> polygon_high = counter1 >= counter2 ? polygon_shape1 : polygon_shape2;
//						Shape<PolygonData> polygon_low = counter1 >= counter2 ? polygon_shape1 : polygon_shape2;
						
/*
						if(counter1 >= counter2) {
							if(polygon_shape->getCentroid().isRightOf(myBrick->getCentroid())) {
									if (myLine->getOrientation() >= (AngPi) (M_PI/2))
										cout << "orientation = " << (AngTwoPi)myLine->getOrientation() - (AngTwoPi)(M_PI/2) << endl;
									else
										cout << "orientation = " << (AngTwoPi)myLine->getOrientation() + (AngTwoPi)(M_PI/2) << endl;
								} else {
									if (myLine->getOrientation() >= (AngPi) (M_PI/2))
										cout << "orientation = " << (AngTwoPi)myLine->getOrientation() + (AngTwoPi)(M_PI/2) << endl;
									else
										cout << "orientation = " << (AngTwoPi)myLine->getOrientation() - (AngTwoPi)(M_PI/2) << endl;
								}
						}
*/	
					}
				}
			} END_ITERATE;

			SHAPEVEC_ITERATE(domino_shapes, DominoData, myDomino) {
				cout << "myDomino->getCentroid = " << myDomino->getCentroid() 
				<< ", myDomino->getLowValue = " << myDomino->getLowValue() 
				<< ", myDomino->getHighValue = " << myDomino->getHighValue() << endl;
			} END_ITERATE;
		} else if (lines.size() == 0) {
			cout << "I don't see any lines" << endl;
		} else if (ellipses_shapes.size() == 0)
			cout << "I don't see any ellipses" << endl;
	}	

	$nodeclass LocaltoWorld : MapBuilderNode($,MapBuilderRequest::worldMap) : constructor {
		mapreq.clearLocal = false;
	}

	$setupmachine{
		PostureNode("lookatdominoes.pos") =C=> Look =MAP=> Report =N=> LocaltoWorld
	}
}

REGISTER_BEHAVIOR(DominoProject);