/*******************************************************************************
 * Copyright (c) 2007, 2010 Association for Decentralized Information Management
 * in Industry THTH ry.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
 * Contributors:
 *     VTT Technical Research Centre of Finland - initial API and implementation
 *******************************************************************************/
package org.simantics.g2d.routing.algorithm1;

import java.awt.geom.Path2D;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.Comparator;
import java.util.HashMap;
import java.util.PriorityQueue;

import org.simantics.g2d.routing.ConnectionDirectionUtil;
import org.simantics.g2d.routing.Constants;
import org.simantics.g2d.routing.Terminal;
import org.simantics.g2d.routing.algorithm1.StopSet.IStopProcedure;
import org.simantics.g2d.routing.algorithm1.StopSet.Line;
import org.simantics.g2d.routing.algorithm1.StopSet.Stop;

public class StaticRouter {
		
	StopSet[] stopSets = new StopSet[4];
	//Graphics2D g;
	
	public StaticRouter(Collection<PenaltyRectangle> rectangles) {
		for(int i=0;i<4;++i) {
			Direction dir = Direction.directions[i];
			Collection<Stop> stops= new ArrayList<Stop>(rectangles.size());
			for(PenaltyRectangle rectangle : rectangles)
				stops.add(new Stop((i&1) == 0 ? rectangle.xPenalty : rectangle.yPenalty, 
					dir.minSide(rectangle),
					dir.maxSide(rectangle),
					dir.front(rectangle)));
			stopSets[i] = new StopSet(stops);
		}
	}
		
	/*public StaticRouter(Collection<Rectangle2D> rectangles,
			Collection<Path2D> paths) {
		stopSets = new StopSet[4];
		Collection<Rectangle> horizontalLines = new ArrayList<Rectangle>();
		Collection<Rectangle> verticalLines = new ArrayList<Rectangle>();
				
		for(Path2D path : paths) {
			PathIterator it = path.getPathIterator(null);
			double[] old = null;
			while(!it.isDone()) {
				double[] cur = new double[2];
				it.currentSegment(cur);
				if(old != null) {
					if(old[1] == cur[1]) {
						horizontalLines.add(new Rectangle(
							Math.min(cur[0], old[0]),
							cur[1]-CONNECTION_MARGINAL,
							Math.max(cur[0], old[0]),
							cur[1]+CONNECTION_MARGINAL
						));						
					}
					else {
						verticalLines.add(new Rectangle(
							cur[0]-CONNECTION_MARGINAL,
							Math.min(cur[1], old[1]),
							cur[0]+CONNECTION_MARGINAL,
							Math.max(cur[1], old[1])							
						));
					}
				}
				it.next();
				old = cur;
			}
		}
		
		for(int i=0;i<4;++i) {
			Direction dir = Direction.directions[i];
			Collection<Stop> stops= new ArrayList<Stop>(rectangles.size()+
					horizontalLines.size()+
					verticalLines.size());
			for(Rectangle2D rectangle : rectangles)
				stops.add(new Stop(obstaclePenalty, 
					dir.minSide(rectangle),
					dir.maxSide(rectangle),
					dir.front(rectangle)));
			for(Rectangle rectangle : horizontalLines)
				stops.add(new Stop((i&1)==0 ? connectionSidePenalty : connectionCrossPenalty, 
					dir.minSide(rectangle),
					dir.maxSide(rectangle),
					dir.front(rectangle)));
			for(Rectangle rectangle : verticalLines)
				stops.add(new Stop((i&1)==1 ? connectionSidePenalty : connectionCrossPenalty, 
					dir.minSide(rectangle),
					dir.maxSide(rectangle),
					dir.front(rectangle)));
			stopSets[i] = new StopSet(stops);
		}
	}*/
	
	static class Continuation implements Comparable<Continuation> {
		Direction direction;
		double min;
		double max;
		double front;
		Line line;
		int pos;
		RoutePencil parent;
		double penalty;
		double minTurnDistance;
		double priority;	
		double distance;

