#include "Behaviors/StateMachine.h"

$nodeclass DetectWhetherRepeatCanFlip : StateNode
{
	$nodeclass CanFlipFirst : DynamicMotionSequenceNode : doStart
	{
		update("canflipfirst.mot");
	}

	$nodeclass CanFlipSecond : DynamicMotionSequenceNode : doStart
	{
		update("canflipsecond.mot");
	}

	$nodeclass DoStuff : VisualRoutinesStateNode
	{
		$nodeclass FindCan : MapBuilderNode : doStart
		{
		    mapreq.addObjectColor(ellipseDataType,"red");
		}

		$nodeclass Processing : VisualRoutinesStateNode : doStart
		{
			bool canInHand = false;
			NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
			SHAPEVEC_ITERATE(ellipses, EllipseData, ellipse)
			rgb color = ellipse->getColor();
			if(color.red == 200 && color.green == 64 && color.blue == 64)
				if(ellipse->getArea() > 30)
				{
					Point point = ellipse->getCentroid();
		 			if(point.coordX() > 290 && point.coordX() < 350)
						if(point.coordY() > 220 && point.coordY() < 260)
							canInHand = true;
				}
			END_ITERATE;

			if(canInHand)
			{
				cout << "found can" << endl;
				postParentSuccess();
			}
			else
			{
				cout << "did not find can" << endl;
				postParentFailure();
			}
		}
	
		$setupmachine {
			startnode: FindCan =C=> Processing
		}
	}

	$setupmachine {
		launch: StateNode =N=> rotateFirst
//		rotateLeftFirst: CanFlipLeftFirst =C=> DoStuff
		rotateFirst: CanFlipFirst =C=> visual
		visual: DoStuff
		visual =S=> rotateSecond
		visual =F=> SpeechNode("Please put down a can") =T(5000)=> rotateFirst
		rotateSecond: CanFlipSecond =C=> PostMachineCompletion
	}	
}

REGISTER_BEHAVIOR(DetectWhetherRepeatCanFlip);