#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;
float next_ori;

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("green");

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

        vector<Point> gazePts;
        gazePts.push_back(Point(400,80,-100));
        gazePts.push_back( Point(800, 300, -100));
        gazePts.push_back( Point(1000, 0, -100));
        gazePts.push_back( Point(800,-300, -100));
        gazePts.push_back( Point(400,-80,-100));
        gazePts.push_back( Point(800,0,-100));

        gazePts.push_back( Point(500,-120,-100));
        gazePts.push_back( Point(500,120,-100));

        NEW_SHAPE(gazePoly,PolygonData, new PolygonData(localShS, gazePts, false));


		MapBuilderRequest req(MapBuilderRequest::localMap);
		req.removePts = false;
        req.motionSettleTime = 500;
        req.searchArea = gazePoly;
		req.maxDist = 10000;
        req.numSamples = 5;
		
		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 {
private:
    bool shouldMove;

public:
 	BuildBoundary() : VisualRoutinesStateNode("BuildBoundary"), shouldMove(false) {}
 	BuildBoundary(bool sm) : VisualRoutinesStateNode("BuildBoundary"), shouldMove(sm) {}

    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("green")));
        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;
        }

        const float maxDistanceAllowed = 700.;

        if (getDist(ballCenterPt) > maxDistanceAllowed)
            return false;

        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;
        targetPt = ballCenterPt - Point(0, -1);
        saved.targetPt = targetPt;

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

    Point getNextWayPoint(Shape<PolygonData> border, float &adjustAngle) {
        Point retVal;
        Point borderCenter = border -> getCentroid();
        vector<LineData> polygonLines = border->getEdges();

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

        float minDist = 1000000.;
        int minIdx = -1;
        float thresh = 100.;

/*        while(minIdx)
        {*/
            for (unsigned int i = 0; i < polygonLines.size(); i++) {
               float dist = getDist(polygonLines[i].getCentroid());
    /*
               if (dist < thresh)
                   continue;
*/
               if (dist < minDist) {
                   minDist = dist;
                   minIdx = i;
               }
            }
/*
            thresh -= 10.;
            if (thresh < 0) {
                cout <<  "*** ERROR:: Not finding the next waypoint ****" << endl;
                minIdx = 0;
            }

        }*/

        minIdx = (minIdx + polygonLines.size() - 2) % polygonLines.size();

        NEW_SHAPE(polypoint, PointData, new PointData(localShS, polygonLines[minIdx].getCentroid()));

        retVal = polygonLines[minIdx].getCentroid();

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

        float distanceAway = 200.;

        cout << distanceAway << ", " << vectorLength << endl;
        
        retVal += (vectorFromCenter * (distanceAway / vectorLength));
        
        ////////////////////////
        Point tmp = borderCenter-retVal;
        adjustAngle = atan2(tmp.coordY(), tmp.coordX());
        ////////////////////////
        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;
        if (shouldMove) {
            next_pos = getNextWayPoint(border, next_ori);
            next_ori -= atan2(next_pos.coordY(), next_pos.coordX());
            NEW_SHAPE (nextPt, PointData, new PointData(localShS, next_pos));
        }

		cout << "** Done processing the image" << endl;

        if (dataResult == false) {
           postStateSignal<bool>(false);
           return;
        }
        postStateCompletion();
	}
};

#endif