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



import java.util.*;

import vnsim.map.object.Cross;
import vnsim.map.object.Globals;
import vnsim.map.object.Point;
import vnsim.map.object.Road;


public class RoutingUtils {

	public static ArrayList<RouteSegment> getMergedDijkstraPath(Location src, Location dest) {
		if(src.roadIdx==dest.roadIdx) {
			ArrayList<RouteSegment> rs=new ArrayList<RouteSegment>();
			rs.add(new RouteSegment((short)src.roadIdx, (short)src.ptIdx, (short)dest.ptIdx));
			return rs;
		}
		ArrayList<Location> points=dijkstraPath(src, dest);

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

		ArrayList<RouteSegment> ret=mergeRoute(points);
		//System.out.println("Route after merge: segments="+ret);
		return ret;
	}

	private static ArrayList<RouteSegment> split(RouteSegment seg) {
		ArrayList<RouteSegment> split=new ArrayList<RouteSegment> ();
		split.add(seg);
		Road r=Globals.map.roads.get(seg.roadIndex);
		boolean done=false;
		while(!done) {
			done=true;
			ArrayList<RouteSegment> newSplit=new ArrayList<RouteSegment> ();
			x:for(int i=0;i<split.size();i++) {
				RouteSegment rs=split.get(i);
				for(int j=0;j<r.crosses.size();j++) {
					int ptCross=r.crosses.get(j).getPointIndex();
					if(( rs.pt1<rs.pt2) && (ptCross < rs.pt2 && ptCross>rs.pt1 )) {
						newSplit.add(new RouteSegment((short)seg.roadIndex, (short)rs.pt1, (short)ptCross));
						newSplit.add(new RouteSegment((short)seg.roadIndex, (short)ptCross, (short)rs.pt2));
						done=false;
						continue x;
					}
					if(( rs.pt1>rs.pt2) && (ptCross < rs.pt1 && ptCross>rs.pt2 )) {
						newSplit.add(new RouteSegment((short)seg.roadIndex, (short)rs.pt1, (short)ptCross));
						newSplit.add(new RouteSegment((short)seg.roadIndex, (short)ptCross, (short)rs.pt2));
						done=false;
						continue x;
					}
				}
				newSplit.add(rs);
			}
			split=newSplit;
		}
		return split;
	}
	
	public static ArrayList<RouteSegment> splitRoute(ArrayList<RouteSegment> route) {
		ArrayList<RouteSegment> ret=new ArrayList<RouteSegment> ();
		for(int i=0;i<route.size();i++) {
			ret.addAll(split(route.get(i)));
		}
		return ret;
	}

	public static ArrayList<RouteSegment> getUnmergedDijkstraPath(Location src, Location dest) {
		if(src.roadIdx==dest.roadIdx) {
			return split(new RouteSegment((short)src.roadIdx, (short)src.ptIdx, (short)dest.ptIdx));
		}
		ArrayList<Location> points=dijkstraPath(src, dest);
		points.add(new Location(dest.roadIdx, dest.ptIdx));
		ArrayList<RouteSegment> ret=new ArrayList<RouteSegment>();
		
		x:for(int i=0;i<points.size()-1;i++) {
			Location l1=points.get(i);
			Location l2=points.get(i+1);

//			System.out.println("L1="+l1+";L2="+l2);

			if(l1.roadIdx==l2.roadIdx) {
				RouteSegment seg=new RouteSegment((short)l1.roadIdx,(short)l1.ptIdx, (short)l2.ptIdx);
				ArrayList<RouteSegment> segs=split(seg);
				for(int j=0;j<segs.size();j++)
					ret.add(segs.get(j));
				continue;
			}

			//is "l1" on "l2.roadIdx" ?
			Road r1=Globals.map.roads.get(l1.roadIdx);
			for(int j=0;j<r1.crosses.size();j++) {
				Cross c=r1.crosses.get(j);
				if(c.getCrossRoadIndex()==l2.roadIdx && c.getPointIndex()==l1.ptIdx) {
					RouteSegment seg=new RouteSegment((short)l2.roadIdx, (short)c.getCrossPointIndex(), (short)l2.ptIdx);
					ret.addAll(split(seg));
					continue x;
				}
			}

			//is "l2" on "l1.roadIdx" ?
			Road r2=Globals.map.roads.get(l2.roadIdx);
			for(int j=0;j<r2.crosses.size();j++) {
				Cross c=r2.crosses.get(j);
				if(c.getCrossRoadIndex()==l1.roadIdx && c.getPointIndex()==l2.ptIdx) {
					ret.addAll(split(new RouteSegment((short)l1.roadIdx, (short)l1.ptIdx, (short)c.getCrossPointIndex())));
					continue x;
				}
			}
			System.out.println("getUnmergedPath - UNIMPLEMENTED!!!");
		}
		return ret;
	}

	private static ArrayList<Location> dijkstraPath(Location src, Location dest) {
		//returns a vector of consecutive RouteSegments, the shortest route
		//between src and dest

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

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

		GraphNodeArrayList set=new GraphNodeArrayList();

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

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

		//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;
			set.addGraphNode(new GraphNode(rd,pt,src.roadIdx,src.ptIdx,srcNeig.dist));
		}

		GraphNodeArrayList solved=new GraphNodeArrayList();

		solved.addGraphNode(sourceGraphNode);

		while(true) {
			//get the first node
			GraphNode first=(GraphNode) set.removeFirst();
			solved.addGraphNode(first);
//			System.out.println("Solved:"+first+"; solved="+solved.size()+";unsolved="+set.size());
			if(isPointOnRoad(first.roadIndex,first.pointIndex,dest.roadIdx)) {
				//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;
			}

			//relaxation
			ArrayList vecini=getNeighbors(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;
					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);
						set.setGraphNode(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 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>();

		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!");
					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));
			}
		}
		return ret;
	}

	public static ArrayList getNeighbors(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;
	}

	private static 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 static 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;
	}
	
	static 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 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 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;
		}
	}

	static 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;
		}
	}

	static 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 location 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;
		}
	}
}