//-*-c++-*-

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

using namespace DualCoding;

#define MAX_FLOAT 1<<15

class BuildMap : public VisualRoutinesStateNode {
public:
    BuildMap() : VisualRoutinesStateNode("BuildMap") {}
  
    virtual void DoStart() {
    
        // set up map builder request
        const int pink_index = ProjectInterface::getColorIndex("pink"); // line color
        const int green_index = ProjectInterface::getColorIndex("green");  // ball color

        MapBuilderRequest req(MapBuilderRequest::cameraMap);
        
        req.objectColors[lineDataType].insert(pink_index);
        //req.objectColors[blobDataType].insert(pink_index);
	req.objectColors[ellipseDataType].insert(green_index);

	req.minBlobAreas[pink_index]=75;

        // execute request
        unsigned int mapreq_id = mapBuilder.executeRequest(req);
        erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::statusETID);
    }

    virtual void processEvent(const EventBase &) {
        postStateCompletion();
    }

};

class ParseMap : public VisualRoutinesStateNode {
public:
    ParseMap() : VisualRoutinesStateNode("ParseMap") {}

    virtual void DoStart() {
        
        NEW_SHAPEVEC(lines,LineData,select_type<LineData>(camShS));
        NEW_SHAPEVEC(ellipses,EllipseData,select_type<EllipseData>(camShS));
	NEW_SHAPEVEC(blobs,BlobData,select_type<BlobData>(camShS));
	

	// construct pointList (vector<Point>)
	std::vector<Point> pointList;

	int line_i = 0;
	int blob_i = 0;

	// loop and remove extra blobs
	SHAPEVEC_ITERATE(lines, LineData, l)
		for(blob_i = 0; (uint)blob_i < blobs.size(); blob_i++) {
			Sketch<bool> sk = (((LineData)l).getRendering() & (BlobData(blobs[blob_i])).getRendering());
			if(!(sk->empty())) {
				blobs.erase(blobs.begin() + blob_i);
				blob_i--;
			}
		}
	END_ITERATE;
	blob_i = 0;

	Point currentPt;
	LineData currentLine(camShS, currentPt, currentPt);
	
	if(lines.size() > 0) {
		currentLine = lines.front();
		pointList.push_back(currentLine.firstPt());
		pointList.push_back(currentLine.secondPt());
		currentPt = currentLine.secondPt();
		lines.erase(lines.begin() + line_i);
	}
	else {
		currentPt = (BlobData(blobs.front())).getCentroid();
		pointList.push_back(currentPt);
		blobs.erase(blobs.begin() + blob_i);
	}

	while(TRUE) {
		int i = 0;
		float min_dist = MAX_FLOAT;
		bool isLine = true;
		bool isFirstPt = false;

		SHAPEVEC_ITERATE(lines, LineData, l)
			float distToFirst = ((LineData)l).firstPt().distanceFrom(currentPt);
			float distToSecond = ((LineData)l).secondPt().distanceFrom(currentPt);

			if(distToFirst < min_dist) {
				min_dist = distToFirst;
				isFirstPt = true;
				currentLine = l;
				line_i = i;
			}
			if(distToSecond < min_dist) {
				min_dist = distToSecond;
				isFirstPt = false;
				currentLine = l;
				line_i = i;
			}
			i++;
		END_ITERATE;
		
		i = 0;
		SHAPEVEC_ITERATE(blobs, BlobData, b)
			float dist = ((BlobData)b).getCentroid().distanceFrom(currentPt);

			if(dist < min_dist) {
				min_dist = dist;
				isLine = false;
				blob_i = i;
			}
			i++;
		END_ITERATE;

		if(min_dist == MAX_FLOAT)
			break;

		if(isLine) {
			if(isFirstPt) {
				pointList.push_back(currentLine.firstPt());
				currentPt = currentLine.secondPt();
				pointList.push_back(currentPt);
			}
			else {
				pointList.push_back(currentLine.secondPt());
				currentPt = currentLine.firstPt();
				pointList.push_back(currentPt);
			}

			lines.erase(lines.begin() + line_i);
		}
		else {
			currentPt = ((BlobData)blobs[blob_i]).getCentroid();
			pointList.push_back(currentPt);
			blobs.erase(blobs.begin() + blob_i);
		}
			

	}

	NEW_SHAPE(region,PolygonData,new PolygonData(camShS,pointList,true,true,true));


        NEW_SKETCH(hull_sketch, bool, region->getRendering());
        hull_sketch = visops::fillInterior(hull_sketch);
        NEW_SKETCH(result, bool, visops::zeros(camSkS));

        SHAPEVEC_ITERATE(ellipses, EllipseData, e)

            NEW_SKETCH(bottom, bool, visops::zeros(camSkS));
            NEW_SKETCH(current_shell, bool, e->getRendering());
            bottom += current_shell & ! current_shell[*camSkS.idxS];
            
            if(!((bottom & hull_sketch)->empty()))
                result |= current_shell;

        END_ITERATE

	}
};


class TagBehavior : public VisualRoutinesStateNode {
public:
    TagBehavior(): VisualRoutinesStateNode("TagBehavior") {}
  
	virtual void setup() {
        #statemachine
	          startnode: BuildMap() =C=> ParseMap()
        #endstatemachine
    }
};