#include "Behaviors/StateMachine.h"

$nodeclass CanInspector : StateNode {

        $setupmachine {

                 inspect : DynamicMotionSequenceNode("graspAndPresentCan.mot")

		//inspect =C=> inspect

		FindRedStuff =C=> inspect =C=> FindRedStuff =C=> check : CheckForCan
		check =S=> DynamicMotionSequenceNode("flipAndReplace.mot") =C=> inspect
		check=F=> SpeechNode("Please put the can back.") =T(5000)=> inspect
        }


	$nodeclass FindRedStuff : VisualRoutinesStateNode : doStart {
		NEW_SKETCH(red_stuff1, bool, visops::colormask(camSkS, "red"));
		postStateCompletion();
	}


	$nodeclass CheckForCan : VisualRoutinesStateNode : doStart {
		//the threshhold of red pixels which indcates
		//that the coke can is in the image

		int thresh = 10;
		int diff = 0;

		/*
		Sketch<bool> red_stuff1 = Sketch<bool>("red_stuff1", camSkS);

		if(!red_stuff1.isValid())
			{
				cout << "invalid" << endl;
					return;
			}
		*/

		NEW_SKETCH(red_stuff2, bool, visops::colormask(camSkS, "red"));

		for(int i = 0; i < red_stuff2.height*red_stuff2.width; i++)
		{
			//if(red_stuff1[i] !=red_stuff2[i])
			if(red_stuff2[i] == true)
				diff++;
		}
		
		if (diff < thresh)
			{
				postStateFailure();
				return;
			}

		postStateSuccess();
	}
}

REGISTER_BEHAVIOR(CanInspector);