package CK.IA;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Enumeration;
import java.util.HashSet;
import java.util.Hashtable;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.Vector;

public class WaypointNavigation {
	private double[][] edge_distances;
	/**
	 * Array of 'edges'. Not used.
	 */
	private Vector<Line3d> edges;
	/**
	 * Hashtable with mapping of waypoint - connected waypoints. Uses Hashtable and HashSet for faster search and insertion operations.
	 */
	private Hashtable<Integer, HashSet<Integer>> neighborhoods;
	private double[][] rectilinear_distances;

	/**
	 * Array of waypoints (Tridimensional vectors). The index is used in the paths.
	 */
	private Vector<Vector3d> waypoints;

	public WaypointNavigation(Vector<Line3d> paths, Vector<Vector3d> waypoints) {
		this.edges = paths;
		this.waypoints = waypoints;
		computeNeighborhoods();
		computeEdgeDistances();
		computeRectilinearDistances();
	}

	public Vector<Integer> AStar(int start, int end) {
		Hashtable<Integer, Integer> camefrom = new Hashtable<Integer, Integer>();
		Vector<Integer> chosen_nodes = new Vector<Integer>();
		OrderedLinkedList openset = new OrderedLinkedList();
		Hashtable<Integer, AStarNode> closedset = new Hashtable<Integer, AStarNode>();
		openset.add(new AStarNode(start, (float) getEdgeLength(0, 0), (float) getRectilinearDistance(start, end)));
		AStarNode current;
		int current_index = -1;
		while (openset.size() > 0) {
			current = (AStarNode) openset.getFirst();
			current_index = current.index;
			if (current_index == end) {
				return (createPathAStarAsVector(current_index, start, camefrom));
			}
			chosen_nodes.add(current_index);
			closedset.put(current_index, current);
			openset.remove(current);
			HashSet<Integer> neighborhood = getNeighborhood(current_index);
			if (neighborhood != null && neighborhood.size() > 0) {
				Iterator<Integer> neighborhood_iterator = neighborhood.iterator();
				while (neighborhood_iterator.hasNext()) {
					int neighbor_index = neighborhood_iterator.next();
					if (!closedset.containsKey(neighbor_index)) {
						float travelled_distance_start = (float) (current.getTravelledDistanceFromStart() + getEdgeLength(current_index,
								neighbor_index));
						float estimated_distance_goal = (float) getRectilinearDistance(neighbor_index, end);
						// check if neighbor node is on openset
						AStarNode neighbor_node = getNode(openset, neighbor_index);
						if (neighbor_node != null) {
							float last_travelled_distance_start = neighbor_node.travelled_distance_start;
							// current visit better?
							if (travelled_distance_start < last_travelled_distance_start) {
								// neigbor visited early with greater distance
								openset.remove(neighbor_node);
								AStarNode node = new AStarNode(neighbor_index, travelled_distance_start, estimated_distance_goal);
								openset.add(node);
								camefrom.put(neighbor_index, current_index);
							} else {
								// neighbor visited early with shorter distance
							}
						} else {
							// neighbor not yet visited, add to openset
							AStarNode node = new AStarNode(neighbor_index, travelled_distance_start, estimated_distance_goal);
							openset.add(node);
							camefrom.put(neighbor_index, current_index);
						}
					}
				}
			}
		}
		return null;
	}

	public void computeDistance(ArrayList<Integer> waypointsi, int waypoint_src_i) {
		Vector3d waypoint_dst = getWaypoint(waypoint_src_i);
		Iterator<Integer> iterator = waypointsi.iterator();
		while (iterator.hasNext()) {
			Integer waypoint_dst_i = iterator.next();
			Vector3d waypoint_src = getWaypoint(waypoint_dst_i);
			waypoint_dst.distanceTo(waypoint_src);
		}
	}

	public void computeEdgeDistances() {
		edge_distances = new double[waypoints.size()][waypoints.size()];
		initializeEdgeDistances();
		Iterator<Line3d> edge_i = edges.iterator();
		while (edge_i.hasNext()) {
			Line3d edge = edge_i.next();
			int first = edge.first;
			int second = edge.second;
			double distance = getWaypoint(first).distanceTo(getWaypoint(second));
			edge_distances[first][second] = distance;
			edge_distances[second][first] = distance;
		}
	}

