#include "Behaviors/StateMachine.h"

using namespace DualCoding;
$nodeclass ShellGame : 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(int choice) : VisualRoutinesStateNode : doStart {
	static int leftnum;
	static int centernum;
	static int rightnum;
	static int *choicenum;
	static int numdiff;
	std::cout<<"new sketch frame - "<<choice<<endl;
	camSkS.clear();
	NEW_SKETCH(camFrame, uchar, sketchFromSeg());
	NEW_SKETCH(rawY, uchar, sketchFromRawY());
	

	NEW_SHAPE(line1, LineData, 
		new LineData(camShS, Point(camFrame->getWidth()/3 , 0), Point(camFrame->getWidth()/3 , camFrame->getHeight() ) ));
	NEW_SHAPE(line2, LineData, 
		new LineData(camShS, Point(camFrame->getWidth()*2/3 , 0), Point(camFrame->getWidth()*2/3 , camFrame->getHeight() ) ));

			NEW_SKETCH(left, bool, visops::leftHalfPlane(line1));
			left->setColor("green");
			//left
			NEW_SKETCH(center, bool, visops::leftHalfPlane(line2)&visops::rightHalfPlane(line1));
			center->setColor("blue");
			//center
			NEW_SKETCH(right, bool, visops::rightHalfPlane(line2));
			right->setColor("pink");
			//right
	
	Sketch<bool> choicemask;
	
	switch (choice){
		case 1:
			choicemask=left;
			choicenum = &leftnum;
		break;
		
		case 2:
			choicemask=center;
			choicenum = &centernum;
		break;
	
		case 3:
			choicemask=right;
			choicenum =&rightnum;
		break;
	}

	NEW_SKETCH(orange_stuff, bool, visops::colormask(camFrame,"orange"));
	NEW_SKETCH(oranges, uint, visops::areacc(orange_stuff));
	NEW_SKETCH(pink_stuff, bool, visops::colormask(camFrame,"pink"));
	NEW_SKETCH(pinks, uint, visops::labelcc(pink_stuff));
	NEW_SKETCH(pink_masked, bool, visops::mask(pink_stuff,choicemask));
	NEW_SKETCH(areaa ,uint, visops::areacc(pink_masked));
	
	numdiff=*choicenum - areaa->max();	
	*choicenum = areaa->max();

		std::cout<<"Difference: "<<numdiff<<endl
			 <<"leftnum - "<<leftnum<<endl
			 <<"centernum - "<<centernum<<endl
			 <<"rightnum - "<<rightnum<<endl
			 <<"choicenum - "<<*choicenum<<endl
			 <<"Pinkarea - "<< areaa->max()<<endl;	
	
	if(numdiff > 50){ 
		if(oranges->max()>50)
			postStateSignal<int>(1);
			//tried and won
		else
			postStateSignal<int>(2);
			//tried and failed
	}
	else
		postStateSignal<int>(0);
		//nothing happened
	
	}

$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{
	
	headinitialize: HeadPointerNode[ getMC()->lookAtPoint(500,0,-400);
					 getMC()->setMaxSpeed(PanOffset,0.2); 
					 getMC()->setMaxSpeed(TiltOffset,0.2); ] =C=> 
		SpeechNode($,"START GAME")=C=>
		random3: StateNode =RND=> {centerr,rightt,leftt} 
		
		 leftt: SpeechNode($,"LEFT") =C=> ArmLeft =C=> setupp1
		 leftt=T(5000)=>setupp1
		 centerr: SpeechNode($,"CENTER") =C=> ArmCenter =C=> setupp2
		 centerr=T(5000)=>setupp2
	  	 rightt: SpeechNode($,"RIGHT") =C=> ArmRight =C=> setupp3
		 rightt=T(5000)=>setupp3

	setupp1: SetupViss($,1) =S<int>(0)=>setupp1
	setupp1=T(1000)=>setupp1
	setupp1=S<int>(1)=>SpeechNode($, "YAY") =C=>win
	setupp1=S<int>(2)=>SpeechNode($, "BOO HOO") =C=> random3
	
	setupp2: SetupViss($,2) =S<int>(0)=>setupp2
	setupp2=T(1000)=>setupp2
	setupp2=S<int>(1)=>SpeechNode($, "YAY")	=C=>win
	setupp2=S<int>(2)=>SpeechNode($, "BOO HOO") =C=>random3

	setupp3: SetupViss($,3) =S<int>(0)=>setupp3
	setupp3=T(1000)=>setupp3
	setupp3=S<int>(1)=>SpeechNode($, "YAY")	=C=>win
	setupp3=S<int>(2)=>SpeechNode($, "BOO HOO") =C=> random3
	
	win: SpeechNode($,"Play Again?")=C=>StateNode =TM("yes")=>headinitialize
	}
}		

REGISTER_BEHAVIOR( ShellGame );