package cz.cuni.amis.pogamut.ut2004.agent.navigation.floydwarshall;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.logging.Logger;

import javax.vecmath.Point3d;

import cz.cuni.amis.pogamut.ut2004.agent.module.AgentModule;
import cz.cuni.amis.pogamut.ut2004.communication.messages.UnrealId;
import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointNeighbourLink;
import cz.cuni.amis.pogamut.ut2004.communication.translator.events.MapPointListObtained;
import cz.cuni.amis.utils.collections.MyCollections;

/**
 * Private map using Floyd-Warshall for path-finding.
 * <p><p>
 * It should be initialized upon receiving {@link MapPointListObtained} event. It precomputes
 * all the paths inside the environment using Floyd-Warshall algorithm (O(n^3)). Use
 * getReachable(), getDistance(), getPath() to obtain the info about the path.
 * <p><p>
 * Based upon the implementation from Martin Krulis with his kind consent - Thank you!
 * <p><p>
 * NOTE: requires O(navpoints.size^3) of memory ~ which is 10000^3 at max for UT2004 (usually
 * maps have are 3000 navpoints). If you are going to use more than one bot in your simulation,
 * we advise you to create a singleton that will provide an instance for all your bots.
 * 
 * @author Martin Krulis
 * @author Jakub Gemrot
 */
public class FloydWarshallMap extends AgentModule {
	
	public static class PathMatrixNode {
		
		private float distance = Float.POSITIVE_INFINITY;
		private Integer viaNode = null;
		private List<NavPoint> path = null;
		
		/**
		 * Doesn't leading anywhere
		 */
		public PathMatrixNode() {			
		}
		
		public PathMatrixNode(float distance) {
			this.distance = distance;
		}
		
		public float getDistance() {
			return distance;
		}

		public void setDistance(float distance) {
			this.distance = distance;
		}

		/**
		 * Returns indice.
		 * @return
		 */
		public Integer getViaNode() {
			return viaNode;
		}

		public void setViaNode(Integer indice) {
			this.viaNode = indice;
		}

		public List<NavPoint> getPath() {
			return path;
		}

		public void setPath(List<NavPoint> path) {
			this.path = path;
		}
		
	}
	
	/**
	 * Flag mask representing unusable edge.
	 */
	public static final int BAD_EDGE_FLAG = 
		LinkFlag.FLY.get() | LinkFlag.LADDER.get() | 	LinkFlag.PROSCRIBED.get() |
	    LinkFlag.SWIM.get() | LinkFlag.PLAYERONLY.get() | LinkFlag.FORCED.get();	
	
	/**
	 * Prohibited edges.
	 */
	protected int badEdgeFlag = 0;
	
	/**
	 * Hash table converting navPoint IDs to our own indices.
	 */
	protected Map<UnrealId, Integer> navPointIndices;
	
	/**
	 * Mapping indices to nav points.
	 * <p><p>
	 * WILL BE NULL AFTER CONSTRUCTION! SERVES AS A TEMPORARY "GLOBAL VARIABLE" DURING
	 * FLOYD-WARSHALL COMPUTATION AND PATH RECONSTRUCTION.
	 */
	protected Map<Integer, NavPoint> indicesNavPoints;
	
	// Warshall's matrix of distances.
	protected PathMatrixNode[][] pathMatrix;
	
	public FloydWarshallMap(MapPointListObtained map, Logger log) {
		this(map, BAD_EDGE_FLAG, log);
	}
		
	public FloydWarshallMap(MapPointListObtained map, int badEdgeFlag, Logger log) {
		super(log);
		log.fine(this + ": Beginning Floyd-Warshall algorithm...");
		long start = System.currentTimeMillis();
		this.badEdgeFlag = badEdgeFlag;
		
		// prepares data structures
		List<NavPoint> navPoints = MyCollections.asList(map.getNavPoints().values());
		int size = navPoints.size();		
		navPointIndices = new HashMap<UnrealId, Integer>(size);
		indicesNavPoints = new HashMap<Integer, NavPoint>(size);
		pathMatrix = new PathMatrixNode[size][size];
		
		// Initialize navPoint indices mapping.
		for (int i = 0; i < navPoints.size(); ++i) {
			navPointIndices.put(navPoints.get(i).getId(), i);
			indicesNavPoints.put(i, navPoints.get(i));
		}
		
		// Initialize distance matrix.
		for (int i = 0; i < size; i++) {
			for (int j = 0; j < size; j++) {
				pathMatrix[i][j] = new PathMatrixNode((i == j) ? 0.0f : Float.POSITIVE_INFINITY);
			}
		}

		// Set edge lengths into distance matrix.
		for (int i = 0; i < size; i++) {
			Point3d location = navPoints.get(i).getLocation().getPoint3d();
			for (NavPointNeighbourLink link : navPoints.get(i).getOutgoingEdges().values()) {
				if (checkLink(link)) {
					pathMatrix[i][navPointIndices.get(link.getToNavPoint().getId())].setDistance(
					    (float)location.distance(link.getToNavPoint().getLocation().getPoint3d())
					);
				}
			}
		}
			
		// Perform Floyd-Warshall.
		for (int k = 0; k < size; k++) {
			for (int i = 0; i < size; i++) {
				for (int j = 0; j < size; j++) {
					float newLen = pathMatrix[i][k].getDistance() + pathMatrix[k][j].getDistance();
					if (pathMatrix[i][j].getDistance() > newLen) {
						pathMatrix[i][j].setDistance(newLen);
						pathMatrix[i][j].setViaNode(k);
					}
				}
			}
		}
			
		// Construct paths + log unreachable paths.
		int count = 0;
		for (int i = 0; i < size; i++) {
			for (int j = 0; j < size; j++) {
				if (pathMatrix[i][j].getDistance() == Float.POSITIVE_INFINITY) {
					log.fine("Unreachable path from " + navPoints.get(i) + " -> " + navPoints.get(j));
					count++;
				} else {
					// path exists ... retrieve it
					pathMatrix[i][j].setPath(retrievePath(i, j));
				}
			}
		}
		
		log.info(this + ": There are " + count + " unreachable nav point pairs.");
		
		log.info(this + ": computation for " + size + " navpoints took " + (System.currentTimeMillis() - start) + " millis");
		
		// null unneeded field to free some memory
		indicesNavPoints = null;
	}
	
