package cz.cuni.amis.pogamut.udk.agent.navigation.martinnavigator;
   
   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.
   *
   * @author Jimmy
   */
  public class MartinNavigator<PATH_ELEMENT extends ILocated> extends AbstractUDKPathNavigator<PATH_ELEMENT>
  {
     
  	/*========================================================================*/
  
      /**
       * Distance, which is considered as close enough..
       */
      public static final int CLOSE_ENOUGH = 60;
      public static final int PRECISION = 100; //empirically it was shown that 60 and 75 is sometimes not achieved. 
  
      @Override
      public double getPrecision() {
          return PRECISION; 
      }
      
      /**
       * Current navigation destination.
       */
      private Location navigDestination = null;
  
      /**
       * Current stage of the navigation.
       */
      private Stage navigStage = Stage.COMPLETED;
  
      
      /*========================================================================*/
      
  	@Override
  	protected void navigate(int pathElementIndex) {
  		switch (navigStage = keepNavigating()) {
  		case NAVIGATING:
  		case REACHING:	
  			// ALL OK, we're running fine
  			return;
  		case CRASHED:
  			// DAMN!
  			executor.stuck();
  			return;
  		case COMPLETED:
  			// HURRAY!
  			executor.targetReached();
  			return;
  		}
  	}
  	
  	@Override
  	public void reset() {
  		// reinitialize the navigator's values
  		navigCurrentLocation = null;
  		navigCurrentNode = null;
          navigCurrentLink = null;
  		navigDestination = null;
  		navigIterator = 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 navigation to the specified destination along specified path.
       * @param destination Destination of the navigation.
       * @param path Navigation path to the destination.
       */
     protected void initPathNavigation(Location destination, 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(destination, path))
         {
             // do it directly then..
             initDirectNavigation(destination);
         }
     }
     
     /**
      * Initializes navigation along path.
      * @param dest Destination of the navigation.
      * @param path Path of the navigation.
      * @return True, if the navigation is successfuly initialized.
      */
     protected 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();
     }
 	
 	/**
      * 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)
     {
         initDirectly (dest);
     }
 	
     /**
      * Initializes direct navigation to given destination.
      * 
      * @param dest Destination of the navigation.
      * @return Next stage of the navigation progress.
      */
     protected Stage initDirectly(Location dest)
     {
         // setup navigation info
         navigDestination = dest;
         // init runner
         runner.reset();
         // reset navigation stage
         return navigStage = Stage.REACHING;
     }
     
     /*========================================================================*/
     
     /**
      * 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;
     }
 
     /*========================================================================*/    
 
     /**
      * 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 MartinNavigator#navigNextLocation}.
      */
     private int navigNextLocationOffset = 0;
 
     /**
      * Current node in the path (the one the agent is running to).
      */
     private Location navigCurrentLocation = null;
     
     /**
      * If {@link MartinNavigator#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 MartinNavigator#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 MartinNavigator#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;
     }
 
    
 
     /**
      * 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;
         }
         
         return navigToCurrentNode();        
     }
 
     /*========================================================================*/
 
     /**
      * Prepares next navigation node in path.
      * <p><p>
      * If necessary just recalls {@link MartinNavigator#prepareNextNodeTeleporter()}.
      */
     private void prepareNextNode ()
     {
     	// 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);
     }
     
     /**
      * 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
         Location navigLastLocation = navigCurrentLocation;
         NavPoint 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
                     
                 );
         } else {
             // 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)
 		        );
         }
 
         // 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);
             }
         } else {
         	// CHECK 2nd LOCATION THEN!
         	if (navigNextLocation != null) {
         		localDistance = (int) memory.getLocation().getDistance(navigNextLocation.getLocation());
                 // get the distance from the current node (neglecting jumps)
                 localDistance2 = (int) memory.getLocation().getDistance(
                     Location.add(navigNextLocation.getLocation(), new Location (0,0,100))
                 );
                 
                 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;
     }
 
     /*========================================================================*/
 
     /**
      * 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;
         }
     };
 
     /**
      * 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; }
         },
         /**
          * 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; }
         };
         
         /*====================================================================*/
 
         /**
          * Whether the nagivation is terminated.
          */
         public boolean terminated;
         /**
          * Whether the navigation has failed.
          */
         public boolean failure;
        
         /*====================================================================*/
 
         /**
          * Constructor: Not finished, not failed
          */
         private Stage ()
         {
             this.terminated = false;
             this.failure = false;
         }
 
         /**
          * Constructor: terminating.
          * @param type Type of terminating navigation stage.
          */
         private Stage (TerminatingStageType type)
         {
             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 ();
 
         /*====================================================================*/
         
     }
 
     /*========================================================================*/
 
     /**
      * 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 MartinNavigator (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 MartinRunner(bot, memory, body, log);
     }
 
     /**
      * Constructor.
      * @param main Agent's main.
      * @param memory Loque memory.
      */
     public MartinNavigator (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");
     }    
     
 }