package cz.cuni.amis.pogamut.ut2004.agent.navigation;
   
   import java.util.ArrayList;
   import java.util.List;
   import java.util.logging.Level;
   
   import cz.cuni.amis.pogamut.base.agent.navigation.IPathExecutorState;
   import cz.cuni.amis.pogamut.base.agent.navigation.IPathFuture;
   import cz.cuni.amis.pogamut.base.agent.navigation.IStuckDetector;
  import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
  import cz.cuni.amis.pogamut.base.utils.logging.LogCategory;
  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.floydwarshall.FloydWarshallMap;
  import cz.cuni.amis.pogamut.ut2004.agent.navigation.loquenavigator.KefikRunner;
  import cz.cuni.amis.pogamut.ut2004.agent.navigation.loquenavigator.LoqueNavigator;
  import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.UT2004DistanceStuckDetector;
  import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.UT2004PositionStuckDetector;
  import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.UT2004TimeStuckDetector;
  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.EndMessage;
  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Item;
  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Player;
  import cz.cuni.amis.utils.flag.FlagListener;
  
  /**
   * Facade for navigation in UT2004. Method navigate() can be called both synchronously and asynchronously.
   * Uses UT2004PathExecutor and FloydWarshallMap for navigating and planning.
   * @author knight
   */
  public class UT2004Navigation {
  
  	/** Log used by this class. */
  	protected LogCategory log;
      /** UT2004PathExecutor that is used for the navigation. */
      protected UT2004PathExecutor<ILocated> pathExecutor;
      /** FloydWarshallMap that is used for path planning. */
      protected FloydWarshallMap fwMap;    
      /** UT2004Bot reference. */
      protected UT2004Bot bot;
      /** UT2004GetBackToNavGraph for returning bot back to the navigation graph. */
      protected UT2004GetBackToNavGraph getBackToNavGraph;
      /** UT2004RunStraight is used to run directly to player at some moment. */
      protected UT2004RunStraight runStraight;
      /** Location threshold for requesting of a new path or switching a path. */
      protected static final int NEW_PATH_DISTANCE_THRESHOLD = 40;
      /** Location threshold for checking whether we have arrived on target. For XY - 2D plane distance */
      protected static final int ARRIVED_AT_LOCATION_XY_THRESHOLD = 50;
      /** Location threshold for checking whether we have arrived on target. For Z distance. */
      protected static final int ARRIVED_AT_LOCATION_Z_THRESHOLD = 100;
      /** When PLAYER is further from currentTarget than this location, recompute the path */
  	protected static final double PLAYER_DISTANCE_TRASHOLD = 600;
  	/** We're managed to get to player */
  	public static final double AT_PLAYER = 100;
      
      /**
       * Listener for UT2004PathExecutor state. Not used right now.
       */
      FlagListener<IPathExecutorState> myUT2004PathExecutorStateListener = new FlagListener<IPathExecutorState>() {
  
          @Override
          public void flagChanged(IPathExecutorState changedValue) {
              switch (changedValue.getState()) {                
                  case TARGET_REACHED:
                  	targetReached();
                      break;
                  case PATH_COMPUTATION_FAILED:
                  case STUCK:
                  	stuck();
                      break;
              }
          }
      };
      
      protected IWorldEventListener<EndMessage> endMessageListener = new IWorldEventListener<EndMessage>() {		
  		@Override
  		public void notify(EndMessage event) {
  			navigate();
  		}
  	};
  	
  
      // ===========
      // CONSTRUCTOR
      // ===========
      
      /**
       * Here you may specify any custom UT2004Navigation parts.
       * 
       * @param bot
       * @param ut2004PathExecutor
       * @param fwMap
       * @param getBackOnPath
       * @param runStraight 
       */
     public UT2004Navigation(UT2004Bot bot, UT2004PathExecutor ut2004PathExecutor, FloydWarshallMap fwMap, UT2004GetBackToNavGraph getBackOnPath, UT2004RunStraight runStraight) {
         this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
     	this.bot = bot;
     	
     	this.fwMap = fwMap;
         this.pathExecutor = ut2004PathExecutor;
         
         this.getBackToNavGraph = getBackOnPath;
         this.runStraight = runStraight;
 
         initListeners();
     }
     
     /**
      * Will auto-create all needed UT2004Navigation subparts.
      * @param bot
      * @param info
      * @param move
      */
 	public UT2004Navigation(UT2004Bot bot, AgentInfo info, AdvancedLocomotion move) {
     	this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
      	this.bot = bot;    	
     	this.fwMap = new FloydWarshallMap(bot);
     	
 		this.pathExecutor = 
         	new UT2004PathExecutor<ILocated>(
         		bot, 
         		new LoqueNavigator<ILocated>(bot, 
         			new KefikRunner(bot, info, move, bot.getLog()), 
         		bot.getLog())
         	);
 		
 		// add stuck detectors that watch over the path-following, if it (heuristicly) finds out that the bot has stuck somewhere,
     	// it reports an appropriate path event and the path executor will stop following the path which in turn allows 
     	// us to issue another follow-path command in the right time
         this.pathExecutor.addStuckDetector(new UT2004TimeStuckDetector(bot, 3000, 100000)); // if the bot does not move for 3 seconds, considered that it is stuck
         this.pathExecutor.addStuckDetector(new UT2004PositionStuckDetector(bot));           // watch over the position history of the bot, if the bot does not move sufficiently enough, consider that it is stuck
         this.pathExecutor.addStuckDetector(new UT2004DistanceStuckDetector(bot));           // watch over distances to target
         
         this.getBackToNavGraph = new UT2004GetBackToNavGraph(bot, info, move);
         this.runStraight = new UT2004RunStraight(bot, info, move);
         
         initListeners();
 	}
 
     private void initListeners() {
     	this.pathExecutor.getState().addListener(myUT2004PathExecutorStateListener);        
         bot.getWorldView().addEventListener(EndMessage.class, endMessageListener);
 	}
 	
 	// ======================
     // PUBLIC INTERFACE
     // ======================
         
 	/**
      * True if UT2004PathExecutor is currently trying to get somewhere.
      * @return
      */
     public boolean isNavigating() {
         return pathExecutor.isExecuting();
     }
     
     /**
      * Sets focus of the bot when navigating (when using this object to run to some location target)!
      * To reset focus call this method with null parameter.
      * @param located
      */
     public void setFocus(ILocated located) {
         pathExecutor.setFocus(located);
         getBackToNavGraph.setFocus(located);
         runStraight.setFocus(located);
     }
 
     /**
      * Stops navigation and resets the class.
      * 
      * Does NOT reset focus!
      */
     public void stopNavigation() {
         reset(true);
     }
     
     /**
      * This method can be called periodically or asynchronously. Will move bot to input location.
      * Uses UT2004PathExecutor and FloydWarshallMap.
      * The bot will stop on bad input (location null).
      * @param target target location
      */
     public void navigate(ILocated target) {
     	if (target == null) {
     		if (log != null && log.isLoggable(Level.WARNING)) log.warning("Cannot navigate to NULL target!");
     		return;
     	}
     	
     	if (target instanceof Player) {
     		// USE DIFFERENT METHOD INSTEAD
     		navigate((Player)target);
     		return;
     	}
     	
     	if (navigating) {
     		if (currentTarget == target || currentTarget.getLocation().equals(target.getLocation())) {
     			// just continue with current execution
     			return;
     		}    		
     		// NEW TARGET!
     		// => reset - stops pathExecutor as well, BUT DO NOT STOP getBackOnPath (we will need to do that eventually if needed, or it is not running)
     		reset(false);
     	}
     	
     	if (log != null && log.isLoggable(Level.FINE)) log.fine("Start navigating to: " + target);
     	
     	navigating = true;
     	
     	currentTarget = target;
     	
     	navigate();
     }
     
     /**
      * This method can be called periodically or asynchronously. Will move bot to input location.
      * Uses UT2004PathExecutor and FloydWarshallMap.
      * The bot will stop on bad input (location null).
      * @param location target location
      */
     public void navigate(Player player) {
     	if (player == null) {
     		if (log != null && log.isLoggable(Level.WARNING)) log.warning("Cannot navigate to NULL player!");
     		return;
     	}
     	
     	if (navigating) {
     		if (currentTarget == player) {
     			// just continue with the execution
     			return;
     		}    		
     		// NEW TARGET!
     		// => reset - stops pathExecutor as well, BUT DO NOT STOP getBackOnPath (we will need to do that eventually if needed, or it is not running)
     		reset(false);
     	}
     	
     	if (log != null && log.isLoggable(Level.FINE)) log.fine("Start pursuing: " + player);
     	
     	navigating = true;
     	
     	currentTarget = player.getLocation();
     	
     	currentTargetPlayer = player;
     	
     	navigate();
     }
     
     /**
      * Returns nearest navigation point to input location. FloydWarshallMap works only
      * on NavPoints.
      * @param location
      * @return
      */
     public NavPoint getNearestNavPoint(ILocated location) {
     	if (location instanceof NavPoint) return (NavPoint)location;
     	if (location instanceof Item) {
     		if (((Item)location).getNavPoint() != null) return ((Item)location).getNavPoint();
     	}
     	return DistanceUtils.getNearest(bot.getWorldView().getAll(NavPoint.class).values(), location);        
     }
     
     /**
      * Returns COPY of current path in list. May take some time to fill up. Returns
      * empty list if path not computed.
      * @return
      */
     public List<ILocated> getCurrentPathCopy() {
         List<ILocated> result = new ArrayList();
         if (currentFuturePath != null) {
             result.addAll(currentFuturePath.get());
         }
         return result;
     }
 
     /**
      * Returns current path as in IPathFuture object that is used by ut2004pathExecutor
      * to navigate. Can be altered. May return null if path not computed!
      * Be carefull when altering this during UT2004PathExecutor run - it may cause
      * undesirable behavior.
      * @return
      */
     public List<ILocated> getCurrentPathDirect() {
         if (currentFuturePath != null) {
             return currentFuturePath.get();
         }
         return null;
     }
     
     /**
      * Current POINT where the navigation is trying to get to.
      * @return
      */
     public ILocated getCurrentTarget() {
     	return currentTarget;
     }
     
     /**
      * If navigation is trying to get to some player, this method returns non-null player we're trying to get to.
      * @return
      */
     public Player getCurrentTargetPlayer() {
     	return currentTargetPlayer;
     }
     
     /**
      * Returns previous location we tried to get to (e.g. what was {@link UT2004Navigation#getCurrentTarget()} before
      * another {@link UT2004Navigation#navigate(ILocated)} or {@link UT2004Navigation#navigate(Player)} was called.
      * @return
      */
     public ILocated getLastTarget() {
     	return lastTarget;
     }
     
     /**
      * If previous target was a player, returns non-null player we previously tried to get to 
      * (e.g. what was {@link UT2004Navigation#getCurrentTargetPlayer()} before
      * another {@link UT2004Navigation#navigate(ILocated)} or {@link UT2004Navigation#navigate(Player)} was called.
      * @return
      */
     public ILocated getLastTargetPlayer() {
     	return lastTarget;
     }
         
     // ======================
     // VARIABLES
     // ======================
     
     /** Last location target. */
     protected ILocated lastTarget = null;
     /** Last location target. */
     protected Player   lastTargetPlayer = null;
     /** Current location target. */
     protected ILocated currentTarget = null;
     /** Current target is player (if not null) */
     protected Player   currentTargetPlayer = null;
     /** Navpoint we're running from (initial position when path executor has been triggered) */
     protected NavPoint fromNavPoint;
     /** Navpoint we're running to, nearest navpoint to currentTarget */
 	protected NavPoint toNavPoint;    
     /** Current path stored in IPathFuture object. */
     protected IPathFuture currentFuturePath;
     /** Whether navigation is running. */
     protected boolean navigating = false;
     /** We're running straight to the player. */
 	protected boolean runningStraightToPlayer = false;
 	/** Where run-straight failed. */
 	protected Location runningStraightToPlayerFailedAt = null;
 	
     // ======================
     // UTILITY METHODS
     // ======================
     
     protected void navigate() {
 		if (!navigating) return;
 		
 		if (log != null && log.isLoggable(Level.FINE)) {
 			log.fine("NAVIGATING");
 		}
 		if (currentTargetPlayer != null) {
 			if (log != null && log.isLoggable(Level.FINE)) log.fine("Pursuing " + currentTargetPlayer);
 			navigatePlayer();
 		} else {
 			if (log != null && log.isLoggable(Level.FINE)) log.fine("Navigating to " + currentTarget);
 			navigateLocation();
 		}
 	}
     
     private void navigateLocation() {
     	if (pathExecutor.isExecuting()) {
 			// Navigation is driven by Path Executor already...		
 			if (log != null && log.isLoggable(Level.FINE)) log.fine("Path executor running");			
 			return;
 		}
 		
 		// PATH EXECUTOR IS NOT RUNNING
 		// => we have not started to run along path yet
 
 		// ARE WE ON NAV-GRAPH?
 		if (!getBackToNavGraph.isOnNavGraph()) {
 			// NO!
 			// => get back to navigation graph
 			if (log != null && log.isLoggable(Level.FINE)) log.fine("Getting back to navigation graph");
     		getBackToNavGraph.execute();
     		return;
     	}
 		// YES!    	
     	// ... getBackToNavGraph will auto-terminate itself when we manage to get back to graph
     	
     	fromNavPoint = getNearestNavPoint(bot.getLocation());
     	toNavPoint   = getNearestNavPoint(currentTarget);
     	
     	if (log != null && log.isLoggable(Level.FINE)) log.fine("Running from " + fromNavPoint.getId().getStringId() + " to " + toNavPoint.getId().getStringId());
     	
     	currentFuturePath = fwMap.computePath(fromNavPoint, toNavPoint);
     	
     	// FLOYD-WARSHAL HAS ALL PATHS PRECOMPUTED...
     	// => path is already ready ;)
     	
     	// TINKER THE PATH
     	processPathFuture(currentFuturePath);
     	// LET'S START RUNNING!
     	pathExecutor.followPath(currentFuturePath);	
 	}
 
 	private void navigatePlayer() {
 		double vDistance = bot.getLocation().getDistanceZ(currentTargetPlayer.getLocation());
 		double hDistance = bot.getLocation().getDistance2D(currentTargetPlayer.getLocation());
 		
 		if (hDistance < AT_PLAYER && vDistance < 50) {
 			// player reached
 			if (log != null && log.isLoggable(Level.FINE)) log.fine("Player reached");	
 			if (pathExecutor.isExecuting()) {
 				pathExecutor.getPath().set(pathExecutor.getPath().size()-1, bot.getLocation());
 			} else {
 				targetReached();
 			}
 			return;
 		}
 		
 		if (hDistance < 400 && Math.abs(vDistance) < 50) {
 			// RUN STRAIGHT			
 			if (runningStraightToPlayer) {
 				if (runStraight.isFailed()) {
 					runningStraightToPlayer = false;
 					runningStraightToPlayerFailedAt = bot.getLocation();
 				}
 			} else {
 				if (runningStraightToPlayerFailedAt == null ||                           // we have not failed previously
 					bot.getLocation().getDistance(runningStraightToPlayerFailedAt) > 500 // or place where we have failed is too distant
 				){
 					if (getBackToNavGraph.isExecuting()) {
 						getBackToNavGraph.stop();
 					}
 					if (pathExecutor.isExecuting()) {
 						pathExecutor.stop();
 					}
 					runningStraightToPlayer = true;
 					runStraight.runStraight(currentTargetPlayer);
 				}				
 			}
 			if (runningStraightToPlayer) {
 				if (log != null && log.isLoggable(Level.FINE)) log.fine("Running straight to player");
 				return;
 			}
 		} else {
 			if (runningStraightToPlayer) {
 				runningStraightToPlayer = false;
 				runStraight.stop();				
 			}
 		}
 		
 		if (pathExecutor.isExecuting()) {
 			// Navigation is driven by Path Executor already...			
 			if (log != null && log.isLoggable(Level.FINE)) log.fine("Path executor running");
 			// check distance between point we're navigating to and current player's location
 			double distance = currentTarget.getLocation().getDistance(currentTargetPlayer.getLocation());
 			if (distance > PLAYER_DISTANCE_TRASHOLD) {
 				if (log != null && log.isLoggable(Level.FINE)) log.fine("Player moved " + distance + " from its original location, checking path...");
 				// WE NEED TO CHECK ON PATH!					
 				NavPoint newToNavPoint = getNearestNavPoint(currentTargetPlayer);
 				if (newToNavPoint != toNavPoint) {
 					// WE NEED TO ALTER THE PATH!
 					if (log != null && log.isLoggable(Level.FINE)) log.fine("Replanning path to get to " + currentTargetPlayer);
 					
 					ILocated currentlyRunningTo = pathExecutor.getPathElement();
 					if (currentlyRunningTo == null) currentlyRunningTo = bot.getLocation();
 					fromNavPoint = getNearestNavPoint(currentlyRunningTo);
 					toNavPoint   = newToNavPoint;
 					currentTarget = currentTargetPlayer.getLocation();
 					
 					currentFuturePath = fwMap.computePath(fromNavPoint, toNavPoint);
 					
 					// TINKER THE PATH
 			    	processPathFuture(currentFuturePath);
 			    	// LET'S START RUNNING!
 			    	pathExecutor.followPath(currentFuturePath);	
 				} else {
 					if (log != null && log.isLoggable(Level.FINE)) log.fine("Path remains the same");
 				}
 			}
 			
 			return;
 		}
 		
 		// PATH EXECUTOR IS NOT RUNNING
 		// => we have not started to run along path yet
 
 		// ARE WE ON NAV-GRAPH?
 		if (!getBackToNavGraph.isOnNavGraph()) {
 			// NO!
 			// => get back to navigation graph
 			if (log != null && log.isLoggable(Level.FINE)) log.fine("Getting back to navigation graph");
     		getBackToNavGraph.execute();
     		return;
     	}
 		// YES!    	
     	// ... getBackToNavGraph will auto-terminate itself when we manage to get back to graph
     	
     	fromNavPoint = getNearestNavPoint(bot.getLocation());
     	toNavPoint   = getNearestNavPoint(currentTarget);
     	
     	if (log != null && log.isLoggable(Level.FINE)) log.fine("Running from " + fromNavPoint.getId().getStringId() + " to " + toNavPoint.getId().getStringId());
     	
     	currentFuturePath = fwMap.computePath(fromNavPoint, toNavPoint);
     	
     	// FLOYD-WARSHAL HAS ALL PATHS PRECOMPUTED...
     	// => path is already ready ;)
     	
     	// TINKER THE PATH
     	processPathFuture(currentFuturePath);
     	// LET'S START RUNNING!
     	pathExecutor.followPath(currentFuturePath);	
 	}
 
 	/**
      * Checks if last path element is in close distance from our desired target and if not, we
      * will add our desired target as the last path element.
      * @param futurePath
      */
     protected void processPathFuture(IPathFuture futurePath) {
         List<ILocated> pathList = futurePath.get();
         
         if (pathList == null) {
         	// we failed to compute the path, e.g., path does not exist
         	return;
         }
         
         if (!pathList.isEmpty()) {
             ILocated lastPathElement = pathList.get(pathList.size() - 1);
             if (lastPathElement.getLocation().getDistance(currentTarget.getLocation()) > NEW_PATH_DISTANCE_THRESHOLD) {
                 currentFuturePath.get().add(currentTarget);
             }
         } else {
             currentFuturePath.get().add(currentTarget);
         }
     }
         
     protected void stuck() {
     	// DAMN ...
     	reset(true);
 	}
 
 	protected void targetReached() {
 		// COOL !!!
 		reset(true);
 	}
     
     protected void reset(boolean stopGetBackToNavGraph) {
     	if (currentTarget != null) {
     		lastTarget = currentTarget;
     		lastTargetPlayer = currentTargetPlayer;
     	}
     	
     	navigating = false;
     	
     	currentTarget = null;
     	currentTargetPlayer = null;
     	
     	fromNavPoint = null;
     	toNavPoint = null;
     	
     	currentFuturePath = null;
     	
     	runningStraightToPlayer = false;
     	runningStraightToPlayerFailedAt = null;
     	
     	
     	pathExecutor.stop();
     	runStraight.stop();
     	if (stopGetBackToNavGraph) getBackToNavGraph.stop();
     }   
     
     // ============================
     // TWEAKING / LISTENERS
     // ============================
 
     /**
      * Use this to register listener to various states of path execution - stuck, target reached, etc.
      * See {@link IPathExecutorState#getState()}.
      * 
      * @param listener
      */
     public void addStrongNavigationListener(FlagListener<IPathExecutorState> listener) {
         pathExecutor.getState().addStrongListener(listener);
     }
 
     /**
      * Removes path state listener.
      * @param listener
      */
     public void removeStrongNavigationListener(FlagListener<IPathExecutorState> listener) {
         pathExecutor.getState().removeListener(listener);
     }
 
     /**
      * Returns list of all stuck detectors.
      * @return
      */
     public List<IStuckDetector> getStuckDetectors() {
         return pathExecutor.getStuckDetectors();
     }
     
     /**
      * Adds stuck detector for UT2004PathExecutor.
      * @param stuckDetector
      */
     public void addStuckDetector(IStuckDetector stuckDetector) {
         pathExecutor.addStuckDetector(stuckDetector);
     }
 
     /**
      * Removes stuck detector for UT2004PathExecutor.
      * @param stuckDetector
      */
     public void removeStuckDetector(IStuckDetector stuckDetector) {
         pathExecutor.removeStuckDetector(stuckDetector);
     }
     
 }