	/**
	 * Digests the constructor's path listing into a set of waypoints and connected ones.
	 */
	public void computeNeighborhoods() {
		neighborhoods = new Hashtable<Integer, HashSet<Integer>>();
		Enumeration<Line3d> path_enum = edges.elements();
		HashSet<Integer> connected;
		// cycle paths
		while (path_enum.hasMoreElements()) {
			Line3d l = path_enum.nextElement();
			// create paths for both wp and second wp
			// get mapping for first wp
			connected = neighborhoods.get(l.first);
			// no list of connected wp exists, create new list
			if (connected == null) {
				connected = new HashSet<Integer>();
			}
			// add current neighbor
			connected.add(l.second);
			neighborhoods.put(l.first, connected);
			// get mapping for second wp
			connected = neighborhoods.get(l.second);
			// no list of connected wp exists, create new list
			if (connected == null) {
				connected = new HashSet<Integer>();
			}
			// add current neighbor
			connected.add(l.first);
			neighborhoods.put(l.second, connected);
		}
		System.out.println("Waypoints:" + waypoints.size() + " Edges:" + edges.size());
	}

	public void computeRectilinearDistances() {
		rectilinear_distances = new double[waypoints.size()][waypoints.size()];
		double distance;
		for (int i = 0; i < rectilinear_distances.length; i++) {
			for (int j = i; j < rectilinear_distances[i].length; j++) {
				if (i == j) {
					distance = 0.0;
				} else {
					distance = getWaypoint(i).distanceTo(getWaypoint(j));
				}
				rectilinear_distances[i][j] = distance;
				rectilinear_distances[j][i] = distance;
			}
		}
	}

	/**
	 * Computes the path's length.
	 * 
	 * @param path
	 *            An array of waypoints.
	 * @return the total distance from the first to the last array's waypoint.
	 */
	public double computeTotalDistance(ArrayList<Integer> path) {
		double distance = 0;
		if (path.size() > 1) {
			// run all path's waypoints and compute total path distance
			for (int index = 1; index < path.size(); index++) {
				Vector3d waypoint0 = getWaypoint(index - 1);
				Vector3d waypoint1 = getWaypoint(index);
				distance += waypoint0.distanceTo(waypoint1);
			}
		}
		return distance;
	}

	/**
	 * Computes the total lengths of all paths.
	 * 
	 * @param the
	 *            set of paths to calculate lengths.
	 * @return an hashtable with the the paths and each one's total length
	 */
	public Hashtable<ArrayList<Integer>, Double> computeTotalDistance(HashSet<ArrayList<Integer>> paths) {
		Hashtable<ArrayList<Integer>, Double> distances = new Hashtable<ArrayList<Integer>, Double>();
		Iterator<ArrayList<Integer>> pathsi = paths.iterator();
		while (pathsi.hasNext()) {
			ArrayList<Integer> path = pathsi.next();
			double distance = computeTotalDistance(path);
			distances.put(path, distance);
		}
		return distances;
	}