		public Continuation(Direction direction, double min, double max,
			double front, int pos, Line line, RoutePencil parent, double penalty,
			double minTurnDistance, double targetX, double targetY) {
			this.direction = direction;
			this.min = min;
			this.max = max;
			this.front = front;
			this.pos = pos;
			this.line = line;
			this.parent = parent;
			this.penalty = penalty;
			this.minTurnDistance = minTurnDistance;
			calcDistance(targetX, targetY);
		}

		@Override
		public int compareTo(Continuation o) {
			if(priority < o.priority)
				return -1;
			if(priority > o.priority)
				return 1;
			if(distance < o.distance)
				return -1;
			if(distance > o.distance)
				return 1;
			return 0;
		}		

		@Override
		public int hashCode() {
			final int prime = 31;
			int result = direction.getId();
			long temp;
			temp = Double.doubleToLongBits(front);
			result = prime * result + (int) (temp ^ (temp >>> 32));
			temp = Double.doubleToLongBits(max);
			result = prime * result + (int) (temp ^ (temp >>> 32));
			temp = Double.doubleToLongBits(min);
			result = prime * result + (int) (temp ^ (temp >>> 32));
			return result;
		}

		@Override
		public boolean equals(Object obj) {
			if (this == obj)
				return true;
			if (obj == null)
				return false;
			if (getClass() != obj.getClass())
				return false;
			Continuation other = (Continuation) obj;
			if (direction == null) {
				if (other.direction != null)
					return false;
			} else if (direction!=other.direction)
				return false;
			if (Double.doubleToLongBits(front) != Double
				.doubleToLongBits(other.front))
				return false;
			if (Double.doubleToLongBits(max) != Double
				.doubleToLongBits(other.max))
				return false;
			if (Double.doubleToLongBits(min) != Double
				.doubleToLongBits(other.min))
				return false;
			return true;
		}
		
		void calcDistance(double x, double y) {
			if(parent == null)
				distance = 0.0;			
			distance = parent.distanceConstant;
			switch(direction.getId()) {
			case Constants.EAST:
				if(front > parent.x1)
					distance += parent.distanceX*parent.x1 + (front-parent.x1);
				else
					distance += parent.distanceX*front;
				distance += Math.abs(x-front);
				if(y < min)
					distance += parent.distanceY*min + (min - y);
				else if(y > max)
					distance += parent.distanceY*max + (y - max);
				else
					distance += parent.distanceY*y;
				break;
			case Constants.SOUTH:
				if(front > parent.y1)
					distance += parent.distanceY*parent.y1 + (front-parent.y1);
				else
					distance += parent.distanceY*front;
				distance += Math.abs(y-front);
				if(x < min)
					distance += parent.distanceX*min + (min - x);
				else if(x > max)
					distance += parent.distanceX*max + (x - max);
				else
					distance += parent.distanceX*x;
				break;
			case Constants.WEST:
				if(-front < parent.x0)
					distance += parent.distanceX*parent.x0 + (parent.x0+front);
				else
					distance += parent.distanceX*(-front);
				distance += Math.abs(x+front);
				if(y < min)
					distance += parent.distanceY*min + (min - y);
				else if(y > max)
					distance += parent.distanceY*max + (y - max);
				else
					distance += parent.distanceY*y;
				break;
			case Constants.NORTH:
				if(-front < parent.y0)
					distance += parent.distanceY*parent.y0 + (parent.y0+front);
				else
					distance += parent.distanceY*(-front);
				distance += Math.abs(y+front);
				if(x < min)
					distance += parent.distanceX*min + (min - x);
				else if(x > max)
					distance += parent.distanceX*max + (x - max);
				else
					distance += parent.distanceX*x;
				break;
			}
		}
		
	}
	
	static class Cost implements Comparable<Cost> {
		double penalty;
		double distance;		
		
		public Cost(double penalty, double distance) {
			this.penalty = penalty;
			this.distance = distance;
		}
		
