#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"
//#include <MotionPtr.h>

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

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

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

class BuildBoundary: public VisualRoutinesStateNode{
public:
	BuildBoundary() : VisualRoutinesStateNode("BuildBoundary") {}
	
    void camSTest() {
		cout << "** Begin boundary construction\n";

		NEW_SHAPEVEC(lines_vec, LineData, select_type<LineData>(camShS));

        NEW_SKETCH(boundary_lines, bool, visops::zeros(camSkS));
		
		SHAPEVEC_ITERATE(lines_vec, LineData, l)
            cout << "Testing a line... " << l->firstPt().coordX() << "," << l->firstPt().coordY() << "  " << l->secondPt().coordX() << "," << l->secondPt().coordY() << endl;

            NEW_SKETCH(firstPtSk, bool, l->firstPtShape()->getRendering())
            NEW_SKETCH(secondPtSk, bool, l->secondPtShape()->getRendering())

            cout << firstPtSk.width << " " << firstPtSk.height << endl;
            cout << secondPtSk.width << " " << secondPtSk.height << endl;

            boundary_lines |= firstPtSk;
            boundary_lines |= secondPtSk;

		END_ITERATE;
		
        NEW_SHAPE(boundary_border, PolygonData, PolygonData::convexHull(boundary_lines));
		NEW_SKETCH(boundary_inside, bool, visops::fillInterior(boundary_border->getRendering()));

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


	void DoStart() {
//        camSTest();

		cout << "** Begin boundary construction\n";

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

//        NEW_SKETCH(boundary_lines, bool, visops::zeros(localSkS));

		SHAPEVEC_ITERATE(lines_vec, LineData, l)
/*            cout << "Testing a line... " << l->firstPt().coordX() << "," << l->firstPt().coordY() << "  " << l->secondPt().coordX() << "," << l->secondPt().coordY() << endl;

            NEW_SKETCH(firstPtSk, bool, l->firstPtShape()->getRendering())
            NEW_SKETCH(secondPtSk, bool, l->secondPtShape()->getRendering())

            cout << firstPtSk.width << " " << firstPtSk.height << endl;
            cout << secondPtSk.width << " " << secondPtSk.height << endl;

            boundary_lines |= firstPtSk;
            boundary_lines |= secondPtSk;*/
            pts.push_back(l->firstPt());
            pts.push_back(l->secondPt());

		END_ITERATE;
		
        NEW_SHAPE(boundary_border, PolygonData, MyPolygonData::ConvexHull(pts, localShS, localSkS.getTmatinv()));
		NEW_SKETCH(boundary_inside, bool, visops::fillInterior(boundary_border->getRendering()));

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

};

#endif