package cz.cuni.pogamut.cup2013.bot;

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

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.base3d.worldview.object.Rotation;
import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.AgentInfo;
import cz.cuni.amis.pogamut.ut2004.agent.module.utils.TabooSet;
import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004GetBackToNavGraph;
import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004PathRunner;
import cz.cuni.amis.pogamut.ut2004.agent.navigation.UT2004RunStraight;
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.NavPoint;

/**
 * This class is meant to provide easy "get-back-to-navigation-graph-in-order-I-can-safely-navigate-through-map"
 * implementation. 
 * 
 * Automatically uses {@link UT2004TimeStuckDetector}, {@link UT2004PositionStuckDetector} and {@link UT2004DistanceStuckDetector}.
 * 
 * @author Jimmy
 */
public class MyGetBackToNavGraph implements IUT2004GetBackToNavGraph {

	public static final int CLOSE_ENOUGH = 50;

	public static final double MAX_ANGLE = UT2004RunStraight.MAX_ANGLE;
	
	protected UT2004Bot bot;
	
	protected AgentInfo info;
	
	protected IUT2004PathRunner runner;
	
	protected boolean executing;
	
	protected LogCategory log;
	
	protected int randomMoveDirection;

	protected IWorldEventListener<EndMessage> endListener = new IWorldEventListener<EndMessage>() {
		
		@Override
		public void notify(EndMessage event) {
			getBackOnNavGraph();
		}
		
	};
	
	protected List<IStuckDetector> stuckDetectors = new ArrayList<IStuckDetector>();

	protected ILocated focus;
	
	public MyGetBackToNavGraph(UT2004Bot bot, AgentInfo info, AdvancedLocomotion move, LogCategory log) {
		this.log = log;
		this.bot = bot;
		this.info = info;		
		this.runner = new MyPathRunner(bot, info, move, log);
		
		stuckDetectors.add(new UT2004TimeStuckDetector(bot, 3000, 10000));
		stuckDetectors.add(new UT2004PositionStuckDetector(bot));
		stuckDetectors.add(new UT2004DistanceStuckDetector(bot));
		
		bot.getWorldView().addEventListener(EndMessage.class, endListener);
	}
	
	@Override
	public void addStuckDetector(IStuckDetector stuckDetector) {
		stuckDetectors.add(stuckDetector);
	}
	
	@Override
	public void removeStuckDetector(IStuckDetector stuckDetector) {
		stuckDetectors.remove(stuckDetector);
	}
	
	@Override
	public void clearStuckDetectors() {
		stuckDetectors.clear();
	}
		
	@Override
	public NavPoint getNearestNavPoint() {
		return info.getNearestNavPoint();
	}
	
	@Override
	public boolean isOnNavGraph() {
		return info.isOnNavGraph();
	}
	
	@Override
	public boolean isExecuting() {
		return executing;
	}
	
	@Override
	public void setFocus(ILocated located) {
		this.focus = located;
	}
	
	@Override
	public void backToNavGraph() {
		if (executing) return;
		
		if (log != null && log.isLoggable(Level.INFO)) log.info("STARTED");
		
		reset();
		
		initialLocation = info.getLocation();
		
		for (IStuckDetector stuckDetector : stuckDetectors) {			
			stuckDetector.reset();
			stuckDetector.setEnabled(true);
		}
			
		executing = true;
	}
	
	@Override
	public void stop() {
		if (!executing) return;
		if (log != null && log.isLoggable(Level.INFO)) log.info("STOPPED");
				
		executing = false;
		reset();
		
		for (IStuckDetector stuckDetector : stuckDetectors) {
			stuckDetector.setEnabled(false);
		}
	}
	
	//
	// VARIABLES
	//
	
	protected TabooSet<NavPoint> tried;

	protected NavPoint tryingNav;
	
	protected Location initialLocation;
	
	//
	// RESET
	// 
	
	protected void reset() {
		if (log != null && log.isLoggable(Level.FINER)) log.finer("Reset");
		
		if (tried == null) {
			tried = new TabooSet<NavPoint>(bot);
		} else {
			tried.clear();
		}
		
		tryingNav = null;		
	}
	
