#include "Behaviors/StateMachine.h"

$nodeclass Lab6p4 {
        $nodeclass FindShapes : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
                mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "green");
        }

	$nodeclass Verify : doStart {
		NEW_SHAPEVEC(ellipse_shapes, EllipseData,select_type<EllipseData>(camShS));

		//find can based on whether or not there is an ellipse of expected size and color
		bool foundCan = false;

		SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myellipse) {
			//The can should consistantly be an ellipse with smaj = 39 and smin = 27
			int sMaj = myellipse->getSemimajor();
			int sMin = myellipse->getSemiminor();
			//check if it is within 3 of these parameters
			if ((sMaj>36 && sMaj < 43) && (sMin > 24 && sMin < 30)) {
				foundCan = true;
			}
		} END_ITERATE;

		cout << "here" << endl;
		if (!foundCan) {	
			//the can wasn't found, post failure
			postStateFailure();
		}

		else {
			//the can was found, post success
			postStateSuccess();
		}
		postStateCompletion();
	}
  
	$setupmachine{
		adjustCamera : DynamicMotionSequenceNode("adjustCamera.mot")
		flip1 : DynamicMotionSequenceNode("flip1.mot")
		flip2 : DynamicMotionSequenceNode("flip2.mot")
		wait : SpeechNode("I can't find my can. Please put it back :-(") =T(5000)=> flip1
		verifier : Verify
		adjustCamera =C=>
			flip1 =C=> 
				FindShapes =C=>
					verifier =S=> flip2 
					verifier =F=> wait
						flip2 =C=>
							flip1
  	}

}

REGISTER_BEHAVIOR(Lab6p4);