#ifndef __NON_CONVEX_BOUNDARIES_H
#define __NON_CONVEX_BOUNDARIES_H

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

class BuildMap : public VisualRoutinesStateNode {
public:
    BuildMap() : VisualRoutinesStateNode("BuildMap") {}
  
    virtual void DoStart() {
        camSkS.clear();
        camShS.clear();
        
        cout << "Begin building board\n";
        //**************** 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.pursueShapes = false;

        req.objectColors[ellipseDataType].insert(blue_index);
        req.objectColors[blobDataType].insert(green_index);    
        
        req.minBlobAreas[blue_index] = 50;
        req.minBlobAreas[green_index] = 50;
        
        req.objectColors[lineDataType].insert(green_index);
        req.occluderColors[lineDataType].insert(blue_index);
    
        req.immediateRequest = true;

        mapBuilder.executeRequest(req);        

        postStateCompletion();
    }
};

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

    /* Get the points of the enclosing polygon in order */
    void getPolygon(vector<Point> firstEndpts,
                    vector<Point> secondEndpts,
                    vector<Point> blobCentroids,
                    vector <Point> &polygon) {
    
        int lsize = firstEndpts.size(), bsize = blobCentroids.size();
        int size = firstEndpts.size() + blobCentroids.size();
        int linesRemoved = 0, blobsRemoved = 0;

        int fromLines;
        int curIndex = 0, whichPt;
        float dist, minDist;

        if (size == 0)
            return;

        /* Take a line or a centroid and insert it as a starting point */
        if (lsize) {
            polygon.push_back(firstEndpts[curIndex]);
            polygon.push_back(secondEndpts[curIndex]);
        
            firstEndpts.erase(firstEndpts.begin()+curIndex);
            secondEndpts.erase(secondEndpts.begin()+curIndex);
            linesRemoved++;
        }
        else {
            polygon.push_back(blobCentroids[curIndex]);
            blobCentroids.erase(blobCentroids.begin()+curIndex);
            blobsRemoved++;
        }                  

        /* Keep adding closest line / centroids until there's nothing left */
        for (int i = 0; i < size-1; i++) {
            minDist = 100000000;
            whichPt = 0;
            fromLines = 1;

            /* Find the closest line by comparing the newest point added against two endpoints */
            for (int j = 0; j < lsize-linesRemoved; j++) {
                dist = polygon.back().xyDistanceFrom(firstEndpts[j]);
                if (dist < minDist) {
                    minDist = dist;
                    curIndex = j;
                    whichPt = 0;
                }
                
                dist = polygon.back().xyDistanceFrom(secondEndpts[j]);
                if (dist < minDist) {
                    minDist = dist;
                    curIndex = j;
                    whichPt = 1;    
                }
            }

            /* Find the closest centroid */
            for (int j = 0; j < bsize - blobsRemoved; j++ ) {
                dist = polygon.back().xyDistanceFrom(blobCentroids[j]);
                if (dist < minDist) {
                    minDist = dist;
                    curIndex = j;
                    fromLines = 0;
                }
            }

            /* Add a line / a centroid accordingly */
            if (fromLines) {
                if (whichPt) {
                    polygon.push_back(secondEndpts[curIndex]);
                    polygon.push_back(firstEndpts[curIndex]);
                }
                else {
                    polygon.push_back(firstEndpts[curIndex]);
                    polygon.push_back(secondEndpts[curIndex]);                    
                }

                linesRemoved ++ ;
                firstEndpts.erase(firstEndpts.begin()+curIndex);
                secondEndpts.erase(secondEndpts.begin()+curIndex);
            }
            else {
                blobsRemoved ++ ;
                polygon.push_back(blobCentroids[curIndex]);
                blobCentroids.erase(blobCentroids.begin()+curIndex);
            }
        }
        return;
    }

	virtual void DoStart() {
        cout << "Begin procedure\n";
        
        /* Take all ellipses, lines, and blobs */
        NEW_SHAPEVEC(blobs, EllipseData, select_type<EllipseData>(camShS));
        NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
        NEW_SHAPEVEC(shortlines, BlobData, select_type<BlobData>(camShS));
        
        NEW_SKETCH_N(sline_sk, bool, visops::zeros(camSkS));
        NEW_SKETCH_N(lines_sk, bool, visops::zeros(camSkS));

        vector <Point> firstEndpts;
        vector <Point> secondEndpts;
        vector <Point> blobCentroids;
        
        /* Create two vectors of endpoints s.t. points at the same index belongs to the same line */
        SHAPEVEC_ITERATE(lines, LineData, l)
            lines_sk |= l->getRendering();
            firstEndpts.push_back(l->firstPt());
            secondEndpts.push_back(l->secondPt());
        END_ITERATE;

        /* Filter out blobs that're already recognized as lines */
        SHAPEVEC_ITERATE(shortlines, BlobData, b)
            sline_sk = b->getRendering();
            sline_sk &= lines_sk;
            if (sline_sk->max() == 0)
                blobCentroids.push_back(b->getCentroid());
        END_ITERATE;

        vector <Point> polygon;

        /* Get the points of the enclosing polygon */    
        getPolygon(firstEndpts, secondEndpts, blobCentroids, polygon);

        /* Create a polygon and fill the interior */
        NEW_SHAPE(poly, PolygonData, new PolygonData(camShS, polygon, true));
        NEW_SKETCH(polyFilled, bool, visops::fillInterior(poly->getRendering()));

        /* Take all the ellipses to compare against the polygon */
        NEW_SKETCH_N(blobs_sk, bool, visops::zeros(camSkS));
        SHAPEVEC_ITERATE(blobs, EllipseData, b) 
            blobs_sk |= b->getRendering();
        END_ITERATE;
        
        /* Determine which ones are inside, which ones are not */
        NEW_SKETCH(inside, bool, blobs_sk & polyFilled);
        NEW_SKETCH(outside, bool, blobs_sk & !polyFilled);
    
        cout << "Done processing the image" << endl;
    }
};

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