	/**
	 * Checks whether the edge is usable.
	 * @param from Starting nav point.
	 * @param edge NeighNav object representing the edge.
	 * @return boolean
	 */
	public boolean checkLink(NavPointNeighbourLink edge) {
		// Bad flags (prohibited edges, swimming, flying...).
		if ((edge.getFlags() & badEdgeFlag) != 0) return false;
		
		// Lift flags.
		if ((edge.getFlags() & LinkFlag.SPECIAL.get()) != 0) return true;
		
		// Check whether the climbing angle is not so steep.
		if (
			(
			 edge.getFromNavPoint().getLocation().getPoint3d().distance(edge.getToNavPoint().getLocation().getPoint3d()) 
			 < 
			 (edge.getToNavPoint().getLocation().z - edge.getFromNavPoint().getLocation().z)
			) 
			  &&
			(
			 edge.getFromNavPoint().getLocation().getPoint3d().distance(edge.getToNavPoint().getLocation().getPoint3d()) 
			 > 
			 100
			)
		   ) {
			return false;
		}
			
		// Check whether the jump is not so high.
		if (
			(
			 (edge.getFlags() & LinkFlag.JUMP.get()) != 0
			)
			  &&
			(
			 edge.getToNavPoint().getLocation().z - edge.getFromNavPoint().getLocation().z > 80
			)
		   ) {
			return false;
		}
		
		return true;
	}
	
	/**
	 * Sub-routine of retrievePath - do not use! ... Well you may, it returns path without
	 * 'from', 'to' or null if such path dosn't exist.
	 * <p><p>
	 * DO NOT USE OUTSIDE CONSTRUCTOR (relies on indicesNavPoints).
	 * @param from
	 * @param to
	 * @return
	 */
	private List<NavPoint> retrievePathInner(Integer from, Integer to) {
		PathMatrixNode node = pathMatrix[from][to];
		if (node.getDistance() == Float.POSITIVE_INFINITY) return null;
		if (node.getViaNode() == null) {
			return new ArrayList<NavPoint>(0);
		}
		if (node.getViaNode() == null) return new ArrayList<NavPoint>(0);
		
		List<NavPoint> path = new ArrayList<NavPoint>();
		path.addAll(retrievePathInner(from, node.getViaNode()));
		path.add(indicesNavPoints.get(node.getViaNode()));
		path.addAll(retrievePathInner(node.getViaNode(), to));
			
		return path;	
	}
	
	/**
	 * Returns path between from-to or null if path doesn't exist. Path begins with 'from' and
	 * ends with 'to'.
	 * <p><p> 
	 * DO NOT USE OUTSIDE CONSTRUCTOR (relies on indicesNavPoints).
	 * @param from
	 * @param to
	 * @return
	 */
	private List<NavPoint> retrievePath(Integer from, Integer to) {
		List<NavPoint> path = new ArrayList<NavPoint>();
		path.add(indicesNavPoints.get(from));
		path.addAll(retrievePathInner(from, to));
		path.add(indicesNavPoints.get(to));
		return path;
	}
	
	protected PathMatrixNode getPathMatrixNode(NavPoint np1, NavPoint np2) {
		return pathMatrix[navPointIndices.get(np1.getId())][navPointIndices.get(np2.getId())];
	}
	
	/**
	 * Whether navpoint 'to' is reachable from the navpoint 'from'.
	 * @param from
	 * @param to
	 * @return
	 */
	public boolean reachable(NavPoint from, NavPoint to) {
		if ((from == null) || (to == null)) return false;
		return getPathMatrixNode(from, to).getDistance() != Float.POSITIVE_INFINITY;
	}
	
	/**
	 * Calculate's distance between two nav points (using pathfinding).
	 * @return Distance or POSITIVE_INFINITY if there's no path.
	 */
	public float getDistance(NavPoint from, NavPoint to) {
		if ((from == null) || (to == null)) return Float.POSITIVE_INFINITY;		
		return getPathMatrixNode(from, to).getDistance();
	}
	
	/**
	 * Returns path between navpoints 'from' -> 'to'. The path begins with 'from' and ends 
	 * with 'to'. If such path doesn't exist, returns null.
	 * @param from
	 * @param to
	 * @return
	 */
	public List<NavPoint> getPath(NavPoint from, NavPoint to) {
		if ((from == null) || (to == null)) return null;
		return getPathMatrixNode(from, to).getPath();
	}
	
	public String toString() {
		return "FloydWarshallMap"; 
	}
	
}
