//Search.cpp

//DJD: Abstract space searching function definitions {

#include <SE/se_dcdt.h>

#include <math.h>
#include <queue>

//functions for comparing SearchNodes
bool operator <(SearchNode sn1, SearchNode sn2)
{
	//compares the f-values, but switches them so the lowest value
	//is kept at the start of the queue instead of the greatest
	return (sn1.f() < sn2.f());
}
/*
bool operator <(SearchNode *sn1, SearchNode *sn2)
{
	return ((sn1->f() < sn2->f()) || ((sn1->f() == sn2->f()) && (sn1->h() < sn2->h())));
}
*/
struct check : public std::binary_function <SearchNode *, SearchNode *, bool> 
   {
      bool operator()(SearchNode *&sn1, SearchNode *&sn2) const
	  {
		  return ((sn1->f() > sn2->f()) || ((sn1->f() == sn2->f()) && (sn1->h() > sn2->h())));;
	  }
};

//functions for comparing SearchNodes
bool operator ==(SearchNode sn1, SearchNode sn2)
{
	//compares the f-values, but switches them so the lowest value
	//is kept at the start of the queue instead of the greatest
	return (sn1.Triangle() == sn2.Triangle());
}

bool SeDcdt::IDA(SrArray<SearchNode *> startNodes, SrArray<SearchNode *> goalNodes, 
				 SearchNode* &goal, float &depthBound, float r)
{
	//TODO: actual search!
	return false;
}

bool SeDcdt::IDA(SrArray<SearchNode *> startNodes, SrArray<SearchNode *> goalNodes, SrArray<SeBase *>& path, float r)
{
	SrArray<SearchNode *> copy;
	float depthBound = INFINITY;
	for (int i = 0; i < startNodes.size(); i++)
	{
		if (startNodes[i]->f() < depthBound)
		{
			depthBound = startNodes[i]->f();
		}
	}
	SearchNode *goal = NULL;
	while (true)
	{
		copy.size(0);
		for (int i = 0; i < startNodes.size(); i++)
		{
			copy.push() = startNodes[i];
		}
		if (!IDA(copy, goalNodes, goal, depthBound, r) || (goal != NULL))
		{
			break;
		}
	}
	path.size(0);
	if (goal == NULL)
	{
        return false;
	}
	SrPolygon tempPath;
	SrArray<SeBase *> channel;
	while (goal != NULL)
	{
		path.push() = goal->Triangle()->se();
		SearchNode *temp = goal;
		goal = goal->Back();
		delete temp;
	}
	path.revert();
	return true;
}

//gets the closest point on an unconstrained edge of the triangle to the point given
//(that is at least r from a vertex) . . . this function can be sped up
SrPnt2 SeDcdt::ClosestPointTo(SeDcdtFace *face, float x, float y, float r)
{
	if (InTriangle(face, x, y))
	{
		SrPnt2 p;
		p.set(x, y);
		return p;
	}
	SrPnt2 closestPoint, point;
	float closestDistance = INFINITY, distance;
	SeBase *s = face->se();
	for (int i = 0; i < 3; i++)
	{
		float x1, y1, x2, y2, x3, y3;
		TriangleVertices(s, x1, y1, x2, y2, x3, y3);
		if (Blocked(s))
		{
			s = s->nxt();
			continue;
		}
		bool accute1 = IsAccute(AngleBetween(x1, y1, x2, y2, x, y));
		bool accute2 = IsAccute(AngleBetween(x2, y2, x1, y1, x, y));
		if (accute1 && accute2)
		{
			//get closest point on this line
			distance = PointLineDistance(x, y, x1, y1, x2, y2);
			float theta = atan2(y2 - y1, x2 - x1);
			theta += (theta > PI / 2.0f) ? -3.0f * PI / 2.0f : PI / 2.0f;
			point.set(x + cos(theta) * distance, y + sin(theta) * distance);
			if (Length(point.x, point.y, x1, y1) < r)
			{
				accute2 = false;
			}
			else if (Length(point.x, point.y, x2, y2) < r)
			{
				accute1 = false;
			}
		}
		if (!accute1 && accute2)
		{
			//get point distance r from (x2, y2) in the direction of (x1, y1)
			float theta = atan2(y1 - y2, x1 - x2);
			point.set(x2 + cos(theta) * r, y2 + sin(theta) * r);
		}
		else if (accute1 && !accute2)
		{
			//get point distance r from (x1, y1) in the direction of (x2, y2)
			float theta = atan2(y2 - y1, x2 - x1);
			point.set(x1 + cos(theta) * r, y1 + sin(theta) * r);
		}
		distance = Length(point.x, point.y, x, y);
		if (distance < closestDistance)
		{
			closestPoint.set(point.x, point.y);
			closestDistance = distance;
		}
		s = s->nxt();
	}
	return closestPoint;
}

//walks from the degree-1 source triangle to the degree-2 destination rectangle at the root of the tree
bool SeDcdt::WalkBetween(SrArray<SeBase *>& path, SeDcdtFace *sourceFace, SeDcdtFace *destinationFace, float r, int direction)
{
	SeDcdtFace *last = NULL;
	//go through all faces between the source and destination faces
	while (true)
	{
		//if we have reached the destination face,
		if (sourceFace == destinationFace)
		{
			//add it to the path and exit successfully
			path.push() = destinationFace->se();
			return true;
		}
		//get the elements of the current triangle
		SeBase *s = sourceFace->se();
		//go through the edges of the triangle
		for (int i = 0; i < 3; i++)
		{
			//if the destination face is across the current edge,
			if ((i == direction) || ((direction == INVALID)
				&& (sourceFace->link->Adjacent(i) == destinationFace) && (s->sym()->fac() != last)))
			{
				last = sourceFace;
				direction = INVALID;
				//make sure the path is wide enough
				if (sourceFace->link->Choke(i) < 2.0f * r)
				{
					//if not, empty the path and return failure
					path.size(0);
					return false;
				}
				else
				{
					//if so, add the current face to the path
					path.push() = sourceFace->se();
					//and move to the next one
					sourceFace = (SeDcdtFace *)s->sym()->fac();
float x, y;
TriangleMidpoint(sourceFace, x, y);
					break;
				}
			}
			//move to the next edge in the current triangle
			s = s->nxt();
		}
	}
}

//checks if a unit of radius r can go through the second triangle between the first and the third
bool SeDcdt::CanCross(SeBase *first, SeBase *second, SeBase *third, float r)
{
	//and goes through its edges
	for (int i = 0; i < 3; i++)
	{
		//gets the face across the current edge
		SeFace *current = second->sym()->fac();
		//if it is neither the first nor the third
		if ((current != first->fac()) && (current != third->fac()))
		{
			//return if the width between the first and the third is large enough for radius r
			return (((SeDcdtFace *)second)->link->Width((i + 1) % 3) >= 2.0f * r);
		}
		//move to the next edge
		second = second->nxt();
	}
	sr_out.warning("ERROR: could not find desired face (should never happen)\n");
	return false;
}

