/************************************************************************************
 * Copyright (C) 2008 by Politehnica University of Bucharest and Rutgers University
 * All rights reserved.
 * Refer to LICENSE for terms and conditions of use.
 ***********************************************************************************/
package vnsim.vehicular.routePlan;


import java.util.ArrayList;
import java.util.Iterator;

import vnsim.map.object.Cross;
import vnsim.map.object.Globals;
import vnsim.map.object.Point;
import vnsim.map.object.Road;
import vnsim.vehicular.routePlan.infrastructureRouted.DelayRecord;
import vnsim.vehicular.scenarios.Route;
import vnsim.vehicular.simulator.Location;
import vnsim.vehicular.simulator.RouteSegment;
import vnsim.vehicular.simulator.RoutingUtils;
import vnsim.vehicular.simulator.intersections.DirectedRoadSegment;


public class RouteComputingUtils {
	public final int SAME_AREA = 0;

	public final int AREA_TO_MAIN = 1;

	public final int MAIN_TO_MAIN = 2;

	public RouteComputingUtils() {

	}

	public ArrayList<Location> shortestDetour(Location src, Location dest,
		Location parentAvoid, Location nodeAvoid) {
		GraphNode avoid = new GraphNode(nodeAvoid.roadIdx, nodeAvoid.ptIdx);
		avoid.parentRoadIndex = parentAvoid.roadIdx;
		avoid.parentPointIndex = parentAvoid.ptIdx;

		return dijkstraPathByShortestDistance(src, dest, avoid);
	}

	public Route shortestPathByTimeAproximation(Location src, Location dest) {
		GraphNode avoid = new GraphNode(-1, -1);
		Route r;
		ArrayList<Location> points = null;

		points = dijkstraPathByTimeAproximation(src, dest, avoid);
		points.add(dest);
		if (points != null) {
			r = new Route();
			r.entry = src;
			r.exit = dest;
			r.route = RoutingUtils.mergeRoute(points);
			return r;
		}
		return null;
	}

	public ArrayList<Route> bestEightRoutes(Location src, Location dest) {
		ArrayList<Route> result = new ArrayList<Route>();
		Point s, d;
		int j;
		s = Globals.map.roads.get(src.roadIdx).points.get(src.ptIdx);
		d = Globals.map.roads.get(dest.roadIdx).points.get(dest.ptIdx);
		if (s.equals(d)) {
			// System.out
			// .println("The source and the destination are the same point");
			return result;
		}

		GraphNode avoidable;

		if (Globals.map.mapSpliter.inSameArea(
				s.areaCode.belongingTo.areaCodeByte,
				d.areaCode.belongingTo.areaCodeByte)) {
			// System.out.println("Same Road Areas");
			ArrayList<RouteSegment> ret;
			Route r;
			ArrayList<Location> points;
			avoidable = new GraphNode(-1, -1);

			points = dijkstraPathInSplittedMap(src, dest, avoidable, this.SAME_AREA);

			if (points != null) {

				points.add(new Location(dest.roadIdx, dest.ptIdx));

				ret = mergeRoute(points);
				r = new Route();
				r.entry = src;
				r.exit = dest;
				r.route = ret;
				result.add(r);
			} else {
				return null;
			}
			points = null;
			points = dijkstraPathInSplittedMap(src, dest, avoidable, this.SAME_AREA);
			if (points != null) {
				points.add(new Location(dest.roadIdx, dest.ptIdx));

				ret = mergeRoute(points);
				r = new Route();
				r.entry = src;
				r.exit = dest;
				r.route = ret;
				result.add(r);
			}

		} else {
			// System.out.println("Plec cu sursa=" + source.roadIdx + "-" +
			// source.ptIdx
			// + " destination=" + destination.roadIdx + "-" + destination.ptIdx);
			Route r;
			int k;
			ArrayList<RouteSegment> tempRet;
			int[] middleIdx = new int[8];
			ArrayList<ArrayList<Location>> allPoints = new ArrayList<ArrayList<Location>>();
			for (j = 0; j < 8; j++) {
				allPoints.add(new ArrayList<Location>());
			}

			ArrayList<Location> pDest, pMain, pSource;
			avoidable = new GraphNode(-1, -1);
			pSource = dijkstraPathInSplittedMap(src, null, avoidable, this.AREA_TO_MAIN);
			if (pSource == null) {
				System.out
						.println("No road was found between SOURCE and a main road");
				return null;
			}

			for (k = 0; k < pSource.size(); k++) {

				allPoints.get(0).add(
						new Location(pSource.get(k).roadIdx,
								pSource.get(k).ptIdx));
				allPoints.get(1).add(
						new Location(pSource.get(k).roadIdx,
								pSource.get(k).ptIdx));
				allPoints.get(2).add(
						new Location(pSource.get(k).roadIdx,
								pSource.get(k).ptIdx));
				allPoints.get(3).add(
						new Location(pSource.get(k).roadIdx,
								pSource.get(k).ptIdx));
				middleIdx[0] = middleIdx[1] = middleIdx[2] = middleIdx[3] = pSource
						.size() - 1;

			}
			pSource = dijkstraPathInSplittedMap(src, null, avoidable, this.AREA_TO_MAIN);
			if (pSource == null) {
				// System.out
				// .println("No second road was found between SOURCE and a main
				// road");
				middleIdx[4] = middleIdx[5] = middleIdx[6] = middleIdx[7] = -1;
			} else {

				for (k = 0; k < pSource.size(); k++) {

					allPoints.get(4).add(
							new Location(pSource.get(k).roadIdx,
									pSource.get(k).ptIdx));
					allPoints.get(5).add(
							new Location(pSource.get(k).roadIdx,
									pSource.get(k).ptIdx));
					allPoints.get(6).add(
							new Location(pSource.get(k).roadIdx,
									pSource.get(k).ptIdx));
					allPoints.get(7).add(
							new Location(pSource.get(k).roadIdx,
									pSource.get(k).ptIdx));
					middleIdx[4] = middleIdx[5] = middleIdx[6] = middleIdx[7] = pSource
							.size() - 1;

				}
			}
			// Destination area
			pSource = null;
			avoidable = new GraphNode(-1, -1);
			pSource = dijkstraPathInSplittedMap(dest, null, avoidable, this.AREA_TO_MAIN);
			if (pSource == null) {
				// System.out
				// .println("No road was found between Destination and a main
				// road");
				return null;
			}

			for (k = 0; k < pSource.size(); k++) {

				allPoints.get(0).add(
						new Location(
								pSource.get(pSource.size() - 1 - k).roadIdx,
								pSource.get(pSource.size() - 1 - k).ptIdx));
				allPoints.get(1).add(
						new Location(
								pSource.get(pSource.size() - 1 - k).roadIdx,
								pSource.get(pSource.size() - 1 - k).ptIdx));
				allPoints.get(6).add(
						new Location(
								pSource.get(pSource.size() - 1 - k).roadIdx,
								pSource.get(pSource.size() - 1 - k).ptIdx));
				allPoints.get(7).add(
						new Location(
								pSource.get(pSource.size() - 1 - k).roadIdx,
								pSource.get(pSource.size() - 1 - k).ptIdx));

			}

			pSource = dijkstraPathInSplittedMap(dest, null, avoidable, this.AREA_TO_MAIN);
			if (pSource == null) {
				// System.out
				// .println("No second road was found between Destination and a
				// main road");

			} else {

				for (k = 0; k < pSource.size(); k++) {

					allPoints.get(4)
							.add(
									new Location(pSource.get(pSource.size() - 1
											- k).roadIdx, pSource.get(pSource
											.size()
											- 1 - k).ptIdx));
					allPoints.get(5)
							.add(
									new Location(pSource.get(pSource.size() - 1
											- k).roadIdx, pSource.get(pSource
											.size()
											- 1 - k).ptIdx));
					allPoints.get(3)
							.add(
									new Location(pSource.get(pSource.size() - 1
											- k).roadIdx, pSource.get(pSource
											.size()
											- 1 - k).ptIdx));
					allPoints.get(2)
							.add(
									new Location(pSource.get(pSource.size() - 1
											- k).roadIdx, pSource.get(pSource
											.size()
											- 1 - k).ptIdx));

				}
			}

			for (j = 0; j < 8; j = j + 2) {
				// System.out.println("--------------------------");
				// System.out.println("-" + j + ": " + allPoints.get(j));
				// System.out.println("-" + (j + 1) + ": " + allPoints.get(j +
				// 1));
				avoidable = new GraphNode(-1, -1);
				if ((middleIdx[j] >= 0)
						|| (middleIdx[j] < (allPoints.get(j).size() - 1))) {
					// There have been found routes from the sorce to a main
					// road and from destination to main road
					pMain = null;
					// System.out.println("La poz " + (j) + " CAUT ruta intre "
					// + allPoints.get(j).get(middleIdx[j]) + " si "
					// + allPoints.get(j).get(middleIdx[j] + 1));
					pMain = dijkstraPathInSplittedMap(allPoints.get(j).get(middleIdx[j]),
							allPoints.get(j).get(middleIdx[j] + 1), avoidable,
							this.MAIN_TO_MAIN);
					if (pMain == null) {
						if (j < 4) {
							k = 0;
						} else {
							k = 1;
						}
						// System.out
						// .println("No initial route between two main roads for
						// source route"
						// + j / 2 + " and destination route " + k);
						allPoints.get(j).clear();
						allPoints.get(j + 1).clear();
						continue;
					} else {
						// System.out.println("gasit la " + j + " ruta "
						// + pMain.size() + " elemente: " + pMain);
						for (k = 1; k < pMain.size(); k++) {

							allPoints.get(j).add(
									middleIdx[j] + k,
									new Location(pMain.get(k).roadIdx, pMain
											.get(k).ptIdx));
						}
						// System.out.println("final la " + j + "este:"
						// + allPoints.get(j));
						// System.out.println("La poz "
						// + (j + 1)
						// + " CAUT intre "
						// + allPoints.get(j + 1).get(middleIdx[j + 1])
						// + " si "
						// + allPoints.get(j + 1)
						// .get(middleIdx[j + 1] + 1));
						pMain = dijkstraPathInSplittedMap(allPoints.get(j + 1).get(
								middleIdx[j + 1]), allPoints.get(j + 1).get(
								middleIdx[j + 1] + 1), avoidable,
								this.MAIN_TO_MAIN);
						if (pMain == null) {
							if (j < 4) {
								k = 0;
							} else {
								k = 1;
							}
							// System.out
							// .println("No second route between two main roads
							// for source route"
							// + j
							// / 2
							// + " and destination route "
							// + k);
							allPoints.get(j + 1).clear();
							continue;
						} else {
							// System.out.println("La poz " + (j + 1)
							// + " in mijloc am " + pMain.size()
							// + " elemente: " + pMain);
							for (k = 1; k < pMain.size(); k++) {
								allPoints.get(j + 1).add(
										middleIdx[j + 1] + k,
										new Location(pMain.get(k).roadIdx,
												pMain.get(k).ptIdx));
							}
							// System.out.println("final la " + (j + 1) +
							// "este:"
							// + allPoints.get(j + 1));
						}

					}

				} else {
					allPoints.get(j).clear();
					allPoints.get(j + 1).clear();
				}
			}

			for (j = 0; j < 8; j++) {
				// System.out.println("Route before merge: points="
				// + allPoints.get(j));
				tempRet = mergeRoute(allPoints.get(j));
				r = new Route();
				r.entry = src;
				r.exit = dest;
				r.route = tempRet;
				result.add(r);

			}
			// System.out.println("Different Road Areas");
		}

		return result;
	}