		@Override
		public int compareTo(Cost o) {
			if(penalty < o.penalty)
				return -1;
			if(penalty > o.penalty)
				return 1;
			if(distance < o.distance)
				return -1;
			if(distance > o.distance)
				return 1;
			return 0;
		}			
	}
	
	class SingleRoute {
		PriorityQueue<Continuation> queue = new PriorityQueue<Continuation>();
		double targetX, targetY;
		RoutePencil[] targets;
		int targetDirs;
		RoutePencil[] solutions = new RoutePencil[4];
		int requiredSolutionCount;		
		double bPenalty = Double.POSITIVE_INFINITY;
		double bDistance = Double.POSITIVE_INFINITY;
		Cost[] sortedCosts = new Cost[4];
		HashMap<Continuation, Continuation> continuations = new HashMap<Continuation, Continuation>();

		public SingleRoute(
				double targetX, double targetY, 
				RoutePencil[] targets, int targetDirs,
				int requiredSolutionCount) {
			this.targetX = targetX;
			this.targetY = targetY;
			this.targets = targets;
			this.targetDirs = targetDirs;
			this.requiredSolutionCount = requiredSolutionCount;
			
			double minPenalty = Double.MAX_VALUE;
			for(RoutePencil target : targets)
				if(target.penalty < minPenalty)
					minPenalty = target.penalty;
			if(minPenalty > 0)
				for(RoutePencil target : targets)
					target.penalty -= minPenalty;
		}

		void calcPriority(Continuation cont) {
			cont.priority = cont.penalty;
			double tDir = cont.direction.getDir(targetX, targetY);
			double tSide = cont.direction.getSide(targetX, targetY);
			if(tDir >= cont.front) {
				if(tSide < cont.min || tSide > cont.max)
					cont.priority += Penalty.BEND_PENALTY;
			}
			else
				cont.priority += 2*Penalty.BEND_PENALTY;
		}
		
		void prime(Direction dir, double min, double max, double front, double minTurnDistance) {
			Continuation cont = new Continuation(dir, min, max, front, 0, null, 
				dir.createPencil(front, front, min, max, 0.0, null), 0.0, minTurnDistance,
				targetX, targetY);
			addContinutation(cont);
		}
		
		void prime(RoutePencil pencil) {
			Direction dir = pencil.direction;
			Continuation cont = new Continuation(
				dir, 
				dir.minSide(pencil),
				dir.maxSide(pencil),
				dir.back(pencil),
				0, null, pencil, pencil.penalty, 0.0,
				targetX, targetY);
			addContinutation(cont);
		}
		
		void solve() {
			int count = 0;
			while(!queue.isEmpty() && count < 10000) {
				Continuation cont = queue.remove();
				if(cont.priority > bPenalty ||
						(cont.priority == bPenalty && cont.distance >= bDistance)) {
					//System.out.println("solution found, next priority would be " + cont.priority);
					return;
				}
				doContinuation(cont);				
				++count;				
			}
		}
		
		void addContinutation(Continuation cont) {	
			Continuation old = continuations.get(cont);
			if(old != null) {
				if(old.priority < cont.priority)
					return;
				if(old.priority == cont.priority && old.distance <= cont.distance)
					return;
			}
			continuations.put(cont, cont);
			calcPriority(cont);
			queue.add(cont);
		}
		
