#include "Behaviors/StateMachine.h"

$nodeclass Lab6Q5 : VisualRoutinesStateNode {
	$nodeclass CheckCan : VisualRoutinesStateNode : doStart{
		uint const threshold = 300;
		camSkS.clear();
		NEW_SKETCH(camFrame, uchar, sketchFromSeg());
		NEW_SKETCH(red_stuff, bool, visops::colormask(camFrame, "red"));
		NEW_SKETCH(r_cc, uint, visops::labelcc(red_stuff));
		NEW_SKETCH(r_area, uint, visops::areacc(r_cc));
		
		//check by area
		if(r_area->max() < threshold){
			//post failure
			postStateFailure();
		}else{
			postStateSuccess();
		}
	}
	$setupmachine {
		launch: StateNode =N=> getReady
		getReady: DynamicMotionSequenceNode[setFile("armready.mot");]
			=C=>DynamicMotionSequenceNode[setFile("headready.mot");]
			=C=>grasp
		grasp: DynamicMotionSequenceNode[setFile("release.mot");] 
			=C=> DynamicMotionSequenceNode[setFile("armdown.mot");] 
			=C=> DynamicMotionSequenceNode[setFile("hold.mot");] 
			=C=> DynamicMotionSequenceNode[setFile("armup.mot");]
			=C=> check
		//there will be a check node here
		check: CheckCan
		check =S=> turn
		check =F=> SpeechNode("put the can back") =C=> failPutBack	

		turn: DynamicMotionSequenceNode[setFile("turn.mot");]
			=C=> putBack
		turnBack: DynamicMotionSequenceNode[setFile("turnback.mot");]
		putBack: DynamicMotionSequenceNode[setFile("armdown.mot");]
			=C=> DynamicMotionSequenceNode[setFile("release.mot");]
			=C=> DynamicMotionSequenceNode[setFile("armup.mot");]
			=C=> turnBack
			=C=> grasp
		
		failPutBack: DynamicMotionSequenceNode[setFile("armdown.mot");]
			=T(5000)=> grasp 
		
		dock: DynamicMotionSequenceNode[setFile("dock.mot");]
		
		

		worker: DynamicMotionSequenceNode[setFile("armready.mot");] 
		=C=> DynamicMotionSequenceNode[setFile("release.mot");]
		=C=> DynamicMotionSequenceNode[setFile("armdown.mot");]
		=C=> DynamicMotionSequenceNode[setFile("hold.mot");]
		=C=> DynamicMotionSequenceNode[setFile("armup.mot");]
		=C=> DynamicMotionSequenceNode[setFile("turn.mot");]
		=C=> DynamicMotionSequenceNode[setFile("armdown.mot");]
		=C=> DynamicMotionSequenceNode[setFile("release.mot");]
		=C=> DynamicMotionSequenceNode[setFile("armup.mot");]
		=C=> DynamicMotionSequenceNode[setFile("dock.mot");]
		=C=> worker
	}	

}

REGISTER_BEHAVIOR(Lab6Q5);