/*******************************************************************************
 * 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 gnu.trove.list.array.TIntArrayList;

import java.util.Arrays;
import java.util.Collection;
import java.util.Comparator;

public class StopSet {
	public static class Line {
		/**
		 * The size of xs is even. The array is strictly ordered. 
		 * For all n, (xs[2n], xs[2n+1]) is an obstacle. 
		 */
		double[] xs;
		public double y;
		
		/* Following tables are indexed by intervals 0..xs.length such that
		   interval i is (xs[i-1],xs[i]), where xs[-1] = -INFINITY and
		   xs[xs.length] = INFINITY 
		 */
		/**
		 * Penalty of crossing the stop (null, if no penalty) 
		 */
		public Penalty[] penalty;
		/**
		 * Next line of stop with positive penalty
		 */
		Line[] nextLine; 
		/**
		 * Index of the first interval in the nextLine[i] 
		 * that blocks fronts from this interval.
		 */
		int[] nextPosition;  
		
		public Line(double[] xs, double y, Penalty[] penalty, Line[] nextLine, int[] nextPosition) {
			this.xs = xs;
			this.y = y;
			this.penalty = penalty;
			this.nextLine = nextLine;
			this.nextPosition = nextPosition;
		}			
	}
	
	Line[] lines;
	
	public static class Stop {
		double x0, x1;
		double y;
		Penalty penalty;
		
		public Stop(Penalty penalty, double x0, double x1, double y) {
			this.penalty = penalty;
			this.x0 = x0;
			this.x1 = x1;
			this.y = y;
		}		
	}

	public static final Comparator<Stop> stopComparator = new Comparator<Stop>() {

		@Override
		public int compare(Stop o1, Stop o2) {
			if(o1.y < o2.y)
				return -1;
			else if(o1.y > o2.y)
				return 1;
			else if(o1.x0 < o2.x0)
				return -1;
			else if(o1.x0 > o2.x0)
				return 1;
			else
				return 0;
		}
		
	};
	
	public StopSet(Collection<Stop> _stops) {
		if(_stops.isEmpty())
			return;
		Stop[] stops = _stops.toArray(new Stop[_stops.size()]);
		Arrays.sort(stops, stopComparator);
		
		TIntArrayList lineBegins = new TIntArrayList();
		lineBegins.add(0);
		for(int i=1;i<stops.length;++i)
			if(stops[i-1].y < stops[i].y)
				lineBegins.add(i);
		
		lines = new Line[lineBegins.size()];
		Line prev = null;
		int from = stops.length;
		for(int i=lineBegins.size()-1;i>=0;--i) {
			int to = from;
			from = lineBegins.get(i);
			
			// Calculate xs and penalty
			
			double[] xs = new double[(to-from)*2];
			Penalty[] penalty = new Penalty[(to-from)*2+1];
			int pointCount = 0;
			for(int j=from;j<to;++j) {
				Stop stop = stops[j];
				if(pointCount == 0 || xs[pointCount-1] < stop.x0) {
					penalty[pointCount] = null;
					xs[pointCount++] = stop.x0;
					penalty[pointCount] = stop.penalty;
					xs[pointCount++] = stop.x1;					
				}
				else if(stop.penalty.equals(penalty[pointCount-1])) {
					if(stop.x1 > xs[pointCount-1])
						xs[pointCount-1] = stop.x1;
				}
				else {
					if(stop.x0 == xs[pointCount-1]) {
						penalty[pointCount] = stop.penalty;
						xs[pointCount++] = stop.x1;
					}
					else if(stop.x1 > xs[pointCount-1]) {
						if(stop.penalty.penalty > penalty[pointCount-1].penalty) {
							if(xs[pointCount-2] == stop.x0)
								--pointCount;
							else
								xs[pointCount-1] = stop.x0;
						}
						penalty[pointCount] = stop.penalty;
						xs[pointCount++] = stop.x1;
					}
					else if(stop.penalty.penalty > penalty[pointCount-1].penalty) {
						// FIXME Not correct if more than two intersecting stops						
					}
				}
			}
			penalty[pointCount] = null;
			if(pointCount < xs.length) {
				xs = Arrays.copyOf(xs, pointCount);
				penalty = Arrays.copyOf(penalty, pointCount+1);
			}
			
			// Calculate nextLine and nextPosition
			
			Line[] nextLine = new Line[pointCount + 1];
			int[] nextPosition = new int[pointCount + 1];
			if(prev != null) {
				int prevI = 0;
				double[] prevXs = prev.xs;
				for(int j=0;j<=pointCount;++j) {
					double min = j==0 ? Double.NEGATIVE_INFINITY : xs[j-1];
					double max = j==pointCount ? Double.POSITIVE_INFINITY : xs[j];
					
					while(prevI < prevXs.length && prevXs[prevI] <= min)
						++prevI;
					
					Line prev2 = prev;
					int prevI2 = prevI;
					double[] prevXs2 = prevXs;
					while(prev2.penalty[prevI2]==null && (prevI2==prevXs2.length || prevXs2[prevI2] >= max)) {
						int temp = prev2.nextPosition[prevI2];
						prev2 = prev2.nextLine[prevI2];
						if(prev2 == null)
							break;
						prevI2 = temp;
						prevXs2 = prev2.xs;		
						while(prevI2 < prevXs2.length && prevXs2[prevI2] <= min)
							++prevI2;
					}
					nextLine[j] = prev2;
					nextPosition[j] = prevI2;  					
				}
			}
			lines[i] = prev = new Line(xs, stops[from].y, penalty, nextLine, nextPosition); 
		}
	}
	
	public static interface IStopProcedure {
		void blockEnd(double y);
		void continuation(double min, double max, int pos, Line line);
	}
	
	public void findStops(double min, double max, double y, IStopProcedure proc) {
		if(lines == null) {
			proc.blockEnd(Double.POSITIVE_INFINITY);
			return;
		}
		int minJ = -1;
		int maxJ = lines.length;
		while(maxJ > minJ+1) {
			int middleJ = (minJ+maxJ) >>> 1;
			double middleY = lines[middleJ].y;
			if(middleY < y)
				minJ = middleJ;
			else if(middleY > y)
				maxJ = middleJ;
			else {
				minJ = maxJ = middleJ;
				break;
			}
		}
		
		if(maxJ == lines.length)
			proc.blockEnd(Double.POSITIVE_INFINITY);
		else {
			double[] xs = lines[maxJ].xs;
			int minI = -1;
			int maxI = xs.length;
			while(maxI > minI+1) {
				int middleI = (minI+maxI) >>> 1;
				double middleX = xs[middleI];
				if(middleX < min)
					minI = middleI;
				else if(middleX > min)
					maxI = middleI;
				else {
					minI = maxI = middleI;
				}
			}
			findStops(maxI, lines[maxJ], min, max, y, proc);
		}
		
	}
	
	static private void findStops(int pos, Line line, double min, double max, double y, IStopProcedure proc) {
		double[] xs = line.xs;
		while(line.penalty[pos]==null && (pos==xs.length || xs[pos] >= max)) {
			int temp = line.nextPosition[pos];
			line = line.nextLine[pos];
			if(line == null)
				break;
			xs = line.xs;			
			pos = temp;
			while(pos < xs.length && xs[pos] <= min)
				++pos;
		}
		if(line == null)
			proc.blockEnd(Double.POSITIVE_INFINITY);
		else {
			if(line.y > y)
				proc.blockEnd(line.y);
			if(pos==xs.length || xs[pos] >= max)
				proc.continuation(min, max, pos, line);
			else {
				proc.continuation(min, xs[pos], pos, line);
				while(pos+1 < xs.length && xs[pos+1] < max) {
					++pos;
					proc.continuation(xs[pos-1], xs[pos], pos, line);
				}
				proc.continuation(xs[pos], max, pos+1, line);
			}
		}
	}
	
	static public void continueStop(final int pos, final Line line, double min, double max, IStopProcedure proc) {
		Line newLine = line.nextLine[pos];
		if(newLine == null)
			proc.blockEnd(Double.POSITIVE_INFINITY);
		else {
			int newPos = line.nextPosition[pos];
			double[] xs = newLine.xs;
			while(newPos < xs.length && xs[newPos] <= min)
				++newPos;
			findStops(newPos, newLine, min, max, line.y, proc);		
		}
	}

}