package Algorithms;

import edu.carleton.motion.*;
import java.io.*;
import java.util.*;
import java.awt.*;

/** An algorithm using the trapezoidal cell decomposition method. The free worldspace is
 * decomposed in trapezoidal cells by "throwing in" the edges of obstacles, one at a time. We then compute
 * an adjacency graph for these cells and perform a graph search to determine a solution path. For the actual
 * solution path, we return to the original cells and move from middle of one cell to the middle of the next cell,
 * passing through the midpoint of their common edge. 
 * This algorithm supports multiple robots. 
 * @author Alina Badus, Jeremy Carr
 * @version 2/09/2004. */

public class Trapezoids implements MPAlgorithm {

    /** A vector of trapezoidal cells. */
    public Vector cellList;
    /** The adjacency matrix representing relations between the cells. */
    public int[][] Tmap;

    /** An internal class describing a trapezoidal cell and its functionality. */
    public class Tcell {
	public Point topLeft, topRight, bottomRight, bottomLeft;
	public Point middle;
	public Polygon cellPolygon;
	//	public Vector leftNeighbors, rightNeighbors;
	public boolean tagged;

	public Tcell() {
	    // leftNeighbors = new Vector(); rightNeighbors = new Vector(); 
	    tagged = false;
	} 

	public Tcell(Point a, Point b, Point c, Point d) {
	    topLeft = new Point(a); topRight = new Point(b);
	    bottomRight = new Point(c); bottomLeft = new Point(d);
	    
	    middle = new Point((int)((topLeft.getX() + topRight.getX())/2),
			       (int)((topLeft.getY() + topRight.getY() + 
				      bottomRight.getY() + bottomLeft.getY())/4));

	    cellPolygon = new Polygon();
	    cellPolygon.addPoint((int)bottomLeft.getX(), (int)bottomLeft.getY());
	    cellPolygon.addPoint((int)topLeft.getX(), (int)topLeft.getY());
	    cellPolygon.addPoint((int)topRight.getX(), (int)topRight.getY());
	    cellPolygon.addPoint((int)bottomRight.getX(), (int)bottomRight.getY());

	    // leftNeighbors = new Vector(); rightNeighbors = new Vector(); 
	    tagged = false;
	}  // Tcell constructor
	
	public Tcell(Tcell myTcell) {
	    
	    topLeft = new Point(myTcell.topLeft); topRight = new Point(myTcell.topRight);
	    bottomRight = new Point(myTcell.bottomRight); 
	    bottomLeft = new Point(myTcell.bottomLeft);
	    
	    middle = new Point((int)((topLeft.getX() + topRight.getX())/2),
			       (int)((topLeft.getY() + topRight.getY() + 
				      bottomRight.getY() + bottomLeft.getY())/4));
	    
	    cellPolygon = new Polygon();
	    cellPolygon.addPoint((int)bottomLeft.getX(), (int)bottomLeft.getY());
	    cellPolygon.addPoint((int)topLeft.getX(), (int)topLeft.getY());
	    cellPolygon.addPoint((int)topRight.getX(), (int)topRight.getY());
	    cellPolygon.addPoint((int)bottomRight.getX(), (int)bottomRight.getY());
	    
	    //  leftNeighbors = new Vector(); rightNeighbors = new Vector(); 
	    tagged = false;
	    
	} //Tcell constructor
	
	public void draw(Graphics g) {
	    g.setColor(Color.magenta);
	    g.drawPolygon(cellPolygon);
	    if (tagged) {
		g.setColor(Color.red);
		g.fillArc((int)middle.getX(), (int)middle.getY(), 5, 5, 0, 360);
	    }
	    else {
		g.setColor(Color.green);
		g.fillArc((int)middle.getX(), (int)middle.getY(), 5, 5, 0, 360);
	    }  // else.. the cell was not tagged
	}  // draw