	/**
	 * Creates all paths from start waypoint to the end waypoint, without any loops between them.
	 * 
	 * @param start
	 *            The identifier of the starting waypoint
	 * @param end
	 *            The identifier of the ending waypoint
	 * @return A set of arrays comprised of the computed paths.
	 */
	public HashSet<ArrayList<Integer>> createAllPaths(int start, int end) {
		long init_time = System.currentTimeMillis();
		// int computed_steps = 0, inner_steps = 0;
		HashSet<ArrayList<Integer>> discovered_paths = new HashSet<ArrayList<Integer>>();
		HashSet<ArrayList<Integer>> discovered_paths_back = new HashSet<ArrayList<Integer>>();
		// start with a path comprised of the starting waypoint
		ArrayList<Integer> current_path = new ArrayList<Integer>();
		current_path.add(start);
		discovered_paths.add(current_path);
		boolean explorable;
		// while there are available nodes to visit
		do {
			// let's see if there's anything to explore
			explorable = false;
			// set discovered paths backup to the last discovered paths
			discovered_paths_back.clear();
			discovered_paths_back.addAll(discovered_paths);
			// cycle the through the discovered paths backup and extend last
			// waypoint
			Iterator<ArrayList<Integer>> pathi = discovered_paths_back.iterator();
			while (pathi.hasNext()) {
				current_path = pathi.next();
				// get the path's last waypoint
				Integer paths_last_waypoint = current_path.get(current_path.size() - 1);
				// if we haven't arrived at the destination, compute new paths
				if (paths_last_waypoint.intValue() != end) {
					// get the neighbors
					HashSet<Integer> neighborhood = getNeighborhood(paths_last_waypoint);
					if (neighborhood != null && neighborhood.size() > 0) {
						// create a new path including the one to this waypoint
						// and for each neighbor
						Iterator<Integer> neighi = neighborhood.iterator();
						// cycle through the neighbors
						// path blocked?
						while (neighi.hasNext()) {
							Integer neighbor = neighi.next();
							// check for loops, ie, do not add waypoints already
							// visited
							if (!current_path.contains(neighbor)) {
								// a new neighbor to explore
								explorable = explorable || true;
								// make a copy of the current path
								ArrayList<Integer> newpath = new ArrayList<Integer>(current_path);
								// add the neighbor to the new path
								newpath.add(neighbor);
								// store the new path on the list of discovered
								// paths
								discovered_paths.add(newpath);
								// computed_steps++;
							}
							// inner_steps++;
						}
					}
					discovered_paths.remove(current_path);
				}
			}
		} while (explorable);
		System.out.println("Generated paths:" + discovered_paths.size());
		// System.out.println("Total steps:" + inner_steps + " Computed steps:"
		// + computed_steps);
		long dif_time = System.currentTimeMillis() - init_time;
		System.out.println("Computation time:" + dif_time + " ms");
		return discovered_paths;
	}

	public LinkedList<Integer> createPathAStarAsLinkedList(int current_index, int start, Hashtable<Integer, Integer> camefrom) {
		LinkedList<Integer> ll = new LinkedList<Integer>();
		ll.addFirst(current_index);
		while (current_index != start) {
			// System.out.println(current_index);
			current_index = camefrom.get(current_index);
			ll.addFirst(current_index);
		}
		return ll;
	}

	public Vector<Integer> createPathAStarAsVector(int current_index, int start, Hashtable<Integer, Integer> camefrom) {
		Vector<Integer> ll = new Vector<Integer>();
		ll.add(current_index);
		while (current_index != start) {
			// System.out.println(current_index);
			current_index = camefrom.get(current_index);
			ll.add(0, current_index);
		}
		return ll;
	}

	public void dumpEdgeDistances() {
		String s;
		for (int i = 0; i < edge_distances.length; i++) {
			s = "";
			for (int j = 0; j < edge_distances[i].length; j++) {
				s += edge_distances[i][j] + "\t";
			}
			System.out.println(s);
		}
	}

	public void dumpPathsDistances(Hashtable<ArrayList<Integer>, Double> distances) {
		Enumeration<ArrayList<Integer>> paths = distances.keys();
		while (paths.hasMoreElements()) {
			ArrayList<Integer> path = paths.nextElement();
			Double distance = distances.get(path);
			System.out.println(distance);
		}
	}

	public void dumpRectilinearDistances() {
		String s;
		for (int i = 0; i < rectilinear_distances.length; i++) {
			s = "";
			for (int j = 0; j < rectilinear_distances[i].length; j++) {
				s += rectilinear_distances[i][j] + "\t";
			}
			System.out.println(s);
		}
	}

	public HashSet<Integer> flatPaths(Hashtable<Integer, HashSet<Integer>> ht) {
		Enumeration<HashSet<Integer>> elements = ht.elements();
		HashSet<Integer> flat = new HashSet<Integer>();
		while (elements.hasMoreElements()) {
			HashSet<Integer> nextElement = elements.nextElement();
			Iterator<Integer> iterator = nextElement.iterator();
			while (iterator.hasNext()) {
				Integer next = iterator.next();
				flat.add(next);
			}
		}
		return flat;
	}

	public double getEdgeLength(int start, int end) {
		return edge_distances[start][end];
	}

	public Vector<Vector3d> getIndexedPath(int from, int to) {
		Vector<Integer> astar = AStar(from, to);
		Vector<Vector3d> l = new Vector<Vector3d>();
		if (astar != null && astar.size() > 0) {
			for (int i : astar) {
				l.add(getWaypoint(i));
			}
		}
		return l;
	}

