#include "Behaviors/StateMachine.h"
  
 
 $nodeclass CountEllipses : VisualRoutinesStateNode {
 
      $setupmachine{
 
             FindEllipsesAndLine  =C=> CountEllipsesOnSides =C=> SpeechNode("done")
      }
  
	//only works for blue lines
         $nodeclass FindEllipsesAndLine : MapBuilderNode : doStart {
             	mapreq.addObjectColor(ellipseDataType, "red");
             	mapreq.addObjectColor(ellipseDataType, "red");
             	mapreq.addObjectColor(ellipseDataType, "blue");
		mapreq.addObjectColor(lineDataType, "blue");
		mapreq.addObjectColor(lineDataType, "red");
         }


	$nodeclass CountEllipsesOnSides : VisualRoutinesStateNode : doStart {
		NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
		if (lines.size() ==0)
			cout << "no lines found, aborting" << endl;
		Shape<LineData> theLine = lines[0];
		theLine->setInfinite();
		int numRight = 0;
		int numLeft = 0;
		NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
			SHAPEVEC_ITERATE(ellipses, EllipseData, currellipse)
			if (theLine->pointIsLeftOf(currellipse->getCentroid()))
				numLeft++;
			else numRight++; 

		END_ITERATE;

                cout << "Number of ellipses to the left: " << numLeft << "\n Number of ellipses to the right: " << numRight << endl;


	}
 
}

REGISTER_BEHAVIOR(CountEllipses);