	public void draw(Graphics g, int num) {
	    g.setColor(Color.magenta);
	    g.drawPolygon(cellPolygon);
	    if (tagged) {
		g.setColor(Color.red);
		//	g.fillArc((int)middle.getX(), (int)middle.getY(), 5, 5, 0, 360);
		String myNumber = "" + num;
		g.drawString(myNumber, (int)middle.getX(), (int)middle.getY());
	    }
	    else {
		g.setColor(Color.green);
		String myNumber = "" + num;
		g.drawString(myNumber, (int)middle.getX(), (int)middle.getY());
		//	g.fillArc((int)middle.getX(), (int)middle.getY(), 5, 5, 0, 360);
	    }  // else.. the cell was not tagged
	}  // draw

	public boolean contains(Point p) {
	    return cellPolygon.contains(p);
	}  // contains

	public void print() {
	    System.out.println("    Top: (" + topLeft.getX() + ", " + topLeft.getY() + ") - (" + topRight.getX() + ", " 
			       + topRight.getY() + ")");
	    System.out.println(" Bottom: (" + bottomLeft.getX() + ", " + bottomLeft.getY() + ") - (" + bottomRight.getX() 
			       + ", " + bottomRight.getY() + ")");	    
	} // print
	
    }  // Tcell
    

    /** Displays the trapezoidal cells, with their corresponding numbers. */
    public void draw (Graphics g) {
	if (cellList == null) 
	    System.out.println("cellList has not been initialized.");
	else {
	    //System.out.println("There are " + cellList.size() + " T-cells to draw");
	    g.setColor(Color.magenta);
	    for (int i = 0; i < cellList.size(); i++) {
	    	Tcell myCell = (Tcell)cellList.elementAt(i);
		myCell.draw(g,i);	
		// draw the adjacency relations
		//x	g.setColor(Color.pink);
		//for (int j = i+1; j < cellList.size(); j++)
	    } // for
	} // else
    } // draw

    /** Sets up the framework for building the trapezoidal map. */
    public void buildMap(Worldspace wspace) {
	System.out.println("Entering buildMap...");
	cellList = new Vector();

	// initialize cellList to be the bounding box (the entire worldspace, in this case)
	cellList.add(new Tcell(new Point(0,0), new Point(wspace.getDimX(),0),
			 new Point(wspace.getDimX(), wspace.getDimY()), 
			       new Point(0,wspace.getDimY())));
	// for every segment in the worldspace, throw the segment in and see what happens
	for (int p = 0; p < wspace.getNumObstacles(); p++) {
	    Polygon currentPoly = wspace.getObstacle(p).getPoly();
	    
	    // first deal with the segment between 0 and n
	    int n = currentPoly.npoints;
	    Point myStart = new Point(currentPoly.xpoints[0], currentPoly.ypoints[0]);
	    Point myEnd = new Point(currentPoly.xpoints[n-1], currentPoly.ypoints[n-1]);
	    intersect(myStart, myEnd);

	    for (int i = 0; i < n-1; i++) {
		myStart = new Point(currentPoly.xpoints[i], currentPoly.ypoints[i]);
		myEnd = new Point(currentPoly.xpoints[i+1], currentPoly.ypoints[i+1]);
		intersect(myStart, myEnd);
	    } // for all the other point	    
	}  // for every polygon in the worldspace
	System.out.println("... finished buildMap");
	
	//Now we have to pop off the cells that are in an Obstacle	
	for (int j = 0; j < wspace.getNumObstacles(); j++) 
	    for (int i = 0; i < cellList.size(); i++) {	
		if (wspace.getObstacle(j).getPoly().contains(((Tcell)cellList.elementAt(i)).middle)) {
		    cellList.removeElementAt(i);
		    i--;
		//((Tcell)cellList.elementAt(i)).tagged = true;
		} // if the polygon contains the center of the trapezoid
	    }  // for every cell in the list
    }  // buildMap

