/*
    * Copyright (C) 2014 AMIS research group, Faculty of Mathematics and Physics, Charles University in Prague, Czech Republic
    *
    * This program is free software: you can redistribute it and/or modify
    * it under the terms of the GNU General Public License as published by
    * the Free Software Foundation, either version 3 of the License, or
    * (at your option) any later version.
    *
    * This program is distributed in the hope that it will be useful,
   * but WITHOUT ANY WARRANTY; without even the implied warranty of
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   * GNU General Public License for more details.
   *
   * You should have received a copy of the GNU General Public License
   * along with this program.  If not, see <http://www.gnu.org/licenses/>.
   */
  package cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.pathfollowing;
  
  import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
  import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObject;
  import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
  import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
  import cz.cuni.amis.pogamut.unreal.communication.messages.UnrealId;
  import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.AgentInfo;
  import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004PathRunner;
  import cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.NavMesh;
  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.gbcommands.Move;
  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointNeighbourLink;
  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Player;
  import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.WallCollision;
  import cz.cuni.amis.pogamut.ut2004.utils.LinkFlag;
  import cz.cuni.amis.pogamut.ut2004.utils.UnrealUtils;
  import cz.cuni.amis.utils.NullCheck;
  import java.util.logging.Level;
  import java.util.logging.Logger;
  
  /**
   * Runner for navigation with navigation mesh. Evolved from {@link KefikRunner}.
   *
   * We assume that we work with path generated by NavMesh path planner. It should
   * contain point on the navigation mesh, which should be navigable by simple
   * running, and off-mesh connections, which we will handle as original runner.
   *
   * Jump computing is working with exact locations, no reserves. Needed edge
   * should be achieved by delay in executing the jump.
   *
   * @author Bogo
   */
  public class NavMeshRunner implements IUT2004PathRunner {
  
      private UT2004Bot bot;
      private AgentInfo memory;
      private AdvancedLocomotion body;
      private Logger log;
  
      private JumpBoundaries jumpBoundaries;
  
      /**
       * Our custom listener for WallCollision messages.
       */
      IWorldEventListener<WallCollision> myCollisionsListener = new IWorldEventListener<WallCollision>() {
          @Override
          public void notify(WallCollision event) {
              lastCollidingEvent = event;
          }
  
      };
  
      private JumpModule jumpModule;
  
      private CollisionDetector collisionDetector;
      private boolean inCollision = false;
      private String collisionReason = null;
  
      // MAINTAINED CONTEXT
      /**
       * Number of steps we have taken.
       */
      private int runnerStep = 0;
  
      /**
       * Jumping sequence of a single-jumps.
       */
      private int jumpStep = 0;
  
      /**
       * Collision counter.
       */
      private int collisionNum = 0;
  
      /**
       * Collision location.
       */
      private Location collisionSpot = null;
  
      // COMPUTED CONTEXT OF THE runToLocation
      /**
      * Current distance to the target, recalculated every
      * {@link NavMeshRunner#runToLocation(Location, Location, ILocated, NavPointNeighbourLink, boolean)}
      * invocation.
      */
     private double distance;
 
     /**
      * Current 2D distance (only in x,y) to the target, recalculated every
      * {@link NavMeshRunner#runToLocation(Location, Location, ILocated, NavPointNeighbourLink, boolean)}
      * invocation.
      */
     private double distance2D;
 
     /**
      * Current Z distance to the target (positive => target is higher than us,
      * negative => target is lower than us), recalculated every
      * {@link NavMeshRunner#runToLocation(Location, Location, ILocated, NavPointNeighbourLink, boolean)}
      * invocation.
      */
     private double distanceZ;
 
     /**
      * Current velocity of the bot, recalculated every
      * {@link NavMeshRunner#runToLocation(Location, Location, ILocated, NavPointNeighbourLink, boolean)}
      * invocation.
      */
     private double velocity;
 
     /**
      * Current velocity in Z-coord (positive, we're going up / negative, we're
      * going down), recalculated every
      * {@link NavMeshRunner#runToLocation(Location, Location, ILocated, NavPointNeighbourLink, boolean)}
      * invocation.
      */
     private double velocityZ;
 
     /**
      * Whether the jump is required somewhere along the link, recalculated every
      * {@link NavMeshRunner#runToLocation(Location, Location, ILocated, NavPointNeighbourLink, boolean)}
      * invocation.
      */
     private boolean jumpRequired;
     
     /**
      * Why are we jumping? 
      */
     private String jumpReason;
 
     // CONTEXT PASSED INTO runToLocation
     /**
      * Current context of the
      * {@link NavMeshRunner#runToLocation(Location, Location, Location, ILocated, NavPointNeighbourLink, boolean)}.
      */
     private Location runningFrom;
 
     /**
      * Current context of the
      * {@link NavMeshRunner#runToLocation(Location, Location, Location, ILocated, NavPointNeighbourLink, boolean)}.
      */
     private Location firstLocation;
 
     /**
      * Current context of the
      * {@link NavMeshRunner#runToLocation(Location, Location, Location, ILocated, NavPointNeighbourLink, boolean)}.
      */
     private Location secondLocation;
 
     /**
      * Current context of the
      * {@link NavMeshRunner#runToLocation(Location, Location, Location, ILocated, NavPointNeighbourLink, boolean)}.
      */
     private ILocated focus;
 
     /**
      * Current context of the
      * {@link NavMeshRunner#runToLocation(Location, Location, Location, ILocated, NavPointNeighbourLink, boolean)}.
      */
     private NavPointNeighbourLink link;
     
     /**
      * Whether we are ordered NOT TO JUMP AT ALL... NEVER EVER.
      */
     private boolean forceNoJump;
 
     /**
      * Current angle of the bot movement to the ideal direction
      */
     private double runDeviation2DDot;
     
     /**
      * Ideal travalling angle (pitch), negative -> we're going down, positive -> we're going up. In radians.
      */
     private double runAngle;
 
     /**
      * If the bot is accelerating
      */
     private boolean accelerating = false;
 
     /**
      * Last received wall colliding event
      */
     protected WallCollision lastCollidingEvent = null;
     /**
      * If we have collided in last 0.5s we will signal it
      */
     private static final int WALL_COLLISION_THRESHOLD_MILLIS = 500;
     
     /**
      * Constructor.
      *
      * @param bot Agent's bot.
      * @param agentInfo
      * @param locomotion
      * @param log
      * @param navMesh
      */
     public NavMeshRunner(UT2004Bot bot, AgentInfo agentInfo, AdvancedLocomotion locomotion, Logger log, NavMesh navMesh) {
         // setup reference to agent
         NullCheck.check(bot, "bot");
         this.bot = bot;
         NullCheck.check(agentInfo, "agentInfo");
         this.memory = agentInfo;
         NullCheck.check(locomotion, "locomotion");
         this.body = locomotion;
 
         //registering listener for wall collisions
         bot.getWorldView().addEventListener(WallCollision.class, myCollisionsListener);
 
         this.log = log;
         if (this.log == null) {
             this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
         }
 
         this.jumpModule = new JumpModule(navMesh, this.log);
         this.collisionDetector = new CollisionDetector();
         
         reset();
     }
 
     @Override
     public void reset() {
     	log.finer("RUNNER RESET");
 
         runnerStep = 0;
         
         jumpRequired = false;
         jumpBoundaries = null;
         jumpReason = null;
         jumpStep = 0;
         forceNoJump = false;
         
         collisionNum = 0;
         collisionSpot = null;
         lastCollidingEvent = null;
         inCollision = false;
         collisionReason = null;
 
         distance = 0;
         distance2D = 0;
         distanceZ = 0;
         velocity = 0;
         velocityZ = 0;
         runDeviation2DDot = 0;
         accelerating = false;
         lastVelocityZ = 0;
     }
 
     @Override
     public boolean runToLocation(Location runningFrom, Location firstLocation, Location secondLocation, ILocated focus, NavPointNeighbourLink navPointsLink, boolean reachable, boolean forceNoJump) {
     	// take another step
         ++runnerStep;
 
         // save context
         this.runningFrom = runningFrom;
         this.firstLocation = firstLocation;
         this.secondLocation = secondLocation;
         this.focus = focus;
         this.link = navPointsLink;
         this.forceNoJump = forceNoJump;
 
         // compute additional context
         distance = memory.getLocation().getDistance(firstLocation);
         distance2D = memory.getLocation().getDistance2D(firstLocation);
         distanceZ = firstLocation.getDistanceZ(memory.getLocation());
 
         double newVelocity = memory.getVelocity().size();
         accelerating = isAccelerating(newVelocity, velocity);
 
         velocity = newVelocity;
         velocityZ = memory.getVelocity().z;
         
         Location direction = Location.sub(firstLocation, memory.getLocation()).setZ(0);
         direction = direction.getNormalized();
         Location velocityDir = new Location(memory.getVelocity().asVector3d()).setZ(0);
         velocityDir = velocityDir.getNormalized();
         runDeviation2DDot = direction.dot(velocityDir);
         
         runAngle = Math.atan2(distanceZ, distance2D);
         
         if (jumpBoundaries == null || jumpBoundaries.getLink() != link) {
             jumpBoundaries = jumpModule.computeJumpBoundaries(link);            
         }
         
         checkJump();
         
         checkCollision();
         
         logIteration();
         
         // DELIBERATION
         
         // IS THIS FIRST EXECUTION?
         if (runnerStep <= 1) {
             debug("FIRST STEP - Start running towards new location");
             move(firstLocation, secondLocation, focus);
             return true;
         }
         
         // ARE WE JUMPING ALREADY?
         if (jumpStep > 0) {
             debug("We're already jumping" + (collisionSpot != null ? " [COLLISION]" : ""));
             return iterateJumpSequence();
         }
 
         // ARE WE COLLIDING?
         if (inCollision) {
         	// WE SHOULD RESOLVE THE COLLISION WITH A JUMP
         	if (!forceNoJump) {
         		// AND WE ARE NOT FORBIDDEN TO DO SO 
         		return resolveCollision();
         	}
         } else {
 	        if (collisionSpot != null || collisionNum != 0) {
 	            debug("Collision waned...");
 	            collisionNum = 0;
 	            collisionSpot = null;
 	        }
         }
 
         // ARE WE STUCK FROM THE BEGINNING?
         if (velocity < 5 && runnerStep > 5) {
         	debug("Velocity is (almost) zero and we're in the middle of running...");
         	if (forceNoJump) {
         		debug("But we will not jump as forceNoJump == true ...");
         	} else {
         		debug("So we're going to jump...");
                 return initJump(true, true);
         	}
         }
         
         // DO WE HAVE TO JUMP?
         if (jumpRequired) {
         	// YES, HAVE TO JUMP
         	if (!forceNoJump) {
         		// AND IT IS NOT FORBIDDEN TO DO SO
         		return decideJump();
         	}
         }
        
         debug("Keeping running to the target");
         move(firstLocation, secondLocation, focus);
         
         return true;
     }
     
     /**
      * Checks whether we need to jump in order to reach the next location.
      * Sets up {@link #jumpRequired} and {@link #jumpReason}.
      * @return
      */
     private boolean checkJump() {
     	if (jumpStep > 0) return true;
     	// WE HAVE NOT JUMPED YET ... should we?
     	if (jumpModule.needsJump(link)) {
     		// NAVIGATION LINK IS INDICATING JUMP
     		jumpRequired = true;
     		jumpReason = "LINK[";
     		boolean first = true;
     		if ((link.getFlags() & LinkFlag.JUMP.get()) != 0) {
     			first = false;
     			jumpReason += "JUMP-FLAG";
     		}
     		if (link.isForceDoubleJump()) {
     			if (!first) jumpReason += "|";
     			jumpReason += "DOUBLE-JUMP";
     		}
     		if (link.getNeededJump() != null) {
     			if (!first) jumpReason += "|";
     			int jumpDistance = (int) memory.getLocation().getDistance(new Location(link.getNeededJump()));
     			jumpReason += "DIST:" + jumpDistance;
     		}
     		jumpReason += "]";
     	} else {
     		jumpRequired = false;
     		jumpReason = null;
     	}
     	return jumpRequired;
     }
 
     /**
      * Checks whether we are colliding.
      * Sets up {@link #inCollision} and {@link #collisionReason}.
      * @return
      */
     private boolean checkCollision() {
     	// INTERNAL COLLISION DETECTOR
     	if (collisionDetector.isColliding(memory.getLocation(), velocity, distance)) {
     		inCollision = true;
     		collisionReason = "COLLISION DETECTED";
     		return true;    		
     	}
     	
     	// COLLISION EVENT
         if (lastCollidingEvent == null) {
         	inCollision = false;
         	collisionReason = null;
             return false;
         }        
         
         int collisionTimeMillis = (int)(memory.getTime() * 1000 - lastCollidingEvent.getSimTime());
         if (collisionTimeMillis <= WALL_COLLISION_THRESHOLD_MILLIS) {
         	inCollision = true;
         	collisionReason = "WALL-EVENT[before " + collisionTimeMillis + "ms]";
             return true;
         }
         
         inCollision = false;
     	collisionReason = null;
         return false;
     }
 
     private void logIteration() {
 		Location curr = memory.getLocation();
 		double d1 = firstLocation == null ? -1 : curr.getDistance(firstLocation);
 		double d2 = firstLocation != null && secondLocation == null ? 0 : firstLocation.getDistance(secondLocation);
 		
 		// DEBUG LOG
         if (log != null && log.isLoggable(Level.FINER)) {
             debug("RUNNER STEP " + runnerStep);
             debug("running     " + memory.getLocation() + " --(D3D:" + ((int)d1) + "|D2D:" + ((int)distance2D) + "|DZ:" + ((int)distanceZ) + ")--> " + firstLocation + (secondLocation == null ? "" : " --(D3D:" + ((int)d2) + ")--> " + secondLocation) + (focus == null ? "" : ", focusing to " + focus));
             debug("velocity    " + ((int)velocity) + " (z:" + ((int)velocityZ) + ")");
             debug("deviation2D " + ((int)(Math.acos(runDeviation2DDot) * 180 / Math.PI)));    
             debug("runAngle    " + ((int)(runAngle / Math.PI * 180)) + " degrees");
             if (jumpRequired) {
                 debug("jump        " + jumpReason);
             }
             if (inCollision) {
                 debug("collision   " + collisionReason);                
             }
             if (jumpBoundaries.getLink() != null) {
             	debug("jump-bounds " + (jumpBoundaries.isJumpable() ? "POSSIBLE" : "IMPOSSIBLE") + " X-" + jumpBoundaries.getTakeOffMin() + "<--(" + (jumpBoundaries.getTakeOffMin() != null && jumpBoundaries.getTakeOffMax() != null ? (int)(jumpBoundaries.getTakeOffMin().getDistance(jumpBoundaries.getTakeOffMax())) : "???") + ")-->" + jumpBoundaries.getTakeOffMax() + "-X--->" + jumpBoundaries.getLandingTarget());
             }
             if (jumpRequired || inCollision) {
             	if (forceNoJump) {
             		debug("jump forbidden [force-no-jump]");
             	}
             }
         }        
 	}
 
     private void move(ILocated firstLocation, ILocated secondLocation, ILocated focus) {
         Move move = new Move();
         if (firstLocation != null) {
             move.setFirstLocation(firstLocation.getLocation());
             if (secondLocation == null || secondLocation.equals(firstLocation)) {
                 //We want to reach end of the path, we won't extend the second location.
                 move.setSecondLocation(firstLocation.getLocation());
             } else {
             	double dist = firstLocation.getLocation().getDistance(secondLocation.getLocation());
             	if (dist < 50) {
                     //Extend the second location so the bot doesn't slow down, when it's near the original target.
             		double quantifier = 1 + (200 / dist);
                     Location extendedSecondLocation = firstLocation.getLocation().interpolate(secondLocation.getLocation(), quantifier);
                     move.setSecondLocation(extendedSecondLocation);
             	} else {
             		move.setSecondLocation(secondLocation.getLocation());
             	}
             }
         } else if (secondLocation != null) {
             //First location was not set
             move.setSecondLocation(secondLocation.getLocation());
         }
 
         if (focus != null) {
             if (focus instanceof Player) {
                 move.setFocusTarget((UnrealId) ((IWorldObject) focus).getId());
             } else {
                 move.setFocusLocation(focus.getLocation());
             }
         }
 
         debug(move.toString());
         bot.getAct().act(move);
     }
 
     private boolean resolveCollision() {
     	// Are we colliding at a new spot?
         if ( // no collision yet
              (collisionSpot == null)
              // or the last collision is far away
              || (memory.getLocation().getDistance2D(collisionSpot) > 120)
         ) {
         	// setup new collision spot info
             collisionSpot = memory.getLocation();
             collisionNum = 1;
             debug("COLLISION[" + collisionNum + "] => just moving...");
             // Meanwhile: keep running to the location..
             move(firstLocation, secondLocation, focus);
             return true;
         } 
         // So, we were already colliding here before...
         // Try to solve the problem according to how long we're here...
         
         // Wait a while if the collision hasn't resolved by itself.
         if (collisionNum > 8) {
             debug("COLLISION[" + collisionNum + "] => JUMPING");
             return initJump(true, true);
         } 
 
         // Increase collision counter
         ++collisionNum;
         debug("COLLISION[" + collisionNum + "] => just moving...");
         
         // Meanwhile: keep running to the location..
         move(firstLocation, secondLocation, focus);
         return true;
     }
     
     private boolean decideJump() {
         debug("decideJump(): called");
 
         int jumpDistance2D = (int) distance2D;
         debug("  ", "jumpDistance2D = " + jumpDistance2D);
 
         // follow the deliberation about the situation we're currently in
         boolean jumpIndicated      = false;       // whether we should jump
         boolean jumpForced         = inCollision; // whether we should jump RIGHT NOW
         boolean doubleJumpRequired = false;       // whether we have to use double jump
 
         // deliberation, whether we should jump
         if (jumpModule.needsJump(link)) {
             debug("  ", "JumpModule is indicating jump");
             jumpIndicated = true;
         } else {
         	debug("  ", "JumpModule is silent");
         }
 
         //We need near perfect conditions for the jump, so we evaluate       
         if (!jumpBoundaries.isJumpable()) {        	
         	if ((runnerStep > 1 && velocity > UnrealUtils.MAX_VELOCITY - 50 && runDeviation2DDot < 20 && jumpDistance2D < 500) || jumpDistance2D < 250) {
         		debug("  ", "jump-bounds impossible and we're heuristically in good position for jump => forcing double jump");
         		jumpForced = true;
         		doubleJumpRequired = true;
         	} else {
         		debug("  ", "jump-bounds indicating impossible jump => requiring double jump");
         		doubleJumpRequired = true;
         	}        	
         }
         
         if (!doubleJumpRequired && runnerStep > 1 && runAngle < -Math.PI/6) {
     		debug("  ", "we have to JUMP DOWN - indicating jump");
     		jumpIndicated = true;
     	}
 
         if (jumpIndicated || jumpForced) {
             return prepareJump(jumpForced, doubleJumpRequired);
         } else {
             debug("  ", "we do not need to jump right now, waiting to reach the right spot to jump from");
             move(firstLocation, secondLocation, focus);
             return true;
         }
     }
 
     /*========================================================================*/
     /**
      * This method is called from {@link NavMeshRunner#decideJump()} that has
      * decided that the jump is necessary to reach the the target.
      *
      * @param jumpForced ... true == we will jump RIGHT NOW; false == we will check whether the time is right for jumping
      * @param doubleJumpRequired ... true == if we decide to jump, we will double jump
      * @return whether we should reach the target
      */
     private boolean prepareJump(boolean jumpForced, boolean doubleJumpRequired) {
         debug("prepareJump(): called");
 
         double jumpVelocity = jumpModule.getCorrectedVelocity(velocity, accelerating);
         
         Double jumpAngleDeviation = Math.acos(jumpModule.getCorrectedAngle(runDeviation2DDot, runnerStep <= 1));
         boolean angleSuitable = !jumpAngleDeviation.isNaN() && jumpAngleDeviation < (Math.PI / 9);
 
         debug("  ", "jumpVelocity       " + ((int)jumpVelocity));
         debug("  ", "jumpAngleDeviation " + ((int)(jumpAngleDeviation * 180 / Math.PI)) + " degress");
         debug("  ", "angleSuitable      " + angleSuitable);
         
         if (!jumpForced && runnerStep > 1 && jumpBoundaries.isPastBoundaries(memory.getLocation())) {
         	debug("  ", "past jump-bounds => forcing the jump");
         	jumpForced = true;
         }
 
         if (!jumpForced && jumpAngleDeviation > Math.PI / 3) {
             debug("  ", "impossible jump angle, postponing the jump...");
             move(firstLocation, secondLocation, focus);
             return true;
         }
 
         if (jumpForced) {
             debug("  ", "jump is forced, bypassing jump checks");
         } else {
             debug("  ", "jump is not forced and we are within jump-bounds, checking jump conditions");
 
             if (!jumpModule.isJumpable(memory.getLocation(), jumpBoundaries.getLandingTarget(), jumpVelocity)) {
                 debug("  ", "not jumpable! Start: " + memory.getLocation() + " Target: " + jumpBoundaries.getLandingTarget() + " Velocity: " + velocity + " Jump Velocity: " + jumpVelocity);
                 debug("  ", "proceeding with the straight movement to gain speed");
                 move(firstLocation, secondLocation, focus);
                 return true;
             }
 
             if (!angleSuitable) {
                 debug("  ", "angle is not suitable for jumping (angle > 20 degrees)");
                 debug("  ", "proceeding with the straight movement to gain speed");
                 move(firstLocation, secondLocation, focus);
                 return true;
             }
 
             //Waiting for ideal JUMP conditions
             if (jumpBoundaries.isJumpable()) {
             	if (jumpBoundaries.getTakeOffMax().getDistance2D(memory.getLocation()) > IDEAL_JUMP_RESERVE) {
             		boolean angleIdeal = !jumpAngleDeviation.isNaN() && jumpAngleDeviation < (Math.PI / 90);
                     if (!angleIdeal) {
                         debug("  ", "proceeding with the straight movement - waiting for IDEAL JUMP ANGLE");
                         move(firstLocation, secondLocation, focus);
                         return true;
                     }
                     if (velocity < UnrealUtils.MAX_VELOCITY - 50) {
                         debug("  ", "proceeding with the straight movement - waiting for IDEAL SPEED");
                         move(firstLocation, secondLocation, focus);
                         return true;
                     }
             	}
             }
                 
             debug("  ", "passed ideal reserve spot, forcing the jump");
             jumpForced = true;
         }
 
         return initJump(jumpForced, doubleJumpRequired);
 
     }
     
     private static final int IDEAL_JUMP_RESERVE = 80;
 
     /**
      * We have to jump RIGHT NOW.
      * <p>
      * <p>
      * Decides whether we need single / double jump and computes the best args
      * for jump command according to current velocity.
      *
      * @param jumpForced
      * @param maxJumpRequired
      */
     private boolean initJump(boolean jumpForced, boolean maxJumpRequired) {
         debug("initJump(): called");
         
         Boolean doubleJump = null;
         Double jumpForce = Double.NaN;
         Double jumpVelocity = jumpModule.getCorrectedVelocity(velocity, accelerating);
         Double jumpAngleCos = jumpModule.getCorrectedAngle(runDeviation2DDot, runnerStep <= 1);
         
         if (jumpForced) {
         	// WE'RE GOING TO JUMP
         	debug("  ", "Jump is forced!");
         	
         	// COLLISION AVOIDANCE?
         	if (inCollision) {
             	debug("  ", "In collision, forcing MAX DOUBLE jump!");
             	inCollision = false;
                 jumpStep = 1; // we have performed the JUMP            
                 return jump(true, UnrealUtils.FULL_DOUBLEJUMP_DELAY, UnrealUtils.FULL_DOUBLEJUMP_FORCE);
             }
 
         	// MAX JUMP?
         	if (maxJumpRequired) {
 	        	debug("  ", "Forced MAX DOUBLE jump!");
 	        	jumpStep = 1; // we have performed the JUMP
 	            return jump(true, UnrealUtils.FULL_DOUBLEJUMP_DELAY, UnrealUtils.FULL_DOUBLEJUMP_FORCE);
         	}
         	
         	// COMPUTE IDEAL JUMP
         	if (runAngle < 0 && distanceZ < -50) {
         		debug("  ", "We are going to fall down, runAngle = " + ((int)(runAngle/Math.PI*180)) + " < 0, distaceZ = " + ((int)distanceZ) + " < -50");
         		
         		try {
         			jumpForce = Math.abs(jumpModule.computeFall(memory.getLocation(), jumpBoundaries, jumpVelocity, jumpAngleCos));
         			if (jumpForce < 110) jumpForce = 110.0d;
         		} catch (Exception e) {
         			debug("  ", "Failed to compute fall-jump force, setting 110.");
         			jumpForce = 110.0d;
         		}
         	} else {
         		debug("  ", "We have to jump normally, computing ideal jump force.");
         		try {
         			jumpForce = Math.abs(jumpModule.computeJump(memory.getLocation(), jumpBoundaries, jumpVelocity, jumpAngleCos));
         		} catch (Exception e) {
         			jumpForce = Double.NaN;
         		}
         	}
         	
         	if (jumpForce == Double.NaN) {
         		debug("  ", "Could not compute jump force (NaN) => forcing MAX DOUBLE jump!");
         		jumpForce = (double)UnrealUtils.FULL_DOUBLEJUMP_FORCE;
         	}
         	doubleJump = jumpForce > UnrealUtils.FULL_JUMP_FORCE;
         	
         	jumpStep = 1; // we have performed the JUMP
             return jump(doubleJump, UnrealUtils.FULL_DOUBLEJUMP_DELAY, jumpForce);
         }
         
         // JUMP IS NOT FORCED
         
         if (!jumpBoundaries.isJumpable()) {
             debug("  ", "Jump boundaries not present! We shouldn't be trying to JUMP!");
             jumpForce = Double.NaN;
         } else if (!jumpBoundaries.isInBoundaries(memory.getLocation())) {
 
             if (runnerStep > 1 && jumpBoundaries.isPastBoundaries(memory.getLocation())) {
                 debug("  ", "Already passed max take-off point, forcing jump!");
                 jumpForced = true;
                 jumpForce = jumpModule.computeJump(memory.getLocation(), jumpBoundaries, jumpVelocity, jumpAngleCos);
             } else {
                 debug("  ", "Not within jump boundaries! We shouldn't JUMP");
                 jumpForce = Double.NaN;
             }
         } else {
             jumpForce = jumpModule.computeJump(memory.getLocation(), jumpBoundaries, jumpVelocity, jumpAngleCos);
             if (jumpForce < UnrealUtils.FULL_JUMP_FORCE) {
                 doubleJump = false;
             }
         }
 
         if (jumpForce.isNaN()) {
             if (jumpForced) {
                 if (!accelerating) {
                     debug("  ", "Forcing jump but NOT accelerating, postpone!");
                     move(firstLocation, secondLocation, focus);
                     return true;
                 }
 
                 jumpStep = 1; // we have performed the JUMP
                 debug("  ", "Forcing jump - MAX!");
                 return jump(true, UnrealUtils.FULL_DOUBLEJUMP_DELAY, UnrealUtils.FULL_DOUBLEJUMP_FORCE);
             }
 
             //We cannot jump
             debug("  ", "Jump failed to compute, continuing with move! Computed force: " + jumpForce);
             move(firstLocation, secondLocation, focus);
             return true;
         } else if (jumpForce < 0) {
             //We don't need to jump, so we will set
             debug("  ", "We don't need to jump, continuing with move! Computed force: " + jumpForce);
             if (jumpBoundaries.isJumpable() && (this.jumpBoundaries.getTakeoffEdgeDirection() != null)) {
                 //TODO: Jump down
 
                 Location movementDirection = jumpBoundaries.getLandingTarget().sub(jumpBoundaries.getTakeOffMax()).setZ(0).getNormalized();
                 Location meshDirection = jumpBoundaries.getTakeoffEdgeDirection();
 
                 double fallAngleCos = meshDirection.setZ(0).getNormalized().dot(movementDirection);
                 double takeOffDistance = jumpBoundaries.getLandingTarget().getDistance2D(jumpBoundaries.getTakeOffMax());
                 if (Math.abs(fallAngleCos) > Math.cos(Math.PI / 2.5)) {
                     //Not direct approach, we should propably jump a little.
                     debug("  ", "Not direct approach to fall, we should jump a little. Angle: " + Math.acos(fallAngleCos) * (180 / Math.PI));
                     //TODO: Fixed here
                     jumpBoundaries.setLandingTarget(jumpBoundaries.getTakeOffMax().interpolate(jumpBoundaries.getLandingTarget(), (IDEAL_JUMP_RESERVE / takeOffDistance)).setZ(jumpBoundaries.getTakeOffMax().z));
                     return true;
                 } else {
                     debug("  ", "Fall solved by not jumping, as angle is suitable. AngleCos: " + fallAngleCos);
                     jumpStep = 1;
                     return true;
                 }
             } else {
                 debug("  ", "Fall solved by not jumping, as angle is suitable. Boundaries not jumpable.");
                 jumpStep = 1;
                 return true;
             }
         } else {
             jumpStep = 1; // we have performed the JUMP
             return jump(doubleJump, UnrealUtils.FULL_DOUBLEJUMP_DELAY, jumpForce);
         }
 
     }
 
     /*========================================================================*/
     /**
      * Perform jump right here and now with provided args.
      */
     private boolean jump(boolean doubleJump, double delay, double force) {
         if (doubleJump) {
             debug("DOUBLE-JUMPING[delay=" + delay + ", force=" + force + "]");
         } else {
             debug("JUMPING[force=" + force + "]");
         }
         body.jump(doubleJump, delay, force);
 
         return true;
     }
 
     /*========================================================================*/
     /**
      * Follows single-jump sequence steps.
      *
      * @return True, if no problem occured.
      */
     private boolean iterateJumpSequence() {
         debug("iterateJumpSequence(): called");
         // what phase of the jump sequence?
         switch (jumpStep) {
             // the first phase: wait for the jump
             case 1:
                 // did the agent started the jump already?
                 if (velocityZ > 100) {
                     debug("iterateJumpSequence(): jumping in progress (velocityZ > 100), increasing jumpStep");
                     jumpStep++;
                 }
                 // meanwhile: just wait for the jump to start
                 debug("iterateJumpSequence(): issuing move command to the target (just to be sure)");
                 move(firstLocation, secondLocation, focus);
                 return true;
 
             //  the last phase: finish the jump
             default:
                 // did the agent started to fall already
                 jumpStep++;
                 if (velocityZ <= 0.01) {
                     if (velocityZ > lastVelocityZ) {
                         debug("iterateJumpSequence(): jump has ended");
                         lastVelocityZ = 0.02;
                         jumpStep = 0;
                     } else {
                         lastVelocityZ = velocityZ;
                     }
 
                 }
                 debug("iterateJumpSequence(): continuing movement to the target");
                 move(firstLocation, secondLocation, focus);
                 return true;
         }
 
     }
 
     // =====
     // UTILS
     // =====
     
     private double lastVelocityZ = 0.02;
 
     private boolean isMaxVelocity(double newVelocity) {
         return Math.abs(newVelocity - UnrealUtils.MAX_VELOCITY) < 5;
     }
     
     private boolean isAccelerating(double newVelocity, double oldVelocity) {
         return velocity > 0 && (isMaxVelocity(newVelocity) || newVelocity > oldVelocity);
     }
     
     private void debug(String message) {
         debug("", message);
     }
     
     private void debug(String prefix, String message) {
         if (log.isLoggable(Level.FINER)) {
             log.log(Level.FINER, prefix + "  +-- {0}", message);
         }
     }
 
 }