//#include "Behaviors/StateMachine.h"
#include "Behaviors/Demos/Navigation/PilotDemo.h"
//#include "Planners/Navigation/ShapeSpacePlannerXYTheta.h"

//typedef std::pair<int,int> xyPair;

//$nodeclass ExploreTest6 : VisualRoutinesStateNode {
$nodeclass* ExploreTest6 : PilotDemo {
	typedef std::pair<int,int> xyPair;
	
	$provide std::vector<Point> lookvec;
	$provide unsigned int lookn;
	$provide unsigned int looki;
	$provide bool bturn;
	$provide Point lookpt;
	
	$provide std::vector<Point> pathToNextPos;
	$provide int nMove;
	$provide int iMove;
	
	$provide float cylinderHeight(100.0f);
	$provide float cylinderRadius(50.0f);
	
	$provide float initX(0.0f);
	$provide float initY(0.0f);
	$provide float initTheta(0.0f);
	
	$nodeclass ClearLocal : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		//std::cout << "ClearLocal" << std::endl;
		mapreq.clearLocal = true;
	}
	
	$nodeclass ClearWorld : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		//std::cout << "ClearWorld" << std::endl;
		mapreq.clearWorld = true;
	}
	
	$nodeclass RefreshSketchLocal : StateNode : doStart {
		//std::cout << "RefreshSketcLocal" << std::endl;
		VRmixin::autoRefreshSketchLocal();
	}
	
	$nodeclass RefreshSketchWorld : StateNode : doStart {
		//std::cout << "RefreshSketchWorld" << std::endl;
		VRmixin::autoRefreshSketchWorld();
	}
	
	$nodeclass CopyToWorld : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		std::cout << "CopyToWorld" << std::endl;
		mapreq.clearLocal = false;
		mapreq.clearWorld = false;
	}
	
	$nodeclass PrintNode(std::string s) : StateNode : doStart {
		std::cout << s << std::endl;
		postStateCompletion();
	}
	
	$nodeclass TurnBack : PilotNode(PilotTypes::walk) : doStart {
		std::cout << "TurnBack" << std::endl;
		pilotreq.da = M_PI;
		pilotreq.collisionAction = collisionIgnore;
	}
	
	static float pi2pi(float x) {
		if (x > 4 * M_PI || x < -4 * M_PI) {
			std::cout << "******error: in pi2pi(), the parameter looks wrong: x=" << x << std::endl;
			return 0.0f;
		}
		while (x > M_PI)
			x -= 2.0f * M_PI;
		while (x < -M_PI)
			x += 2.0f * M_PI;
		return x;
	}
	
	//static Shape<PolygonData> fieldOfView(const float maxDist=5000.f) {
	static ShapeRoot fieldOfView(const float maxDist=5000.f) {
		//const float minDist = 5.f;
		//const float maxDist = 5000.f;
		
		const float offset = 10.f;
		const float caml = 0.f;
		const float camr = 639.f;
		const float camt = 0.f;
		const float camb = 479.f;
		
		//std::cout << "------fieldOfView1" << std::endl;
		Point tlp = Point(caml+offset, camt+offset, 0.f, egocentric);
		Point trp = Point(camr-offset, camt+offset, 0.f, egocentric);
		Point blp = Point(caml+offset, camb-offset, 0.f, egocentric);
		Point brp = Point(camr-offset, camb-offset, 0.f, egocentric);
		NEW_SHAPE_N(blpt, PointData, new PointData(camShS, blp));
		NEW_SHAPE_N(brpt, PointData, new PointData(camShS, brp));
		NEW_SHAPE_N(tlpt, PointData, new PointData(camShS, tlp));
		NEW_SHAPE_N(trpt, PointData, new PointData(camShS, trp));
		ShapeRoot tempbl = VRmixin::mapBuilder->projectToLocal(blpt);
		ShapeRoot tempbr = VRmixin::mapBuilder->projectToLocal(brpt);
		ShapeRoot temptl = VRmixin::mapBuilder->projectToLocal(tlpt);
		ShapeRoot temptr = VRmixin::mapBuilder->projectToLocal(trpt);
		Shape<PointData> localblpt = ShapeRootType(tempbl, PointData);
		Shape<PointData> localbrpt = ShapeRootType(tempbr, PointData);
		Shape<PointData> localtlpt = ShapeRootType(temptl, PointData);
		Shape<PointData> localtrpt = ShapeRootType(temptr, PointData);
		//std::cout << "localtlpt: " << localtlpt->getCentroid().getCoords() << std::endl;
		//std::cout << "localtrpt: " << localtrpt->getCentroid().getCoords() << std::endl;
		//std::cout << "localblpt: " << localblpt->getCentroid().getCoords() << std::endl;
		//std::cout << "localbrpt: " << localbrpt->getCentroid().getCoords() << std::endl;
		
		//std::cout << "------fieldOfView2" << std::endl;
		fmat::Column<3> camtl = fmat::rotationZ(-state->outputs[HeadOffset+PanOffset]) * localtlpt->getCentroid().getCoords();
		fmat::Column<3> camtr = fmat::rotationZ(-state->outputs[HeadOffset+PanOffset]) * localtrpt->getCentroid().getCoords();
		fmat::Column<3> cambl = fmat::rotationZ(-state->outputs[HeadOffset+PanOffset]) * localblpt->getCentroid().getCoords();
		fmat::Column<3> cambr = fmat::rotationZ(-state->outputs[HeadOffset+PanOffset]) * localbrpt->getCentroid().getCoords();
		//std::cout << "camtl: " << camtl << std::endl;
		//std::cout << "camtr: " << camtr << std::endl;
		//std::cout << "cambl: " << cambl << std::endl;
		//std::cout << "cambr: " << cambr << std::endl;
		
		//std::cout << "------fieldOfView3" << std::endl;
		float x1 = localblpt->getCentroid().coordX();
		float y1 = localblpt->getCentroid().coordY();
		float d1 = localblpt->getCentroid().xyNorm();
		float x2 = localbrpt->getCentroid().coordX();
		float y2 = localbrpt->getCentroid().coordY();
		float d2 = localbrpt->getCentroid().xyNorm();
		
		if (cambl[0] < 0 || cambr[0] < 0 || d1 > maxDist || d2 > maxDist) {
			//std::cout << "Invalid field of view" << std::endl;
			return ShapeRoot();
		}
		
		float x3 = localtlpt->getCentroid().coordX();
		float y3 = localtlpt->getCentroid().coordY();
		float d3 = localtlpt->getCentroid().xyNorm();
		float x4 = localtrpt->getCentroid().coordX();
		float y4 = localtrpt->getCentroid().coordY();
		float d4 = localtrpt->getCentroid().xyNorm();
		
		if (camtl[0] < 0 || camtr[0] < 0 || d3 > maxDist || d4 > maxDist) {
			//std::cout << "Invalid top points" << std::endl;
			d3 = maxDist;
			d4 = maxDist;
			x3 = x1 * (d3 / d1);
			y3 = y1 * (d3 / d1);
			x4 = x2 * (d4 / d2);
			y4 = y2 * (d4 / d2);
		}
		
		//std::cout << "------fieldOfView4" << std::endl;
		Point localblp = Point(x1, y1, 0.f, egocentric);
		Point localbrp = Point(x2, y2, 0.f, egocentric);
		Point localtlp = Point(x3, y3, 0.f, egocentric);
		Point localtrp = Point(x4, y4, 0.f, egocentric);
		vector<Point> pts;
		pts.push_back(localblp);
		pts.push_back(localbrp);
		pts.push_back(localtrp);
		pts.push_back(localtlp);
		NEW_SHAPE_N(localfov, PolygonData, new PolygonData(localShS, pts, true));
		
		ShapeRoot temp = VRmixin::mapBuilder->importLocalShapeToWorld(localfov);
		return temp;
		//Shape<PolygonData> worldfov = ShapeRootType(temp, PolygonData);
		//return worldfov;
	}
	
	static std::vector<xyPair> boundaryPoints(const Sketch<uint>& dest, uint& mindist, const uint maxval=(uint)-1) {
		SketchSpace &space = dest->getSpace();
		const int width = space.getWidth();
		const int height = space.getHeight();
		std::vector<xyPair> boundaryPt;
		boundaryPt.clear();
		mindist = maxval;
		/*
		{
			int i, j;
			
			i=0; j=0;
			if (dest(i,j)<maxval) {
				if ( dest(i+1,j)==maxval || dest(i,j+1)==maxval || dest(i+1,j+1)==maxval )
				{
					boundaryPt.push_back(xyPair(i,j));
					if (dest(i,j)<mindist) mindist = dest(i,j);
				}
			}
			i=width; j=0;
			if (dest(i,j)<maxval) {
				if ( dest(i-1,j)==maxval || dest(i-1,j+1)==maxval || dest(i,j+1)==maxval )
				{
					boundaryPt.push_back(xyPair(i,j));
					if (dest(i,j)<mindist) mindist = dest(i,j);
				}
			}
			i=0; j=height;
			if (dest(i,j)<maxval) {
				if ( dest(i,j-1)==maxval || dest(i+1,j-1)==maxval || dest(i+1,j)==maxval )
				{
					boundaryPt.push_back(xyPair(i,j));
					if (dest(i,j)<mindist) mindist = dest(i,j);
				}
			}
			i=width; j=height;
			if (dest(i,j)<maxval) {
				if ( dest(i-1,j-1)==maxval || dest(i-1,j)==maxval || dest(i,j-1)==maxval )
				{
					boundaryPt.push_back(xyPair(i,j));
					if (dest(i,j)<mindist) mindist = dest(i,j);
				}
			}
			
			i=0;
			for (j=1; j<height-1; j++) {
				if (dest(i,j)<maxval) {
					if ( dest(i,j-1)==maxval || dest(i+1,j-1)==maxval || 
						 dest(i+1,j)==maxval || 
						 dest(i,j+1)==maxval || dest(i+1,j+1)==maxval )
					{
						boundaryPt.push_back(xyPair(i,j));
						if (dest(i,j)<mindist) mindist = dest(i,j);
					}
				}
			}
			i=width;
			for (j=1; j<height-1; j++) {
				if (dest(i,j)<maxval) {
					if ( dest(i-1,j-1)==maxval || dest(i,j-1)==maxval || 
						 dest(i-1,j)==maxval || 
						 dest(i-1,j+1)==maxval || dest(i,j+1)==maxval )
					{
						boundaryPt.push_back(xyPair(i,j));
						if (dest(i,j)<mindist) mindist = dest(i,j);
					}
				}
			}
			j=0;
			for (i=1; i<width-1; i++) {
				if (dest(i,j)<maxval) {
					if ( dest(i-1,j)==maxval || dest(i+1,j)==maxval || 
						 dest(i-1,j+1)==maxval || dest(i,j+1)==maxval || dest(i+1,j+1)==maxval )
					{
						boundaryPt.push_back(xyPair(i,j));
						if (dest(i,j)<mindist) mindist = dest(i,j);
					}
				}
			}
			j=height;
			for (i=1; i<width-1; i++) {
				if (dest(i,j)<maxval) {
					if ( dest(i-1,j-1)==maxval || dest(i,j-1)==maxval || dest(i+1,j-1)==maxval || 
						 dest(i-1,j)==maxval || dest(i+1,j)==maxval )
					{
						boundaryPt.push_back(xyPair(i,j));
						if (dest(i,j)<mindist) mindist = dest(i,j);
					}
				}
			}
		}
		*/
		for (int i=1; i<width-1; i++)
			for (int j=1; j<height-1; j++)
				if (dest(i,j)<maxval) {
					/*
					if ( dest(i-1,j-1)>=maxval || dest(i,j-1)>=maxval || dest(i+1,j-1)>=maxval || 
						 dest(i-1,j)>=maxval || dest(i+1,j)>=maxval || 
						 dest(i-1,j+1)>=maxval || dest(i,j+1)>=maxval || dest(i+1,j+1)>=maxval )
					{
						boundaryPt.push_back(xyPair(i,j));
						if (dest(i,j)<mindist) mindist = dest(i,j);
					}
					*/
					if ( dest(i-1,j-1)==maxval || dest(i,j-1)==maxval || dest(i+1,j-1)==maxval || 
						 dest(i-1,j)==maxval || dest(i+1,j)==maxval || 
						 dest(i-1,j+1)==maxval || dest(i,j+1)==maxval || dest(i+1,j+1)==maxval )
					{
						boundaryPt.push_back(xyPair(i,j));
						if (dest(i,j)<mindist) mindist = dest(i,j);
					}
					/*
					if ( dest(i,j-1)==maxval || dest(i-1,j)==maxval || dest(i+1,j)==maxval || dest(i,j+1)==maxval )
					{
						boundaryPt.push_back(xyPair(i,j));
						if (dest(i,j)<mindist) mindist = dest(i,j);
					}
					*/
				}
		return boundaryPt;
	}
	
	static bool radiate(const xyPair center, const Sketch<bool>& dest, const Sketch<bool>& obst, Sketch<uint>& dist, Sketch<uint>& xSk, Sketch<uint>& ySk, const uint maxval=(uint)-1) {
		SketchSpace &space = dist->getSpace();
		const int width = space.getWidth();
		const int height = space.getHeight();
		const int ctxiw = center.first;
		const int ctyjh = center.second;
		const uint uctx = (uint)ctxiw;
		const uint ucty = (uint)ctyjh;
		const uint centerdis = dist(ctxiw,ctyjh);
		
		float r[360];
		int theta[5];
		for (int i=0; i<360; i++) r[i]=maxval;
		for (int i=1; i<width-1; i++)
			for (int j=1; j<height-1; j++) {
				if (i==ctxiw && j== ctyjh) continue;
				//else if (obst(i,j)||dest(i,j)) {
				else if (obst(i,j)) {
					if ((i-1==ctxiw && j==ctyjh)) {r[0]=1;continue;}
					if ((i+1==ctxiw && j==ctyjh)) {r[180]=1;continue;}
					if ((i==ctxiw && j-1==ctyjh)) {r[270]=1;continue;}
					if ((i==ctxiw && j+1==ctyjh)) {r[90]=1;continue;}
					float temp = std::sqrt( (j-ctyjh)*(j-ctyjh) + (i-ctxiw)*(i-ctxiw) );
					theta[0] = int(std::atan2(j-ctyjh, i-ctxiw)*180/M_PI+360)%360;
					theta[1] = int(std::atan2(j-ctyjh, i+1-ctxiw)*180/M_PI+360)%360;
					theta[2] = int(std::atan2(j-1-ctyjh, i-ctxiw)*180/M_PI+360)%360;
					theta[3] = int(std::atan2(j-ctyjh, i-1-ctxiw)*180/M_PI+360)%360;
					theta[4] = int(std::atan2(j+1-ctyjh, i-ctxiw)*180/M_PI+360)%360;
					bool area1 = false;
					bool area4 = false;
					for (int k=0; k<5; k++) {
						if (theta[k] >=0 && theta[k] < 90) area1 = true;
						else if (theta[k] >=270 && theta[k] < 360) area4 = true;
					}
					if (area1 && area4) {
						for (int k=0; k<5; k++)
							if (theta[k]<=90) theta[k]+=360;
					}
					int start = 1000;
					int end = -1;
					for (int k=0; k<5; k++) {
						if (start>theta[k]) start = theta[k];
						if (end<theta[k]) end = theta[k];
					}
					for (int k=start; k<=end+1; k++)
						if (r[k%360] > temp) r[k%360] = temp;
				}
			}
		
		bool changed = false;
		for (int i=0; i<width; i++)
			for (int j=0; j<height; j++) {
				if (obst(i,j) || dest(i,j)) continue;
				int thet = int(std::atan2(j-ctyjh, i-ctxiw)*180/M_PI+360)%360;
				float temp = std::sqrt( (j-ctyjh)*(j-ctyjh) + (i-ctxiw)*(i-ctxiw) );
				//if ( temp<r[thet] && dist(i,j)>centerdis+temp && dist(i,j)!=maxval+1) {
				if ( temp<r[thet] && dist(i,j)>centerdis+temp) {
					dist(i,j) = centerdis+temp;
					xSk(i,j) = uctx;
					ySk(i,j) = ucty;
					changed = true;
				}
			}
		
		return changed;
	}

	static Sketch<uint> ebdist(const Sketch<bool>& dest, const Sketch<bool>& obst, Sketch<uint>& xSk, Sketch<uint>& ySk, bool &result, const uint maxdist=(uint)-1, const uint time=3) {
		SketchSpace &space = dest->getSpace();
		const int width = space.getWidth();
		const int height = space.getHeight();
		const uint maxval = (uint)-2;
		
		Sketch<uint> distSk("ebdist("+dest->getName()+","+obst->getName()+")", dest);
		distSk = maxval;
		distSk->setColorMap(jetMapScaled);
		
		xSk = 0;
		ySk = 0;
		
		for (int i=0; i<width; i++)
			for (int j=0; j<height; j++)
				if (dest(i,j)) distSk(i,j)=0;
	
		for (int i=0; i<width; i++)
			for (int j=0; j<height; j++)
				if (obst(i,j)) distSk(i,j)=maxval+1;
		
		bool finish = false;
		uint t = 0;
		uint mindist;
		std::vector<xyPair> boundary = boundaryPoints(distSk, mindist, maxval);
		if (boundary.size() == 0) {
			std::cout << "boundary.size() == 0" << std::endl;
			finish = true;
		}
		else if (mindist >= maxval) {
			std::cout << "mindist >= maxval" << std::endl;
			finish = true;
		}
		while (!finish) {
			bool changed = false;
			for(std::vector<xyPair>::iterator it=boundary.begin(); it!=boundary.end(); it++) {
				//if (radiate(*it, distSk, dest, obst, maxval)) changed = true;
				if (radiate(*it, dest, obst, distSk, xSk, ySk, maxval)) changed = true;
			}
			//for display only
			//for(std::vector<xyPair>::iterator it=boundary.begin(); it!=boundary.end(); it++) {
			//  distSk(it->first, it->second) = maxval+1;
			//}
			//end display
			t++;
			if (!changed) break;
			if (t >= time && time!=0) break;
			boundary = boundaryPoints(distSk, mindist, maxval);
			if (boundary.size() == 0) break;
			else if (mindist >= maxval) break;
		}
		
		std::cout << "Run radiate for " << t << " times." << std::endl;
		if (t == 0) result = false;
		else result = true;
		
		for (int i=0; i<width; i++)
			for (int j=0; j<height; j++)
				if ( distSk(i,j) > maxdist ) distSk(i,j)=maxdist;
		
		return distSk;
	}
	
	$nodeclass InitPos : StateNode : doStart {
		$reference ExploreTest6::initX, ExploreTest6::initY, ExploreTest6::initTheta;
		std::cout << "InitPos" << std::endl;
		
		initX = state->sensors[GPSXOffset];
		initY = state->sensors[GPSYOffset];
		initTheta = state->sensors[GPSHeadingOffset];
		
		postStateCompletion();
	}
	
	$nodeclass InitWorldSkS : VisualRoutinesStateNode : doStart {
		std::cout << "InitWorldSkS" << std::endl;
		
		const float scale = 0.01f;
		//const float worldwidth = 225.f;
		//const float worldheight = 225.f;
		const float worldwidth = VRmixin::worldSkS.getWidth();
		const float worldheight = VRmixin::worldSkS.getHeight();
		worldSkS.setTmat(scale, worldwidth/2, worldheight/2);
		localSkS.setTmat(scale, worldwidth/2, worldheight/2);
		NEW_SKETCH(viewed, bool, visops::zeros(worldSkS));
		//NEW_SKETCH(viewed, uint, visops::zeros(worldSkS));
		NEW_SKETCH(obst, bool, visops::zeros(worldSkS));
		NEW_SKETCH(world, uint, visops::zeros(worldSkS));
		NEW_SKETCH(bound, bool, visops::zeros(worldSkS));
		
		postStateCompletion();
	}
	
	$nodeclass InitWorldShS : VisualRoutinesStateNode : doStart {
		std::cout << "InitWorldShS" << std::endl;
		
		NEW_SHAPE(gPathToNextPos, GraphicsData, new GraphicsData(VRmixin::worldShS));
		
		{
			GET_SHAPE(particles, GraphicsData, worldShS);
			particles->setViewable(false);
			//particles.deleteShape();
		}
		
		/*Point tlp = Point(-10000.f,  10000.f, 0.f, allocentric);
		Point trp = Point( 10000.f,  10000.f, 0.f, allocentric);
		Point blp = Point(-10000.f, -10000.f, 0.f, allocentric);
		Point brp = Point( 10000.f, -10000.f, 0.f, allocentric);
		NEW_SHAPE(topline, LineData, new LineData(worldShS, tlp, trp));
		NEW_SHAPE(bottomline, LineData, new LineData(worldShS, blp, brp));
		NEW_SHAPE(leftline, LineData, new LineData(worldShS, tlp, blp));
		NEW_SHAPE(rightline, LineData, new LineData(worldShS, trp, brp));
		topline->setObstacle(true);
		bottomline->setObstacle(true);
		leftline->setObstacle(true);
		rightline->setObstacle(true);*/
		
		postStateCompletion();
	}
	
	$nodeclass InitWorld : VisualRoutinesStateNode : doStart {
		
		std::cout << "InitWorld" << std::endl;
		
		postStateCompletion();
	}
	
	$nodeclass InitLook : StateNode : doStart {
		$reference ExploreTest6::lookvec;
		$reference ExploreTest6::lookn;
		$reference ExploreTest6::looki;
		$reference ExploreTest6::bturn;
		
		std::cout << "InitLook" << std::endl;
		lookvec.clear();
		
		const float mostUp = M_PI/2;
		//const float mostDown = M_PI/4-M_PI*5/180;
		const float mostDown = M_PI/4;
		const float mostLeft = M_PI/2;
		const float mostRight = -M_PI/2;
		const float vAngle = M_PI/6;
		const float hAngle = M_PI/6;
		const float height = 400.f;
		
		std::vector<Point> ptVec;
		Point ctPt = Point(1500, 0, 0, egocentric);
		ptVec.push_back(ctPt);
		float theta;
		float r;
		for (int i=0; i<(mostUp-mostDown)/vAngle; i++) {
			theta = mostDown + i * vAngle;
			if (theta >= M_PI/2) theta = M_PI/2 - M_PI*5/180;
			r = height * std::tan(theta);
			//std::cout << "v_angle: " << (mostDown + i * vAngle) * 180 / M_PI << ", r: " << r << std::endl;
			Point pt1 = Point(r*std::cos(mostLeft), r*std::sin(mostLeft), 0, egocentric);
			//std::cout << "h_angle: " << mostLeft * 180 / M_PI << ", x: " << r*std::cos(mostLeft) << ", y: " << r*std::sin(mostLeft) << std::endl;
			ptVec.push_back(pt1);
			for (int j=int(mostLeft/hAngle); j>(mostRight/hAngle); j--) {
				float x = r * std::cos(j * hAngle);
				float y = r * std::sin(j * hAngle);
				//std::cout << "h_angle: " << j * hAngle * 180 / M_PI << ", x: " << x << ", y: " << y << std::endl;
				Point pt = Point(x, y, 0, egocentric);
				ptVec.push_back(pt);
			}
			Point pt2 = Point(r*std::cos(mostRight), r*std::sin(mostRight), 0, egocentric);
			//std::cout << "h_angle: " << mostRight * 180 / M_PI << ", x: " << r*std::cos(mostRight) << ", y: " << r*std::sin(mostRight) << std::endl;
			ptVec.push_back(pt2);
		}
		ptVec.push_back(ctPt);
		
		lookvec = ptVec;
		lookn = lookvec.size();
		looki = 0;
		bturn = true;
		
		for (unsigned int j=0; j<lookn; j++) {
			std::cout << lookvec[j] << std::endl;
		}
		
		postStateCompletion();
	}
	
	$nodeclass SetLook : StateNode : doStart {
		$reference ExploreTest6::lookvec;
		$reference ExploreTest6::lookn;
		$reference ExploreTest6::looki;
		$reference ExploreTest6::bturn;
		$reference ExploreTest6::lookpt;
		
		std::cout << "SetLook" << std::endl;
		
		if (looki >= lookn) {
			looki = 0;
			if (bturn) {
				bturn = false;
				postStateSuccess();
				return;
			}
			else {
				bturn = true;
				postStateCompletion();
				return;
			}
		}
		
		lookpt = lookvec[looki];
		looki++;
		postStateFailure();
	}
	
	$nodeclass TakeAPicture : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		$reference ExploreTest6::lookpt;
		
		std::cout << "TakeAPicture" << std::endl;
		const float maxWorldDist = 5500.f;
		
		NEW_SHAPE_N(aheadpt, PointData, new PointData(localShS, lookpt));
		aheadpt->setObstacle(false);
		mapreq.searchArea = aheadpt;
		
		mapreq.maxDist = maxWorldDist;
		//mapreq.pursueShapes = true;
		
		//mapreq.addObjectColor(blobDataType,"red");
		//mapreq.addBlobOrientation("red", BlobData::pillar);
		//mapreq.addMinBlobArea("red", 100);
		//mapreq.addObjectColor(blobDataType,"green");
		//mapreq.addBlobOrientation("green", BlobData::pillar);
		//mapreq.addMinBlobArea("green", 100);
		//mapreq.addObjectColor(blobDataType,"blue");
		//mapreq.addBlobOrientation("blue", BlobData::pillar);
		//mapreq.addMinBlobArea("blue", 100);
		mapreq.addObjectColor(cylinderDataType,"red");
		mapreq.addCylinderHeight("red", 100);
		mapreq.addObjectColor(cylinderDataType,"green");
		mapreq.addCylinderHeight("green", 100);
		mapreq.addObjectColor(cylinderDataType,"blue");
		mapreq.addCylinderHeight("blue", 100);
		
		mapreq.addObjectColor(lineDataType,"black");
		mapreq.addOccluderColor(lineDataType,"red");
		mapreq.addOccluderColor(lineDataType,"green");
		mapreq.addOccluderColor(lineDataType,"blue");
	}
	
	$nodeclass AddObstacles : VisualRoutinesStateNode : doStart {
		std::cout << "AddObstacles" << std::endl;
		const float obstacleDistance = 200.f;
		const float scale = 0.01f;
		
		NEW_SKETCH_N(obstacles, bool, visops::zeros(VRmixin::worldSkS));
		NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(VRmixin::worldShS));
		SHAPEVEC_ITERATE(ellipses, EllipseData, ellipse) {
			if (ellipse->isObstacle()) {
				NEW_SKETCH_N(s1, bool, ellipse->getRendering());
				obstacles |= s1;
			}
		END_ITERATE }
		NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(VRmixin::worldShS));
		SHAPEVEC_ITERATE(blobs, BlobData, blob) {
			if (blob->isObstacle()) {
				NEW_SKETCH_N(s1, bool, blob->getRendering());
				obstacles |= s1;
			}
		END_ITERATE }
		NEW_SHAPEVEC(cylinders, CylinderData, select_type<CylinderData>(VRmixin::worldShS));
		SHAPEVEC_ITERATE(cylinders, CylinderData, cylinder) {
			if (cylinder->isObstacle()) {
				NEW_SKETCH_N(s1, bool, cylinder->getRendering());
				obstacles |= s1;
			}
		END_ITERATE }
		NEW_SHAPEVEC(lines, LineData, select_type<LineData>(VRmixin::worldShS));
		SHAPEVEC_ITERATE(lines, LineData, line) {
			if (line->isObstacle()) {
				NEW_SKETCH_N(s1, bool, line->getRendering());
				obstacles |= s1;
			}
		END_ITERATE }

		NEW_SKETCH_N(dist, float, visops::edist(obstacles));
		NEW_SKETCH_N(cand, bool, dist < obstacleDistance*scale);
		
		GET_SKETCH(obst, bool, worldSkS);
		obst |= cand;
		
		postStateCompletion();
	}
	
	$nodeclass AddFieldOfView : VisualRoutinesStateNode : doStart {
		std::cout << "AddFieldOfView" << std::endl;
		
		ShapeRoot temp = fieldOfView();
		//std::cout << "------AddFieldOfView call fieldOfView() fin" << std::endl;
		if (!temp.isValid()) {
			std::cout << "Invalid field of view" << std::endl;
			postStateCompletion();
			return;
		}
		Shape<PolygonData> worldfov = ShapeRootType(temp, PolygonData);
		worldfov->setColor("red");
		
		Point ctpt = worldfov->getCentroid();
		std::vector<Point> pts = worldfov->getVertices();
		if (pts.size() != 5) {
			std::cout << "Invalid field of view polygon, vertices: " << pts.size() << std::endl;
			postStateCompletion();
			return;
		}
		
		NEW_SHAPE_N(line0, LineData, new LineData(worldShS, ctpt, pts[0]));
		NEW_SHAPE_N(line1, LineData, new LineData(worldShS, ctpt, pts[1]));
		NEW_SHAPE_N(line2, LineData, new LineData(worldShS, ctpt, pts[2]));
		NEW_SHAPE_N(line3, LineData, new LineData(worldShS, ctpt, pts[3]));
		NEW_SKETCH_N(inside0, bool, line0->getRendering());
		NEW_SKETCH_N(inside1, bool, line1->getRendering());
		NEW_SKETCH_N(inside2, bool, line2->getRendering());
		NEW_SKETCH_N(inside3, bool, line3->getRendering());
		
		NEW_SKETCH_N(inside, bool, inside0 | inside1 | inside2 | inside3);
		NEW_SKETCH_N(bound1, bool, worldfov->getRendering());
		NEW_SKETCH_N(bound0, bool, visops::fillin(bound1, 1, 2, 8));
		
		//NEW_SKETCH_N(render, uint, visops::bdist(inside, bound0, (uint)-1));
		NEW_SKETCH_N(render, uint, visops::ebdist(inside, bound0, (uint)-1, 1));
		
		//NEW_SKETCH_N(fov, bool, visops::fillInterior(bound1));
		NEW_SKETCH_N(fov, bool, (render+2));
		//NEW_SKETCH_N(fovuint, uint, fov);
		
		GET_SKETCH(viewed, bool, worldSkS);
		viewed |= fov;
		//GET_SKETCH(viewed, uint, worldSkS);
		//viewed += fovuint;
		
		worldShS.deleteShape(worldfov);
		worldShS.deleteShape(line0);
		worldShS.deleteShape(line1);
		worldShS.deleteShape(line2);
		worldShS.deleteShape(line3);
		
		//std::cout << "------AddFieldOfView post signal" << std::endl;
		postStateCompletion();
		//std::cout << "------AddFieldOfView fin" << std::endl;
	}
	
	$nodeclass DrawWorld : VisualRoutinesStateNode : doStart {
		std::cout << "DrawWorld" << std::endl;
		
		GET_SKETCH(world, uint, worldSkS);
		GET_SKETCH(viewed, bool, worldSkS);
		GET_SKETCH(obst, bool, worldSkS);
		GET_SKETCH(bound, bool, worldSkS);
		bound = 0;
		
		const uint unexplored = 0;
		const uint explored = 100;
		const uint boundary = 200;
		const uint blocked = 300;
		
		const float width = VRmixin::worldSkS.getWidth();
		const float height = VRmixin::worldSkS.getHeight();
		
		for (int i=0; i<width; i++)
			for (int j=0; j<height; j++) {
				if (!viewed(i,j)) {
					unsigned int count=0;
					if (viewed(i-1,j-1)) count++;
					if (viewed(i,j-1)) count++;
					if (viewed(i+1,j-1)) count++;
					if (viewed(i-1,j)) count++;
					if (viewed(i+1,j)) count++;
					if (viewed(i-1,j+1)) count++;
					if (viewed(i,j+1)) count++;
					if (viewed(i+1,j+1)) count++;
					if (count>=6) viewed(i,j)=1;
				}
			}
		
		for (int i=0; i<width; i++)
			for (int j=0; j<height; j++) {
				if (obst(i,j)) world(i,j)=blocked;
				else if((world(i,j)==unexplored || world(i,j)==boundary) && viewed(i,j)) world(i,j)=explored;
			}
		/*for (int i=0; i<width; i++)
			for (int j=0; j<height; j++) {
				if (world(i,j)==unexplored) {
					unsigned int count=0;
					if ( world(i-1,j-1)==explored || world(i-1,j-1)==blocked ) count++;
					if ( world(i,j-1)==explored || world(i,j-1)==blocked ) count++;
					if ( world(i+1,j-1)==explored || world(i+1,j-1)==blocked ) count++;
					if ( world(i-1,j)==explored || world(i-1,j)==blocked ) count++;
					if ( world(i+1,j)==explored || world(i+1,j)==blocked ) count++;
					if ( world(i-1,j+1)==explored || world(i-1,j+1)==blocked ) count++;
					if ( world(i,j+1)==explored || world(i,j+1)==blocked ) count++;
					if ( world(i+1,j+1)==explored || world(i+1,j+1)==blocked ) count++;
					if (count>=6) world(i,j)=explored;
				}
			}
		*/
		std::vector<xyPair> boundaryPt;
		boundaryPt.clear();
		for (int i=1; i<width-1; i++)
			for (int j=1; j<height-1; j++)
				if (world(i,j)==explored)
					if ( world(i-1,j-1)==unexplored || world(i,j-1)==unexplored || world(i+1,j-1)==unexplored || 
						world(i-1,j)==unexplored || world(i+1,j)==unexplored || 
						world(i-1,j+1)==unexplored || world(i,j+1)==unexplored || world(i+1,j+1)==unexplored )
					{
						boundaryPt.push_back(xyPair(i,j));
						world(i,j)=boundary;
						bound(i,j)=true;
					}
		
		postStateCompletion();
	}
	
	$nodeclass GetNextPos : VisualRoutinesStateNode : doStart {
		$reference ExploreTest6::pathToNextPos;
		$reference ExploreTest6::nMove;
		$reference ExploreTest6::iMove;
		
		std::cout << "GetNextPos" << std::endl;
		
		const float offset = 0.f;
		const float minX = -22000 / 2 + offset;
		const float maxX = 22000 / 2 - offset;
		const float minY = -22000 / 2 + offset;
		const float maxY = 22000 / 2 - offset;
		
		const float width = VRmixin::worldSkS.getWidth();
		const float height = VRmixin::worldSkS.getHeight();
		
		//GET_SKETCH(world, uint, worldSkS);
		GET_SKETCH(obst, bool, worldSkS);
		GET_SKETCH(bound, bool, worldSkS);
		
		Point agentPos = theAgent->getCentroid();
		NEW_SHAPE(agentPosShape, PointData, new PointData(worldShS, agentPos));
		NEW_SKETCH_N(ct, bool, agentPosShape->getRendering());
		worldShS.deleteShape(agentPosShape);
		
		std::vector<unsigned int> vecCtX;
		std::vector<unsigned int> vecCtY;
		vecCtX.clear();
		vecCtY.clear();
		for (unsigned int i=0; i<width; i++)
			for (unsigned int j=0; j<height; j++)
				if (ct(i,j)) {
					vecCtX.push_back(i);
					vecCtY.push_back(j);
				}
		
		bool result;
		NEW_SKETCH_N(posx, uint, visops::zeros(worldSkS));
		NEW_SKETCH_N(posy, uint, visops::zeros(worldSkS));
		NEW_SKETCH_N(dist, uint, ebdist(ct, obst, posx, posy, result, (uint)-1, 3));
		NEW_SKETCH_N(bounddist, uint, dist * bound);
		
		bool foundbound = false;
		//for (unsigned int i=0; i<width; i++)
		//	for (unsigned int j=0; j<height; j++)
		//		if (bound(i,j))
		//			std::cout << "bound: " << i << "" << j << std::endl;
		for (unsigned int i=0; i<width; i++) {
			for (unsigned int j=0; j<height; j++) {
				if (bounddist(i,j) != 0 && bounddist(i,j) < (uint)-2) {
					foundbound = true;
					break;
				}
			}
			if (foundbound) break;
		}
		
		int pos = bounddist->findMinPlus();
		if (pos == -1 || !foundbound) {
			if (pos == -1) std::cout << "pos == -1" << std::endl;
			if (!foundbound) std::cout << "bound not found" << std::endl;
			std::cout << "finish exploring" << std::endl;
			postStateCompletion();
		}
		else if (result==false) {
			std::cout << "error, can't find next pos" << std::endl;
			postStateFailure();
		}
		else {
			//std::cout << "pos = " << pos << std::endl;
			NEW_SKETCH(nextpos, bool, visops::zeros(worldSkS));
			nextpos[pos] = true;
			nextpos = visops::fillin(nextpos, 1, 1, 8);
			
			Point pt = bounddist->indexPoint(pos);
			std::cout << pt << std::endl;
			//std::cout << posx[pos] << ", " << posy[pos] << std::endl;
			
			bool fin = false;
			std::vector<Point> path;
			path.clear();
			Point pt2 = bounddist->indexPoint(pos);
			float x = pt2.coordX();
			float y = pt2.coordY();
			if (x < minX) x = minX;
			else if (x > maxX) x = maxX;
			if (y < minY) y = minY;
			else if (y > maxY) y = maxY;
			pt2.setCoords(x, y);
			std::cout << pt2 << std::endl;
			path.push_back(pt2);
			
			unsigned int newx = posx[pos];
			unsigned int newy = posy[pos];
			//std::cout << newx << ", " << newy << ": " << newy * width + newx << std::endl;
			Point newpt = bounddist->indexPoint(newy * width + newx);
			//std::cout << newpt << std::endl;
			path.push_back(newpt);
			for (unsigned int k=0; k<vecCtX.size(); k++)
				if (newx==vecCtX[k] && newy==vecCtY[k]) {
					fin = true;
					break;
				}
			
			unsigned int count = 1;
			while (!fin && count < 10) {
				count++;
				unsigned int tempx = posx(newx,newy);
				unsigned int tempy = posy(newx,newy);
				newx = tempx;
				newy = tempy;
				//std::cout << newx << ", " << newy << ": " << newy * width + newx << std::endl;
				newpt = bounddist->indexPoint(newy * width + newx);
				//std::cout << newpt << std::endl;
				path.push_back(newpt);
				for (unsigned int k=0; k<vecCtX.size(); k++)
					if (newx==vecCtX[k] && newy==vecCtY[k]) {
						fin = true;
						break;
					}
			}
			
			if (path.size() <= 1) {
				std::cout << "error, empty path" << std::endl;
				postStateFailure();
				return;
			}
			
			pathToNextPos.clear();
			std::cout << "path: " << std::endl;
			Point curpos = theAgent->getCentroid();
			std::cout << curpos << std::endl;
			GET_SHAPE(gPathToNextPos, GraphicsData, VRmixin::worldShS);
			GraphicsData::xyPair U, V;
			U.first = curpos.coordX();
			U.second = curpos.coordY();
			for (std::vector<Point>::const_reverse_iterator it=path.rbegin()+1; it!=path.rend(); it++) {
				pathToNextPos.push_back(*it);
				std::cout << *it << std::endl;
				V.first = it->coordX();
				V.second = it->coordY();
				gPathToNextPos->add(new GraphicsData::LineElement("p", U, V, rgb(0,0,255)));
				U = V;
			}
			nMove = pathToNextPos.size();
			iMove = -1;
			postStateSuccess();
		}
	}
	
	$nodeclass SetMove : StateNode : doStart {
		$reference ExploreTest6::nMove;
		$reference ExploreTest6::iMove;
		
		std::cout << "SetMove" << std::endl;
		
		iMove++;
		if (iMove < nMove) {
			postStateSuccess();
		}
		else {
			postStateFailure();
		}
	}
	
	$nodeclass Move : StateNode {
		$provide float rot1;
		$provide float trans;
		$provide float rot2;
		
		$nodeclass LoadCtrl : StateNode : doStart {
			$reference ExploreTest6::pathToNextPos;
			$reference ExploreTest6::iMove;
			$reference Move::rot1, Move::trans, Move::rot2;
			
			std::cout << "Move" << std::endl;
			std::cout << ">>>>>>>>LoadCtrl" << std::endl;
			
			float x = state->sensors[GPSXOffset];
			float y = state->sensors[GPSYOffset];
			float theta = state->sensors[GPSHeadingOffset];
			//AngTwoPi heading = theAgent->getOrientation();
			//Point position = theAgent->getCentroid();
			//float x = position.coordX();
			//float y = position.coordY();
			//float theta = (float)heading;
			
			Point nextpt = pathToNextPos[iMove];
			float xt = nextpt.coordX();
			float yt = nextpt.coordY();
			
			rot1 = pi2pi(atan2(yt-y, xt-x) - theta);
			trans = sqrt((xt-x) * (xt-x) + (yt-y) * (yt-y));
			rot2 = 0.0f;
			
			postStateCompletion();
		}
		
		$nodeclass Rotate1 : PilotNode(PilotTypes::walk) : doStart {
			$reference Move::rot1;
			
			std::cout << ">>>>>>>>Rotate 1 start" << std::endl;
			pilotreq.da = rot1;
			pilotreq.turnSpeed = 0.25f;
			//pilotreq.collisionAction = collisionIgnore;
		}
		
		$nodeclass Forward : PilotNode(PilotTypes::walk) : doStart {
			$reference Move::trans;
			
			std::cout << ">>>>>>>>Forward start" << std::endl;
			pilotreq.dx = trans;
			pilotreq.forwardSpeed = 100.f;
			//pilotreq.collisionAction = collisionIgnore;
		}
		
		$nodeclass Rotate2 : PilotNode(PilotTypes::walk) : doStart {
			$reference Move::rot2;
			
			std::cout << ">>>>>>>>Rotate 2 start" << std::endl;
			pilotreq.da = rot2;
			pilotreq.turnSpeed = 0.25f;
			//pilotreq.collisionAction = collisionIgnore;
		}
		
		$nodeclass ReportResult(int r) : StateNode : doStart {
			if (r == 1)
				postParentCompletion();
			else if (r == 2)
				postParentSuccess();
			else
				postParentFailure();
		}
		
		$setupmachine{
			//startmove: LoadCtrl =C=> Rotate1 =C=> Forward =C=> Rotate2 =C=> ReportResult(1)
			startmove: LoadCtrl =C=> Rotate1 =C=> Forward =C=> ReportResult(1)
		}
	}
	
	$nodeclass Backup : PilotNode(PilotTypes::walk) : doStart {
		std::cout << ">>>>>>>>Backup start" << std::endl;
		pilotreq.dx = -400.f;
		pilotreq.forwardSpeed = 100.f;
		//pilotreq.collisionAction = collisionIgnore;
	}
	
	$setupmachine{
		startdemo: ParkArm =C=>
		InitPos =C=>
		InitWorldSkS =C=> InitWorldShS =C=>
		InitLook =C=>
		
		setlook: SetLook
		setlook =F=> TakeAPicture =C=> CopyToWorld =C=> AddObstacles =C=> AddFieldOfView =C=> setlook
		setlook =S=> TurnBack =C=> setlook
		setlook =C=> DrawWorld =C=> RefreshSketchWorld =N=> PrintNode("Finish scanning at current pos") =C=> getnextpos
		
		getnextpos: GetNextPos
		getnextpos =S=> setmove
		getnextpos =F=> PrintNode("Done")
		getnextpos =C=> PrintNode("Done")
		
		setmove: SetMove
		setmove =S=> Move =C=> setmove
		setmove =F=> Backup =C=> setlook
		//setmove =F=> setlook
	}
}

REGISTER_BEHAVIOR(ExploreTest6);