	//
	// EXECUTION
	//

	protected void getBackOnNavGraph() {
		if (!executing) return;
	
		if (isOnNavGraph()) {
			// WE'VE MANAGED TO GET BACK ON NAVIGATION GRAPH
			if (log != null && log.isLoggable(Level.INFO)) log.info("Got back to Navigation Graph.");
			stop();
			return;
		}
		
		while (true) {
			if (runToNavPoint()) {
				// WE'RE ON THE WAY...
				return;
			}
			// RUNNING FAILED
			// => select new navpoint
			while (tryingNav == null) {
				tryingNav = DistanceUtils.getNearest(tried.filter(bot.getWorldView().getAll(NavPoint.class).values()), info.getLocation());
			
				// CHECK SUITABILITY
				if (tryingNav == null || info.getLocation().getDistance(tryingNav.getLocation()) > 2000) {
					// FAILURE, NO MORE SUITABLE NAV POINTS TO TRY
					// => clear taboo set and start retrying...
					if (tried.size() == 0) {
						// TOTAL FAILURE!!! No suitable navpoints :-( lest try to move the bot a bit.
						runSomewhere();
						//stop();
						return;
					}
					
					tried.clear();
					tryingNav = null;	
					
					continue;
				}
			}
			
			if (log != null && log.isLoggable(Level.FINE)) log.fine("Trying to get to: " + tryingNav);
			
			// NAV POINT CHOSEN
			initialLocation = info.getLocation();
			for (IStuckDetector stuckDetector : stuckDetectors) {
				stuckDetector.reset();
				stuckDetector.setBotTarget(tryingNav);
			}
			runner.reset();
			
			// CONTINUE WITH NEXT CYCLE THAT WILL TRY TO RUN TO tryingNav
		}
	}
	
	/**
	 * Last resort - keep trying random points 200 UT units from bot location - 90 degrees left, right and backwards. :-)
	 */
	protected void runSomewhere() {
		randomMoveDirection++;
		if (randomMoveDirection >= 2)
			randomMoveDirection = -1;
					
		Location backwardsLoc = bot.getLocation();
		
		Rotation rot = bot.getRotation();
		rot.setYaw(rot.getYaw() + randomMoveDirection * 16000);
		backwardsLoc = backwardsLoc.sub(rot.toLocation().getNormalized().scale(200));					
		
		double hDistance = bot.getLocation().getDistance2D(backwardsLoc);
		double vDistance = bot.getLocation().getDistanceZ(backwardsLoc);
		
		double angle = Math.atan(Math.abs(vDistance) / hDistance);		
		runner.runToLocation(initialLocation, backwardsLoc, null, focus == null ? backwardsLoc : focus, null, angle < MAX_ANGLE);				
	}

	protected boolean runToNavPoint() {
		if (tryingNav == null) return false;
		// WE'RE TRYING TO RUN TO SOME NAVPOINT
		
		if (log != null && log.isLoggable(Level.FINE)) log.fine("Running to: " + tryingNav);
		
		double hDistance = bot.getLocation().getDistance2D(tryingNav.getLocation());
		double vDistance = bot.getLocation().getDistanceZ(tryingNav.getLocation());
		
		double angle = Math.atan(Math.abs(vDistance) / hDistance);
		
		if (runner.runToLocation(initialLocation, tryingNav.getLocation(), null, focus == null ? tryingNav.getLocation() : focus, null, angle < MAX_ANGLE)) {
			// RUNNING SEEMS OK
			// => check stuck detectors
			for (IStuckDetector stuckDetector : stuckDetectors) {
				if (stuckDetector.isStuck()) {
					// WE'RE STUCK!
					runFailed();
					return false;
				}
			}
			
			return true;
		} else {
			// RUNNER FAILED...
			runFailed();			
			return false;
		}
	}

	protected void runFailed() {
		// RUNNING FAILED 
		// => ban nav point
		tried.add(tryingNav);
		tryingNav = null;
	}

}