//finds a path in the degree-1 tree with both the start and goal in it
bool SeDcdt::Degree1Path(SrArray<SeBase *>& path, float x1, float y1, SeDcdtFace *startFace,
						 float x2, float y2, SeDcdtFace *goalFace, float r)
{
	//starts marking the mesh
	_mesh->begin_marking();
	//create a priority queue for searching the tree
//	std::priority_queue<SearchNode> pq;
	std::priority_queue<SearchNode *, std::vector<SearchNode *>, check> pq;
	float x, y;
	//each face is represented by its midpoint (since only one path must exist)
	TriangleMidpoint(startFace, x, y);
	SrPnt2 p;
	p.set(x, y);
	//creates the first search node
	SearchNode *current = new SearchNode(Length(x, y, x1, y1), Length(x, y, x2, y2), p, startFace, NULL);
	//put it in the queue to be searched
	pq.push(current);
	//continues until the tree has been exhausted or a path has been found
	while (!pq.empty())
	{
		//gets the first node to search
		/*SearchNode */current = pq.top();
		pq.pop();
		//checks if that node is the goal
		if (InTriangle(current->Triangle(), x2, y2))
		{
			while (!pq.empty())
			{
				pq.top()->Close();
				delete pq.top();
				pq.pop();
			}
			SearchNode *temp = current;
			//if so, visits all the triangles between the start and the goal
			while (current != NULL)
			{
				//and adds each to the path
				path.push() = current->Triangle()->se();
				//until the first node is reached
//				if (current->Back() == NULL)
//				{
//					break;
//				}
				//moves to the previous triangle
				current = current->Back();
			}
			temp->Close();
			delete temp;
			//since they were visited in reverse, flip the path around
			path.revert();
			path.pop();
			//return a path was successfully found
//			return true;
			_mesh->end_marking();
			if (ValidPath(path, x1, y1, x2, y2, r))
			{
				return true;
			}
			else
			{
				path.size(0);
				return false;
			}
		}
		//gets the triangle associated with the current node
		SeDcdtFace *currentFace = current->Triangle();
		//marks it as visited
		_mesh->mark(currentFace);
		//get the elements of the triangle
		SeBase *s = currentFace->se();
		//go through the edges
		for (int i = 0; i < 3; i++)
		{
			//gets the face on the opposite side of this edge
			SeDcdtFace *tempFace = (SeDcdtFace *)s->sym()->fac();
			//only consider it if it's degree-1 and hasn't been visited before
			if ((!_mesh->marked(tempFace)) && (tempFace->link->Degree() == 1) && (!Blocked(s)))
			{
/*
				//make sure the current triangle is wide enough
				float width;
				//if it's the first triangle, assume it is for now
				if (current.Back() == NULL)
				{
					width = INFINITY;
				}
				//otherwise, get the width through this triangle between
				//the last triangle and the next one
				else
				{
					if (s->nxt()->sym()->fac() == current.Back()->Triangle())
					{
						width = currentFace->link->Width(i);
					}
					else
					{
						width = currentFace->link->Width((i + 2) % 3);
					}
				}
				//only add this triangle if the path is wide enough
				if (width >= 2.0f * r)
				{
*/
					//get the new triangle's midpoint
					TriangleMidpoint(tempFace, x, y);
					p.set(x, y);
					//create a search node corresponding to this triangle
					SearchNode *temp = new SearchNode(current->g() + Length(current->Point().x, current->Point().y, x, y),
						Length(x, y, x2, y2), p, tempFace, current);
					//and add it to the search queue
					pq.push(temp);
					current->OpenChild();
//				}
			}
			//move to the next edge
			s = s->nxt();
		}
		if (current->OpenChildren() <= 0)
		{
			current->Close();
			delete current;
		}
	}
	//stop marking the mesh
	_mesh->end_marking();
	//the tree was exhausted and a path not found - return failure
	return false;
}

//follows a degree-2 ring from one node to another, making sure that each is wide enough for the given radius
bool SeDcdt::Degree2Path(SrArray<SeBase *>& path, SeDcdtFace *startFace, SeDcdtFace *nextFace,
						 SeDcdtFace *goalFace, float r)
{
	//empties the path first
	path.size(0);
	//goes until it has reached the goal triangle
	while (nextFace != goalFace)
	{
		//gets the elements of the current triangle
		SeBase *s = nextFace->se();
		//goes through its edges
		for (int i = 0; i < 3; i++)
		{
			//when the face across the current edge is the last triangle,
			if (s->sym()->fac() == startFace)
			{
				//check if the triangle across the next edge is the next triangle in the ring
				if (nextFace->link->Angle((i + 1) % 3) == INFINITY)
				{
					//makes sure this triangle is wide enough for radius r
					if (nextFace->link->Width(i) < 2.0f * r)
					{
						//if not, empties the path and returns failure
						path.size(0);
						return false;
					}
					//otherwise, adds the current triangle to the path
					path.push() = nextFace->se();
					//and moves to the next triangle in the ring
					startFace = nextFace;
					nextFace = (SeDcdtFace *)s->nxt()->sym()->fac();
				}
				else
				{
					//similar to above, checks the width
					if (nextFace->link->Width((i + 2) % 3) < 2.0f * r)
					{
						path.size(0);
						return false;
					}
					//and continues along the ring
					path.push() = nextFace->se();
					startFace = nextFace;
					nextFace = (SeDcdtFace *)s->nxt()->nxt()->sym()->fac();
				}
				//process the next triangle in the ring
				break;
			}
			//move to the next edge
			s = s->nxt();
		}
	}
	//return that a path was found successfully
	return true;
}

//determines if the given endpoint has to be checked against the current triangle
bool SeDcdt::ValidEndpoint(SeBase *s, float x, float y, bool left)
{
	//gets the vertices of the current triangle
	float x1, y1, x2, y2, x3, y3, x4, y4;
	TriangleVertices(s, x1, y1, x2, y2, x3, y3);
	//checks if a base angle is obtuse
	if ((abs(AngleBetween(x1, y1, x2, y2, x3, y3)) >= PI / 2.0f)
		|| (abs(AngleBetween(x1, y1, x3, y3, x2, y2)) >= PI / 2.0f))
	{
		//if so, we don't have to worry
		return true;
	}
	//gets the angle of the opposite edge
	float theta = atan2(y3 - y2, x3 - x2);
	//turn 90 degrees clockwise
	theta += (theta <= PI / -2.0f) ? 3.0f * PI / 2.0f : PI / -2.0f;
	//create a point this angle from the "top" vertex
    x4 = x1 + cos(theta);
	y4 = y1 + sin(theta);
	//returns whether the unit doesn't have to pass through the narrowest point in this triangle
	return ((Orientation(x1, y1, x4, y4, x, y) > 0) == (left));
}

