package cz.cuni.pogamut.cup2013.bot;

import java.util.Iterator;
import java.util.List;
import java.util.logging.Level;
import java.util.logging.Logger;

import cz.cuni.amis.pogamut.base.communication.worldview.IWorldView;
import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectEventListener;
import cz.cuni.amis.pogamut.base.communication.worldview.object.event.WorldObjectUpdatedEvent;
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.ut2004.agent.module.sensor.AgentInfo;
import cz.cuni.amis.pogamut.ut2004.agent.navigation.AbstractUT2004PathNavigator;
import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004PathRunner;
import cz.cuni.amis.pogamut.ut2004.bot.command.AdvancedLocomotion;
import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Mover;
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.messages.gbinfomessages.Self;
import cz.cuni.amis.pogamut.ut2004.utils.LinkFlag;
import cz.cuni.amis.pogamut.ut2004.utils.UnrealUtils;
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 KefikRunner}
 * 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 MyPathNavigator<PATH_ELEMENT extends ILocated> extends AbstractUT2004PathNavigator<PATH_ELEMENT>
{
    /**
     * Current navigation destination.
     */
    private Location navigDestination = null;

    /**
     * Current stage of the navigation.
     */
    private Stage navigStage = Stage.COMPLETED;

    /**
     * Current focus of the bot, if null, provide default focus.
     * <p><p>
     * Filled at the beginning of the {@link MyPathNavigator#navigate(ILocated, int)}.
     */
	private ILocated focus = null;
	
	/**
	 * {@link Self} listener.
	 */
	private class SelfListener implements IWorldObjectEventListener<Self, WorldObjectUpdatedEvent<Self>>
	{
		private IWorldView worldView;

		/**
		 * Constructor. Registers itself on the given WorldView object.
		 * @param worldView WorldView object to listent to.
		 */
		public SelfListener(IWorldView worldView)
		{
			this.worldView = worldView;
			worldView.addObjectListener(Self.class, WorldObjectUpdatedEvent.class, this);
		}

		@Override
		public void notify(WorldObjectUpdatedEvent<Self> event) {
			self = event.getObject();			
		}
	}
	
	/** {@link Self} listener */
	private SelfListener selfListener;

    /*========================================================================*/

    /**
     * Distance, which is considered as close enough for considering the bot to be AT LOCATION/NAV POINT.
     * 
     * If greater than 50, navigation will failed on DM-Flux2 when navigating between health vials in one of map corers.
     */
    public static final int CLOSE_ENOUGH = 50;

    /*========================================================================*/
    
	@Override
	protected void navigate(ILocated focus, int pathElementIndex) {
		if (log != null && log.isLoggable(Level.FINE)) log.fine("Current stage: " + navigStage);
		this.focus = focus;
		switch (navigStage = keepNavigating()) {
		case AWAITING_MOVER:
		case RIDING_MOVER:
			setBotWaiting(true);
			break;
		case TELEPORT:
		case NAVIGATING:
		case REACHING:
			setBotWaiting(false);
			break;
		
		case TIMEOUT:
		case CRASHED:
		case CANCELED:
			if (log != null && log.isLoggable(Level.WARNING)) log.warning("Navigation " + navigStage);
			executor.stuck();
			return;
		
		case COMPLETED:
			executor.targetReached();
			break;
		}
		if (log != null && log.isLoggable(Level.FINE)) log.fine("Next stage: " + navigStage);
	}
	
	@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;
		setBotWaiting(false);
		
		resetNavigMoverVariables();
	}

	@Override
	public void newPath(List<PATH_ELEMENT> path) {
		// prepare for running along new path
		reset();
		
		// 1) obtain the destination
		Location dest = path.get(path.size()-1).getLocation();
				
		// 2) init the navigation
		initPathNavigation(dest, path);		
	}
	
	@Override
	public void pathExtended(List<PATH_ELEMENT> path, int currentPathIndex) {
		if (path == null || path.size() == 0) {
			throw new RuntimeException("path is null or 0-sized!");
		}
		navigDestination = path.get(path.size() - 1).getLocation();
		navigIterator = path.iterator();
		
		int newOffset = -currentPathIndex;
		for (int i = 0; i < path.size() && i < currentPathIndex+navigNextLocationOffset && navigIterator.hasNext(); ++i) {
			++newOffset;
			navigIterator.next();
		}
		log.fine("PATH EXTEND ... curr index " + currentPathIndex + ", old offset " + navigNextLocationOffset + ", new offset " + newOffset + ", path size " + path.size());
		navigNextLocationOffset = newOffset;
	}
	
	public NavPointNeighbourLink getCurrentLink() {
		return navigCurrentLink;
	}
	
	/*========================================================================*/
	
	/**
     * 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;
      
        if (log != null && log.isLoggable(Level.FINE)) {
        	if (navigLastNode != null) {
        		log.fine("LoqueNavigator.keepNavigating(): navigating from " + navigLastNode.getId().getStringId() + navigLastNode.getLocation() );    	
        	} else
        	if (navigLastLocation != null) {
        		log.fine("LoqueNavigator.keepNavigating(): navigating from " + navigLastLocation + " (navpoint is unknown)" );
        	}
        	if (navigCurrentNode != null) {
        		log.fine("LoqueNavigator.keepNavigating(): navigating to   " + navigCurrentNode.getId().getStringId() + navigCurrentNode.getLocation() );
        	} else
        	if (navigCurrentLocation != null) {
        		log.fine("LoqueNavigator.keepNavigating(): navigating to       " + navigCurrentLocation + " (navpoint is unknown)" );
        	}
        	if (navigLastLocation != null && navigCurrentLocation != null) {
        		log.fine("LoqueNavigator.keepNavigating(): distance in-between " + navigCurrentLocation.getDistance(navigLastLocation));
        	}
        }
        
        // 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 <= CLOSE_ENOUGH)
        {
            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 (navigLastLocation, navigDestination, null, (focus == null ? navigDestination : focus), 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.finer ("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 MyPathNavigator#navigNextLocation}.
     */
    private int navigNextLocationOffset = 0;

    /**
     * Last location in the path (the one the agent already reached).
     */
    private Location navigLastLocation = null;
    
    /**
     * If {@link MyPathNavigator#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 MyPathNavigator#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 MyPathNavigator#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 MyPathNavigator#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 = bot.getLocation();
        navigCurrentNode = DistanceUtils.getNearest(bot.getWorldView().getAll(NavPoint.class).values(), bot.getLocation(), 40);
        // 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 <= CLOSE_ENOUGH)
        {
            log.finest ("Navigator.navigAlongPath(): destination close enough: " + totalDistance);
            return Stage.COMPLETED;
        }
        
        // navigate
        if (navigStage.mover) {
        	log.fine("MOVER");
        	return navigThroughMover();
        } else
        if (navigStage.teleport) {
        	log.fine("TELEPORT");
        	return navigThroughTeleport();
        } else {
        	log.fine("STANDARD");
            return navigToCurrentNode(true); // USE FOCUS, normal navigation
        }
    }

    /*========================================================================*/

    /**
     * Prepares next navigation node in path.
     * <p><p>
     * If necessary just recalls {@link MyPathNavigator#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 ()
    {    	
    	if (log != null && log.isLoggable(Level.FINER)) log.finer ("Navigator.switchToNextNode(): switching!");
    	
        // 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);
        
        if (navigCurrentLink == null) {
        	getNavPointsLink(navigLastNode, navigCurrentNode);
        	if (log.isLoggable(Level.INFO)) {
        		log.info("No link information...");
        	}
        }
        
        // ensure that the last node is not null
        if (navigLastLocation == null) {
            navigLastLocation = bot.getLocation();
            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();	            
	            resetNavigMoverVariables();
	            
	            // AREN'T WE ALREADY ON THE LIFT CENTER?
	            if (memory.getLocation().getDistance(navigCurrentNode.getLocation()) < CLOSE_ENOUGH) {
	            	// YES WE ARE!
	            	navigStage = navigStage.next();
	            }	            
	        } else
	        // are we still moving on mover?
	        if (navigStage.mover)
	        {
	        	navigStage = navigStage.next();
	            // init the runner
	            runner.reset();
	        } else
	        if (navigStage.teleport) {
	        	navigStage = navigStage.next();
	            // init the runner
	            runner.reset();
	        } else
	        // no movers & teleports
	        {
	            // 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;
        
        prepareNextNode();
        
        if (localDistance < 20) {
        	return switchToNextNode();
        }
        
        return true;
    }

    protected 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;
	}

	//
    // NAVIG MOVER VARIABLES
    //
    
    private int navigMoverRideUpCount;

	private int navigMoverRideDownCount;

	private Boolean navigMoverIsRidingUp;

	private Boolean navigMoverIsRidingDown;
	
	private void resetNavigMoverVariables() {
    	navigMoverIsRidingUp = null;
    	navigMoverIsRidingDown = null;
        navigMoverRideUpCount = 0;
        navigMoverRideDownCount = 0;
	}
	
	private void checkMoverMovement(Mover mover) {
		// ASSUMING THAT MOVER IS ALWAYS ... riding fully UP, riding fully DOWN (or vice versa) and passing all possible exits
		if (mover.getVelocity().z > 0) {
			// mover is riding UP
			if (navigMoverIsRidingUp == null) {
				navigMoverIsRidingUp = true;
				navigMoverIsRidingDown = false;
				navigMoverRideUpCount = 1;
				navigMoverRideDownCount = 0;
			} else
			if (navigMoverIsRidingDown) {
				navigMoverIsRidingUp = true;
				navigMoverIsRidingDown = false;
				++navigMoverRideUpCount;
			}
		} else 
		if (mover.getVelocity().z < 0) {
			// mover is riding DOWN
			if (navigMoverIsRidingDown == null) {
				navigMoverIsRidingUp = false;
				navigMoverIsRidingDown = true;
				navigMoverRideUpCount = 0;
				navigMoverRideDownCount = 1;
			} else 
			if (navigMoverIsRidingUp) {
				navigMoverIsRidingUp = false;
				navigMoverIsRidingDown = true;
				++navigMoverRideDownCount;
			}
		}
	}

	/*========================================================================*/

    /**
     * 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 (boolean useFocus)
    {
    	if (navigCurrentNode != null) {
    		// update location of the current place we're reaching ... it might be Mover after all
    		navigCurrentLocation = navigCurrentNode.getLocation();
       	}
    	if (navigNextNode != null) {
    		// update location of the next place we're reaching ... it might be Mover after all
    		navigNextLocation = navigNextNode.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))
        );
        int distanceZ = (int) memory.getLocation().getDistanceZ(navigCurrentLocation);

        // where are we going to run to
        Location firstLocation = navigCurrentLocation.getLocation();
        // where we're going to continue
        Location secondLocation = (navigNextNode != null ?
        		 						(navigNextNode.isLiftCenter() || navigNextNode.isLiftExit() ? 
        		 								null // let navigThroughMover() to handle these cases with care!
        		 							:	navigNextNode.getLocation())
        		 						: navigNextLocation);
        // and what are we going to look at
        ILocated focus = (this.focus == null || !useFocus ?
        						((navigNextLocation == null) ? firstLocation : navigNextLocation.getLocation())
        						:
        						this.focus
        				 );

        // run to the current node..
        if (!runner.runToLocation (navigLastLocation, 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);

        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;
        }
        if (navigCurrentLink != null && (navigCurrentLink.getFlags() & LinkFlag.JUMP.get()) != 0) {
        	// we need to jump to reach the destination ... do not switch based on localDistance2
        	localDistance2 = 10000;
        }
        
        if (navigCurrentLocation != null && navigCurrentLocation.equals(executor.getPath().get(executor.getPath().size()-1))
        	|| (!navigIterator.hasNext() && (navigNextLocation == null || navigCurrentLocation == navigNextLocation))) {
        	// if we're going to the LAST POINT ... be sure to get to the exact location in order to ensure pick up of the item
        	testDistance = 2 * ((int) UnrealUtils.CHARACTER_COLLISION_RADIUS);
        }
            
        // are we close enough to switch to the next node? (mind the distanceZ particularly!)
        if ( distanceZ < 40 && ((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(): switching 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;
        }
        
        Mover mover = (Mover) bot.getWorldView().get(navigCurrentNode.getMover());
        if (mover == null) {
        	if (log != null && log.isLoggable(Level.WARNING)) log.warning("LoqueNavigator.navigThroughMover("+stage+"): can't navigate through the mover as current node does not represent a mover (moverId == null): " + navigCurrentNode);
        	return Stage.CRASHED;
        }
        
        // update navigCurrentLocation as the mover might have moved
        navigCurrentLocation = navigCurrentNode.getLocation();
        
        if (navigNextNode != null) {
        	// update navigNextLocation as the mover might have moved
        	navigNextLocation = navigNextNode.getLocation();
        }
                
        log.info("navig-curr: " + navigCurrentNode);
        log.info("navig-next: " + navigNextNode);
        
        // get horizontal distance from the mover center node ... always POSITIVE
        int hDistance = (int) memory.getLocation().getDistance2D(navigCurrentLocation.getLocation());
        // get vertical distance from the mover center node ... +/- ... negative -> mover is below us, positive -> mover is above us
        int zDistance = (int) navigCurrentLocation.getLocation().getDistanceZ(memory.getLocation());
        // whether mover is riding UP
        boolean moverRidingUp = mover.getVelocity().z > 0;
        // whether mover is riding DOWN
		boolean moverRidingDown = mover.getVelocity().z < 0;
		// whether mover is standing still
		boolean moverStandingStill = Math.abs(mover.getVelocity().z) < Location.DISTANCE_ZERO;
        
    	if (navigStage == Stage.AWAITING_MOVER) {   
    		// Aren't we under the mover?
    		if (zDistance > 0 && moverRidingUp) {
    			// we're under the mover and the mover is riding up...
    			if (log != null && log.isLoggable(Level.FINER)) {
	            	log.finer (
		                "LoqueNavigator.navigThroughMover("+stage+"): we are UNDER the mover and mover is RIDING UP ... getting back to waiting position"
		                + ", zDistance " + zDistance + ", mover.velocity.z " + mover.getVelocity().z + ", mover " + (moverRidingUp ? "riding UP" : (moverRidingDown ? "riding DOWN" : moverStandingStill ? "standing STILL" : " movement unknown"))
		            );
    			}
	            // run to the last node, the one we need to be waiting on for the mover to come
	        	// WE MUST NOT USE FOCUS! Because we need to see the mover TODO: provide turning behavior, i.e., if focus is set, once in a time turn to then direction
    			if (!runner.runToLocation(memory.getLocation(), navigLastLocation, null, navigCurrentLocation, null, (navigLastNode == null ? true : isReachable(navigLastNode))))
	            {
	                if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigThroughMover("+stage+"): navigation to wait-for-mover node failed");
	                return Stage.CRASHED;
	            }
    			return navigStage;
    		}
    		
	        // 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 (hDistance > 400) {
    			if (log != null && log.isLoggable(Level.WARNING)) log.warning("LoqueNavigator.navigThroughMover("+stage+"): failed to get onto the mover as its 2D distance is > 400, hDistance " + hDistance + ", unsupported!");
                return Stage.CRASHED;
    		}
    		
	        if (zDistance > 30 && moverRidingUp) // mover is riding UP and is already above us, we won't make it...
	        {
	            // run to the last node, the one we need to be waiting on for the mover to come
	        	if (log != null && log.isLoggable(Level.FINER)) {
	            	log.finer (
		                "LoqueNavigator.navigThroughMover("+stage+"): waiting for the mover to come"
		                + " | zDistance " + zDistance + ", hDistance " + hDistance + ", mover " + (moverRidingUp ? "riding UP" : (moverRidingDown ? "riding DOWN" : moverStandingStill ? "standing STILL" : " movement unknown"))
		                + ", node " + navigCurrentNode.getId().getStringId()
		            );
	        	}
	        	// WE MUST NOT USE FOCUS! Because we need to see the mover TODO: provide turning behavior, i.e., if focus is set, once in a time turn to then direction
	            if (!runner.runToLocation(memory.getLocation(), navigLastLocation, null, navigCurrentLocation, 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;
	            }
	
	            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"		            
		            + " | zDistance " + zDistance + ", hDistance " + hDistance + ", mover " + (moverRidingUp ? "riding UP" : (moverRidingDown ? "riding DOWN" : moverStandingStill ? "standing STILL" : " movement unknown"))
		            + ", node " + navigCurrentNode.getId().getStringId()
		        );
	        
	        // LET'S MOVE TO THE LIFT CENTER (do not use focus)
	        return navigToCurrentNode(false);
    	} else
    	if (navigStage == Stage.RIDING_MOVER) {
    		checkMoverMovement(mover);
    		
    		if (navigMoverRideDownCount > 2 || navigMoverRideUpCount > 2) {
    			// we're riding up & down without any effect ... failure :(
    			if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigThroughMover("+stage+"): navigation to mover exit node failed, we've rided twice up & down and there was no place suitable to exit the mover in order to get to get to " + navigCurrentNode);
	            return Stage.CRASHED;
    		}
    		
    		if (hDistance > 400) {
    			if (log != null && log.isLoggable(Level.WARNING)) log.warning("LoqueNavigator.navigThroughMover("+stage+"): navigation to mover exit node failed, the node is too far, hDistance " + hDistance + " > 400, unsupported (wiered navigation graph link)");
	            return Stage.CRASHED;
    		}
    		
    		// wait for the mover to ride us up/down
    		if ( Math.abs(zDistance) > 50 ) {
 	            // run to the last node, the one we're waiting on
    			if (log != null && log.isLoggable(Level.FINER)) 
 	            	log.finer (
 		                "LoqueNavigator.navigThroughMover("+stage+"): riding the mover"               
 		                + " | zDistance " + zDistance + ", hDistance " + hDistance + ", mover " + (moverRidingUp ? "riding UP" : (moverRidingDown ? "riding DOWN" : moverStandingStill ? "standing STILL" : " movement unknown"))
 		                + ", node " + navigCurrentNode.getId().getStringId()
 		            ); 	
    			
    			// WE MUST NOT USE FOCUS! We have to see the mover. TODO: provide turning behavior, i.e., turn to desired focus once in a time
 	            if (!runner.runToLocation(memory.getLocation(), navigLastLocation, null, navigCurrentLocation, 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
 	            
 	            return navigStage;
 	        }
    		
    		// MOVER HAS ARRIVED TO POSITION FOR EXIST (at least that what we're thinking so...)
	        if (log != null && log.isLoggable(Level.FINER)) 
		        log.finer (
		            "Navigator.navigThroughMover("+stage+"): exiting the mover"
		            + " | zDistance " + zDistance + ", hDistance " + hDistance + ", mover " + (moverRidingUp ? "riding UP" : (moverRidingDown ? "riding DOWN" : moverStandingStill ? "standing STILL" : " movement unknown"))
		            + ", node " + navigCurrentNode.getId().getStringId()		            
		        );
	        
	        // LET'S MOVE TO THE LIFT EXIT (do not use focus)
	        return navigToCurrentNode(false);    		
    	} 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 (navigNextNode != null) {
    		// update location of the Next place we're reaching
    		navigNextLocation = navigNextNode.getLocation();
    	}
    	
    	// 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
        ILocated focus = (this.focus == null ? 
        						((navigNextLocation == null) ? firstLocation : navigNextLocation.getLocation())
        						:
        						this.focus
        				 );

        // run to the current node..
        if (!runner.runToLocation(navigLastLocation, 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 IUT2004PathRunner runner;

    /*========================================================================*/

    /** Agent's main. */
    protected UT2004Bot 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 MyPathNavigator (UT2004Bot 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 MyPathRunner(bot, memory, body, log);
        
        this.selfListener = new SelfListener( bot.getWorldView() );
    }

    /**
     * Constructor.
     * @param main Agent's main.
     * @param memory Loque memory.
     */
    public MyPathNavigator (UT2004Bot bot, IUT2004PathRunner runner, Logger log)
    {
        // setup reference to agent
        this.main = bot;
        this.memory = new AgentInfo(bot);
        this.body = new AdvancedLocomotion(bot, log);
        this.log = log;
        
        this.selfListener = new SelfListener( bot.getWorldView() );

        // save runner object
        this.runner = runner;
        NullCheck.check(this.runner, "runner");
    }

	public Logger getLog() {
		return log;
	}
        
}