    /** Updates the tmap by considering its intersection with 
     *	the segment determined by first and last. 
     *	Called by buildmap.*/
    public void intersect(Point first, Point last) {
	// notice: the segment should never intersect one of the top or bottom edges of the trapezoid
	boolean found = false;
	// special case: if the segment is vertical
	if (first.getX() == last.getX()) {
	    //System.out.println("    This is a vertical segment");
	    for (int i = 0; i < cellList.size() && !found; i++) {
		if (((Tcell)cellList.elementAt(i)).contains(first)) {
		    Tcell oldCell = (Tcell)cellList.elementAt(i);
		    found = true;
		    
		    double topRatio = (double)(oldCell.topLeft.getY() - 
					       oldCell.topRight.getY())/
			(oldCell.topLeft.getX() - oldCell.topRight.getX());
		    double bottomRatio = (double)(oldCell.bottomLeft.getY() - 
						  oldCell.bottomRight.getY())/
			(oldCell.bottomLeft.getX() - oldCell.bottomRight.getX());
		    double topY = oldCell.topLeft.getY() + topRatio * 
			(oldCell.topRight.getY() - oldCell.topLeft.getY());
		    double bottomY = oldCell.bottomLeft.getY() + 
			bottomRatio * (oldCell.bottomRight.getY() - 
				       oldCell.bottomLeft.getY());
		    // TO DO: FIGURE OUT WHICH WAY TO ROUND; see Trapezoid.java

		    Tcell leftCell = new Tcell(oldCell.topLeft,
					       new Point((int)first.getX(), (int)topY),
					       new Point((int)first.getX(), (int)bottomY), 
					       oldCell.bottomLeft); 
		    Tcell rightCell = new Tcell(new Point(leftCell.topRight), 
						oldCell.topRight, 
						oldCell.bottomRight,
						new Point(leftCell.bottomRight) );

		    // update the vector of cells
		    cellList.removeElementAt(i);
		    cellList.add(leftCell); cellList.add(rightCell);
		}  // if cell contains the point
	    }  // for every cell in the list	    
	} // if segment is vertical
			
	else { //if segment is not vertical
	    //first will be the leftmost point
	    if (first.getX() > last.getX()) {
		Point temp = new Point(first);
		first = last;
		last = temp;
	    }  // if we want to switch the first / last points
	    double myRatio = (double)((double)first.getY() - (double)last.getY())/
		(double)((double)first.getX() - (double)last.getX());
	    //System.out.println("     myRatio = " + myRatio);

	    //Construct a templist of all the cells that are intersected
	    Vector tempList = new Vector();
	    for (int i = 0; i < cellList.size(); i++) {
		Tcell tempCell = new Tcell((Tcell)cellList.elementAt(i));
		
		if (tempCell.contains(first)) 
		    { if (tempCell.contains(last)) { // both first and last are in the cell
			//System.out.println("    Found a cell containing both first " + first.getX() + ", " + first.getY() + 
			//   " and last " + last.getX() + ", " + last.getY());
			double topRatio = (double)(tempCell.topLeft.getY() - tempCell.topRight.getY())/
			    (tempCell.topLeft.getX() - tempCell.topRight.getX());
			double bottomRatio = (double)(tempCell.bottomLeft.getY() - tempCell.bottomRight.getY())/
			    (tempCell.bottomLeft.getX() - tempCell.bottomRight.getX());
			
			double firstTopY = tempCell.topLeft.getY() + topRatio * (first.getX() - tempCell.topLeft.getX());
			double lastTopY = tempCell.topLeft.getY() + topRatio * (last.getX() - tempCell.topLeft.getX());
			double firstBottomY = tempCell.bottomLeft.getY() + bottomRatio * 
			    (first.getX() - tempCell.bottomLeft.getX());
			double lastBottomY = tempCell.bottomLeft.getY() + bottomRatio * 
			    (last.getX() - tempCell.bottomLeft.getX());
			
			Point firstTop = new Point((int)first.getX(), (int)firstTopY);
			Point lastTop = new Point((int)last.getX(), (int)lastTopY);
			Point firstBottom = new Point((int)first.getX(), (int)firstBottomY);
			Point lastBottom = new Point((int)last.getX(), (int)lastBottomY);
			
			Tcell leftCell = new Tcell(tempCell.topLeft, firstTop, firstBottom, tempCell.bottomLeft);
			Tcell topCell = new Tcell(firstTop, lastTop, last, first);
			Tcell bottomCell = new Tcell(first, last, lastBottom, firstBottom);
			Tcell rightCell = new Tcell(lastTop, tempCell.topRight, tempCell.bottomRight, lastBottom);
			
			tempList.add(leftCell); tempList.add(topCell); tempList.add(bottomCell); tempList.add(rightCell);
			cellList.removeElementAt(i);
			i--;
		    }
		   
			else {  // only first is in the cell
			    //System.out.println("    Found a cell containing only first: " + first.getX() + ", " + first.getY());
			    //tempCell.print();
			    
			    // split this cell and put the pieces in tempList
			    double topRatio = (double)(tempCell.topLeft.getY() - tempCell.topRight.getY())/
				(tempCell.topLeft.getX() - tempCell.topRight.getX());
			    double bottomRatio = (double)(tempCell.bottomLeft.getY() - tempCell.bottomRight.getY())/
				(tempCell.bottomLeft.getX() - tempCell.bottomRight.getX());
			    
			    double middleY = first.getY() + myRatio * (tempCell.bottomRight.getX() - first.getX());
			    double topY = tempCell.topLeft.getY() + topRatio * (first.getX() - tempCell.topLeft.getX());
			    double bottomY = tempCell.bottomLeft.getY() + bottomRatio * (first.getX() - tempCell.bottomLeft.getX());
			    // FIGURE OUT ROUNDING, if needed
			    Point middleRight = new Point((int)tempCell.bottomRight.getX(), (int)middleY);
			    Point middleTop = new Point((int)first.getX(), (int)topY);
			    Point middleBottom = new Point((int)first.getX(), (int)bottomY);
			    
			    Tcell leftCell = new Tcell(tempCell.topLeft, middleTop, middleBottom, tempCell.bottomLeft);
			    Tcell topCell = new Tcell(middleTop, tempCell.topRight, middleRight, first);
			    Tcell bottomCell = new Tcell(first, middleRight, tempCell.bottomRight, middleBottom);
			    
			    tempList.add(new Tcell(leftCell)); tempList.add(new Tcell(topCell)); tempList.add(new Tcell(bottomCell));
			    //System.out.println(" ... and split it into three pieces");
			    cellList.removeElementAt(i);	
			    i--;
			} // else
		    }  // if it contains the first point
		    else { 
		    if (tempCell.contains(last)) { // only last is in cell
			//System.out.println("    Found a cell containing only last: " + last.getX() + ", " + last.getY());
			//tempCell.print();
			//System.out.println("   Received " + tempList.size() + " cells in tempList");
			// split this cell and put the pieces in tempList
			double topRatio = (double)(tempCell.topLeft.getY() - tempCell.topRight.getY())/
			    (tempCell.topLeft.getX() - tempCell.topRight.getX());
			double bottomRatio = (double)(tempCell.bottomLeft.getY() - tempCell.bottomRight.getY())/
			    (tempCell.bottomLeft.getX() - tempCell.bottomRight.getX());
			
			double middleY = last.getY() - myRatio * (last.getX() - tempCell.bottomLeft.getX());
			double topY = tempCell.topLeft.getY() + topRatio * (last.getX() - tempCell.topLeft.getX());
			double bottomY = tempCell.bottomLeft.getY() + bottomRatio * (last.getX() - tempCell.bottomLeft.getX());
			// FIGURE OUT ROUNDING, if needed
			Point middleLeft = new Point((int)tempCell.bottomLeft.getX(), (int)middleY);
			Point middleTop = new Point((int)last.getX(), (int)topY);
			Point middleBottom = new Point((int)last.getX(), (int)bottomY);
			
			Tcell topCell = new Tcell(tempCell.topLeft, middleTop, last, middleLeft);
			Tcell bottomCell = new Tcell(middleLeft, last, middleBottom, tempCell.bottomLeft);
			Tcell rightCell = new Tcell(middleTop, tempCell.topRight, tempCell.bottomRight, middleBottom);
			
			tempList.add(new Tcell(topCell)); tempList.add(new Tcell(bottomCell)); tempList.add(new Tcell(rightCell));
			cellList.removeElementAt(i);
			i--;
		    }
		    else  // none is in cell, so check for segment case
			if (first.getX() < tempCell.bottomLeft.getX() && last.getX() >= tempCell.bottomRight.getX()) {
			    // potential intersection... check the y's
			    double leftY = first.getY() + myRatio * (tempCell.bottomLeft.getX() - first.getX());
			    
			    if (leftY <= tempCell.bottomLeft.getY() && leftY >= tempCell.topLeft.getY()) { // split the cell!
				
				//System.out.println("    Found a cell through which the segment passes; original cell is: ");
				//tempCell.print();
				//System.out.println("    leftY = " + leftY);
				double rightY = first.getY() + myRatio * (tempCell.bottomRight.getX() - first.getX());
				
				// FIGURE OUT HOW TO ROUND THESE Y's, if needed
				Point leftMiddle = new Point((int)tempCell.bottomLeft.getX(), (int)leftY);
				Point rightMiddle = new Point((int)tempCell.bottomRight.getX(), (int)rightY);
				Tcell topCell = new Tcell(tempCell.topLeft, tempCell.topRight, rightMiddle, leftMiddle);
				Tcell bottomCell = new Tcell(leftMiddle, rightMiddle, tempCell.bottomRight, tempCell.bottomLeft);
				tempList.add(new Tcell(topCell));
				tempList.add(new Tcell(bottomCell));
				//System.out.println(" ... and created the cells: ");
				//topCell.print(); bottomCell.print();
				cellList.removeElementAt(i);
				i--;
			    } // if split the cell
			} // if there are chances for intersection
		    } // for each cell in the list
		//tempList now contains all the pieces from Tcell's that intersected the line segment; copy this to the cellList
		//System.out.println("  This segment created " + tempList.size() + " new cells");
	    }
	    //	    System.out.println("Created the new cells: ");
	    for (int index = 0; index < tempList.size(); index++) {
		Tcell tempCell = new Tcell((Tcell)tempList.elementAt(index));
		if ((int)tempCell.topLeft.getX() != (int)tempCell.topRight.getX()) // if cell is not too thin... 
		{    cellList.add(tempCell);
		//		tempCell.print();
		}
	    }
	    //System.out.println("NOW there are " + cellList.size() + " cells in the universe.");
	}  // if segment is not vertical
    } // oldIntersect