//checks whether there is a valid path from the start or goal through their corresponding triangles
bool SeDcdt::ValidEndpoint(SeDcdtFace *first, SeDcdtFace *second, float x, float y, float r)
{
	SeBase *s = first->se();
	for (int i = 0; i < 3; i++)
	{
		if (s->sym()->fac() == second)
		{
//			return (!(((!ValidEndpoint(s, x, y, false)) && (first->link->Width((i + 2) % 3) < 2.0f * r))
//				|| ((!ValidEndpoint(s->nxt(), x, y, true)) && (first->link->Width(i) < 2.0f * r))));
			return (((ValidEndpoint(s, x, y, false)) || (first->link->Width((i + 2) % 3) >= 2.0f * r))
				&& ((ValidEndpoint(s->nxt(), x, y, true)) || (first->link->Width(i) >= 2.0f * r)));
		}
		s = s->nxt();
	}
	return false;
}

//checks if the start and goal points can validly enter the path
bool SeDcdt::ValidEndpoints(SrArray<SeBase *>& path, float x1, float y1, float x2, float y2, float r)
{
//	return ((path.size() > 1) && (ValidEndpoint((SeDcdtFace *)path[0]->fac(), (SeDcdtFace *)path[1]->fac(), x1, y1, r))
//		&& (ValidEndpoint((SeDcdtFace *)path[path.size() - 1]->fac(), (SeDcdtFace *)path[path.size() - 2]->fac(), x2, y2, r)));
	return ((path.size() < 2) || ((ValidEndpoint((SeDcdtFace *)path[0]->fac(), (SeDcdtFace *)path[1]->fac(), x1, y1, r))
		&& (ValidEndpoint((SeDcdtFace *)path[path.size() - 1]->fac(), (SeDcdtFace *)path[path.size() - 2]->fac(), x2, y2, r))));
}

//checks if a path is valid for a unit of radius r
bool SeDcdt::ValidPath(SrArray<SeBase *>& path, float x1, float y1, float x2, float y2, float r)
{
	//first checks if the endpoints are valid
	if (!ValidEndpoints(path, x1, y1, x2, y2, r))
	{
		return false;
	}
	//then goes through the interior points
	for (int i = 1; i < path.size() - 1; i++)
	{
		//gets the elements of the current triangle in the path
		SeBase *s = path[i]->fac()->se();
		//goes through the edges in this triangle
		for (int j = 0; j < 3; j++)
		{
			//find the edge across which is the next triangle in the path
			if (s->sym()->fac() == path[i + 1]->fac())
			{
				//get the width through this triangle between the last one in the path and the next one
				float width = ((SeDcdtFace *)s->fac())->link->Width(
					(s->nxt()->sym()->fac() == path[i - 1]->fac()) ? j : ((j + 2) % 3));
				//if it's less than the diameter of the unit, the path is invalid
				if (width < 2.0f * r)
				{
					return false;
				}
				break;
			}
			//moves to the next edge in the triangle
			s = s->nxt();
		}
	}
	//if no triangle in the path was invalid, the path is valid
	return true;
}

void SeDcdt::ConstructBasePath(SrArray<SeBase *> &path, SearchNode *goalNode, SeDcdtFace *start, SeDcdtFace *goal, int direction)
{
	SrArray<SeBase *> backwardsPath;
	SeDcdtFace *goal2;
	if (goal->link->Degree() == 1)
	{
		for (int i = 0; i < 3; i++)
		{
			if (goal->link->Adjacent(i) != NULL)
			{
				WalkBetween(backwardsPath, goal, goal->link->Adjacent(i), 0.0f);
				backwardsPath.pop();
//				WalkBetween(backwardsPath, goal->link->Adjacent(i), goalNode->Triangle(), 0.0f);
//				backwardsPath.pop();
				goal2 = goal->link->Adjacent(i);
				break;
			}
		}
	}
	else
	{
		goal2 = goal;
	}
	if (goal->link->Degree() < 3)
	{
		WalkBetween(backwardsPath, goal2, goalNode->Triangle(), 0.0f, direction);
		backwardsPath.pop();
	}
float x, y;
	while (goalNode->Back() != NULL)
	{
TriangleMidpoint(goalNode->Triangle(), x, y);
sr_out << "SearchNode at (" << x << ", " << y << ")\n";
		SrArray<SeBase *> tempPath;
		WalkBetween(tempPath, goalNode->Back()->Triangle(), goalNode->Triangle(), 0.0f, goalNode->Direction());
		while (!tempPath.empty())
		{
			backwardsPath.push() = tempPath.pop();
		}
//		WalkBetween(backwardsPath, goalNode->Triangle(), goalNode->Back()->Triangle(), 0.0f);
		backwardsPath.pop();
		goalNode = goalNode->Back();
	}
TriangleMidpoint(goalNode->Triangle(), x, y);
sr_out << "SearchNode at (" << x << ", " << y << ")\n";
	SeDcdtFace *start2;
	if (start->link->Degree() == 1)
	{
		for (int i = 0; i < 3; i++)
		{
			if (start->link->Adjacent(i) != NULL)
			{
				WalkBetween(path, start, start->link->Adjacent(i), 0.0f);
				path.pop();
//				WalkBetween(path, start->link->Adjacent(i), goalNode->Triangle(), 0.0f);
				start2 = start->link->Adjacent(i);
				break;
			}
		}
	}
	else
	{
		start2 = start;
	}
	if (start->link->Degree() < 3)
	{
		WalkBetween(path, start2, goalNode->Triangle(), 0.0f, goalNode->Direction());
	}
	while (!backwardsPath.empty())
	{
		path.push() = backwardsPath.pop();
	}
	path.pop();
/*
float x, y;
sr_out << "\n{";
for (int i = 0; i < path.size() - 1; i++)
{
TriangleMidpoint((SeDcdtFace *)path[i]->fac(), x, y);
sr_out << "(" << x << ", " << y << "), ";
}
TriangleMidpoint((SeDcdtFace *)path[path.size() - 1], x, y);
sr_out << "(" << x << ", " << y << ")}\n\n";
*/
}