	/**
	 * Finds the nearest waypoint to the given position, within a specified tolerance
	 * 
	 * @param position
	 *            the position to get the nearest waypoint
	 * @param precision
	 *            the maximum distance allowed between the position and the waypoint
	 * @return index of the nearest waypoint on the waypoint's list
	 */
	public int getNearestWaypoint(Vector3d position, double precision) {
		Vector3d currentv = null;
		int nearesti = -1;
		double currentdist = 0;
		double nearestdist = Double.MAX_VALUE;
		int index = 0;
		while (index < waypoints.size()) {
			currentv = getWaypoint(index);
			currentdist = currentv.distanceTo(position);
			if (currentdist < nearestdist) {
				nearestdist = currentdist;
				nearesti = index;
				// this should be very rare, but if happens, even better
				if (currentdist <= precision)
					break;
			}
			index++;
		}
		return nearesti;
	}

	/**
	 * Given a set of indexes returns a hashtable with each one's neighborhood
	 * 
	 * @param indexes
	 *            The waypoints to get the neighborhood from
	 * @return A hashtable with the waypoints and neighborhoods
	 */
	public Hashtable<Integer, HashSet<Integer>> getNeighborhood(HashSet<Integer> indexes) {
		Hashtable<Integer, HashSet<Integer>> neighborhoods = new Hashtable<Integer, HashSet<Integer>>();
		// iterate through the indexes
		Iterator<Integer> iterator = indexes.iterator();
		while (iterator.hasNext()) {
			// get the index
			Integer index = iterator.next();
			// get it's neighborhood
			HashSet<Integer> neighborhood = getNeighborhood(index);
			// store the neighborhood
			neighborhoods.put(index, neighborhood);
		}
		// return data
		return neighborhoods;
	}

	/**
	 * Returns the list of connected waypoints to the given one
	 * 
	 * @param index
	 *            the waypoint to get the neighborhood from
	 * @return the set of indexes containing the neighborhood
	 */
	public HashSet<Integer> getNeighborhood(int index) {
		return neighborhoods.get(index);
	}

	public AStarNode getNode(OrderedLinkedList openset, int index) {
		for (int i = 0; i < openset.size(); i++) {
			AStarNode object = (AStarNode) openset.get(i);
			if (object.index == index)
				return object;
		}
		return null;
	}

	public LinkedList<Vector3d> getPathAsLinkedList(Vector3d from, Vector3d to) {
		int from_i = getNearestWaypoint(from, 0);
		int to_i = getNearestWaypoint(to, 0);
		Vector<Integer> astar = AStar(from_i, to_i);
		LinkedList<Vector3d> path = new LinkedList<Vector3d>();
		if (astar != null && astar.size() > 0) {
			for (int i : astar) {
				path.addLast(getWaypoint(i));
			}
		}
		// because the destination may be different than the last waypoint, add it to the path
		path.addLast(to);
		return path;
	}

	public Vector<Vector3d> getPathAsVector(Vector3d from, Vector3d to) {
		int from_i = getNearestWaypoint(from, 0);
		int to_i = getNearestWaypoint(to, 0);
		Vector<Integer> astar = AStar(from_i, to_i);
		Vector<Vector3d> path = new Vector<Vector3d>();
		if (astar != null && astar.size() > 0) {
			for (int i : astar) {
				path.add(getWaypoint(i));
			}
		}
		// because the destination may be different than the last waypoint, add it to the path
		path.add(to);
		return path;
	}

	public Vector3d getRandomWaypoint() {
		return getWaypoint((int) (Math.random() * waypoints.size()));
	}

	public double getRectilinearDistance(int start, int end) {
		return rectilinear_distances[start][end];
	}

	/**
	 * Returns the waypoint at the specified index
	 * 
	 * @param index
	 *            the position on the waypoint list
	 * @return the waypoint at the given index
	 */
	public Vector3d getWaypoint(int index) {
		return waypoints.elementAt(index);
	}

	public void initializeEdgeDistances() {
		for (int i = 0; i < edge_distances.length; i++) {
			Arrays.fill(edge_distances[i], Double.POSITIVE_INFINITY);
			edge_distances[i][i] = 0.0;
		}
	}
}