    /** Given a cellList, it turns Tmap into an adjacency matrix for the cells. */
    public void buildMatrix() {
	int ncells = cellList.size();
	//System.out.println("buildMatrix has received a list of " + ncells + " cells");

	Tmap = new int[ncells][ncells];

	for (int i = 0; i < ncells; i++) 
	    for (int j = i+1; j < ncells; j++){
		Tcell firstCell = (Tcell)cellList.elementAt(i);
		Tcell secondCell = (Tcell)cellList.elementAt(j);

		if ((int)firstCell.bottomRight.getX() == (int)secondCell.bottomLeft.getX()) // firstCell is to the left of secondCell
		    if (firstCell.bottomRight.getY() < secondCell.topLeft.getY() || firstCell.topRight.getY() > secondCell.bottomLeft.getY()); 
		    else { // they are adjacent!
			Tmap[i][j] = 1; Tmap[j][i] = 1;
			//System.out.println("   equal x: " + firstCell.bottomRight.getX() + " " + secondCell.bottomLeft.getX());
		    }

		if ((int)firstCell.bottomLeft.getX() == (int)secondCell.bottomRight.getX()) // firstCell is to the right of secondCell
		    if (secondCell.bottomRight.getY() < firstCell.topLeft.getY() || secondCell.topRight.getY() > firstCell.bottomLeft.getY());
		    else { // they are adjacent!
			Tmap[i][j] = 1; Tmap[j][i] = 1;
			//System.out.println("   equal x: " + firstCell.bottomLeft.getX() + " " + secondCell.bottomRight.getX());
		    }		    
	    } // for every decent pair of cells		
    }