//searches the abstract graph for a path between two points
bool SeDcdt::SearchPath(/*SrArray<SeBase *>& path,*/ float x1, float y1, float x2, float y2, float r, int update)
{
sr_out << "SEARCH PATH STARTED:\n";
	//initialize the path to empty
	currentPath.size(0);
	SeBase *start, *goal;
	//find the triangle containing the start point
	SeTriangulator::LocateResult startResult = LocatePoint(x1, y1, start);
	//if it wasn't found, return a path could not be found
	if (startResult == SeTriangulator::LocateResult::NotFound)
	{
		return false;
	}
	//find the triangle containing the goal point
	SeTriangulator::LocateResult goalResult = LocatePoint(x2, y2, goal);
	//if it couldn't, return a failure
	if (goalResult == SeTriangulator::LocateResult::NotFound)
	{
		return false;
	}
	SeDcdtFace *startFace = (SeDcdtFace *)start->fac();
	SeDcdtFace *goalFace = (SeDcdtFace *)goal->fac();
sr_out << "START AND GOAL FACES FOUND\n";
	//if either triangle isn't abstracted, this search fails
	//also if the triangles aren't in the same connected component, no path connecting them exists
	if ((startFace->link == NULL) || (goalFace->link == NULL)
		|| (startFace->link->Component() != goalFace->link->Component()))
	{
sr_out << ((startFace->link == NULL) ? "start face not abstracted" : (goalFace->link == NULL) ?
"goal face not abstracted" : "start and goal in different components") << srnl;
		return false;
	}
sr_out << "START AND GOAL ARE ABSTRACTED AND IN THE SAME COMPONENT\n";
	//check that the start and goal points aren't within the radius of any constraints
	if ((!ValidPosition(x1, y1, startFace, r)) || (!ValidPosition(x2, y2, goalFace, r)))
	{
		//if either are, return false
		return false;
	}
	//if the start and goal are in the same triangle,
	if (startFace == goalFace)
	{
		//the path is a straight line
		currentPath.push().set(x1, y1);
		currentPath.push().set(x2, y2);
		return true;
	}
sr_out << "START AND GOAL ARE VALID POSITIONS AND NOT IN THE SAME TRIANGLE\n";
	//if both start and goal faces are degree-1,
	if ((startFace->link->Degree() == 1) && (goalFace->link->Degree() == 1))
	{
		//checks if the start (and thus the goal as well) is in a tree component
		bool inTree = true;
		//(true if there are no adjacent degree-2 nodes)
		for (int i = 0; i < 3; i++)
		{
			if (startFace->link->Adjacent(i) != NULL)
			{
				inTree = false;
			}
		}
		//if they are in a tree component,
		if (inTree)
		{
            //perform search in it
			SrArray<SeBase *> channel;
			if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r))
			{
				GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
				return true;
			}
			else
			{
				return false;
			}
		}
	}
sr_out << "START AND GOAL ARE NOT IN A TREE\n";
	//if the start face is degree-1, moves to the adjacent degree-2 node
	SeDcdtFace *startFace2;
	float startAngle;
	float startChoke;
	if (startFace->link->Degree() == 1)
	{
		for (int i = 0; i < 3; i++)
		{
			SeDcdtFace *adjacent = startFace->link->Adjacent(i);
			if (adjacent != NULL)
			{
				startFace2 = adjacent;
				startAngle = startFace->link->Angle(i);
				startChoke = startFace->link->Choke(i);
				break;
			}
		}
	}
	else
	{
		startFace2 = startFace;
		startAngle = 0.0f;
		startChoke = INFINITY;
	}
	//if the goal face is degree-1, moves to the adjacent degree-2 node
	SeDcdtFace *goalFace2;
	float goalAngle;
	float goalChoke;
	if (goalFace->link->Degree() == 1)
	{
		for (int i = 0; i < 3; i++)
		{
			SeDcdtFace *adjacent = goalFace->link->Adjacent(i);
			if (adjacent != NULL)
			{
				goalFace2 = adjacent;
				goalAngle = goalFace->link->Angle(i);
				goalChoke = goalFace->link->Choke(i);
				break;
			}
		}
	}
	else
	{
		goalFace2 = goalFace;
		goalAngle = 0.0f;
		goalChoke = INFINITY;
	}
	//if the start and goal faces are both degree-1 or 2,
	if ((startFace->link->Degree() < 3) && (goalFace->link->Degree() < 3))
	{
		//checks if the start and goal have the same root,
		if (startFace2 == goalFace2)
		{
			//if the start face is at the root of the tree that the goal face is in,
			if (startFace2 == startFace)
			{
				SrArray<SeBase *> channel;
				//walk from the goal face to the start face
				if (WalkBetween(channel, goalFace, startFace, r))
				{
					channel.revert();
channel.pop();
					GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
					return true;
				}
				else
				{
					return false;
				}
			}
			//if the goal face is at the root of the tree that the start face is in,
			else if (goalFace2 == goalFace)
			{
				SrArray<SeBase *> channel;
				//walk from the start face to the goal face
				if (WalkBetween(channel, startFace, goalFace, r))
				{
channel.pop();
					GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
					return true;
				}
				else
				{
					return false;
				}
			}
			//if the start and goal are both in the same tree component,
			else
			{
				SrArray<SeBase *> channel;
				//perform search in tree component
				if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r))
				{
					GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
					return true;
				}
				else
				{
					return false;
				}
			}
		}
	}
sr_out << "ONE IS NOT AT THE ROOT OF THE OTHER\n";
	//if we can't get from the start or goal to the rest of the graph, no path exists
	if ((startChoke < 2.0f * r) || (goalChoke < 2.0f * r))
	{
sr_out << "startChoke = " << startChoke << ", goalChoke = " << goalChoke << srnl;
		return false;
	}
sr_out << "START AND GOAL CAN GET ON TO THE GRAPH\n";
	//TODO: if the nodes are both in a degree-2 ring, walk from the start to the goal (both ways)
	//TODO: checking that the path is wide enough at all times
	//TODO: if both paths are wide enough, run the funnel algorithm and take the shorter path
	//TODO: if only one is wide enough, take that, and if neither are, return false
	if ((startFace2->link->Degree() == 2) && (goalFace2->link->Degree() == 2))
	{
		for (int i = 0; i < 3; i++)
		{
			if (startFace2->link->Adjacent(i) != NULL)
			{
				break;
			}
		}
		if (i >= 3)
		{
			//degree-2 ring!
			SrArray<SeBase *> startPath, goalPath, leftPath, rightPath;
			if (startFace == startFace2)
			{
				startPath.size(0);
			}
			else if (!WalkBetween(startPath, startFace, startFace2, r))
			{
				return false;
			}
			if (goalFace == goalFace2)
			{
				goalPath.size(0);
			}
			else if (!WalkBetween(goalPath, goalFace, goalFace2, r))
			{
				return false;
			}
			goalPath.revert();
			SeBase *s = startFace2->se();
			bool leftValid, rightValid;
			for (int i = 0; i < 3; i++)
			{
				if (startFace2->link->Angle(i) == INVALID)
				{
					rightValid = Degree2Path(rightPath, startFace2, (SeDcdtFace *)s->nxt()->sym()->fac(), goalFace2, r);
					leftValid = Degree2Path(leftPath, startFace2, (SeDcdtFace *)s->nxt()->nxt()->sym()->fac(), goalFace2, r);
					break;
				}
				s = s->nxt();
			}
			if (!leftValid && !rightValid)
			{
				return false;
			}
			SrArray<SeBase *> fullLeftPath, fullRightPath;
			fullLeftPath.size(0);
			fullRightPath.size(0);
			if (leftValid && CanCross(startPath[startPath.size() - 2], startPath[startPath.size() - 1], leftPath[0], r)
				&& CanCross(goalPath[goalPath.size() - 2], goalPath[goalPath.size() - 1], leftPath[leftPath.size() - 1], r))
			{
				for (int i = 0; i < startPath.size(); i++)
				{
					fullLeftPath.push() = startPath[i];
				}
				for (int i = 0; i < leftPath.size(); i++)
				{
					fullLeftPath.push() = leftPath[i];
				}
				for (int i = 0; i < goalPath.size(); i++)
				{
					fullLeftPath.push() = goalPath[i];
				}
			}
			if (rightValid && CanCross(startPath[startPath.size() - 2], startPath[startPath.size() - 1], rightPath[0], r)
				&& CanCross(goalPath[goalPath.size() - 2], goalPath[goalPath.size() - 1], rightPath[rightPath.size() - 1], r))
			{
				for (int i = 0; i < startPath.size(); i++)
				{
					fullLeftPath.push() = startPath[i];
				}
				for (int i = 0; i < rightPath.size(); i++)
				{
					fullLeftPath.push() = rightPath[i];
				}
				for (int i = 0; i < goalPath.size(); i++)
				{
					fullLeftPath.push() = goalPath[i];
				}
			}
			if ((fullLeftPath.size() == 0) && (fullRightPath.size() == 0))
			{
				return false;
			}
			else if ((fullLeftPath.size() > 0) && (fullRightPath.size() > 0))
			{
				SrPolygon funnelLeft, funnelRight;
				float leftLength = GetShortestPath(funnelLeft, fullLeftPath, x1, y1, x2, y2, r);
				float rightLength = GetShortestPath(funnelRight, fullRightPath, x1, y1, x2, y2, r);
				if (leftLength < rightLength)
				{
					fullRightPath.size(0);
					//maybe save other values?
				}
				else
				{
					fullLeftPath.size(0);
					//maybe save other values?
				}
			}
			SrArray<SeBase *> channel;
			if (fullRightPath.size() == 0)
			{
				for (int i = 0; i < fullLeftPath.size(); i++)
				{
					channel.push() = fullLeftPath[i];
				}
			}
			else //if (fullLeftPath.size() == 0)
			{
				for (int i = 0; i < fullRightPath.size(); i++)
				{
					channel.push() = fullRightPath[i];
				}
			}
			GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
			return true;
		}
	}
