package vnsim.vehicular.routePlan.cityRouting;

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.simulator.Location;
import vnsim.vehicular.simulator.RouteSegment;
import vnsim.vehicular.simulator.intersections.DirectedRoadSegment;
import vnsim.vehicular.simulator.intersections.Intersection;

public class CityRouteComputingUtlis {
	private static final double kCongestionRatio = 0.01;

	private ArrayList<RoadSegmentCongestion> updatedCongestion;

	public CityRouteComputingUtlis(
			ArrayList<RoadSegmentCongestion> updatedCongestion) {
		this.updatedCongestion = updatedCongestion;
	}

	private double getDistance(GraphNodeArrayList t, Point p) {
		// get the distance from the set for the corresponding point;
		Iterator<GraphNode> 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;
	}

	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;
			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, p1, p2;
			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<GraphNode> 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<Location> dijkstraPath(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();

		GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
		ArrayList<NeighborNode> sourceNeighbors = getNeighborsWithCost(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 = 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+";
			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+";
					// 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<GraphNode> 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+";
					// 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<GraphNode> 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<NeighborNode> vecini = getNeighborsWithCost(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) {
					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) {
						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;
	}

	private ArrayList<NeighborNode> getNeighborsWithCost(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, getCostForDirectedRoadSegment(
									c.getCrossRoadIndex(), index2,
									crossingRoad.points.size() - 1));
							rez.add(n);
						}
					} else {
						NeighborNode n = new NeighborNode(
								c.getCrossRoadIndex(), minCross,
								getCostForDirectedRoadSegment(c
										.getCrossRoadIndex(), index2, minCross));
						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,
									getCostForDirectedRoadSegment(c
											.getCrossRoadIndex(), index2, 0));
							rez.add(n);
						}
					} else {
						NeighborNode n = new NeighborNode(
								c.getCrossRoadIndex(), maxCross,
								getCostForDirectedRoadSegment(c
										.getCrossRoadIndex(), index2, maxCross));
						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,
						getCostForDirectedRoadSegment(node.roadIndex,
								node.pointIndex, road.points.size() - 1));
				rez.add(n);
			}
		} else {
			NeighborNode n = new NeighborNode(node.roadIndex, min,
					getCostForDirectedRoadSegment(node.roadIndex,
							node.pointIndex, min));
			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,
						getCostForDirectedRoadSegment(node.roadIndex,
								node.pointIndex, 0));
				rez.add(n);
			}
		} else {
			NeighborNode n = new NeighborNode(node.roadIndex, max,
					getCostForDirectedRoadSegment(node.roadIndex,
							node.pointIndex, max));
			rez.add(n);
		}
		return rez;
	}

	private double getCostForDirectedRoadSegment(int r, int p1, int p2) {
		// all intersections must be in server db
		boolean dir;
		if (p1 < p2) {
			dir = true;
		} else {
			dir = false;
		}
		DirectedRoadSegment drs = new DirectedRoadSegment(r, p2, (!dir));
		RoadSegmentCongestion rsc;
		for (int i = 0; i < updatedCongestion.size(); i++) {
			rsc = updatedCongestion.get(i);
			if (rsc.equals(drs)) {
				return getRoadSegmentDistance(r, p1, p2)
						* (1 + kCongestionRatio * rsc.getCongestion());
			}
		}
		int time = Globals.engine.crtTime / Server.kTimeCompression;
		time = time / 3600; // hours
		int hour = time % 24;
		time = time / 24;
		int day = time % 7;

		Intersection it;
		DirectedRoadSegment tempDrs;
		for (int i = 0; i < Globals.map.allIntersections.size(); i++) {
			it = Globals.map.allIntersections.get(i);
			for (int j = 0; j < it.segments.size(); j++) {
				tempDrs = it.segments.get(j);
				if (tempDrs.equals(drs)) {
					Double congestion = Globals.lastWeekCongestions.get(i).get(
							j).getCongestion(day, hour);
					return getRoadSegmentDistance(r, p1, p2)
							* (1 + kCongestionRatio * congestion);
				}
			}
		}

		return getRoadSegmentDistance(r, p1, p2);

	}

	// road segment length in meters
	private static double getRoadSegmentDistance(int r, int p1, int p2) {
		Road road = Globals.map.roads.get(r);
		Point point1 = road.points.get(p1);
		Point point2 = road.points.get(p2);
		return Math.abs(point2.getDistance() - point1.getDistance()) * 1000;
	}

	// road segment length in meters
	public static Double getRoadSegmentDistance(DirectedRoadSegment drs) {
		Road road = Globals.map.roads.get(drs.roadIndex);
		Point p1 = road.points.get(drs.pointIndex);
		Point p2;
		Cross cross;
		if (drs.direction) {
			cross = getNextCross(drs.roadIndex, drs.pointIndex, true);
			if (cross == null) { // last point on the road
				p2 = road.points.get(road.points.size() - 1);
			} else {
				p2 = road.points.get(cross.getPointIndex());
			}
		} else {
			cross = getNextCross(drs.roadIndex, drs.pointIndex, false);
			if (cross == null) { // first point on the road
				p2 = road.points.get(0);
			} else {
				p2 = road.points.get(cross.getPointIndex());
			}
		}
		return Math.abs(p2.getDistance() - p1.getDistance()) * 1000;
	}

	// gets the next or previews cross from the Location(roadIndex, pointIndex)
	// located on the same road
	// if next is false return the previews cross
	private static Cross getNextCross(int roadIndex, int pointIndex,
			boolean next) {
		Road road = Globals.map.roads.get(roadIndex);
		Cross cross;
		int i;
		for (i = 0; i < road.crosses.size(); i++) {
			cross = road.crosses.get(i);
			if (cross.getPointIndex() == pointIndex) {
				break;
			}
		}
		if (next) {
			i++;
		} else {
			i--;
		}
		if (i < 0 || i >= road.crosses.size()) {
			return null;
		}
		return road.crosses.get(i);
	}

	public static ArrayList<RouteSegment> mergeCityRoute(
			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>();

		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 {
				boolean found = false;
				// 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!");
					return null;
				}
			}
			index++;
			if (index == routePoints.size()) {
				// it is the last iteration; add the last RouteSegment;
				ret.add(new RouteSegment((short) r1, (short) p1,
						(short) pLastCand));
			}
		}
		// it happens that a route segment has the same source and destination
		// remove this route segment if it happens
		RouteSegment rs = ret.get(0);
		if (rs.pt1==rs.pt2) {
			ret.remove(0);
		}
		return ret;
	}
}