#include "Behaviors/StateMachine.h"
$nodeclass AprilEllipses : VisualRoutinesStateNode{
 $nodeclass FindAprilTags : MapBuilderNode($,MapBuilderRequest::cameraMap) : constructor {
    mapreq.clearCamera=false;
    mapreq.setAprilTagFamily();
   }
  $nodeclass FindEllipses : MapBuilderNode : doStart{
		mapreq.addAllObjectColors(ellipseDataType);
	}
  $nodeclass Sides : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
    NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(camShS));
		if(ellipses.size()<=0)
		{
			cout << "No ellipses found" << endl;
		}		
		if(tags.size() <= 0)
		{
			cout << "No April Tags found" << endl;
		}
		else
		{
			NEW_SHAPE(line1, LineData, new LineData(camShS, tags[0]->getTopLeft(), tags[0]->getBottomLeft()))
			 NEW_SHAPE(line2, LineData, new LineData(camShS, tags[0]->getTopRight(), tags[0]->getBottomRight()))
			int left = 0, right = 0;
			SHAPEVEC_ITERATE(ellipses, EllipseData, myellipses)
				if(line1->pointIsLeftOf(myellipses->getCentroid()))
				{
					left = left+1;
				}
				 if(line2->pointIsRightOf(myellipses->getCentroid()))
                                {       
                                        right = right+1;
                                }

			END_ITERATE;
			if(left!=0 && right!=0)
			{
				cout << "This is mixed" << endl;
				cout << "Left: " << left << endl;
				cout << "Right: " << right << endl;
			}
			else if( left!=0 )
			{
				cout << "They are to the left" << endl;
				cout << "Left: " << left << endl;
				cout << "Right: " << right << endl;
			}
			else
			{
				cout << "They are to the right" << endl;
				cout << "Left: " << left << endl;
				cout << "Right: " << right << endl;
			}
		}
  }

  $setupmachine{
    FindEllipses=C=>FindAprilTags=MAP=>Sides
  }
}
REGISTER_BEHAVIOR(AprilEllipses);