EmohawkNavigation

package cz.cuni.amis.pogamut.emohawk.agent.module.sensomotoric;

import java.util.ArrayList;
import java.util.List;
import java.util.logging.Level;

import SteeringProperties.ObstacleAvoidanceProperties;
import SteeringProperties.PathFollowingProperties;
import SteeringProperties.PeopleAvoidanceProperties;
import SteeringProperties.StickToPathProperties;
import SteeringProperties.SteeringProperties.BehaviorType;

import cz.cuni.amis.pogamut.base.agent.navigation.IPathExecutorState;
import cz.cuni.amis.pogamut.base.agent.navigation.IPathFuture;
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.navigation.IUT2004GetBackToNavGraph;
import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004PathExecutor;
import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004RunStraight;
import cz.cuni.amis.pogamut.ut2004.agent.navigation.floydwarshall.FloydWarshallMap;
import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
import cz.cuni.amis.pogamut.ut2004.communication.messages.gbcommands.Stop;
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 {@link IUT2004PathExecutor}, {@link FloydWarshallMap}, {@link IUT2004RunStraight} and {@link IUT2004GetBackToNavGraph}
 * to handle all possible navigation cases.
 *
 * @author knight
 * @author jimmy
 */
public class EmohawkNavigation {

	/** 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 = 150;
	/** We're managed to get to location */
	public static final double AT_LOCATION = 150;

	/** Log used by this class. */
	protected LogCategory log;
	/** Steering used for navigation. */
	private Steering steering;
	/** FloydWarshallMap that is used for path planning. */
    protected FloydWarshallMap fwMap;
    /** UT2004Bot reference. */
    protected UT2004Bot bot;

    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 EmohawkNavigation(UT2004Bot bot, Steering steering, FloydWarshallMap fwMap) {
        this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
    	this.bot = bot;

    	this.steering = steering;
    	this.fwMap = fwMap;

        initListeners();
    }

    private void initListeners() {
    	bot.getWorldView().addEventListener(EndMessage.class, endMessageListener);
	}

	// ======================
    // PUBLIC INTERFACE
    // ======================

    public boolean isNavigating() {
        return navigating;
    }

    public void setFocus(ILocated located) {
        throw new UnsupportedOperationException("Not implemented yet.");
    }

    public void stopNavigation() {
        reset(true);
        bot.getAct().act(new Stop());
    }

    public void navigate(ILocated target) {
    	if (target == null) {
    		if (log != null && log.isLoggable(Level.WARNING)) log.warning("Cannot navigate to NULL target!");
    		stopNavigation();
    		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);

    	currentTarget = target;

    	startNavigate();
    }

    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);

    	currentTarget = player.getLocation();

    	currentTargetPlayer = player;

    	startNavigate();
    }

    public NavPoint getNearestNavPoint(ILocated location) {
    	if (location == null) return null;
    	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);
    }

    public List<ILocated> getCurrentPathCopy() {
        List<ILocated> result = new ArrayList();
        if (currentFuturePath != null) {
            result.addAll(currentFuturePath.get());
        }
        return result;
    }

    public List<ILocated> getCurrentPathDirect() {
        if (currentFuturePath != null) {
            return currentFuturePath.get();
        }
        return null;
    }

    public ILocated getCurrentTarget() {
    	return currentTarget;
    }

    public Player getCurrentTargetPlayer() {
    	if (currentTarget instanceof Player) return (Player) currentTarget;
    	return null;
    }

    public NavPoint getCurrentTargetNavPoint() {
    	if (currentTarget instanceof NavPoint) return (NavPoint) currentTarget;
    	return null;
    }


    public ILocated getLastTarget() {
    	return lastTarget;
    }

    public NavPoint getLastTargetPlayer() {
    	if (lastTarget instanceof Player) return (NavPoint)lastTarget;
    	return null;
    }

    public NavPoint getLastTargetNavPoint() {
    	if (lastTarget instanceof NavPoint) return (NavPoint)lastTarget;
    	return null;
    }

    // ======================
    // 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 startNavigate() {
		if (!navigating) {
			if (steering.isNavigating()) steering.stopNavigation();
			steering.clearAllSteerings();
		}
		navigating = true;
		navigate();
	}

    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 (steering.isNavigating()) {
    		if (log != null && log.isLoggable(Level.FINE)) log.fine("Steering running");

    		if (atLocation(currentTarget)) {
    			reset(true);
    		}

    		return;
    	}

    	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);

    	if (currentFuturePath == null || currentFuturePath.get() == null || currentFuturePath.get().size() == 0) {
    		log.warning("NON EXISTING PATH BETWEEN: " + fromNavPoint.getId().getStringId() + " -> " + toNavPoint.getId().getStringId());
    	}

    	// FLOYD-WARSHAL HAS ALL PATHS PRECOMPUTED...
    	// => path is already ready ;)

    	// TINKER THE PATH
    	processPathFuture(currentFuturePath);
    	// LET'S START RUNNING!

    	PathFollowingProperties pathFollowingProperties = new PathFollowingProperties();
    	pathFollowingProperties.setBehaviorType(BehaviorType.ADVANCED);
    	pathFollowingProperties.setTargetLocation(currentTarget.getLocation());
    	pathFollowingProperties.setPath(currentFuturePath);
    	steering.addPathFollowingSteering(pathFollowingProperties);

    	PeopleAvoidanceProperties peopleAvoidanceProperties = new PeopleAvoidanceProperties();
    	peopleAvoidanceProperties.setBehaviorType(BehaviorType.ADVANCED);
    	peopleAvoidanceProperties.setAcceleration(true);
    	peopleAvoidanceProperties.setCircumvention(true);
    	peopleAvoidanceProperties.setDeceleration(true);
    	steering.addPeopleAvoidanceSteering(peopleAvoidanceProperties);

    	ObstacleAvoidanceProperties obstacleAvoidanceProperties = new ObstacleAvoidanceProperties();
    	obstacleAvoidanceProperties.setBehaviorType(BehaviorType.ADVANCED);
    	steering.addObstacleAvoidanceSteering(obstacleAvoidanceProperties);

//    	StickToPathProperties stickToPathProperties = new StickToPathProperties();
//    	stickToPathProperties.setPath(currentFuturePath);
//    	steering.addStickToPathSteering(stickToPathProperties);

    	steering.startNavigation();
	}

	private boolean atLocation(ILocated currentTarget) {
		return bot.getLocation().getDistance(currentTarget.getLocation()) < AT_LOCATION;
	}

	private void navigatePlayer() {
		throw new UnsupportedOperationException("Not implemented yet.");
	}

	/**
     * 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 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;

    	steering.stopNavigation();
    	steering.clearAllSteerings();
    }

}