sr_out << "NOT A DEGREE-2 RING\n";
	//TODO: next, if either start or goal triangle is degree-2, move to adjacent degree-3 node(s)
	//TODO: (whenever performing these "moves", record the distance or angle travelled)

	//TODO: search (A* or IDA*?) from start degree-3 node(s) to goal ones on degree-3 nodes only
	//TODO: whenever a path is found, run funnel algorithm to determine the length
	//TODO: if this path is shorter than the shortest path so far, it becomes the shortest
	//TODO: ends when either there are no more nodes to try (connected component exhausted)
	//TODO: or the shortest path being searched is >= the shortest path found
	//TODO: return true and the shortest path found, false if none were found
//	SrArray<SearchNode *> startNodes;
//	std::priority_queue<SearchNode> q;
	SeDcdtFace *startTriangle1 = NULL, *startTriangle2 = NULL;
	std::priority_queue<SearchNode *, std::vector<SearchNode *>, check> q;
	if (startFace2->link->Degree() == 3)
	{
		SrPnt2 p = ClosestPointTo(startFace2, x2, y2, r);
//		startNodes.push() = new SearchNode(Maximum(Length(p.x, p.y, x1, y1), startAngle * r),
//			Length(p.x, p.y, x2, y2), p, startFace2, NULL);
		q.push(new SearchNode(0.0f, //Maximum(Length(p.x, p.y, x1, y1), startAngle * r),
			Length(p.x, p.y, x2, y2), p, startFace2, NULL));
	}
	else
	{
		SeBase *s = startFace2->se();
//		SeDcdtFace *first = NULL;
		for (int i = 0; i < 3; i++)
		{
			SeDcdtFace *face = startFace2->link->Adjacent(i);
			if (face == NULL)
			{
				s = s->nxt();
				continue;
			}
			((startTriangle1 == NULL) ? startTriangle1 : startTriangle2) = face;
			SrPnt2 p = ClosestPointTo(face, x2, y2, r);
			SrPnt2 p2 = ClosestPointTo(face, x1, y1, r);
			SrPnt2 p3 = ClosestPointTo(startFace2, x1, y1, r);
			float distance1 = Maximum(startAngle * r, Length(x1, y1, p3.x, p3.y));
			float distance2 = startFace2->link->Angle(i) * r;
			float x_1, y_1, x_2, y_2, x_3, y_3;
			TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3);
			float theta = (startFace2->link->Adjacent((i + 1) % 3) == NULL) ?
				AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3) : AngleBetween(x_2, y_2, x_1, y_1, x_3, y_3);
			q.push(new SearchNode(Maximum(Length(p2.x, p2.y, x1, y1), distance1 + distance2 + theta * r),
				Length(p.x, p.y, x2, y2), p, face, NULL, i));
			s = s->nxt();
		}
	}
	SrArray<SearchNode> goalNodes;
	if (goalFace2->link->Degree() == 3)
	{
		SrPnt2 p = ClosestPointTo(goalFace2, x2, y2, r);
		goalNodes.push() = SearchNode(Length(p.x, p.y, x1, y1),
			Length(p.x, p.y, x2, y2), p, goalFace2, NULL);
	}
	else
	{
//		SeDcdtFace *first = NULL;
		for (int i = 0; i < 3; i++)
		{
			SeDcdtFace *face = goalFace2->link->Adjacent(i);
			if (face == NULL)
			{
				continue;
			}
//TODO: better estimation of distance to goal (and what of g-value?)
			SrPnt2 p = ClosestPointTo(face, x2, y2, r);
			goalNodes.push(SearchNode(0.0f, //Maximum(Length(p.x, p.y, x1, y1), startAngle * r),
				Length(p.x, p.y, x2, y2), p, face, NULL, i));
		}
	}