    /** Performs a graph search using the adjacency matrix of trapezoidal cells. */
    public Vector findTrapPath(int startIndex, int endIndex) {
	// basically, find a path in the graph described by Tmap
	// depth-first search? 
	int ncells = cellList.size();

	// make a temp copy of Tmap, where we can mark visited edges and such
	int[][] tempMap = new int[ncells][ncells];
	for (int i = 0; i < ncells; i++) 
	    for (int j = 0; j < ncells; j++) tempMap[i][j] = Tmap[i][j];

	// breadth-first search; how to keep track of the path? 
	Stack searchStack = new Stack();
	Stack soluStack = new Stack();
	boolean hasNeighbor = false;
	int current = endIndex;
	
	searchStack.push(new Integer(current));
	soluStack.push(new Integer(current));

	do {	
	    current = ((Integer)searchStack.pop()).intValue();
	    tempMap[current][current] = 13; //mark as visited
	    
	    if (current == startIndex) { //Destination reached
		//System.out.println("found destination");
		soluStack.push(new Integer(current));
		Vector tempPath = new Vector();
		while (!soluStack.empty()){
		    Integer myInt = (Integer)soluStack.pop();
		    tempPath.add(myInt);
		}	 
		return tempPath;
	    } //if destination reached
	    else {
		hasNeighbor = false;
		for (int i = 0; i < ncells; i++) {
		    if (tempMap[i][current] == 1 && tempMap[i][i] == 0) { //if neighbor and notvisited
			hasNeighbor = true;
			searchStack.push(new Integer(i));
			tempMap[i][i] = 13;
			//System.out.println("Adding the neighbor: " + i);
		    }
		}
		if (!hasNeighbor) {
		   
		    int searchIndex = ((Integer)searchStack.peek()).intValue();
		    int solIndex = 0;
		    do {
			solIndex = ((Integer)soluStack.pop()).intValue();
		    } while (tempMap[searchIndex][solIndex] == 0); 
		    soluStack.push(new Integer(solIndex));
		    //		    soluStack.pop();
		}
		if (hasNeighbor) soluStack.push(new Integer(current));
	    }
	}
	while(!searchStack.empty());
	return null;
}