		void addBend(RoutePencil pencil) {			
			//System.out.println(pencil + " (" + pencil.x0 + " " + pencil.y0 + " " + pencil.x1 + " " + pencil.y1 + ")");
			boolean foundSolution = false;
			for(RoutePencil target : targets) 
				if(pencil.intersects(target)) {				
					int dir = target.direction.getId();
					if(dir==pencil.direction.getId())
						continue;
					dir = (dir+2)%4;
					double penalty = pencil.penalty + target.penalty;
					if(dir != pencil.direction.getId())
						penalty += Penalty.BEND_PENALTY;
					if(solutions[dir] == null || solutions[dir].penalty > penalty) {
						//System.out.println("Solution by penalty");
						solutions[dir] = new RoutePencil(targetX, targetY, targetX, targetY, penalty,
							Direction.directions[dir], pencil);
						foundSolution = true;
					}
					else if(solutions[dir].penalty == penalty) {						
						RoutePencil sol = new RoutePencil(targetX, targetY, targetX, targetY, penalty,
							Direction.directions[dir], pencil);
						double d1 = sol.distance();
						double d2 = solutions[dir].distance();
						//System.out.println("Solution by distance " + d1 + " " + d2);
						if(d1 < d2) {
							solutions[dir] = sol;
							foundSolution = true;
						}
					}
					else
						; /*System.out.println("Ignored solution: " +
								"penalty=" + penalty + 
								", pencil.penalty=" + pencil.penalty + 
								", distance=" + pencil.distance() + 
								", better=" + solutions[dir].penalty);*/
				}			
			if(foundSolution) {				
				for(int i=0;i<4;++i)
					sortedCosts[i] =
						solutions[i] == null ? Penalty.INFINITE_COST 
								: new Cost(solutions[i].penalty, solutions[i].distance());
				Arrays.sort(sortedCosts);
				bPenalty = sortedCosts[requiredSolutionCount-1].penalty;
				bDistance = sortedCosts[requiredSolutionCount-1].distance;
			}			
			else {
				{
					Direction left = pencil.direction.turnLeft();
					double front = left.back(pencil);
					if(front < Double.POSITIVE_INFINITY) {
						Continuation leftCont = new Continuation(left, 
								left.minSide(pencil),
								left.maxSide(pencil),
								front,
								0, null, pencil, pencil.penalty+Penalty.BEND_PENALTY, 0.0,
								targetX, targetY);
						addContinutation(leftCont);
					}
				}

				{
					Direction right = pencil.direction.turnRight();
					double front = right.back(pencil);
					if(front < Double.POSITIVE_INFINITY) {
						Continuation rightCont = new Continuation(right, 
								right.minSide(pencil),
								right.maxSide(pencil),
								right.back(pencil),
								0, null, pencil, pencil.penalty+Penalty.BEND_PENALTY, 0.0,
								targetX, targetY);
						addContinutation(rightCont);
					}
				}
			}
		}


		void doContinuation(final Continuation cont) {
			//System.out.println("cont " + cont.parent + " -> " + cont.direction.getId() + " x=(" + cont.min + ", " + cont.max + ") y=" + cont.front);
			IStopProcedure proc = new IStopProcedure() {

				@Override
				public void blockEnd(double y) {					
					double front = cont.front + cont.minTurnDistance;
					//System.out.println(cont.minTurnDistance);
					if(front <= y)
						addBend(cont.direction.createPencil(front, y, cont.min, cont.max, cont.penalty, cont.parent));
				}

				@Override
				public void continuation(double min, double max, int pos, Line line) {
					double newPenalty = cont.penalty;
					double newMinTurnDistance; 
						
					Penalty penalty = line.penalty[pos];					
					if(penalty != null) {
						newPenalty += penalty.penalty;
						newMinTurnDistance = Math.max(penalty.minDistance, 
								cont.minTurnDistance - line.y + cont.front);
					}
					else
						newMinTurnDistance = Math.max(0.0, 
								cont.minTurnDistance - line.y + cont.front);
					Continuation newCont = new Continuation(cont.direction,
						min, max, line.y,
						pos, line, cont.parent, newPenalty, newMinTurnDistance,
						targetX, targetY);
					addContinutation(newCont);					
				}
				
			};
			if(cont.line != null)
				StopSet.continueStop(cont.pos, cont.line, cont.min, cont.max, proc);
			else {
				StopSet ss = stopSets[cont.direction.getId()];
				ss.findStops(cont.min, cont.max, cont.front, proc);
			}
		}
	}
	static double makeFinite(double x) {
		if(x == Double.POSITIVE_INFINITY)
			return 10000.0;
		if(x == Double.NEGATIVE_INFINITY)
			return -10000.0;
		return x;
	}
	
