package cz.cuni.amis.pogamut.udk.agent.navigation.loquenavigator;
   
   import java.util.Iterator;
   import java.util.List;
   import java.util.logging.Level;
   import java.util.logging.Logger;
   
   import cz.cuni.amis.pogamut.base.utils.math.DistanceUtils;
   import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
  import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
  import cz.cuni.amis.pogamut.udk.agent.module.sensor.AgentInfo;
  import cz.cuni.amis.pogamut.udk.agent.navigation.AbstractUDKPathNavigator;
  import cz.cuni.amis.pogamut.udk.agent.navigation.IUDKPathRunner;
  import cz.cuni.amis.pogamut.udk.bot.command.AdvancedLocomotion;
  import cz.cuni.amis.pogamut.udk.bot.impl.UDKBot;
  import cz.cuni.amis.pogamut.udk.communication.messages.gbinfomessages.NavPoint;
  import cz.cuni.amis.pogamut.udk.communication.messages.gbinfomessages.NavPointNeighbourLink;
  import cz.cuni.amis.utils.NullCheck;
  
  /**
   * Responsible for navigation to location.
   *
   * <p>This class navigates the agent along path. Silently handles all casual
   * trouble with preparing next nodes, running along current nodes, switching
   * between nodes at appropriate distances, etc. In other words, give me a
   * destination and a path and you'll be there in no time.</p>
   *
   * <h4>Preparing ahead</h4>
   *
   * Nodes in the path are being prepared ahead, even before they are actually
   * needed. The agent decides ahead, looks at the next nodes while still running
   * to current ones, etc.
   *
   * <h4>Reachability checks</h4>
   *
   * Whenever the agent switches to the next node, reachcheck request is made to
   * the engine. The navigation routine then informs the {@link LoqueRunner}
   * beneath about possible troubles along the way.
   *
   * <h4>Movers</h4>
   *
   * This class was originally supposed to contain handy (and fully working)
   * navigation routines, including safe navigation along movers. However, the
   * pogamut platform is not quite ready for movers yet. Especial when it comes
   * to mover frames and correct mover links.
   *
   * <p>Thus, we rely completely on navigation points. Since the mover navigation
   * points (LiftCenter ones) always travel with the associated mover, we do not
   * try to look for movers at all. We simply compare navigation point location
   * to agent's location and wait or move accordingly.</p>
   *
   * <h4>Future</h4>
   *
   * The bot could check from time to time, whether the target destination he is
   * traveling to is not an empty pickup spot, since the memory is now capable of
   * reporting empty pickups, when they are visible. The only pitfall to this is
   * the way the agent might get <i>trapped</i> between two not-so-far-away items,
   * each of them empty. The more players playe the same map, the bigger is the
   * chance of pickup emptyness. The agent should implement a <i>fadeing memory
   * of which items are empty</i> before this can be put safely into logic.
   *
   * @author Juraj Simlovic [jsimlo@matfyz.cz]
   * @author Jimmy
   */
  public class LoqueNavigator<PATH_ELEMENT extends ILocated> extends AbstractUDKPathNavigator<PATH_ELEMENT>
  {
      /**
       * Current navigation destination.
       */
      private Location navigDestination = null;
  
      /**
       * Current stage of the navigation.
       */
      private Stage navigStage = Stage.COMPLETED;
  
      /*========================================================================*/
  
      /**
       * Distance, which is considered as close enough..
       */
      public static final int CLOSE_ENOUGH = 60;
      public static final int PRECISION = 128;
  
          @Override
          public double getPrecision() {
              return PRECISION; //it was shown empirically, that CLOSE_ENOUGH itself might not be achieved
          }
  
      
      
      /*========================================================================*/
      
  	@Override
  	protected void navigate(int pathElementIndex) {
  		switch (navigStage = keepNavigating()) {
  		case TELEPORT:
  		case AWAITING_MOVER:
  		case RIDING_MOVER:
 		case NAVIGATING:
 		case REACHING:		
 			return;
 		
 		case TIMEOUT:
 		case CRASHED:
 		case CANCELED:
 			executor.stuck();
 			return;
 		
 		case COMPLETED:
 			executor.targetReached();
 			return;
 		}
 	}
 	
 	@Override
 	public void reset() {
 		// reinitialize the navigator's values
 		
 		navigCurrentLocation = null;
 		navigCurrentNode = null;
         navigCurrentLink = null;
 		navigDestination = null;
 		navigIterator = null;
 		navigLastLocation = null;
 		navigLastNode = null;
 		navigNextLocation = null;
 		navigNextNode = null;
 		navigNextLocationOffset = 0;
 		navigStage = Stage.COMPLETED;
 	}
 
 	@Override
 	public void newPath(List<PATH_ELEMENT> path) {
 		// 1) obtain the destination
 		Location dest = path.get(path.size()-1).getLocation();
 				
 		// 2) init the navigation
 		initPathNavigation(dest, path);		
 	}
 	
 	/*========================================================================*/
 	
 	/**
      * Initializes direct navigation to the specified destination.
      * @param dest Destination of the navigation.
      * @param timeout Maximum timeout of the navigation. Use 0 to auto-timeout.
      */
     protected void initDirectNavigation (Location dest)
     {
         // calculate destination distance
         int distance = (int) memory.getLocation().getDistance(dest);
         // init the navigation
         if (log != null && log.isLoggable(Level.FINE)) log.fine (
             "LoqueNavigator.initDirectNavigation(): initializing direct navigation"
             + ", distance " + distance
         );
         // init direct navigation
         initDirectly (dest);
     }
 	
 	/*========================================================================*/
 
     /**
      * Initializes navigation to the specified destination along specified path.
      * @param dest Destination of the navigation.
      * @param path Navigation path to the destination.
      */
     protected void initPathNavigation(Location dest, List<PATH_ELEMENT> path)
     {
         // init the navigation
         if (log != null && log.isLoggable(Level.FINE)) 
         	log.fine (
         			"LoqueNavigator.initPathNavigation(): initializing path navigation"
         			+ ", nodes " + path.size ()
         	);
         // init path navigation
         if (!initAlongPath(dest, path))
         {
             // do it directly then..
             initDirectNavigation(dest);
         }
     }
 
     /*========================================================================*/
 
     /**
      * Navigates with the current navigation request.
      * @return Stage of the navigation progress.
      */
     protected Stage keepNavigating ()
     {
         // is there any point in navigating further?
         if (navigStage.terminated)
             return navigStage;
       
         // try to navigate
         switch (navigStage)
         {
             case REACHING:
                 navigStage = navigDirectly();
                 break;
             default:
                 navigStage = navigAlongPath();
                 break;
         }
 
         // return the stage
         if (log != null && log.isLoggable(Level.FINEST)) log.finest ("Navigator.keepNavigating(): navigation stage " + navigStage);
         return navigStage;
     }
 
     /*========================================================================*/
 
     /**
      * Initializes direct navigation to given destination.
      * 
      * @param dest Destination of the navigation.
      * @return Next stage of the navigation progress.
      */
     private Stage initDirectly(Location dest)
     {
         // setup navigation info
         navigDestination = dest;
         // init runner
         runner.reset();
         // reset navigation stage
         return navigStage = Stage.REACHING;
     }
 
     /**
      * Tries to navigate the agent directly to the navig destination.
      * @return Next stage of the navigation progress.
      */
     private Stage navigDirectly ()
     {
         // get the distance from the target
         int distance = (int) memory.getLocation().getDistance(navigDestination);
 
         // are we there yet?
         if (distance <= getPrecision())
         {
             if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigDirectly(): destination close enough: " + distance);
             return Stage.COMPLETED;
         }
 
         // run to that location..
         if (!runner.runToLocation (navigDestination, null, navigDestination, null, true))
         {
             if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigDirectly(): direct navigation failed");
             return Stage.CRASHED;
         }
 
         // well, we're still running
         if (log != null && log.isLoggable(Level.FINEST)) log.finest ("LoqueNavigator.navigDirectly(): traveling directly, distance = " + distance);
         return navigStage;
     }
 
     /*========================================================================*/
 
     /**
      * Iterator through navigation path.
      */
     private Iterator<PATH_ELEMENT> navigIterator = null;
     
     /**
      * How many path elements we have iterated over before selecting the current {@link LoqueNavigator#navigNextLocation}.
      */
     private int navigNextLocationOffset = 0;
 
     /**
      * Last location in the path (the one the agent already reached).
      */
     private Location navigLastLocation = null;
     
     /**
      * If {@link LoqueNavigator#navigLastLocation} is a {@link NavPoint} or has NavPoint near by, its instance
      * is written here (null otherwise).
      */
     private NavPoint navigLastNode = null;
 
     /**
      * Current node in the path (the one the agent is running to).
      */
     private Location navigCurrentLocation = null;
     
     /**
      * If {@link LoqueNavigator#navigCurrentLocation} is a {@link NavPoint} or has NavPoint near by,
      * its instance is written here (null otherwise).
      */
     private NavPoint navigCurrentNode = null;
 
     /**
      * If moving between two NavPoints {@link NavPoint} the object {@link NeighbourLink} holding infomation
      * about the link (if any) will be stored here (null otherwise).
      */
     private NavPointNeighbourLink navigCurrentLink = null;
 
     /**
      * Next node in the path (the one being prepared).
      */
     private Location navigNextLocation = null;
     
     /**
      * If {@link LoqueNavigator#navigNextLocation} is a {@link NavPoint} or has NavPoint near by,
      * its instance is written here (null otherwise).
      */
     private NavPoint navigNextNode = null;
     
     /**
      * Returns {@link NavPoint} instance for a given location. If there is no navpoint in the vicinity of {@link LoqueNavigator#CLOSE_ENOUGH}
      * null is returned.
      * 
      * @param location
      * @return
      */
     protected NavPoint getNavPoint(ILocated location) {
     	if (location instanceof NavPoint) return (NavPoint) location;
     	NavPoint np = DistanceUtils.getNearest(main.getWorldView().getAll(NavPoint.class).values(), location);
     	if (np.getLocation().getDistance(location.getLocation()) < CLOSE_ENOUGH) return np;
     	return null;
     }
 
     /**
      * Initializes navigation along path.
      * @param dest Destination of the navigation.
      * @param path Path of the navigation.
      * @return True, if the navigation is successfuly initialized.
      */
     private boolean initAlongPath(Location dest, List<PATH_ELEMENT> path)
     {
         // setup navigation info
         navigDestination = dest;
         navigIterator = path.iterator();
         // reset current node
         navigCurrentLocation = null;
         navigCurrentNode = null;
         // prepare next node
         prepareNextNode ();
         // reset navigation stage
         navigStage = Stage.NAVIGATING;
         // reset node navigation info
         return switchToNextNode ();
     }
 
     /**
      * Tries to navigate the agent safely along the navigation path.
      * @return Next stage of the navigation progress.
      */
     private Stage navigAlongPath()
     {
         // get the distance from the destination
         int totalDistance = (int) memory.getLocation().getDistance(navigDestination);
 
         // are we there yet?
         if (totalDistance <= getPrecision())
         {
             log.finest ("Navigator.navigAlongPath(): destination close enough: " + totalDistance);
             return Stage.COMPLETED;
         }
         
         // navigate
         if (navigStage.mover) {
         	return navigThroughMover();
         } else
         if (navigStage.teleport) {
         	return navigThroughTeleport();
         } else {
             return navigToCurrentNode();
         }
     }
 
     /*========================================================================*/
 
     /**
      * Prepares next navigation node in path.
      * <p><p>
      * If necessary just recalls {@link LoqueNavigator#prepareNextNodeTeleporter()}.
      */
     private void prepareNextNode ()
     {
     	if (navigCurrentNode != null && navigCurrentNode.isTeleporter()) {
     		// current node is a teleporter! ...
     		prepareNextNodeTeleporter();
     		return;
     	}
     	
         // retreive the next node, if there are any left
         // note: there might be null nodes along the path!
     	ILocated located = null;
         navigNextLocation = null;
         navigNextLocationOffset = 0;
         while ((located == null) && navigIterator.hasNext ())
         {
             // get next node in the path
         	located = navigIterator.next();
         	navigNextLocationOffset += 1;
         	if (located == null) {
         		continue;            
         	}
         }
 
         // did we get the next node?
         if (located == null) {
         	navigNextLocationOffset = 0;
         	return;
         }
         
         if (executor.getPathElementIndex() + navigNextLocationOffset >= executor.getPath().size()) {
         	navigNextLocationOffset = 0; // WTF?
         }
        
         // obtain next location
         navigNextLocation = located.getLocation();
         // obtain navpoint instance for a given location
         navigNextNode = getNavPoint(located);
     }
     
     /**
      * Prepares next node in the path assuming the currently pursued node is a teleporter.
      */
     private void prepareNextNodeTeleporter() {
     	// Retrieve the next node, if there are any left
         // note: there might be null nodes along the path!
     	ILocated located = null;
         navigNextLocation = null;
         navigNextLocationOffset = 0;
         boolean nextTeleporterFound = false;
         while ((located == null) && navigIterator.hasNext ())
         {
             // get next node in the path
         	located = navigIterator.next();
         	navigNextLocationOffset += 1;
         	if (located == null) {
         		continue;            
         	}
         	navigNextNode = getNavPoint(located);
         	if (navigNextNode != null && navigNextNode.isTeleporter()) {
         		// next node is 
         		if (!nextTeleporterFound) {
         			// ignore first teleporter as it is the other end of the teleporter we're currently trying to enter
         			located = null;
         		}
         		nextTeleporterFound = true;
         	} else {
         		break;
         	}
         }
         
         // did we get the next node?
         if (located == null) {
         	navigNextLocationOffset = 0;
         	return;
         }
         
         if (executor.getPathElementIndex() + navigNextLocationOffset >= executor.getPath().size()) {
         	navigNextLocationOffset = 0; // WTF?
         }
        
         // obtain next location
         navigNextLocation = located.getLocation();
         // obtain navpoint instance for a given location
         navigNextNode = getNavPoint(located);        
     }
 
     /**
      * Initializes next navigation node in path.
      * @return True, if the navigation node is successfully switched.
      */
     private boolean switchToNextNode ()
     {
         // move the current node into last node
         navigLastLocation = navigCurrentLocation;
         navigLastNode = navigCurrentNode;
 
         // get the next prepared node
         if (null == (navigCurrentLocation = navigNextLocation))
         {
             // no nodes left there..
             if (log != null && log.isLoggable(Level.FINER)) log.finer ("Navigator.switchToNextNode(): no nodes left");
             navigCurrentNode = null;
             return false;
         }
         // rewrite the navpoint as well
         navigCurrentNode = navigNextNode;
 
         // store current NavPoint link
         navigCurrentLink = getNavPointsLink(navigLastNode, navigCurrentNode);
         
         // ensure that the last node is not null
         if (navigLastLocation == null) {
             navigLastLocation = navigCurrentLocation;
             navigLastNode = navigCurrentNode;
         }
 
         // get next node distance
         int localDistance = (int) memory.getLocation().getDistance(navigCurrentLocation.getLocation());
 
         if (navigCurrentNode == null) {
         	// we do not have extra information about the location we're going to reach
         	runner.reset();
         	if (log != null && log.isLoggable(Level.FINE)) 
         		log.fine (
                     "LoqueNavigator.switchToNextNode(): switch to next location " + navigCurrentLocation
                     + ", distance " + localDistance
                     + ", mover " + navigStage.mover
                 );
         } else {
         	// is this next node a teleporter?
         	if (navigCurrentNode.isTeleporter()) {
         		navigStage = Stage.TeleporterStage();
         	} else
 	        // is this next node a mover?
 	        if (navigCurrentNode.isLiftCenter())
 	        {
 	            // setup mover sequence
 	            navigStage = Stage.FirstMoverStage();
 	        } else
 	        // are we still moving on mover?
 	        if (navigStage.mover)
 	        {
 	        	navigStage = navigStage.next();
 	            // init the runner
 	            runner.reset();
 	        } else
 	        // no movers
 	        {
 	            // init the runner
 	            runner.reset();
 	        }
 	
 	        // switch to next node
 	        if (log != null && log.isLoggable(Level.FINE)) 
 	        	log.fine (
 		            "LoqueNavigator.switchToNextNode(): switch to next node " + navigCurrentNode.getId().getStringId()
 		            + ", distance " + localDistance
 		            + ", reachable " + isReachable(navigCurrentNode)
 		            + ", mover " + navigStage.mover
 		        );
         }
 
         // tell the executor that we have moved in the path to the next element
         if (executor.getPathElementIndex() < 0) {
         	executor.switchToAnotherPathElement(0);
         } else {
         	if (navigNextLocationOffset > 0) {
         		executor.switchToAnotherPathElement(executor.getPathElementIndex()+navigNextLocationOffset);
         	} else {
         		executor.switchToAnotherPathElement(executor.getPathElementIndex());
         	}        	
         }
         navigNextLocationOffset = 0;
         
         return true;
     }
 
     private boolean isReachable(NavPoint node) {
     	if (node == null) return true;
 		int hDistance = (int) memory.getLocation().getDistance2D(node.getLocation());
 		int vDistance = (int) node.getLocation().getDistanceZ(memory.getLocation());
 		double angle; 
 		if (hDistance == 0) {
 			angle = vDistance == 0 ? 0 : (vDistance > 0 ? Math.PI/2 : -Math.PI/2);
 		} else {
 			angle = Math.atan(vDistance / hDistance);
 		}
 		return Math.abs(vDistance) < 30 && Math.abs(angle) < Math.PI / 4;
 	}
 
 
 
 	/*========================================================================*/
 
     /**
      * Gets the link with movement information between two navigation points. Holds
      * information about how we can traverse from the start to the end navigation
      * point.
      * 
      * @return NavPointNeighbourLink or null
      */
     private NavPointNeighbourLink getNavPointsLink(NavPoint start, NavPoint end) {
         if (start == null) {
             //if start NavPoint is not provided, we try to find some
             NavPoint tmp = getNavPoint(memory.getLocation());
             if (tmp != null)
                 start = tmp;
             else
                 return null;
         }
         if (end == null)
             return null;
 
         if (end.getIncomingEdges().containsKey(start.getId()))
             return end.getIncomingEdges().get(start.getId());
         
         return null;
     }
 
     /*========================================================================*/
 
     /**
      * Tries to navigate the agent safely to the current navigation node.
      * @return Next stage of the navigation progress.
      */
     private Stage navigToCurrentNode ()
     {
     	if (navigCurrentNode != null) {
     		// update location of the current place we're reaching
     		navigCurrentLocation = navigCurrentNode.getLocation();
     	}
     	
         // get the distance from the current node
         int localDistance = (int) memory.getLocation().getDistance(navigCurrentLocation.getLocation());
         // get the distance from the current node (neglecting jumps)
         int localDistance2 = (int) memory.getLocation().getDistance(
             Location.add(navigCurrentLocation.getLocation(), new Location (0,0,100))
         );
 
         // where are we going to run to
         Location firstLocation = navigCurrentLocation.getLocation();
         // where we're going to continue
         Location secondLocation = (navigNextNode != null && !navigNextNode.isLiftCenter() && !navigNextNode.isLiftCenter() ? 
         		                  	navigNextNode.getLocation() :
         		                  	navigNextLocation);
         // and what are we going to look at
         Location focus = (navigNextLocation == null) ? firstLocation : navigNextLocation.getLocation();
 
         // run to the current node..
         if (!runner.runToLocation (firstLocation, secondLocation, focus, navigCurrentLink, (navigCurrentNode == null ? true : isReachable(navigCurrentNode)))) {
             if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigToCurrentNode(): navigation to current node failed");
             return Stage.CRASHED;
         }
 
         // we're still running
         if (log != null && log.isLoggable(Level.FINEST)) log.finest ("LoqueNavigator.navigToCurrentNode(): traveling to current node, distance = " + localDistance);
 
         // are we close enough to think about the next node?
 //        if ( (localDistance < 600) || (localDistance2 < 600) )
 //        {
             // prepare the next node only when it is not already prepared..
             if ((navigCurrentNode != null && navigCurrentNode == navigNextNode) || navigCurrentLocation.equals(navigNextLocation))
             {
                 // prepare next node in the path
                 prepareNextNode();
             }
 //        }
 
         int testDistance = 200; // default constant suitable for default running 
         if (navigCurrentNode != null && (navigCurrentNode.isLiftCenter() || navigCurrentNode.isLiftExit())) {
         	// if we should get to lift exit or the lift center, we must use more accurate constants
         	testDistance = 150;
         }
             
         // are we close enough to switch to the next node?
         if ( (localDistance < testDistance) || (localDistance2 < testDistance) )
         {
             // switch navigation to the next node
             if (!switchToNextNode ())
             {
                 // switch to the direct navigation
                 if (log != null && log.isLoggable(Level.FINE)) log.fine ("Navigator.navigToCurrentNode(): switch to direct navigation");
                 return initDirectly(navigDestination);
             }
         }
 
         // well, we're still running
         return navigStage;
     }
 
     /*========================================================================*/
 
     /**
      * Tries to navigate the agent safely along mover navigation nodes.
      *
      * <h4>Pogamut troubles</h4>
      *
      * Since the engine does not send enough reasonable info about movers and
      * their frames, the agent relies completely and only on the associated
      * navigation points. Fortunatelly, LiftCenter navigation points move with
      * movers.
      *
      * <p>Well, do not get too excited. Pogamut seems to update the position of
      * LiftCenter navpoint from time to time, but it's not frequent enough for
      * correct and precise reactions while leaving lifts.</p>
      *
      * @return Next stage of the navigation progress.
      */
     private Stage navigThroughMover ()
     {
     	Stage stage = navigStage;
     	        
         if (navigCurrentNode == null) {
         	if (log != null && log.isLoggable(Level.WARNING)) log.warning("LoqueNavigator.navigThroughMover("+stage+"): can't navigate through the mover without the navpoint instance (navigCurrentNode == null)");
         	return Stage.CRASHED;
         }
         
         // update navigCurrentLocation as the mover might have moved
         navigCurrentLocation = navigCurrentNode.getLocation();
         
         // get horizontal distance from the mover center node
         int hDistance = (int) memory.getLocation().getDistance2D(navigCurrentLocation.getLocation());
         // get vertical distance from the mover center node
         int vDistance = (int) navigCurrentLocation.getLocation().getDistanceZ(memory.getLocation());
     	
     	if (navigStage == Stage.AWAITING_MOVER) {
 	        // wait for the current node to come close in both, vert and horiz
 	        // the horizontal distance can be quite long.. the agent will hop on
 	        // TODO: There may be problem when LiftExit is more than 400 ut units far from LiftCenter!
 	        if ( ( (vDistance > 50) || (hDistance > 400) ) ) 
 	        {
 	            // run to the last node, the one we're waiting on
 	            if (!runner.runToLocation(navigLastLocation.getLocation(), null, navigCurrentLocation.getLocation(), navigCurrentLink, (navigLastNode == null ? true : isReachable(navigLastNode))))
 	            {
 	                if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigThroughMover("+stage+"): navigation to last node failed");
 	                return Stage.CRASHED;
 	            }
 	            // and keep waiting for the mover
 	            if (log != null && log.isLoggable(Level.FINER)) 
 	            	log.finer (
 		                "LoqueNavigator.navigThroughMover("+stage+"): waiting for mover"
 		                + ", node " + navigCurrentNode.getId().getStringId()
 		                + ", vDistance " + vDistance + ", hDistance " + hDistance
 		            );
 	
 	            return navigStage;
 	        }
 	        
 	        // MOVER HAS ARRIVED (at least that what we're thinking so...)
 	        if (log != null && log.isLoggable(Level.FINER)) 
 		        log.finer (
 		            "Navigator.navigThroughMover("+stage+"): mover arrived"
 		            + ", node " + navigCurrentNode.getId().getStringId()
 		            + ", vDistance " + vDistance + ", hDistance " + hDistance
 		        );
 	        
 	        // LET'S MOVE TO THE LIFT CENTER
 	        return navigToCurrentNode();
     	} else
     	if (navigStage == Stage.RIDING_MOVER){
     		// wait for the mover to ride us up/down
     		if ( ( (Math.abs(vDistance) > 50) || (hDistance > 400) ) ) {
  	            // run to the last node, the one we're waiting on
  	            if (!runner.runToLocation(navigLastLocation.getLocation(), null, navigCurrentLocation.getLocation(), navigCurrentLink, (navigLastNode == null ? true : isReachable(navigLastNode))))
  	            {
  	                if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigThroughMover("+stage+"): navigation to last node failed");
  	                return Stage.CRASHED;
  	            }
  	            // and keep waiting for the mover to go to the correct position
  	            if (log != null && log.isLoggable(Level.FINER)) 
  	            	log.finer (
  		                "LoqueNavigator.navigThroughMover("+stage+"): riding the mover"
  		                + ", node " + navigCurrentNode.getId().getStringId()
  		                + ", vDistance " + vDistance + ", hDistance " + hDistance
  		            ); 	
  	            return navigStage;
  	        }
     		// MOVER HAS ARRIVED (at least that what we're thinking so...)
 	        if (log != null && log.isLoggable(Level.FINER)) 
 		        log.finer (
 		            "Navigator.navigThroughMover("+stage+"): exiting the mover"
 		            + ", node " + navigCurrentNode.getId().getStringId()
 		            + ", vDistance " + vDistance + ", hDistance " + hDistance
 		        );
 	        
 	        // LET'S MOVE TO THE LIFT CENTER
 	        return navigToCurrentNode();    		
     	} else {
     		if (log != null && log.isLoggable(Level.WARNING)) {
     			log.warning("Navigator.navigThroughMover("+stage+"): invalid stage, neither AWAITING_MOVER nor RIDING MOVER");
     		}
     		return Stage.CRASHED;
     	}
 
     }
     
     /*========================================================================*/
     
     /**
      * Tries to navigate the agent safely to the current navigation node.
      * @return Next stage of the navigation progress.
      */
     private Stage navigThroughTeleport()
     {
     	if (navigCurrentNode != null) {
     		// update location of the current place we're reaching
     		navigCurrentLocation = navigCurrentNode.getLocation();
     	}
     	
     	if ((navigCurrentNode != null && navigCurrentNode == navigNextNode) || navigCurrentLocation.equals(navigNextLocation))
         {
             // prepare next node in the path as soon as possible
             prepareNextNode();
         }
     	
     	// now we have to compute whether we should switch to another navpoint
         // it has two flavours, we should switch if:
         //			1. we're too near to teleport, we should run into
         //          2. we're at the other end of the teleport, i.e., we've already got through the teleport
         
         // 1. DISTANCE TO THE TELEPORT
         // get the distance from the current node
         int localDistance1_1 = (int) memory.getLocation().getDistance(navigCurrentLocation.getLocation());
         // get the distance from the current node (neglecting jumps)
         int localDistance1_2 = (int) memory.getLocation().getDistance(
             Location.add(navigCurrentLocation.getLocation(), new Location (0,0,100))
         );        
         
         // 2. DISTANCE TO THE OTHER END OF THE TELEPORT
         // ---[[ WARNING ]]--- we're assuming that there is only ONE end of the teleport
         int localDistance2_1 = Integer.MAX_VALUE;
         int localDistance2_2 = Integer.MAX_VALUE;
         for (NavPointNeighbourLink link : navigCurrentNode.getOutgoingEdges().values()) {
         	if (link.getToNavPoint().isTeleporter()) {
         		localDistance2_1 = (int)memory.getLocation().getDistance(link.getToNavPoint().getLocation());
         		localDistance2_2 = (int) memory.getLocation().getDistance(
         	            Location.add(link.getToNavPoint().getLocation(), new Location (0,0,100))
                 );        
         		break;
         	}
         }
                 
         boolean switchedToNextNode = false;
         // are we close enough to switch to the OTHER END of the teleporter?
         if ( (localDistance2_1 < 200) || (localDistance2_2 < 200))
         {
         	// yes we are! we already passed the teleporter, so DO NOT APPEAR DUMB and DO NOT TRY TO RUN BACK 
             // ... better to switch navigation to the next node
             if (!switchToNextNode ())
             {
                 // switch to the direct navigation
                 if (log != null && log.isLoggable(Level.FINE)) log.fine ("Navigator.navigToCurrentNode(): switch to direct navigation");
                 return initDirectly(navigDestination);
             }
             switchedToNextNode = true;
         }
     	
         // where are we going to run to
         Location firstLocation = navigCurrentLocation.getLocation();
         // where we're going to continue
         Location secondLocation = (navigNextNode != null && !navigNextNode.isLiftCenter() && !navigNextNode.isLiftCenter() ? 
         		                  	navigNextNode.getLocation() :
         		                  	navigNextLocation);
         // and what are we going to look at
         Location focus = (navigNextLocation == null) ? firstLocation : navigNextLocation.getLocation();
 
         // run to the current node..
         if (!runner.runToLocation (firstLocation, secondLocation, focus, navigCurrentLink, (navigCurrentNode == null ? true : isReachable(navigCurrentNode)))) {
             if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigToCurrentNode(): navigation to current node failed");
             return Stage.CRASHED;
         }
 
         // we're still running
         if (log != null && log.isLoggable(Level.FINEST)) log.finest ("LoqueNavigator.navigToCurrentNode(): traveling to current node");        
         
         // now as we've tested the first node ... test the second one
         if ( !switchedToNextNode && ((localDistance1_1 < 200) || (localDistance1_2 < 200)) )
         {
             // switch navigation to the next node
             if (!switchToNextNode ())
             {
                 // switch to the direct navigation
                 if (log != null && log.isLoggable(Level.FINE)) log.fine ("Navigator.navigToCurrentNode(): switch to direct navigation");
                 return initDirectly(navigDestination);
             }
         }
 
         // well, we're still running
         return navigStage;
     }
 
     /*========================================================================*/
 
     /**
      * Enum of types of terminating navigation stages.
      */
     private enum TerminatingStageType {
         /** Terminating with success. */
         SUCCESS (false),
         /** Terminating with failure. */
         FAILURE (true);
 
         /** Whether the terminating with failure. */
         public boolean failure;
 
         /**
          * Constructor.
          * @param failure Whether the terminating with failure.
          */
         private TerminatingStageType (boolean failure)
         {
             this.failure = failure;
         }
     };
 
     /**
      * Enum of types of mover navigation stages.
      */
     private enum MoverStageType {
         /** Waiting for mover. */
         WAITING,
         /** Riding mover. */
         RIDING;
     };
     
     /**
      * Enum of types of mover navigation stages.
      */
     private enum TeleportStageType {
         /** Next navpoint is a teleport */
         GOING_THROUGH;
     };
 
     /**
      * All stages the navigation can come to.
      */
     public enum Stage
     {
         /**
          * Running directly to the destination.
          */
         REACHING ()
         {
             protected Stage next () { return this; }
         },
         /**
          * Navigating along the path.
          */
         NAVIGATING ()
         {
             protected Stage next () { return this; }
         },
         /**
          * Waiting for a mover to arrive.
          */
         AWAITING_MOVER (MoverStageType.WAITING)
         {
             protected Stage next () { return RIDING_MOVER; }
         },
         /**
          * Waiting for a mover to ferry.
          */
         RIDING_MOVER (MoverStageType.RIDING)
         {
             protected Stage next () { return NAVIGATING; }
         },
         /**
          * Navigation cancelled by outer force.
          */
         CANCELED (TerminatingStageType.FAILURE)
         {
             protected Stage next () { return this; }
         },
         /**
          * Navigation timeout reached.
          */
         TIMEOUT (TerminatingStageType.FAILURE)
         {
             protected Stage next () { return this; }
         },
         /**
          * Navigation failed because of troublesome obstacles.
          */
         CRASHED (TerminatingStageType.FAILURE)
         {
             protected Stage next () { return this; }
         },
         /**
          * Navigation finished successfully.
          */
         COMPLETED (TerminatingStageType.SUCCESS)
         {
             protected Stage next () { return this; }
         },
         /**
          * We're going through the teleport.
          */
         TELEPORT (TeleportStageType.GOING_THROUGH) {
         	protected Stage next() { return NAVIGATING; };
         };
         
 
         /*====================================================================*/
 
         /**
          * Running through the mover.
          */
         private boolean mover;
         /**
          * Whether the nagivation is terminated.
          */
         public boolean terminated;
         /**
          * Whether the navigation has failed.
          */
         public boolean failure;
         /**
          * We're going through the teleport.
          */
         public boolean teleport;
 
         /*====================================================================*/
 
         /**
          * Constructor: Not finished, not failed
          */
         private Stage ()
         {
             this.mover = false;
             this.teleport = false;
             this.terminated = false;
             this.failure = false;
         }
 
         private Stage(TeleportStageType type) {
         	this.mover = false;
         	this.teleport = true;
         	this.failure = false;
         	this.terminated = false;
         }
         
         /**
          * Constructor: mover.
          * @param type Type of mover navigation stage.
          */
         private Stage (MoverStageType type)
         {
             this.mover = true;
             this.teleport = false;
             this.terminated = false;
             this.failure = false;
         }
 
         /**
          * Constructor: terminating.
          * @param type Type of terminating navigation stage.
          */
         private Stage (TerminatingStageType type)
         {
             this.mover = false;
             this.teleport = false;
             this.terminated = true;
             this.failure = type.failure;
         }
 
         /*====================================================================*/
 
         /**
          * Retreives the next step of navigation sequence the stage belongs to.
          * @return The next step of navigation sequence. Note: Some stages are
          * not part of any logical navigation sequence. In such cases, this
          * method simply returns the same stage.
          */
         protected abstract Stage next ();
 
         /*====================================================================*/
 
         /**
          * Returns the first step of mover sequence.
          * @return The first step of mover sequence.
          */
         protected static Stage FirstMoverStage ()
         {
             return AWAITING_MOVER;
         }
         
         /**
          * Returns the first step of the teleporter sequence.
          * @return
          */
         protected static Stage TeleporterStage() {
         	return Stage.TELEPORT;
         }
     }
 
     /*========================================================================*/
 
     /**
      * Default: Loque Runner.
      */
     private IUDKPathRunner runner;
 
     /*========================================================================*/
 
     /** Agent's main. */
     protected UDKBot main;
     /** Loque memory. */
     protected AgentInfo memory;
     /** Agent's body. */
     protected AdvancedLocomotion body;
     /** Agent's log. */
     protected Logger log;
 
     /*========================================================================*/
 
     /**
      * Constructor.
      * @param main Agent's main.
      * @param memory Loque memory.
      */
     public LoqueNavigator (UDKBot bot, Logger log)
     {
         // setup reference to agent
         this.main = bot;
         this.memory = new AgentInfo(bot);
         this.body = new AdvancedLocomotion(bot, log);
         this.log = log;
 
         // create runner object
         this.runner = new LoqueRunner(bot, memory, body, log);
     }
 
     /**
      * Constructor.
      * @param main Agent's main.
      * @param memory Loque memory.
      */
     public LoqueNavigator (UDKBot bot, IUDKPathRunner runner, Logger log)
     {
         // setup reference to agent
         this.main = bot;
         this.memory = new AgentInfo(bot);
         this.body = new AdvancedLocomotion(bot, log);
         this.log = log;
 
         // save runner object
         this.runner = runner;
         NullCheck.check(this.runner, "runner");
     }    
     
 }