import java.util.*;

//import josx.platform.rcx.TextLCD;
//import josx.platform.rcx.LCD;
import josx.platform.rcx.Motor;
import josx.platform.rcx.Sound;

/**
 * Map.java -- This program is for challenge #5 of the Robotics
 * project. Given a start and goal location, as well as how the points
 * of a map are connected, the robot will calculate the shortest path
 * and then traverse it.
 *
 * Created: Mon Nov 29 21:59:12 2004
 *
 * @author <a href="mailto:albaab01@gettysburg.edu">Disha Al Baqui</a>
 * @author <a href="mailto:bassdo02@gettysburg.edu">Donald W. Bassett</a> 
 * @author <a href="mailto:ranamo01@gettysburg.edu">Monica A. Ranadive</a>
 * @author <a href="mailto:schmky0101@gettysburg.edu">Kyle Schmidt</a>
 * @version 1.0
 */
public class Map extends JavaRCX{
    Vector roadPts;
    Vector path;
    int start,goal;
    
    /**
     * <code>Map</code> - The constructor initializes roadPts (a
     * vector of all the points on the map) as well as ptsExplored (a
     * vector that holds all the points that have been explored - this
     * is used by the BreadthFirstSearcher). The method mapPoints()
     * actually places the points in the roadPts vector.
     */
    public Map() {
	roadPts = new Vector();
	mapPoints();
    } // Map constructor
    

    /**
     * <code>mapPoints</code> - Very important method because it gives
     * all the information about the map to the rest of the
     * program. First it creates all the points and puts them in a
     * two-dimentional array. This array holds all the nodes that each
     * point is connected to (clockwise order ***very important***). 
     *
     */
    private void mapPoints() 
    {	
	int [][] points = {{1,5,5},
			   {4,2,0},
			   {1,3,6},
			   {4,7,2},
			   {1,8,3},
			   {0,6,8,0},
			   {2,7,5},
			   {3,8,6},
			   {7,4,5}};
	for(int i = 0; i < points.length; i++)
	    roadPts.addElement(new RoadPoints(i));
	for(int i = 0; i < points.length; i++){
	    RoadPoints point = (RoadPoints) roadPts.elementAt(i);
	    for(int j = 0; j < points[i].length; j++)
		point.addChild((RoadPoints)roadPts.elementAt(points[i][j]));
	}
    }

    /**
     * <code>setStart</code> - this method sets the start of the path
     * for the robot
     *
     * @param index an <code>int</code> value - sends a RoadPoint
     * index to the function, the start is then set to that index
     */
    public void setStart(int index)
    {
	start = index;
    }

    /**
     * <code>setGoal</code> - method sets a RoadPoint to start
     *
     * @param index an <code>int</code> value - the goal node index
     */
    public void setGoal(int index)
    {
	goal = index;
	RoadPoints point = (RoadPoints) roadPts.elementAt(index);
	point.setGoal();
    }

    /**
     * <code>search</code> - method searches from the start node using
     * a Breadth First Searcher and returns whether or not a path was
     * found to the goal node
     *
     * @return a <code>boolean</code> value - if true, a path was
     * found, if false, a path was not found
     */
    public boolean search()
    {
	BreadthFirstSearcher searcher = new BreadthFirstSearcher();
	SearchNode node = (RoadPoints) roadPts.elementAt(start);
	boolean found = searcher.search(node);
	path = searcher.getPath();
	return found;
    }
    
    /**
     * <code>traverse</code> - method that actually creates the path
     * the robot will take on the map. Using the nodes in the path
     * array, this method creates a list of spokes the robot will take
     * at each intersection in order to successfully reach the goal
     * node.
     *
     */
    public void traverse()
    {
	RCXIntersectionChooser.initialize();
	
	/* 
	   Starts at the root node and then looks at the next step in
	   the path. Adds that spoke to the directions array and then
	   enters the for loop
	*/
	RoadPoints root = (RoadPoints) path.elementAt(path.size() - 1);
	RoadPoints child = (RoadPoints) path.elementAt(path.size() - 2);
	Vector children = root.expand();
	int [] directions = new int [path.size()];
	int directionCount = 0;
	int spoke = children.indexOf(child);
	directions[directionCount] = spoke;
	/* 
	   The program looks at the parent (where it came from) as
	   spoke 0 and then moves through the array of children until
	   it finds the next step in the path (while updating what
	   spoke to take when it comes to that particular
	   interesection). When it finds the right child, it puts the
	   current directionCount (spoke) in the directions array.
	*/
	for(int i = path.size() - 2; i > 0; i--){
	    spoke = 0;
	    directionCount++;
	    RoadPoints node = (RoadPoints) path.elementAt(i);
	    RoadPoints parent = (RoadPoints) path.elementAt(i+1);	    
	    children = node.expand();
	    int index = children.indexOf(parent);
	    child = (RoadPoints) path.elementAt(i-1);
	    while(!child.equals((RoadPoints) children.elementAt(index))){
		index ++;
		if(index == children.size())
		    index = 0;
		spoke ++;
	    }
	    directions[directionCount] = spoke;
	}
	//at the end of this loop, there are a list of spokes to take
	//in the directions array
	RCXIntersectionChooser.move(directions);
    }
    
    public static void main (String [] args)
    {
	Map map = new Map();
	int start = 4;
	int goal = 1;
	    
	map.setStart(start);
	map.setGoal(goal);
	    
	if(map.search()) map.traverse();
    }
} // Map