int expanded = 0;
	float shortestPathLength = INFINITY;
	SrPolygon shortestPath;
	float toGoal = Minimum(goalNodes[0].h(), (goalNodes.size() > 1) ? goalNodes[1].h() : INFINITY);
	if ((q.size() == 2) && (goalNodes.size() == 2) &&
		(((startTriangle1 == goalNodes[0].Triangle()) && (startTriangle2 == goalNodes[1].Triangle())) ||
		((startTriangle1 == goalNodes[1].Triangle()) && (startTriangle2 == goalNodes[0].Triangle()))))
	{
		//if they're on the same edge, walk between them for one path,
		// set start to one and goal to other degree-3 nodes and continue
		SeDcdtFace *face = startFace2;
		SeDcdtFace *destinationFace;
		float destinationAngle;
		for (int i = 0; i < 3; i++)
		{
			if (face->link->Adjacent(i) == goalNodes[0].Triangle())
			{
				destinationAngle = face->link->Angle(i);
				break;
			}
		}
		for (int i = 0; i < 3; i++)
		{
			if (goalFace2->link->Adjacent(i) == goalNodes[0].Triangle())
			{
				destinationFace = goalNodes[(goalFace2->link->Angle(i) < destinationAngle) ? 0 : 1].Triangle();
				break;
			}
		}
float x, y;
TriangleMidpoint(destinationFace, x, y);
sr_out << "destinationFace = (" << x << ", " << y << ")\n";
		SrArray<SeBase *> channel;
//		bool sameEdge = false;
		while (true)
		{
TriangleMidpoint(face, x, y);
sr_out << "face = (" << x << ", " << y << ")\n";
			if (face == destinationFace)
			{
				//not on the same edge - skip to below
				break;
			}
			else if (face == goalFace2)
			{
				//on the same edge - construct channel: startFace -> startFace2 -> goalFace2 -> goalFace
				//calculate path and measure length and save values
//				sameEdge = true;
				SrArray<SeBase *> startPath;
				WalkBetween(startPath, startFace, startFace2, 0.0f);
				startPath.pop();
				SrArray<SeBase *> goalPath;
				WalkBetween(goalPath, goalFace, goalFace2, 0.0f);
				channel.revert();
float x, y;
sr_out << "startPath:\n";
for (int i = 0; i < startPath.size(); i++)
{
TriangleMidpoint(((SeDcdtFace *)startPath[i]->fac()), x, y);
sr_out << "(" << x << ", " << y << ")\n";
}
sr_out << "channel:\n";
for (int i = 0; i < channel.size(); i++)
{
TriangleMidpoint(((SeDcdtFace *)channel[i]->fac()), x, y);
sr_out << "(" << x << ", " << y << ")\n";
}
sr_out << "goalPath:\n";
for (int i = 0; i < goalPath.size(); i++)
{
TriangleMidpoint(((SeDcdtFace *)goalPath[i]->fac()), x, y);
sr_out << "(" << x << ", " << y << ")\n";
}
				while (!channel.empty())
				{
					startPath.push() = channel.pop();
				}
				while (!goalPath.empty())
				{
					startPath.push() = goalPath.pop();
				}
				startPath.pop();
				if (ValidPath(startPath, x1, y1, x2, y2, r))
				{
					shortestPathLength = GetShortestPath(shortestPath, startPath, x1, y1, x2, y2, r);
				}
				//then make goalNodes only the one at destinationFace and remove that one from q
				//then skip down to searching
				goalNodes.remove((goalNodes[0].Triangle() == destinationFace) ? 1 : 0);
				if (q.top()->Triangle() == destinationFace)
				{
					q.pop();
				}
				else
				{
					SearchNode *temp = q.top();
					q.pop();
					q.pop();
					q.push(temp);
				}
				break;
			}
			SeBase *s = face->se();
			for (int i = 0; i < 3; i++)
			{
				if (face->link->Adjacent(i) == destinationFace)
				{
					channel.push() = s;
					face = (SeDcdtFace *)s->sym()->fac();
					break;
				}
				s = s->nxt();
			}
		}
/*
		//if they just have the same endpoints, walk both ways around and choose the shortest
		if (!sameEdge)
		{
			while (!channel.empty())
			{
				channel.pop();
			}
			SrArray<SeBase *> startPath, startLeftPath, startRightPath, goalPath, goalLeftPath, goalRightPath;
			WalkBetween(startPath, startFace, startFace2, 0.0f);
			WalkBetween(startLeftPath, startFace2, goalNodes[0].Triangle(), 0.0f);
			WalkBetween(startRightPath, startFace2, goalNodes[1].Triangle(), 0.0f);
			WalkBetween(goalPath, goalFace, goalFace2, 0.0f);
			WalkBetween(goalLeftPath, goalFace2, goalNodes[0].Triangle(), 0.0f);
			WalkBetween(goalRightPath, goalFace2, goalNodes[1].Triangle(), 0.0f);
			SrArray<SeBase *> leftChannel, rightChannel;
			for (int i = 0; i < startPath.size() - 1; i++) // int i = 1 ?
			{
				leftChannel.push() = startPath[i];
				rightChannel.push() = startPath[i];
			}
			for (int i = 0; i < startLeftPath.size() - 1; i++)
			{
				leftChannel.push() = startLeftPath[i];
			}
			for (int i = 0; i < startRightPath.size() - 1; i++)
			{
				rightChannel.push() = startRightPath[i];
			}
			for (int i = goalLeftPath.size() - 1; i > 0; i--)
			{
				leftChannel.push() = goalLeftPath[i];
			}
			for (int i = goalRightPath.size() - 1; i > 0; i--)
			{
				rightChannel.push() = goalRightPath[i];
			}
			for (int i = goalPath.size() - 1; i >= 0; i--) // i > 0 ?
			{
				leftChannel.push() = goalPath[i];
				rightChannel.push() = goalPath[i];
			}
			SrPolygon leftPath, rightPath;
			float leftPathLength = (ValidPath(leftChannel, x1, y1, x2, y2, r)) ? 
				GetShortestPath(leftPath, leftChannel, x1, y1, x2, y2, r) : INFINITY;
			float rightPathLength = (ValidPath(rightChannel, x1, y1, x2, y2, r)) ?
				GetShortestPath(rightPath, rightChannel, x1, y1, x2, y2, r) : INFINITY;
			if (Minimum(leftPathLength, rightPathLength) == INFINITY)
			{
				return false;
			}
			currentPath = (leftPathLength < rightPathLength) ? leftPath : rightPath;
			return true;
		}
*/
	}