	class TargetFinder implements IStopProcedure {
		
		Collection<RoutePencil> targets;
		Direction dir;
		double dd;
		double side;		
		double penalty;
		double minDistance;
		
		public TargetFinder(Direction dir, double dd, double side, double penalty,
				double minDistance,
				Collection<RoutePencil> targets) {
			this.dd = dd;
			this.dir = dir;
			this.side = side;
			this.targets = targets;
			this.penalty = penalty;
			this.minDistance = minDistance;
		}

		@Override
		public void blockEnd(double y) {
			if(y >= dd+minDistance)
				targets.add(dir.createPencil(dd+minDistance, 
						y, side, side, penalty, null));
		}

		@Override
		public void continuation(double min, double max,
				int pos, Line line) {
			Penalty p = line.penalty[pos];
			if(p.penalty >= Penalty.OBSTACLE_PENALTY.penalty)
				return;
			StopSet.continueStop(pos, line, side, side,  
					new TargetFinder(dir, line.y, side, 
							Math.max(p.minDistance, minDistance-line.y+dd), 
							penalty+p.penalty, targets));		
		}
	}
	
	public void addTargets(final Collection<RoutePencil> targets, 
			final double x, final double y, double minDist, int dirId) {
		final Direction dir = Direction.directions[dirId];  
		final double side = dir.getSide(x,y);
		final double dd = dir.getDir(x, y);
		stopSets[dirId].findStops(side, side, dd, 
				new TargetFinder(dir, dd, side, 0.0, minDist, targets));			
	}	
	