	public Route bestRoute(Location src, Location dest) {
		Route result = new Route();
		Point s, d;
		int j;
		s = Globals.map.roads.get(src.roadIdx).points.get(src.ptIdx);
		d = Globals.map.roads.get(dest.roadIdx).points.get(dest.ptIdx);
		
		System.out.println("area code");
		//System.out.println("area code " + s.areaCode.areaCodeToString());
		
		if (s.equals(d)) {
			// System.out.println("The source and the destination are the same
			// point");
			return result;
		}

		GraphNode avoidable;
		if (Globals.map.mapSpliter.inSameArea(
				s.areaCode.belongingTo.areaCodeByte,
				d.areaCode.belongingTo.areaCodeByte)) {
			// System.out.println("Same Road Areas");
			ArrayList<RouteSegment> ret;
			Route r;
			ArrayList<Location> points;
			avoidable = new GraphNode(-1, -1);

			points = dijkstraPathInSplittedMap(src, dest, avoidable, this.SAME_AREA);
			if (points != null) {

				points.add(new Location(dest.roadIdx, dest.ptIdx));

				ret = mergeRoute(points);
				r = new Route();
				r.entry = src;
				r.exit = dest;
				r.route = ret;
				result = r;
			} else {
				return null;
			}

		} else {
			Route r;
			int k;
			ArrayList<Location> pointsSrc = null, pointsDest = null, pointsMiddle = null;

			avoidable = new GraphNode(-1, -1);
			pointsSrc = dijkstraPathInSplittedMap(src, null, avoidable, this.AREA_TO_MAIN);
			avoidable = new GraphNode(-1, -1);
			pointsDest = dijkstraPathInSplittedMap(dest, null, avoidable, this.AREA_TO_MAIN);
			if (pointsDest == null || pointsSrc == null) {
				return null;
			} else {
				avoidable = new GraphNode(-1, -1);
				pointsMiddle = dijkstraPathInSplittedMap(
						pointsSrc.get(pointsSrc.size() - 1), pointsDest
								.get(pointsDest.size() - 1), avoidable,
						this.MAIN_TO_MAIN);
				if (pointsMiddle == null) {
					return null;
				} else {
					pointsMiddle.remove(0);
					pointsSrc.addAll(pointsMiddle);
					for (k = pointsDest.size() - 1; k >= 0; k--) {
						pointsSrc.add(pointsDest.get(k));
					}
					result.entry = new Location(src.roadIdx, src.ptIdx);
					result.exit = new Location(dest.roadIdx, dest.ptIdx);
					result.route = mergeRoute(pointsSrc);

				}
			}
		}
		return result;
	}

	private ArrayList<Location> dijkstraPathInSplittedMap(Location src, Location dest,
			GraphNode avoid, int type) {
		// System.out.println("Avoid is: " + avoid.roadIndex + " "
		// + avoid.pointIndex + " si parent " + avoid.parentRoadIndex
		// + " " + avoid.parentPointIndex);
		if (type == this.AREA_TO_MAIN) {
			// System.out.println("Looking for a main street starting from road"
			// + source.roadIdx + " and point " + source.ptIdx);
			
			if(Globals.map.mapSpliter.isOnBorder(Globals.map.roads.get(src.roadIdx).points.get(src.ptIdx).areaCode.belongingTo.areaCodeByte))
			{
				ArrayList<Location> routePoints = new ArrayList<Location>();
				routePoints.add(src);
				return routePoints;
			}
		}
		if (type == this.MAIN_TO_MAIN) {
			// System.out.println("Looking for a main routes btw r=" +
			// source.roadIdx
			// + " p= " + source.ptIdx + " and r=" + destination.roadIdx + " p="
			// + destination.ptIdx);
		}
		if (type == this.SAME_AREA) {
			// System.out.println("Looking for streets in same area rs="
			// + source.roadIdx + " p= " + source.ptIdx + " and sourceRoadIdx="
			// + destination.roadIdx + " p=" + destination.ptIdx);
		}

		Point s, d, tp;
		s = Globals.map.roads.get(src.roadIdx).points.get(src.ptIdx);
		if (type == this.AREA_TO_MAIN) {
			d = null;
		} else {
			d = Globals.map.roads.get(dest.roadIdx).points.get(dest.ptIdx);
		}

		ArrayList<Location> routePoints = new ArrayList<Location>();
		GraphNodeArrayList set = new GraphNodeArrayList();

		GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
		ArrayList sourceNeighbors = getNeighborsByTimeAproximation(sourceGraphNode);
		GraphNode tempNode;

		// add the direct neighbors of the source to the set of nodes;
		for (int i = 0; i < sourceNeighbors.size(); i++) {
			NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
			int rd = srcNeig.neigh.roadIndex;
			int pt = srcNeig.neigh.pointIndex;

			if (type == this.AREA_TO_MAIN) {
				if (Globals.map.mapSpliter
						.inSameArea(
								Globals.map.roads.get(rd).points.get(pt).areaCode.belongingTo.areaCodeByte,
								s.areaCode.belongingTo.areaCodeByte)) {
					tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
							srcNeig.dist);
					if ((avoid.roadIndex == -1)
							|| ((avoid.roadIndex >= 0) && avoid
									.differentFromJustIdx(tempNode))) {
						set.addGraphNode(tempNode);
					}

				}

			}

			if (type == this.MAIN_TO_MAIN) {

				if (Globals.map.mapSpliter
						.isOnBorder(Globals.map.roads.get(rd).points.get(pt).areaCode.belongingTo.areaCodeByte)) {

					tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
							srcNeig.dist);
					if ((avoid.roadIndex == -1)
							|| ((avoid.roadIndex >= 0) && avoid
									.differentFromJustIdx(tempNode))) {
						set.addGraphNode(tempNode);
					}
				}
			}

