//-*-c++-*-
#ifndef INCLUDED_SwatBehavior_h_
#define INCLUDED_SwatBehavior_h_



#include "Behaviors/Leapers.h"

using namespace std;
using namespace DualCoding;

Shape<BlobData> blobMarker;

//! just a quick little demo behavior which lights up the LED with the same index number as the button which is pushed

#nodeclass SwatBehavior : LeapMachine

	#shortnodeclass SearchForObjects : MapBuilderNode($, MapBuilderRequest::localMap) : DoStart
		NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));
		mapreq.searchArea = gazePoly;
		mapreq.motionSettleTime = 2000;
		mapreq.maxDist = 1e6;
		mapreq.clearShapes = true;
		mapreq.pursueShapes = true;
		mapreq.addObjectColor(blobDataType,"blue");
		mapreq.addObjectColor(blobDataType,"green");
		mapreq.minBlobAreas[ProjectInterface::getColorIndex("blue")] = 100;
		mapreq.minBlobAreas[ProjectInterface::getColorIndex("green")] = 100;
		mapreq.rawY = true;
		
	#shortnodeclass GetLargestBlob : VisualRoutinesStateNode : DoStart
		NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
		if(blobs.empty())
			postStateFailure();
		blobMarker = max_element(blobs, BlobData::AreaLessThan());
		postStateCompletion();
		
	#shortnodeclass IsBlobGreen : StateNode : DoStart
		if(IsColor("green")(blobMarker))
			postStateCompletion();
		else
			postStateFailure();
		
	#shortnodeclass OrientToTarget : StateNode : DoStart
			Point cent = blobMarker->getCentroid();
			float ang = atan2(cent.coordY(), cent.coordX());
			if(abs(ang) < M_PI/18)
				postStateCompletion(0);
			else
				postStateSignal<float>(ang);
	
	#shortnodeclass ApproachTarget: StateNode : DoStart
			Point cent = blobMarker->getCentroid();
			float x = cent.coordX();
			float y = cent.coordY();
			float d = sqrt(x*x + y*y);
			if(d > 350) 
				postStateSignal<float>(d - 300);
			else
				postStateCompletion();

	#nodemethod setup
		#statemachine
		   startnode: SwingArm($, 3000, 0, 0, 0) =T(5000)=> search:SearchForObjects =MAP=> blobGetter: GetLargestBlob =F=> Play($, "whimper.wav") =T(10000)=> startnode
		   
		   blobGetter =C=> colortest:IsBlobGreen =C=> SwingArmRight($, 3000) =T(3500)=> doTurn:OrientToTarget =S<float>=> Turn =C=> search
		   
		   colortest =F=> SwingArmLeft($, 3000) =T(3500)=> doTurn
		   
		   doTurn =C=> approach:ApproachTarget =S<float>=> Forward =C=> search
		   
		   approach =C=>  Speak($, "Hee yah") =N=> swingtest:IsBlobGreen =C=> SwingArmLeft($, 500) =T(2000)=> startnode
		   
		   swingtest =F=> SwingArmRight($, 500) =T(2000)=> startnode
		   
		#endstatemachine
	#endnodemethod

#endnodeclass

#endif