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

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Sound/SoundManager.h"

using namespace DualCoding;

class TagSafezoneBehavior : public VisualRoutinesStateNode {
public:
  TagSafezoneBehavior(): VisualRoutinesStateNode("TagSafezoneBehavior") {}
  
class BuildTagSafezone : public VisualRoutinesStateNode {
public:
  BuildTagSafezone() : VisualRoutinesStateNode("BuildCamMap") {}
  
  virtual void DoStart() {
    
    //**************** Set up map builder request ****************
    const int green_index = ProjectInterface::getColorIndex("green");
    const int blue_index = ProjectInterface::getColorIndex("blue");

    MapBuilderRequest req(MapBuilderRequest::cameraMap);
		req.maxDist = 1500;
		req.numSamples = 1;
		req.clearShapes = true;
		req.minBlobAreas[green_index] = 50;
		req.minBlobAreas[blue_index] = 50;

    req.objectColors[blobDataType].insert(green_index);

    req.objectColors[blobDataType].insert(blue_index);

		req.immediateRequest = true;
		unsigned int mapreq_id = mapBuilder.executeRequest(req);
		postStateCompletion();

  }

};


	class ExamineBoard : public VisualRoutinesStateNode {
	public:
		ExamineBoard() : VisualRoutinesStateNode("ExamineMapLab") {}

		virtual void DoStart() {
    	sndman->playFile("barklow.wav");
    	cout << "MapBuilder found " << camShS.allShapes().size() << " shapes." << endl;

			NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(camShS));
			NEW_SHAPEVEC(bndry, BlobData, subset(blobs, IsColor(ProjectInterface::getColorIndex("green"))));
			NEW_SHAPEVEC(pieces, BlobData, subset(blobs, IsColor(ProjectInterface::getColorIndex("blue"))));
			cout<< "Found " << bndry.size() << " green blobs!" << endl;
			int * visited = new int [bndry.size()];
			int i = 0;
			int j = 0;
			int minIndex = 0;
			for (i = 0; i < bndry.size(); i++) { visited[i] = 0; }
			vector<Point> ptsList;
			Point currentPoint = bndry[0]->getCentroid();
			Point nextPoint = bndry[1]->getCentroid();
			Point tmp;
			ptsList.push_back(currentPoint);
			for (i = 1; i < bndry.size(); i++) {
				for (j = 1; j < bndry.size(); j++) {
					if (visited[j] == 1) continue;
					nextPoint = bndry[j]->getCentroid();
					minIndex = j;
					break;
				}
				for (j = 1; j < bndry.size(); j++) {
					if (visited[j] == 1) continue;
					tmp = bndry[j]->getCentroid();
					if (tmp.distanceFrom(currentPoint) < nextPoint.distanceFrom(currentPoint)) {
						nextPoint = tmp;
						minIndex = j;
					}
				}
				ptsList.push_back(nextPoint);
				visited[minIndex] = 1;
				currentPoint = nextPoint;
			}
			NEW_SHAPE(safezone, PolygonData, new PolygonData(camShS, ptsList, true));
			NEW_SKETCH(result, bool, visops::zeros(camSkS));
			SHAPEVEC_ITERATE(pieces, BlobData, p)
				if(safezone->isInside(p->getCentroid())) {
					result |= p->getRendering();
				}
			END_ITERATE;	
	}

	};
		virtual void setup() {
#statemachine
			startnode: BuildTagSafezone() =C=> ExamineBoard()
#endstatemachine
		}
};

#endif