public class NetworkConfiguration {
	
	/**
	 * The probability of not receiving a message.
	 */
	public static double PROBABILITY_OF_PACKET_LOSS = 0.0;
		
	/**
	 * The maximum transmission delay.
	 */
	protected static int maximumTxDelay = 0;
	
	/**
	 * The minimum transmission delay.
	 */
	protected static int minimumTxDelay = 0;
	
	/**
	 * Static method to set minimum and maximum network delays.
	 */
	public static void setDelays(int minimum, int maximum) {
		int max = Math.max(0, Math.max(minimum, maximum));
		int min = Math.max(0, Math.min(minimum, maximum));
		if ( max != maximum || min != minimum)
			System.out.println("* WARNING * Wrong transmission delays. Setting max: " + max + ", min: " + min +".");
		maximumTxDelay = max;
		minimumTxDelay = min;	
	}

	/**
	 * Get the maximum transmission delay.
	 * @return The maximum transmission delay.
	 */
	public static int getMaximumTxDelay() {
		return maximumTxDelay;
	}
	
	/**
	 * Get the minimum transmission delay.
	 * @return The minimum transmission delay.
	 */
	public static int getMinimumTxDelay() {
		return minimumTxDelay;
	}
	
} public class ConstantAccelerationForwardModel implements ForwardModel {
		
	private double maxAccel, maxVel;
	private double temporalResolution = -1;
	private int trackingPeriodInMillis = 0;
	private int controlPeriodInMillis = -1;
	
	public ConstantAccelerationForwardModel(double maxAccel, double maxVel, double temporalResolution, int controlPeriodInMillis, int trackingPeriodInMillis) {
		this.maxAccel = maxAccel;
		this.maxVel = maxVel;	
		this.temporalResolution = temporalResolution;
		this.controlPeriodInMillis = controlPeriodInMillis;
		this.trackingPeriodInMillis = trackingPeriodInMillis;
	}

	@Override
	public boolean canStop(TrajectoryEnvelope te, RobotReport currentState, int targetPathIndex, boolean useVelocity) {
		if (useVelocity && currentState.getVelocity() <= 0.0) return true;
		double distance = se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeTrackerRK4.computeDistance(te.getTrajectory(), (currentState.getPathIndex() != -1 ? currentState.getPathIndex() : 0), targetPathIndex);
		State state = new State(0.0, currentState.getVelocity());
		double time = 0.0;
		double deltaTime = 0.0001;
		long lookaheadInMillis = this.controlPeriodInMillis + 2*(TrajectoryEnvelopeCoordinator.MAX_TX_DELAY + trackingPeriodInMillis);
		if (lookaheadInMillis > 0) {
			while (time*this.temporalResolution < lookaheadInMillis) {
				se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeTrackerRK4.integrateRK4(state, time, deltaTime, false, maxVel, 1.0, maxAccel);
				time += deltaTime;
			}
		}
		//decelerate from maximum to stop
		while (state.getVelocity() > 0) {
			if (state.getPosition() > distance) return false;
			se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeTrackerRK4.integrateRK4(state, time, deltaTime, true, maxVel, 1.0, maxAccel);
			time += deltaTime;
		}
		return true;
	}

	private int getPathIndex(TrajectoryEnvelope te, State auxState) {
		if (auxState == null) return -1;
		Pose pose = null;
		int currentPathIndex = -1;
		double accumulatedDist = 0.0;
		Trajectory traj = te.getTrajectory();
		Pose[] poses = traj.getPose();
		for (int i = 0; i < poses.length-1; i++) {
			double deltaS = poses[i].distanceTo(poses[i+1]);
			accumulatedDist += deltaS;
			if (accumulatedDist > auxState.getPosition()) {
				double ratio = 1.0-(accumulatedDist-auxState.getPosition())/deltaS;
				pose = poses[i].interpolate(poses[i+1], ratio);
				currentPathIndex = i;
				break;
			}
		}
		if (currentPathIndex == -1) {
			currentPathIndex = poses.length-1;
			pose = poses[currentPathIndex];
		}
		return currentPathIndex;
	}
	
	@Override
	public int getEarliestStoppingPathIndex(TrajectoryEnvelope te, RobotReport currentState) {
		State state = new State(currentState.getDistanceTraveled(), currentState.getVelocity());
		double time = 0.0;
		double deltaTime = 0.0001;
		long lookaheadInMillis = this.controlPeriodInMillis + 2*(TrajectoryEnvelopeCoordinator.MAX_TX_DELAY + trackingPeriodInMillis);
		if (lookaheadInMillis > 0) {
			while (time*temporalResolution < lookaheadInMillis) {
				se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeTrackerRK4.integrateRK4(state, time, deltaTime, false, maxVel, 1.0, maxAccel*1.1);
				time += deltaTime;
			}
		}
		while (state.getVelocity() > 0) {
			se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeTrackerRK4.integrateRK4(state, time, deltaTime, true, maxVel, 1.0, maxAccel*0.9);
			time += deltaTime;
		}
		return getPathIndex(te,state);
	}

} public class CriticalSection {
	
	private TrajectoryEnvelope te1;
	private TrajectoryEnvelope te2;
	private int te1Start = -1;
	private int te2Start = -1;
	private int te1End = -1;
	private int te2End = -1;
	private int te1Break = -1;
	private int te2Break = -1;

	public CriticalSection(TrajectoryEnvelope te1, TrajectoryEnvelope te2, int te1Start, int te2Start, int te1End, int te2End) {
		this.te1 = te1;
		this.te2 = te2;
		this.te1Start = te1Start;
		this.te2Start = te2Start;
		this.te1End = te1End;
		this.te2End = te2End;
	}
	
	@Override
	public boolean equals(Object obj) {
		if (this == obj)
			return true;
		if (obj == null)
			return false;
		if (getClass() != obj.getClass())
			return false;
		CriticalSection other = (CriticalSection) obj;
		if (te1 == null || te2 == null) {
			if (other.te1 != null && other.te2 != null)
				return false;
			else if (te1 == null) 
				return te2.equals((other.te1 == null) ? other.te2 : other.te1);
		} else {
			if (!(te1.equals(other.te1) && te2.equals(other.te2) || te1.equals(other.te2) && te2.equals(other.te1)))
				return false;
			if (te1.equals(other.te1) && te2.equals(other.te2)) {
				if (te1End != other.te1End || te2End != other.te2End || te1Start != other.te1Start || te2Start != other.te2Start)
					return false;
			}
			else if (te1.equals(other.te2) && te2.equals(other.te1)) {
				if (te1End != other.te2End || te2End != other.te1End || te1Start != other.te2Start || te2Start != other.te1Start)
					return false;
			}
		}
		return true;
	}

	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		result = prime * result + ((te1 == null) ? 0 : te1.hashCode()) + ((te2 == null) ? 0 : te2.hashCode());
		result = prime * result + te1End + te2End;
		result = prime * result + Math.abs(te1End - te2End);
		result = prime * result + te1Start + te2Start;
		result = prime * result + Math.abs(te1Start - te2Start);
		return result;
	}

	public TrajectoryEnvelope getTe1() {
		return te1;
	}

	public TrajectoryEnvelope getTe2() {
		return te2;
	}

	public int getTe1Start() {
		return te1Start;
	}

	public int getTe2Start() {
		return te2Start;
	}

	public int getTe1End() {
		return te1End;
	}

	public int getTe2End() {
		return te2End;
	}

	public int getTe1Break() {
		return te1Break;
	}
	
	public int getTe2Break() {
		return te2Break;
	}
	
	public void setTe1Break(int te1Break) {
		this.te1Break = te1Break;
	}
	
	public void setTe2Break(int te2Break) {
		this.te2Break = te2Break;
	}
		
	@Override
	public String toString() {
		String ret = "";
		String robot1 = (te1 == null ? "null" : "Robot"+te1.getRobotID());
		String robot2 = (te2 == null ? "null" : "Robot"+te2.getRobotID());
		ret += "CriticalSection (" + robot1 + " [" + te1Start + ";" + te1End + "], " + robot2 + " [" + te2Start + ";" + te2End + "])";
		return ret;
	}
} public class Mission implements Comparable<Mission> {
	protected static int NUMMISSIONS = 0;
	protected int robotID;
	protected PoseSteering[] path;
	protected int order = NUMMISSIONS++;
	protected String fromLocation = null;
	protected String toLocation = null;
	protected Pose fromPose = null;
	protected Pose toPose = null;
	protected ArrayList<Pose> stoppingPoints = new ArrayList<Pose>();
	protected ArrayList<Integer> stoppingPointDurations = new ArrayList<Integer>();
	//protected HashMap<Pose,Integer> stoppingPoints = new HashMap<Pose, Integer>();
	
	/**
	 * Instantiates a {@link Mission} for a given robot to navigate between two locations, but where the path
	 * is not given and should be computed subsequently (before adding the mission to the {@link TrajectoryEnvelopeCoordinator}).
	 * @param robotID The ID of the robot.
	 * @param fromLocation The identifier of the source location.
	 * @param toLocation The identifier of the destination location.
	 * @param fromPose The pose of the source location.
	 * @param toPose The pose of the destination location.
	 */
	public Mission(int robotID, String fromLocation, String toLocation, Pose fromPose, Pose toPose) {
		this.robotID = robotID;
		this.path = null;
		this.fromLocation = fromLocation;
		this.toLocation = toLocation;
		this.fromPose = fromPose;
		this.toPose = toPose;		
	}

	/**
	 * Instantiates a {@link Mission} for a given robot to navigate between two locations via a given path.
	 * 
	 * @param robotID The ID of the robot.
	 * @param path An array of {@link PoseSteering}s representing the path to be driven.
	 */
	public Mission(int robotID, PoseSteering[] path) {
		this(robotID, path, path[0].getPose().toString(), path[path.length-1].getPose().toString(), path[0].getPose(), path[path.length-1].getPose());
	}
	
	/**
	 * Instantiates a {@link Mission} for a given robot to navigate between two locations via a given path.
	 * 
	 * @param robotID The ID of the robot.
	 * @param fromLocation The identifier of the source location.
	 * @param path An array of {@link PoseSteering}s representing the path to be driven.
	 */
	public Mission(int robotID, String fromLocation, String toLocation, PoseSteering[] path) {
		this(robotID, path, fromLocation, toLocation, path[0].getPose(), path[path.length-1].getPose());
	}

	/**
	 * Make the robot stop at the nearest location to a given pose for a given duration.
	 * @param pose A pose to stop at.
	 * @param duration Stopping time in milliseconds. 
	 */
	public void setStoppingPoint(Pose pose, int duration) {
		this.stoppingPoints.add(pose);
		this.stoppingPointDurations.add(duration);
		//this.stoppingPoints.put(pose, duration);
	}
	
	/**
	 * Clear the stopping points of this {@link Mission}.
	 */
	public void clearStoppingPoints() {
		this.stoppingPoints.clear();
		this.stoppingPointDurations.clear();
	}
	
	/**
	 * Get the stopping points along this {@link Mission}'s trajectory along with their durations.
	 * @return The stopping points along this {@link Mission}'s trajectory along with their durations.
	 */
	public HashMap<Pose,Integer> getStoppingPoints() {
		HashMap<Pose,Integer> ret = new HashMap<Pose, Integer>();
		for (int i = 0; i < this.stoppingPoints.size(); i++) {
			ret.put(this.stoppingPoints.get(i), this.stoppingPointDurations.get(i));
		}
		return ret;
		//return this.stoppingPoints;
	}
	
	/**
	 * Set the destination location for this {@link Mission}.
	 * @param location The destination of this {@link Mission}.
	 */
	public void setToLocation(String location) {
		this.toLocation = location;
	}

	/**
	 * Set the source location for this {@link Mission}.
	 * @param location The source of this {@link Mission}.
	 */
	public void setFromLocation(String location) {
		this.fromLocation = location;
	}

	/**
	 * Instantiates a {@link Mission} for a given robot to navigate between two locations via a given path.
	 * 
	 * @param robotID The ID of the robot.
	 * @param path An array of {@link PoseSteering}s representing the path to be driven.
	 * @param fromLocation The identifier of the source location.
	 * @param toLocation The identifier of the destination location.
	 * @param fromPose The pose of the source location.
	 * @param toPose The pose of the destination location.
	 */
	public Mission(int robotID, PoseSteering[] path, String fromLocation, String toLocation, Pose fromPose, Pose toPose) {
		this.robotID = robotID;
		this.path = path;
		this.fromLocation = fromLocation;
		this.toLocation = toLocation;
		this.fromPose = fromPose;
		this.toPose = toPose;
	}

	/**
	 * Instantiates a {@link Mission} for a given robot to navigate between two locations via a given path.
	 * 
	 * @param robotID The ID of the robot.
	 * @param pathFile A pointer to a file containing the path to be driven.
	 * @param fromPose The pose of the source location.
	 * @param toPose The pose of the destination location.
	 */
	public Mission(int robotID, String pathFile, Pose fromPose, Pose toPose) {
		this(robotID, pathFile, null, null, fromPose, toPose);
	}
		
	/**
	 * Instantiates a {@link Mission} for a given robot to navigate between two locations via a given path.
	 * 
	 * @param robotID The ID of the robot.
	 * @param pathFile A pointer to a file containing the path to be driven.
	 * @param fromLocation The identifier of the source location.
	 * @param toLocation The identifier of the destination location.
	 * @param fromPose The pose of the source location.
	 * @param toPose The pose of the destination location.
	 */
	public Mission(int robotID, String pathFile, String fromLocation, String toLocation, Pose fromPose, Pose toPose) {
		this.robotID = robotID;
		this.path = Missions.loadPathFromFile(pathFile);
		this.fromLocation = fromLocation;
		this.toLocation = toLocation;
		this.fromPose = fromPose;
		this.toPose = toPose;
	}
	
	@Override
	public int compareTo(Mission o) {
		return this.order-o.order;
	}
	
	/**
	 * Get the ID of the robot driving this {@link Mission}.
	 * @return The ID of the robot driving this {@link Mission}.
	 */
	public int getRobotID() {
		return this.robotID;
	}
	
	/**
	 * Get the array of {@link PoseSteering}s representing the path to be driven.
	 * @return The array of {@link PoseSteering}s representing the path to be driven.
	 */
	public PoseSteering[] getPath() {
		return this.path;
	}
	
	/**
	 * Set the path of this mission.
	 * @param path The path of this mission.
	 */
	public void setPath(PoseSteering[] path) {
		this.path = path;
	}
		
	/**
	 * Get the name of the source location of this {@link Mission}.
	 * @return The name of the source location of this {@link Mission}.
	 */
	public String getFromLocation() {
		return fromLocation;
	}
	
	/**
	 * Get the name of the destination location of this {@link Mission}.
	 * @return The name of the destination location of this {@link Mission}.
	 */
	public String getToLocation() {
		return toLocation;
	}

	@Override
	public String toString() {
		return "Robot" + this.getRobotID() + ": " + fromLocation + " --> " + toLocation + (path != null ? " (path length: " + path.length + ")" : "");
	}

	/**
	 * Get the {@link Pose} of source location of this {@link Mission}.
	 * @return The {@link Pose} of source location of this {@link Mission}.
	 */
	public Pose getFromPose() {
		return fromPose;
	}

	/**
	 * Set the {@link Pose} of source location of this {@link Mission}.
	 * @param fromPose The {@link Pose} of source location of this {@link Mission}.
	 */
	public void setFromPose(Pose fromPose) {
		this.fromPose = fromPose;
	}


	/**
	 * Get the {@link Pose} of destination location of this {@link Mission}.
	 * @return The {@link Pose} of destination location of this {@link Mission}.
	 */
	public Pose getToPose() {
		return toPose;
	}

	/**
	 * Get the {@link Pose} of destination location of this {@link Mission}.
	 * @param toPose The {@link Pose} of destination location of this {@link Mission}.
	 */
	public void setToPose(Pose toPose) {
		this.toPose = toPose;
	}
} public class RobotAtCriticalSection {
	
	private RobotReport rr;
	private CriticalSection cs;
	
	public RobotAtCriticalSection(RobotReport rr, CriticalSection cs) {
		this.rr = rr;
		this.cs = cs;
	}
	
	public RobotReport getRobotReport() {
		return this.rr;
	}
	
	public CriticalSection getCriticalSection() {
		return this.cs;
	}

} public class RobotReport {
	
	private Pose pose = null;
	private int pathIndex = -1;
	private double velocity = 0.0;
	private double distanceTraveled = 0.0;
	private int criticalPoint = -1;
	private int robotID = -1;
	
	/**
	 * Create a {@link RobotReport} with given current state of the robot.
	 * @param pose The current pose of the robot.
	 * @param pathIndex The index of the last pose passed by the robot. 
	 * @param velocity The current speed of the robot.
	 * @param distanceTraveled The distance traveled so far along the current current path.
	 * @param criticalPoint The current active critical point of the robot (-1 if no critical point).
	 */
	public RobotReport(int robotID, Pose pose, int pathIndex, double velocity, double distanceTraveled, int criticalPoint) {
		this.robotID = robotID;
		this.pose = pose;
		this.velocity = velocity;
		this.pathIndex = pathIndex;
		this.distanceTraveled = distanceTraveled;
		this.criticalPoint = criticalPoint;
	}
	
	/**
	 * Get the ID of the robot to which this report refers to.
	 * @return The ID of the robot.
	 */
	public int getRobotID() {
		return this.robotID;
	}

	/**
	 * Get the current pose of the robot.
	 * @return The current pose of the robot.
	 */
	public Pose getPose() {
		return pose;
	}

	/**
	 * Get the index of the last pose passed by the robot. 
	 * @return The index of the last pose passed by the robot.
	 */
	public int getPathIndex() {
		return pathIndex;
	}

	/**
	 * Get the current speed of the robot.
	 * @return The current speed of the robot.
	 */
	public double getVelocity() {
		return velocity;
	}

	/**
	 * Get the distance traveled so far along the current current path.
	 * @return The distance traveled so far along the current current path.
	 */
	public double getDistanceTraveled() {
		return distanceTraveled;
	}

	/**
	 * Get the current active critical point of the robot (-1 if no critical point).
	 * @return The current active critical point of the robot (-1 if no critical point).
	 */
	public int getCriticalPoint() {
		return criticalPoint;
	}

	@Override
	public String toString() {
		return "Distance: " + MetaCSPLogging.printDouble(this.distanceTraveled,4) +  "  Pose: " + this.pose + "  Index: " + this.pathIndex + "  Velocity: " + MetaCSPLogging.printDouble(velocity,4);
	}

} public abstract class TrajectoryEnvelopeCoordinator extends AbstractTrajectoryEnvelopeCoordinator {

	//@note: currentOrdersGraph and currentCyclesList should be synchronized with allCriticalSection variable.
	protected SimpleDirectedWeightedGraph<Integer,DefaultWeightedEdge> currentOrdersGraph = new SimpleDirectedWeightedGraph<Integer,DefaultWeightedEdge>(DefaultWeightedEdge.class);
	protected HashMap<Pair<Integer,Integer>, HashSet<ArrayList<Integer>>> currentCyclesList = new HashMap<Pair<Integer,Integer>, HashSet<ArrayList<Integer>>>();

	//Robots currently involved in a re-plan which critical point cannot increase beyond the one used for re-plan
	//till the re-plan has not finished yet.
	protected HashMap<Integer,Dependency> replanningStoppingPoints = new HashMap<Integer,Dependency>();

	protected boolean breakDeadlocksByReordering = true;
	protected boolean breakDeadlocksByReplanning = true;
	protected AtomicBoolean avoidDeadlockGlobally = new AtomicBoolean(false);
	protected AtomicInteger nonliveStatesDetected = new AtomicInteger(0);
	protected AtomicInteger nonliveStatesAvoided = new AtomicInteger(0);
	protected AtomicInteger currentOrdersHeurusticallyDecided = new AtomicInteger(0);
	protected List<List<Integer>> nonliveCyclesOld = new ArrayList<List<Integer>>();
	protected AtomicInteger replanningTrialsCounter = new AtomicInteger(0);
	protected AtomicInteger successfulReplanningTrialsCounter = new AtomicInteger(0);
	protected HashMap<Integer, Boolean> forceCriticalPointReTransmission = new HashMap <Integer, Boolean>();

	//True if waiting for deadlocks to happen.
	protected boolean staticReplan = false;
	protected boolean isBlocked = false;
	protected boolean isDeadlocked = false;

	protected Callback deadlockedCallback = null;

	//Set if inferring precedence constraints
	protected boolean fake = false;


	/**
	 * Get whether there is a robot in a blocked situation (waiting for a parked robot).
	 * @return <code>true</code> iff a robot is waiting for another robot that is parked.
	 */
	public boolean isBlocked() {
		return this.isBlocked;
	}


	/**
	 * Set whether the coordinator should try to break deadlocks by either global re-ordering, or by re-ordering and/or re-planning.
	 * @param global <code>true</code> if deadlocks should be broken via global re-ordering (complete, complexity: O(2^nlogn) in the worst case).
	 * @param reorder <code>true</code> if deadlocks should be broken via local re-ordering (incomplete, complexity: O(n^2) in the worst case).
	 * @param replan <code>true</code> if deadlocks should be broken via local re-planning (incomplete).
	 */
	public void setBreakDeadlocks(boolean global, boolean reorder, boolean replan) {
		if (global && (reorder||replan)) {
			metaCSPLogger.severe("Enable either the global or the local strategies for deadlock prevention!! Using default values (i.e., enabling local re-order + replan).");
			this.avoidDeadlockGlobally.getAndSet(false);
			this.breakDeadlocksByReordering = true;
			this.breakDeadlocksByReplanning = true;
			return;
		}
		this.avoidDeadlockGlobally.getAndSet(global);
		this.breakDeadlocksByReordering = reorder;
		this.breakDeadlocksByReplanning = replan;
	}


	/**Add a {@link Callback} that will be called when a deadlock is detected.
	 * @param cb A callback object.
	 */
	public void setDeadlockedCallback(Callback cb) {
		this.deadlockedCallback = cb;
	}


	/**
	 * Set whether waiting for deadlocks to happen before starting re-plan to recover from deadlock.
	 * @param value <code>true</code> if all the robots should yield at their deadlocking critical points 
	 * before starting a recovery strategy based on re-plan.
	 * Default value: false.
	 */
	public void setStaticReplan(boolean value) {
		this.staticReplan = value;
	}

	/**
	 * Enable fake coordination.
	 * @param enable <code>true</code> whether the coordinator does not impose any precedence constraints.
	 */
	public void setFakeCoordination(boolean fake) {
		this.fake = fake;
	}

	/**
	 * Create a new {@link TrajectoryEnvelopeCoordinator}, with control period 1000 msec,
	 * and temporal resolution 1000 (milliseconds).
	 */
	public TrajectoryEnvelopeCoordinator() {
		this(1000, 1000.0);
	}

	/**
	 * Create a new {@link TrajectoryEnvelopeCoordinator}, with given control period and temporal resolution.
	 * @param CONTROL_PERIOD The control period of the coordinator, e.g., 1000 msec.
	 * @param TEMPORAL_RESOLUTION The temporal resolution of the control period, e.g., 1000 for milliseconds.
	 */
	public TrajectoryEnvelopeCoordinator(int CONTROL_PERIOD, double TEMPORAL_RESOLUTION) {
		this.CONTROL_PERIOD = CONTROL_PERIOD;
		this.TEMPORAL_RESOLUTION = TEMPORAL_RESOLUTION;
		//FOR PRINTING
		if (overlay) {
			//clear screen
			System.out.printf(((char) 0x1b) + "[2J");
			//make window from row 8 to bottom (200)
			System.out.printf(((char) 0x1b) + "[8;200r");
		}
		setupLogging();
	}

	@Override
	protected String[] getStatistics() {

		String CONNECTOR_BRANCH = (char)0x251C + "" + (char)0x2500 + " ";
		String CONNECTOR_LEAF = (char)0x2514 + "" + (char)0x2500 + " ";
		ArrayList<String> ret = new ArrayList<String>();
		int numVar = solver.getConstraintNetwork().getVariables().length;
		int numCon = solver.getConstraintNetwork().getConstraints().length;

		synchronized (trackers) {
			ret.add("Status @ "  + getCurrentTimeInMillis() + " ms");
			ret.add(CONNECTOR_BRANCH + "Eff period ..... " + EFFECTIVE_CONTROL_PERIOD + " ms");
			ret.add(CONNECTOR_BRANCH + "Network ........ " + numVar + " variables, " + numCon + " constraints");
			HashSet<Integer> allRobots = new HashSet<Integer>();
			for (Integer robotID : trackers.keySet()) {
				allRobots.add(robotID);
			}
			String st = CONNECTOR_BRANCH + "Robots view .... ";
			for (Integer robotID : allRobots) {
				AbstractTrajectoryEnvelopeTracker tracker = trackers.get(robotID);
				RobotReport rr = getRobotReport(robotID); 
				if (rr != null ) {
					int currentPP = rr.getPathIndex();
					st += tracker.getTrajectoryEnvelope().getComponent();
					if (tracker instanceof TrajectoryEnvelopeTrackerDummy) st += " (P)";
					else st += " (D)";
					st += ": " + currentPP + "   ";
				}
			}
			ret.add(st);
		}
		synchronized (currentDependencies) {
			ret.add(CONNECTOR_BRANCH + "Dependencies ... " + currentDependencies);
		}
		ret.add(CONNECTOR_BRANCH + "Total number of obsolete critical sections ... " + criticalSectionCounter.get() + ".");
		ret.add(CONNECTOR_BRANCH + "Total messages sent: ... " + totalMsgsSent.get() + ", retransmitted: " + totalMsgsReTx.get() + ", number of replicas: " + numberOfReplicas + ".");
		ret.add(CONNECTOR_BRANCH + "Total nonlive states detected: ... " + nonliveStatesDetected.get() + ", avoided: " + nonliveStatesAvoided.get() + ", revised according to heuristic: " + currentOrdersHeurusticallyDecided.get() + ".");
		ret.add(CONNECTOR_LEAF + "Total re-planned path: ... " + replanningTrialsCounter.get() + ", successful: " + successfulReplanningTrialsCounter.get() + ".");
		return ret.toArray(new String[ret.size()]);
	}

	protected List<List<Integer>> findSimpleNonliveCycles(SimpleDirectedGraph<Integer,Dependency> g) {

		JohnsonSimpleCycles<Integer, Dependency> cycleFinder = new JohnsonSimpleCycles<Integer, Dependency>(g);
		List<List<Integer>> cycles = cycleFinder.findSimpleCycles();
		List<List<Integer>> nonliveCycles = new ArrayList<List<Integer>>();

		//Find all the nonlive cycles

		//For each cycle...
		//example [1,2]
		for (List<Integer> cycle : cycles) {

			//Get edges along the cycle...
			ArrayList<Dependency> edgesAlongCycle = new ArrayList<Dependency>();
			for (int i = 0; i < cycle.size(); i++) {
				Dependency dep = g.getEdge(cycle.get(i), cycle.get(i < cycle.size()-1 ? i+1 : 0));
				if (dep != null) edgesAlongCycle.add(dep);
			}
			//[1,2], [2,1]

			//Check for nonlive condition, that is:
			//  All along the cycle, check if there are (u,v,dep1) and (v,w,dep2) such that
			//    v.dep2.waitingpoint <= v.dep1.releasingpoint
			//  And if so mark cycle as deadlock
			//[1,2] ,[2,1]			[1,2], [2,3], [3,1]
			//i = 0: [1,2], [2,1]   [1,2], [2,3]
			//i = 1: [2,1], [1,2]	[2,3], [3,1]
			//i = 2:  -             [3,1], [1,2]
			for (int i = 0; i < edgesAlongCycle.size(); i++) {
				boolean safe = true;
				if (nonlivePair(edgesAlongCycle.get(i), edgesAlongCycle.get(i < edgesAlongCycle.size()-1 ? i+1 : 0))) {
					safe = false;
				}
				if (safe) {
					metaCSPLogger.finest("Cycle: " + edgesAlongCycle + " is deadlock-free");
					break;
				}
				if (i == edgesAlongCycle.size()-1) {
					metaCSPLogger.info("Cycle: " + edgesAlongCycle + " is NOT deadlock-free. Current cycle list contains this cycle? :" + currentCyclesList.containsValue(cycle));
					nonliveCycles.add(cycle);
				}
			}			
		}

		return nonliveCycles;
	}


	protected SimpleDirectedGraph<Integer,Dependency> depsToGraph(HashMap<Integer, Dependency> deps) {
		SimpleDirectedGraph<Integer,Dependency> g = new SimpleDirectedGraph<Integer,Dependency>(Dependency.class);
		for (int key : deps.keySet()) {
			Dependency dep = deps.get(key);
			g.addVertex(dep.getWaitingRobotID());
			g.addVertex(dep.getDrivingRobotID());
			if (!g.addEdge(dep.getWaitingRobotID(), dep.getDrivingRobotID(), dep))
				metaCSPLogger.severe("<<<<<<<<< Add dependency fails (13). Dep: " + dep);	
		}
		return g;
	}

	protected HashMap<Integer, Dependency> computeClosestDependencies(HashMap<Integer,HashSet<Dependency>> allDeps, HashMap<Integer, HashSet<Dependency>> artificialDeps) {

		HashMap<Integer, Dependency> closestDeps = new HashMap<Integer, Dependency>();

		if (allDeps == null && artificialDeps == null || allDeps.isEmpty() && artificialDeps.isEmpty()) {
			return closestDeps;
		}

		metaCSPLogger.finest("currentDeps: " + allDeps.toString() + ".");	
		metaCSPLogger.finest("artificialDeps: " + artificialDeps.toString() + ".");	

		//Fill the TreeSet
		HashMap<Integer, TreeSet<Dependency>> allDepsTree = new HashMap<Integer, TreeSet<Dependency>>();
		for (Integer key : allDeps.keySet()) allDepsTree.put(key, new TreeSet<Dependency>(allDeps.get(key)));
		HashMap<Integer, TreeSet<Dependency>> artificialDepsTree = new HashMap<Integer, TreeSet<Dependency>>();
		for (Integer key : artificialDeps.keySet()) artificialDepsTree.put(key, new TreeSet<Dependency>(artificialDeps.get(key)));

		HashSet<Integer> robotIDs = new HashSet<Integer>();
		robotIDs.addAll(allDeps.keySet());
		robotIDs.addAll(artificialDeps.keySet());

		for (Integer robotID : robotIDs) {						
			if (allDeps.containsKey(robotID) || artificialDeps.containsKey(robotID)) {
				Dependency firstDep = null;
				Dependency firstArtificialDep = null;
				if (allDeps.containsKey(robotID)){
					firstDep = allDepsTree.get(robotID).first();
				}
				if (artificialDeps.containsKey(robotID)) {
					firstArtificialDep = artificialDepsTree.get(robotID).first();
					metaCSPLogger.info("Artificial critical point " + firstArtificialDep.getWaitingPoint() + " for Robot" + firstArtificialDep.getWaitingRobotID() +".");
				}

				Dependency depToSend = null; 
				if (firstDep != null)
					depToSend = firstArtificialDep != null ? (firstDep.compareTo(firstArtificialDep) < 0 ? firstDep : firstArtificialDep) : firstDep;
				else depToSend = firstArtificialDep;
				closestDeps.put(depToSend.getWaitingRobotID(), depToSend);
			}
		}
		return closestDeps;
	}

	/**
	 * Check whether there is or will be a deadlock.
	 * @return <code>true</code> iff there is currently a deadlock, or the situation is
	 * such that a deadlock will occur (non-live state).
	 */
	public boolean isDeadlocked() {
		synchronized(solver) {
			SimpleDirectedGraph<Integer,Dependency> g = depsToGraph(currentDependencies);
			List<List<Integer>> nonliveCycles = findSimpleNonliveCycles(g);
			for (List<Integer> cycle : nonliveCycles) {
				//Check if all robots in the cycle are waiting at their current critical point.
				this.isDeadlocked = true;
				for (int i = 0; i < cycle.size(); i++) {
					int robotID = cycle.get(i);
					AbstractTrajectoryEnvelopeTracker tracker = trackers.get(robotID);
					RobotReport rr = tracker.getLastRobotReport();
					if (!(communicatedCPs.containsKey(tracker) && communicatedCPs.get(tracker).getFirst() == rr.getCriticalPoint() && rr.getCriticalPoint() == rr.getPathIndex())) {
						this.isDeadlocked = false;
						break;
					}
				}
				if (this.isDeadlocked) {
					if (breakDeadlocksByReplanning) metaCSPLogger.info("The deadlock may be solved via replanning.");
					break;
				}
			}
		}
		if (this.deadlockedCallback != null && this.isDeadlocked) this.deadlockedCallback.performOperation();
		return this.isDeadlocked;
	}

	protected HashMap<Integer,HashSet<Dependency>> findAndRepairNonliveCycles(HashMap<Integer,HashSet<Dependency>> currentDeps, HashMap<Integer,HashSet<Dependency>> artificialDeps, HashSet<Dependency> reversibleDeps, HashMap<Integer,RobotReport> currentReports, Set<Integer> robotIDs) {

		//Create a copy of the current set of dependencies
		HashMap<Integer,HashSet<Dependency>> allDeps = new HashMap<Integer, HashSet<Dependency>>();
		for (int robotID : currentDeps.keySet()) {
			HashSet<Dependency> set = new HashSet<Dependency>();
			set.addAll(currentDeps.get(robotID));
			allDeps.put(robotID, set);
		}

		//1. COMPUTE NONLIVE CYCLES
		//Dep graph G = (V,E)
		//  V = robots involved in current deps
		//  E = {(u,v,dep) | exists dep stating that robot u should wait for robot v}
		//Nonlive cycle iff:
		//  Cycle <r_1, r_2, ... r_1> in G
		//  Exists one selection of edges along the cycle such that
		//    Exists (u,v,dep1) and (v,w,dep2) such that  v.dep2.waitingpoint <= v.dep1.releasingpoint
		SimpleDirectedGraph<Integer,Dependency> g = depsToGraph(currentDependencies);
		List<List<Integer>> nonliveCycles = findSimpleNonliveCycles(g);

		// ... keep tracks of size and old cycles for statistics
		nonliveStatesDetected.addAndGet(nonliveCycles.size());

		//2. IF THERE ARE NONLIVE CYCLES AND RE-ORDERING IS ENABLED, TRY RE-ORDER
		if (breakDeadlocksByReordering) allDeps = callLocalReordering(nonliveCycles, artificialDeps, g, reversibleDeps, allDeps, currentReports);

		//3. OTHERWISE, IF RE-PLAN IS ENABLED, TRY REPLANNING
		if (breakDeadlocksByReplanning) {
			for (List<Integer> cycle : nonliveCycles) callOnePathReplan(cycle, g);	
		}

		return allDeps;
	}

	protected HashMap<Integer,HashSet<Dependency>> callLocalReordering(List<List<Integer>> nonliveCycles, HashMap<Integer, HashSet<Dependency>> artificialDeps, SimpleDirectedGraph<Integer,Dependency> g, HashSet<Dependency> reversibleDeps, HashMap<Integer,HashSet<Dependency>> allDeps, HashMap<Integer,RobotReport> currentReports) {
		List<List<Integer>> nonliveCyclesNew = new ArrayList<List<Integer>>();
		nonliveCyclesNew.addAll(nonliveCycles);
		nonliveCyclesNew.removeAll(nonliveCyclesOld);
		nonliveCyclesOld.clear();
		nonliveCyclesOld.addAll(nonliveCycles);

		int counter = 0;
		while (counter < nonliveCycles.size()) { //if empty no problem

			//get one cycle
			List<Integer> cycle = nonliveCycles.get(counter);
			counter++;

			//Get edges along the cycle...
			ArrayList<Dependency> reversibleDepsAlongCycle = new ArrayList<Dependency>();
			for (int i = 0; i < cycle.size(); i++) {
				Dependency dep = null;
				if (i < cycle.size()-1) {
					dep = g.getEdge(cycle.get(i), cycle.get(i+1));
				}
				else {
					dep = g.getEdge(cycle.get(i), cycle.get(0));
				}
				if (reversibleDeps.contains(dep) && !artificialDeps.containsKey(dep.getWaitingRobotID())) reversibleDepsAlongCycle.add(dep);
			}

			//For each reversible edge ... 
			//(note that if the dependency is reversible, then it is neither an artificial dependency,
			//nor one related to an active parking critical section)
			for (Dependency dep : reversibleDepsAlongCycle) {

				if (depsToCS.containsKey(dep)) {
					CriticalSection cs = depsToCS.get(dep);

					//Try reversing the precedence
					int waitingRobotID = dep.getDrivingRobotID();
					TrajectoryEnvelope waitingTE = dep.getDrivingTrajectoryEnvelope();
					int drivingRobotID = dep.getWaitingRobotID();
					TrajectoryEnvelope drivingTE = dep.getWaitingTrajectoryEnvelope();
					int drivingCurrentIndex = currentReports.get(drivingRobotID).getPathIndex();
					int waitingPoint = getCriticalPoint(waitingRobotID, cs, drivingCurrentIndex);
					int drivingCSEnd = (drivingRobotID == cs.getTe1().getRobotID()) ? cs.getTe1End() : cs.getTe2End();

					Dependency revDep = new Dependency(waitingTE, drivingTE, waitingPoint, drivingCSEnd);

					HashMap<Integer,HashSet<Dependency>> allDepsTmp = new HashMap<Integer,HashSet<Dependency>>(allDeps);

					//and add the reversed dependency to the new waiting one
					if (!allDepsTmp.containsKey(waitingRobotID)) allDepsTmp.put(waitingRobotID, new HashSet<Dependency>());
					allDepsTmp.get(revDep.getWaitingRobotID()).add(revDep);

					//remove the dependency to the old waiting robot
					allDepsTmp.get(dep.getWaitingRobotID()).remove(dep);
					if (allDepsTmp.get(dep.getWaitingRobotID()).isEmpty()) allDepsTmp.remove(dep.getWaitingRobotID());

					//update a temporary map between critical sections and orders
					HashMap<CriticalSection, Pair<Integer,Integer>> CSToDepsOrderTmp = new HashMap<CriticalSection, Pair<Integer,Integer>>(CSToDepsOrder);
					CSToDepsOrderTmp.put(cs, new Pair<Integer,Integer>(revDep.getWaitingRobotID(), revDep.getWaitingPoint()));

					//create a temporary map between dependencies and critical sections
					HashMap<Dependency,CriticalSection> depsToCSTmp = new HashMap<Dependency, CriticalSection>(depsToCS);
					depsToCSTmp.remove(dep);
					depsToCSTmp.put(revDep, cs);

					//update currentDeps
					HashMap<Integer, Dependency> currentDepsTmp = computeClosestDependencies(allDepsTmp, artificialDeps);
					SimpleDirectedGraph<Integer,Dependency> gTmp = depsToGraph(currentDepsTmp);

					//compute cycles again. If the number of cycles is lower, keep this solution
					List<List<Integer>> nonliveCyclesTmp = findSimpleNonliveCycles(gTmp);
					if (nonliveCyclesTmp.size() < nonliveCycles.size()) {
						nonliveStatesAvoided.incrementAndGet();
						metaCSPLogger.info("REVERSING a precedence constraint to break a deadlock: " + revDep.getWaitingRobotID() + " now waits for " + revDep.getDrivingRobotID());						

						//reset the counter
						counter = 0;

						//update all the variables according to the new choice
						g = gTmp;
						currentDependencies.clear();
						currentDependencies.putAll(currentDepsTmp);				
						allDeps = allDepsTmp;
						nonliveCycles = nonliveCyclesTmp;					
						CSToDepsOrder.clear();
						CSToDepsOrder.putAll(CSToDepsOrderTmp);							
						depsToCS.clear();
						depsToCS.putAll(depsToCSTmp);	

						break;
					}
					//else you should not update the solution (the cycle will reversed by re-planning if possible)
				}
			}
		}

		return allDeps;
	}

	/**
	 * Re-plan the path for a given robot.
	 * @param robotID The robot which path should be re-planned.
	 * @return <code>true</code> if re-planning is correctly spawned.
	 */
	public boolean replanEnvelope(int robotID) {
		return replanEnvelope(robotID, false);
	}

	protected boolean replanEnvelope(int robotID, boolean onlyIfDeadlocks) {
		synchronized (solver) {
			SimpleDirectedGraph<Integer,Dependency> g = depsToGraph(currentDependencies);
			List<List<Integer>> nonliveCycles = null;

			if (onlyIfDeadlocks) {
				//find the nonlive cycles and spawn one replanning thread only if the robots are in deadlocks
				nonliveCycles = findSimpleNonliveCycles(g);
				nonliveStatesDetected.addAndGet(nonliveCycles.size());

				//find one cycle containing the robot
				for (List<Integer> cycle : nonliveCycles) {
					if (cycle.contains(robotID)) return callOnePathReplan(cycle, g);
				}
			}
			List<Integer> cycle = new ArrayList<Integer>();
			cycle.add(robotID);
			return callOnePathReplan(cycle, g);
		}
	}

	protected boolean callOnePathReplan(List<Integer> cycle, SimpleDirectedGraph<Integer,Dependency> g) {

		//Get edges along the cycle...
		Set<Integer> robotsToReplan = new HashSet<Integer>();
		for (int i = 0; i < cycle.size(); i++) {
			int robotID = cycle.get(i);
			Dependency dep = g.getEdge(cycle.get(i), cycle.get(i < cycle.size()-1 ? i+1 : 0));
			if (dep == null) {
				AbstractTrajectoryEnvelopeTracker tracker = null;
				synchronized (trackers) {
					tracker = trackers.get(robotID);
				}
				if (tracker instanceof TrajectoryEnvelopeTrackerDummy) return false;
				TrajectoryEnvelope te = tracker.getTrajectoryEnvelope();
				int waitingPoint = getForwardModel(robotID).getEarliestStoppingPathIndex(te, tracker.getLastRobotReport());
				dep = new Dependency(te, null, waitingPoint, 0);
				metaCSPLogger.info("Added a stopping point to replan path of robot" + robotID + ": " + dep + ".");
				currentDependencies.put(robotID, dep);
				setCriticalPoint(robotID, waitingPoint, true);
			}
			if (dep.getDrivingTrajectoryEnvelope() != null) robotsToReplan.add(dep.getDrivingRobotID());
			robotsToReplan.add(dep.getWaitingRobotID());
			RobotReport rrWaiting = getRobotReport(dep.getWaitingRobotID());


			//avoid to re-plan if one of the robots is parked.
			//In case of static re-plan, wait for a deadlock to happen before starting it
			if (dep.getDrivingTrajectoryEnvelope() != null && inParkingPose(dep.getDrivingRobotID()) || inParkingPose(dep.getWaitingRobotID())
					|| staticReplan && ((dep.getWaitingPoint() == 0 ? (rrWaiting.getPathIndex() < 0) : (rrWaiting.getPathIndex() < dep.getWaitingPoint()-1)))) 
				return false;
		}


		//Get other robots (all the robot in the maximum connected set of a deadlocked one)

		/*------------------------------------------------------------------------------------------
		 * Notes on connectivity: example code. While strongly connected components are useful for 
		 * computing cycles, connected components are used to get the set of robots to be considered 
		 * as obstacles when replanning.
		 * -----------------------------------------------------------------------------------------
		 * 
		 * 	SimpleDirectedGraph<Integer,Integer> g = new SimpleDirectedGraph(Integer.class);
			for (int v=1; v < 6; v++) g.addVertex(v);
			g.addEdge(1,2,1);
			g.addEdge(2,3,2);
			g.addEdge(3,1,3);
			g.addEdge(4,2,4);
			g.addEdge(3,5,5);
			ConnectivityInspector<Integer,Integer> connInsp = new ConnectivityInspector<Integer,Integer>(g);
			KosarajuStrongConnectivityInspector<Integer,Integer> kconnInsp = new KosarajuStrongConnectivityInspector<Integer,Integer>(g);
			Set<Integer> cc2 = connInsp.connectedSetOf(2);
			Set<Integer> cc3 = connInsp.connectedSetOf(3);
			List<Set<Integer>> scc = kconnInsp.stronglyConnectedSets();
			System.out.println("cc2: " + cc2.toString() + ", cc3: " + cc3.toString() + ", scc: " + scc.toString());
			//Result: cc2: [1, 2, 3, 4, 5], cc3: [1, 2, 3, 4, 5], scc: [[4], [1, 2, 3], [5]]
			---------------------------------------------------------------------------------------
		 */
		ConnectivityInspector<Integer,Dependency> connInsp = new ConnectivityInspector<Integer,Dependency>(g);
		HashSet<Integer> allConnectedRobots = g.containsVertex(cycle.get(0)) ? (HashSet<Integer>) connInsp.connectedSetOf(cycle.get(0)) : new HashSet<Integer>(); //it may be empty if the robot has no dependency
		return spawnReplanning(robotsToReplan, allConnectedRobots);
	}

	//returns true if robot1 should go before robot2
	//returns false if robot2 should go before robot1
	protected boolean getOrder(AbstractTrajectoryEnvelopeTracker robotTracker1, RobotReport robotReport1, AbstractTrajectoryEnvelopeTracker robotTracker2, RobotReport robotReport2, CriticalSection cs) {

		//If both can stop before entering, use ordering function (or closest if no ordering function)
		metaCSPLogger.finest("Both robots can stop at " + cs);

		if (yieldIfParking) {
			boolean robot1ParksInCS = cs.getTe1End() == cs.getTe1().getPathLength()-1;
			boolean robot2ParksInCS = cs.getTe2End() == cs.getTe2().getPathLength()-1;
			if (robot1ParksInCS && !robot2ParksInCS) {
				metaCSPLogger.finest("Robot" + cs.getTe1().getRobotID() + " will park in " + cs + ", letting Robot" + cs.getTe2().getRobotID() + " go first");
				return false;
			}
			else if (!robot1ParksInCS && robot2ParksInCS) {
				metaCSPLogger.finest("Robot" + cs.getTe2().getRobotID() + " will park in " + cs + ", letting Robot" + cs.getTe1().getRobotID() + " go first");
				return true;
			}
			else if (robot1ParksInCS && robot2ParksInCS) 
				metaCSPLogger.finest("Both Robot" + cs.getTe1().getRobotID() + " and Robot" + cs.getTe2().getRobotID() + " will park in " + cs + ". Decide according to the heuristic.");
		}

		RobotAtCriticalSection r1atcs = new RobotAtCriticalSection(robotReport1, cs);
		RobotAtCriticalSection r2atcs = new RobotAtCriticalSection(robotReport2, cs);
		boolean ret = false;
		if (this.comparators.size() > 0) ret = (this.comparators.compare(r1atcs,r2atcs) < 0);
		//No ordering function, decide an ordering based on distance (closest goes first)
		else ret = ((cs.getTe2Start()-robotReport2.getPathIndex()) > (cs.getTe1Start()-robotReport1.getPathIndex()));
		if (ret && muted.contains(robotReport2.getRobotID())) return false;
		if (!ret && muted.contains(robotReport1.getRobotID())) return true;
		return ret;
	}

	@Override
	protected void updateDependencies() {
		synchronized(solver) {
			if (this.fake) {
				for (int robotID : trackers.keySet()) setCriticalPoint(robotID, -1, true);
				return;
			}
			if (this.avoidDeadlockGlobally.get()) globalCheckAndRevise();
			else localCheckAndRevise();
		}
	}

	protected void localCheckAndRevise() {

		//System.out.println("Caller of updateDependencies(): " + Thread.currentThread().getStackTrace()[2]);
		synchronized(solver) {
			HashMap<Integer,RobotReport> currentReports = new HashMap<Integer,RobotReport>();
			HashMap<Integer,HashSet<Dependency>> currentDeps = new HashMap<Integer,HashSet<Dependency>>();
			HashMap<Integer,HashSet<Dependency>> artificialDependencies = new HashMap<Integer,HashSet<Dependency>>(); 
			HashSet<Dependency> currentReversibleDependencies = new HashSet<Dependency>();
			HashMap<Integer,Integer> earliestStoppingPoints = new HashMap<Integer,Integer>();

			//Make deps from un-reached stopping points
			Set<Integer> robotIDs = trackers.keySet();
			for (int robotID : robotIDs) {
				AbstractTrajectoryEnvelopeTracker robotTracker = trackers.get(robotID);
				//Update the coordinator view
				RobotReport robotReport = robotTracker.getRobotReport();
				currentReports.put(robotID, robotReport);
				synchronized(stoppingPoints) {
					if (stoppingPoints.containsKey(robotID)) {
						metaCSPLogger.info("Stopping points Robot"+robotID+": "+stoppingPoints.get(robotID).toString());
						for (int i = 0; i < stoppingPoints.get(robotID).size(); i++) {
							int stoppingPoint = stoppingPoints.get(robotID).get(i);
							int duration = stoppingTimes.get(robotID).get(i);
							if (robotReport.getPathIndex() <= stoppingPoint) {
								Dependency dep = new Dependency(robotTracker.getTrajectoryEnvelope(), null, stoppingPoint, 0);
								if (!currentDeps.containsKey(robotID)) currentDeps.put(robotID, new HashSet<Dependency>());
								currentDeps.get(robotID).add(dep);
							}
							//Start waiting thread if the stopping point has been reached
							//if (Math.abs(robotReport.getPathIndex()-stoppingPoint) <= 1 && robotReport.getCriticalPoint() == stoppingPoint && !stoppingPointTimers.containsKey(robotID)) {
							if (Math.abs(robotReport.getPathIndex()-stoppingPoint) <= 1 && robotReport.getCriticalPoint() <= stoppingPoint && !stoppingPointTimers.containsKey(robotID)) {
								spawnWaitingThread(robotID, i, duration);
							}
						}
					}
				}
				synchronized(replanningStoppingPoints) {
					if (replanningStoppingPoints.containsKey(robotID)) 	{
						//We should enforce the last robot stopping point while it is involved in a re-plan.
						if (!currentDeps.containsKey(robotID)) currentDeps.put(robotID, new HashSet<Dependency>());
						currentDeps.get(robotID).add(replanningStoppingPoints.get(robotID));
					}			
				}
			}



			//Make deps from critical sections, and remove obsolete critical sections
			synchronized(allCriticalSections) {

				//FIXME Add a CriticalSectionManager class
				if (allCriticalSections.size() > 0) 
					for (int robotID : robotIDs) 
						earliestStoppingPoints.put(robotID, getForwardModel(robotID).getEarliestStoppingPathIndex(trackers.get(robotID).getTrajectoryEnvelope(), currentReports.get(robotID)));

				depsToCS.clear();			
				this.isBlocked = false;

				HashSet<CriticalSection> toRemove = new HashSet<CriticalSection>();
				for (CriticalSection cs : this.allCriticalSections) {

					//Will be assigned depending on current situation of robot reports...
					AbstractTrajectoryEnvelopeTracker waitingTracker = null;
					AbstractTrajectoryEnvelopeTracker drivingTracker = null;
					int drivingCurrentIndex = -1;

					AbstractTrajectoryEnvelopeTracker robotTracker1 = trackers.get(cs.getTe1().getRobotID());
					RobotReport robotReport1 = currentReports.get(cs.getTe1().getRobotID());
					AbstractTrajectoryEnvelopeTracker robotTracker2 = trackers.get(cs.getTe2().getRobotID());
					RobotReport robotReport2 = currentReports.get(cs.getTe2().getRobotID());

					//One or both robots past end of the critical section --> critical section is obsolete
					//This condition is still valid in case of delays (new mission phases <--> new trackers)
					if (robotReport1.getPathIndex() > cs.getTe1End() || robotReport2.getPathIndex() > cs.getTe2End()) {
						toRemove.add(cs);
						metaCSPLogger.finest("Obsolete critical section\n\t" + cs);
						continue;
					}

					//The critical section could be still active. One of the two robots could already have exited the critical section,
					//but the information has not been received.
					int waitingPoint = -1;

					if (robotTracker1 instanceof TrajectoryEnvelopeTrackerDummy || robotTracker2 instanceof TrajectoryEnvelopeTrackerDummy) {

						boolean createAParkingDep = false;
						this.isBlocked = true;

						//Robot1 is parking in critical section. If it is the driver, make robot 2 wait.
						if (robotTracker1 instanceof TrajectoryEnvelopeTrackerDummy) {
							drivingCurrentIndex = robotReport1.getPathIndex();
							drivingTracker = robotTracker1;
							waitingTracker = robotTracker2;
							waitingPoint = getCriticalPoint(robotReport2.getRobotID(), cs, drivingCurrentIndex);

							if (!communicatedCPs.containsKey(waitingTracker) && robotReport1.getPathIndex() <= waitingPoint 
									|| communicatedCPs.containsKey(waitingTracker) && communicatedCPs.get(waitingTracker).getFirst() != -1 && communicatedCPs.get(waitingTracker).getFirst() <= waitingPoint) {
								createAParkingDep = true;
							}

						}
						else if (robotTracker2 instanceof TrajectoryEnvelopeTrackerDummy) {
							drivingCurrentIndex = robotReport2.getPathIndex();
							drivingTracker = robotTracker2;
							waitingTracker = robotTracker1;
							waitingPoint = getCriticalPoint(robotReport1.getRobotID(), cs, drivingCurrentIndex);

							if (!communicatedCPs.containsKey(waitingTracker) && robotReport2.getPathIndex() <= waitingPoint
									|| communicatedCPs.containsKey(waitingTracker) && communicatedCPs.get(waitingTracker).getFirst() != -1 && communicatedCPs.get(waitingTracker).getFirst() <= waitingPoint) {
								createAParkingDep = true;						
							}
						}

						if (createAParkingDep) {
							int drivingCSEnd = drivingTracker.getTrajectoryEnvelope().getRobotID() == cs.getTe1().getRobotID() ? cs.getTe1End() : cs.getTe2End();
							metaCSPLogger.finest("Robot" + drivingTracker.getTrajectoryEnvelope().getRobotID() + " is parked, so Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " will have to wait");
							if (!currentDeps.containsKey(waitingTracker.getTrajectoryEnvelope().getRobotID())) currentDeps.put(waitingTracker.getTrajectoryEnvelope().getRobotID(), new HashSet<Dependency>());
							Dependency dep = new Dependency(waitingTracker.getTrajectoryEnvelope(), drivingTracker.getTrajectoryEnvelope(), waitingPoint, drivingCSEnd);
							currentDeps.get(waitingTracker.getTrajectoryEnvelope().getRobotID()).add(dep);
							CSToDepsOrder.put(cs, new Pair<Integer,Integer>(dep.getWaitingRobotID(),dep.getWaitingPoint()));
							depsToCS.put(dep, cs);
						}
					}
					else {//Both robots are driving, let's determine an ordering for them through this critical section


						//Check if the robots can stop while changing the current order of accessing the critical section.
						boolean canStopRobot1 = false;
						boolean canStopRobot2 = false;
						boolean wakeUpinCSRobot1 = false;
						boolean wakeUpinCSRobot2 = false;


						//Force the dependency for the robot footprint
						if (//the last critical point was before the critical section (can stop by induction)
								(communicatedCPs.containsKey(robotTracker1) && communicatedCPs.get(robotTracker1).getFirst() != -1 && communicatedCPs.get(robotTracker1).getFirst() < cs.getTe1Start())
								|| !communicatedCPs.containsKey(robotTracker1) && Math.max(0, robotReport1.getPathIndex()) < cs.getTe1Start())
							canStopRobot1 = true;
						else
							//Due to temporal delays we cannot trust the velocity.
							canStopRobot1 =  earliestStoppingPoints.get(robotReport1.getRobotID()) < cs.getTe1Start();

						if ((communicatedCPs.containsKey(robotTracker2) && communicatedCPs.get(robotTracker2).getFirst() != -1 && communicatedCPs.get(robotTracker2).getFirst() < cs.getTe2Start())
								|| !communicatedCPs.containsKey(robotTracker2) && Math.max(0, robotReport2.getPathIndex()) < cs.getTe2Start())
							canStopRobot2 = true;
						else canStopRobot2 = earliestStoppingPoints.get(robotReport2.getRobotID()) < cs.getTe2Start();


						//Both the robots can stop before accessing the critical section --> follow ordering heuristic if FW model allows it
						if (canStopRobot1 && canStopRobot2) {

							//If robot 1 has priority over robot 2
							if (getOrder(robotTracker1, robotReport1, robotTracker2, robotReport2, cs)) {
								drivingCurrentIndex = robotReport1.getPathIndex();
								drivingTracker = robotTracker1;
								waitingTracker = robotTracker2;
								metaCSPLogger.finest("Both-can-stop (1) and Robot" + drivingTracker.getTrajectoryEnvelope().getRobotID() + " ahead of Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " and CS is: " + cs);
							}

							//If robot 2 has priority over robot 1
							else {
								drivingCurrentIndex = robotReport2.getPathIndex();
								drivingTracker = robotTracker2;
								waitingTracker = robotTracker1;
								metaCSPLogger.finest("Both-can-stop (2) and Robot" + drivingTracker.getTrajectoryEnvelope().getRobotID() + " ahead of Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " and CS is: " + cs);
							}	
						}

						//Robot 1 can stop before entering the critical section, robot 2 can't --> robot 1 waits
						else if (canStopRobot1 && !canStopRobot2) {
							drivingCurrentIndex = robotReport2.getPathIndex();
							drivingTracker = robotTracker2;
							waitingTracker = robotTracker1;
							metaCSPLogger.finest("One-can-one-can't-stop (1) and Robot" + drivingTracker.getTrajectoryEnvelope().getRobotID() + " (can't) is ahead of Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " (can) and CS is: " + cs);						
						}

						//Robot 2 can stop before entering critical section, robot 1 can't --> robot 2 waits
						else if (!canStopRobot1 && canStopRobot2) {
							drivingCurrentIndex = robotReport1.getPathIndex();
							drivingTracker = robotTracker1;
							waitingTracker = robotTracker2;
							metaCSPLogger.finest("One-can-one-can't-stop (2) and Robot" + drivingTracker.getTrajectoryEnvelope().getRobotID() + " (can't) ahead of Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " (can) and CS is: " + cs);
						}

						else {			

							//Both robots in critical section --> re-impose previously decided dependency if possible
							int drivingRobotID = -1;
							int waitingRobotID = -1;

							//Handle the particular case of starting from a critical section at the FIRST communication.
							wakeUpinCSRobot1 = !communicatedCPs.containsKey(robotTracker1) && Math.max(robotReport1.getPathIndex(), 0) >= cs.getTe1Start();
							wakeUpinCSRobot2 = !communicatedCPs.containsKey(robotTracker2) && Math.max(robotReport2.getPathIndex(), 0) >= cs.getTe2Start();
							if (wakeUpinCSRobot1 || wakeUpinCSRobot2) {
								//Check if the robot that is parked can exit from critical section.
								drivingRobotID = wakeUpinCSRobot1 ? robotReport1.getRobotID() : robotReport2.getRobotID();
								waitingRobotID = wakeUpinCSRobot1 ? robotReport2.getRobotID() : robotReport1.getRobotID();	
								metaCSPLogger.finest("Robot" + robotReport1.getRobotID() + " wake up: " +wakeUpinCSRobot1 + ", Robot"+ robotReport2.getRobotID() + " wake up: " + wakeUpinCSRobot2 +" in CS " + cs);

							}
							else {
								if (this.CSToDepsOrder.containsKey(cs) && this.CSToDepsOrder.get(cs) != null) {
									//The critical section is not new and no re-plan happens.
									//Standard case: there is just one dependency in the previous set. Let's impose that.
									waitingRobotID = this.CSToDepsOrder.get(cs).getFirst();
									drivingRobotID = waitingRobotID == robotReport1.getRobotID() ? robotReport2.getRobotID() : robotReport1.getRobotID();

									//If one of the robot was parked previously in critical section and has just started driving again,
									//then the set of previous constraints contains at least one dependency (due to parking).
								}
								else {
									metaCSPLogger.severe("Both cannot stop but lost critical section to dep. CS: " + cs + ", TE: " + cs.getTe1().getID() + ", " + cs.getTe2().getID() + ".");

									//Try to recover the lost order.
									int ahead = 0;
									//Only the leading robot may be already commanded to go beyond the critical section end.
									if (communicatedCPs.containsKey(robotTracker1) && (communicatedCPs.get(robotTracker1).getFirst() == -1 || communicatedCPs.get(robotTracker1).getFirst() > cs.getTe1End())) ahead = 1;
									else if (communicatedCPs.containsKey(robotTracker2) && (communicatedCPs.get(robotTracker2).getFirst() == -1 || communicatedCPs.get(robotTracker2).getFirst() > cs.getTe2End())) ahead = -1;

									//Otherwise, if both the robots are inside the critical section check their poses.
									if (ahead == 0) ahead = isAhead(cs, robotReport1, robotReport2);

									//Otherwise, the dependency is lost. Try an error. FIXME
									if (ahead == 0) {
										if (!this.CSToDepsOrder.containsKey(cs)) throw new Error("FIXME! Lost dependency and order cannot be restored! Key value not found. RobotReport1: " + robotReport1.toString() + ", RobotReport2: " + robotReport2.toString()
										+", last communicated CPs: (" + (communicatedCPs.containsKey(robotTracker1) ? communicatedCPs.get(robotTracker1).getFirst() : "null") + "," + (communicatedCPs.containsKey(robotTracker2) ? communicatedCPs.get(robotTracker2).getFirst() : "null),") + 
										"), cs: "+ cs.toString());
										else if (this.CSToDepsOrder.get(cs) == null) throw new Error("FIXME! Lost dependency and order cannot be restored! Empty value." );	
									}
									else {
										drivingRobotID = ahead == 1 ? robotReport1.getRobotID() : robotReport2.getRobotID();
										waitingRobotID = drivingRobotID == robotReport1.getRobotID() ? robotReport2.getRobotID() : robotReport1.getRobotID();
										metaCSPLogger.info("<<<<<<<<< Restoring the lost order via estimation for Robot" + drivingRobotID);
									}					
								}			
							}

							drivingCurrentIndex = drivingRobotID == robotReport1.getRobotID() ? robotReport1.getPathIndex() : robotReport2.getPathIndex();
							drivingTracker = drivingRobotID == robotReport1.getRobotID() ? robotTracker1 : robotTracker2;
							waitingTracker = waitingRobotID == robotReport1.getRobotID() ? robotTracker1 : robotTracker2;

							//check if the driving robot can really escape
							int waitingCurrentIndex = communicatedCPs.containsKey(waitingTracker) ? 
									//((communicatedCPs.get(trackers.get(waitingRobotID)).getFirst() == -1) ? 
									//		waitingTE.getPathLength()-1 : 
									communicatedCPs.get(waitingTracker).getFirst() : (
											waitingRobotID == robotReport1.getRobotID() ? robotReport1.getPathIndex() : robotReport2.getPathIndex());
							int lastIndexOfCSDriving = drivingRobotID == cs.getTe1().getRobotID() ? cs.getTe1End() : cs.getTe2End();
							//if (lastWaitingRobotCP >= startingWaitingRobotCS) {
							if (!canExitCriticalSection(drivingCurrentIndex, waitingCurrentIndex, drivingTracker.getTrajectoryEnvelope(), waitingTracker.getTrajectoryEnvelope(),lastIndexOfCSDriving)) {
								//it's too late for escaping. Let's create a deadlock: the parked robot should wait for the moving one,
								//while the other should be commanded to do not proceed beyond its last communicated CP. Both the dependencies should be added to the current set.									

								//The parked robot should wait for the other at its current path index till the moving robot has not been exited from its critical section.
								int artWaitingPoint = communicatedCPs.containsKey(drivingTracker) ? communicatedCPs.get(drivingTracker).getFirst() :
									(drivingRobotID == robotReport1.getRobotID() ? robotReport1.getPathIndex() : robotReport2.getPathIndex());
								int artDrivingCSEnd = waitingRobotID == cs.getTe1().getRobotID() ? cs.getTe1End() : cs.getTe2End();
								Dependency dep = new Dependency(drivingTracker.getTrajectoryEnvelope(),waitingTracker.getTrajectoryEnvelope(),Math.max(0, artWaitingPoint),artDrivingCSEnd);
								if (!artificialDependencies.containsKey(drivingRobotID)) artificialDependencies.put(drivingRobotID, new HashSet<Dependency>());
								artificialDependencies.get(drivingRobotID).add(dep);
								metaCSPLogger.info("Robot" + drivingRobotID + " cannot escape from CS: " + cs + ". Let's create a deadlock. Add artificial dependency for Robot" + drivingRobotID + " at " + dep.getWaitingPoint() + ".");
							}	
							metaCSPLogger.finest("Both can't. Driving Robot" + drivingRobotID + " at " + drivingCurrentIndex + " makes " + waitingRobotID + " wait at CS " + cs + ".");
						}

						//Compute waiting path index point for waiting robot
						waitingPoint = getCriticalPoint(waitingTracker.getTrajectoryEnvelope().getRobotID(), cs, drivingCurrentIndex);

						//Impose holding the previous dependence in some cases
						if (wakeUpinCSRobot1 && communicatedCPs.containsKey(robotTracker2)) {
							if (communicatedCPs.get(robotTracker2).getFirst() > waitingPoint) {
								metaCSPLogger.info("Wake-up Robot"+robotReport1.getRobotID()+"; revising waiting point of Robot" + robotReport2.getRobotID() + ": " + waitingPoint + "-->" + communicatedCPs.get(robotTracker2).getFirst());
								waitingPoint = communicatedCPs.get(robotTracker2).getFirst();
								escapingCSToWaitingRobotIDandCP.put(cs, new Pair<Integer,Integer>(robotReport2.getRobotID(),waitingPoint));
							}
						}
						else if (wakeUpinCSRobot2 && communicatedCPs.containsKey(robotTracker1)) {
							if (communicatedCPs.get(robotTracker1).getFirst() > waitingPoint) {
								metaCSPLogger.info("Wake-up Robot"+robotReport2.getRobotID()+"; revising waiting point of Robot" + robotReport1.getRobotID() + ": " + waitingPoint + "-->" + communicatedCPs.get(robotTracker1).getFirst());
								waitingPoint = communicatedCPs.get(robotTracker1).getFirst();
								escapingCSToWaitingRobotIDandCP.put(cs, new Pair<Integer,Integer>(robotReport1.getRobotID(),waitingPoint));
							}
						}

						if (escapingCSToWaitingRobotIDandCP.containsKey(cs) && escapingCSToWaitingRobotIDandCP.get(cs).getFirst() == waitingTracker.getTrajectoryEnvelope().getRobotID()) {
							waitingPoint = escapingCSToWaitingRobotIDandCP.get(cs).getSecond();
							metaCSPLogger.info("Use escaping waiting point. Make Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " stopping at " + waitingPoint + ".");
						}

						if (waitingPoint >= 0) {		
							//Make new dependency
							int drivingCSEnd =  drivingTracker.getTrajectoryEnvelope().getRobotID() == cs.getTe1().getRobotID() ? cs.getTe1End() : cs.getTe2End();
							if (!currentDeps.containsKey(waitingTracker.getTrajectoryEnvelope().getRobotID())) currentDeps.put(waitingTracker.getTrajectoryEnvelope().getRobotID(), new HashSet<Dependency>());
							Dependency dep = new Dependency(waitingTracker.getTrajectoryEnvelope(), drivingTracker.getTrajectoryEnvelope(), waitingPoint, drivingCSEnd);
							currentDeps.get(waitingTracker.getTrajectoryEnvelope().getRobotID()).add(dep);

							//Update the history of decisions for each critical section that has been updated
							CSToDepsOrder.put(cs, new Pair<Integer,Integer>(dep.getWaitingRobotID(),dep.getWaitingPoint()));
							depsToCS.put(dep, cs);
							if (canStopRobot1 && canStopRobot2) currentReversibleDependencies.add(dep);
						}
						else {
							//If robot is asked to wait in an invalid path point, throw error and give up!
							metaCSPLogger.severe("Waiting point < 0 for critical section " + cs);
							throw new Error("Waiting point < 0 for critical section " + cs);
						}
					}						
				}

				//Remove obsolete critical sections
				for (CriticalSection cs : toRemove) {
					this.allCriticalSections.remove(cs);
					this.escapingCSToWaitingRobotIDandCP.remove(cs);
				}

				//increment the counter
				this.criticalSectionCounter.addAndGet(toRemove.size());
			}


			//get current dependencies
			synchronized(currentDependencies) {

				//FIXME: already synchronized (so maybe is ok currentDependencies = computeClosestDependencies(currentDeps, artificialDependencies);)
				HashMap<Integer, Dependency> closestDeps  = computeClosestDependencies(currentDeps, artificialDependencies);
				currentDependencies.clear();
				currentDependencies.putAll(closestDeps);

				//find nonlive cycles and revise dependencies if necessary
				currentDeps = findAndRepairNonliveCycles(currentDeps, artificialDependencies, currentReversibleDependencies, currentReports, robotIDs);

				//send revised dependencies
				for (int robotID : robotIDs) sendCriticalPoint(robotID, currentReports);

				//Check if the robots are in deadlocks
				isDeadlocked();
			}
		}
	}

	/**
	 * Spawn a re-planning thread to remove the nonlive set of dependencies deadlockedDeps.
	 * @param robotsToReplan The set of robots which may attempt to re-plan the path.
	 * @param allConnectedRobots Set of robots in the same weakly connected component of the analyzed nonlive set.
	 * @return <code>true</code> if the replanning thread is correctly spawned.
	 */
	protected boolean spawnReplanning(final Set<Integer> robotsToReplan, final Set<Integer> allConnectedRobots) {

		//You should lock the robots if you want to start re-planning. 
		//In this way, the last critical point communicated when the re-plan is started is forced not to be updated
		synchronized (replanningStoppingPoints) {
			if (setMaxCPDependencies(robotsToReplan)) {
				metaCSPLogger.info("Will re-plan for one of the following deadlocked robots: " + robotsToReplan + " (" + allConnectedRobots + ")...");
				new Thread() {
					public void run() {
						rePlanPath(robotsToReplan, allConnectedRobots);
					}
				}.start();
				return true;

			}
			return false;
		}
	}

	protected boolean setMaxCPDependencies(Set<Integer> robotsIDs) {
		synchronized (replanningStoppingPoints) {
			boolean tryLocking = true;
			for (int robotID : robotsIDs) {
				if (replanningStoppingPoints.containsKey(robotID)) {
					tryLocking = false;
					break;
				}
			}
			if (tryLocking) {
				HashMap<Integer, Dependency> currentDeps = getCurrentDependencies();
				for (int robotID : robotsIDs) {
					if (currentDeps.containsKey(robotID)) {
						Dependency dep = currentDeps.get(robotID);
						replanningStoppingPoints.put(dep.getWaitingRobotID(), new Dependency(dep.getWaitingTrajectoryEnvelope(), null, dep.getWaitingPoint(), 0));
					}
				}
				metaCSPLogger.finest("Add maximum CP dependency (re-plan) for robots: " + robotsIDs.toString());
			}
			return tryLocking;
		}
	}

	protected boolean nonlivePair(Dependency dep1, Dependency dep2) {
		if (dep2.getWaitingPoint() <= dep1.getReleasingPoint()) return true;
		return false;
	}


	/**
	 * Try re-planning ONE path AMONG the set of robots robotsToReplan while considering robotsAsObstacles placed in their current CP as additional obstacles. 
	 * @param robotsToReplan The set of robots which may attempt to re-plan the path.
	 * @param robotsAsObstacles The set of robots to consider as additional obstacles while re-planning (only if they have a critical point).
	 * @param useStaticReplan <code>true</code> iff all robotsToReplan should yield in their current critical point before starting the re-plan.
	 */
	protected boolean rePlanPath(Set<Integer> robotsToReplan, Set<Integer> robotsAsObstacles) {
		boolean ret = false;
		
		for (int robotID : robotsToReplan) {
			int currentWaitingIndex = -1;
			Pose currentWaitingPose = null;
			Pose currentWaitingGoal = null;
			PoseSteering[] oldPath = null;

			//FIXME not synchronized on current dependencies
			Geometry[] obstacles = null;
			HashSet<Integer> otherRobotIDs = new HashSet<Integer>();
			synchronized (getCurrentDependencies()) {
				HashMap<Integer, Dependency> currentDeps = getCurrentDependencies();
				Dependency dep = null;
				if (robotsToReplan.size() == 1) {
					synchronized(replanningStoppingPoints) {
						if (!replanningStoppingPoints.containsKey(robotID)) {
							metaCSPLogger.info("Invalid replan " + robotID + " ... ");
							return false;
						}
						dep = replanningStoppingPoints.get(robotID);
					}
				}
				else { //replanning for deadlock
					if (!currentDeps.containsKey(robotID)) {
						metaCSPLogger.info("Robot " + robotID + " is not deadlocked ... ");
						continue;
					}
					dep = currentDeps.get(robotID);
				}
				currentWaitingIndex = dep.getWaitingPoint();
				currentWaitingPose = dep.getWaitingPose();
				if (currentWaitingPose == null) throw new Error("Waiting pose should not be null in dep: " + dep);
				Trajectory traj = dep.getWaitingTrajectoryEnvelope().getTrajectory();
				oldPath = traj.getPoseSteering();
				currentWaitingGoal = oldPath[oldPath.length-1].getPose();
				if (currentWaitingGoal == null) throw new Error("Waiting goal should not be null in dep: " + dep);
				
				if (robotsAsObstacles.size() > 0) {
					otherRobotIDs = new HashSet<Integer>();
					for (int otherRobotID : robotsAsObstacles) if (otherRobotID != robotID) otherRobotIDs.add(otherRobotID);
					if (!otherRobotIDs.isEmpty()) obstacles = getObstaclesInCriticalPoints(ArrayUtils.toPrimitive(otherRobotIDs.toArray(new Integer[otherRobotIDs.size()])));
				}
			}

			metaCSPLogger.info("Attempting to re-plan path of Robot" + robotID + " (with obstacles for robots " + otherRobotIDs.toString() + ", from " + 
					currentWaitingPose + ", to " + currentWaitingGoal + ")...");
			AbstractMotionPlanner mp = null;
			if (this.motionPlanners.containsKey(robotID)) mp = this.motionPlanners.get(robotID);
			else {
				metaCSPLogger.severe("Motion planner is not initialized for Robot" + robotID + ", cannot replan");
				continue;
			}
			synchronized (mp) {
				PoseSteering[] newPath = doReplanning(mp, currentWaitingPose, currentWaitingGoal, obstacles);
				replanningTrialsCounter.incrementAndGet();
				if (newPath != null && newPath.length > 0) {
					PoseSteering[] newCompletePath = new PoseSteering[newPath.length+currentWaitingIndex];
					for (int i = 0; i < newCompletePath.length; i++) {
						if (i < currentWaitingIndex) newCompletePath[i] = oldPath[i];
						else newCompletePath[i] = newPath[i-currentWaitingIndex];
					}
					replacePath(robotID, newCompletePath, currentWaitingIndex, robotsToReplan);
					successfulReplanningTrialsCounter.incrementAndGet();
					metaCSPLogger.info("Successfully re-planned path of Robot" + robotID);
					ret = true;
					break;
				}
				else {
					metaCSPLogger.info("Failed to re-plan path of Robot" + robotID);
				}
			}
		}
		synchronized(replanningStoppingPoints) {
			for (int robotID : robotsToReplan) replanningStoppingPoints.remove(robotID);
			metaCSPLogger.info("Removing replanning stopping points of robots: " + robotsToReplan.toString());
		}
		return ret;
	}

	@Override
	protected void setupInferenceCallback() {

		this.stopInference = false;
		this.inference = new Thread("Coordinator inference") {

			@Override
			public void run() {

				String fileName = new String(System.getProperty("user.home")+File.separator+"coordinator_stat.txt");
				initStat(fileName, "Init statistics: @"+Calendar.getInstance().getTimeInMillis() + "\n");
				String stat = new String(//"Legend: at each control period\n\t 1. elapsed time to compute critical sections\n\t 2.elapsed time to update dependencies\n\t 3. number of new critical sections\n\t"
						"elapsedTimeComputeCriticalSections\t elapsedTimeUpdateDependencies\t numberNewCriticalSections\t numberAllCriticalSections\t numberNewAddedMissions \t numberDrivingRobots\t expectedSleepingTime\t effectiveSleepingTime\t printStatisticsTime\t effectiveTc");
				if (avoidDeadlockGlobally.get()) stat = stat + new String("\t numberReversedPrecedenceOrder\t numberCurrentCycles");
				stat.concat("\n");				
				writeStat(fileName, stat);

				long threadLastUpdate = Calendar.getInstance().getTimeInMillis();
				long elapsedTimeComputeCriticalSections = -1;
				long elapsedTimeUpdateDependencies = -1;
				int numberNewCriticalSections = -1;
				int numberAllCriticalSections = -1;
				int numberNewAddedMissions = 0;
				int numberDrivingRobots = 0;
				int MAX_ADDED_MISSIONS = 1;
				long expectedSleepingTime = -1;
				long effectiveSleepingTime = -1;
				long printStatisticsTime = -1;

				while (!stopInference) {
					elapsedTimeComputeCriticalSections = -1;
					elapsedTimeUpdateDependencies = -1;
					numberNewCriticalSections = -1;
					numberAllCriticalSections = -1;
					numberNewAddedMissions = 0;
					numberDrivingRobots = 0;

					synchronized (solver) {	
						for (Integer robotID : trackers.keySet()) 
							if (!(trackers.get(robotID) instanceof TrajectoryEnvelopeTrackerDummy)) numberDrivingRobots++;

						if (!missionsPool.isEmpty()) {

							//FIXME critical sections should be computed incrementally/asynchronously
							while (!missionsPool.isEmpty() && numberNewAddedMissions < MAX_ADDED_MISSIONS) {
								Pair<TrajectoryEnvelope,Long> te = missionsPool.pollFirst();
								envelopesToTrack.add(te.getFirst());
								onNewMissionDispatched(te.getFirst().getRobotID());
								numberNewAddedMissions++;
							}
							numberNewCriticalSections = allCriticalSections.size();
							elapsedTimeComputeCriticalSections = Calendar.getInstance().getTimeInMillis();
							computeCriticalSections();
							elapsedTimeComputeCriticalSections = Calendar.getInstance().getTimeInMillis()-elapsedTimeComputeCriticalSections;
							numberAllCriticalSections = allCriticalSections.size();
							numberNewCriticalSections = numberAllCriticalSections-numberNewCriticalSections;

							startTrackingAddedMissions();
						}
						elapsedTimeUpdateDependencies = Calendar.getInstance().getTimeInMillis();
						updateDependencies();
						elapsedTimeUpdateDependencies = Calendar.getInstance().getTimeInMillis()-elapsedTimeUpdateDependencies;
						numberAllCriticalSections = allCriticalSections.size();

						if (!quiet) {
							printStatisticsTime = Calendar.getInstance().getTimeInMillis();
							printStatistics();
							printStatisticsTime = Calendar.getInstance().getTimeInMillis()-printStatisticsTime;
						}
						if (overlay) overlayStatistics();
					}

					//Sleep a little...
					expectedSleepingTime = Math.max(500,CONTROL_PERIOD-Calendar.getInstance().getTimeInMillis()+threadLastUpdate);
					effectiveSleepingTime = Calendar.getInstance().getTimeInMillis();
					if (CONTROL_PERIOD > 0) {
						try { Thread.sleep(expectedSleepingTime); }
						catch (InterruptedException e) { e.printStackTrace(); }
					}
					effectiveSleepingTime = Calendar.getInstance().getTimeInMillis()-effectiveSleepingTime;

					EFFECTIVE_CONTROL_PERIOD = (int)(Calendar.getInstance().getTimeInMillis()-threadLastUpdate);
					threadLastUpdate = Calendar.getInstance().getTimeInMillis();

					if (inferenceCallback != null) inferenceCallback.performOperation();

					stat = new String(elapsedTimeComputeCriticalSections + "\t" + elapsedTimeUpdateDependencies + "\t" + numberNewCriticalSections + "\t" + numberAllCriticalSections + "\t" + numberNewAddedMissions + "\t" + numberDrivingRobots
							+ "\t" + expectedSleepingTime + "\t" + effectiveSleepingTime + "\t" + printStatisticsTime + "\t" + EFFECTIVE_CONTROL_PERIOD);
					if (avoidDeadlockGlobally.get()) stat = stat + new String("\t" + currentOrdersHeurusticallyDecided.get() + "\t" + currentCyclesList.size());
					stat.concat("\n");
					writeStat(fileName, stat);
				}

			}
		};
		inference.setPriority(Thread.MAX_PRIORITY);
		inference.start();
	}

	@Override
	protected void cleanUpRobotCS(int robotID, int lastWaitingPoint) {

		synchronized (allCriticalSections) {

			metaCSPLogger.info("Cleaning up critical sections of Robot" + robotID);
			ArrayList<CriticalSection> toRemove = new ArrayList<CriticalSection>();

			//Clear the critical sections for which we have stored a dependency ...
			for (CriticalSection cs : CSToDepsOrder.keySet()) {
				if (cs.getTe1().getRobotID() == robotID || cs.getTe2().getRobotID() == robotID) toRemove.add(cs);
			}

			//... and all the critical sections which are currently alive.
			for (CriticalSection cs : allCriticalSections) {
				if ((cs.getTe1().getRobotID() == robotID || cs.getTe2().getRobotID() == robotID) && !toRemove.contains(cs)) {
					toRemove.add(cs);
					metaCSPLogger.severe("<<<<<<<< WARNING: Cleaning up a critical section which was not associated to a dependency: " + cs + ".");
					//increment the counter
					if (cs.getTe1().getRobotID() == robotID && (cs.getTe1Start() <= lastWaitingPoint || lastWaitingPoint == -1) || 
							cs.getTe2().getRobotID() == robotID && (cs.getTe2Start() <= lastWaitingPoint || lastWaitingPoint == -1))
						this.criticalSectionCounter.incrementAndGet();
				}
			}

			for (CriticalSection cs : toRemove) {
				if (this.avoidDeadlockGlobally.get()) {
					if (CSToDepsOrder.containsKey(cs)) {
						int waitingRobID = CSToDepsOrder.get(cs).getFirst();
						int drivingRobID = cs.getTe1().getRobotID() == waitingRobID ? cs.getTe2().getRobotID() : cs.getTe1().getRobotID(); 
						deleteEdge(new Pair<Integer,Integer>(waitingRobID,drivingRobID));
						//metaCSPLogger.info(currentCyclesList.toString());
					}
				}
				CSToDepsOrder.remove(cs);
				allCriticalSections.remove(cs);
				escapingCSToWaitingRobotIDandCP.remove(cs);
			}
		}
	}


	/**
	 * Replace the path of a robot's {@link TrajectoryEnvelope} on the fly.
	 * @param robotID The ID of the robot whose {@link TrajectoryEnvelope} is to be recomputed.
	 * @param newPath The path based on which the new {@link TrajectoryEnvelope} should be computed.
	 * @param breakingPathIndex Last point on the current path to preserve. 
	 * @param lockedRobotIDs The set of robots which have been locked when the re-plan started.
	 * @param <code>true</code> whether the robot should be at the current critical point before starting a re-plan.
	 */
	public void replacePath(int robotID, PoseSteering[] newPath, int breakingPathIndex, Set<Integer> lockedRobotIDs) {
		replacePath(robotID, newPath, breakingPathIndex, true, lockedRobotIDs);
	}

	/**
	 * Replace the path of a robot's {@link TrajectoryEnvelope} on the fly while discarding a piece of the old path.
	 * @param robotID The ID of the robot whose {@link TrajectoryEnvelope} is to be recomputed.
	 * @param newPath The path based on which the new {@link TrajectoryEnvelope} should be computed.
	 * @param breakingPathIndex Last point on the current path to preserve. 
	 * @param concatenatePaths <code>true</code> whether the new path should be concatenated with the old one.
	 * @param lockedRobotIDs The set of robots which have been locked when the re-plan started.
	 * @param <code>true</code> whether the robot should be at the current critical point before starting a re-plan.
	 * @ATTENTION When the path is not concatenated, it is supposed the robot has already traversed the overall path.
	 */
	public void replacePath(int robotID, PoseSteering[] newPath, int breakingPathIndex, boolean concatenatePaths, Set<Integer> lockedRobotIDs) {

		synchronized (solver) {

			synchronized(trackers) {
				if (!trackers.containsKey(robotID)) {
					metaCSPLogger.warning("Invalid robotID. Place the robot before!");
					return;
				}
			}

			synchronized (allCriticalSections) {
				//Get current envelope
				TrajectoryEnvelope te = this.getCurrentTrajectoryEnvelope(robotID);

				if (viz != null) {
					viz.removeEnvelope(te);
				}

				metaCSPLogger.info("Replacing TE " + te.getID() + " (Robot" + robotID + ") with breaking point " + breakingPathIndex + ".");

				//------------------ (dynamic re-plan) --------------------
				//store the order of precedence (and waiting points) for every 
				//critical section involving the robot which is re-planning
				HashMap<CriticalSection,Pair<Integer,Integer>> holdingCS = new HashMap<CriticalSection,Pair<Integer,Integer>>();
				for (CriticalSection cs : CSToDepsOrder.keySet()) {
					if (cs.getTe1().getRobotID() == robotID && cs.getTe1Start() <= breakingPathIndex || 
							cs.getTe2().getRobotID() == robotID && cs.getTe2Start() <= breakingPathIndex) {
						Pair<Integer,Integer> order = new Pair<Integer,Integer>(CSToDepsOrder.get(cs).getFirst(), CSToDepsOrder.get(cs).getSecond());
						holdingCS.put(cs, order);
						metaCSPLogger.finest("Saving " + order.toString() + " for critical section " + cs + ".");
					}
				}
				//---------------------------------------------------------

				//Remove CSs involving this robot, clearing the history.
				cleanUpRobotCS(te.getRobotID(), breakingPathIndex);
				//---------------------------------------------------------

				//Make new envelope
				TrajectoryEnvelope newTE = solver.createEnvelopeNoParking(robotID, newPath, "Driving", this.getFootprint(robotID));

				//Notify tracker
				synchronized (trackers) {
					this.trackers.get(robotID).updateTrajectoryEnvelope(newTE);
				}

				//Stitch together with rest of constraint network (temporal constraints with parking envelopes etc.)
				for (Constraint con : solver.getConstraintNetwork().getOutgoingEdges(te)) {
					if (con instanceof AllenIntervalConstraint) {
						AllenIntervalConstraint aic = (AllenIntervalConstraint)con;
						if (aic.getTypes()[0].equals(AllenIntervalConstraint.Type.Meets)) {
							TrajectoryEnvelope newEndParking = solver.createParkingEnvelope(robotID, PARKING_DURATION, newTE.getTrajectory().getPose()[newTE.getTrajectory().getPose().length-1], "whatever", getFootprint(robotID));
							TrajectoryEnvelope oldEndParking = (TrajectoryEnvelope)aic.getTo();

							solver.removeConstraints(solver.getConstraintNetwork().getIncidentEdges(te));
							solver.removeVariable(te);
							solver.removeConstraints(solver.getConstraintNetwork().getIncidentEdges(oldEndParking));
							solver.removeVariable(oldEndParking);

							AllenIntervalConstraint newMeets = new AllenIntervalConstraint(AllenIntervalConstraint.Type.Meets);
							newMeets.setFrom(newTE);
							newMeets.setTo(newEndParking);
							solver.addConstraint(newMeets);

							break;
						}
					}
				}

				if (viz != null) {
					viz.addEnvelope(newTE);
				}

				//Add as if it were a new envelope, that way it will be accounted for in computeCriticalSections()
				envelopesToTrack.add(newTE);

				//Recompute CSs involving this robot
				computeCriticalSections();

				//------------------ (dynamic re-plan) --------------------
				//Here we have to shift the start and end of the old critical section if the path is new
				int offset = concatenatePaths ? 0 : -breakingPathIndex;
				for (CriticalSection cs1 : allCriticalSections) {
					if (cs1.getTe1().getRobotID() == robotID && cs1.getTe1Start() <= breakingPathIndex+offset || 
							cs1.getTe2().getRobotID() == robotID && cs1.getTe2Start() <= breakingPathIndex+offset) {
						for (CriticalSection cs2 : holdingCS.keySet()) {//cs2 are the old ones which should be affected by the offset for the robot that is replanning
							if (cs1.getTe1().getRobotID() == cs2.getTe1().getRobotID() && cs1.getTe2().getRobotID() == cs2.getTe2().getRobotID() ||
									cs1.getTe1().getRobotID() == cs2.getTe2().getRobotID() && cs1.getTe2().getRobotID() == cs2.getTe1().getRobotID()) {

								//We should restore all the dependency order which cannot be safely reverted.
								//Each new critical section (cs1 = [start11,end11], [start12,end12]) containing the breaking point 
								//can be matched to its related old one (one cs2 = [start21,end21], [start22,end22]) by reasoning about the spatial overlaps between TEs' intervals. 
								//Note that start and end of each critical sections are both included (i.e., still overlapping configurations) 
								//-- see {@link CriticalSection}, {@link AbstractTrajectoryEnvelopeCoordinaror}, function: getCriticalSections).

								int start11 = cs1.getTe1().getRobotID() == robotID ? cs1.getTe1Start() : cs1.getTe2Start();
								int start12 = cs1.getTe1().getRobotID() == robotID ? cs1.getTe2Start() : cs1.getTe1Start();
								int end11 = cs1.getTe1().getRobotID() == robotID ? cs1.getTe1End() : cs1.getTe2End();
								int end12 = cs1.getTe1().getRobotID() == robotID ? cs1.getTe2End() : cs1.getTe1End();
								int start21 = cs2.getTe1().getRobotID() == robotID ? cs2.getTe1Start()+offset : cs2.getTe2Start();
								int start22 = cs2.getTe1().getRobotID() == robotID ? cs2.getTe2Start()+offset : cs2.getTe1Start();
								int end21 = cs2.getTe1().getRobotID() == robotID ? cs2.getTe1End()+offset : cs2.getTe2End();
								int end22 = cs2.getTe1().getRobotID() == robotID ? cs2.getTe2End()+offset : cs2.getTe1End();

								//Here we should consider all the cases of spatial overlap between the new critical section (cs1 = [start11,end11], [start12,end12]) and the old 
								//critical section (cs2 = [start21,end21], [start22,end22]). Note that start and end are still overlapping configurations (see {@link CriticalSection}, {@link AbstractTrajectoryEnvelopeCoordinaror}, function: getCriticalSections).
								if ((start21 <= start11 && start11 <= end21) && !(end22 < start12 || end12 < start22)) {
									metaCSPLogger.finest("Restoring  the order of critical section " + cs2 + "(" + holdingCS.get(cs2).toString() + ") for critical section " + cs1 + ".");
									CSToDepsOrder.put(cs1, holdingCS.get(cs2));

									if (this.avoidDeadlockGlobally.get()) {
										//re-add dependency to cyclesList and currentOrdersGraph
										HashMap<Pair<Integer,Integer>,Integer> edgesToAdd = new HashMap<Pair<Integer,Integer>,Integer>();
										int waitingRobotID = holdingCS.get(cs2).getFirst();
										int drivingRobotID = waitingRobotID == cs2.getTe1().getRobotID() ? cs2.getTe2().getRobotID() : cs2.getTe1().getRobotID();
										edgesToAdd.put(new Pair<Integer,Integer>(waitingRobotID, drivingRobotID),1);
										addEdges(edgesToAdd);
									}
									break;
								}
							}		
						}
					}
				}

				envelopesToTrack.remove(newTE);

				synchronized (replanningStoppingPoints) {
					for (int ID : lockedRobotIDs) replanningStoppingPoints.remove(ID);
					metaCSPLogger.finest("Unlocking robots: " + lockedRobotIDs.toString());
				}

				forceCriticalPointReTransmission.put(robotID, true);
				updateDependencies();											

			}
		}
	}


	/**
	 * Truncate the {@link TrajectoryEnvelope} of a given robot at the closest dynamically-feasible path point. This path point is computed via the robot's {@link ForwardModel}.
	 * @param robotID The ID of the robot whose {@link TrajectoryEnvelope} should be truncated.
	 * @return <code>true</code> iff the envelope is successfully truncated.
	 */
	public boolean truncateEnvelope(int robotID) {
		return truncateEnvelopeAt(robotID, -1);
	}

	/**
	 * Truncate the {@link TrajectoryEnvelope} of a given robot at the closest dynamically-feasible path point. This path point is computed via the robot's {@link ForwardModel}.
	 * @param robotID The ID of the robot whose {@link TrajectoryEnvelope} should be truncated.
	 * @param pathIndex The path index at which the envelope should be truncated (-1 if not specified)
	 * @return <code>true</code> iff the envelope is successfully truncated at the desired path index (if specified and if kinodynamically feasible),
	 * 							 or at the closest kinodynamically-feasible one otherwise.
	 */
	public boolean truncateEnvelopeAt(int robotID, int pathIndex) {

		synchronized (solver) {

			synchronized (replanningStoppingPoints) {
				if (replanningStoppingPoints.containsKey(robotID)) return false;
			}

			TrajectoryEnvelope te = this.getCurrentTrajectoryEnvelope(robotID);
			AbstractTrajectoryEnvelopeTracker tet = null;
			synchronized(trackers) {
				tet = this.trackers.get(robotID); 
			}

			int earliestStoppingPathIndex = -1;

			if (!(tet instanceof TrajectoryEnvelopeTrackerDummy)) {

				earliestStoppingPathIndex = this.getForwardModel(robotID).getEarliestStoppingPathIndex(te, this.getRobotReport(robotID));

				//Check if you were already slowing down to stop in your critical point.
				int lastCommunicatedCP = -1;
				lastCommunicatedCP = communicatedCPs.get(tet).getFirst();
				earliestStoppingPathIndex = (lastCommunicatedCP != -1) ? Math.min(lastCommunicatedCP, earliestStoppingPathIndex) : earliestStoppingPathIndex;
				if (pathIndex != -1 && pathIndex < earliestStoppingPathIndex) return false;

				//Compute and add new TE, remove old TE (both driving and final parking)
				int stoppingIndex = pathIndex != -1 ? pathIndex : earliestStoppingPathIndex;
				PoseSteering[] truncatedPath = Arrays.copyOf(te.getTrajectory().getPoseSteering(),stoppingIndex+1);

				//replace the path of this robot (will compute new envelope)
				replacePath(robotID, truncatedPath, truncatedPath.length-1, new HashSet<Integer>(robotID));

				metaCSPLogger.info("Truncating " + te + " at " + stoppingIndex + ".");
			}

			return true;
		}
	}

	/**
	 * Reverse the {@link TrajectoryEnvelope} of a given robot at the closest dynamically-feasible path point.
	 * This path point is computed via the robot's {@link ForwardModel}.
	 * @param robotID The ID of the robot whose {@link TrajectoryEnvelope} should be reversed.
	 * @return <code>true</code> iff the envelope is successfully truncated.
	 */
	public boolean reverseEnvelope(int robotID) {
		synchronized (solver) {

			synchronized (replanningStoppingPoints) {
				if (replanningStoppingPoints.containsKey(robotID)) return false;
			}

			TrajectoryEnvelope te = this.getCurrentTrajectoryEnvelope(robotID);
			AbstractTrajectoryEnvelopeTracker tet = null;
			synchronized(trackers) {
				tet = this.trackers.get(robotID); 
			}

			int earliestStoppingPathIndex = -1;

			if (!(tet instanceof TrajectoryEnvelopeTrackerDummy)) {
				earliestStoppingPathIndex = this.getForwardModel(robotID).getEarliestStoppingPathIndex(te, this.getRobotReport(robotID));
				//Check if you were already slowing down to stop in your critical point.
				int lastCommunicatedCP = -1;
				lastCommunicatedCP = communicatedCPs.get(tet).getFirst();
				earliestStoppingPathIndex = (lastCommunicatedCP != -1) ? Math.min(lastCommunicatedCP, earliestStoppingPathIndex) : earliestStoppingPathIndex;

				if (earliestStoppingPathIndex != -1) {

					//Compute and add new TE, remove old TE (both driving and final parking)
					PoseSteering[] truncatedPath = Arrays.copyOf(te.getTrajectory().getPoseSteering(), earliestStoppingPathIndex+1);
					PoseSteering[] overallPath = new PoseSteering[truncatedPath.length*2-1];
					for (int i = 0; i < truncatedPath.length; i++) overallPath[i] = truncatedPath[i];
					for (int i = 1; i < truncatedPath.length; i++) overallPath[truncatedPath.length-1+i] = truncatedPath[truncatedPath.length-i];

					//replace the path of this robot (will compute new envelope)
					replacePath(robotID, overallPath, truncatedPath.length-1, new HashSet<Integer>(robotID));

					metaCSPLogger.info("Reversing " + te + " at " + earliestStoppingPathIndex + ".");
				}
			}

			return true;
		}
	}

	protected void deleteEdge(Pair<Integer,Integer> edge) {
		HashMap<Pair<Integer,Integer>, Integer> edgesToDelete = new HashMap<Pair<Integer,Integer>, Integer>();
		edgesToDelete.put(edge, 1);
		deleteEdges(edgesToDelete);
	}

	protected void deleteEdges(HashMap<Pair<Integer,Integer>, Integer> edgesToDelete) {

		synchronized(allCriticalSections) {

			if (edgesToDelete == null || edgesToDelete.isEmpty()) return;

			for (Pair<Integer,Integer> edge : edgesToDelete.keySet()) {
				Integer occurrence = edgesToDelete.get(edge);
				if (occurrence == null || occurrence == 0) {
					metaCSPLogger.severe("<<<<<<<<<< Found edge " + edge.toString() + " with invalid weigth. Skipping deletion." );
					continue;
				}
				DefaultWeightedEdge e = currentOrdersGraph.getEdge(edge.getFirst(), edge.getSecond());
				if (e != null) {
					int numEdge = (int) currentOrdersGraph.getEdgeWeight(e);
					if (numEdge > occurrence) currentOrdersGraph.setEdgeWeight(e, numEdge-occurrence);
					else {
						//FIXME Waste of time is probably here
						metaCSPLogger.finest("Removing the edge: " + edge.toString());
						currentOrdersGraph.removeEdge(edge.getFirst(), edge.getSecond());
						if (currentCyclesList.containsKey(edge)) {
							HashMap<Pair<Integer, Integer>, HashSet<ArrayList<Integer>>> toRemove = new HashMap<Pair<Integer, Integer>, HashSet<ArrayList<Integer>>>();
							for (ArrayList<Integer> cycle : currentCyclesList.get(edge)) {
								for (int i = 0; i < cycle.size(); i++) {
									Pair<Integer,Integer> key = new Pair<Integer,Integer>(cycle.get(i), cycle.get(i < cycle.size()-1 ? i+1 : 0));
									if (!toRemove.containsKey(key)) toRemove.put(key, new HashSet<ArrayList<Integer>>());
									toRemove.get(key).add(cycle);
								}
							}
							for (Pair<Integer, Integer> key : toRemove.keySet()) {
								currentCyclesList.get(key).removeAll(toRemove.get(key));
								if (currentCyclesList.get(key).isEmpty()) currentCyclesList.remove(key);
							}
						}
					}
				}
			}
		}
	}

	protected void addEdges(HashMap<Pair<Integer,Integer>, Integer> edgesToAdd) {

		if (edgesToAdd == null || edgesToAdd.isEmpty()) return;

		synchronized(allCriticalSections) {

			HashSet<Pair<Integer,Integer>> toAdd = new HashSet<Pair<Integer,Integer>>();

			//add the edges if not already in the graph
			for (Pair<Integer,Integer> edge : edgesToAdd.keySet()) {
				Integer occurrence = edgesToAdd.get(edge);
				if (occurrence == null || occurrence == 0) {
					metaCSPLogger.severe("<<<<<<<<<< Found edge " + edge.toString() + " with invalid weigth. Skipping addition." );
					continue;
				}
				if (!currentOrdersGraph.containsEdge(edge.getFirst(),edge.getSecond())) {
					toAdd.add(edge);
					currentOrdersGraph.addVertex(edge.getFirst());
					currentOrdersGraph.addVertex(edge.getSecond());
					DefaultWeightedEdge e = currentOrdersGraph.addEdge(edge.getFirst(),edge.getSecond());
					if (e == null) System.out.println("<<<<<<<<< Add dependency order fails (12). Edge: " + edge.getFirst() + "->" + edge.getSecond());				
					currentOrdersGraph.setEdgeWeight(e, occurrence);
					metaCSPLogger.finest("Add " + occurrence + " edges:" + edge.toString());
				}
				else {
					DefaultWeightedEdge e = currentOrdersGraph.getEdge(edge.getFirst(),edge.getSecond());
					currentOrdersGraph.setEdgeWeight(e,currentOrdersGraph.getEdgeWeight(e)+occurrence);
				}
			}
			if (toAdd.isEmpty()) return;

			//compute strongly connected components
			KosarajuStrongConnectivityInspector<Integer,DefaultWeightedEdge> ksccFinder = new KosarajuStrongConnectivityInspector<Integer,DefaultWeightedEdge>(currentOrdersGraph);
			List<Set<Integer>> sets = ksccFinder.stronglyConnectedSets();
		    List<SimpleDirectedWeightedGraph<Integer,DefaultWeightedEdge>> sccs = new ArrayList<SimpleDirectedWeightedGraph<Integer,DefaultWeightedEdge>>(sets.size());
            for (Set<Integer> set : sets) {
            	SimpleDirectedWeightedGraph<Integer,DefaultWeightedEdge> subG = new SimpleDirectedWeightedGraph<Integer,DefaultWeightedEdge>(DefaultWeightedEdge.class);
            	for(Integer v : set) subG.addVertex(v);
            	for(DefaultWeightedEdge e : currentOrdersGraph.edgeSet()) {
            		if(subG.containsVertex(currentOrdersGraph.getEdgeSource(e)) && subG.containsVertex(currentOrdersGraph.getEdgeTarget(e))) {
            			DefaultWeightedEdge e_ = subG.addEdge(currentOrdersGraph.getEdgeSource(e), currentOrdersGraph.getEdgeTarget(e));
            			if (e_ == null) metaCSPLogger.severe("<<<<<<<<< Add egde fails (8). Edge: " + e.toString());
            			subG.setEdgeWeight(e_, currentOrdersGraph.getEdgeWeight(e));
            		}
            	}
            	sccs.add(subG);
            }
			
			metaCSPLogger.info("Connected components: " + sccs.toString());

			//update the cycle list. Use a map to avoid recomputing cycles of each connected component.
			for (Pair<Integer,Integer> pair : toAdd) {
				//search the strongly connected components containing the two vertices
				for (SimpleDirectedWeightedGraph<Integer,DefaultWeightedEdge> scc : sccs) {
					if (scc.containsVertex(pair.getFirst()) ||	scc.containsVertex(pair.getSecond())) {
						if (scc.containsVertex(pair.getFirst()) && scc.containsVertex(pair.getSecond())) {
							//get cycles in this strongly connected components
							JohnsonSimpleCycles<Integer,DefaultWeightedEdge> cycleFinder = new JohnsonSimpleCycles<Integer,DefaultWeightedEdge>(scc);
							List<List<Integer>> cycles = cycleFinder.findSimpleCycles();
							if (!cycles.isEmpty()) {
								for(List<Integer> cycle : cycles) {
									//update the list of cycles for each edge
									for (int i = 0; i < cycle.size(); i++) {
										int j = i < cycle.size()-1 ? i+1 : 0;
										Pair<Integer,Integer> edge = new Pair<Integer,Integer>(cycle.get(i), cycle.get(j));
										if (!currentCyclesList.containsKey(edge)) currentCyclesList.put(edge, new HashSet<ArrayList<Integer>>());
										currentCyclesList.get(edge).add((ArrayList<Integer>)cycle);
										//metaCSPLogger.info("edge: " + edge.toString() + "currentCyclesList:" + currentCyclesList.get(edge));
									}
								}
							}
						}
						break;
					}
				}
			}
		}
	}

	protected void updateGraph(HashMap<Pair<Integer,Integer>, Integer> edgesToDelete, HashMap<Pair<Integer,Integer>, Integer> edgesToAdd) {

		synchronized(allCriticalSections) {		

			HashMap<Pair<Integer,Integer>, Integer> toDelete = null;
			if (edgesToDelete != null && !edgesToDelete.isEmpty()) {
				toDelete = new HashMap<Pair<Integer,Integer>, Integer>();
				for (Pair<Integer,Integer> edge : edgesToDelete.keySet()) {
					if (edgesToAdd.containsKey(edge) && edgesToAdd.get(edge) != null && edgesToAdd.get(edge) < edgesToDelete.get(edge)) 
						toDelete.put(edge, edgesToDelete.get(edge)-edgesToAdd.get(edge));
					else toDelete.put(edge, edgesToDelete.get(edge));
				}
			}

			HashMap<Pair<Integer,Integer>, Integer> toAdd = null;
			if (edgesToAdd != null && !edgesToAdd.isEmpty()) {
				toAdd = new HashMap<Pair<Integer,Integer>, Integer>();
				for (Pair<Integer,Integer> edge : edgesToAdd.keySet()) {
					if (edgesToDelete.containsKey(edge) && edgesToDelete.get(edge) != null && edgesToDelete.get(edge) < edgesToAdd.get(edge)) 
						toAdd.put(edge, edgesToAdd.get(edge)-edgesToDelete.get(edge));
					else toAdd.put(edge, edgesToAdd.get(edge));
				}
			}

			deleteEdges(toDelete);			
			addEdges(toAdd);

		}
	}

	protected void globalCheckAndRevise() {

		synchronized(solver) {
			HashMap<Integer,RobotReport> currentReports = new HashMap<Integer,RobotReport>();
			HashMap<Integer,HashSet<Dependency>> currentDeps = new HashMap<Integer,HashSet<Dependency>>();
			HashMap<Integer,HashSet<Dependency>> artificialDependencies = new HashMap<Integer,HashSet<Dependency>>(); 
			DirectedMultigraph<Integer,Dependency> depsGraph = new DirectedMultigraph<Integer, Dependency>(Dependency.class);
			HashSet<Integer> askForReplan = new HashSet<Integer>();			
			HashMap<Integer, Integer> earliestStoppingPoints = new HashMap<Integer,Integer>();

			HashMap<Pair<Integer,Integer>,Integer> edgesToDelete = new HashMap<Pair<Integer,Integer>,Integer> ();
			HashMap<Pair<Integer,Integer>,Integer> edgesToAdd = new HashMap<Pair<Integer,Integer>,Integer> ();
			HashSet<CriticalSection> reversibleCS = new HashSet<CriticalSection>();

			//Make deps from un-reached stopping points
			Set<Integer> robotIDs = trackers.keySet();
			for (int robotID : robotIDs) {
				AbstractTrajectoryEnvelopeTracker robotTracker = trackers.get(robotID);
				//Update the coordinator view
				RobotReport robotReport = robotTracker.getRobotReport();
				currentReports.put(robotID, robotReport);
				synchronized(stoppingPoints) {
					if (stoppingPoints.containsKey(robotID)) {
						metaCSPLogger.info("Stopping points Robot"+robotID+": "+stoppingPoints.get(robotID).toString());
						for (int i = 0; i < stoppingPoints.get(robotID).size(); i++) {
							int stoppingPoint = stoppingPoints.get(robotID).get(i);
							int duration = stoppingTimes.get(robotID).get(i);
							if (robotReport.getPathIndex() <= stoppingPoint) {
								Dependency dep = new Dependency(robotTracker.getTrajectoryEnvelope(), null, stoppingPoint, 0);
								if (!currentDeps.containsKey(robotID)) currentDeps.put(robotID, new HashSet<Dependency>());
								if (!currentDeps.get(robotID).add(dep)) {
									metaCSPLogger.severe("<<<<<<<<< Add dependency fails (1). Dep: " + dep);
								}
							}
							//Start waiting thread if the stopping point has been reached
							//if (Math.abs(robotReport.getPathIndex()-stoppingPoint) <= 1 && robotReport.getCriticalPoint() == stoppingPoint && !stoppingPointTimers.containsKey(robotID)) {
							if (Math.abs(robotReport.getPathIndex()-stoppingPoint) <= 1 && robotReport.getCriticalPoint() <= stoppingPoint && !stoppingPointTimers.containsKey(robotID)) {
								spawnWaitingThread(robotID, i, duration);
							}
						}
					}
					synchronized(replanningStoppingPoints) {
						if (replanningStoppingPoints.containsKey(robotID)) 	{
							//We should enforce the last robot stopping point while it is involved in a re-plan.
							if (!currentDeps.containsKey(robotID)) currentDeps.put(robotID, new HashSet<Dependency>());
							currentDeps.get(robotID).add(replanningStoppingPoints.get(robotID));
							metaCSPLogger.info("Add Locking dependency to Robot" + robotID + ". Dep: " + replanningStoppingPoints.get(robotID));
						}			
					}
				}
			}



			//Make deps from critical sections, and remove obsolete critical sections
			synchronized(allCriticalSections) {

				//FIXME Add a CriticalSectionManager class
				if (allCriticalSections.size() > 0) 
					for (int robotID : robotIDs) 
						earliestStoppingPoints.put(robotID, getForwardModel(robotID).getEarliestStoppingPathIndex(trackers.get(robotID).getTrajectoryEnvelope(), currentReports.get(robotID)));

				depsToCS.clear();			
				this.isBlocked = false;

				HashSet<CriticalSection> toRemove = new HashSet<CriticalSection>();
				for (CriticalSection cs : this.allCriticalSections) {

					//Will be assigned depending on current situation of robot reports...
					int waitingPoint = -1;
					int drivingCurrentIndex = -1;
					AbstractTrajectoryEnvelopeTracker drivingTracker = null;
					AbstractTrajectoryEnvelopeTracker waitingTracker = null;

					AbstractTrajectoryEnvelopeTracker robotTracker1 = trackers.get(cs.getTe1().getRobotID());
					RobotReport robotReport1 = currentReports.get(cs.getTe1().getRobotID());
					AbstractTrajectoryEnvelopeTracker robotTracker2 = trackers.get(cs.getTe2().getRobotID());
					RobotReport robotReport2 = currentReports.get(cs.getTe2().getRobotID());

					//One or both robots past end of the critical section --> critical section is obsolete
					if (robotReport1.getPathIndex() > cs.getTe1End() || robotReport2.getPathIndex() > cs.getTe2End()) {
						toRemove.add(cs);
						metaCSPLogger.finest("Obsolete critical section\n\t" + cs);
						continue;
					}

					//If the precedence IS CONSTRAINED BY PARKED ROBOTS ...
					if (robotTracker1 instanceof TrajectoryEnvelopeTrackerDummy || robotTracker2 instanceof TrajectoryEnvelopeTrackerDummy) {

						boolean createAParkingDep = false;
						this.isBlocked = true;

						//Robot1 is parking in critical section. If it is the driver, make robot 2 wait.
						if (robotTracker1 instanceof TrajectoryEnvelopeTrackerDummy) {
							drivingCurrentIndex = robotReport1.getPathIndex();
							drivingTracker = robotTracker1;
							waitingTracker = robotTracker2;
							waitingPoint = getCriticalPoint(waitingTracker.getTrajectoryEnvelope().getRobotID(), cs, drivingCurrentIndex);

							if (!communicatedCPs.containsKey(waitingTracker) && robotReport1.getPathIndex() <= waitingPoint 
									|| communicatedCPs.containsKey(waitingTracker) && communicatedCPs.get(waitingTracker).getFirst() != -1 && communicatedCPs.get(waitingTracker).getFirst() <= waitingPoint) {
								createAParkingDep = true;
							}

						}
						else if (robotTracker2 instanceof TrajectoryEnvelopeTrackerDummy) {
							drivingCurrentIndex = robotReport2.getPathIndex();
							drivingTracker = robotTracker2;
							waitingTracker = robotTracker1;
							waitingPoint = getCriticalPoint(waitingTracker.getTrajectoryEnvelope().getRobotID(), cs, drivingCurrentIndex);

							if (!communicatedCPs.containsKey(waitingTracker) && robotReport2.getPathIndex() <= waitingPoint
									|| communicatedCPs.containsKey(waitingTracker) && communicatedCPs.get(waitingTracker).getFirst() != -1 && communicatedCPs.get(waitingTracker).getFirst() <= waitingPoint) {
								createAParkingDep = true;						
							}
						}

						if (createAParkingDep) {
							int drivingCSEnd = drivingTracker.getTrajectoryEnvelope().getRobotID() == cs.getTe1().getRobotID() ? cs.getTe1End() : cs.getTe2End();
							metaCSPLogger.finest("Robot" + drivingTracker.getTrajectoryEnvelope().getRobotID() + " is parked, so Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " will have to wait");
							if (!currentDeps.containsKey(waitingTracker.getTrajectoryEnvelope().getRobotID())) currentDeps.put(waitingTracker.getTrajectoryEnvelope().getRobotID(), new HashSet<Dependency>());
							Dependency dep = new Dependency(waitingTracker.getTrajectoryEnvelope(), drivingTracker.getTrajectoryEnvelope(), waitingPoint, drivingCSEnd);
							if (!currentDeps.get(waitingTracker.getTrajectoryEnvelope().getRobotID()).add(dep)) {
								metaCSPLogger.severe("<<<<<<<<< Add dependency fails (2). Dep: " + dep);
							}

							//If cs is new, update the of edges to add (otherwise, it is already in the graph)
							if (!CSToDepsOrder.containsKey(cs)) {
								Pair<Integer,Integer> edge = new Pair<Integer,Integer>(dep.getWaitingRobotID(), dep.getDrivingRobotID());
								if (edgesToAdd.containsKey(edge)) edgesToAdd.put(edge, edgesToAdd.get(edge)+1);
								else edgesToAdd.put(edge, 1);
							}

							//update the global graph
							if (!depsGraph.containsEdge(dep)) {
								if (!depsGraph.containsVertex(dep.getWaitingRobotID())) depsGraph.addVertex(dep.getWaitingRobotID());
								if (!depsGraph.containsVertex(dep.getDrivingRobotID())) depsGraph.addVertex(dep.getDrivingRobotID());
								if (!depsGraph.addEdge(dep.getWaitingRobotID(), dep.getDrivingRobotID(), dep))
									metaCSPLogger.severe("<<<<<<<<< Add dependency fails (3). Dep: " + dep);
							}
							CSToDepsOrder.put(cs, new Pair<Integer,Integer>(dep.getWaitingRobotID(),dep.getWaitingPoint()));
							depsToCS.put(dep, cs);
						}
					}
					else { //both the robots are driving. Check if the precedence is reversible.

						//Check if the robots can stop while changing the current order of accessing the critical section.
						boolean canStopRobot1 = false;
						boolean canStopRobot2 = false;
						boolean wakeUpinCSRobot1 = false;
						boolean wakeUpinCSRobot2 = false;


						//Force the dependency for the robot footprint
						if (//the last critical point was before the critical section (can stop by induction)
								(communicatedCPs.containsKey(robotTracker1) && communicatedCPs.get(robotTracker1).getFirst() != -1 && communicatedCPs.get(robotTracker1).getFirst() < cs.getTe1Start())
								|| !communicatedCPs.containsKey(robotTracker1) && Math.max(0, robotReport1.getPathIndex()) < cs.getTe1Start())
							canStopRobot1 = true;
						else
							//Due to temporal delays we cannot trust the velocity.
							canStopRobot1 = earliestStoppingPoints.get(robotReport1.getRobotID()) < cs.getTe1Start();
						if ((communicatedCPs.containsKey(robotTracker2) && communicatedCPs.get(robotTracker2).getFirst() != -1 && communicatedCPs.get(robotTracker2).getFirst() < cs.getTe2Start())
								|| !communicatedCPs.containsKey(robotTracker2) && Math.max(0, robotReport2.getPathIndex()) < cs.getTe2Start())
							canStopRobot2 = true;
						else canStopRobot2 = earliestStoppingPoints.get(robotReport2.getRobotID()) < cs.getTe2Start();


						//For each reversible constraint, we pre-load the precedence according to the FCFS heuristic or to the previous decided ones.
						//We will check if the heuristic after.
						if (canStopRobot1 && canStopRobot2) {
							reversibleCS.add(cs);
							boolean robot2Yields;	
							if (!CSToDepsOrder.containsKey(cs)) {
								if (robotTracker1.getStartingTimeInMillis() < robotTracker2.getStartingTimeInMillis() ||
										robotTracker1.getStartingTimeInMillis() == robotTracker2.getStartingTimeInMillis() && robotReport1.getRobotID() < robotReport2.getRobotID()) 
									robot2Yields = true;
								else robot2Yields = false;
							}
							else robot2Yields = (CSToDepsOrder.get(cs).getFirst() == robotReport2.getRobotID());
							metaCSPLogger.finest("Robot" + robotTracker1.getRobotReport().getRobotID() + ": " + robotTracker1.getStartingTimeInMillis() + ", " +
									"Robot" + robotTracker2.getRobotReport().getRobotID() + ": " + robotTracker2.getStartingTimeInMillis() + ", yielding: " + (robot2Yields ?
											robotReport2.getRobotID() : robotReport1.getRobotID()));
							drivingCurrentIndex = robot2Yields ? robotReport1.getPathIndex() : robotReport2.getPathIndex();
							drivingTracker = robot2Yields ? robotTracker1 : robotTracker2;
							waitingTracker = robot2Yields ? robotTracker2 : robotTracker1;
						}

						//otherwise, 
						//the precedence is CONSTRAINED BY DYNAMICS

						//Robot 1 can stop before entering the critical section, robot 2 can't --> robot 1 waits
						else if (canStopRobot1 && !canStopRobot2) {
							drivingCurrentIndex = robotReport2.getPathIndex();
							drivingTracker = robotTracker2;
							waitingTracker = robotTracker1;
							metaCSPLogger.finest("One-can-one-can't-stop (1) and Robot" + drivingTracker.getTrajectoryEnvelope().getRobotID() + " (can't) is ahead of Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " (can) and CS is: " + cs);						
						}

						//Robot 2 can stop before entering critical section, robot 1 can't --> robot 2 waits
						else if (!canStopRobot1 && canStopRobot2) {
							drivingCurrentIndex = robotReport1.getPathIndex();
							drivingTracker = robotTracker1;
							waitingTracker = robotTracker2;
							metaCSPLogger.finest("One-can-one-can't-stop (2) and Robot" + drivingTracker.getTrajectoryEnvelope().getRobotID() + " (can't) ahead of Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " (can) and CS is: " + cs);
						}

						else {	//Otherwise re-impose previously decided dependency if possible			

							int drivingRobotID = -1;
							int waitingRobotID = -1;

							//Handle the particular case of starting from a critical section at the FIRST communication.
							wakeUpinCSRobot1 = !communicatedCPs.containsKey(robotTracker1) && Math.max(robotReport1.getPathIndex(), 0) >= cs.getTe1Start();
							wakeUpinCSRobot2 = !communicatedCPs.containsKey(robotTracker2) && Math.max(robotReport2.getPathIndex(), 0) >= cs.getTe2Start();
							if (wakeUpinCSRobot1 || wakeUpinCSRobot2) {
								//Check if the robot that is parked can exit from critical section.
								drivingRobotID = wakeUpinCSRobot1 ? robotReport1.getRobotID() : robotReport2.getRobotID();
								waitingRobotID = wakeUpinCSRobot1 ? robotReport2.getRobotID() : robotReport1.getRobotID();	
								metaCSPLogger.finest("Robot" + robotReport1.getRobotID() + " wake up: " +wakeUpinCSRobot1 + ", Robot"+ robotReport2.getRobotID() + " wake up: " + wakeUpinCSRobot2 +" in CS " + cs);

							}
							else {
								if (this.CSToDepsOrder.containsKey(cs) && this.CSToDepsOrder.get(cs) != null) {
									//The critical section is not new and no re-plan happens.
									//Standard case: there is just one dependency in the previous set. Let's impose that.
									waitingRobotID = this.CSToDepsOrder.get(cs).getFirst();
									drivingRobotID = waitingRobotID == robotReport1.getRobotID() ? robotReport2.getRobotID() : robotReport1.getRobotID();

									//If one of the robot was parked previously in critical section and has just started driving again,
									//then the set of previous constraints contains at least one dependency (due to parking).
								}
								else {
									metaCSPLogger.severe("Both cannot stop but lost critical section to dep. CS: " + cs + ", TE: " + cs.getTe1().getID() + ", " + cs.getTe2().getID() + ".");

									//Try to recover the lost order.
									int ahead = 0;
									//Only the leading robot may be already commanded to go beyond the critical section end.
									if (communicatedCPs.containsKey(robotTracker1) && (communicatedCPs.get(robotTracker1).getFirst() == -1 || communicatedCPs.get(robotTracker1).getFirst() > cs.getTe1End())) ahead = 1;
									else if (communicatedCPs.containsKey(robotTracker2) && (communicatedCPs.get(robotTracker2).getFirst() == -1 || communicatedCPs.get(robotTracker2).getFirst() > cs.getTe2End())) ahead = -1;

									//Otherwise, if both the robots are inside the critical section check their poses.
									if (ahead == 0) ahead = isAhead(cs, robotReport1, robotReport2);

									//Otherwise, the dependency is lost. Try an error. FIXME
									if (ahead == 0) {
										if (!this.CSToDepsOrder.containsKey(cs)) throw new Error("FIXME! Lost dependency and order cannot be restored! Key value not found. RobotReport1: " + robotReport1.toString() + ", RobotReport2: " + robotReport2.toString()
										+", last communicated CPs: (" + (communicatedCPs.containsKey(robotTracker1) ? communicatedCPs.get(robotTracker1).getFirst() : "null") + "," + (communicatedCPs.containsKey(robotTracker2) ? communicatedCPs.get(robotTracker2).getFirst() : "null),") + 
										"), cs: "+ cs.toString());
										else if (this.CSToDepsOrder.get(cs) == null) throw new Error("FIXME! Lost dependency and order cannot be restored! Empty value." );	
									}
									else {
										drivingRobotID = ahead == 1 ? robotReport1.getRobotID() : robotReport2.getRobotID();
										waitingRobotID = drivingRobotID == robotReport1.getRobotID() ? robotReport2.getRobotID() : robotReport1.getRobotID();
										metaCSPLogger.info("<<<<<<<<< Restoring the lost order via estimation for Robot" + drivingRobotID);
									}					
								}			
							}

							drivingCurrentIndex = drivingRobotID == robotReport1.getRobotID() ? robotReport1.getPathIndex() : robotReport2.getPathIndex();
							waitingTracker = waitingRobotID == robotReport1.getRobotID() ? robotTracker1 : robotTracker2;
							drivingTracker = drivingRobotID == robotReport1.getRobotID() ? robotTracker1 : robotTracker2;

							//check if the driving robot can really escape
							int waitingCurrentIndex = communicatedCPs.containsKey(waitingTracker) ? 
									//((communicatedCPs.get(trackers.get(waitingRobotID)).getFirst() == -1) ? 
									//		waitingTE.getPathLength()-1 : 
									communicatedCPs.get(waitingTracker).getFirst() : (
											waitingRobotID == robotReport1.getRobotID() ? robotReport1.getPathIndex() : robotReport2.getPathIndex());
							int lastIndexOfCSDriving = drivingRobotID == cs.getTe1().getRobotID() ? cs.getTe1End() : cs.getTe2End();
							//if (lastWaitingRobotCP >= startingWaitingRobotCS) {
							if (!canExitCriticalSection(drivingCurrentIndex, waitingCurrentIndex, drivingTracker.getTrajectoryEnvelope(), waitingTracker.getTrajectoryEnvelope(),lastIndexOfCSDriving)) {
								//it's too late for escaping. Let's create a deadlock: the parked robot should wait for the moving one,
								//while the other should be commanded to do not proceed beyond its last communicated CP. Both the dependencies should be added to the current set.									

								//The parked robot should wait for the other at its current path index till the moving robot has not been exited from its critical section.
								int artWaitingPoint = communicatedCPs.containsKey(drivingTracker) ? communicatedCPs.get(drivingTracker).getFirst() :
									(drivingRobotID == robotReport1.getRobotID() ? robotReport1.getPathIndex() : robotReport2.getPathIndex());
								int artDrivingCSEnd = waitingRobotID == cs.getTe1().getRobotID() ? cs.getTe1End() : cs.getTe2End();
								Dependency dep = new Dependency(drivingTracker.getTrajectoryEnvelope(),waitingTracker.getTrajectoryEnvelope(),Math.max(0, artWaitingPoint),artDrivingCSEnd);
								if (!artificialDependencies.containsKey(drivingRobotID)) artificialDependencies.put(drivingRobotID, new HashSet<Dependency>());
								artificialDependencies.get(drivingRobotID).add(dep);
								askForReplan.add(drivingRobotID);

								//update the global graph
								if (!depsGraph.containsEdge(dep)) {
									if (!depsGraph.containsVertex(dep.getWaitingRobotID())) depsGraph.addVertex(dep.getWaitingRobotID());
									if (!depsGraph.containsVertex(dep.getDrivingRobotID())) depsGraph.addVertex(dep.getDrivingRobotID());
									if (!depsGraph.addEdge(dep.getWaitingRobotID(), dep.getDrivingRobotID(), dep))
										metaCSPLogger.severe("<<<<<<<<< Add dependency fails (5a). Dep: " + dep);
								}

								metaCSPLogger.info("Robot" + drivingRobotID + " cannot escape from CS: " + cs + ". Let's create a deadlock. Add artificial dependency for Robot" + drivingRobotID + " at " + dep.getWaitingPoint() + ".");
							}	
							metaCSPLogger.finest("Both can't. Driving Robot" + drivingRobotID + " at " + drivingCurrentIndex + " makes " + waitingRobotID + " waiting at CS " + cs + ".");
						}
						//Compute waiting path index point for waiting robot
						waitingPoint = getCriticalPoint(waitingTracker.getTrajectoryEnvelope().getRobotID(), cs, drivingCurrentIndex);

						//Impose holding the previous dependence in some cases
						if (wakeUpinCSRobot1 && communicatedCPs.containsKey(robotTracker2)) {
							if (communicatedCPs.get(robotTracker2).getFirst() > waitingPoint) {
								metaCSPLogger.info("Wake-up Robot"+robotReport1.getRobotID()+"; revising waiting point of Robot" + robotReport2.getRobotID() + ": " + waitingPoint + "-->" + communicatedCPs.get(robotTracker2).getFirst());
								waitingPoint = communicatedCPs.get(robotTracker2).getFirst();
								escapingCSToWaitingRobotIDandCP.put(cs, new Pair<Integer,Integer>(robotReport2.getRobotID(),waitingPoint));
							}
						}
						else if (wakeUpinCSRobot2 && communicatedCPs.containsKey(robotTracker1)) {
							if (communicatedCPs.get(robotTracker1).getFirst() > waitingPoint) {
								metaCSPLogger.info("Wake-up Robot"+robotReport2.getRobotID()+"; revising waiting point of Robot" + robotReport1.getRobotID() + ": " + waitingPoint + "-->" + communicatedCPs.get(robotTracker1).getFirst());
								waitingPoint = communicatedCPs.get(robotTracker1).getFirst();
								escapingCSToWaitingRobotIDandCP.put(cs, new Pair<Integer,Integer>(robotReport1.getRobotID(),waitingPoint));
							}
						}

						if (escapingCSToWaitingRobotIDandCP.containsKey(cs) && escapingCSToWaitingRobotIDandCP.get(cs).getFirst() == waitingTracker.getTrajectoryEnvelope().getRobotID()) {
							waitingPoint = escapingCSToWaitingRobotIDandCP.get(cs).getSecond();
							metaCSPLogger.info("Use escaping waiting point. Make Robot" + waitingTracker.getTrajectoryEnvelope().getRobotID() + " stopping at " + waitingPoint + ".");
						}

						if (waitingPoint >= 0) {		
							//Make new dependency
							int drivingCSEnd =  drivingTracker.getTrajectoryEnvelope().getRobotID() == cs.getTe1().getRobotID() ? cs.getTe1End() : cs.getTe2End();
							if (!currentDeps.containsKey(waitingTracker.getTrajectoryEnvelope().getRobotID())) currentDeps.put(waitingTracker.getTrajectoryEnvelope().getRobotID(), new HashSet<Dependency>());
							Dependency dep = new Dependency(waitingTracker.getTrajectoryEnvelope(), drivingTracker.getTrajectoryEnvelope(), waitingPoint, drivingCSEnd);
							if (!currentDeps.get(waitingTracker.getTrajectoryEnvelope().getRobotID()).add(dep)) {
								metaCSPLogger.severe("<<<<<<<<< Add dependency fails (4). Dep: " + dep);
							}

							//If cs is new, update the of edges to add (otherwise, it is already in the graph)
							if (!CSToDepsOrder.containsKey(cs)) {
								Pair<Integer,Integer> edge = new Pair<Integer,Integer>(dep.getWaitingRobotID(), dep.getDrivingRobotID());
								if (edgesToAdd.containsKey(edge)) edgesToAdd.put(edge, edgesToAdd.get(edge)+1);
								else edgesToAdd.put(edge,1);
							}

							//update the global graph
							if (!depsGraph.containsEdge(dep)) {
								if (!depsGraph.containsVertex(dep.getWaitingRobotID())) depsGraph.addVertex(dep.getWaitingRobotID());
								if (!depsGraph.containsVertex(dep.getDrivingRobotID())) depsGraph.addVertex(dep.getDrivingRobotID());
								if (!depsGraph.addEdge(dep.getWaitingRobotID(), dep.getDrivingRobotID(), dep))
									metaCSPLogger.severe("<<<<<<<<< Add dependency fails (5). Dep: " + dep);
							}
							CSToDepsOrder.put(cs, new Pair<Integer,Integer>(dep.getWaitingRobotID(), dep.getWaitingPoint()));
							depsToCS.put(dep, cs);
						}
						else {
							//If robot is asked to wait in an invalid path point, throw error and give up!
							metaCSPLogger.severe("Waiting point < 0 for critical section " + cs);
							throw new Error("Waiting point < 0 for critical section " + cs);
						}

					}
				}

				//Remove obsolete critical sections
				for (CriticalSection cs : toRemove) {
					this.allCriticalSections.remove(cs);
					if (CSToDepsOrder.containsKey(cs)) {
						int waitingRobID = CSToDepsOrder.get(cs).getFirst();
						int drivingRobID = cs.getTe1().getRobotID() == waitingRobID ? cs.getTe2().getRobotID() : cs.getTe1().getRobotID(); 
						Pair<Integer,Integer> edge = new Pair<Integer,Integer>(waitingRobID, drivingRobID);
						if (edgesToDelete.containsKey(edge)) edgesToDelete.put(edge, edgesToDelete.get(edge)+1);
						else edgesToDelete.put(edge,1);
					}
					else metaCSPLogger.info("WARNING: Obsolete critical section " + cs + " was not assigned to a dependency.");
					this.escapingCSToWaitingRobotIDandCP.remove(cs);
				}

				//increment the counter
				this.criticalSectionCounter.addAndGet(toRemove.size());		

				//update graph with all the not reversible constraints
				updateGraph(edgesToDelete, edgesToAdd);
				metaCSPLogger.finest("Constrained precedences leads to graph: " + currentOrdersGraph.toString() + ".");		

				//Let's try to reverse reversible constraints according to the user defined heuristic if this preserves liveness.		

				/*/////////////////////////////
						CHECK HEURISTIC
				/////////////////////////////*/
				for (CriticalSection cs : reversibleCS) {

					//check each edge one by one
					edgesToDelete.clear();
					edgesToAdd.clear();

					AbstractTrajectoryEnvelopeTracker robotTracker1 = trackers.get(cs.getTe1().getRobotID());
					RobotReport robotReport1 = currentReports.get(cs.getTe1().getRobotID());
					AbstractTrajectoryEnvelopeTracker robotTracker2 = trackers.get(cs.getTe2().getRobotID());
					RobotReport robotReport2 = currentReports.get(cs.getTe2().getRobotID());

					boolean robot2Yields = getOrder(robotTracker1, robotReport1, robotTracker2, robotReport2, cs);
					//true if robot1 should go before robot2, false vice versa
					boolean robot2YieldsOld = CSToDepsOrder.get(cs).getFirst() == robotReport2.getRobotID();

					if (robot2YieldsOld != robot2Yields) {
						metaCSPLogger.finest("Trying reversing a precedence at critical section " + cs + ".");

						//try reversing the order
						int drivingCurrentIndex = robot2Yields ? robotReport1.getPathIndex() : robotReport2.getPathIndex();
						AbstractTrajectoryEnvelopeTracker drivingTracker = robot2Yields ? robotTracker1 : robotTracker2;
						AbstractTrajectoryEnvelopeTracker waitingTracker = robot2Yields ? robotTracker2 : robotTracker1;
						int waitingPoint = getCriticalPoint(waitingTracker.getTrajectoryEnvelope().getRobotID(), cs, drivingCurrentIndex);

						//Update graphs
						if (waitingPoint >= 0) {		

							//Store previous graph (FIXME)
							DirectedMultigraph<Integer,Dependency> backupDepsGraph = new DirectedMultigraph<Integer, Dependency>(Dependency.class);
							for (int v : depsGraph.vertexSet()) backupDepsGraph.addVertex(v);
							for (Dependency dep : depsGraph.edgeSet()) {
								if (!backupDepsGraph.addEdge(dep.getWaitingRobotID(), dep.getDrivingRobotID(), dep)) 
									metaCSPLogger.severe("<<<<<<<<< Add dependency fails (6). Dep: " + dep);
							}

							SimpleDirectedWeightedGraph<Integer, DefaultWeightedEdge> backupGraph = new SimpleDirectedWeightedGraph<Integer, DefaultWeightedEdge>(DefaultWeightedEdge.class);
							for (int v : currentOrdersGraph.vertexSet()) backupGraph.addVertex(v);
							for (DefaultWeightedEdge e : currentOrdersGraph.edgeSet()) {
								DefaultWeightedEdge e_ = backupGraph.addEdge(currentOrdersGraph.getEdgeSource(e), currentOrdersGraph.getEdgeTarget(e));
								if (e_ == null) metaCSPLogger.severe("<<<<<<<<< Add egde fails (7). Edge: " + e.toString());
								backupGraph.setEdgeWeight(e_, currentOrdersGraph.getEdgeWeight(e));
							}

							HashMap<Pair<Integer, Integer>, HashSet<ArrayList<Integer>>> backupcurrentCyclesList = new HashMap<Pair<Integer, Integer>, HashSet<ArrayList<Integer>>>();
							for (Pair<Integer,Integer> key : currentCyclesList.keySet())
								backupcurrentCyclesList.put(key, new HashSet<ArrayList<Integer>>(currentCyclesList.get(key)));

							edgesToDelete.put(new Pair<Integer,Integer>(drivingTracker.getTrajectoryEnvelope().getRobotID(), waitingTracker.getTrajectoryEnvelope().getRobotID()), 1);
							Pair<Integer,Integer> newEdge = new Pair<Integer,Integer>(waitingTracker.getTrajectoryEnvelope().getRobotID(), drivingTracker.getTrajectoryEnvelope().getRobotID());
							edgesToAdd.put(newEdge, 1);
							updateGraph(edgesToDelete,edgesToAdd);
							metaCSPLogger.finest("Graph after revising according to the heuristic: " + currentOrdersGraph.toString() + ".");		

							//Make new dependency
							int drivingCSEnd =  drivingTracker.getTrajectoryEnvelope().getRobotID() == cs.getTe1().getRobotID() ? cs.getTe1End() : cs.getTe2End();
							int drivingCSEndOld =  drivingTracker.getTrajectoryEnvelope().getRobotID()  == cs.getTe1().getRobotID() ? cs.getTe2End() : cs.getTe1End();
							Dependency depNew = new Dependency(waitingTracker.getTrajectoryEnvelope(), drivingTracker.getTrajectoryEnvelope(), waitingPoint, drivingCSEnd);
							Dependency depOld = new Dependency(drivingTracker.getTrajectoryEnvelope(), waitingTracker.getTrajectoryEnvelope(), CSToDepsOrder.get(cs).getSecond(), drivingCSEndOld);

							//update the global graph and treeset
							depsGraph.removeEdge(depOld);
							if (!depsGraph.containsVertex(depNew.getWaitingRobotID())) depsGraph.addVertex(depNew.getWaitingRobotID());
							if (!depsGraph.containsVertex(depNew.getDrivingRobotID())) depsGraph.addVertex(depNew.getDrivingRobotID());
							if (!depsGraph.addEdge(depNew.getWaitingRobotID(), depNew.getDrivingRobotID(), depNew))
								metaCSPLogger.severe("<<<<<<<<< Add dependency fails (10). Dep: " + depNew);

							//check if safe (continue here) 
							boolean safe = true;
							if (currentCyclesList.containsKey(newEdge)) {
								//for each cycle involving the new edge
								for (List<Integer> cycle : currentCyclesList.get(newEdge)) {

									//Get edges along the cycle...
									//  Recall: there could be more than one edge per pair of vertices, as this is a multi-graph
									ArrayList<ArrayList<Dependency>> edgesAlongCycle = new ArrayList<ArrayList<Dependency>>();
									for (int i = 0; i < cycle.size(); i++) {
										int j = i < cycle.size()-1 ? i+1 : 0;
										if (cycle.get(i) == depNew.getWaitingRobotID() && cycle.get(j) == depNew.getDrivingRobotID()) {
											ArrayList<Dependency> array = new ArrayList<Dependency>();
											array.add(depNew);
											edgesAlongCycle.add(array);
										}
										else {
											Set<Dependency> allEdges = depsGraph.getAllEdges(cycle.get(i), cycle.get(j));
											if (allEdges == null) {
												metaCSPLogger.severe("<<<<<<<<< Unfound one vertex between " + cycle.get(i) + " and "+ cycle.get(j));
												//metaCSPLogger.info(depsGraph.toString());
												//metaCSPLogger.info(currentCyclesList.toString());
											}
											else {
												if (allEdges.isEmpty()) {
													metaCSPLogger.severe("<<<<<<<<< Unfound deps from: " + cycle.get(i) + " to: "+ cycle.get(j) + " when reversing dep  " + depOld + " to " + depNew);
													metaCSPLogger.severe("currentCyclesList: " + currentCyclesList.get(newEdge).toString());
													metaCSPLogger.severe("currentOrdersGraph contains it? " + currentOrdersGraph.containsEdge(cycle.get(i), cycle.get(j)));
													metaCSPLogger.severe("backupDepsGraph contains it? " + backupDepsGraph.containsEdge(cycle.get(i), cycle.get(j)));
													metaCSPLogger.severe("backupGraph contains it? " + backupGraph.containsEdge(cycle.get(i), cycle.get(j)));
													boolean got = false;
													if (currentDeps.containsKey(cycle.get(i))) {
														for (Dependency dep1 : currentDeps.get(cycle.get(i))) {
															if (dep1.getDrivingRobotID() == cycle.get(j)) {
																got = true;
																break;
															}
														}
													}
													metaCSPLogger.severe("currentDeps contains it? " + got);
												}
												edgesAlongCycle.add(new ArrayList<Dependency>(allEdges));
											}
										}
									}

									//Get all the possible permutation of edges
									int maxSize = 0;
									for (ArrayList<Dependency> oneDepList : edgesAlongCycle) if (oneDepList.size() > maxSize) maxSize = oneDepList.size();
									PermutationsWithRepetition gen = new PermutationsWithRepetition(maxSize, edgesAlongCycle.size());
									int[][] v = gen.getVariations();
									ArrayList<ArrayList<Dependency>> newDeps = new ArrayList<ArrayList<Dependency>>();
									for (int k = 0; k < v.length; k++) {
										int[] oneComb = v[k];
										ArrayList<Dependency> oneSelection = new ArrayList<Dependency>();
										for (int i = 0; i < oneComb.length; i++) {
											if (oneComb[i] < edgesAlongCycle.get(i).size()) oneSelection.add(edgesAlongCycle.get(i).get(oneComb[i]));
											else {
												oneSelection = null;
												break;
											}
										}
										if (oneSelection != null) newDeps.add(oneSelection);
									}

									//Check for nonlive cycles, that is:
									//  All along the cycle, check if there are (u,v,dep1) and (v,w,dep2) such that
									//    v.dep2.waitingpoint <= v.dep1.releasingpoint

									for (ArrayList<Dependency> depList : newDeps) {
										for (int i = 0; i < depList.size(); i++) {
											safe = true;
											Dependency dep1 = depList.get(i);
											Dependency dep2 = depList.get(i < depList.size()-1 ? i+1 : 0);
											if (nonlivePair(dep1,dep2)) safe = false;
											if (safe) break; //if one pair in the cycle is safe, then the cycle is safe
										}
										//  If there exists almost one nonlive cycle, then the precedence cannot be reversed.	
										if(!safe) {
											metaCSPLogger.finest("A nonlive cycle: " + depList + ".");
											break;
										}
									}

									if(!safe) {
										metaCSPLogger.finest("Cycle: " + edgesAlongCycle + " is NOT deadlock-free");
										break;
									}
								}
							}

							if (!safe) {								
								metaCSPLogger.finest("Restore previous precedence " + depOld + ".");
								depsGraph = backupDepsGraph;
								currentOrdersGraph = backupGraph;
								currentCyclesList = backupcurrentCyclesList;
								nonliveStatesAvoided.incrementAndGet();
							}
							else {
								//update the maps
								if (!currentDeps.get(depOld.getWaitingRobotID()).remove(depOld)) metaCSPLogger.severe("<<<<<<<< Error in removing dep: " + depOld);
								if (currentDeps.get(depOld.getWaitingRobotID()).isEmpty()) currentDeps.remove(depOld.getWaitingRobotID());
								if (!currentDeps.containsKey(waitingTracker.getTrajectoryEnvelope().getRobotID())) currentDeps.put(waitingTracker.getTrajectoryEnvelope().getRobotID(), new HashSet<Dependency>());
								if (!currentDeps.get(waitingTracker.getTrajectoryEnvelope().getRobotID()).add(depNew)) {
									metaCSPLogger.severe("<<<<<<<<< Add dependency fails (11). Dep: " + depNew);
								}
								CSToDepsOrder.put(cs, new Pair<Integer,Integer>(depNew.getWaitingRobotID(), depNew.getWaitingPoint()));
								depsToCS.put(depNew, cs);
								metaCSPLogger.finest("Update precedences " + depNew + " according to heuristic.");
								currentOrdersHeurusticallyDecided.incrementAndGet();
							}
							metaCSPLogger.finest("Final graph: " + currentOrdersGraph.toString() + ".");	

						}
					}
				}//end for reversibleCS*/
			}//end synchronized(allCriticalSections)

			//update and communicate critical points
			//get current dependencies
			synchronized(currentDependencies) {

				//FIXME: already synchronized (so maybe is ok currentDependencies = computeClosestDependencies(currentDeps, artificialDependencies);)
				HashMap<Integer, Dependency> closestDeps  = computeClosestDependencies(currentDeps, artificialDependencies);
				currentDependencies.clear();
				currentDependencies.putAll(closestDeps);

				//The global strategy builds upon the assumption that robots do not start from critical section. 
				//If this is not the case, them pre-loading may bring to nonlive cycles. 
				//To handle this case, switch to a local strategy whenever a robot is starting from a critical section and cannot
				//exit from it.
				for (int robotID : askForReplan) replanEnvelope(robotID, true);

				//send revised dependencies
				for (int robotID : robotIDs) sendCriticalPoint(robotID, currentReports);

				//Check if the robots are in deadlocks
				isDeadlocked();
			}

		}//end synchronized(solver)

	}//end checkAndRevise

	protected void sendCriticalPoint(int robotID, HashMap<Integer, RobotReport> currentReports) {
		AbstractTrajectoryEnvelopeTracker tracker = null;
		synchronized (trackers) {
			tracker = trackers.get(robotID);
		}
		int maxDelay = 2*(MAX_TX_DELAY+CONTROL_PERIOD+tracker.getTrackingPeriodInMillis()) + CONTROL_PERIOD; //add an extra control period to the theoretical upperbound to handle the case o equality
		boolean retransmitt = forceCriticalPointReTransmission.containsKey(robotID) && forceCriticalPointReTransmission.get(robotID);
		synchronized (currentDependencies) {
			if (currentDependencies.containsKey(robotID)) {
				Dependency dep = currentDependencies.get(robotID);
				metaCSPLogger.finest("Set critical point " + dep.getWaitingPoint() + " to Robot" + dep.getWaitingRobotID() +".");
				retransmitt = retransmitt || communicatedCPs.containsKey(tracker) && communicatedCPs.get(tracker).getFirst() == dep.getWaitingPoint() && currentReports.get(robotID).getCriticalPoint() != dep.getWaitingPoint()
						&& ((int)(Calendar.getInstance().getTimeInMillis()-communicatedCPs.get(tracker).getSecond().longValue()) > maxDelay);
				setCriticalPoint(dep.getWaitingRobotID(), dep.getWaitingPoint(), retransmitt);

			}
			else {
				retransmitt = retransmitt || communicatedCPs.containsKey(tracker) && communicatedCPs.get(tracker).getFirst() == -1 && currentReports.get(robotID).getCriticalPoint() != -1
						&& ((int)(Calendar.getInstance().getTimeInMillis()-communicatedCPs.get(tracker).getSecond().longValue()) > maxDelay);
				setCriticalPoint(robotID, -1, retransmitt);
			}
			forceCriticalPointReTransmission.put(robotID, false);
		}

	}

}//end class public class ReedsSheppCarPlanner extends AbstractMotionPlanner {
	
	public static enum PLANNING_ALGORITHM { RRTConnect, RRTstar, TRRT, SST, LBTRRT, PRMstar, SPARS, pRRT, LazyRRT }; 

	private double robotRadius = 1.0;
	private PointerByReference path = null;
	private IntByReference pathLength = null;
	private double distanceBetweenPathPoints = 0.5;
	private double turningRadius = 1.0;
	private double planningTimeInSecs = 30.0;
	private Coordinate[] collisionCircleCenters = null;
	private PLANNING_ALGORITHM algo = PLANNING_ALGORITHM.RRTConnect;
	
	public static ReedsSheppCarPlannerLib INSTANCE = null;
	static {
		NativeLibrary.addSearchPath("simplereedssheppcarplanner", "SimpleReedsSheppCarPlanner");
		INSTANCE = Native.loadLibrary("simplereedssheppcarplanner", ReedsSheppCarPlannerLib.class);
	}

	@Override
	public AbstractMotionPlanner getCopy(boolean copyObstacles) {
		ReedsSheppCarPlanner ret = new ReedsSheppCarPlanner(this.algo);
		ret.setRadius(this.robotRadius);
		ret.setDistanceBetweenPathPoints(this.distanceBetweenPathPoints);
		ret.setTurningRadius(this.turningRadius);
		ret.setFootprint(this.footprintCoords);
		ret.setPlanningTimeInSecs(planningTimeInSecs);
		if (this.om != null) ret.om = new OccupancyMap(this.om, copyObstacles);
		return ret;
	}
	
	@Override
	public void setFootprint(Coordinate ... coords) {
		super.setFootprint(coords);
		GeometryFactory gf = new GeometryFactory();
		Coordinate[] newCoords = new Coordinate[coords.length+1];
		for (int i = 0; i < coords.length; i++) {
			newCoords[i] = coords[i];
		}
		newCoords[newCoords.length-1] = coords[0];
		Polygon footprint = gf.createPolygon(newCoords);
		GeometrySmoother gs = new GeometrySmoother(gf);
		SmootherControl sc = new SmootherControl() {
	        public double getMinLength() {
	            return robotRadius;
	        }	        
	        public int getNumVertices(double length) {
	            return (int)(length/(2*robotRadius))+2;
	        }
	    };
	    gs.setControl(sc);
	    Polygon smoothFootprint = gs.smooth(footprint, 1.0);
		collisionCircleCenters = smoothFootprint.getCoordinates();
	}
	
	public Coordinate[] getCollisionCircleCenters() {
		return collisionCircleCenters;
	}
	
	public ReedsSheppCarPlanner() {
		this.algo = PLANNING_ALGORITHM.RRTConnect;
	}
	
	public ReedsSheppCarPlanner(PLANNING_ALGORITHM algo) {
		this.algo = algo;
	}

	public void setCirclePositions(Coordinate ... circlePositions) {
		this.collisionCircleCenters = circlePositions;
	}

	public void setRadius(double rad) {
		this.robotRadius = rad;
	}

	public void setDistanceBetweenPathPoints(double maxDistance) {
		this.distanceBetweenPathPoints = maxDistance;
	}

	public void setTurningRadius(double rad) {
		this.turningRadius = rad;
	}
	
	public void setPlanningTimeInSecs(double planningTimeInSecs) {
		this.planningTimeInSecs = planningTimeInSecs;
	}
	
	public Pose getStart() {
		return this.start;
	}
	
	public Pose[] getGoals() {
		return this.goal;
	}
	
	public double getPlanningTimeInSecs() {
		return this.planningTimeInSecs;
	}
	
	@Override
	public boolean doPlanning() {
		ArrayList<PoseSteering> finalPath = new ArrayList<PoseSteering>();  
		for (int i = 0; i < this.goal.length; i++) {
			Pose start_ = null;
			Pose goal_ = this.goal[i];
			if (i == 0) start_ = this.start;
			else start_ = this.goal[i-1];
			path = new PointerByReference();
			pathLength = new IntByReference();
			double[] xCoords = new double[collisionCircleCenters.length];
			double[] yCoords = new double[collisionCircleCenters.length];
			int numCoords = collisionCircleCenters.length;
			for (int j = 0; j < collisionCircleCenters.length; j++) {
				xCoords[j] = collisionCircleCenters[j].x;
				yCoords[j] = collisionCircleCenters[j].y;
			}
			metaCSPLogger.info("Path planning with " + collisionCircleCenters.length + " circle positions");
			if (this.om != null) {
				byte[] occ = om.asByteArray();
				int w = om.getPixelWidth();
				int h = om.getPixelHeight();
				double res = om.getResolution();
				double mapOriginX = om.getMapOrigin().x;
				double mapOriginY = om.getMapOrigin().y;
				if (!INSTANCE.plan_multiple_circles(occ, w, h, res, mapOriginX, mapOriginY, robotRadius, xCoords, yCoords, numCoords, start_.getX(), start_.getY(), start_.getTheta(), goal_.getX(), goal_.getY(), goal_.getTheta(), path, pathLength, distanceBetweenPathPoints, turningRadius, planningTimeInSecs, algo.ordinal())) return false;
			}
			else {
				if (!INSTANCE.plan_multiple_circles_nomap(xCoords, yCoords, numCoords, start_.getX(), start_.getY(), start_.getTheta(), goal_.getX(), goal_.getY(), goal_.getTheta(), path, pathLength, distanceBetweenPathPoints, turningRadius, planningTimeInSecs, algo.ordinal())) return false;					
			}
			final Pointer pathVals = path.getValue();
			final PathPose valsRef = new PathPose(pathVals);
			valsRef.read();
			int numVals = pathLength.getValue();
			if (numVals == 0) return false;
			PathPose[] pathPoses = (PathPose[])valsRef.toArray(numVals);
			if (i == 0) finalPath.add(new PoseSteering(pathPoses[0].x, pathPoses[0].y, pathPoses[0].theta, 0.0));
			for (int j = 1; j < pathPoses.length; j++) finalPath.add(new PoseSteering(pathPoses[j].x, pathPoses[j].y, pathPoses[j].theta, 0.0));
			INSTANCE.cleanupPath(pathVals);
		}
		this.pathPS = finalPath.toArray(new PoseSteering[finalPath.size()]);
		return true;
	}

} public class TrajectoryEnvelopeCoordinatorSimulation extends TrajectoryEnvelopeCoordinator {

	protected static final long START_TIME = Calendar.getInstance().getTimeInMillis();
	protected boolean useInternalCPs = true;
	
	protected boolean checkCollisions = false;
	protected ArrayList<CollisionEvent> collisionsList = new ArrayList<CollisionEvent>();
	protected Thread collisionThread = null;
	
	protected AtomicInteger totalMsgsLost = new AtomicInteger(0);
	protected AtomicInteger totalPacketsLost = new AtomicInteger(0);
	
	protected double DEFAULT_MAX_VELOCITY;
	protected double DEFAULT_MAX_ACCELERATION;
	
	/**
	 * The default footprint used for robots if none is specified.
	 * NOTE: coordinates in footprints must be given in in CCW or CW order. 
	 */
	public static Coordinate[] DEFAULT_FOOTPRINT = new Coordinate[] {
			new Coordinate(-1.7, 0.7),	//back left
			new Coordinate(-1.7, -0.7),	//back right
			new Coordinate(2.7, -0.7),	//front right
			new Coordinate(2.7, 0.7)	//front left
	};

	/**
	 * Dimension of the default footprint.
	 */
	public static double MAX_DEFAULT_FOOTPRINT_DIMENSION = 4.4;
	
	/**
	 * Enable the collision checking thread.
	 * @param enable <code>true</code>  if the thread for checking collisions should be enabled.
	 */
	public void setCheckCollisions(boolean enable) {
		this.checkCollisions = enable;
	}
		
	/** 
	 * Just for statistic purposes (simulation).
	 */
	public void incrementLostMsgsCounter() {
		this.totalMsgsLost.incrementAndGet();
	}
	
	/** 
	 * Just for statistic purposes (simulation).
	 */
	public void incrementLostPacketsCounter() {
		this.totalPacketsLost.incrementAndGet();
	}
	
	/**
	 * Create a new {@link TrajectoryEnvelopeCoordinatorSimulation} with given parameters.
	 * @param CONTROL_PERIOD The control period of the coordinator (e.g., 1000 msec)
	 * @param TEMPORAL_RESOLUTION The temporal resolution at which the control period is specified (e.g., 1000)
	 * @param MAX_VELOCITY The maximum speed of a robot (used to appropriately instantiate the {@link TrajectoryEnvelopeTrackerRK4} instances).
	 * @param MAX_ACCELERATION The maximum acceleration/deceleration of a robot (used to appropriately instantiate the {@link TrajectoryEnvelopeTrackerRK4} instances).
	 * @param DEFAULT_ROBOT_TRACKING_PERIOD The default tracking period in milliseconds (used to appropriately instantiate the {@link TrajectoryEnvelopeTrackerRK4} instances).
	 */
	public TrajectoryEnvelopeCoordinatorSimulation(int CONTROL_PERIOD, double TEMPORAL_RESOLUTION, double MAX_VELOCITY, double MAX_ACCELERATION, int DEFAULT_ROBOT_TRACKING_PERIOD) {
		super(CONTROL_PERIOD, TEMPORAL_RESOLUTION);
		this.DEFAULT_MAX_VELOCITY = MAX_VELOCITY;
		this.DEFAULT_MAX_ACCELERATION = MAX_ACCELERATION;
		this.DEFAULT_ROBOT_TRACKING_PERIOD = DEFAULT_ROBOT_TRACKING_PERIOD;
	}
	
	/**
	 * Create a new {@link TrajectoryEnvelopeCoordinatorSimulation} with the following default values:
	 * <ul>
	 * <li><code>CONTROL_PERIOD</code> = 1000</li>
	 * <li><code>TEMPORAL_RESOLUTION</code> = 1000</li>
	 * <li><code>DEFAULT_ROBOT_TRACKING_PERIOD</code> = 30</li>
	 * <li><code>PARKING_DURATION</code> = 3000</li>
	 * </ul>
	 */
	public TrajectoryEnvelopeCoordinatorSimulation(int CONTROL_PERIOD, double TEMPORAL_RESOLUTION, double MAX_VELOCITY, double MAX_ACCELERATION) {
		this(CONTROL_PERIOD, TEMPORAL_RESOLUTION, MAX_VELOCITY, MAX_ACCELERATION, 30);
	}

	/**
	 * Create a new {@link TrajectoryEnvelopeCoordinatorSimulation} with the following default values:
	 * <ul>
	 * <li><code>CONTROL_PERIOD</code> = 1000</li>
	 * <li><code>TEMPORAL_RESOLUTION</code> = 1000</li>
	 * <li><code>DEFAULT_ROBOT_TRACKING_PERIOD</code> = 30</li>
	 * <li><code>PARKING_DURATION</code> = 3000</li>
	 * </ul>
	 */
	public TrajectoryEnvelopeCoordinatorSimulation(double MAX_VELOCITY, double MAX_ACCELERATION) {
		this(1000, 1000, MAX_VELOCITY, MAX_ACCELERATION, 30);
	}
	
	/**
	 * Create a new {@link TrajectoryEnvelopeCoordinatorSimulation} with the following default values:
	 * <ul>
	 * <li><code>CONTROL_PERIOD</code> = 1000</li>
	 * <li><code>TEMPORAL_RESOLUTION</code> = 1000</li>
	 * <li><code>MAX_VELOCITY</code> = 10.0</li>
	 * <li><code>MAX_ACCELERATION</code> = 1.0</li>
	 * <li><code>DEFAULT_ROBOT_TRACKING_PERIOD</code> = 30</li>
	 * <li><code>PARKING_DURATION</code> = 3000</li>
	 * </ul>
	 */
	public TrajectoryEnvelopeCoordinatorSimulation() {
		this(1000, 1000, 10.0, 1.0, 30);
	}

	private ArrayList<Integer> computeStoppingPoints(PoseSteering[] poses) {
		ArrayList<Integer> ret = new ArrayList<Integer>();
		double prevTheta = poses[0].getTheta();
		if (poses.length > 1) prevTheta = Math.atan2(poses[1].getY() - poses[0].getY(), poses[1].getX() - poses[0].getX());
		for (int i = 0; i < poses.length-1; i++) {
			double theta = Math.atan2(poses[i+1].getY() - poses[i].getY(), poses[i+1].getX() - poses[i].getX());
			double deltaTheta = (theta-prevTheta);
			prevTheta = theta;
			if (Math.abs(deltaTheta) > Math.PI/2 && Math.abs(deltaTheta) < 1.9*Math.PI) {
				ret.add(i);
			}
		}
		return ret;
	}

	@Override
	public boolean addMissions(Mission... missions) {
		HashMap<Mission, HashMap<Pose, Integer>> userStoppingPoints = new HashMap<Mission, HashMap<Pose,Integer>>();
		if (this.useInternalCPs) {
			for (Mission m : missions) {
				PoseSteering[] path = m.getPath();
				ArrayList<Integer> sps = computeStoppingPoints(path);
				userStoppingPoints.put(m, m.getStoppingPoints());
				for (Integer i : sps) m.setStoppingPoint(path[i-1].getPose(), 100);				
			}
		}
		if (!super.addMissions(missions)) {
			if (this.useInternalCPs) {
				for (Mission m : missions) {
					m.clearStoppingPoints();
					for (Pose p : userStoppingPoints.get(m).keySet()) {
						m.setStoppingPoint(p, userStoppingPoints.get(m).get(p));
					}
				}			
			}
			return false;
		}
		return true;
	}


	/**
	 * Enable (default) or disable the use of internal critical points in the {@link TrajectoryEnvelopeTrackerRK4} trackers.
	 * @param value <code>true</code> if these critical points should be used to slow down, <code>false</code> otherwise.
	 */
	public void setUseInternalCriticalPoints(boolean value) {
		this.useInternalCPs = value;
	}
		
	/**
	 * Get the maximum velocity of a given robot (m/s).
	 * @param robotID The ID of the robot.
	 * @return The maximum velocity of the robot (or the default value if not specified).
	 */
	@Override
	public Double getRobotMaxVelocity(int robotID) {
		if (this.robotMaxVelocity.containsKey(robotID)) 
			return this.robotMaxVelocity.get(robotID);
		return DEFAULT_MAX_VELOCITY;
	}
	
	/**
	 * Get the maximum acceleration of a given robot (m/s^2).
	 * @param robotID The ID of the robot.
	 * @return The maximum acceleration of the robot (or the default value if not specified).
	 */
	@Override
	public Double getRobotMaxAcceleration(int robotID) {
		if (this.robotMaxAcceleration.containsKey(robotID)) 
			return this.robotMaxAcceleration.get(robotID);
		return DEFAULT_MAX_ACCELERATION;
	}
	
	@Override
	protected Double getMaxFootprintDimension(int robotID) {
		if (this.footprints.containsKey(robotID)) return maxFootprintDimensions.get(robotID);
		return MAX_DEFAULT_FOOTPRINT_DIMENSION;
	}

	/**
	 * Get the {@link Coordinate}s defining the default footprint of robots.
	 * @return The {@link Coordinate}s defining the default footprint of robots.
	 */
	public Coordinate[] getDefaultFootprint() {
		return DEFAULT_FOOTPRINT;
	}

	/**
	 * Get the {@link Coordinate}s defining the footprint of a given robot.
	 * @param robotID the ID of the robot
	 * @return The {@link Coordinate}s defining the footprint of a given robot.
	 */
	@Override
	public Coordinate[] getFootprint(int robotID) {
		if (this.footprints.containsKey(robotID)) return this.footprints.get(robotID);
		return DEFAULT_FOOTPRINT;
	}
	
	/**
	 * Get a {@link Geometry} representing the default footprint of robots.
	 * @return A {@link Geometry} representing the default footprint of robots.
	 */
	public Geometry getDefaultFootprintPolygon() {
		Geometry fpGeom = TrajectoryEnvelope.createFootprintPolygon(DEFAULT_FOOTPRINT);
		return fpGeom;
	}
	
	/**
	 * Set the default footprint of robots, which is used for computing spatial envelopes.
	 * Provide the bounding polygon of the machine assuming its reference point is in (0,0), and its
	 * orientation is aligned with the x-axis. The coordinates must be in CW or CCW order.
	 * @param coordinates The coordinates delimiting bounding polygon of the footprint.
	 */
	public void setDefaultFootprint(Coordinate ... coordinates) {
		DEFAULT_FOOTPRINT = coordinates;
		MAX_DEFAULT_FOOTPRINT_DIMENSION = computeMaxFootprintDimension(coordinates);
	}

	@Override
	public AbstractTrajectoryEnvelopeTracker getNewTracker(TrajectoryEnvelope te, TrackingCallback cb) {
		if (this.getRobotTrackingPeriodInMillis(te.getRobotID()) == null || this.getRobotMaxVelocity(te.getRobotID()) == null || this.getRobotMaxAcceleration(te.getRobotID()) == null) throw new Error("Robot" +  te.getRobotID() + ": missing kinodynamic parameters.");
		TrajectoryEnvelopeTrackerRK4 ret = new TrajectoryEnvelopeTrackerRK4(te, this.getRobotTrackingPeriodInMillis(te.getRobotID()), TEMPORAL_RESOLUTION, this.getRobotMaxVelocity(te.getRobotID()), this.getRobotMaxAcceleration(te.getRobotID()), this, cb) {

			//Method for measuring time in the trajectory envelope tracker
			@Override
			public long getCurrentTimeInMillis() {
				return tec.getCurrentTimeInMillis();
			}
		};
		//ret.setUseInternalCriticalPoints(this.useInternalCPs);
		ret.setUseInternalCriticalPoints(false);
		return ret;
	}

	//Method for measuring time in the trajectory envelope coordinator
	@Override
	public long getCurrentTimeInMillis() {
		return Calendar.getInstance().getTimeInMillis()-START_TIME;
	}
	
	@Override
	protected String[] getStatistics() {
		
		String CONNECTOR_BRANCH = (char)0x251C + "" + (char)0x2500 + " ";
		String CONNECTOR_LEAF = (char)0x2514 + "" + (char)0x2500 + " ";
		ArrayList<String> ret = new ArrayList<String>();
		int numVar = solver.getConstraintNetwork().getVariables().length;
		int numCon = solver.getConstraintNetwork().getConstraints().length;
		
		synchronized (trackers) {
			ret.add("Status @ "  + getCurrentTimeInMillis() + " ms");
			ret.add(CONNECTOR_BRANCH + 		"Eff period .......... " + EFFECTIVE_CONTROL_PERIOD + " ms");
			ret.add(CONNECTOR_BRANCH + 		"Constraint network .. " + numVar + " variables, " + numCon + " constraints");
			HashSet<Integer> allRobots = new HashSet<Integer>();
			for (Integer robotID : trackers.keySet()) {
				allRobots.add(robotID);
			}
			String st = CONNECTOR_BRANCH +	"Robot states ........ ";
			for (Integer robotID : allRobots) {
				AbstractTrajectoryEnvelopeTracker tracker = trackers.get(robotID);
				RobotReport rr = tracker.getRobotReport(); 
				int currentPP = rr.getPathIndex();
				st += tracker.getTrajectoryEnvelope().getComponent();
				if (tracker instanceof TrajectoryEnvelopeTrackerDummy) st += " (P)";
				else st += " (D)";
				st += ": " + currentPP + "   ";
			}
			ret.add(st);
			st = CONNECTOR_BRANCH + 		"Robots' view ........ ";
			for (Integer robotID : allRobots) {
				AbstractTrajectoryEnvelopeTracker tracker = trackers.get(robotID);
				RobotReport rr = getRobotReport(robotID); 
				if (rr != null ) {
					int currentPP = rr.getPathIndex();
					st += tracker.getTrajectoryEnvelope().getComponent();
					if (tracker instanceof TrajectoryEnvelopeTrackerDummy) st += " (P)";
					else st += " (D)";
					st += ": " + currentPP + "   ";
				}
			}
			ret.add(st);
		}
		synchronized (currentDependencies) {
			ret.add(CONNECTOR_BRANCH + 		"Dependencies ........ " + currentDependencies);
		}
		if (checkCollisions) {
			int numberOfCollisions = 0;
			synchronized (collisionsList) {
				numberOfCollisions = collisionsList.size();		
				ret.add(CONNECTOR_BRANCH + 	"Collisions .......... " + numberOfCollisions + ".");
				if (numberOfCollisions>0) {
					for (int cindex = 0; cindex < collisionsList.size()-1; cindex++) {
						CollisionEvent ce = collisionsList.get(cindex);
						ret.add(" " + CONNECTOR_BRANCH + " " + ce.toString());
					}
					CollisionEvent ce = collisionsList.get(collisionsList.size()-1);
					ret.add(" " + CONNECTOR_LEAF + " " + ce.toString());
				}
			}
			
		}
		ret.add(CONNECTOR_BRANCH + 			"Obsolete crit sect .. " + criticalSectionCounter.get() + ".");
		ret.add(CONNECTOR_BRANCH + 			"Messages sent ....... " + totalMsgsSent.get() + ", lost: " + totalMsgsLost.get() + ", retransmitted: " + totalMsgsReTx.get() + ". Packets lost: " + totalPacketsLost.get() + ", number of replicas: " + numberOfReplicas + ".");
		ret.add(CONNECTOR_BRANCH + 			"Unalive states ...... " + nonliveStatesDetected.get() + ", avoided: " + nonliveStatesAvoided.get() + ", revised according to heuristic: " + currentOrdersHeurusticallyDecided.get() + ".");
		ret.add(CONNECTOR_LEAF + 			"Re-planned paths .... " + replanningTrialsCounter.get() + ", successful: " + successfulReplanningTrialsCounter.get() + ".");
		return ret.toArray(new String[ret.size()]);
	}
	
	@Override
	public void onCriticalSectionUpdate() {
		
		if (checkCollisions && (collisionThread == null) && (this.allCriticalSections.size() > 0)) {
			// Start the collision checking thread. 
			// The tread will be alive until there will be almost one critical section.
			collisionThread = new Thread("Collision checking thread.") {

				@Override
				public void run() {
					metaCSPLogger.info("Starting the collision checking thread.");
					ArrayList<CriticalSection> previousCollidingCS = new ArrayList<CriticalSection>();
					ArrayList<CriticalSection> newCollidingCS = new ArrayList<CriticalSection>();
					
					while(true) {
						newCollidingCS.clear();
						
						//collisions can happen only in critical sections						
						synchronized (allCriticalSections) {
							if (allCriticalSections.isEmpty())
								break; //break the thread if there are no critical sections to control
							
							for (CriticalSection cs : allCriticalSections) {
								//check if both the robots are inside the critical section
								
								//FIXME sample the real pose of the robots (ok in simulation, but not otherwise)
								RobotReport robotReport1, robotReport2;
								AbstractTrajectoryEnvelopeTracker tracker1, tracker2;
								try {
									synchronized (trackers)	{
										tracker1 = trackers.get(cs.getTe1().getRobotID());
										robotReport1 = tracker1.getRobotReport();
										tracker2 = trackers.get(cs.getTe2().getRobotID());
										robotReport2 = tracker2.getRobotReport();
									}
								}
								catch (NullPointerException e) {
									continue; //skip this cycle
								}
								
								if ( robotReport1 != null && robotReport2 != null &&
								(robotReport1.getPathIndex() <= cs.getTe1End()) && (robotReport1.getPathIndex() >= cs.getTe1Start()) && //robot1 is inside
								(robotReport2.getPathIndex() <= cs.getTe2End()) && (robotReport2.getPathIndex() >= cs.getTe2Start())  	//robot2 is inside
								) {
									//place robot  in pose and get geometry
									PoseSteering[] path1 = cs.getTe1().getTrajectory().getPoseSteering();
									Geometry placement1 = cs.getTe1().makeFootprint(path1[robotReport1.getPathIndex()]);
									
									PoseSteering[] path2 = cs.getTe2().getTrajectory().getPoseSteering();
									Geometry placement2 = cs.getTe2().makeFootprint(path2[robotReport2.getPathIndex()]);
									
									//check intersection
									if (placement1.intersects(placement2)) {
										if (!previousCollidingCS.contains(cs)) {
											metaCSPLogger.info(" * NEW COLLISION *");
											CollisionEvent ce = new CollisionEvent(Calendar.getInstance().getTimeInMillis(),robotReport1,robotReport2);
											synchronized (collisionsList) {
												collisionsList.add(ce);
											}		
											newCollidingCS.add(cs);
										}
									}
									else if (previousCollidingCS.contains(cs))
										previousCollidingCS.remove(cs); //remove the ones that are not colliding anymore
								}
							}
						}
						previousCollidingCS.addAll(newCollidingCS);
						
						try { Thread.sleep((long)(1000.0/30.0)); }
						catch (InterruptedException e) { e.printStackTrace(); }
					}
					metaCSPLogger.info("Ending the collision checking thread.");
				}
					
			};
			collisionThread.start();
		}
	}
} public class BrowserVisualization implements FleetVisualization {
	
	private ArrayList<String> msgQueue = new ArrayList<String>();
	private static int UPDATE_PERIOD = 30;
	private double robotFootprintArea = -1;
	private double robotFootprintXDim = -1;
	private String overlayText = null;

	public BrowserVisualization() {
		this("localhost", 30);
	}

	public BrowserVisualization(String serverHostNameOrIP) {
		this(serverHostNameOrIP, 30);
	}

	public BrowserVisualization(int updatePeriodInMillis) {
		this("localhost", updatePeriodInMillis);
	}
	
	public BrowserVisualization(String serverHostNameOrIP, int updatePeriodInMillis) {
		UPDATE_PERIOD = updatePeriodInMillis;
		BrowserVisualization.setupVizMessageServer();
        Thread updateThread = new Thread("Visualization update thread") {
        	public void run() {
        		while (true) {
        			sendMessages();
        			try { Thread.sleep(UPDATE_PERIOD); }
        			catch (InterruptedException e) { e.printStackTrace(); }
        		}
        	}
        };
        updateThread.start();
        BrowserVisualization.setupVizServer(serverHostNameOrIP);
        startOpenInBrowser(serverHostNameOrIP);
	}
	
	private void startOpenInBrowser(String serverHostNameOrIP) {
		if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) {
			try { Desktop.getDesktop().browse(new URI("http://" + serverHostNameOrIP + ":8080")); }
			catch (IOException e) { e.printStackTrace(); }
			catch (URISyntaxException e) { e.printStackTrace(); }
		}
	}
	
	private void updateOverlayText() {
		if (this.overlayText != null) {
			String jsonString = "{ \"operation\" : \"setOverlayText\","
					+ "\"data\" : "
					+ "{ \"text\" : \""+ this.overlayText + "\" }}";
			sendMessage(jsonString);
		}
	}
	
	public void setOverlayText(String text) {
		this.overlayText = text;
	}

	private void updateRobotFootprintArea(Geometry geom) {
		if (robotFootprintArea == -1) {
			robotFootprintArea = geom.getArea();
			double minX = Double.MAX_VALUE;
			double maxX = Double.MIN_VALUE;
			for (Coordinate coord : geom.getCoordinates()) {
				if (coord.x < minX) minX = coord.x;
				if (coord.x > maxX) maxX = coord.x;
			}
			this.robotFootprintXDim = maxX-minX;
		}
	}
	
	public void setInitialTransform(double scale, double xTrans, double yTrans) {
		BrowserVisualizationSocket.initialScale = scale;
		BrowserVisualizationSocket.initialTranslation = new Coordinate(xTrans,yTrans);		
	}
	
	private static int getScreenDPI() {
		//Dimension screen = java.awt.Toolkit.getDefaultToolkit().getScreenSize();
		//System.out.println("Screen width: "+screen.getWidth()); 
		//System.out.println("Screen height: "+screen.getHeight()); 
		int pixelPerInch = java.awt.Toolkit.getDefaultToolkit().getScreenResolution(); 
		//System.out.println("DPI: " + pixelPerInch); 
		return pixelPerInch; 
	}
	
	private static double getScreenHeight() {
		Dimension screen = java.awt.Toolkit.getDefaultToolkit().getScreenSize();
		return screen.getHeight(); 
	}
	
	public void guessInitialTransform(double robotDimension, Pose ... robotPoses) {
		BrowserVisualizationSocket.initialScale = getScreenDPI()/robotDimension;
		double avgX = 0;
		double avgY = 0;
		for (int i = 0; i < robotPoses.length; i++) {
			avgX += robotPoses[i].getX();
			avgY += robotPoses[i].getY();
		}
		avgX /= robotPoses.length;
		avgY /= robotPoses.length;
		avgY -= 0.45*(getScreenHeight()/getScreenDPI());
		BrowserVisualizationSocket.initialTranslation = new Coordinate(avgX,avgY);		
	}

	private static void setupVizServer(String serverHostNameOrIP) {
		Server server = new Server(8080);
		server.setHandler(new BrowserVisualizationServer(serverHostNameOrIP));
		try {
			server.start();
			//server.join();
		}
        catch (Throwable t) { t.printStackTrace(System.err); }
	}
	
	private static void setupVizMessageServer() {
        Server server = new Server();
        ServerConnector connector = new ServerConnector(server);
        connector.setPort(8081);
        server.addConnector(connector);

        // Setup the basic application "context" for this application at "/"
        // This is also known as the handler tree (in jetty speak)
        ServletContextHandler context = new ServletContextHandler(ServletContextHandler.SESSIONS);
        context.setContextPath("/");
        server.setHandler(context);
        
        // Add a websocket to a specific path spec
        ServletHolder holderEvents = new ServletHolder("ws-events", BrowserVisualizationServlet.class);
        context.addServlet(holderEvents, "/fleet-events/*");
        
        try {
            server.start();
            server.dump(System.err);
            //server.join();
        }
        catch (Throwable t) { t.printStackTrace(System.err); }		
	}
	
	private void enqueueMessage(String message) {
		if (BrowserVisualizationSocket.ENDPOINTS != null && BrowserVisualizationSocket.ENDPOINTS.size() > 0) {
			synchronized (BrowserVisualizationSocket.ENDPOINTS) {
				this.msgQueue.add(message);
			}
		}
	}
	
	private void sendMessages() {
		if (BrowserVisualizationSocket.ENDPOINTS != null && BrowserVisualizationSocket.ENDPOINTS.size() > 0) {
			synchronized (BrowserVisualizationSocket.ENDPOINTS) {
				for (String message : this.msgQueue) {
					sendMessage(message);
				}
				msgQueue.clear();
				updateOverlayText();
				sendUpdate();
			}
		}
	}
	
	private void sendMessage(String text) {
		if (BrowserVisualizationSocket.ENDPOINTS != null) {
			for (RemoteEndpoint rep : BrowserVisualizationSocket.ENDPOINTS) {
				try {
					rep.sendString(text);
				}
				catch(IOException e) { e.printStackTrace(); }
			}
		}
	}

	@Override
	public void displayRobotState(TrajectoryEnvelope te, RobotReport rr, String... extraStatusInfo) {
		double x = rr.getPathIndex() != -1 ? rr.getPose().getX() : te.getTrajectory().getPose()[0].getX();
		double y = rr.getPathIndex() != -1 ? rr.getPose().getY() : te.getTrajectory().getPose()[0].getY();
		double theta = rr.getPathIndex() != -1 ? rr.getPose().getTheta() : te.getTrajectory().getPose()[0].getTheta();
		
		String name = "R"+te.getRobotID();
		String extraData = " : " + rr.getPathIndex();
		if (extraStatusInfo != null) {
			for (String st : extraStatusInfo) {
				extraData += (" | " + st);
			}
		}
		
		Geometry geom = TrajectoryEnvelope.getFootprint(te.getFootprint(), x, y, theta);
		this.updateRobotFootprintArea(geom);
		double scale = Math.sqrt(robotFootprintArea)*0.2;
		Geometry arrowGeom = createArrow(rr.getPose(), robotFootprintXDim/scale, scale);
		String jsonString = "{ \"operation\" : \"addGeometry\", \"data\" : " + this.geometryToJSONString(name, geom, "#ff0000", -1, true, extraData) + "}";
		String jsonStringArrow = "{ \"operation\" : \"addGeometry\", \"data\" : " + this.geometryToJSONString("_"+name, arrowGeom, "#ffffff", -1, true, null) + "}";
		enqueueMessage(jsonString);
		enqueueMessage(jsonStringArrow);
	}
	
	@Override
	public void displayRobotState(Polygon fp, RobotReport rr, String... extraStatusInfo) {
		double x = rr.getPose().getX();
		double y = rr.getPose().getY();
		double theta = rr.getPose().getTheta();
		
		String name = "R"+rr.getRobotID();
		String extraData = " : " + rr.getPathIndex();
		if (extraStatusInfo != null) {
			for (String st : extraStatusInfo) {
				extraData += (" | " + st);
			}
		}
		
		Geometry geom = TrajectoryEnvelope.getFootprint(fp, x, y, theta);
		this.updateRobotFootprintArea(geom);
		double scale = Math.sqrt(robotFootprintArea)*0.2;
		Geometry arrowGeom = createArrow(rr.getPose(), robotFootprintXDim/scale, scale);
		String jsonString = "{ \"operation\" : \"addGeometry\", \"data\" : " + this.geometryToJSONString(name, geom, "#ff0000", -1, true, extraData) + "}";
		String jsonStringArrow = "{ \"operation\" : \"addGeometry\", \"data\" : " + this.geometryToJSONString("_"+name, arrowGeom, "#ffffff", -1, true, null) + "}";
		enqueueMessage(jsonString);
		enqueueMessage(jsonStringArrow);
	}

	@Override
	public void displayDependency(RobotReport rrWaiting, RobotReport rrDriving, String dependencyDescriptor) {
		Geometry arrow = createArrow(rrWaiting.getPose(), rrDriving.getPose());
		String jsonString = "{ \"operation\" : \"addGeometry\", \"data\" : " + this.geometryToJSONString(dependencyDescriptor, arrow, "#adccff", 1000, true, null) + "}";
		enqueueMessage(jsonString);
	}
	
	private String geometryToJSONString(String name, Geometry geom, String color, long age, boolean filled, String extraData) {
		String ret = "{ \"name\" : \"" + name + "\", \"color\" : \"" + color + "\", ";
		if (age > 0) ret += " \"age\" : " + age + ", ";
		ret += " \"filled\" : " + filled + ", ";
		if (extraData != null && !extraData.trim().equals("")) ret += " \"extraData\" : \"" + extraData + "\", ";		
		ret += "\"coordinates\" : [";
		Coordinate[] coords = geom.getCoordinates();
		for (int i = 0; i < coords.length; i++) {
			ret += "{\"x\" : " + coords[i].x + ", \"y\" : " + coords[i].y + "}";
			if (i < coords.length-1) ret += ", ";
		}
		ret += "]}";
		return ret;
	}

	@Override
	public void addEnvelope(TrajectoryEnvelope te) {
		GeometricShapeDomain dom = (GeometricShapeDomain)te.getEnvelopeVariable().getDomain();
		Geometry geom = dom.getGeometry();
		String jsonString = "{ \"operation\" : \"addGeometry\", \"data\" : " + this.geometryToJSONString("_"+te.getID(), geom, "#efe007", -1, false, null) + "}";
		enqueueMessage(jsonString);
	}

	@Override
	public void removeEnvelope(TrajectoryEnvelope te) {
		String jsonString = "{ \"operation\" : \"removeGeometry\","
				+ "\"data\" : "
				+ "{ \"name\" : \""+ "_"+te.getID() +"\" }}";
		enqueueMessage(jsonString);
	}

	@Override
	public void updateVisualization() {
		// This method does nothing - reason:
		// Viz change events are buffered and sent by an internal thread
		// in bursts every UPDATE_PERIOD ms to avoid blocking of RemoteEndpopints
	}
	
	public void sendUpdate() {
		String callRefresh = "{ \"operation\" : \"refresh\" }";
		sendMessage(callRefresh);
	}
	
	private Geometry createArrow(Pose pose, double length, double size) {		
		GeometryFactory gf = new GeometryFactory();
		double aux = 1.8;
		double aux1 = 0.8;
		double aux2 = 0.3;
		double theta = pose.getTheta();
		Coordinate[] coords = new Coordinate[8];
		coords[0] = new Coordinate(0.0,-aux2);
		coords[1] = new Coordinate(length-aux,-aux2);
		coords[2] = new Coordinate(length-aux,-aux1);
		coords[3] = new Coordinate(length,0.0);
		coords[4] = new Coordinate(length-aux,aux1);
		coords[5] = new Coordinate(length-aux,aux2);
		coords[6] = new Coordinate(0.0,aux2);
		coords[7] = new Coordinate(0.0,-aux2);
		Polygon arrow = gf.createPolygon(coords);
		AffineTransformation at = new AffineTransformation();
		at.scale(size, size);
		at.rotate(theta);
		at.translate(pose.getX(), pose.getY());
		Geometry ret = at.transform(arrow);
		return ret;
	}
	
	private Geometry createArrow(Pose pose1, Pose pose2) {		
		GeometryFactory gf = new GeometryFactory();
		double aux = 1.8;
		double aux1 = 0.8;
		double aux2 = 0.3;
		double factor = Math.sqrt(robotFootprintArea)*0.5;
		double distance = Math.sqrt(Math.pow((pose2.getX()-pose1.getX()),2)+Math.pow((pose2.getY()-pose1.getY()),2))/factor;
		double theta = Math.atan2(pose2.getY() - pose1.getY(), pose2.getX() - pose1.getX());
		Coordinate[] coords = new Coordinate[8];
		coords[0] = new Coordinate(0.0,-aux2);
		coords[1] = new Coordinate(distance-aux,-aux2);
		coords[2] = new Coordinate(distance-aux,-aux1);
		coords[3] = new Coordinate(distance,0.0);
		coords[4] = new Coordinate(distance-aux,aux1);
		coords[5] = new Coordinate(distance-aux,aux2);
		coords[6] = new Coordinate(0.0,aux2);
		coords[7] = new Coordinate(0.0,-aux2);
		Polygon arrow = gf.createPolygon(coords);
		AffineTransformation at = new AffineTransformation();
		at.scale(factor, factor);
		at.rotate(theta);
		at.translate(pose1.getX(), pose1.getY());
		Geometry ret = at.transform(arrow);
		return ret;
	}

	public void setMap(BufferedImage mapImage, double resolution, Coordinate origin) {
		BrowserVisualizationSocket.map = mapImage;
		BrowserVisualizationSocket.resolution = resolution;
		BrowserVisualizationSocket.origin = origin;
	}
	
	public void setMap(String mapYAMLFile) {
		try {
			File file = new File(mapYAMLFile);
			BufferedReader br = new BufferedReader(new FileReader(file));
			String imageFileName = null;
			String st;
			//FIXME Handle map origin
			//Coordinate bottomLeftOrigin = null;
			while((st=br.readLine()) != null){ 
				if (!st.trim().startsWith("#") && !st.trim().isEmpty()) {
					String key = st.substring(0, st.indexOf(":")).trim();
					String value = st.substring(st.indexOf(":")+1).trim();
					if (key.equals("image")) imageFileName = file.getParentFile()+File.separator+value;
					else if (key.equals("resolution")) BrowserVisualizationSocket.resolution = Double.parseDouble(value);
					else if (key.equals("origin")) {
						String x = value.substring(1, value.indexOf(",")).trim();
						String y = value.substring(value.indexOf(",")+1, value.indexOf(",", value.indexOf(",")+1)).trim();
						BrowserVisualizationSocket.origin = new Coordinate(Double.parseDouble(x),Double.parseDouble(y));
						//bottomLeftOrigin = new Coordinate(Double.parseDouble(x),Double.parseDouble(y));
					}
				}
			}
			br.close();
			BrowserVisualizationSocket.map = ImageIO.read(new File(imageFileName));
			//BrowserVisualizationSocket.origin = new Coordinate(bottomLeftOrigin.x, BrowserVisualizationSocket.map.getHeight()*BrowserVisualizationSocket.resolution-bottomLeftOrigin.y);
		}
		catch (IOException e) { e.printStackTrace(); }
	}
	
	public void setMapYAML(String mapYAMLSpec, String pathPrefix) {
		try {
			String imageFileName = "";
			if (pathPrefix != null) imageFileName = pathPrefix+File.separator;
			for (String st : mapYAMLSpec.split("\n")) { 
				if (!st.trim().startsWith("#") && !st.trim().isEmpty()) {
					String key = st.substring(0, st.indexOf(":")).trim();
					String value = st.substring(st.indexOf(":")+1).trim();
					if (key.equals("image")) imageFileName += value;
					else if (key.equals("resolution")) BrowserVisualizationSocket.resolution = Double.parseDouble(value);
					else if (key.equals("origin")) {
						String x = value.substring(1, value.indexOf(",")).trim();
						String y = value.substring(value.indexOf(",")+1, value.indexOf(",", value.indexOf(",")+1)).trim();
						BrowserVisualizationSocket.origin = new Coordinate(Double.parseDouble(x),Double.parseDouble(y));
						//bottomLeftOrigin = new Coordinate(Double.parseDouble(x),Double.parseDouble(y));
					}
				}
			}
			System.out.println(imageFileName);
			BrowserVisualizationSocket.map = ImageIO.read(new File(imageFileName));
			//BrowserVisualizationSocket.origin = new Coordinate(bottomLeftOrigin.x, BrowserVisualizationSocket.map.getHeight()*BrowserVisualizationSocket.resolution-bottomLeftOrigin.y);
		}
		catch (IOException e) { e.printStackTrace(); }
	}
	
	@Override
	public int periodicEnvelopeRefreshInMillis() {
		return 1000;
	}

} public class JTSDrawingPanelVisualization implements FleetVisualization {

	private JTSDrawingPanel panel = null;

	public JTSDrawingPanelVisualization() {
		this(null);
	}
	
	public JTSDrawingPanelVisualization(String mapYAMLFile) {
		this.setupGUI(mapYAMLFile);
	}
	
	public JTSDrawingPanel getPanel() {
		return this.panel;
	}
	
	public void setSize(int width, int height) {
		JFrame topFrame = (JFrame) SwingUtilities.getWindowAncestor(this.getPanel());
		topFrame.setSize(width,height);
	}

	@Override
	public void displayRobotState(TrajectoryEnvelope te, RobotReport rr, String ... extraStatusInfo) {
		double x = rr.getPose().getX();
		double y = rr.getPose().getY();
		double theta = rr.getPose().getTheta();
		String name = "R"+te.getRobotID();
		if (extraStatusInfo != null) {
			for (String st : extraStatusInfo) {
				name += ("\\"+st);
			}
		}
		if (rr.getPathIndex() != -1) panel.addGeometry(name, TrajectoryEnvelope.getFootprint(te.getFootprint(), x, y, theta), false, true, false, "#FF0000");
		else panel.addGeometry(name, TrajectoryEnvelope.getFootprint(te.getFootprint(), te.getTrajectory().getPose()[0].getX(), te.getTrajectory().getPose()[0].getY(), te.getTrajectory().getPose()[0].getTheta()), false, true, false, "#4286F4");
	}
	
	@Override
	public void displayRobotState(Polygon fp, RobotReport rr, String ... extraStatusInfo) {
		double x = rr.getPose().getX();
		double y = rr.getPose().getY();
		double theta = rr.getPose().getTheta();
		String name = "R"+rr.getRobotID();
		if (extraStatusInfo != null) {
			for (String st : extraStatusInfo) {
				name += ("\\"+st);
			}
		}
		panel.addGeometry(name, TrajectoryEnvelope.getFootprint(fp, x, y, theta), false, true, false, "#FF0000");
	}

	@Override
	public void displayDependency(RobotReport rrWaiting, RobotReport rrDriving, String dependencyDescriptor) {
		panel.addArrow(dependencyDescriptor, rrWaiting.getPose(), rrDriving.getPose());
	}
	
	private void setupGUI(String mapYAMLFile) {
		//Show everything in a GUI (vehicle positions are updated in real time by the trackers, see below)
		this.panel = JTSDrawingPanel.makeEmpty("Current status of robots");
		//setPriorityOfEDT(Thread.MIN_PRIORITY);
		//setPriorityOfEDT(Thread.MAX_PRIORITY);
		panel.setSmoothTransitions(true);
		panel.setArrowHeadSizeInMeters(0.6*TrajectoryEnvelopeCoordinatorSimulation.MAX_DEFAULT_FOOTPRINT_DIMENSION);
		panel.setTextSizeInMeters(0.8*TrajectoryEnvelopeCoordinatorSimulation.MAX_DEFAULT_FOOTPRINT_DIMENSION);
		//System.out.println("TEXT SIZE IN METERS IS " + 0.5*getMaxFootprintDimension(1));
		if (mapYAMLFile != null) panel.setMap(mapYAMLFile);
		panel.addKeyListener(new KeyListener() {

			@Override
			public void keyTyped(KeyEvent e) {
				String fileName = new SimpleDateFormat("yyyy-MM-dd-HH:mm:ss:SSS").format(new Date());
				if (e.getKeyChar() == 's') {
					fileName += ".svg";
					dumpSVG(fileName);
					System.out.println("Saved screenshot " + fileName);
				}
				else if (e.getKeyChar() == 'p') {
					fileName += ".pdf";
					dumpPDF(fileName);
					System.out.println("Saved screenshot " + fileName);
				}
				else if (e.getKeyChar() == 'e') {
					fileName += ".eps";
					dumpEPS(fileName);
					System.out.println("Saved screenshot " + fileName);
				}
//				else if (e.getKeyChar() == 'm') {
//					System.out.println("Muted robots: " + Arrays.toString(tec.getMuted()));
//				}
//				else {
//					try {
//						int robotID = Integer.parseInt(""+e.getKeyChar());
//						tec.toggleMute(robotID);
//					}
//					catch(NumberFormatException e1) {}
//				}

			}

			@Override
			public void keyReleased(KeyEvent e) { }

			@Override
			public void keyPressed(KeyEvent e) { }
		});
		panel.setFocusable(true);
	}
	
	/**
	 * Dump a vector graphics file with the contents of the GUI. Note: this operation is slow, so this method
	 * should not be called within the control loop.
	 * @param fileName Name of the PDF file to write to.
	 */
	public void dumpPDF(String fileName) {
		panel.writePDF(fileName);
	}

	/**
	 * Dump a vector graphics file with the contents of the GUI. Note: this operation is slow, so this method
	 * should not be called within the control loop.
	 * @param fileName Name of the SVG file to write to.
	 */
	public void dumpSVG(String fileName) {
		panel.writeSVG(fileName);
	}

	/**
	 * Dump a vector graphics file with the contents of the GUI. Note: this operation is slow, so this method
	 * should not be called within the control loop.
	 * @param fileName Name of the EPS file to write to.
	 */
	public void dumpEPS(String fileName) {
		panel.writeEPS(fileName);
	}

	/**
	 * Scale and translate the view of the GUI to include all geometries. 
	 */
	public void centerView() {
		//		this.panel.centerView();
		this.panel.reinitVisualization();
	}
	

	@Override
	public void updateVisualization() {
		panel.removeOldGeometries(1000);
		panel.updatePanel();
	}

	@Override
	public void addEnvelope(TrajectoryEnvelope te) {
		GeometricShapeDomain dom = (GeometricShapeDomain)te.getEnvelopeVariable().getDomain();
		panel.addGeometry("_"+te.getID(), dom.getGeometry(), true, false);
		panel.setPermanent("_"+te.getID());
	}

	@Override
	public void removeEnvelope(TrajectoryEnvelope te) {
		panel.removeGeometry("_"+te.getID());
	}
	
	public void setMinimumVisibleFrame(double minX, double minY, double maxX, double maxY) {
		GeometryFactory gf = new GeometryFactory();
		Geometry frame = gf.createPolygon(new Coordinate[] {
				new Coordinate(minX,minY),
				new Coordinate(minX,maxY),
				new Coordinate(maxX,maxY),
				new Coordinate(maxX,minY),
				new Coordinate(minX,minY)
		});	
		panel.removeGeometry("_frame");
		panel.addGeometry("_frame", frame, true, false, true, "#000000");
		panel.setPermanent("_frame");
	}

	@Override
	public void setMap(String yamlFile) {
		panel.setMap(yamlFile);
	}

	@Override
	public int periodicEnvelopeRefreshInMillis() {
		// TODO Auto-generated method stub
		return 0;
	}

} public class Missions {

	protected static HashMap<String,Pose> locations = new HashMap<String, Pose>();
	protected static HashMap<String,PoseSteering[]> paths = new HashMap<String, PoseSteering[]>();
	private static Logger metaCSPLogger = MetaCSPLogging.getLogger(Missions.class);
	protected static HashMap<Integer,ArrayList<Mission>> missions = new HashMap<Integer, ArrayList<Mission>>();
	protected static HashMap<Integer,MissionDispatchingCallback> mdcs = new HashMap<Integer, MissionDispatchingCallback>();
	protected static HashMap<Mission,ArrayList<Mission>> concatenatedMissions = new HashMap<Mission, ArrayList<Mission>>();
	//protected static String pathPrefix = "";
	protected static SimpleDirectedWeightedGraph<String, DefaultWeightedEdge> graph = new SimpleDirectedWeightedGraph<String, DefaultWeightedEdge>(DefaultWeightedEdge.class); 
	protected static String mapYAML = null;
	protected static String mapImageFilename = null;
	protected static BufferedImage map = null;
	protected static double mapResolution = -1;
	protected static Coordinate mapOrigin = null;

	protected static double minPathDistance = -1;
	
	protected static Thread missionDispatchThread = null;
	protected static HashSet<Integer> dispatchableRobots = new HashSet<Integer>();
	protected static HashMap<Integer,Boolean> loopMissions = new HashMap<Integer,Boolean>();
	
	/**
	 * Set the minimum acceptable distance between path poses. This is used to re-sample paths
	 * when they are loaded from file or when the method {@link #resamplePathsInRoadMap()} is called.
	 * @param minPathDist The minimum acceptable distance between path poses.
	 */
	public static void setMinPathDistance(double minPathDist) {
		minPathDistance = minPathDist;
	}
	
	/**
	 * Re-sample all paths so that the minimum distance between path poses is the value
	 * set by {@link #setMinPathDistance(double)}.
	 */
	public static void resamplePathsInRoadMap() {
		for (String pathname : paths.keySet()) {
			paths.put(pathname, resamplePath(paths.get(pathname)));
		}
	}
	
	public static PoseSteering[] resamplePath(PoseSteering[] path) {
		if (minPathDistance < 0) return path;
		ArrayList<PoseSteering> ret = new ArrayList<PoseSteering>();
		PoseSteering lastAdded = path[0];
		ret.add(lastAdded);
		for (int i = 1; i < path.length; i++) {
			Coordinate p1 = lastAdded.getPose().getPosition();
			Coordinate p2 = path[i].getPose().getPosition();
			if (p2.distance(p1) > minPathDistance) {
				lastAdded = path[i];
				ret.add(path[i]);
			}
		}
		return ret.toArray(new PoseSteering[ret.size()]);
	}

	/**
	 * Get the length in meters of a path. This is the sum of distances between path poses.
	 * @return The length in meters of a path. This is the sum of distances between path poses.
	 */
	public static double getPathLength(PoseSteering[] path) {
		double length = 0.0;
		for (int i = 0; i < path.length-1; i++) {
			length += path[i].getPose().distanceTo(path[i+1].getPose());
		}
		return length;
	}


	/**
	 * Get the length in meters of a path. This is the sum of distances between path poses.
	 * @return The length in meters of a path. This is the sum of distances between path poses.
	 */
	public static double getPathLength(Pose[] path) {
		double length = 0.0;
		for (int i = 0; i < path.length-1; i++) {
			length += path[i].distanceTo(path[i+1]);
		}
		return length;
	}

	/**
	 * Get the image of the current map, if set.
	 * @return The image of the current map, <code>null</code> if no map is known.
	 */
	public static BufferedImage getMap() {
		return Missions.map;
	}
	
	/**
	 * Get the resolution of the current map, if set.
	 * @return The resolution of the current map, <code>0</code> if no map is known.
	 */
	public static double getMapResolution() {
		return Missions.mapResolution;
	}

	/**
	 * Get the origin of the current map, if set.
	 * @return The origin of the current map, <code>null</code> if no map is known.
	 */
	public static Coordinate getMapOrigin() {
		return Missions.mapOrigin;
	}

	/**
	 * Get the YAML file of the current map, if set.
	 * @return The YAML file of the current map, <code>null</code> if no map is known.
	 */
	public static String getMapYAML() {
		return Missions.mapYAML;
	}
	
	/**
	 * Save the current map (if known) as PNG with the corresponding YAML descriptor.
	 * @param fileName The name of the image/YAML files to save.
	 */
	public static void saveMap(String fileName) {
		if (Missions.map == null) throw new Error("Cannot save map as no map is known");
		File imageFilename = new File(fileName+".png");
		String yamlFilename = fileName + ".yaml";
		Missions.mapYAML = "\nimage: " + imageFilename;
		Missions.mapYAML += "\nresolution: " + Missions.mapResolution;
		Missions.mapYAML += "\norigin: [" + Missions.mapOrigin.x + ", " + Missions.mapOrigin.y + ", 0]";
		//TODO: make these static members of Missions and load them as well
		Missions.mapYAML +=  "\nnegate: 0";
		Missions.mapYAML += "\noccupied_thresh: 0.3";
		try { 
			ImageIO.write(Missions.map, "png", imageFilename);
			PrintWriter writer = new PrintWriter(yamlFilename);
			writer.println(Missions.mapYAML);
			writer.close();
		}
		catch (IOException e) { e.printStackTrace(); }
	}
	
	private static String extractZipFile(String fileName) {
		String json = "";
		try {
	        byte[] buffer = new byte[1024];
	        ZipInputStream zis = new ZipInputStream(new FileInputStream(fileName));
	        ZipEntry zipEntry = zis.getNextEntry();
	        while(zipEntry != null){
	            String oneFileName = zipEntry.getName();
	            int len;
	            ArrayList<Byte> bytes = new ArrayList<Byte>();
	            while ((len = zis.read(buffer)) > 0) {
	                for (int i = 0; i < len; i++) bytes.add(buffer[i]);
	            }
            	byte[] dataBytes = new byte[bytes.size()];
            	for (int i = 0; i < bytes.size(); i++) dataBytes[i] = bytes.get(i);
	            if (oneFileName.endsWith(".json")) json = new String(dataBytes);
	            else {
	            	ByteArrayInputStream bais = new ByteArrayInputStream(dataBytes);
	            	Missions.map = ImageIO.read(bais);
	            	//Missions.mapImageFilename = oneFileName;
	            }
	            zipEntry = zis.getNextEntry();
	        }
	        zis.closeEntry();
	        zis.close();
		}
		catch (IOException e) { 
			e.printStackTrace(); 
			throw new Error("Unable to estract the ZIP file: " + e.toString());
		}
        return json;
	}
	
	private static void makeZipFile(String ... fileNames) {
		try {
			File f = new File(fileNames[fileNames.length-1]);
			ZipOutputStream out = new ZipOutputStream(new FileOutputStream(f));
			for (int i = 0; i < fileNames.length-1; i++) {
				ZipEntry e = new ZipEntry(fileNames[i]);
				out.putNextEntry(e);
				File entryFile = new File(fileNames[i]);
				byte[] data = Files.readAllBytes(entryFile.toPath());
				out.write(data, 0, data.length);
				out.closeEntry();
			}
			out.close();
		}
		catch (IOException e) { e.printStackTrace(); }
	}
	
	private static class ScenarioContainer {
		private String locationsJSON;
		private String pathsJSON;
		private String missionsJSON;
		private String mapYAMLJSON;
		private String mapImageFilenameJSON;
		private String mapResolutionJSON;
		private String mapOriginJSON;
		private ScenarioContainer() {
			this.missionsJSON = Missions.getJSONString(Missions.missions);
            this.pathsJSON = Missions.getJSONString(Missions.paths);
            this.locationsJSON = Missions.getJSONString(Missions.locations);
            this.mapImageFilenameJSON = Missions.getJSONString(Missions.mapImageFilename);
            this.mapYAMLJSON = Missions.getJSONString(Missions.mapYAML);
            this.mapOriginJSON = Missions.getJSONString(Missions.mapOrigin);
            this.mapResolutionJSON = Missions.getJSONString(Missions.mapResolution);
		}
	}
	
	/**
	 * Save the current scenario. A scenario consists of all known locations and paths (a roadmap),
	 * all known missions, and a map of the environment. These are stored in a ZIP file with the
	 * given name, and can be loaded via a call to {@link Missions#loadScenario(String)}.
	 * @param scenarioName The name of the archive in which to save the current scenario.
	 */
	public static void saveScenario(String scenarioName) {
        try {
        	String zipFilename = scenarioName+".zip";
            metaCSPLogger.info("Saving scenario in ZIP file: " + zipFilename);
            String jsonFilename = scenarioName + ".json";
            PrintWriter writer = new PrintWriter(jsonFilename);
            ScenarioContainer sc = new ScenarioContainer();
            String scenarioJSON = Missions.getJSONString(sc);
            writer.println(scenarioJSON);
            writer.close();
            
            //Create map files if the map is only in memory for some reason...
            if (Missions.map != null && (Missions.mapImageFilename == null || Missions.mapYAML == null)) {
            	Missions.saveMap(scenarioName);
            	Missions.setMap(scenarioName+".yaml");
            }
            
            if (Missions.mapImageFilename != null) {
            	makeZipFile(Missions.mapImageFilename,jsonFilename,zipFilename);
            }
            else {
            	metaCSPLogger.info("Saving scenario without map because no map is known - use setMap() method to set one if you want to save the map along with the scenario.");
            	makeZipFile(jsonFilename,zipFilename);
            }
        }
        catch (IOException e) { 
        	e.printStackTrace(); 
        	throw new Error("Unable to save the scenario: " + e.toString()); 
        }		
	}
	
	/**
	 * Load a scenario. A scenario contains one or more of the following:
	 * locations, paths, missions, and a map of the environment.
	 * Scenarios are stored in ZIP files, which can be saved via a call
	 * to {@link Missions#saveScenario(String)}.
	 * @param scenarioName The name of the scenario to load.
	 */
	@SuppressWarnings("unchecked")
	public static void loadScenario(String scenarioName) {
		String zipFilename = scenarioName+".zip";
		System.out.println("Loading scenario from ZIP file: " + zipFilename);
		String json = Missions.extractZipFile(zipFilename);
		Gson gson = new GsonBuilder().serializeSpecialFloatingPointValues().serializeNulls().create();
		ScenarioContainer sc = gson.fromJson(json, ScenarioContainer.class);
		Type collectionType = new TypeToken<HashMap<Integer,ArrayList<Mission>>>(){}.getType();
		Missions.missions = (HashMap<Integer, ArrayList<Mission>>) parseJSONString(collectionType, sc.missionsJSON);
		collectionType = new TypeToken<HashMap<String,Pose>>(){}.getType();
		Missions.locations = (HashMap<String,Pose>) parseJSONString(collectionType, sc.locationsJSON);
		collectionType = new TypeToken<HashMap<String,PoseSteering[]>>(){}.getType();
		Missions.paths = (HashMap<String,PoseSteering[]>) parseJSONString(collectionType, sc.pathsJSON);
		Missions.mapImageFilename = (String)parseJSONString(String.class, sc.mapImageFilenameJSON);
		//Missions.map = ImageIO.read(new File(Missions.mapImageFilename));
		Missions.mapYAML = (String)parseJSONString(String.class, sc.mapYAMLJSON);
		Missions.mapResolution = (Double)parseJSONString(Double.TYPE, sc.mapResolutionJSON);
		Missions.mapOrigin = (Coordinate)parseJSONString(Coordinate.class, sc.mapOriginJSON);
		Missions.buildGraph();		
	}
	
	private static String getJSONString(Object o) {
		Gson gson = new GsonBuilder().serializeSpecialFloatingPointValues().serializeNulls().create();
		String json = gson.toJson(o);
		return json;
	}
	
	private static Object parseJSONString(Type t, String json) {
		Gson gson = new GsonBuilder().serializeSpecialFloatingPointValues().serializeNulls().create();
		return gson.fromJson(json, t);
	}
	
	/**
	 * Set the current map.
	 * @param mapYAMLFile A file containing the description of the current map and a pointer to its image.
	 */
	public static void setMap(String mapYAMLFile) {
		try {
			File file = new File(mapYAMLFile);
			BufferedReader br = new BufferedReader(new FileReader(file));
			String st;
			Missions.mapYAML = "";
			while((st=br.readLine()) != null){
				if (!st.trim().startsWith("#") && !st.trim().isEmpty()) {
					Missions.mapYAML += st+"\n";
					String key = st.substring(0, st.indexOf(":")).trim();
					String value = st.substring(st.indexOf(":")+1).trim();
					if (key.equals("image")) Missions.mapImageFilename = file.getParentFile()+File.separator+value;
					else if (key.equals("resolution")) Missions.mapResolution = Double.parseDouble(value);
					else if (key.equals("origin")) {
						String x = value.substring(1, value.indexOf(",")).trim();
						String y = value.substring(value.indexOf(",")+1, value.indexOf(",", value.indexOf(",")+1)).trim();
						Missions.mapOrigin = new Coordinate(Double.parseDouble(x),Double.parseDouble(y));
					}
				}
			}
			br.close();
			Missions.map = ImageIO.read(new File(Missions.mapImageFilename));
		}
		catch (IOException e) { e.printStackTrace(); }
	}
	
	/**
	 * Get the name of the initial location of each robot's first {@link Mission}.
	 * The pose corresponding to each location name can be looked up via the method {@link Missions#getLocation(String)}.
	 * @return The name of the initial location of each robot's first {@link Mission}.
	 */
	public static HashMap<Integer,String> getInitialLocations() {
		HashMap<Integer,String> ret = new HashMap<Integer, String>();
		for (Integer robotID : missions.keySet()) {
			Mission m = Missions.peekMission(robotID);
			if (m != null) ret.put(robotID, m.getFromLocation());
		}
		return ret;
	}
	
	/**
	 * Get the pose of the initial location of each robot's first {@link Mission}.
	 * @return The pose of the initial location of each robot's first {@link Mission}.
	 */
	public static HashMap<Integer,Pose> getInitialPoses() {
		HashMap<Integer,Pose> ret = new HashMap<Integer, Pose>();
		for (Integer robotID : missions.keySet()) {
			Mission m = Missions.peekMission(robotID);
			if (m != null) ret.put(robotID, m.getFromPose());
		}
		return ret;
	}
	
	/**
	 * Get the IDs of robots involved in at least one {@link Mission}.
	 * @return The IDs of robots involved in at least one {@link Mission}.
	 */
	public static int[] getIDsOfRobotsWithMissions() {
		int[] ret = new int[missions.keySet().size()];
		int index = 0;
		for (int robotID : missions.keySet()) {
			ret[index++] = robotID;
		}
		return ret;
	}
	
	private static void buildGraph() {
		
		graph = new SimpleDirectedWeightedGraph<String, DefaultWeightedEdge>(DefaultWeightedEdge.class);
		
		metaCSPLogger.info("Updating the roadmap...");
		
		for (String oneLoc : locations.keySet()) {
			graph.addVertex(oneLoc);
			metaCSPLogger.info("Added vertex " + oneLoc);
		}
		
		for (String from : locations.keySet()) {
			for (String to : locations.keySet()) {
				if (!from.equals(to)) {
					if (isKnownPath(from, to)) {
						DefaultWeightedEdge e = graph.addEdge(from, to);
						//PoseSteering[] path = loadKnownPath(from, to);
						PoseSteering[] path = paths.get(from+"->"+to);
						graph.setEdgeWeight(e, path.length);
						metaCSPLogger.info("Added edge " + e);
					}
				}
			}	
		}
	}

	/**
	 * Get the shortest path connecting given locations (two or more). The path between successive pairs of locations
	 * is computed with Dijkstra's algorithm, where edge weights are path lengths.
	 * @param locations At least two location names.
	 * @return The shortest path connecting given locations.
	 */
	public static PoseSteering[] getShortestPath(String ... locations) {
		if (locations.length < 2) throw new Error("Please provide at least two locations for path extraction!");
		DijkstraShortestPath<String, DefaultWeightedEdge> dijkstraShortestPath = new DijkstraShortestPath<String, DefaultWeightedEdge>(graph);
		ArrayList<PoseSteering> overallShortestPath = new ArrayList<PoseSteering>();
		for (int k = 0; k < locations.length-1; k++) {
		    GraphPath<String, DefaultWeightedEdge> gp = dijkstraShortestPath.getPath(locations[k], locations[k+1]);			
		    if (gp == null) return null;
		    List<String> oneShortestPath = gp.getVertexList();
		    ArrayList<PoseSteering> allPoses = new ArrayList<PoseSteering>();
		    for (int i = 0; i < oneShortestPath.size()-1; i++) {
		    	//PoseSteering[] onePath = loadKnownPath(oneShortestPath.get(i),oneShortestPath.get(i+1));
		    	PoseSteering[] onePath = paths.get(oneShortestPath.get(i)+"->"+oneShortestPath.get(i+1));
		    	if (i == 0) allPoses.add(onePath[0]);
		    	for (int j = 1; j < onePath.length-1; j++) {
		    		allPoses.add(onePath[j]);
		    	}
		    	if (i == oneShortestPath.size()-2) allPoses.add(onePath[onePath.length-1]);
		    }
		    if (k == 0) overallShortestPath.add(allPoses.get(0));
		    for (int i = 1; i < allPoses.size(); i++) {
		    	overallShortestPath.add(allPoses.get(i));
		    }
		}
		return overallShortestPath.toArray(new PoseSteering[overallShortestPath.size()]);
	}
	
	/**
	 * Get the path between two locations if such a path exists in the roadmap.
	 * @param from The name of the source location.
	 * @param to The name of the target location.
	 * @return The path between the given locations, <code>null</code> if the roadmap does not contain this edge.
	 */
	public static PoseSteering[] getPath(String from, String to) {
		return paths.get(from+"->"+to);
	}
	
	/**
	 * Get all the {@link Mission}s currently known for one robot
	 * @param robotID A robot identifier
	 * @return All the {@link Mission}s currently known for one robot
	 */
	public static ArrayList<Mission> getMissions(int robotID) {
		return missions.get(robotID);
	}
	
	/**
	 * Ascertain whether there is at least one {@link Mission} for a given robot. 
	 * @param robotID The ID of a robot.
	 * @return <code>true</code> iff the robot with the given ID has a mission.
	 */
	public static boolean hasMissions(int robotID) {
		return missions.containsKey(robotID) && !missions.get(robotID).isEmpty();
	}
	
	/**
	 * Remove one or more {@link Mission}s.
	 * @param m The mission(s) to remove.
	 */
	public static void removeMissions(Mission ... m) {
		for (Mission mis : m) missions.get(mis.getRobotID()).remove(mis);
	}
	
	/**
	 * Set the pose of a location.
	 * @param locationName The name of the location.
	 * @param p The pose of the location.
	 */
	@Deprecated
	public static void setLocation(String locationName, Pose p) {
		locations.put(locationName, p);
	}
	
	/**
	 * Get all the {@link Mission}s currently currently known
	 * @return All the {@link Mission}s currently currently known
	 */
	public static HashMap<Integer,ArrayList<Mission>> getMissions() {
		return missions;
	}

	/**
	 * Add a new {@link Mission} for a robot.
	 * @param m The mission to push.
	 */
	@Deprecated
	public static void putMission(Mission m) {
		if (!missions.containsKey(m.getRobotID())) missions.put(m.getRobotID(), new ArrayList<Mission>());
		missions.get(m.getRobotID()).add(m);
	}
	
	/**
	 * Enqueue a new {@link Mission} for a robot.
	 * @param m The mission to enqueue.
	 */
	public static void enqueueMission(Mission m) {
		if (!missions.containsKey(m.getRobotID())) missions.put(m.getRobotID(), new ArrayList<Mission>());
		missions.get(m.getRobotID()).add(m);
	}

	/**
	 * Push a new {@link Mission} for a robot on the mission queue.
	 * @param m The mission to push
	 */
	@Deprecated
	public static void pushMission(Mission m) {
		if (!missions.containsKey(m.getRobotID())) missions.put(m.getRobotID(), new ArrayList<Mission>());
		missions.get(m.getRobotID()).add(m);
	}

	/**
	 * Get the i-th mission for a given robot.	
	 * @param robotID A robot identifier.
	 * @param missionNumber The mission to get.
	 * @return The i-th mission for a given robot.
	 */
	public static Mission getMission(int robotID, int missionNumber) {
		return missions.get(robotID).get(missionNumber);
	}
	
	/**
	 * Dequeue the first mission from the queue of a given robot. 
	 * @param robotID The ID of the robot.
	 * @return The first mission from the queue of a given robot.
	 */
	public static Mission dequeueMission(int robotID) {
		if (!missions.get(robotID).isEmpty()) {
			Mission m = missions.get(robotID).get(0);
			missions.get(robotID).remove(0);
			return m;
		}
		return null;
	}
	
	/**
	 * Pop the first mission from the queue of a given robot. 
	 * @param robotID The ID of the robot.
	 * @return The first mission from the queue of a given robot.
	 */
	@Deprecated
	public static Mission popMission(int robotID) {
		if (!missions.get(robotID).isEmpty()) {
			Mission m = missions.get(robotID).get(0);
			missions.get(robotID).remove(0);
			return m;
		}
		return null;
	}
	
	/**
	 * Pop the first mission from the queue of a given robot, but do NOT remove the mission from the queue. 
	 * @param robotID The ID of the robot.
	 * @return The first mission from the queue of a given robot.
	 */
	public static Mission peekMission(int robotID) {
		if (!missions.get(robotID).isEmpty()) {
			Mission m = missions.get(robotID).get(0);
			return m;
		}
		return null;
	}

	/**
	 * Normalize an angle to be within (-PI,PI).
	 * @param th The angle to normalize
	 * @return A value within (-PI,PI)
	 */
	public static double wrapAngle180b(double th) {
		double ret = Math.atan2(Math.sin(th), Math.cos(th));
		if (Math.abs(ret-Math.PI) < 0.00001) return Math.PI-0.01;
		return ret;
	}

	/**
	 * Normalize an angle to be within (-PI,PI].
	 * @param th The angle to normalize
	 * @return A value within (-PI,PI]
	 */
	public static double wrapAngle180a(double th) {
		double ret = Math.atan2(Math.sin(th), Math.cos(th));
		if (Math.abs(ret-Math.PI) < 0.00001) return Math.PI;
		return ret;
	}
	
	/**
	 * Normalize an angle to be within [-PI,PI).
	 * @param th The angle to normalize
	 * @return A value within [-PI,PI)
	 */
	public static double wrapAngle180(double th) {
		double ret = Math.atan2(Math.sin(th), Math.cos(th));
		if (Math.abs(ret-Math.PI) < 0.00001) return -Math.PI;
		return ret;
	}

	/**
	 * Normalize an angle to be within [0,2*PI).
	 * @param th The angle to normalize
	 * @return A value within [0,2*PI)
	 */
	public static double wrapAngle360(double th) {
		double ret = Math.atan2(Math.sin(th), Math.cos(th));
		if (ret < 0) return ret+2*Math.PI;
		return ret;
	}

	/**
	 * Get the last placement along the {@link Trajectory} of a {@link TrajectoryEnvelope} that does
	 * not overlap with the final pose of the robot.
	 * @param te The trajectory envelope to search on
	 * @return The last placement along the {@link Trajectory} of a {@link TrajectoryEnvelope} that does
	 * not overlap with the final pose of the robot.
	 */
	public static Geometry getBackBlockingObstacle(TrajectoryEnvelope te) {
		Trajectory traj = te.getTrajectory();
		PoseSteering[] path = traj.getPoseSteering();
		Geometry placementLast = te.makeFootprint(path[path.length-1]);
		for (int i = path.length-2; i >= 0; i--) {
			Geometry placement = te.makeFootprint(path[i]);
			if (!placement.intersects(placementLast)) return placement;
		}
		return null;
	}

	/**
	 * Get the known locations and their poses.
	 * @return All known locations and their poses.
	 */
	@Deprecated
	public static HashMap<String,Pose> getLocations() {
		return locations;
	}

	/**
	 * Get the known locations and their poses.
	 * @return All known locations and their poses.
	 */
	public static HashMap<String,Pose> getLocationsAndPoses() {
		return locations;
	}

	
	/**
	 * Load location and path data from a file.
	 * @param fileName The file to load the data from.
	 */
	@Deprecated
	public static void loadLocationAndPathData(String fileName) {
		Missions.loadRoadMap(fileName);
	}
	
	/**
	 * Load a roadmap stored in a give directory or file.
	 * @param fileName The directory or file to load the roadmap from.
	 * If a directory is given, the filename is assumed to he "roadmap.txt"
	 * (the same convention used in saving, see {@link #saveRoadMap(String)}).
	 */
	public static void loadRoadMap(String path) {
		try {
			String fileOnly;
			String pathOnly;
			File f = new File(path);
			if (f.isDirectory()) {
				pathOnly = path;
				if (!pathOnly.endsWith(File.separator)) pathOnly += File.separator;
				fileOnly = "roadmap.txt";
			}
			else {
				pathOnly = f.getAbsolutePath().substring(0,f.getAbsolutePath().lastIndexOf(File.separator))+File.separator;
				fileOnly = f.getAbsolutePath().substring(f.getAbsolutePath().lastIndexOf(File.separator)+1);
			}
			Scanner in = new Scanner(new FileReader(pathOnly+fileOnly));
			
			while (in.hasNextLine()) {
				String line = in.nextLine().trim();
				if (line.length() != 0 && !line.startsWith("#")) {
					String[] oneline = line.split(" |\t");
					Pose ps = null;
					if (line.contains("->")) {
						PoseSteering[] knownPath = loadPathFromFile(pathOnly+oneline[3]);
						paths.put(oneline[0]+oneline[1]+oneline[2], knownPath);
						//paths.put(oneline[0]+oneline[1]+oneline[2], oneline[3]);
						//metaCSPLogger.info("Loaded path: " + oneline[0]+oneline[1]+oneline[2] + " --> " + oneline[3]);
					}
					else {
						ArrayList<String> newLineContents = new ArrayList<String>();
						for (int i = 0; i < oneline.length; i++) {
							if (!oneline[i].trim().equals("")) newLineContents.add(oneline[i].trim()); 
						}
						String locationName = newLineContents.get(0);
						ps = new Pose(
								Double.parseDouble(newLineContents.get(1)),
								Double.parseDouble(newLineContents.get(2)),
								Double.parseDouble(newLineContents.get(3)));
						locations.put(locationName, ps);
					}
				}
			}
			in.close();
		}
		catch (FileNotFoundException e) { 
			e.printStackTrace(); 
			throw new Error("Unable to load the required scenario: " + e.toString());
		}
		Missions.buildGraph();
	}
	
	/**
	 * Save all locations and paths into a given directory.
	 * @param pathname The name of a directory in which to store the data.
	 */
	@Deprecated
	public static void saveLocationAndPathData(String pathname) {
		Missions.saveRoadMap(pathname);
	}
	
	/**
	 * Save the current roadmap into a given directory. All known paths referred to
	 * in the roadmap are saved in the given directory. The roadmap itself is saved
	 * in a file named "roadmap.txt" within the given directory.
	 * @param pathname The name of a directory in which to store the roadmap data.
	 */
	public static void saveRoadMap(String pathname) {
		if (new File(pathname).exists()) throw new Error("Directory \"" + pathname + "\" exists, will abort saving");
		if (!new File(pathname).mkdir()) throw new Error("Could not make path \"" + pathname + "\"");
		if (!pathname.endsWith(File.separator)) pathname += File.separator;
		String st = "#Locations\n";
		for (Entry<String,Pose> entry : locations.entrySet()) {
			st += entry.getKey() + "\t" + entry.getValue().getX() + "\t" + entry.getValue().getY() + "\t" + entry.getValue().getTheta() + "\n";
		}
		st+="\n#Paths\n";
		for (Entry<String,PoseSteering[]> entry : paths.entrySet()) {
			String pathFilename = entry.getKey().replaceAll("->", "-")+".path";
			st += entry.getKey().replaceAll("->", " -> ") + "\t" + pathFilename + "\n";
			writePath(pathname+pathFilename, entry.getValue());
		}
        try {
        	String newFilename = pathname+"roadmap.txt";
            File file = new File(newFilename);
            PrintWriter writer = new PrintWriter(file);
            writer.write(st);
            writer.close();
            System.out.println("Saved roadmap " + newFilename);
        }
        catch (Exception ex) { ex.printStackTrace(); }
	}
	

	/**
	 * Add a path to the set of known paths.
	 * @param start The starting location of the path.
	 * @param goal The goal location of the path.
	 * @param path The path to add.
	 */
	@Deprecated
	public static void addKnownPath(String start, String goal, PoseSteering[] path) {
		Missions.addPathToRoadMap(start, goal, path);
	}
		
	/**
	 * Add a path to the current roadmap.
	 * @param start The starting location of the path.
	 * @param goal The goal location of the path.
	 * @param path The path to add.
	 */
	public static void addPathToRoadMap(String start, String goal, PoseSteering[] path) {
		if (!locations.containsKey(start) || !locations.containsKey(goal) || path == null || path.length == 0) throw new Error("Locations unknown or path is invalid (" + (start+"->"+goal) + ")!");
		paths.put(start+"->"+goal, path);
		Missions.buildGraph();
	}

	/**
	 * Remove a named location.
	 * @param locationName The name of the location to remove.
	 */
	@Deprecated
	public static void removeLocation(String locationName) {
		Missions.removeLocationFromRoadMap(locationName);
	}
	
	/**
	 * Remove a named location from the roadmap.
	 * @param locationName The name of the location to remove.
	 */
	public static void removeLocationFromRoadMap(String locationName) {
		locations.remove(locationName);
		ArrayList<String> toRemove = new ArrayList<String>();
		for (String key : paths.keySet()) {
			if (key.substring(0,key.indexOf("->")).equals(locationName)) toRemove.add(key);
			else if (key.substring(key.indexOf("->")).equals(locationName)) toRemove.add(key);
		}
		for (String key : toRemove) paths.remove(key);
		if (graph != null) graph.removeVertex(locationName);
	}
	
	/**
	 * Add a location to the roadmap.
	 * @param locationName The name of the location.
	 * @param pose The pose of the location.
	 */
	public static void addLocationToRoadMap(String locationName, Pose pose) {
		locations.put(locationName, pose);
	}
	
	/**
	 * Get locations from the data loaded by method loadLocationAndPathData()
	 * @param name The name of the location to get
	 * @return The {@link Pose} of the location
	 */
	@Deprecated
	public static Pose getLocation(String name) {
		Pose ret = locations.get(name);
		if (ret == null) throw new Error("Unknown location " + name);
		return ret;
	}
	
	/**
	 * Get the pose of a given named location in the roadmap.
	 * @param name The name of the location to get the pose of.
	 * @return The {@link Pose} of the location
	 */
	public static Pose getLocationPose(String name) {
		Pose ret = locations.get(name);
		if (ret == null) throw new Error("Unknown location " + name);
		return ret;
	}

	
	/**
	 * Get the mission following a given mission.
	 * @param m A mission.
	 * @return The mission following the given mission.
	 */
	public static Mission getNextMission(Mission m) {
		for (int i = 0; i < missions.get(m.getRobotID()).size(); i++) {
			if (missions.get(m.getRobotID()).get(i).equals(m)) return missions.get(m.getRobotID()).get((i+1)%missions.get(m.getRobotID()).size()); 
		}
		return null;
	}

	/**
	 * Get the mission preceding a given mission.
	 * @param m A mission.
	 * @return The mission preceding the given mission.
	 */
	public static Mission getPreviousMission(Mission m) {
		for (int i = 0; i < missions.get(m.getRobotID()).size(); i++) {
			if (missions.get(m.getRobotID()).get(i).equals(m)) {
				if (i == 0) return missions.get(m.getRobotID()).get(missions.get(m.getRobotID()).size()-1);
				return missions.get(m.getRobotID()).get((i-1)); 
			}
		}
		return null;
	}

	/**
	 * Get path files from the data loaded by method {@link Missions#loadLocationAndPathData(String)}.
	 * @param fromLocation The name of the source location.
	 * @param toLocation The name of the destination location.
	 * @return The name of the file where the path is stored. 
	 */
	@Deprecated
	public static String getPathFile(String fromLocation, String toLocation) {
		//String ret = paths.get(fromLocation+"->"+toLocation);
		String ret = fromLocation+"->"+toLocation;
		if (!locations.containsKey(fromLocation)) throw new Error("Unknown location " + fromLocation);
		if (!locations.containsKey(toLocation)) throw new Error("Unknown location " + toLocation);
		File f = new File(ret);
		if(!f.exists() || f.isDirectory()) throw new Error("No path between " + fromLocation + " and " + toLocation);
		//if (ret == null) throw new Error("No path between " + fromLocation + " and " + toLocation);
		return ret;
	}
	
	/**
	 * Queries whether a path between two named locations is known.
	 * @param fromLocation The source location.
	 * @param toLocation The goal location.
	 * @return <code>true</code> iff a path between two locations is known.
	 */
	public static boolean isKnownPath(String fromLocation, String toLocation) {
		return paths.containsKey(fromLocation+"->"+toLocation);
	}
	
	/**
	 * Return a path between locations if available (throws error if locations and/or path are now known)
	 * @param fromLocation The source location
	 * @param toLocation The goal location
	 * @return The path between the two (known) locations
	 */
//	public static PoseSteering[] loadKnownPath(String fromLocation, String toLocation) {
//		if (!isKnownPath(fromLocation, toLocation)) throw new Error("No path between " + fromLocation + " and " + toLocation);
//		return loadPathFromFile(pathPrefix+getPathFile(fromLocation, toLocation));
//	}

	/**
	 * Get a property from a YAML file
	 * @param property The name of the property
	 * @param yamlFile The YAML file from which to get the property
	 * @return The value of the property in the YAML file
	 */
	public static String getProperty(String property, String yamlFile) {
		String ret = null;
		try {
			File file = new File(yamlFile);
			BufferedReader br = new BufferedReader(new FileReader(file));
			String st;
			while((st=br.readLine()) != null){
				String key = st.substring(0, st.indexOf(":")).trim();
				String value = st.substring(st.indexOf(":")+1).trim();
				if (key.equals(property)) {
					ret = value;
					break;
				}
			}
			br.close();
		}
		catch (IOException e) { e.printStackTrace(); }
		return ret;
	}

	/**
	 * Exclude the given robots from the dispatching thread.
	 * @param robotIDs The IDs of the robots to be excluded.
	 */
	public static void stopMissionDispatchers(int ... robotIDs) {
		for (int robotID : robotIDs) {
			dispatchableRobots.remove(robotID);
			mdcs.remove(robotID);
		}
	}

	/**
	 * Save a path to a file.
	 * @param fileName The name of the file.
	 * @param path The path to save.
	 */
	public static void writePath(String fileName, ArrayList<PoseSteering> path) {
        try {
            File file = new File(fileName);
            System.out.println("Saved path file: " + file.getAbsolutePath());
            PrintWriter writer = new PrintWriter(file);
            for (PoseSteering ps : path) {
            	writer.println(ps.getPose().getX() + "\t" + ps.getPose().getY() + "\t" + ps.getPose().getTheta() + "\t" + ps.getSteering());
            }
            writer.close();
        }
        catch (Exception e) { e.printStackTrace(); }
	}

	/**
	 * Save a path to a file.
	 * @param fileName The name of the file.
	 * @param path The path to save.
	 */
	public static void writePath(String fileName, PoseSteering[] path) {
        try {
            File file = new File(fileName);
            System.out.println("Saved path file " + file.getAbsolutePath());
            PrintWriter writer = new PrintWriter(file);
            for (PoseSteering ps : path) {
            	writer.println(ps.getPose().getX() + "\t" + ps.getPose().getY() + "\t" + ps.getPose().getTheta() + "\t" + ps.getSteering());
            }
            writer.close();
        }
        catch (Exception e) { e.printStackTrace(); }
	}

	/**
	 * Tag a set of {@link Mission}s as concatenated, meaning that they should be executed in sequence.
	 * This marking will be accounted for by the dispatchers started via methods {@link Missions#startMissionDispatchers(TrajectoryEnvelopeCoordinator, int...)}
	 * and {@link Missions#startMissionDispatchers(TrajectoryEnvelopeCoordinator, boolean, int...)}.
	 * @param m The {@link Mission}s that should be considered concatenated.
	 */
	public static void concatenateMissions(Mission ... m) {
		ArrayList<Mission> toAdd = new ArrayList<Mission>();
		for (Mission oneM : m) toAdd.add(oneM);
		concatenatedMissions.put(m[0], toAdd);
	}
		
	/**
	 * Add a {@link MissionDispatchingCallback} which defines methods to be called before and after mission dispatching.
	 * @param robotID The callback functions will be invoked whenever a mission for this robot is dispatched. 
	 * @param cb The {@link MissionDispatchingCallback} to attach.
	 */
	public static void addMissionDispatchingCallback(int robotID, MissionDispatchingCallback cb) {
		mdcs.put(robotID, cb);
	}
	
	/**
	 * Include the given robots in the periodic mission dispatching thread (and start the thread if it is not started).
	 * The thread cycles through the known missions for each robot and dispatches as soon as the robot is free.
	 * This method will loop through all mission forever.
	 * @param tec The {@link TrajectoryEnvelopeCoordinator} that coordinates the missions.
	 * @param robotIDs The robot IDs which should be considered dispatchable.
	 */
	public static void startMissionDispatchers(final TrajectoryEnvelopeCoordinator tec, int ... robotIDs) {
		startMissionDispatchers(tec, true, robotIDs);
	}
	
	/**
	 * Include the given robots in the periodic mission dispatching thread (and start the thread if it is not started).
	 * The thread cycles through the known missions for each robot and dispatches as soon as the robot is free.
	 * @param tec The {@link TrajectoryEnvelopeCoordinator} that coordinates the missions.
	 * @param loop Set to <code>false</code> if missions should be de-queued once dispatched. 
	 * @param robotIDs The robot IDs which should be considered dispatchable.
	 */
	public static void startMissionDispatchers(final TrajectoryEnvelopeCoordinator tec, final boolean loop, int ... robotIDs) {
		
		for (int robotID : robotIDs) {
			dispatchableRobots.add(robotID);
			loopMissions.put(robotID, loop);
		}
		
		if (missionDispatchThread == null) {
			missionDispatchThread = new Thread() {
				@Override
				public void run() {
					while (true) {
						for (int robotID : dispatchableRobots) {
							if (Missions.hasMissions(robotID)) {
								Mission m = Missions.peekMission(robotID);
								if (m != null) {								
									synchronized(tec) {
										if (tec.isFree(m.getRobotID())) {
											//cat with future missions if necessary
											if (concatenatedMissions.containsKey(m)) {
												ArrayList<Mission> catMissions = concatenatedMissions.get(m);
												m = new Mission(m.getRobotID(), m.getFromLocation(), catMissions.get(catMissions.size()-1).getToLocation(), m.getFromPose(), catMissions.get(catMissions.size()-1).getToPose());
												ArrayList<PoseSteering> path = new ArrayList<PoseSteering>();
												for (int i = 0; i < catMissions.size(); i++) {
													Mission oneMission = catMissions.get(i);
													if (mdcs.containsKey(robotID)) mdcs.get(robotID).beforeMissionDispatch(oneMission);
													if (i == 0) path.add(oneMission.getPath()[0]);
													for (int j = 1; j < oneMission.getPath().length-1; j++) {
														path.add(oneMission.getPath()[j]);
													}
													if (i == catMissions.size()-1) path.add(oneMission.getPath()[oneMission.getPath().length-1]);
												}
												m.setPath(path.toArray(new PoseSteering[path.size()]));
											}
											else if (mdcs.containsKey(robotID)) mdcs.get(robotID).beforeMissionDispatch(m);							
										}
			
										//addMission returns true iff the robot was free to accept a new mission
										if (tec.addMissions(m)) {
											//tec.computeCriticalSectionsAndStartTrackingAddedMission();
											if (mdcs.containsKey(robotID)) mdcs.get(robotID).afterMissionDispatch(m);
											if (!loopMissions.get(robotID)) {
												Missions.removeMissions(m);
												System.out.println("Removed mission " + m);
												if (concatenatedMissions.get(m) != null) {
													for (Mission cm : concatenatedMissions.get(m)) {
														Missions.removeMissions(cm);
													}
												}
											}
											else {
												Missions.dequeueMission(m.getRobotID());
												Missions.enqueueMission(m);
											}
										}
									}
								}
							}
						}
						//Sleep for a little (2 sec)
						try { Thread.sleep(2000); }
						catch (InterruptedException e) { e.printStackTrace(); }
					}
				}
			};
			missionDispatchThread.start();
		}
	}
	
	/**
	 * Read a path from a file.
	 * @param fileName The name of the file containing the path
	 * @return The path read from the file
	 */
	public static PoseSteering[] loadPathFromFile(String fileName) {
		ArrayList<PoseSteering> ret = new ArrayList<PoseSteering>();
		try {
			Scanner in = new Scanner(new FileReader(fileName));
			String prevLine = "Gibberish";
			while (in.hasNextLine()) {
				String line = in.nextLine().trim();
				if (!line.equals(prevLine)) {
					prevLine = line;
					if (line.length() != 0) {
						String[] oneline = line.split(" |\t");
						PoseSteering ps = null;
						if (oneline.length == 4) {
						ps = new PoseSteering(
								Double.parseDouble(oneline[0]),
								Double.parseDouble(oneline[1]),
								Double.parseDouble(oneline[2]),
								Double.parseDouble(oneline[3]));
						}
						else {
							ps = new PoseSteering(
									Double.parseDouble(oneline[0]),
									Double.parseDouble(oneline[1]),
									Double.parseDouble(oneline[2]),
									0.0);					
						}
						ret.add(ps);
					}
				}
			}
			in.close();
		}
		catch (FileNotFoundException e) { e.printStackTrace(); }
		PoseSteering[] retArray = ret.toArray(new PoseSteering[ret.size()]);
		return resamplePath(retArray);
	}
	

	
	/**
	 * Create a mission following a given mission. The mission will make the follower robot navigate from its
	 * current pose to the start pose of the leader's mission, after which the follower robot will follow the
	 * leader's mission.
	 * @param leaderMission The mission to follow.
	 * @param followerID The ID of the follower robot.
	 * @param followerStartingPose The current pose of the follower robot.
	 * @param mp The motion planner that should be used to compute the path from the follower robot's
	 * current pose to goal pose of the leader's mission, passing through the start pose of the leader's mission.
	 * @param computePathToLeaderGoal Set this to <code>true</code> iff the follower's path to the goal of the 
	 * leader's mission should be recomputed (otherwise the leader's path will be re-used).  
	 * @return
	 */
	public static Mission followMission(Mission leaderMission, int followerID, Pose followerStartingPose, AbstractMotionPlanner mp, boolean computePathToLeaderGoal) {
		mp.setStart(followerStartingPose);
		if (computePathToLeaderGoal) mp.setGoals(leaderMission.getFromPose(), leaderMission.getToPose());
		else mp.setGoals(leaderMission.getFromPose());
		if (!mp.plan()) return null;
		PoseSteering[] followerPath = mp.getPath();
		if (!computePathToLeaderGoal) {
			PoseSteering[] newPath = new PoseSteering[followerPath.length+leaderMission.getPath().length-1];
			int counter = 0;
			for (PoseSteering ps : followerPath) newPath[counter++] = ps;
			for (int i = 1; i < leaderMission.getPath().length; i++) newPath[counter++] = leaderMission.getPath()[i];
			followerPath = newPath;
		}
		Mission followerMission = new Mission(followerID, followerPath);
		return followerMission;
	}
	
	public static Set<String> getAllGraphVertices() {
		return graph.vertexSet();
	}
	
	public static Set<DefaultWeightedEdge> getAllGraphEdges() {
		return graph.edgeSet();
	}

} public class RVizVisualization implements FleetVisualization, NodeMain {

	//private final String mapFrameID = "/map_laser2d";
	private String mapFrameID = "/map";
	private ConnectedNode node = null;
	private HashMap<String,Publisher<visualization_msgs.MarkerArray>> boxMarkerPublishers = null;
	private HashMap<String,ArrayList<visualization_msgs.Marker>> boxMarkerMarkers = null;

	private HashMap<Integer,Publisher<visualization_msgs.MarkerArray>> robotStatusPublishers = null;
	private HashMap<Integer,Publisher<visualization_msgs.MarkerArray>> dependencyPublishers = null;
	private HashMap<Integer,ArrayList<visualization_msgs.Marker>> robotStatusMarkers = null;
	private HashMap<Integer,ArrayList<visualization_msgs.Marker>> dependencyMarkers = null;
	private HashMap<Integer,visualization_msgs.Marker> envelopeMarkers = null;
	private boolean ready = false;
	private String mapFileName = null;
	private boolean darkColors = true;
	private boolean publishPartialEnvelope = false;
	
	private static String rvizEntry = ""+
			"    - Class: rviz/MarkerArray\n" + 
			"      Enabled: true\n" + 
			"      Marker Topic: /ROBOTi/deps\n" + 
			"      Name: MarkerArray\n" + 
			"      Namespaces:\n" + 
			"        {}\n" + 
			"      Queue Size: 100\n" + 
			"      Value: true\n" + 
			"    - Class: rviz/MarkerArray\n" + 
			"      Enabled: true\n" + 
			"      Marker Topic: /ROBOTi/status\n" + 
			"      Name: MarkerArray\n" + 
			"      Namespaces:\n" + 
			"        {}\n" + 
			"      Queue Size: 100\n" + 
			"      Value: true\n";

	public static void writeRVizConfigFile(int ... robotIDs) {
		try {
			File file = new File(System.getProperty("user.home")+File.separator+"config.rviz");
			
			ClassLoader loader = Thread.currentThread().getContextClassLoader();
			
			//Read pre
			InputStream is = loader.getResourceAsStream("coordinator_default_config_pre.rviz");
			BufferedReader br = new BufferedReader(new InputStreamReader(is));
			String output = "";
			String oneLine = null;
			while ((oneLine = br.readLine()) != null) {
				output += (oneLine+"\n");  
			}
			br.close();
			is.close();
			
			//Make robot specific entries
			for (int robotID : robotIDs) {
            	output += rvizEntry.replaceAll("ROBOTi", "robot"+robotID);
            }
			
			//Read post
			is = loader.getResourceAsStream("coordinator_default_config_post.rviz");
			br = new BufferedReader(new InputStreamReader(is));
			oneLine = null;
			while ((oneLine = br.readLine()) != null) {
				output += (oneLine+"\n");  
			}
			br.close();
			is.close();
			
			//Dump it out
            PrintWriter writer = new PrintWriter(file);
            writer.write(output);
            writer.close();
		}
		catch (IOException e) { e.printStackTrace(); }

	}
	
	public RVizVisualization() {
		this("/map");
	}
	
	public RVizVisualization(String mapFrameID) {
		this(true,mapFrameID);
	}

	public RVizVisualization(ConnectedNode node) {
		this(node,"/map");
	}
	
	public RVizVisualization(ConnectedNode node, String mapFrameID) {
		this(false, mapFrameID);
		this.node = node;
		this.ready = true;
	}
	
	public RVizVisualization(ConnectedNode node, String mapFrameID, boolean publishPartialEnvelope) {
		this(false, mapFrameID);
		this.node = node;
		this.ready = true;
		this.publishPartialEnvelope = publishPartialEnvelope;
	}

	public RVizVisualization(boolean startROSCore) {
		this(startROSCore,"/map");
	}
	
	public RVizVisualization(boolean startROSCore, String mapFrameID) {
		this.mapFrameID = mapFrameID;
		this.robotStatusPublishers = new HashMap<Integer,Publisher<visualization_msgs.MarkerArray>>();
		this.dependencyPublishers = new HashMap<Integer,Publisher<visualization_msgs.MarkerArray>>();
		this.robotStatusMarkers = new HashMap<Integer,ArrayList<visualization_msgs.Marker>>();
		this.dependencyMarkers = new HashMap<Integer,ArrayList<visualization_msgs.Marker>>();

		this.boxMarkerPublishers = new HashMap<String,Publisher<visualization_msgs.MarkerArray>>();
		this.boxMarkerMarkers = new HashMap<String,ArrayList<visualization_msgs.Marker>>();
		

		NodeConfiguration nodeConfiguration = NodeConfiguration.newPublic("localhost");
		NodeMainExecutor executor = DefaultNodeMainExecutor.newDefault();
		executor.execute(this, nodeConfiguration);

		if (startROSCore) {
			RosCore mRosCore = RosCore.newPublic("localhost", 11311);
			mRosCore.start();
			try {
				mRosCore.awaitStart(5, TimeUnit.SECONDS);
			}
			catch (InterruptedException e) { e.printStackTrace(); }
			System.out.println("ROS-core started");
		}
	}
	
	public void setPublishPartialEnvelope(boolean publishPartialEnvelope) {
		this.publishPartialEnvelope = publishPartialEnvelope;
	}
	
	public void setDarkColors(boolean dark) {
		this.darkColors = dark;
	}

	private BufferedImage toGrayScale(BufferedImage imgIn) {
		BufferedImage img = new BufferedImage(imgIn.getWidth(), imgIn.getHeight(), BufferedImage.TYPE_BYTE_GRAY);
        ColorConvertOp op = new ColorConvertOp(ColorSpace.getInstance(ColorSpace.CS_GRAY), null);
        op.filter(imgIn, img);
        return img;
	}
	
	private BufferedImage toBlackAndWhite(BufferedImage image, int threshold) {
	    BufferedImage result = new BufferedImage(image.getWidth(), image.getHeight(), BufferedImage.TYPE_BYTE_GRAY);
	    result.getGraphics().drawImage(image, 0, 0, null);
	    WritableRaster raster = result.getRaster();
	    int[] pixels = new int[image.getWidth()];
	    for (int y = 0; y < image.getHeight(); y++) {
	        raster.getPixels(0, y, image.getWidth(), 1, pixels);
	        for (int i = 0; i < pixels.length; i++) {
	            if (pixels[i] < threshold) pixels[i] = 0;
	            else pixels[i] = 255;
	        }
	        raster.setPixels(0, y, image.getWidth(), 1, pixels);
	    }
	    return result;
	}
	
	private BufferedImage flipVertically(BufferedImage imgIn) {
        AffineTransform tx = AffineTransform.getScaleInstance(1, -1);
        tx.translate(0, -imgIn.getHeight(null));
        AffineTransformOp op1 = new AffineTransformOp(tx, AffineTransformOp.TYPE_NEAREST_NEIGHBOR);
        imgIn = op1.filter(imgIn, null);
        return imgIn;
	}

	private BufferedImage flipHorizontally(BufferedImage imgIn) {
		AffineTransform tx = AffineTransform.getScaleInstance(-1, 1);
		tx.translate(-imgIn.getWidth(null), 0);
		AffineTransformOp op = new AffineTransformOp(tx, AffineTransformOp.TYPE_NEAREST_NEIGHBOR);
		imgIn = op.filter(imgIn, null);        
        return imgIn;
	}

	@Override
	public void setMap(String yamlFile) {
		String prefix = yamlFile.substring(0, yamlFile.indexOf(File.separator));
		this.setMapFileName(yamlFile, prefix, false, true);
	}
	
	public void setMapFileName(String mapYAMLFile, String prefix, boolean flipHorizontally, boolean flipVertically) {
		this.mapFileName = Missions.getProperty("image", mapYAMLFile);
		if (prefix != null) this.mapFileName = prefix + File.separator + this.mapFileName;
		while (!ready) {
			try { Thread.sleep(100); }
			catch (InterruptedException e) { e.printStackTrace(); }
		}
		if (mapFileName != null) {
			try {
				final OccupancyGrid occMap = node.getTopicMessageFactory().newFromType(OccupancyGrid._TYPE);
				BufferedImage img = ImageIO.read(new File(mapFileName));
				//img = toGrayScale(img);
				img = toBlackAndWhite(img, 128);
				if (flipHorizontally) img = flipHorizontally(img);
				if (flipVertically) img = flipVertically(img);
				System.out.println("Loaded map: " + img.getHeight() + "x" + img.getWidth());
				WritableRaster raster = img.getRaster();
				DataBufferByte data = (DataBufferByte)raster.getDataBuffer();
				ChannelBuffer buffer = ChannelBuffers.copiedBuffer(ByteOrder.nativeOrder(), data.getData());
				occMap.setData(buffer);
				occMap.getHeader().setFrameId(mapFrameID);
				occMap.getInfo().setHeight((int)(img.getHeight()));
				occMap.getInfo().setWidth((int)(img.getWidth()));
				geometry_msgs.Pose pose = node.getTopicMessageFactory().newFromType(geometry_msgs.Pose._TYPE);
				pose.getPosition().setX(0);
				pose.getPosition().setY(0);
				occMap.getInfo().setOrigin(pose);
				double res = Double.parseDouble(Missions.getProperty("resolution", mapYAMLFile));
				occMap.getInfo().setResolution((float)res);
				final Publisher<OccupancyGrid> publisher = node.newPublisher("/map", OccupancyGrid._TYPE);
				node.executeCancellableLoop(new CancellableLoop() {
					@Override
					protected void loop() throws InterruptedException {
						publisher.publish(occMap);
						Thread.sleep(1000);
					}
				});

			}
			catch (IOException e) { e.printStackTrace(); }	
		}

	}


	@Override
	public void displayRobotState(TrajectoryEnvelope te, RobotReport rr, String ... extraStatusInfo) {
		if (ready) {
			double x = rr.getPose().getX();
			double y = rr.getPose().getY();
			double theta = rr.getPose().getTheta();

			visualization_msgs.Marker marker = node.getTopicMessageFactory().newFromType(visualization_msgs.Marker._TYPE);
			marker.getHeader().setFrameId(mapFrameID);
			marker.getScale().setX(0.2f);
			marker.getColor().setR(100f);
			marker.getColor().setG(0.0f);
			marker.getColor().setB(0.0f);
			marker.getColor().setA(0.8f);
			marker.setAction(visualization_msgs.Marker.ADD);                                
			marker.setNs("current_pose");
			marker.setType(visualization_msgs.Marker.LINE_STRIP);
			marker.setId(rr.getRobotID());
			marker.setLifetime(new Duration(10.0));

			ArrayList<geometry_msgs.Point> points = new ArrayList<geometry_msgs.Point>();
			Coordinate[] coords = TrajectoryEnvelope.getFootprint(te.getFootprint(), x, y, theta).getCoordinates();
			for (Coordinate coord : coords) {
				geometry_msgs.Point point = node.getTopicMessageFactory().newFromType(geometry_msgs.Point._TYPE);
				point.setX(coord.x);
				point.setY(coord.y);
				point.setZ(0.0);
				points.add(point);
			}
			points.add(points.get(0));
			marker.setPoints(points);
			if (!this.robotStatusPublishers.containsKey(rr.getRobotID())) {
				Publisher<visualization_msgs.MarkerArray> markerArrayPublisher = node.newPublisher("robot"+rr.getRobotID()+"/status", visualization_msgs.MarkerArray._TYPE);
				this.robotStatusPublishers.put(rr.getRobotID(), markerArrayPublisher);
				synchronized(robotStatusMarkers) {
					this.robotStatusMarkers.put(rr.getRobotID(), new ArrayList<visualization_msgs.Marker>());
				}
			}
			synchronized(robotStatusMarkers) {
				this.robotStatusMarkers.get(rr.getRobotID()).add(marker);
			}

			//////////////
			visualization_msgs.Marker markerName = node.getTopicMessageFactory().newFromType(visualization_msgs.Marker._TYPE);
			markerName.getHeader().setFrameId(mapFrameID);
			markerName.getScale().setX(1.0f);
			markerName.getScale().setY(1.0f);
			markerName.getScale().setZ(1.0f);
			float R = 0.0f;
			float G = 0.0f;
			float B = 0.0f;
			float A = 1.0f;
			if (darkColors) {
				R = 1.0f;
				G = 1.0f;
				B = 1.0f;
				A = 0.8f;
			}
			markerName.getColor().setR(R);
            markerName.getColor().setG(G);
            markerName.getColor().setB(B);
            markerName.getColor().setA(A);
			markerName.setAction(visualization_msgs.Marker.ADD);                                
			markerName.setNs("robot_state");
			markerName.setType(visualization_msgs.Marker.TEXT_VIEW_FACING);
			//markerName.setId(te.getRobotID());
			markerName.setLifetime(new Duration(10.0));
			String markerText  = "R" + te.getRobotID() + ": " + rr.getPathIndex();
			if (extraStatusInfo != null) for (String extra : extraStatusInfo) markerText += "\n" + extra;
			markerName.setText(markerText);
			geometry_msgs.Pose pose = node.getTopicMessageFactory().newFromType(geometry_msgs.Pose._TYPE);
			geometry_msgs.Point pos = node.getTopicMessageFactory().newFromType(geometry_msgs.Point._TYPE);
			pos.setX(x);
			pos.setY(y);
			pos.setZ(0);
			pose.setPosition(pos);
			markerName.setPose(pose);
			synchronized(robotStatusMarkers) {
				this.robotStatusMarkers.get(rr.getRobotID()).add(markerName);
			}
			//if (this.publishPartialEnvelope) createPartialEnvelopeGeometryMarker(te, rr.getPathIndex() >= 0 ? rr.getPathIndex() : te.getPathLength()-1, te.getPathLength()-1);
			if (this.publishPartialEnvelope && rr.getPathIndex() > 0) { 
				createPartialEnvelopeGeometryMarker(te, rr.getPathIndex(), te.getPathLength()-1);
			}
		}
	}
	
	@Override
	public void displayRobotState(Polygon fp, RobotReport rr, String ... extraStatusInfo) {
		if (ready) {
			double x = rr.getPose().getX();
			double y = rr.getPose().getY();
			double theta = rr.getPose().getTheta();

			visualization_msgs.Marker marker = node.getTopicMessageFactory().newFromType(visualization_msgs.Marker._TYPE);
			marker.getHeader().setFrameId(mapFrameID);
			marker.getScale().setX(0.2f);
			marker.getColor().setR(100.0f);
			marker.getColor().setG(0.0f);
			marker.getColor().setB(0.0f);
			marker.getColor().setA(0.8f);
			marker.setAction(visualization_msgs.Marker.ADD);                                
			marker.setNs("current_pose");
			marker.setType(visualization_msgs.Marker.LINE_STRIP);
			marker.setId(rr.getRobotID());
			marker.setLifetime(new Duration(10.0));

			ArrayList<geometry_msgs.Point> points = new ArrayList<geometry_msgs.Point>();
			Coordinate[] coords = TrajectoryEnvelope.getFootprint(fp, x, y, theta).getCoordinates();
			for (Coordinate coord : coords) {
				geometry_msgs.Point point = node.getTopicMessageFactory().newFromType(geometry_msgs.Point._TYPE);
				point.setX(coord.x);
				point.setY(coord.y);
				point.setZ(0.0);
				points.add(point);
			}
			points.add(points.get(0));
			marker.setPoints(points);
			if (!this.robotStatusPublishers.containsKey(rr.getRobotID())) {
				Publisher<visualization_msgs.MarkerArray> markerArrayPublisher = node.newPublisher("robot"+rr.getRobotID()+"/status", visualization_msgs.MarkerArray._TYPE);
				this.robotStatusPublishers.put(rr.getRobotID(), markerArrayPublisher);
				synchronized(robotStatusMarkers) {
					this.robotStatusMarkers.put(rr.getRobotID(), new ArrayList<visualization_msgs.Marker>());
				}
			}
			synchronized(robotStatusMarkers) {
				this.robotStatusMarkers.get(rr.getRobotID()).add(marker);
			}

			//////////////
			visualization_msgs.Marker markerName = node.getTopicMessageFactory().newFromType(visualization_msgs.Marker._TYPE);
			markerName.getHeader().setFrameId(mapFrameID);
			markerName.getScale().setX(1.0f);
			markerName.getScale().setY(1.0f);
			markerName.getScale().setZ(1.0f);
			float R = 0.0f;
			float G = 0.0f;
			float B = 0.0f;
			float A = 1.0f;
			if (darkColors) {
				R = 1.0f;
				G = 1.0f;
				B = 1.0f;
				A = 0.8f;
			}
			markerName.getColor().setR(R);
            markerName.getColor().setG(G);
            markerName.getColor().setB(B);
            markerName.getColor().setA(A);
			markerName.setAction(visualization_msgs.Marker.ADD);                                
			markerName.setNs("robot_state");
			markerName.setType(visualization_msgs.Marker.TEXT_VIEW_FACING);
			//markerName.setId(te.getRobotID());
			markerName.setLifetime(new Duration(10.0));
			String markerText  = "R" + rr.getRobotID() + ": " + rr.getPathIndex();
			if (extraStatusInfo != null) for (String extra : extraStatusInfo) markerText += "\n" + extra;
			markerName.setText(markerText);
			geometry_msgs.Pose pose = node.getTopicMessageFactory().newFromType(geometry_msgs.Pose._TYPE);
			geometry_msgs.Point pos = node.getTopicMessageFactory().newFromType(geometry_msgs.Point._TYPE);
			pos.setX(x);
			pos.setY(y);
			pos.setZ(0);
			pose.setPosition(pos);
			markerName.setPose(pose);
			synchronized(robotStatusMarkers) {
				this.robotStatusMarkers.get(rr.getRobotID()).add(markerName);
			}

		}
	}
	
	public void displayBox(String markerLabel, Coordinate[] shape, int markerID, double x, double y, double durationInSeconds) {
		if (ready) {
			visualization_msgs.Marker marker = node.getTopicMessageFactory().newFromType(visualization_msgs.Marker._TYPE);
			marker.getHeader().setFrameId(mapFrameID);
			marker.getScale().setX(0.2f);
			marker.getColor().setR(0.0f);
			marker.getColor().setG(100.0f);
			marker.getColor().setB(0.0f);
			marker.getColor().setA(0.8f);
			marker.setAction(visualization_msgs.Marker.ADD);                                
			marker.setNs("box_marker");
			marker.setType(visualization_msgs.Marker.LINE_STRIP);
			marker.setId(markerID);
			marker.setLifetime(new Duration(durationInSeconds));

			ArrayList<geometry_msgs.Point> points = new ArrayList<geometry_msgs.Point>();
			for (Coordinate coord : shape) {
				geometry_msgs.Point point = node.getTopicMessageFactory().newFromType(geometry_msgs.Point._TYPE);
				point.setX(coord.x);
				point.setY(coord.y);
				point.setZ(0.0);
				points.add(point);
			}
			points.add(points.get(0));
			marker.setPoints(points);
			if (!this.boxMarkerPublishers.containsKey(markerLabel)) {
				Publisher<visualization_msgs.MarkerArray> markerArrayPublisher = node.newPublisher(markerLabel, visualization_msgs.MarkerArray._TYPE);
				this.boxMarkerPublishers.put(markerLabel, markerArrayPublisher);
				synchronized(boxMarkerMarkers) {
					this.boxMarkerMarkers.put(markerLabel, new ArrayList<visualization_msgs.Marker>());
				}
			}
			synchronized(boxMarkerMarkers) {
				this.boxMarkerMarkers.get(markerLabel).add(marker);
			}

			//////////////
			visualization_msgs.Marker markerName = node.getTopicMessageFactory().newFromType(visualization_msgs.Marker._TYPE);
			markerName.getHeader().setFrameId(mapFrameID);
			markerName.getScale().setX(1.0f);
			markerName.getScale().setY(1.0f);
			markerName.getScale().setZ(1.0f);
			markerName.getColor().setR(1.0f);
			markerName.getColor().setG(1.0f);
			markerName.getColor().setB(1.0f);
			markerName.getColor().setA(0.8f);
			markerName.setAction(visualization_msgs.Marker.ADD);                                
			markerName.setNs("label");
			markerName.setType(visualization_msgs.Marker.TEXT_VIEW_FACING);
			//markerName.setId(te.getRobotID());
			markerName.setLifetime(new Duration(durationInSeconds));
			markerName.setText(markerLabel);
			geometry_msgs.Pose pose = node.getTopicMessageFactory().newFromType(geometry_msgs.Pose._TYPE);
			geometry_msgs.Point pos = node.getTopicMessageFactory().newFromType(geometry_msgs.Point._TYPE);
			pos.setX(x);
			pos.setY(y);
			pos.setZ(0);
			pose.setPosition(pos);
			markerName.setPose(pose);
			synchronized(boxMarkerMarkers) {
				this.boxMarkerMarkers.get(markerLabel).add(markerName);
			}
		}
	}


	@Override
	public void displayDependency(RobotReport rrWaiting, RobotReport rrDriving, String dependencyDescriptor) {
		if(ready) {
			Pose from = rrWaiting.getPose();
			Pose to = rrDriving.getPose();
			visualization_msgs.Marker mArrow = node.getTopicMessageFactory().newFromType(visualization_msgs.Marker._TYPE);
			mArrow.setAction(visualization_msgs.Marker.ADD);
			mArrow.setNs(dependencyDescriptor);
			mArrow.setType(visualization_msgs.Marker.ARROW);
			ArrayList<geometry_msgs.Point> points = new ArrayList<geometry_msgs.Point>();		
			geometry_msgs.Point pointFrom = node.getTopicMessageFactory().newFromType(geometry_msgs.Point._TYPE);
			geometry_msgs.Point pointTo = node.getTopicMessageFactory().newFromType(geometry_msgs.Point._TYPE);
			pointFrom.setX(from.getX());
			pointFrom.setY(from.getY());
			pointFrom.setZ(0.0);
			points.add(pointFrom);
			pointTo.setX(to.getX());
			pointTo.setY(to.getY());
			pointTo.setZ(0.0);
			points.add(pointTo);
			mArrow.setPoints(points);
			mArrow.setId(dependencyDescriptor.hashCode());
			mArrow.getHeader().setFrameId(mapFrameID);
			mArrow.getScale().setX(0.4);
			mArrow.getScale().setY(1.0);
			mArrow.getScale().setZ(1.2);
			float R = 0.0f;
			float G = 0.0f;
			float B = 0.0f;
			float A = 0.3f;
			if (darkColors) {
				R = 15.0f;
				G = 100.0f;
				B = 200.0f;
				A = 0.2f;
			}
			mArrow.getColor().setR(R);
			mArrow.getColor().setG(G);
			mArrow.getColor().setB(B);
			mArrow.getColor().setA(A);
			mArrow.setLifetime(new Duration(1.0));
			if (!this.dependencyPublishers.containsKey(rrWaiting.getRobotID())) {
				Publisher<visualization_msgs.MarkerArray> markerArrayPublisher = node.newPublisher("robot"+rrWaiting.getRobotID()+"/deps", visualization_msgs.MarkerArray._TYPE);
				this.dependencyPublishers.put(rrWaiting.getRobotID(), markerArrayPublisher);
				synchronized(dependencyMarkers) {
					this.dependencyMarkers.put(rrWaiting.getRobotID(), new ArrayList<visualization_msgs.Marker>());
				}
			}
			synchronized(dependencyMarkers) {
				this.dependencyMarkers.get(rrWaiting.getRobotID()).add(mArrow);
			}
		}
	}

	@Override
	public void updateVisualization() {
		for (Entry<Integer, Publisher<MarkerArray>> entry : robotStatusPublishers.entrySet()) {
			synchronized(robotStatusMarkers) {
				if (!robotStatusMarkers.get(entry.getKey()).isEmpty()) { 
					visualization_msgs.MarkerArray ma = node.getTopicMessageFactory().newFromType(visualization_msgs.MarkerArray._TYPE);
					ArrayList<visualization_msgs.Marker> copy = new ArrayList<visualization_msgs.Marker>();
					for (visualization_msgs.Marker m : robotStatusMarkers.get(entry.getKey())) copy.add(m);
					if (envelopeMarkers != null) {
						synchronized(envelopeMarkers) {
							if (envelopeMarkers != null && envelopeMarkers.containsKey(entry.getKey())) copy.add(envelopeMarkers.get(entry.getKey()));
						}
					}
					ma.setMarkers(copy);
					entry.getValue().publish(ma);
					robotStatusMarkers.get(entry.getKey()).clear();
				}
			}
		}
		
		for (Entry<Integer, Publisher<MarkerArray>> entry : dependencyPublishers.entrySet()) {
			synchronized(dependencyMarkers) {
				if (!dependencyMarkers.get(entry.getKey()).isEmpty()) { 				
					visualization_msgs.MarkerArray ma = node.getTopicMessageFactory().newFromType(visualization_msgs.MarkerArray._TYPE);
					ArrayList<visualization_msgs.Marker> copy = new ArrayList<visualization_msgs.Marker>();
					for (visualization_msgs.Marker m : dependencyMarkers.get(entry.getKey())) copy.add(m);
					ma.setMarkers(copy);
					entry.getValue().publish(ma);
					dependencyMarkers.get(entry.getKey()).clear();
				}
			}
		}
		
		for (Entry<String, Publisher<MarkerArray>> entry : boxMarkerPublishers.entrySet()) {
			synchronized(boxMarkerMarkers) {
				if (!boxMarkerMarkers.get(entry.getKey()).isEmpty()) { 
					visualization_msgs.MarkerArray ma = node.getTopicMessageFactory().newFromType(visualization_msgs.MarkerArray._TYPE);
					ArrayList<visualization_msgs.Marker> copy = new ArrayList<visualization_msgs.Marker>();
					for (visualization_msgs.Marker m : boxMarkerMarkers.get(entry.getKey())) copy.add(m);
					ma.setMarkers(copy);
					entry.getValue().publish(ma);
					boxMarkerMarkers.get(entry.getKey()).clear();
				}
			}
		}

	}


	@Override
	public void onError(Node arg0, Throwable arg1) {
		// TODO Auto-generated method stub

	}


	@Override
	public void onShutdown(Node arg0) {
		// TODO Auto-generated method stub

	}


	@Override
	public void onShutdownComplete(Node arg0) {
		// TODO Auto-generated method stub

	}


	@Override
	public void onStart(ConnectedNode arg0) {
		this.node = arg0;		
		while (true) {
			try {
				node.getCurrentTime();
				break;
			}
			catch(NullPointerException e) { }
		}
		this.ready = true;
	}


	@Override
	public GraphName getDefaultNodeName() {
		return GraphName.of("coordinator_viz");
	}

	@Override
	public void addEnvelope(TrajectoryEnvelope te) {
		createPartialEnvelopeGeometryMarker(te, 0, te.getPathLength()-1);
	}
	
	private void createPartialEnvelopeGeometryMarker(TrajectoryEnvelope te, int indexFrom, int indexTo) {
		
		Coordinate[] verts = te.getPartialEnvelopeGeometry(indexFrom, indexTo).getCoordinates();

		visualization_msgs.Marker marker = node.getTopicMessageFactory().newFromType(visualization_msgs.Marker._TYPE);
		marker.getHeader().setFrameId(mapFrameID);
		marker.getScale().setX(0.1f);
		marker.getColor().setR(50f);
		marker.getColor().setG(50.0f);
		marker.getColor().setB(0.0f);
		marker.getColor().setA(0.8f);
		float R = 0.0f;
		float G = 0.0f;
		float B = 0.0f;
		float A = 0.7f;
		if (darkColors) {
			R = 50.0f;
			G = 50.0f;
			A = 0.8f;
		}
		marker.getColor().setR(R);
		marker.getColor().setG(G);
		marker.getColor().setB(B);
		marker.getColor().setA(A);
		marker.setAction(visualization_msgs.Marker.ADD);
		marker.setNs("current_envelope");
		marker.setType(visualization_msgs.Marker.LINE_STRIP);
		marker.setId(te.getRobotID());
		marker.setLifetime(new Duration(1.0));

		ArrayList<geometry_msgs.Point> points = new ArrayList<geometry_msgs.Point>();
		for (Coordinate coord : verts) {
			geometry_msgs.Point point = node.getTopicMessageFactory().newFromType(geometry_msgs.Point._TYPE);
			point.setX(coord.x);
			point.setY(coord.y);
			point.setZ(0.0);
			points.add(point);
		}
		points.add(points.get(0));
		marker.setPoints(points);

		if (this.envelopeMarkers == null) this.envelopeMarkers = new HashMap<Integer,visualization_msgs.Marker>();
		synchronized(envelopeMarkers) {
			this.envelopeMarkers.put(te.getRobotID(), marker);
		}

	}

	@Override
	public void removeEnvelope(TrajectoryEnvelope te) {
		if (this.envelopeMarkers == null) return;
		synchronized(envelopeMarkers) {
			this.envelopeMarkers.remove(te.getRobotID());
		}		
	}
	
	public static void main(String[] args) {
		System.out.println("test");
	}

	@Override
	public int periodicEnvelopeRefreshInMillis() {
		// TODO Auto-generated method stub
		return 0;
	}

}
import java.util.Comparator;
import org.metacsp.multi.spatioTemporal.paths.Pose;
import com.vividsolutions.jts.geom.Coordinate;
import se.oru.coordination.coordination_oru.NetworkConfiguration;
import se.oru.coordination.coordination_oru.ConstantAccelerationForwardModel;
import se.oru.coordination.coordination_oru.CriticalSection;
import se.oru.coordination.coordination_oru.Mission;
import se.oru.coordination.coordination_oru.RobotAtCriticalSection;
import se.oru.coordination.coordination_oru.RobotReport;
import se.oru.coordination.coordination_oru.TrajectoryEnvelopeCoordinator;
import se.oru.coordination.coordination_oru.demo.DemoDescription;
import se.oru.coordination.coordination_oru.motionplanning.ompl.ReedsSheppCarPlanner;
import se.oru.coordination.coordination_oru.simulation2D.TrajectoryEnvelopeCoordinatorSimulation;
import se.oru.coordination.coordination_oru.util.BrowserVisualization;
import se.oru.coordination.coordination_oru.util.JTSDrawingPanelVisualization;
import se.oru.coordination.coordination_oru.util.Missions;
import se.oru.coordination.coordination_oru.util.RVizVisualization;
package se.oru.coordination.coordination_oru.tests.clean;





@DemoDescription(desc = "Example showing coordination in opposing directions (following should happen here).")
public class TwoRobotsFollowing {

	public static void main(String[] args) throws InterruptedException {

		double MAX_ACCEL = 2.0;
		double MAX_VEL = 3.0;
		//Instantiate a trajectory envelope coordinator.
		//The TrajectoryEnvelopeCoordinatorSimulation implementation provides
		// -- the factory method getNewTracker() which returns a trajectory envelope tracker
		// -- the getCurrentTimeInMillis() method, which is used by the coordinator to keep time
		//You still need to add one or more comparators to determine robot orderings thru critical sections (comparators are evaluated in the order in which they are added)
		final TrajectoryEnvelopeCoordinatorSimulation tec = new TrajectoryEnvelopeCoordinatorSimulation(MAX_VEL,MAX_ACCEL);