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

#include "Behaviors/Leapers.h"

#include <sstream>
#include <string>
using namespace std;

template <class T>
inline std::string to_string (const T& t)
{
   std::stringstream ss;
   ss << t;
   return ss.str();
}

#nodeclass MapBuilderTest: VisualRoutinesStateNode

	#shortnodeclass GetpinkBlobs : VisualRoutinesStateNode : DoStart
	        NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
		NEW_SHAPEVEC(pink_blobs, BlobData, subset(blobs, IsColor("pink")));
	        if(blobs.empty())
	            postStateFailure();
	        postStateSignal<string>("I found " + to_string(pink_blobs.size()) + " pink blobs");

	#shortnodeclass GetgreenBlobs : VisualRoutinesStateNode : DoStart
	        NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
		NEW_SHAPEVEC(green_blobs, BlobData, subset(blobs, IsColor("green")));
	        if(blobs.empty())
	            postStateFailure();
	         postStateSignal<string>("And I found " + to_string(green_blobs.size()) + " green blobs");

	#shortnodeclass FindBlobs : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));
		mapreq.searchArea = gazePoly;
		mapreq.motionSettleTime = 2000;
		mapreq.maxDist = 1e6;
		mapreq.clearShapes = true;
		mapreq.pursueShapes = false;
		mapreq.rawY = true;
		mapreq.addObjectColor(blobDataType, "pink");
		mapreq.addObjectColor(blobDataType, "green");
	
	#shortnodemethod setup
		#statemachine
			startnode: FindBlobs =MAP=> GetpinkBlobs =S<string>=> Speak =T(2000)=> GetgreenBlobs =S<string>=> Speak =B(ChiaraInfo::RedButOffset)=> startnode
		#endstatemachine

#endnodeclass

#endif