			if (type == this.SAME_AREA) {
				if (Globals.map.mapSpliter
						.inSameArea(
								Globals.map.roads.get(rd).points.get(pt).areaCode.belongingTo.areaCodeByte,
								s.areaCode.belongingTo.areaCodeByte)) {
					tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
							srcNeig.dist);

					if ((avoid.roadIndex == -1)
							|| ((avoid.roadIndex >= 0) && avoid
									.differentFromJustIdx(tempNode))) {
						set.addGraphNode(tempNode);
					}
				}

			}

		}

		GraphNodeArrayList solved = new GraphNodeArrayList();
		int maxIdx = -1;
		double maxVal = 100000.0;

		solved.addGraphNode(sourceGraphNode);

		boolean finalizare = false;
		while (true) {
			// get the first node
			GraphNode first;

			first = (GraphNode) set.removeFirst();
			if (first == null) {
				if (avoid.roadIndex < 0) {
					return null;
				}
				// System.out.println("AAAAAAAAAAAAAAAAAAADDDDDD avoid");
				first = new GraphNode(avoid);
				avoid.roadIndex = -1;
			}
			solved.addGraphNode(first);
			// System.out.println("Solved:"+first+";
			// solved="+solved.size()+";unsolved="+set.size());

			finalizare = false;

			if (type == this.MAIN_TO_MAIN) {
				if (isPointOnRoad(first.roadIndex, first.pointIndex,
						dest.roadIdx)) {
					finalizare = true;
				}

			}

			if (type == this.AREA_TO_MAIN) {
				tp = Globals.map.roads.get(first.roadIndex).points
						.get(first.pointIndex);
				if (Globals.map.mapSpliter
						.isOnBorder(tp.areaCode.belongingTo.areaCodeByte)) {
					finalizare = true;
				}

			}
			if (type == this.SAME_AREA) {
				if (isPointOnRoad(first.roadIndex, first.pointIndex,
						dest.roadIdx)) {
					finalizare = true;
				}
			}

			if (finalizare) {
				// first is solved; try to get the route built into 'ret'
				GraphNode aux = first;

				routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
				while (true) {
					if (aux.parentRoadIndex == -1 || aux.parentPointIndex == -1) {
						break;
					}

					routePoints.add(0, new Location(aux.parentRoadIndex,
							aux.parentPointIndex));

					// System.out.println("Parent: road "+aux.parentRoadIndex+";
					// point "+aux.parentPointIndex+";
					// distance="+aux.distance+"--");
					// find the parent in the 'solved' set
					Road rdAux = (Road) Globals.map.roads
							.get(aux.parentRoadIndex);
					Point ptAux = (Point) rdAux.points
							.get(aux.parentPointIndex);
					Iterator iter = solved.iterator();
					boolean found = false;
					while (iter.hasNext()) {
						GraphNode aux2 = (GraphNode) iter.next();
						Road rdAux2 = (Road) Globals.map.roads
								.get(aux2.roadIndex);
						Point ptAux2 = (Point) rdAux2.points
								.get(aux2.pointIndex);
						if (ptAux2.equals(ptAux)) {
							if ((aux.distance - aux2.distance < maxVal)) {
								maxVal = aux.distance - aux2.distance;
								maxIdx = routePoints.size();

							}
							aux = aux2;
							found = true;

							break;
						}
					}
					if (found == false) {
						System.out.println("ERROR! Parent NOT FOUND!:road "
								+ aux.parentRoadIndex + "; point "
								+ aux.parentPointIndex);
						break;
					} else {
					}
				}
				break;
			}

			// relaxation
			ArrayList vecini = getNeighborsByTimeAproximation(first);

			// System.out.println("Neighbors for "+first+":"+vecini);

			for (int j = 0; j < vecini.size(); j++) {
				NeighborNode n = (NeighborNode) vecini.get(j);
				if (solved.contains(n.neigh))
					continue;
				// only if it's not solved

				Point ptAux = (Point) (((Road) (Globals.map.roads
						.get(n.neigh.roadIndex))).points
						.get(n.neigh.pointIndex));
				double oldDist = getDistance(set, ptAux);
				double d1 = n.dist;
				if (oldDist == -1.0) {
					finalizare = false;
					if (type == this.AREA_TO_MAIN) {
						if (Globals.map.mapSpliter
								.inSameArea(
										Globals.map.roads
												.get(n.neigh.roadIndex).points
												.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte,
										s.areaCode.belongingTo.areaCodeByte)) {
							finalizare = true;
						}
					}
					if (type == this.MAIN_TO_MAIN) {
						if (Globals.map.mapSpliter
								.isOnBorder(Globals.map.roads
										.get(n.neigh.roadIndex).points
										.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte)) {
							finalizare = true;
						}
					}
					if (type == this.SAME_AREA) {
						if (Globals.map.mapSpliter
								.inSameArea(
										Globals.map.roads
												.get(n.neigh.roadIndex).points
												.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte,
										s.areaCode.belongingTo.areaCodeByte)) {
							finalizare = true;
						}
					}
					if (finalizare) {
						n.neigh.distance = d1 + first.distance;
						n.neigh.parentPointIndex = first.pointIndex;
						n.neigh.parentRoadIndex = first.roadIndex;

						if ((avoid.roadIndex == -1)
								|| ((avoid.roadIndex >= 0) && avoid
										.differentFromJustIdx(n.neigh))) {
							set.addGraphNode(n.neigh);
						}

					}
				} else {
					if (d1 + first.distance < oldDist) {
						finalizare = false;
						if (type == this.AREA_TO_MAIN) {
							if (Globals.map.mapSpliter
									.inSameArea(
											Globals.map.roads
													.get(n.neigh.roadIndex).points
													.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte,
											s.areaCode.belongingTo.areaCodeByte)) {
								finalizare = true;
							}
						}
						if (type == this.MAIN_TO_MAIN) {
							if (Globals.map.mapSpliter
									.isOnBorder(Globals.map.roads
											.get(n.neigh.roadIndex).points
											.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte)) {
								finalizare = true;
							}
						}
						if (type == this.SAME_AREA) {
							if (Globals.map.mapSpliter
									.inSameArea(
											Globals.map.roads
													.get(n.neigh.roadIndex).points
													.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte,
											s.areaCode.belongingTo.areaCodeByte)) {
								finalizare = true;
							}
						}
						if (finalizare) {
							n.neigh.distance = d1 + first.distance;
							n.neigh.parentPointIndex = first.pointIndex;
							n.neigh.parentRoadIndex = first.roadIndex;
							if ((avoid.roadIndex == -1)
									|| ((avoid.roadIndex >= 0) && avoid
											.differentFromJustIdx(n.neigh))) {
								set.addGraphNode(n.neigh);
							}
						}
					}
				}
			}
		}

		// now I have the 'routePoints' vector filled with Location objects;
		// do merging stuff; obtain the rez vector filled with RouteSegment
		// objects

		avoid.roadIndex = routePoints
				.get((int) (routePoints.size() - maxIdx + 1)).roadIdx;
		avoid.pointIndex = routePoints
				.get((int) (routePoints.size() - maxIdx + 1)).ptIdx;
		avoid.parentRoadIndex = routePoints
				.get((int) (routePoints.size() - maxIdx)).roadIdx;
		avoid.parentPointIndex = routePoints
				.get((int) (routePoints.size() - maxIdx)).ptIdx;
		avoid.distance = Integer.MAX_VALUE - 1;
		;
		// avoid.distance =0;
		return routePoints;
	}

	public static ArrayList<RouteSegment> mergeRoute(
			ArrayList<Location> routePoints) {
		// takes a vector of Location objects (points) and returns a vector of
		// RouteSegment objects,
		// representing the route as a sequence of roads, rather than individual
		// points

		int p1, pLastCand, p2; // point indexes
		int r1, r2; // road indexes
		int index = 0;
		ArrayList<RouteSegment> ret = new ArrayList<RouteSegment>();
		// System.out.println("Primesc la intrare "+routePoints.toString());
		if (routePoints == null || routePoints.size() == 0) {
			// System.out
			// .println("ERROR! routingModule.dijkstraPath() - routePoints has
			// no points!");
			return ret;
		}

		p1 = ((Location) routePoints.get(0)).ptIdx;
		r1 = ((Location) routePoints.get(0)).roadIdx;
		pLastCand = ((Location) routePoints.get(0)).ptIdx;

		index++;

		while (index < routePoints.size()) {
			p2 = ((Location) routePoints.get(index)).ptIdx;
			r2 = ((Location) routePoints.get(index)).roadIdx;
			if (r2 == r1) {
				pLastCand = p2;
			} else {
				// p2 could also be on r1; try to find it
				boolean found = false;
				Point pt1 = (Point) (((Road) (Globals.map.roads.get(r2))).points
						.get(p2));
				Road road1 = (Road) Globals.map.roads.get(r1);
				for (int i = 0; i < road1.points.size(); i++) {
					Point pt2 = (Point) road1.points.get(i);
					if (pt1.equals(pt2)) {
						found = true;
						ret.add(new RouteSegment((short) r1, (short) p1,
								(short) i));
						p1 = p2;
						r1 = r2;
						pLastCand = p2;
						break;
					}
				}
				if (!found) {
					// find (r1,pLastCand) on r2
					Point ptLC2 = (Point) (((Road) (Globals.map.roads.get(r1))).points
							.get(pLastCand));
					Road road2 = (Road) Globals.map.roads.get(r2);
					for (int i = 0; i < road2.points.size(); i++) {
						Point pt2 = (Point) road2.points.get(i);
						if (pt2.equals(ptLC2)) {
							found = true;
							ret.add(new RouteSegment((short) r1, (short) p1,
									(short) pLastCand));
							p1 = i;
							r1 = r2;
							pLastCand = p2;
							break;
						}
					}
				}
				if (!found) {
					System.out
							.println("RoutingModule.mergePoints() - situation not implemented!; returning null!");
					System.out.println("for rute {" + r1 + ", " + p1
							+ "]  si [" + r2 + " ," + p2 + " ]");
					return null;
					// return new ArrayList<RouteSegment>();
				}
			}
			index++;
			if (index == routePoints.size()) {
				// it is the last iteration; add the last RouteSegment;
				ret.add(new RouteSegment((short) r1, (short) p1,
						(short) pLastCand));
			}
		}
		return ret;
	}

	public double getAproxTimeForSegment(int rd, int pt1, int pt2) {
		double rez = 0.0, speed;
		Road currentRoad = Globals.map.roads.get(rd);
		int j, lanePercent;
		if (pt1 > pt2) {
			j = pt1;
			pt1 = pt2;
			pt2 = j;
		}
		if (currentRoad.getRoadinfo()[1] > 50) {
			speed = 50.0;
		} else {
			speed = 130.0;
		}
		rez = (double) (currentRoad.points.get(pt2).getDistance());

		rez = rez - (double) (currentRoad.points.get(pt1)).getDistance();
		lanePercent = currentRoad.laneNo;
		if (currentRoad.oneWay == 0) {
			lanePercent = lanePercent / 2;
		}

		rez = (double) (rez * ((((5 - lanePercent) * Globals.routePlanConstants.increasePerLane) + 100)))
				/ (speed * 100.0);

		return rez;

	}

	public ArrayList getNeighborsByDistance(GraphNode node) {
		// get all the direct neighbors of 'node'; returns a vector of
		// 'NeighborNode' objects
		ArrayList<NeighborNode> rez = new ArrayList<NeighborNode>();

		// find the closest points on the same road which are cross-points
		// if there is none, take the end points of the road (except when the
		// node is already an end point);
		// also check if there are cross-roads in 'node';
		Road road = (Road) Globals.map.roads.get(node.roadIndex);
		int nodeIndex = node.pointIndex;

		int min = Integer.MAX_VALUE;
		int max = -1;
		for (int i = 0; i < road.crosses.size(); i++) {
			Cross c = (Cross) road.crosses.get(i);
			int idx = c.getPointIndex();
			if (idx > max && idx < nodeIndex)
				max = idx;
			if (idx < min && idx > nodeIndex)
				min = idx;
			Point pt = (Point) road.points.get(nodeIndex);
			if (idx == nodeIndex) {
				// have to go on the crossing road, to find the closest points;
				Road crossingRoad = (Road) Globals.map.roads.get(c
						.getCrossRoadIndex());
				Point pt2 = null;
				int index2 = -1;
				// have to find the index of the point on the crossing road
				for (int j = 0; j < crossingRoad.points.size(); j++) {
					Point ptAux = (Point) crossingRoad.points.get(j);
					if (ptAux.equals(pt)) {
						// that's the one
						pt2 = ptAux;
						index2 = j;
						break;
					}
				}
				if (pt2 == null) {
					// System.out.println("RoutingModule.getNeighbors(): ERROR!
					// Could not find the point on the crossing
					// road:"+c.getCrossRoadIndex());
				} else {
					// find the neighbors on the crossing road;
					int minCross = Integer.MAX_VALUE;
					int maxCross = -1;
					for (int k = 0; k < crossingRoad.crosses.size(); k++) {
						Cross otherCross = (Cross) crossingRoad.crosses.get(k);
						int otherIndex = otherCross.getPointIndex();
						if (otherIndex > maxCross && otherIndex < index2)
							maxCross = otherIndex;
						if (otherIndex < minCross && otherIndex > index2)
							minCross = otherIndex;

					}
					if (minCross == Integer.MAX_VALUE) {
						// there was no min; have to consider last point of
						// crossing road
						// only if it is not the last
						if ((index2 != (crossingRoad.points.size() - 1))) {
							NeighborNode n = new NeighborNode(
									c.getCrossRoadIndex(),
									crossingRoad.points.size() - 1,
									((Point) (crossingRoad.points
											.get(crossingRoad.points.size() - 1)))
											.getDistance()
											- ((Point) (crossingRoad.points
													.get(index2)))
													.getDistance());
							rez.add(n);
						}
					} else {
						NeighborNode n = new NeighborNode(
								c.getCrossRoadIndex(), minCross,
								((Point) (crossingRoad.points.get(minCross)))
										.getDistance()
										- ((Point) (crossingRoad.points
												.get(index2))).getDistance());
						rez.add(n);
					}
					if (maxCross == -1) {
						// there was no max; have to consider first point of
						// crossing road
						// only if it is not first
						if (index2 != 0) {
							NeighborNode n = new NeighborNode(c
									.getCrossRoadIndex(), 0,
									((Point) (crossingRoad.points.get(index2)))
											.getDistance()
											- ((Point) (crossingRoad.points
													.get(0))).getDistance());
							rez.add(n);
						}
					} else {
						NeighborNode n = new NeighborNode(
								c.getCrossRoadIndex(), maxCross,
								((Point) (crossingRoad.points.get(index2)))
										.getDistance()
										- ((Point) (crossingRoad.points
												.get(maxCross))).getDistance());
						rez.add(n);
					}
				}
			}
		}
		if (min == Integer.MAX_VALUE) {
			// there was no min; have to consider last point of the road
			// ONLY if I am not the last!
			if (((road.points.size() - 1) != node.pointIndex)) {
				NeighborNode n = new NeighborNode(node.roadIndex, road.points
						.size() - 1, ((Point) (road.points.get(road.points
						.size() - 1))).getDistance()
						- ((Point) (road.points.get(node.pointIndex)))
								.getDistance());
				rez.add(n);
			}
		} else {
			NeighborNode n = new NeighborNode(node.roadIndex, min,
					((Point) (road.points.get(min))).getDistance()
							- ((Point) (road.points.get(node.pointIndex)))
									.getDistance());
			rez.add(n);
		}
		if (max == -1) {
			// there was no max; have to consider first point of the road
			// only if I am not first
			if (node.pointIndex != 0) {
				NeighborNode n = new NeighborNode(node.roadIndex, 0,
						((Point) (road.points.get(node.pointIndex)))
								.getDistance()
								- ((Point) (road.points.get(0))).getDistance());
				rez.add(n);
			}
		} else {
			NeighborNode n = new NeighborNode(node.roadIndex, max,
					((Point) (road.points.get(node.pointIndex))).getDistance()
							- ((Point) (road.points.get(max))).getDistance());
			rez.add(n);
		}
		return rez;
	}

	public ArrayList getNeighborsByTimeAproximation(GraphNode node) {
		// get all the direct neighbors of 'node'; returns a vector of
		// 'NeighborNode' objects
		ArrayList<NeighborNode> rez = new ArrayList<NeighborNode>();

		// find the closest points on the same road which are cross-points
		// if there is none, take the end points of the road (except when the
		// node is already an end point);
		// also check if there are cross-roads in 'node';
		Road road = (Road) Globals.map.roads.get(node.roadIndex);
		int nodeIndex = node.pointIndex;

		int min = Integer.MAX_VALUE;
		int max = -1;
		for (int i = 0; i < road.crosses.size(); i++) {
			Cross c = (Cross) road.crosses.get(i);
			int idx = c.getPointIndex();
			if (idx > max && idx < nodeIndex)
				max = idx;
			if (idx < min && idx > nodeIndex)
				min = idx;
			Point pt = (Point) road.points.get(nodeIndex);
			if (idx == nodeIndex) {
				// have to go on the crossing road, to find the closest points;
				Road crossingRoad = (Road) Globals.map.roads.get(c
						.getCrossRoadIndex());
				Point pt2 = null;
				int index2 = -1;
				// have to find the index of the point on the crossing road
				for (int j = 0; j < crossingRoad.points.size(); j++) {
					Point ptAux = (Point) crossingRoad.points.get(j);
					if (ptAux.equals(pt)) {
						// that's the one
						pt2 = ptAux;
						index2 = j;
						break;
					}
				}
				if (pt2 == null) {
					// System.out.println("RoutingModule.getNeighbors(): ERROR!
					// Could not find the point on the crossing
					// road:"+c.getCrossRoadIndex());
				} else {
					// find the neighbors on the crossing road;
					int minCross = Integer.MAX_VALUE;
					int maxCross = -1;
					for (int k = 0; k < crossingRoad.crosses.size(); k++) {
						Cross otherCross = (Cross) crossingRoad.crosses.get(k);
						int otherIndex = otherCross.getPointIndex();
						if (otherIndex > maxCross && otherIndex < index2)
							maxCross = otherIndex;
						if (otherIndex < minCross && otherIndex > index2)
							minCross = otherIndex;

					}
					if (minCross == Integer.MAX_VALUE) {
						// there was no min; have to consider last point of
						// crossing road
						// only if it is not the last
						if ((index2 != (crossingRoad.points.size() - 1))) {
							NeighborNode n = new NeighborNode(c
									.getCrossRoadIndex(), crossingRoad.points
									.size() - 1, getAproxTimeForSegment(c
									.getCrossRoadIndex(), index2,
									crossingRoad.points.size() - 1));
							// ((Point) (crossingRoad.points
							// .get(crossingRoad.points.size() - 1)))
							// .getDistance()
							// - ((Point) (crossingRoad.points
							// .get(index2)))
							// .getDistance());
							rez.add(n);
						}
					} else {
						NeighborNode n = new NeighborNode(
								c.getCrossRoadIndex(), minCross,
								getAproxTimeForSegment(c.getCrossRoadIndex(),
										minCross, index2));
						// ((Point) (crossingRoad.points.get(minCross)))
						// .getDistance()
						// - ((Point) (crossingRoad.points
						// .get(index2))).getDistance());
						rez.add(n);
					}
					if (maxCross == -1) {
						// there was no max; have to consider first point of
						// crossing road
						// only if it is not first
						if (index2 != 0) {
							NeighborNode n = new NeighborNode(c
									.getCrossRoadIndex(), 0,
									getAproxTimeForSegment(c
											.getCrossRoadIndex(), 0, index2));
							// ((Point) (crossingRoad.points.get(index2)))
							// .getDistance()
							// - ((Point) (crossingRoad.points
							// .get(0))).getDistance());
							rez.add(n);
						}
					} else {
						NeighborNode n = new NeighborNode(
								c.getCrossRoadIndex(), maxCross,
								getAproxTimeForSegment(c.getCrossRoadIndex(),
										index2, maxCross));
						// ((Point) (crossingRoad.points.get(index2)))
						// .getDistance()
						// - ((Point) (crossingRoad.points
						// .get(maxCross))).getDistance());
						rez.add(n);
					}
				}
			}
		}
		if (min == Integer.MAX_VALUE) {
			// there was no min; have to consider last point of the road
			// ONLY if I am not the last!
			if (((road.points.size() - 1) != node.pointIndex)) {
				NeighborNode n = new NeighborNode(node.roadIndex, road.points
						.size() - 1, getAproxTimeForSegment(node.roadIndex,
						node.pointIndex, road.points.size() - 1));
				// ((Point) (road.points.get(road.points
				// .size() - 1))).getDistance()
				// - ((Point) (road.points.get(node.pointIndex)))
				// .getDistance());
				rez.add(n);
			}
		} else {
			NeighborNode n = new NeighborNode(
					node.roadIndex,
					min,
					getAproxTimeForSegment(node.roadIndex, min, node.pointIndex));
			// ((Point) (road.points.get(min))).getDistance()
			// - ((Point) (road.points.get(node.pointIndex)))
			// .getDistance());
			rez.add(n);
		}
		if (max == -1) {
			// there was no max; have to consider first point of the road
			// only if I am not first
			if (node.pointIndex != 0) {
				NeighborNode n = new NeighborNode(node.roadIndex, 0,
						getAproxTimeForSegment(node.roadIndex, 0,
								node.pointIndex));
				// ((Point) (road.points.get(node.pointIndex)))
				// .getDistance()
				// - ((Point) (road.points.get(0))).getDistance());
				rez.add(n);
			}
		} else {
			NeighborNode n = new NeighborNode(
					node.roadIndex,
					max,
					getAproxTimeForSegment(node.roadIndex, node.pointIndex, max));
			// ((Point) (road.points.get(node.pointIndex))).getDistance()
			// - ((Point) (road.points.get(max))).getDistance());
			rez.add(n);
		}
		return rez;
	}

	private double getDistance(GraphNodeArrayList t, Point p) {
		// get the distance from the set for the corresponding point;
		Iterator iter = t.iterator();
		while (iter.hasNext()) {
			GraphNode n = (GraphNode) iter.next();
			Point pct = (Point) (((Road) (Globals.map.roads.get(n.roadIndex))).points
					.get(n.pointIndex));
			if (pct.equals(p))
				return n.distance;
		}
		// System.out.println("ERROR! getDistance() NOT FOUND!\n");
		return -1.0;
	}

	private boolean isPointOnRoad(int roadIndex, int pointIndex,
			int otherRoadIndex) {
		// [roadIndex,pointIndex] is also situated on "otherRoadIndex" ?
		Location loc = new Location(roadIndex, pointIndex);
		for (int i = 0; i < Globals.map.roads.get(otherRoadIndex).points.size(); i++) {
			Location otherLoc = new Location(otherRoadIndex, i);
			if (loc.equals(otherLoc))
				return true;
		}
		return false;
	}

	private ArrayList<Location> dijkstraPathByShortestDistance(Location src, Location dest,
			GraphNode avoid) {
		// returns a vector of consecutive RouteSegments, the shortest route
		// between source and destination

		// System.out.println("Have to compute a route: from "+source+" -- to
		// "+destination);

		ArrayList<Location> routePoints = new ArrayList<Location>();

		GraphNodeArrayList set = new GraphNodeArrayList();

		// Point destPoint=(Point)
		// ((Road)(Globals.map.roads.get(destination.roadIndex))).points.get(destination.pt1);

		GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
		ArrayList sourceNeighbors = getNeighborsByDistance(sourceGraphNode);
		GraphNode tempNode;
		// add the direct neighbors of the source to the set of nodes;
		for (int i = 0; i < sourceNeighbors.size(); i++) {
			NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
			int rd = srcNeig.neigh.roadIndex;
			int pt = srcNeig.neigh.pointIndex;
			tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
					srcNeig.dist);
			if ((avoid.roadIndex == -1)
					|| ((avoid.roadIndex >= 0) && avoid.differentFrom(tempNode))) {
				set.addGraphNode(tempNode);
			}

		}

		GraphNodeArrayList solved = new GraphNodeArrayList();

		solved.addGraphNode(sourceGraphNode);

		while (true) {
			// get the first node
			GraphNode first = null;
			first = (GraphNode) set.removeFirst();
			if (first == null) {
				return null;
			}
			solved.addGraphNode(first);
			// System.out.println("Solved:"+first+";
			// solved="+solved.size()+";unsolved="+set.size());
			// if (isPointOnRoad(first.roadIndex, first.pointIndex,
			// destination.roadIdx)) {
			if ((first.roadIndex == dest.roadIdx)
					&& (first.pointIndex == dest.ptIdx)) {
				// System.out.println("Solved is "+first);
				// first is solved; try to get the route built into 'ret'
				GraphNode aux = first;
				routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
				while (true) {
					if (aux.parentRoadIndex == -1 || aux.parentPointIndex == -1) {
						break;
					}

					routePoints.add(0, new Location(aux.parentRoadIndex,
							aux.parentPointIndex));

					// System.out.println("Parent: road "+aux.parentRoadIndex+";
					// point "+aux.parentPointIndex+";
					// distance="+aux.distance+"--");
					// find the parent in the 'solved' set
					Road rdAux = (Road) Globals.map.roads
							.get(aux.parentRoadIndex);
					Point ptAux = (Point) rdAux.points
							.get(aux.parentPointIndex);
					Iterator iter = solved.iterator();
					boolean found = false;
					while (iter.hasNext()) {
						GraphNode aux2 = (GraphNode) iter.next();
						Road rdAux2 = (Road) Globals.map.roads
								.get(aux2.roadIndex);
						Point ptAux2 = (Point) rdAux2.points
								.get(aux2.pointIndex);
						if (ptAux2.equals(ptAux)) {
							aux = aux2;
							found = true;
							break;
						}
					}
					if (found == false) {
						System.out.println("ERROR! Parent NOT FOUND!:road "
								+ aux.parentRoadIndex + "; point "
								+ aux.parentPointIndex);
						break;
					} else {
					}
				}
				break;
			}
			if (first.containsLocation(dest)) {
				// System.out.println("Solved is "+first);
				// first is solved; try to get the route built into 'ret'
				GraphNode aux = first;
				first.roadIndex = dest.roadIdx;
				first.pointIndex = dest.ptIdx;

				routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
				while (true) {
					if (aux.parentRoadIndex == -1 || aux.parentPointIndex == -1) {
						break;
					}

					routePoints.add(0, new Location(aux.parentRoadIndex,
							aux.parentPointIndex));

					// System.out.println("Parent: road "+aux.parentRoadIndex+";
					// point "+aux.parentPointIndex+";
					// distance="+aux.distance+"--");
					// find the parent in the 'solved' set
					Road rdAux = (Road) Globals.map.roads
							.get(aux.parentRoadIndex);
					Point ptAux = (Point) rdAux.points
							.get(aux.parentPointIndex);
					Iterator iter = solved.iterator();
					boolean found = false;
					while (iter.hasNext()) {
						GraphNode aux2 = (GraphNode) iter.next();
						Road rdAux2 = (Road) Globals.map.roads
								.get(aux2.roadIndex);
						Point ptAux2 = (Point) rdAux2.points
								.get(aux2.pointIndex);
						if (ptAux2.equals(ptAux)) {
							aux = aux2;
							found = true;
							break;
						}
					}
					if (found == false) {
						System.out.println("ERROR! Parent NOT FOUND!:road "
								+ aux.parentRoadIndex + "; point "
								+ aux.parentPointIndex);
						break;
					} else {
					}
				}
				break;
			}

			// relaxation
			ArrayList vecini = getNeighborsByDistance(first);

			// System.out.println("Neighbors for "+first+":"+vecini);

			for (int j = 0; j < vecini.size(); j++) {
				NeighborNode n = (NeighborNode) vecini.get(j);
				if (solved.contains(n.neigh))
					continue;
				// only if it's not solved

				Point ptAux = (Point) (((Road) (Globals.map.roads
						.get(n.neigh.roadIndex))).points
						.get(n.neigh.pointIndex));
				double oldDist = getDistance(set, ptAux);
				double d1 = n.dist;
				if (oldDist == -1.0) {
					// System.out.println("ERROR!;
					// roadIndex="+n.neigh.roadIndex+";
					// ptIndex="+n.neigh.pointIndex+";
					// lat="+ptAux.getLatitude()+";long="+ptAux.getLongitude());
					n.neigh.distance = d1 + first.distance;
					n.neigh.parentPointIndex = first.pointIndex;
					n.neigh.parentRoadIndex = first.roadIndex;
					if ((avoid.roadIndex == -1)
							|| ((avoid.roadIndex >= 0) && avoid
									.differentFrom(n.neigh))) {
						set.addGraphNode(n.neigh);
					}

				} else {
					if (d1 + first.distance < oldDist) {
						// set.remove(n.neigh);
						n.neigh.distance = d1 + first.distance;
						n.neigh.parentPointIndex = first.pointIndex;
						n.neigh.parentRoadIndex = first.roadIndex;
						// set.add(n.neigh);
						if ((avoid.roadIndex == -1)
								|| ((avoid.roadIndex >= 0) && avoid
										.differentFrom(n.neigh))) {
							set.addGraphNode(n.neigh);
						}
					}
				}
			}
		}

		// now I have the 'routePoints' vector filled with Location objects;
		// do merging stuff; obtain the rez vector filled with RouteSegment
		// objects

		// System.out.println("Route before merge: points=" + routePoints);
		return routePoints;
	}

	private ArrayList<Location> dijkstraPathTest(Location src, Location dest,
			GraphNode avoid) {
		// returns a vector of consecutive RouteSegments, the shortest route
		// between source and destination

		// System.out.println("Have to compute a route: from "+source+" -- to
		// "+destination);

		ArrayList<Location> routePoints = new ArrayList<Location>();

		GraphNodeArrayList set = new GraphNodeArrayList();

		// Point destPoint=(Point)
		// ((Road)(Globals.map.roads.get(destination.roadIndex))).points.get(destination.pt1);

		GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
		ArrayList sourceNeighbors = getNeighborsByDistance(sourceGraphNode);
		GraphNode tempNode;
		// add the direct neighbors of the source to the set of nodes;
		for (int i = 0; i < sourceNeighbors.size(); i++) {
			NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
			int rd = srcNeig.neigh.roadIndex;
			int pt = srcNeig.neigh.pointIndex;
			tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
					srcNeig.dist);
			if (avoid.roadIndex == -1) {
				set.addGraphNode(tempNode);
			} else {
				if ((avoid.roadIndex >= 0) && avoid.differentFromJustIdx(tempNode)) {
					set.addGraphNode(tempNode);
				}

			}
		}
		GraphNodeArrayList solved = new GraphNodeArrayList();

		solved.addGraphNode(sourceGraphNode);

		int maxIdx = -1;
		double maxVal = Double.MAX_VALUE;
		while (true) {
			// get the first node
			GraphNode first = null;
			first = (GraphNode) set.removeFirst();
			if (first == null) {
				// System.out.println("null de aici1");
				return null;
			}

			solved.addGraphNode(first);
			// System.out.println("Solved:"+first+";
			// solved="+solved.size()+";unsolved="+set.size());
			// if (isPointOnRoad(first.roadIndex, first.pointIndex,
			// destination.roadIdx)) {
			if (isPointOnRoad(first.roadIndex, first.pointIndex, dest.roadIdx)) {
				// System.out.println("Solved is "+first);
				// first is solved; try to get the route built into 'ret'
				GraphNode aux = first;
				routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
				while (true) {
					if (aux.parentRoadIndex == -1 || aux.parentPointIndex == -1) {
						break;
					}

					routePoints.add(0, new Location(aux.parentRoadIndex,
							aux.parentPointIndex));

					// System.out.println("Parent: road "+aux.parentRoadIndex+";
					// point "+aux.parentPointIndex+";
					// distance="+aux.distance+"--");
					// find the parent in the 'solved' set
					Road rdAux = (Road) Globals.map.roads
							.get(aux.parentRoadIndex);
					Point ptAux = (Point) rdAux.points
							.get(aux.parentPointIndex);
					Iterator iter = solved.iterator();
					boolean found = false;
					while (iter.hasNext()) {
						GraphNode aux2 = (GraphNode) iter.next();
						Road rdAux2 = (Road) Globals.map.roads
								.get(aux2.roadIndex);
						Point ptAux2 = (Point) rdAux2.points
								.get(aux2.pointIndex);
						if (ptAux2.equals(ptAux)) {
							if ((aux.distance - aux2.distance < maxVal)) {
								maxVal = aux.distance - aux2.distance;
								maxIdx = routePoints.size();

							}

							aux = aux2;
							found = true;
							break;
						}
					}
					if (found == false) {
						System.out.println("ERROR! Parent NOT FOUND!:road "
								+ aux.parentRoadIndex + "; point "
								+ aux.parentPointIndex);
						break;
					} else {
					}
				}
				break;
			}

			// relaxation
			ArrayList vecini = getNeighborsByDistance(first);

			// System.out.println("Neighbors for "+first+":"+vecini);

			for (int j = 0; j < vecini.size(); j++) {
				NeighborNode n = (NeighborNode) vecini.get(j);
				if (solved.contains(n.neigh))
					continue;
				// only if it's not solved

				Point ptAux = (Point) (((Road) (Globals.map.roads
						.get(n.neigh.roadIndex))).points
						.get(n.neigh.pointIndex));
				double oldDist = getDistance(set, ptAux);
				double d1 = n.dist;
				if (oldDist == -1.0) {
					// System.out.println("ERROR!;
					// roadIndex="+n.neigh.roadIndex+";
					// ptIndex="+n.neigh.pointIndex+";
					// lat="+ptAux.getLatitude()+";long="+ptAux.getLongitude());
					n.neigh.distance = d1 + first.distance;
					n.neigh.parentPointIndex = first.pointIndex;
					n.neigh.parentRoadIndex = first.roadIndex;
					if (avoid.roadIndex == -1) {
						set.addGraphNode(n.neigh);
					} else {

						if ((avoid.roadIndex >= 0)
								&& avoid.differentFromJustIdx(n.neigh)) {
							set.addGraphNode(n.neigh);
						}
					}

				} else {
					if (d1 + first.distance < oldDist) {
						// set.remove(n.neigh);
						n.neigh.distance = d1 + first.distance;
						n.neigh.parentPointIndex = first.pointIndex;
						n.neigh.parentRoadIndex = first.roadIndex;
						// set.add(n.neigh);
						if ((avoid.roadIndex == -1)) {
							set.addGraphNode(n.neigh);
						} else {
							if ((avoid.roadIndex >= 0)
									&& avoid.differentFromJustIdx(n.neigh)) {
								set.addGraphNode(n.neigh);
							}
						}
					}
				}
			}
		}

		// now I have the 'routePoints' vector filled with Location objects;
		// do merging stuff; obtain the rez vector filled with RouteSegment
		// objects
		if (maxIdx > -1) {
			avoid.roadIndex = routePoints.get((int) (routePoints.size()
					- maxIdx + 1)).roadIdx;
			avoid.pointIndex = routePoints.get((int) (routePoints.size()
					- maxIdx + 1)).ptIdx;
			avoid.parentRoadIndex = routePoints
					.get((int) (routePoints.size() - maxIdx)).roadIdx;
			avoid.parentPointIndex = routePoints
					.get((int) (routePoints.size() - maxIdx)).ptIdx;
			avoid.distance = Integer.MAX_VALUE - 1;
		}
		// System.out.println("Route before merge: points=" + routePoints);
		// System.out.println("null de aici2");
		return routePoints;
	}

	private ArrayList<Location> dijkstraPathByTimeAproximation(Location src, Location dest,
			GraphNode avoid) {
		// returns a vector of consecutive RouteSegments, the shortest route
		// between source and destination

		// System.out.println("Have to compute a route: from "+source+" -- to
		// "+destination);

		ArrayList<Location> routePoints = new ArrayList<Location>();

		GraphNodeArrayList set = new GraphNodeArrayList();

		// Point destPoint=(Point)
		// ((Road)(Globals.map.roads.get(destination.roadIndex))).points.get(destination.pt1);

		GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
		ArrayList sourceNeighbors = getNeighborsByTimeAproximation(sourceGraphNode);
		GraphNode tempNode;
		// add the direct neighbors of the source to the set of nodes;
		for (int i = 0; i < sourceNeighbors.size(); i++) {
			NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
			int rd = srcNeig.neigh.roadIndex;
			int pt = srcNeig.neigh.pointIndex;
			tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
					srcNeig.dist);
			// if (avoid.roadIndex == -1) {
			// set.addGraphNode(tempNode);
			// } else {
			// if ((avoid.roadIndex >= 0) && avoid.differentFrom2(tempNode)) {
			set.addGraphNode(tempNode);
			// }
			//
			// }
		}
		GraphNodeArrayList solved = new GraphNodeArrayList();

		solved.addGraphNode(sourceGraphNode);

		int maxIdx = -1;
		double maxVal = Double.MAX_VALUE;
		while (true) {
			// get the first node
			GraphNode first = null;
			first = (GraphNode) set.removeFirst();
			if (first == null) {
				// System.out.println("null de aici1");
				return null;
			}

			solved.addGraphNode(first);
			// System.out.println("Solved:"+first+";
			// solved="+solved.size()+";unsolved="+set.size());
			// if (isPointOnRoad(first.roadIndex, first.pointIndex,
			// destination.roadIdx)) {
			if (isPointOnRoad(first.roadIndex, first.pointIndex, dest.roadIdx)) {
				// System.out.println("Solved is "+first);
				// first is solved; try to get the route built into 'ret'
				GraphNode aux = first;
				routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
				while (true) {
					if (aux.parentRoadIndex == -1 || aux.parentPointIndex == -1) {
						break;
					}

					routePoints.add(0, new Location(aux.parentRoadIndex,
							aux.parentPointIndex));

					// System.out.println("Parent: road "+aux.parentRoadIndex+";
					// point "+aux.parentPointIndex+";
					// distance="+aux.distance+"--");
					// find the parent in the 'solved' set
					Road rdAux = (Road) Globals.map.roads
							.get(aux.parentRoadIndex);
					Point ptAux = (Point) rdAux.points
							.get(aux.parentPointIndex);
					Iterator iter = solved.iterator();
					boolean found = false;
					while (iter.hasNext()) {
						GraphNode aux2 = (GraphNode) iter.next();
						Road rdAux2 = (Road) Globals.map.roads
								.get(aux2.roadIndex);
						Point ptAux2 = (Point) rdAux2.points
								.get(aux2.pointIndex);
						if (ptAux2.equals(ptAux)) {
							// if ((aux.distance - aux2.distance < maxVal)) {
							// maxVal = aux.distance - aux2.distance;
							// maxIdx = routePoints.size();
							//
							// }

							aux = aux2;
							found = true;
							break;
						}
					}
					if (found == false) {
						System.out.println("ERROR! Parent NOT FOUND!:road "
								+ aux.parentRoadIndex + "; point "
								+ aux.parentPointIndex);
						break;
					} else {
					}
				}
				break;
			}

			// relaxation
			ArrayList vecini = getNeighborsByTimeAproximation(first);

			// System.out.println("Neighbors for "+first+":"+vecini);

			for (int j = 0; j < vecini.size(); j++) {
				NeighborNode n = (NeighborNode) vecini.get(j);
				if (solved.contains(n.neigh))
					continue;
				// only if it's not solved

				Point ptAux = (Point) (((Road) (Globals.map.roads
						.get(n.neigh.roadIndex))).points
						.get(n.neigh.pointIndex));
				double oldDist = getDistance(set, ptAux);
				double d1 = n.dist;
				if (oldDist == -1.0) {
					// System.out.println("ERROR!;
					// roadIndex="+n.neigh.roadIndex+";
					// ptIndex="+n.neigh.pointIndex+";
					// lat="+ptAux.getLatitude()+";long="+ptAux.getLongitude());
					n.neigh.distance = d1 + first.distance;
					n.neigh.parentPointIndex = first.pointIndex;
					n.neigh.parentRoadIndex = first.roadIndex;
					// if (avoid.roadIndex == -1) {
					// set.addGraphNode(n.neigh);
					// } else {
					//
					// if ((avoid.roadIndex >= 0)
					// && avoid.differentFrom2(n.neigh)) {
					set.addGraphNode(n.neigh);
					// }
					// }

				} else {
					if (d1 + first.distance < oldDist) {
						// set.remove(n.neigh);
						n.neigh.distance = d1 + first.distance;
						n.neigh.parentPointIndex = first.pointIndex;
						n.neigh.parentRoadIndex = first.roadIndex;
						// // set.add(n.neigh);
						// if ((avoid.roadIndex == -1)) {
						// set.addGraphNode(n.neigh);
						// } else {
						// if ((avoid.roadIndex >= 0)
						// && avoid.differentFrom2(n.neigh)) {
						set.addGraphNode(n.neigh);
						// }
						// }
					}
				}
			}
		}

		// now I have the 'routePoints' vector filled with Location objects;
		// do merging stuff; obtain the rez vector filled with RouteSegment
		// objects
		if (maxIdx > -1) {
			avoid.roadIndex = routePoints.get((int) (routePoints.size()
					- maxIdx + 1)).roadIdx;
			avoid.pointIndex = routePoints.get((int) (routePoints.size()
					- maxIdx + 1)).ptIdx;
			avoid.parentRoadIndex = routePoints
					.get((int) (routePoints.size() - maxIdx)).roadIdx;
			avoid.parentPointIndex = routePoints
					.get((int) (routePoints.size() - maxIdx)).ptIdx;
			avoid.distance = Integer.MAX_VALUE - 1;
		}
		// System.out.println("Route before merge: points=" + routePoints);
		// System.out.println("null de aici2");
		return routePoints;
	}