	public Path2D route(Terminal begin, double[] points, Terminal end) {
		
		/* Handle begin==null (end==null) case by taking the first (last) 
		 * route point as terminal 
		 */
		
		if(begin == null) {
			if(points.length < 2)
				return null;
			begin = new Terminal(points[0], points[1], 0x1, Terminal.ZEROS, null);
			points = Arrays.copyOfRange(points, 2, points.length);
		}
		if(end == null) {
			if(points.length < 2)
				return null;
			end = new Terminal(points[points.length-2], points[points.length-1], 0xf, Terminal.ZEROS, null);
			points = Arrays.copyOf(points, points.length-2);
		}
		
		{
		    double dx = end.x - begin.x;
		    double dy = end.y - begin.y;
		    if(dx < begin.minDist[0]+end.minDist[2]+20.0 && -dx < begin.minDist[2]+end.minDist[0]+20.0 &&
		       dy < begin.minDist[1]+end.minDist[3]+20.0 && -dy < begin.minDist[3]+end.minDist[1]+20.0 ) {
		        if((begin.directions & Constants.EAST_FLAG) == Constants.EAST_FLAG &&
		            (end.directions & Constants.WEST_FLAG) == Constants.WEST_FLAG && dx > 0.0) {
		            Path2D path = new Path2D.Double();
		            path.moveTo(begin.x, begin.y);
		            path.lineTo(0.5 * (begin.x + end.x), begin.y);
		            path.lineTo(0.5 * (begin.x + end.x), end.y);
		            path.lineTo(end.x, end.y);
		            return path;
		        }
		        else if((begin.directions & Constants.WEST_FLAG) == Constants.WEST_FLAG &&
		            (end.directions & Constants.EAST_FLAG) == Constants.EAST_FLAG && dx < 0.0) {
		            Path2D path = new Path2D.Double();
		            path.moveTo(begin.x, begin.y);
		            path.lineTo(0.5 * (begin.x + end.x), begin.y);
		            path.lineTo(0.5 * (begin.x + end.x), end.y);
		            path.lineTo(end.x, end.y);
		            return path;
		        }
		        else if((begin.directions & Constants.SOUTH_FLAG) == Constants.SOUTH_FLAG &&
                    (end.directions & Constants.NORTH_FLAG) == Constants.NORTH_FLAG && dy > 0.0) {
                    Path2D path = new Path2D.Double();
                    path.moveTo(begin.x, begin.y);
                    path.lineTo(begin.x, 0.5 * (begin.y + end.y));
                    path.lineTo(end.x, 0.5 * (begin.y + end.y));                    
                    path.lineTo(end.x, end.y);
                    return path;
                }
		        else if((begin.directions & Constants.NORTH_FLAG) == Constants.NORTH_FLAG &&
                    (end.directions & Constants.SOUTH_FLAG) == Constants.SOUTH_FLAG && dy < 0.0) {
                    Path2D path = new Path2D.Double();
                    path.moveTo(begin.x, begin.y);
                    path.lineTo(begin.x, 0.5 * (begin.y + end.y));
                    path.lineTo(end.x, 0.5 * (begin.y + end.y));                    
                    path.lineTo(end.x, end.y);
                    return path;
                }
		    }
		}
		
		/*
		 * Route
		 */
		SingleRoute oldRoute = null;
		for(int p=0;p<=points.length;p+=2) {
			/*
			 * Create SingleRoute-class and targets
			 */
			SingleRoute route;			
			if(p==points.length) {
				Collection<RoutePencil> targets = new ArrayList<RoutePencil>(1);
				for(int i=0;i<4;++i) 
					if((end.directions & (1 << i)) != 0)
						addTargets(targets, end.x, end.y, end.minDist[i], i);
				route = new SingleRoute(end.x, end.y, targets.toArray(new RoutePencil[targets.size()]), 
						ConnectionDirectionUtil.reverseDirections(end.directions), 1);
			}
			else {
				double x = points[p];
				double y = points[p+1];
				Collection<RoutePencil> targets = new ArrayList<RoutePencil>(4);				
				for(int i=0;i<4;++i)
					addTargets(targets, x, y, 0.0, i);
				//targets[i] = new RoutePencil(x, y, x, y, 0.0, Direction.directions[i], null);
				route = new SingleRoute(x, y, targets.toArray(new RoutePencil[targets.size()]), 0xf, 1);
			}
			
			/*
			 * Prime route class
			 */
			if(oldRoute == null) {
				for(int i=0;i<4;++i)
					if((begin.directions & (1 << i)) != 0) {
						Direction dir = Direction.directions[i];
						double side = dir.getSide(begin.x, begin.y);
						double dd = dir.getDir(begin.x, begin.y);
						route.prime(dir, side, side, dd, begin.minDist[i]);
					}
			}
			else {
				for(int i=0;i<4;++i)
					if(oldRoute.solutions[i] != null)
						route.prime(oldRoute.solutions[i]);
			}
			
			/*
			 * Solve
			 */			
			route.solve();
			/*
			System.out.println("Stage " + (p/2));
			for(int i=0;i<4;++i)
				if(route.solutions[i] != null)
					System.out.println("    " + i + ": " + route.solutions[i] + 
							", penalty: " + route.solutions[i].penalty + 
							", distance: " + route.solutions[i].distance());
			*/
			oldRoute = route;			
		}		
		
		/*
		 * Choose the best path
		 */
		RoutePencil[] solutions = oldRoute.solutions;
		Arrays.sort(solutions, solutionComparator);
		if(solutions[0] == null)
			return null;		
		return solutions[0].createPath(end.x, end.y);
	}

	final static Comparator<RoutePencil> solutionComparator = new Comparator<RoutePencil>() {

		@Override
		public int compare(RoutePencil o1, RoutePencil o2) {
			if(o1 == null) {
				if(o2 == null)
					return 0;
				else
					return 1;
			}
			if(o2 == null)
				return -1;
			if(o1.penalty < o2.penalty)
				return -1;
			if(o1.penalty > o2.penalty)
				return 1;
			double d1 = o1.distance();
			double d2 = o2.distance();
			if(d1 < d2)
				return -1;
			if(d1 > d2)
				return 1;
			return 0;
		}
		
	};
}