    /** The main method. Given a scenario, calls the other methods and returns a vector
     * of solution paths (one path for every robot in the Scenario). */
    public Vector solve(Scenario problem)
    {
	Vector paths = new Vector(problem.getNRobots());
	
	System.out.println("MapTrapezoids received a problem with " + problem.getNRobots() + " robots / paths");
	
	buildMap(problem.getWorldspace());
	buildMatrix();
	
		for (int k=0; k < problem.getNRobots(); k++)
		    paths.add(k, new SolutionPath());
		
		Vector tempPath;
		Point pbuff;

		Vector tempbuff;

		// for each robot/SE pair in the problem...
		for (int r = 0; r < problem.getNRobots(); r++) {
		    System.out.println("computing a path for robot " + r);
		    tempPath = new Vector();
		    // put the start & end points into a new vector
		    tempPath = ((SolutionPath) problem.getPath(r)).getPath();


		    Point startPoint = (Point)tempPath.elementAt(0);
		    Point endPoint = (Point)tempPath.elementAt(tempPath.size() - 1);
		    
		    tempPath = new Vector(); // throw away whatever was still in the path

		    // what shall we do if the robot is outside the worldspace?  
		    if (startPoint.getX() > problem.getWorldspace().getDimX() || startPoint.getX() < 0 
			|| startPoint.getY() > problem.getWorldspace().getDimY() || startPoint.getY() < 0) ;// if robot is outside the worldspace
		    if (endPoint.getX() > problem.getWorldspace().getDimX() || endPoint.getX() < 0 
			|| endPoint.getY() > problem.getWorldspace().getDimY() || endPoint.getY() < 0) ;// if robot is outside the worldspace

		    // figuring out which cells these points belong to
		    int startIndex = 0, endIndex = 0;
		    for (int i = 0; i < cellList.size(); i++) {
			if (((Tcell)cellList.elementAt(i)).contains(startPoint))
			    startIndex = i;
			if (((Tcell)cellList.elementAt(i)).contains(endPoint)) endIndex = i;
			}

		    if (startIndex == endIndex) // no need to traverse any cells; StraightLine!	    
			((SolutionPath)paths.elementAt(r)).setPath(tempPath);
		    else { // do stuff
			System.out.println(" startIndex = " + startIndex + " endIndex = " + endIndex);
			Vector trapSolutions = new Vector();

			//buildMatrix(); // UGLY, UGLY HACK!!!
			trapSolutions = findTrapPath(startIndex, endIndex); //do something silly in the meanwhile
					  
			if (trapSolutions != null) {// build a SolutionPath from this vector of indices

			    System.out.println("The " + r + "-th path is: ");
			    // take care of the stuff in the first cell
			    tempPath.add(startPoint); 
			    // move from here in a straight line to the middle of the first cell
			    tempPath.add(((Tcell)cellList.elementAt(startIndex)).middle);
			    int prev = startIndex, cur = 0;

			    System.out.println(" first cell in the path is " + ((Integer)trapSolutions.elementAt(0)).intValue());

			    for (int p = 1; p < trapSolutions.size()-1; p++) {
				cur = ((Integer)trapSolutions.elementAt(p)).intValue();
				//System.out.println(" prev = " + prev + " , cur = " + cur);

				Tcell curCell = (Tcell)cellList.elementAt(cur);
				Tcell prevCell = (Tcell)cellList.elementAt(prev);

				System.out.print(cur + " ");
				((Tcell)cellList.elementAt(cur)).tagged = true; 
				// move from the previous cell "prev" to the center of cell "cur"
				// direction comes into play...  
				Point boundaryPoint = new Point();
				int boundaryX = 0, boundaryY = 0;
				if (curCell.bottomLeft.getX() == prevCell.bottomRight.getX()) { // curCell is to the right of prevCell
				    boundaryX = (int)curCell.bottomLeft.getX();
				    int topY = (int)Math.max(curCell.topLeft.getY(), prevCell.topRight.getY());
				    int botY = (int)Math.min(curCell.bottomLeft.getY(), prevCell.bottomRight.getY());
				    boundaryY = (int)((topY + botY)/2);
				}
				else { // curCell is to the left of prevCell
				    boundaryX = (int)curCell.bottomRight.getX();
				    int topY = (int)Math.max(curCell.topRight.getY(), prevCell.topLeft.getY());
				    int botY = (int)Math.min(curCell.bottomRight.getY(), prevCell.bottomLeft.getY());
				    boundaryY = (int)((topY + botY)/2);
				}
				
				//System.out.println("    boundary point: " + boundaryX + ", " + boundaryY);
				tempPath.add(new Point(boundaryX, boundaryY));
				tempPath.add(((Tcell)cellList.elementAt(cur)).middle);

				prev = cur; // move on... 				
			    }
			    // this should leave us in the middle of the cell containing the end point... 
			    tempPath.add(((Tcell)cellList.elementAt(endIndex)).middle);
			    tempPath.add(endPoint);

			    //System.out.println(" ");
			((SolutionPath)paths.elementAt(r)).setPath(tempPath);
			} 
			else { // figure out what to do if there is no path for the robot... 
			    System.out.println("Didn't find a path for this robot");
			}
		    } // else
		    // Put this vector into the vector of paths..
		}
	
		return paths;
    }
}