#include "Behaviors/StateMachine.h"

using namespace DualCoding;
$nodeclass CenterRightLeft : StateNode {

//pick random third. 
//create a node that looks at masked (mask area gotten by parameter) sketch for blue (and pink) blobs. 
//if there is no blobs in the area, then see if there is orange in the area.
//if there is orange blob in random third success, otherwise try from step one.

$nodeclass SetupViss : VisualRoutinesStateNode : doStart {
	std::cout<<"clear sketch"<<endl;
	camSkS.clear();
	NEW_SKETCH(camFrame, uchar, sketchFromSeg());
	NEW_SKETCH(rawY, uchar, sketchFromRawY());
	NEW_SKETCH(pink_stuff, bool, visops::colormask(camFrame,"pink"));
	NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame,"blue"));
	NEW_SKETCH(pinks, uint, visops::areacc(pink_stuff));
	NEW_SKETCH(blues, uint, visops::areacc(blue_stuff));
		
		if(abs(((int)pinks->max())-((int)blues->max()))<50 )
		;
		else 
			if((pinks->max())>(blues->max()))
			;
			else
			;
	}

$nodeclass ArmCenter : ArmNode : doStart {
	getMC()->setTolerance(.1);
	getMC()->setJoints(-.70, 1.60, -1.0);
	}

$nodeclass ArmLeft : ArmNode : doStart {
	getMC()->setTolerance(.1);
	getMC()->setJoints(1.59, -1.59, 0.0);
	}

$nodeclass ArmRight : ArmNode : doStart {
	getMC()->setTolerance(.1);
	getMC()->setJoints(-1.59, 1.59, 0.0, 0.0, 0.0, 0.0 );
	}

$setupmachine{
	startt: StateNode=N=> {headinitialize, setupp}
	setupp: SetupViss
		 headinitialize: HeadPointerNode[getMC()->lookAtPoint(500,0,-400)] =C=> 
		 centerr: SpeechNode($,"CENTER") =C=> ArmCenter =C=>  
		 SpeechNode($,"RIGHT") =C=> ArmRight =C=> 
		 SpeechNode($,"LEFT") =C=> ArmLeft =C=>
		  SpeechNode($,"MATT IS COOL")=C=>centerr	
	}
}		

REGISTER_BEHAVIOR( CenterRightLeft );