/*
	SrArray<SeBase *> firstChannel;
	if (IDA(startNodes, goalNodes, firstChannel, r))
	{
		//construct channel
		SrPolygon firstPath;
		float depthBound = GetShortestPath(firstPath, firstChannel, x1, y1, x2, y2, r);
		SearchNode *goalNode;
		IDA(startNodes, goalNodes, goalNode, depthBound, r);
	}
*/
	while (!q.empty())
	{
		SearchNode *current = q.top();
		q.pop();
float x, y;
TriangleMidpoint(current->Triangle(), x, y);
sr_out << "Expanding (" << x << ", " << y << ") g = " <<
current->g() << ", h = " << current->h() << ", f = " << current->f() << "\n";
		if ((current->f() + toGoal) >= shortestPathLength)
		{
			while (!q.empty())
			{
				q.top()->Close();
				delete q.top();
				q.pop();
			}
			break;
		}
		if (current->g() >= current->Triangle()->link->ShortestPath(update) + current->Triangle()->link->LongestEdge())
		{
sr_out << "PRUNED!\n";
			continue;
		}
expanded++;
		bool found = false;
		for (int i = 0; i < goalNodes.size(); i++)
		{
			if (current->Triangle() == goalNodes[i].Triangle())
			{
				SrArray<SeBase *> channel;
				ConstructBasePath(channel, current, startFace, goalFace, goalNodes[i].Direction());
				current->Close();
				delete current;
				if (ValidEndpoints(channel, x1, y1, x2, y2, r))
				{
					SrPolygon currentShortestPath;
					float currentPathLength = GetShortestPath(currentShortestPath, channel, x1, y1, x2, y2, r);
char string[20];
sprintf(string, "%f", currentPathLength);
if (strcmp(string, "-1.#IND00") == 0)//((currentPathLength < 0.0f) || (currentPathLength > 100.0f))
{
float x, y;
sr_out << "CHANNEL = {";
for (int j = 0; j < channel.size() - 1; j++)
{
TriangleMidpoint((SeDcdtFace *)channel[j]->fac(), x, y);
sr_out << "(" << x << ", " << y << "), ";
}
TriangleMidpoint((SeDcdtFace *)channel[channel.size() - 1]->fac(), x, y);
sr_out << "(" << x << ", " << y << ")}\n";
sr_out << "PATH = {";
for (int j = 0; j < currentShortestPath.size() - 1; j++)
{
sr_out << "(" << currentShortestPath[j].x << ", " << currentShortestPath[j].y << "), ";
}
sr_out << "(" << currentShortestPath[currentShortestPath.size() - 1].x
<< ", " << currentShortestPath[currentShortestPath.size() - 1].y << ")}\n";
}
sr_out << "GENERATED PATH WITH LENGTH = " << currentPathLength << "\n";
					if (currentPathLength < shortestPathLength)
					{
						shortestPathLength = currentPathLength;
						shortestPath.size(0);
						shortestPath = currentShortestPath;
					}
				}
				found = true;
			}
		}
		if (found)
		{
			continue;
		}
//		SeBase *s = current.Triangle()->se();
		for (int i = 0; i < 3; i++)
		{
			if (((current->Triangle() == startTriangle1) && (current->Triangle()->link->Adjacent(i) == startTriangle2))
				|| ((current->Triangle() == startTriangle2) && (current->Triangle()->link->Adjacent(i) == startTriangle1)))
			{
				continue;
			}
			if (current->Triangle()->link->Adjacent(i) != NULL) //!Blocked(s))
			{
				SeDcdtFace *childFace = current->Triangle()->link->Adjacent(i); //(SeDcdtFace *)s->sym()->fac();
				float width = (current->Back() == NULL) ? INFINITY :
					(current->Triangle()->link->Adjacent((i + 1) % 3) == current->Back()->Triangle()) ?
					current->Triangle()->link->Width(i) : current->Triangle()->link->Width((i + 2) % 3);
				width = Minimum(current->Triangle()->link->Choke(i), width);
				if ((!current->Searched(childFace)) && (width >= 2.0f * r)
					&& ((current->Back() == NULL) || (childFace != current->Back()->Triangle())))
				{
					SrPnt2 p = ClosestPointTo(childFace, x2, y2, r);
//					float x_1, y_1, x_2, y_2, x_3, y_3;
//					TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3);
//					float theta = (current.Back() == NULL) ? 0.0f : (current.Triangle()->link->Adjacent((i + 1) % 3) == current.Back()->Triangle()) ?
//						AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3) : AngleBetween(x_3, y_3, x_1, y_1, x_2, y_2);
					float h = Length(p.x, p.y, x2, y2);
					float minDistance = current->g() + Maximum(current->Triangle()->link->Angle(i) * r, current->h() - h);
SrPnt2 p2 = ClosestPointTo(childFace, x1, y1, r);
					minDistance = Maximum(Length(x1, y1, p2.x, p2.y), minDistance);
					if (minDistance < childFace->link->ShortestPath(update))
					{
						childFace->link->ShortestPath(minDistance, update);
					}
					else if (minDistance >= childFace->link->ShortestPath(update) + childFace->link->LongestEdge())
					{
sr_out << "PREPRUNED!\n";
						continue;
					}
					q.push(new SearchNode(minDistance, h, p, childFace, current, i));
					current->OpenChild();
TriangleMidpoint(childFace, x, y);
sr_out << "  Generated (" << x << ", " << y << ") g = "
<< minDistance << ", h = " << h << ", f = " << (minDistance + h) << "\n";
				}
			}
//			s = s->nxt();
		}
		if (current->OpenChildren() <= 0)
		{
			current->Close();
			delete current;
		}
	}
	//TODO: make sure the SeBase*s are being added to the paths the right way
	//TODO: (interior edges => path lengths should be # of triangles in path - 1)
	//TODO: improve way GetShortestPath gets the vertices (see SeTriangulator::get_channel_boundary())
	//TODO: save stuff to member variables?
sr_out << expanded << " NODES EXPANDED IN SEARCH\n";
	currentPath = shortestPath;
	return (shortestPathLength < INFINITY);
}

//searches the abstract graph for a path between two points
bool SeDcdt::SearchPathBase(float x1, float y1, float x2, float y2, float r, int update)
{
int expanded = 0;
	//initialize the path to empty
	currentPath.size(0);
	SeBase *start;//, *goal;
	//find the triangle containing the start point
	SeTriangulator::LocateResult startResult = LocatePoint(x1, y1, start);
	//if it wasn't found, return a path could not be found
	if (startResult == SeTriangulator::LocateResult::NotFound)
	{
		return false;
	}
	SeDcdtFace *startFace = (SeDcdtFace *)start->fac();
	std::priority_queue<SearchNode *, std::vector<SearchNode *>, check> q;
	SrPnt2 p = ClosestPointTo(startFace, x2, y2, r);
	q.push(new SearchNode(0.0f /*Length(x1, y1, p.x, p.y)*/, Length(p.x, p.y, x2, y2), p, startFace, NULL));
	float shortestPathLength = INFINITY;
	SrPolygon shortestPath;
	while (!q.empty())
	{
		SearchNode *current = q.top();
		q.pop();
expanded++;
float x, y;
TriangleMidpoint(current->Triangle(), x, y);
sr_out << "Expanding (" << x << ", " << y << ") g = " <<
current->g() << ", h = " << current->h() << ", f = " << current->f() << "\n";
		if (current->f() >= shortestPathLength)
		{
			while (!q.empty())
			{
				q.top()->Close();
				delete q.top();
				q.pop();
			}
			break;
		}
		else if (current->g() >= current->Triangle()->link->ShortestPath(update) + current->Triangle()->link->LongestEdge())
		{
sr_out << "PRUNED!\n";
			continue;
		}
		else if (InTriangle(current->Triangle(), x2, y2))
		{
			SrArray<SeBase *> channel = ConstructBaseChannel(current);
			current->Close();
			delete current;
			if (ValidEndpoints(channel, x1, y1, x2, y2, r))
			{
				SrPolygon currentShortestPath;
				float currentPathLength = GetShortestPath(currentShortestPath, channel, x1, y1, x2, y2, r);
sr_out << "GENERATED PATH WITH LENGTH = " << currentPathLength << "\n";
				if (currentPathLength < shortestPathLength)
				{
					shortestPathLength = currentPathLength;
					shortestPath.size(0);
					shortestPath = currentShortestPath;
				}
			}
			continue;
		}
		SeBase *s = current->Triangle()->se();
		for (int i = 0; i < 3; i++)
		{
			if (!Blocked(s))
			{
				SeDcdtFace *childFace = (SeDcdtFace *)s->sym()->fac();
				float width = (current->Back() == NULL) ? INFINITY : (s->nxt()->sym()->fac() == current->Back()->Triangle()) ?
					current->Triangle()->link->Width(i) : current->Triangle()->link->Width((i + 2) % 3);
				if ((!current->Searched(childFace)) && (width >= 2.0f * r)
					&& ((current->Back() == NULL) || (childFace != current->Back()->Triangle())))
				{
					p = ClosestPointTo(childFace, x2, y2, r);
					float x_1, y_1, x_2, y_2, x_3, y_3;
					TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3);
					float theta = (current->Back() == NULL) ? 0.0f : (s->nxt()->sym()->fac() == current->Back()->Triangle()) ?
						AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3) : AngleBetween(x_3, y_3, x_1, y_1, x_2, y_2);
					float h = Length(p.x, p.y, x2, y2);
					float minDistance = current->g() + Maximum(theta * r, current->h() - h);
SrPnt2 p2 = ClosestPointTo(childFace, x1, y1, r);
					minDistance = Maximum(Length(x1, y1, p2.x, p2.y), minDistance);
//					minDistance = Maximum(Length(x1, y1, p.x, p.y), minDistance);
					if (minDistance < childFace->link->ShortestPath(update))
					{
						childFace->link->ShortestPath(minDistance, update);
					}
					else if (minDistance >= childFace->link->ShortestPath(update) + childFace->link->LongestEdge())
					{
sr_out << "PREPRUNED!\n";
						continue;
					}
					q.push(new SearchNode(minDistance, h, p, childFace, current));
					current->OpenChild();
TriangleMidpoint(childFace, x, y);
sr_out << "  Generated (" << x << ", " << y << ") g = "
<< minDistance << ", h = " << h << ", f = " << (minDistance + h) << "\n";
				}
			}
			s = s->nxt();
		}
		if (current->OpenChildren() <= 0)
		{
			current->Close();
			delete current;
		}
	}