//	public double getValueForRute(ArrayList<RouteSegment> rute) {
//		double rez = 0.0;
//		int i, j, k;
//		double speed, lanePercent, distance;
//		Road currentRoad;
//		Point currentPoint;
//		RouteSegment currentSegment;
//		int pt1, pt2;
//		for (i = 0; i < rute.size(); i++) {
//			currentSegment = rute.get(i);
//			currentRoad = Globals.map.roads.get(currentSegment.roadIndex);
//
//			pt1 = currentSegment.pt1;
//			pt2 = currentSegment.pt2;
//			if (pt1 > pt2) {
//				j = pt1;
//				pt1 = pt2;
//				pt2 = j;
//			}
//			if (currentRoad.getRoadinfo()[1] > 50) {
//				speed = 50.0;
//			} else {
//				speed = 130.0;
//			}
//			distance = (double) (currentRoad.points.get(pt2).getDistance());
//
//			distance = distance
//					- (double) (currentRoad.points.get(pt1)).getDistance();
//			lanePercent = currentRoad.laneNo;
//			if (currentRoad.oneWay == 0) {
//				lanePercent = lanePercent / 2;
//			}
//
//			rez = rez
//					+ (double) (distance * ((((5 - lanePercent) * Globals.routePlanConstants.increasePerLane) + 100)))
//					/ (speed * 100.0);
//
//		}
//
//		return rez;
//	}

	public static double getDistanceForRoute(RouteSegment[] route) {
		double dist = 0.0, d;
		int i, p1, p2, k;
		if (route == null) {
			return -1.0;
		}
		RouteSegment s;
		for (i = 0; i < route.length; i++) {
			s = route[i];
			p1 = s.pt1;
			p2 = s.pt2;
			if (p2 < p1) {
				k = p2;
				p2 = p1;
				p1 = k;
			}
			d = (double) Globals.map.roads.get(s.roadIndex).points.get(p2)
					.getDistance()
					- Globals.map.roads.get(s.roadIndex).points.get(p1)
							.getDistance();
			d = Math.abs(d);
			dist = dist + d;
		}

		return dist;
	}

	class GraphNode {
		// a point on the discrete map;
		// used for the dijkstra implementation;

		public int roadIndex;

		public int pointIndex; // these two uniquely identify the node;

		int parentRoadIndex;

		int parentPointIndex; // these two uniquely identify the parent

		public double distance; // used in the dijkstra implementation

		public GraphNode(int roadIndex, int pointIndex) {
			this.roadIndex = roadIndex;
			this.pointIndex = pointIndex;
			parentRoadIndex = -1;
			parentPointIndex = -1;
			distance = Double.MAX_VALUE;
		}

		public boolean differentFromJustIdx(GraphNode t) {

			if ((this.parentPointIndex != t.parentPointIndex)
					|| (this.parentRoadIndex != t.parentRoadIndex)
					|| (this.pointIndex != t.pointIndex)
					|| (this.roadIndex != t.roadIndex)) {

				return true;
			}
			return false;
		}

		public boolean differentFrom(GraphNode t) {
			// System.out.println("Compar parent:"+this.parentRoadIndex+"
			// sourcePointIdx="+this.parentPointIndex+ "cu parent"+t.parentRoadIndex+" cu
			// p"+t.parentPointIndex);
			// System.out.println("si copill:"+this.roadIndex+"
			// sourcePointIdx="+this.pointIndex+ "cu parent"+t.roadIndex+" cu
			// p"+t.pointIndex);
			boolean dif1 = false, dif2 = false, found = false;
			int i, j;
			Cross c;
			if ((t.parentRoadIndex == this.parentRoadIndex)
					&& (t.parentPointIndex != this.parentPointIndex)) {
				dif1 = true;

			} else {
				if (t.parentRoadIndex != this.parentRoadIndex) {

					for (i = 0; i < Globals.map.roads.get(t.parentRoadIndex).crosses
							.size(); i++) {
						c = Globals.map.roads.get(t.parentRoadIndex).crosses
								.get(i);
						if (c.getCrossRoadIndex() == this.parentRoadIndex) {
							if ((c.getPointIndex() == t.parentPointIndex)
									&& (c.getCrossPointIndex() == this.parentPointIndex)) {
								found = true;
								break;
							}
						}
					}
					if (!found) {
						dif1 = true;
					}

				}

			}

			///////fara parent
			found = false;
			if ((t.roadIndex == this.roadIndex)
					&& (t.pointIndex != this.pointIndex)) {
				dif2 = true;

			} else {
				if (t.roadIndex != this.roadIndex) {

					for (i = 0; i < Globals.map.roads.get(t.roadIndex).crosses
							.size(); i++) {
						c = Globals.map.roads.get(t.roadIndex).crosses.get(i);
						if (c.getCrossRoadIndex() == this.roadIndex) {
							if ((c.getPointIndex() == t.pointIndex)
									&& (c.getCrossPointIndex() == this.pointIndex)) {
								found = true;
								break;
							}
						}
					}
					if (!found) {
						dif2 = true;
					}

				}

			}

			return (dif1 || dif2);
		}

		public GraphNode(int roadIndex, int pointIndex, int parentRoadIndex,
				int parentPointIndex, double distance) {
			this.roadIndex = roadIndex;
			this.pointIndex = pointIndex;
			this.parentRoadIndex = parentRoadIndex;
			this.parentPointIndex = parentPointIndex;
			this.distance = distance;
		}

		public GraphNode(GraphNode g) {
			this.roadIndex = g.roadIndex;
			this.pointIndex = g.pointIndex;
			this.parentRoadIndex = g.parentRoadIndex;
			this.parentPointIndex = g.parentPointIndex;
			this.distance = g.distance;
		}

		public boolean equals(GraphNode n) {
			Point p1 = (Point) ((Road) (Globals.map.roads.get(roadIndex))).points
					.get(pointIndex);
			Point p2 = (Point) ((Road) (Globals.map.roads.get(n.roadIndex))).points
					.get(n.pointIndex);
			return p1.equals(p2);
		}

		public String toString() {
			String ret = "[Road=" + roadIndex + ";Point=" + pointIndex + "]";
			return ret;
		}

		public boolean containsLocation(Location dest) {
			int i, j, p1, p2;
			Cross c;
			Road r;
			if (dest.roadIdx != this.roadIndex) {
				return false;
			}
			if (this.parentRoadIndex != this.roadIndex) {

				for (i = 0; i < Globals.map.roads.get(this.parentRoadIndex).crosses
						.size(); i++) {
					if (Globals.map.roads.get(this.parentRoadIndex).crosses
							.get(i).getCrossRoadIndex() == this.roadIndex) {
						p1 = Globals.map.roads.get(this.parentRoadIndex).crosses
								.get(i).getCrossPointIndex();
						p2 = this.pointIndex;
						if (p1 < p2) {
							if (p1 < dest.ptIdx && dest.ptIdx < p2) {
								return true;
							}
						} else {
							if (p1 > dest.ptIdx && dest.ptIdx > p2) {
								return true;
							}
						}

					}
				}

			} else {

				p1 = this.parentPointIndex;
				p2 = this.pointIndex;
				if (p1 < p2) {
					if (p1 < dest.ptIdx && dest.ptIdx < p2) {
						return true;
					}
				} else {
					if (p1 > dest.ptIdx && dest.ptIdx > p2) {
						return true;
					}
				}

			}

			return false;
		}
	}

	class NeighborNode {
		GraphNode neigh;

		double dist; // distance to that neighbor

		public NeighborNode(int roadIndex, int pointIndex, double dist) {
			this.dist = dist;
			neigh = new GraphNode(roadIndex, pointIndex);
		}

		public String toString() {
			String ret = "[Neigh:" + neigh + " ; dist=" + dist + "]";
			return ret;
		}
	}

	class GraphNodeArrayList {
		// a vector of "graph nodes"; it keeps the graph nodes ordered (performs
		// an insertion sorting, when
		// adding an element); it also prevent duplicates

		ArrayList<GraphNode> v;

		public GraphNodeArrayList() {
			v = new ArrayList<GraphNode>();
		}

		public void addGraphNode(GraphNode n) {
			// insertion-sorting add, based on the 'distance' of the graph nodes
			for (int i = 0; i < v.size(); i++) {
				GraphNode node = (GraphNode) v.get(i);
				if (n.distance < node.distance) {
					v.add(i, n);
					return;
				}
			}
			v.add(n);
		}

		public GraphNode removeFirst() {
			// gets the first node and removes it from the collection
			if (v.size() > 0) {
				GraphNode n = (GraphNode) v.get(0);
				v.remove(0);
				return n;
			}
			return null;
		}

		public Iterator iterator() {
			return v.iterator();
		}

		public void setGraphNode(GraphNode n) {
			// looks through the vector, finds a graph node referring to the
			// same point (by
			// checking the roadSegment of the point), and replaces that graph
			// node;
			Point pct1 = (Point) (((Road) (Globals.map.roads.get(n.roadIndex))).points
					.get(n.pointIndex));
			for (int i = 0; i < v.size(); i++) {
				GraphNode node = (GraphNode) v.get(i);
				Point pct2 = (Point) (((Road) (Globals.map.roads
						.get(node.roadIndex))).points.get(node.pointIndex));
				if (pct1.equals(pct2)) {
					// that's the point; replace it
					v.set(i, n);
				}
			}
		}

		public int size() {
			return v.size();
		}

		public boolean contains(GraphNode node) {
			for (int i = 0; i < v.size(); i++) {
				GraphNode node1 = (GraphNode) v.get(i);
				if (node1.equals(node))
					return true;
			}
			return false;
		}
	}

