#ifndef __Boundary_H_
#define __Boundary_H_

#include "ConvexHull.h"

#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"

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

typedef struct {
    Point objPt;
    Point targetPt;
} ManipulationData;

extern Point ball_pos;
extern Point target_pos;
Point next_pos;

class HerdingBuildLMap : public VisualRoutinesStateNode {
public:
	HerdingBuildLMap() : VisualRoutinesStateNode("HerdingBuildLMap") {}
	
	void DoStart() {
		//**************** Set up map builder request ****************
		const int pink_index = ProjectInterface::getColorIndex("pink");
		const int blue_index = ProjectInterface::getColorIndex("blue");
		const int orange_index = ProjectInterface::getColorIndex("orange");

        localShS.clear();
        localSkS.clear();
	
        //localSkS.setTmat(0.2, -50, -10);

		MapBuilderRequest req(MapBuilderRequest::localMap);
		
		req.maxDist = 10000;
        req.numSamples = 10;
//			req.pursueShapes = true;
		
		req.objectColors[ellipseDataType].insert(blue_index);
		req.objectColors[ellipseDataType].insert(orange_index);
		
		req.objectColors[lineDataType].insert(pink_index);
		
		mapBuilder.executeRequest(this,req);
	}
};

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

    float getDist(Point p) {
        float x = p.coordX();
        float y = p.coordY();

        return sqrt(x * x + y * y);
    }
	
    
    /*
     * Note: Had to make this function return boolean instead of Shape<EllipseData> 
     * because of cases that should return NULL, which eventually causes the program to segfault.
     */
    bool getClosestIncorrectBall(Shape<EllipseData> &saved, 
                                 Shape<PolygonData> boundaries, 
                                 vector<Shape<EllipseData> > balls,
                                 bool correctValue) {
        int minIdx = -1, i=0;
        float minDist = 1000000.;
        
        SHAPEVEC_ITERATE(balls, EllipseData, e) 
            if (boundaries->isInside(e->getCentroid())!= correctValue) {
                float dist = getDist(e->getCentroid());

                if (dist < minDist) {
                    minDist = dist;
                    minIdx = i;
                }
                
            }
            i++;
        END_ITERATE;

        if (minIdx == -1)
            return false;

        saved = balls[minIdx];        
        return true;
    }

    bool isFirstCloserThanSecond (Point p1, Point p2) {
        float dist1 = getDist(p1);
        float dist2 = getDist(p2);
        
        if (dist1<dist2)
            return true;
        else
            return false;
    }

    bool getManipulationData(ManipulationData &saved,
                             Shape<PolygonData> &border, 
                             vector<Shape<EllipseData> > balls) {
        
        NEW_SHAPEVEC(oranges, EllipseData, subset(balls, IsColor("orange")));
        NEW_SHAPEVEC(blues, EllipseData, subset(balls, IsColor("blue")));
        
        Shape<EllipseData> closestOrange;
        bool gotOrange = getClosestIncorrectBall(closestOrange, border, oranges, true);
        Shape<EllipseData> closestBlue;
        bool gotBlue = getClosestIncorrectBall(closestBlue, border, blues, false);

        Point ballCenterPt;                            
        bool shouldBeInside;

        if (!gotOrange && !gotBlue)
            return false;
        
        if (gotBlue && gotOrange) {
            shouldBeInside = isFirstCloserThanSecond(closestOrange->getCentroid(), closestBlue->getCentroid());

            if (shouldBeInside) 
                ballCenterPt = closestOrange->getCentroid();
            else
                ballCenterPt = closestBlue->getCentroid();
        }
        else if (gotBlue) {
            ballCenterPt = closestBlue->getCentroid();
            shouldBeInside = false;    
        }
        else {
            ballCenterPt = closestOrange->getCentroid();
            shouldBeInside = true;
        }

        Point targetPt;
        float minDist = 10000000.;
        int minIdx; // debug purpose

//        if (shouldBeInside) { // Should hit things into the region
            vector<Point> polygonPoints = border->getVertices();
            for (unsigned int i = 0; i < polygonPoints.size(); i++) {
                cout << "Point (" << polygonPoints[i].coordX() << ", " << polygonPoints[i].coordY() << endl;

                float dist = ballCenterPt.xyDistanceFrom(polygonPoints[i]);
                if (dist < minDist) {
                    minDist = dist;
                    targetPt = polygonPoints[i];
                    minIdx = i;
                }
            }
            
            // If the target is supposed to moved out of the region, the direction doesn't really matter
            // (we can always hit again!)
            
            vector<LineData> polygonLines = border->getEdges();
            for (unsigned int i = 0; i < polygonLines.size(); i++) {
                cout << "Line " << i << ": (" << polygonLines[i].firstPt().coordX() << "," << polygonLines[i].firstPt().coordY() << ") -> (";
                cout << polygonLines[i].secondPt().coordX() << "," << polygonLines[i].secondPt().coordY() << ")" << endl;
    
                const float PI = 3.141592;
                AngPi newOrientation = polygonLines[i].getOrientation() + AngPi(PI/2.);
    
                // Debug Purpose
                // TODO:: DELETE THIS LINE
    //            NEW_SHAPE(perp, LineData, new LineData(polygonLines[i].getSpace(), ballCenterPt, newOrientation));
    
                LineData perpLine(polygonLines[i].getSpace(), ballCenterPt, newOrientation);
                bool onBorder, onPerp;
                Point intersection = polygonLines[i].intersectionWithLine(perpLine, onBorder, onPerp);
    
                if (shouldBeInside && !onBorder)
                    continue;
                                 
                float dist = ballCenterPt.xyDistanceFrom(intersection);
                if (dist < minDist) {
                    minDist = dist;
                    targetPt = intersection;
                    minIdx = i;
                }
    
                cout << "Intersection at (" << intersection.coordX() << "," << intersection.coordY() << "), onthis = " << onBorder << ", onPerp = " << onPerp << ", dist: " << dist << endl;
            }
//        }

//        else {
//            vector <LineData
//            for (unsigned int i = 0; i<
//        }



        cout << "Closest target is at " << minIdx << ":(" << targetPt.coordX() << ", " << targetPt.coordY() << " with distance " << minDist << endl;

        saved.objPt = ballCenterPt;
        saved.targetPt = targetPt;

        NEW_SHAPE(target, PointData, new PointData(localShS, targetPt));
    
        return true;
    }

    Point getNextWayPoint(Shape<PolygonData> border) {
        Point retVal;
        Point borderCenter = border -> getCentroid();
        vector<Point> polygonPoints = border->getVertices();

        NEW_SHAPE(border_center, PointData, new PointData(localShS, borderCenter));

        float minDist = 1000000.;
        int minIdx;

        for (unsigned int i = 0; i < polygonPoints.size(); i++) {
            float dist = getDist(polygonPoints[i]);
            if (dist < minDist) {
                minDist = dist;
                minIdx = i;
            }
        }

        minIdx = (minIdx + 1) % polygonPoints.size();

        NEW_SHAPE(polypoint, PointData, new PointData(localShS, polygonPoints[minIdx]));

        retVal = polygonPoints[minIdx];

        Point vectorFromCenter = retVal - borderCenter;
        float vectorLength = getDist(vectorFromCenter);

        float distanceAway = 350.;

        cout << distanceAway << ", " << vectorLength << endl;
        
        retVal += (vectorFromCenter * (distanceAway / vectorLength));
        
        return retVal;
    }

	void DoStart() {
		cout << "** Begin boundary construction\n";

        NEW_SHAPEVEC(balls, EllipseData, select_type<EllipseData>(localShS));

		NEW_SHAPEVEC(lines_vec, LineData, select_type<LineData>(localShS));
        vector<Point> pts;

		SHAPEVEC_ITERATE(lines_vec, LineData, l)
            pts.push_back(l->firstPt());
            pts.push_back(l->secondPt());
		END_ITERATE;
		
        NEW_SHAPE(border, PolygonData, MyPolygonData::ConvexHull(pts, localShS, localSkS.getTmatinv()));

        ManipulationData data = {};
        bool dataResult = getManipulationData(data, border, balls);

        ball_pos = data.objPt;
        target_pos = data.targetPt;
        next_pos = getNextWayPoint(border);
        NEW_SHAPE(nextPosition, PointData, new PointData(localShS, next_pos));

		cout << "** Done processing the image" << endl;
        postStateCompletion();
	}
};

#endif