sr_out << expanded << " NODES EXPANDED IN SEARCH\n";
	currentPath = shortestPath;
	//TODO: save results as local variables?
	return (shortestPathLength < INFINITY);
}

//construct the channel given the last SearchNode in the solution
SrArray<SeBase *> SeDcdt::ConstructBaseChannel(SearchNode *goal)
{
	SrArray<SeBase *> channel;
	if (goal->Back() == NULL)
	{
		return channel;
	}
	SearchNode *current = goal->Back();
	while (current->Back() != NULL)
	{
		SeBase *s = current->Triangle()->se();
		for (int i = 0; i < 3; i++)
		{
			if (s->sym()->fac() == current->Back()->Triangle())
			{
				channel.push() = s;
				break;
			}
			s = s->nxt();
		}
		current = current->Back();
	}
	channel.revert();
	return channel;
}

//uses the modified funnel algorithm to get the shortest path for nonzero-radius units
//and return the length of this path
float SeDcdt::GetShortestPath(SrPolygon &path, SrArray<SeBase *> Channel, float x1, float y1, float x2, float y2, float r)
{
	//initializes the path
	path.open(true);
	path.size(0);
	//gets the edges that make up the channel
//	SrArray<SeBase *> Channel = _triangulator->get_channel_interior_edges();
	//if there are no edges, there is no path
	if (Channel.size() < 1)
	{
		return 0.0f;
	}
	//if there is only one, the path is a straight line
	else if (Channel.size() < 2)
	{
		path.push().set(x1, y1);
		path.push().set(x2, y2);
		return Length(x1, y1, x2, y2);
	}
	//otherwise, initialize a funnel deque
	//with the starting point
	FunnelDeque fd(x1, y1, r);
	//get the first edge in the channel
	SeBase *s = Channel[0];
	//get the reference whose sym edge
	//belongs to the next triangle in the channel
	for (int i = 0; i < 3; i++)
	{
		if (s->sym()->fac() == Channel[1]->fac())
		{
			break;
		}
		s = s->nxt();
	}
	//get the point on the right side of that edge
	SrPnt2 p;
	p.set(((SeDcdtVertex *)s->vtx())->p);
	//insert it into the deque
	fd.Add(p, FunnelDeque::CornerType::LeftTangent, path);
	SeVertex *right = s->vtx();
	//get the point on the left side of that edge
	s = s->nxt();
	SeVertex *left = s->vtx();
	p.set(((SeDcdtVertex *)s->vtx())->p);
	//insert it into the deque
	fd.Add(p, FunnelDeque::CornerType::RightTangent, path);
	//go through the other edges in the funnel
	for (int i = 1; i < Channel.size() - 1; i++)
	{
		s = Channel[i];
		//get the reference whose sym edge
		//belongs to the next triangle in the channel
		for (int j = 0; j < 3; j++)
		{
			if (s->sym()->fac() == Channel[i + 1]->fac())
			{
				break;
			}
			s = s->nxt();
		}
		//if that edge shares a vertex with the right side
		//of the funnel, the opposite point should be
		//added to the left side of the funnel
		if (s->vtx() == right)
		{
			p.set(((SeDcdtVertex *)s->nxt()->vtx())->p);
			fd.Add(p, FunnelDeque::CornerType::RightTangent, path);
			left = s->nxt()->vtx();
		}
		//otherwise, add it to the right side of the funnel
		else //(s->nxt()->vtx() == left)
		{
			p.set(((SeDcdtVertex *)s->vtx())->p);
			fd.Add(p, FunnelDeque::CornerType::LeftTangent, path);
			right = s->vtx();
		}
	}
	//go to the last edge in the channel
	s = Channel[Channel.size() - 1];
	//find the reference across which is
	//the triangle with the goal in it
	for (int j = 0; j < 3; j++)
	{
		if (InTriangle((SeDcdtFace *)s->sym()->fac(), x2, y2))
		{
			break;
		}
		s = s->nxt();
	}
	//like above, determines whether to add
	//the point to the left or right side of the funnel
	if (s->vtx() == right)
	{
		p.set(((SeDcdtVertex *)s->nxt()->vtx())->p);
		fd.Add(p, FunnelDeque::CornerType::RightTangent, path);
	}
	else //(s->nxt()->vtx() == left)
	{
		p.set(((SeDcdtVertex *)s->vtx())->p);
		fd.Add(p, FunnelDeque::CornerType::LeftTangent, path);
	}
	//finally, add the goal point to the funnel
	p.set(x2, y2);
	fd.Add(p, FunnelDeque::CornerType::Point, path);
	//calculate the length of the path sand return it
	return fd.Length(path);
}

SrPolygon SeDcdt::GetChannelBoundary()
{
	SrPolygon boundary;
	return boundary;
}

SrPolygon &SeDcdt::GetPath()
{
	return currentPath;
}

//Abstract space searching function definitions }