//	public ArrayList<Route> bestSPRoutes(Location src, Location dest) {
//		ArrayList<Route> rez = new ArrayList<Route>();
//		int i;
//		Route r = null;
//		ArrayList<Location> points = null;
//		GraphNode avoid = new GraphNode(-1, -1);
//		// System.out.println("dij cu source=" + source + "si destination=" + destination
//		// + " si avoid" + avoid);
//		i = 0;
//		while (i < 3) {
//
//			points = null;
//			points = dijkstraPathTest(src, dest, avoid);
//			// System.out.println("cu avoid dijjkstra da " + points);
//			r = new Route();
//			r.route = mergeRoute(points);
//
//			if (r.route == null) {
//				break;
//			}
//			// System.out.println("dupa merge " + r.route);
//			r.entry = src;
//			r.exit = dest;
//			rez.add(r);
//			i++;
//
//		}
//
//		return null;
//	}

	// //////////////////////////////////////////////////////////////

	public ArrayList<Location> dijkstraPathWithDelay(Location src,
			Location dest, Location parentAvoid, Location nodeAvoid) {
		// returns a vector of consecutive RouteSegments, the shortest route
		// between source and destination

		// System.out.println("Have to compute a route: from "+source+" -- to
		// "+destination);
		GraphNode avoid = new GraphNode(-1, -1);
		avoid.parentRoadIndex = parentAvoid.roadIdx;
		avoid.parentPointIndex = parentAvoid.ptIdx;
		avoid.roadIndex = nodeAvoid.roadIdx;
		avoid.pointIndex = nodeAvoid.ptIdx;

		//		System.out.println("avoid is " + avoid.parentRoadIndex + " "
		//				+ avoid.parentPointIndex + " spre " + avoid.roadIndex + " "
		//				+ avoid.pointIndex);

		ArrayList<Location> routePoints = new ArrayList<Location>();

		GraphNodeArrayList set = new GraphNodeArrayList();

		// Point destPoint=(Point)
		// ((Road)(Globals.map.roads.get(destination.roadIndex))).points.get(destination.pt1);

		GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
		ArrayList sourceNeighbors = getNeighborsWithDelay(sourceGraphNode);
		GraphNode tempNode;
		// add the direct neighbors of the source to the set of nodes;
		for (int i = 0; i < sourceNeighbors.size(); i++) {
			NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
			int rd = srcNeig.neigh.roadIndex;
			int pt = srcNeig.neigh.pointIndex;
			tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
					srcNeig.dist);
			if (avoid.roadIndex == -1) {
				set.addGraphNode(tempNode);
			} else {
				if ((avoid.roadIndex >= 0)
						&& avoid.differentFrom(tempNode)) {
					set.addGraphNode(tempNode);
				}

			}

		}

		GraphNodeArrayList solved = new GraphNodeArrayList();

		solved.addGraphNode(sourceGraphNode);

		while (true) {
			// get the first node
			GraphNode first = null;
			first = (GraphNode) set.removeFirst();
			if (first == null) {
				return null;
			}
			solved.addGraphNode(first);
			// System.out.println("Solved:"+first+";
			// solved="+solved.size()+";unsolved="+set.size());
			// if (isPointOnRoad(first.roadIndex, first.pointIndex,
			// destination.roadIdx)) {
			if ((first.roadIndex == dest.roadIdx)
					&& (first.pointIndex == dest.ptIdx)) {
				// System.out.println("Solved is "+first);
				// first is solved; try to get the route built into 'ret'
				GraphNode aux = first;
				routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
				while (true) {
					if (aux.parentRoadIndex == -1 || aux.parentPointIndex == -1) {
						break;
					}

					routePoints.add(0, new Location(aux.parentRoadIndex,
							aux.parentPointIndex));

					// System.out.println("Parent: road "+aux.parentRoadIndex+";
					// point "+aux.parentPointIndex+";
					// distance="+aux.distance+"--");
					// find the parent in the 'solved' set
					Road rdAux = (Road) Globals.map.roads
							.get(aux.parentRoadIndex);
					Point ptAux = (Point) rdAux.points
							.get(aux.parentPointIndex);
					Iterator iter = solved.iterator();
					boolean found = false;
					while (iter.hasNext()) {
						GraphNode aux2 = (GraphNode) iter.next();
						Road rdAux2 = (Road) Globals.map.roads
								.get(aux2.roadIndex);
						Point ptAux2 = (Point) rdAux2.points
								.get(aux2.pointIndex);
						if (ptAux2.equals(ptAux)) {
							aux = aux2;
							found = true;
							break;
						}
					}
					if (found == false) {
						System.out.println("ERROR! Parent NOT FOUND!:road "
								+ aux.parentRoadIndex + "; point "
								+ aux.parentPointIndex);
						break;
					} else {
					}
				}
				break;
			}

			if (first.containsLocation(dest)) {
				// System.out.println("Solved is "+first);
				// first is solved; try to get the route built into 'ret'
				GraphNode aux = first;
				first.roadIndex = dest.roadIdx;
				first.pointIndex = dest.ptIdx;

				routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
				while (true) {
					if (aux.parentRoadIndex == -1 || aux.parentPointIndex == -1) {
						break;
					}

					routePoints.add(0, new Location(aux.parentRoadIndex,
							aux.parentPointIndex));

					// System.out.println("Parent: road "+aux.parentRoadIndex+";
					// point "+aux.parentPointIndex+";
					// distance="+aux.distance+"--");
					// find the parent in the 'solved' set
					Road rdAux = (Road) Globals.map.roads
							.get(aux.parentRoadIndex);
					Point ptAux = (Point) rdAux.points
							.get(aux.parentPointIndex);
					Iterator iter = solved.iterator();
					boolean found = false;
					while (iter.hasNext()) {
						GraphNode aux2 = (GraphNode) iter.next();
						Road rdAux2 = (Road) Globals.map.roads
								.get(aux2.roadIndex);
						Point ptAux2 = (Point) rdAux2.points
								.get(aux2.pointIndex);
						if (ptAux2.equals(ptAux)) {
							aux = aux2;
							found = true;
							break;
						}
					}
					if (found == false) {
						System.out.println("ERROR! Parent NOT FOUND!:road "
								+ aux.parentRoadIndex + "; point "
								+ aux.parentPointIndex);
						break;
					} else {
					}
				}
				break;
			}

			// relaxation
			ArrayList vecini = getNeighborsWithDelay(first);

			// System.out.println("Neighbors for "+first+":"+vecini);

			for (int j = 0; j < vecini.size(); j++) {
				NeighborNode n = (NeighborNode) vecini.get(j);
				if (solved.contains(n.neigh))
					continue;
				// only if it's not solved

				Point ptAux = (Point) (((Road) (Globals.map.roads
						.get(n.neigh.roadIndex))).points
						.get(n.neigh.pointIndex));
				double oldDist = getDistance(set, ptAux);
				double d1 = n.dist;
				if (oldDist == -1.0) {
					// System.out.println("ERROR!;
					// roadIndex="+n.neigh.roadIndex+";
					// ptIndex="+n.neigh.pointIndex+";
					// lat="+ptAux.getLatitude()+";long="+ptAux.getLongitude());
					n.neigh.distance = d1 + first.distance;
					n.neigh.parentPointIndex = first.pointIndex;
					n.neigh.parentRoadIndex = first.roadIndex;

					if (avoid.roadIndex == -1) {
						set.addGraphNode(n.neigh);
					} else {
						if ((avoid.roadIndex >= 0)
								&& avoid.differentFrom(n.neigh)) {
							set.addGraphNode(n.neigh);
						}

					}

				} else {
					if (d1 + first.distance < oldDist) {
						// set.remove(n.neigh);
						n.neigh.distance = d1 + first.distance;
						n.neigh.parentPointIndex = first.pointIndex;
						n.neigh.parentRoadIndex = first.roadIndex;
						if (avoid.roadIndex == -1) {
							set.addGraphNode(n.neigh);
						} else {
							if ((avoid.roadIndex >= 0)
									&& avoid.differentFrom(n.neigh)) {
								set.addGraphNode(n.neigh);
							}

						}

					}
				}
			}
		}

		// now I have the 'routePoints' vector filled with Location objects;
		// do merging stuff; obtain the rez vector filled with RouteSegment
		// objects

		// System.out.println("Route before merge: points=" + routePoints);
		return routePoints;
	}

	public ArrayList getNeighborsWithDelay(GraphNode node) {
		// get all the direct neighbors of 'node'; returns a vector of
		// 'NeighborNode' objects
		ArrayList<NeighborNode> rez = new ArrayList<NeighborNode>();

		// find the closest points on the same road which are cross-points
		// if there is none, take the end points of the road (except when the
		// node is already an end point);
		// also check if there are cross-roads in 'node';
		Road road = (Road) Globals.map.roads.get(node.roadIndex);
		int nodeIndex = node.pointIndex;

		int min = Integer.MAX_VALUE;
		int max = -1;
		for (int i = 0; i < road.crosses.size(); i++) {
			Cross c = (Cross) road.crosses.get(i);
			int idx = c.getPointIndex();
			if (idx > max && idx < nodeIndex)
				max = idx;
			if (idx < min && idx > nodeIndex)
				min = idx;
			Point pt = (Point) road.points.get(nodeIndex);
			if (idx == nodeIndex) {
				// have to go on the crossing road, to find the closest points;
				Road crossingRoad = (Road) Globals.map.roads.get(c
						.getCrossRoadIndex());
				Point pt2 = null;
				int index2 = -1;
				// have to find the index of the point on the crossing road
				for (int j = 0; j < crossingRoad.points.size(); j++) {
					Point ptAux = (Point) crossingRoad.points.get(j);
					if (ptAux.equals(pt)) {
						// that's the one
						pt2 = ptAux;
						index2 = j;
						break;
					}
				}
				if (pt2 == null) {
					// System.out.println("RoutingModule.getNeighbors(): ERROR!
					// Could not find the point on the crossing
					// road:"+c.getCrossRoadIndex());
				} else {
					// find the neighbors on the crossing road;
					int minCross = Integer.MAX_VALUE;
					int maxCross = -1;
					for (int k = 0; k < crossingRoad.crosses.size(); k++) {
						Cross otherCross = (Cross) crossingRoad.crosses.get(k);
						int otherIndex = otherCross.getPointIndex();
						if (otherIndex > maxCross && otherIndex < index2)
							maxCross = otherIndex;
						if (otherIndex < minCross && otherIndex > index2)
							minCross = otherIndex;

					}
					if (minCross == Integer.MAX_VALUE) {
						// there was no min; have to consider last point of
						// crossing road
						// only if it is not the last
						if ((index2 != (crossingRoad.points.size() - 1))) {
							NeighborNode n = new NeighborNode(c
									.getCrossRoadIndex(), crossingRoad.points
									.size() - 1,
									getDelayForDirectedRoadSegment(c
											.getCrossRoadIndex(), index2,
											crossingRoad.points.size() - 1));
							// ((Point) (crossingRoad.points
							// .get(crossingRoad.points.size() - 1)))
							// .getDistance()
							// - ((Point) (crossingRoad.points
							// .get(index2)))
							// .getDistance());
							rez.add(n);
						}
					} else {
						NeighborNode n = new NeighborNode(
								c.getCrossRoadIndex(), minCross,
								getDelayForDirectedRoadSegment(c
										.getCrossRoadIndex(), index2, minCross));
						// ((Point) (crossingRoad.points.get(minCross)))
						// .getDistance()
						// - ((Point) (crossingRoad.points
						// .get(index2))).getDistance());
						rez.add(n);
					}
					if (maxCross == -1) {
						// there was no max; have to consider first point of
						// crossing road
						// only if it is not first
						if (index2 != 0) {
							NeighborNode n = new NeighborNode(c
									.getCrossRoadIndex(), 0,
									getDelayForDirectedRoadSegment(c
											.getCrossRoadIndex(), index2, 0));
							// ((Point) (crossingRoad.points.get(index2)))
							// .getDistance()
							// - ((Point) (crossingRoad.points
							// .get(0))).getDistance());
							rez.add(n);
						}
					} else {
						NeighborNode n = new NeighborNode(
								c.getCrossRoadIndex(), maxCross,
								getDelayForDirectedRoadSegment(c
										.getCrossRoadIndex(), index2, maxCross));
						// ((Point) (crossingRoad.points.get(index2)))
						// .getDistance()
						// - ((Point) (crossingRoad.points
						// .get(maxCross))).getDistance());
						rez.add(n);
					}
				}
			}
		}
		if (min == Integer.MAX_VALUE) {
			// there was no min; have to consider last point of the road
			// ONLY if I am not the last!
			if (((road.points.size() - 1) != node.pointIndex)) {
				NeighborNode n = new NeighborNode(node.roadIndex, road.points
						.size() - 1,
						getDelayForDirectedRoadSegment(node.roadIndex,
								node.pointIndex, road.points.size() - 1));
				// ((Point) (road.points.get(road.points
				// .size() - 1))).getDistance()
				// - ((Point) (road.points.get(node.pointIndex)))
				// .getDistance());
				rez.add(n);
			}
		} else {
			NeighborNode n = new NeighborNode(node.roadIndex, min,
					getDelayForDirectedRoadSegment(node.roadIndex,
							node.pointIndex, min));
			// ((Point) (road.points.get(min))).getDistance()
			// - ((Point) (road.points.get(node.pointIndex)))
			// .getDistance());
			rez.add(n);
		}
		if (max == -1) {
			// there was no max; have to consider first point of the road
			// only if I am not first
			if (node.pointIndex != 0) {
				NeighborNode n = new NeighborNode(node.roadIndex, 0,
						getDelayForDirectedRoadSegment(node.roadIndex,
								node.pointIndex, 0));
				// ((Point) (road.points.get(node.pointIndex)))
				// .getDistance()
				// - ((Point) (road.points.get(0))).getDistance());
				rez.add(n);
			}
		} else {
			NeighborNode n = new NeighborNode(node.roadIndex, max,
					getDelayForDirectedRoadSegment(node.roadIndex,
							node.pointIndex, max));
			// ((Point) (road.points.get(node.pointIndex))).getDistance()
			// - ((Point) (road.points.get(max))).getDistance());
			rez.add(n);
		}
		return rez;
	}

	public long getDelayForDirectedRoadSegment(int r, int p1, int p2) {
		boolean dir;
		DirectedRoadSegment dr = null;
		if (p1 < p2) {
			dir = true;
		} else {
			dir = false;
		}
		// try{
		dr = new DirectedRoadSegment(r, p2, (!dir));
		// }catch(Exception e){
		// e.printStackTrace();
		// }
		int i;
		for (i = 0; i < Globals.routePlanConstants.delays.size(); i++) {
			// System.out.println("Compar: " + Globals.delays.get(i).location
			// + " cu " + dr);
			if (Globals.routePlanConstants.delays.get(i).roadSegment.equals(dr)) {
				ArrayList<DelayRecord> tmp = Globals.routePlanConstants.delays;
				// System.out.println("^^^^^^^^^^^^^^^^^^^^^^got average
				// "+Globals.delays.get(i).getDelay());
				return Globals.routePlanConstants.delays.get(i).getDelay();
			}

		}
		return 0L;

	}
}