vtm/vtm/src/org/oscim/utils/RTree.java

/*
 * Copyright 2014 Hannes Janetzek
 *
 * This file is part of the OpenScienceMap project (http://www.opensciencemap.org).
 *
 * This program is free software: you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 * PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along with
 * this program. If not, see <http://www.gnu.org/licenses/>.
 */
package org.oscim.utils;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import org.oscim.core.Box;
import org.oscim.utils.RTree.Branch;
import org.oscim.utils.RTree.Node;
import org.oscim.utils.RTree.Rect;
import org.oscim.utils.pool.Inlist;
import org.oscim.utils.pool.SyncPool;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

/**
 * Implementation of RTree, a multidimensional bounding rectangle tree.
 * 
 * @author 1983 Original algorithm and test code by Antonin Guttman and Michael
 *         Stonebraker, UC Berkely
 * @author 1994 ANCI C ported from original test code by Melinda Green -
 *         melinda@superliminal.com
 * @author 1995 Sphere volume fix for degeneracy problem submitted by Paul Brook
 * @author 2004 Templated C++ port by Greg Douglas
 * @author 2008 Portability issues fixed by Maxence Laurent
 * @author 2014 Ported to Java by Hannes Janetzek
 */
public class RTree<T> implements SpatialIndex<T>, Iterable<T> {

	static final Logger log = LoggerFactory.getLogger(RTree.class);

	final static int MAXNODES = 8;
	final static int MINNODES = 4;

	final static int NUMDIMS = 2;

	static final boolean DEBUG = true;

	/**
	 * A Branch may be data or may be another subtree
	 * The parents level determines this.
	 * If the parents level is 0, then this is data
	 */
	final static class Branch<E> extends Rect {
		/** Child node or data item */
		E node;

		@Override
		public String toString() {
			return node.toString();
		}
	}

	/**
	 * Node for each branch level
	 */
	final static class Node {

		/** Leaf is zero, others positive */
		int level = -1;

		/** Fill count */
		int count;

		/** Branches */
		final Branch<?> branch[];

		public Node(int maxnodes) {
			branch = new Branch[maxnodes];
		}

		/** A leaf, contains data */
		boolean isLeaf() {
			return (level == 0);
		}

		@SuppressWarnings("unchecked")
		Branch<Node>[] children() {
			if (level == 0)
				throw new IllegalStateException();

			return (Branch<Node>[]) branch;
		}

		/**
		 * Add a branch to a node. Split the node if necessary.
		 * 
		 * @return <code>false</code> if node not split. Old node will be
		 *         updated.
		 *         Otherwise if node split, sets newNode to address of new node.
		 *         Old node updated, becomes one of two.
		 */
		boolean addBranch(Branch<?> branch) {
			assert (branch != null);

			if (count < MAXNODES) {
				/* Split won't be necessary */
				this.branch[count] = branch;
				count++;
				return false;
			}

			return true;
		}

		@Override
		public String toString() {
			return count + "/" + Arrays.deepToString(branch) + '\n';
		}
	}

	/** Minimal bounding rectangle (n-dimensional) */
	static class Rect {

		/** dimensions of bounding box */
		double xmin, ymin, xmax, ymax;

		public Rect() {
		}

		public Rect(Box box) {
			if (DEBUG) {
				assert (xmin <= xmax);
				assert (ymin <= ymax);
			}

			xmin = box.xmin;
			ymin = box.ymin;
			xmax = box.xmax;
			ymax = box.ymax;
		}

		public Rect(double[] min, double[] max) {

			if (DEBUG) {
				for (int index = 0; index < NUMDIMS; index++)
					assert (min[index] <= max[index]);
			}

			xmin = min[0];
			ymin = min[1];
			xmax = max[0];
			ymax = max[1];
		}

		/**
		 * Calculate the n-dimensional volume of a rectangle
		 */
		public double calcRectVolume() {
			return (xmax - xmin) * (ymax - ymin);
		}

		/**
		 * Decide whether two rectangles overlap.
		 */
		public boolean overlap(Rect other) {
			return !(xmin > other.xmax || xmax < other.xmin || ymin > other.ymax || ymax < other.ymin);
		}

		/**
		 * Combine two rectangles into larger one containing both
		 */
		public void combine(Rect rectA, Rect rectB) {
			xmin = Math.min(rectA.xmin, rectB.xmin);
			ymin = Math.min(rectA.ymin, rectB.ymin);
			xmax = Math.max(rectA.xmax, rectB.xmax);
			ymax = Math.max(rectA.ymax, rectB.ymax);
		}

		public void add(Rect rect) {
			xmin = Math.min(xmin, rect.xmin);
			ymin = Math.min(ymin, rect.ymin);
			xmax = Math.max(xmax, rect.xmax);
			ymax = Math.max(ymax, rect.ymax);
		}

		public void set(Rect rect) {
			xmin = rect.xmin;
			ymin = rect.ymin;
			xmax = rect.xmax;
			ymax = rect.ymax;
		}

		public void set(double[] min, double[] max) {
			if (DEBUG) {
				for (int index = 0; index < NUMDIMS; index++) {
					assert (min[index] <= max[index]);
				}
			}

			xmin = min[0];
			ymin = min[1];
			xmax = max[0];
			ymax = max[1];
		}

		public void set(Box box) {
			if (DEBUG) {
				assert (box.xmin <= box.xmax);
				assert (box.ymin <= box.ymax);
			}
			xmin = box.xmin;
			ymin = box.ymin;
			xmax = box.xmax;
			ymax = box.ymax;
		}

		/**
		 * Find the smallest rectangle that includes all rectangles in branches
		 * of a node.
		 */
		void setCover(Node node) {
			assert (node != null);

			set(node.branch[0]);
			for (int idx = 1; idx < node.count; idx++) {
				add(node.branch[idx]);
			}
		}
	}

	/** Root of tree */
	protected Node mRoot;

	private final Partition mLocalVars = new Partition(MAXNODES, MINNODES);

	private Rect mTmpRect = new Rect();

	private Rect getRect() {
		if (mTmpRect == null) {
			return new Rect();
		}
		Rect r = mTmpRect;
		mTmpRect = null;
		return r;
	}

	private void releaseRect(Rect r) {
		mTmpRect = r;
	}

	public RTree() {
		mRoot = allocNode();
		mRoot.level = 0;
	}

	/**
	 * Insert item.
	 * 
	 * @param min Min of bounding rect
	 * @param max Max of bounding rect
	 * @param item data.
	 */
	public void insert(double min[], double max[], T item) {
		Rect r = getRect();
		r.set(min, max);
		insertRect(r, item, 0);
		releaseRect(r);
	}

	public void insert(Box box, T item) {
		Rect r = getRect();
		r.set(box);
		insertRect(r, item, 0);
		releaseRect(r);
	}

	/**
	 * Remove item.
	 * 
	 * @param min Min of bounding rect
	 * @param max Max of bounding rect
	 * @param item data.
	 */
	public boolean remove(double min[], double max[], T item) {
		Rect r = getRect();
		r.set(min, max);
		boolean removed = removeRect(r, item);
		releaseRect(r);
		return removed;
	}

	public boolean remove(Box box, T item) {
		Rect r = getRect();
		r.set(box);
		boolean removed = removeRect(r, item);
		releaseRect(r);
		return removed;
	}

	/**
	 * Find all items within search rectangle.
	 * 
	 * @param a_min Min of search bounding rect
	 * @param a_max Max of search bounding rect
	 * @param a_searchResult Search result array. Caller should set grow size.
	 *            Function will reset, not append to array.
	 * @param a_resultCallback Callback function to return result. Callback
	 *            should return 'true' to continue searching
	 * @param a_context User context to pass as parameter to a_resultCallback
	 * @return Returns the number of entries found
	 */
	public boolean search(double min[], double max[], SearchCb<T> cb, Object context) {
		Rect r = getRect();
		r.set(min, max);

		searchStack(r, cb, context);

		releaseRect(r);
		return true;
	}

	public boolean search(Box bbox, SearchCb<T> cb, Object context) {
		Rect r = getRect();
		r.set(bbox);

		searchStack(r, cb, context);

		releaseRect(r);
		return true;
	}

	public List<T> search(Box bbox, List<T> results) {
		if (results == null)
			results = new ArrayList<T>(16);

		Rect r = getRect();
		r.set(bbox);

		//search(mRoot, r, results);
		searchStack(r, results);

		releaseRect(r);
		return results;
	}

	/**
	 * Count the data elements in this container. This is slow as no internal
	 * counter is maintained.
	 */
	public int size() {
		int[] count = { 0 };
		countRec(mRoot, count);

		return count[0];
	}

	private void countRec(Node node, int[] count) {
		if (node.isLeaf()) {
			count[0] += node.count;
			return;
		}

		/* not a leaf node */
		Branch<Node>[] children = node.children();
		for (int idx = 0; idx < node.count; idx++) {
			countRec(children[idx].node, count);
		}
	}

	/**
	 * Remove all entries from tree.
	 */
	public void clear() {
		/* Delete all existing nodes */
		reset();

		mRoot = allocNode();
		mRoot.level = 0;
	}

	void reset() {
		removeAllRec(mRoot);
	}

	void removeAllRec(Node node) {
		assert (node != null);
		assert (node.level >= 0);

		if (!node.isLeaf()) {
			Branch<Node>[] children = node.children();
			/* This is an internal node in the tree */
			for (int idx = 0; idx < node.count; idx++) {
				removeAllRec(children[idx].node);
			}
		}
		freeNode(node);
	}

	public int nodesAlloc;
	public int nodesFree;

	public void printStats() {
		System.out.println("nodes alloc:\t" + nodesAlloc);
		System.out.println("nodes free:\t" + nodesFree);

	}

	Node allocNode() {
		nodesAlloc++;
		Node newNode;
		newNode = new Node(MAXNODES);
		return newNode;
	}

	void freeNode(Node node) {
		nodesFree++;
		assert (node != null);
	}

	/**
	 * Inserts a new data rectangle into the index structure.
	 * Recursively descends tree, propagates splits back up.
	 * 
	 * @param level The level argument specifies the number of steps up from the
	 *            leaf level to insert;
	 *            e.g. a data rectangle goes in at level = 0.
	 * 
	 * @return <code>false</code> if node was not split, old node was updated.
	 *         If node was split, returns <code>true</code> and sets the pointer
	 *         pointed to by newNode to point to the new node. Old node is
	 *         updated to become one of two.
	 */
	private Node insertRectRec(Rect rect, T item, Node node, int level) {
		//assert (rect != null && node != null && newNode != null);
		//assert (level >= 0 && level <= node.level);

		if (node.level > level) {
			/* Still above level for insertion, go down tree recursively */
			int idx = pickBranch(node, rect);
			Branch<Node>[] children = node.children();

			Node newNode = insertRectRec(rect, item, children[idx].node, level);
			if (newNode != null) {
				/* Child was split */
				node.branch[idx].setCover(children[idx].node);
				Branch<Node> branch = new Branch<Node>();
				branch.node = newNode;
				branch.setCover(newNode);
				if (node.addBranch(branch)) {
					return splitNode(node, branch);
				}
				return null;
			} else {
				/* Child was not split */
				node.branch[idx].add(rect);
				return null;
			}
		}

		/* Have reached level for insertion. Add rect, split if necessary */
		assert (node.level == level);
		Branch<T> branch = new Branch<T>();
		branch.set(rect);
		branch.node = item;

		/* Child field of leaves contains id of data record */
		if (node.addBranch(branch)) {
			return splitNode(node, branch);
		}
		return null;
	}

	final static double mergedArea(Rect a, Rect b) {
		return ((a.xmax > b.xmax ? a.xmax : b.xmax) - (a.xmin < b.xmin ? a.xmin : b.xmin)
		        * ((a.ymax > b.ymax ? a.ymax : b.ymax) - (a.ymin < b.ymin ? a.ymin : b.ymin)));
	}

	/**
	 * Pick a branch. Pick the one that will need the smallest increase
	 * in area to accomodate the new rectangle. This will result in the
	 * least total area for the covering rectangles in the current node.
	 * In case of a tie, pick the one which was smaller before, to get
	 * the best resolution when searching.
	 */
	int pickBranch(Node node, Rect rect) {
		boolean firstTime = true;
		double increase;
		double bestIncr = (double) -1;
		double area;
		double bestArea = 0;
		int best = 0;

		for (int idx = 0; idx < node.count; idx++) {
			Rect curRect = node.branch[idx];
			area = curRect.calcRectVolume();

			increase = mergedArea(rect, curRect) - area;

			if ((increase < bestIncr) || firstTime) {
				best = idx;
				bestArea = area;
				bestIncr = increase;
				firstTime = false;
			} else if ((increase == bestIncr) && (area < bestArea)) {
				best = idx;
				bestArea = area;
				bestIncr = increase;
			}
		}
		return best;
	}

	/**
	 * Insert a data rectangle into an index structure.
	 * InsertRect provides for splitting the root;
	 * returns 1 if root was split, 0 if it was not.
	 * The level argument specifies the number of steps up from the leaf
	 * level to insert; e.g. a data rectangle goes in at level = 0.
	 * InsertRect2 does the recursion.
	 */
	public boolean insertRect(Rect rect, T item, int level) {
		//		if (DEBUG) {
		//			assert (rect != null && root != null);
		//			assert (level >= 0 && level <= root.level);
		//		}

		Node root = mRoot;
		Node newNode = insertRectRec(rect, item, root, level);

		if (newNode != null) {
			/* Root split, Grow tree taller and new root */
			Node newRoot = allocNode();
			newRoot.level = root.level + 1;

			Branch<Node> branch = new Branch<Node>();
			branch.setCover(root);
			branch.node = root;
			newRoot.addBranch(branch);

			branch = new Branch<Node>();
			branch.setCover(newNode);
			branch.node = newNode;
			newRoot.addBranch(branch);

			mRoot = newRoot;

			return true;
		}

		return false;
	}

	/**
	 * Disconnect a dependent node.
	 * Caller must return (or stop using iteration index) after this as count
	 * has changed.
	 */
	void disconnectBranch(Node node, int idx) {
		assert (node != null && (idx >= 0) && (idx < MAXNODES));
		assert (node.count > 0);

		/* Remove element by swapping with the last element to
		 * prevent gaps in array */
		node.count--;

		if (node.count != idx) {
			node.branch[idx] = node.branch[node.count];
		}
		node.branch[node.count] = null;
	}

	/**
	 * Split a node.
	 * Divides the nodes branches and the extra one between two nodes.
	 * Old node is one of the new ones, and one really new one is created.
	 * Tries more than one method for choosing a partition, uses best result.
	 */
	Node splitNode(Node node, Branch<?> branch) {
		assert (node != null);
		assert (branch != null);

		Partition partition = mLocalVars.clear();

		/* Load all the branches into a buffer, initialize old node */
		int level = node.level;
		partition.getBranches(node, branch);

		/* Find partition */
		partition.choosePartition();

		/* Put branches from buffer into 2 nodes according to
		 * chosen partition */
		Node newNode = allocNode();
		newNode.level = node.level = level;
		partition.loadNodes(node, newNode);

		assert ((node.count + newNode.count) == partition.total);

		for (int i = node.count; i < MAXNODES; i++)
			node.branch[i] = null;

		return newNode;
	}

	private final ArrayList<Node> mReinsertNodes = new ArrayList<Node>();

	/**
	 * Delete a data rectangle from an index structure.
	 * Pass in a pointer to a Rect, the tid of the record, ptr to ptr to root
	 * node.
	 * 
	 * @return <code>false</code> if record not found, <code>true</code> if
	 *         success.
	 *         RemoveRect provides for eliminating the root.
	 */
	@SuppressWarnings("unchecked")
	public boolean removeRect(Rect rect, T item) {
		//assert (rect != null && root != null);
		//assert (root != null);

		Node root = mRoot;

		ArrayList<Node> reInsertList = mReinsertNodes;

		if (removeRectRec(rect, item, root, reInsertList)) {
			/* Found and deleted a data item
			 * Reinsert any branches from eliminated nodes */
			for (Node node : reInsertList) {
				for (int idx = 0; idx < node.count; idx++) {
					insertRect((node.branch[idx]),
					           (T) node.branch[idx].node,
					           node.level);
				}
				freeNode(node);
			}
			reInsertList.clear();

			/* Check for redundant root (not leaf, 1 child) and eliminate */
			if (root.count == 1 && !root.isLeaf()) {

				Node tempNode = root.children()[0].node;
				assert (tempNode != null);

				freeNode(root);
				mRoot = tempNode;
			}
			return true;
		}
		return false;
	}

	/**
	 * Delete a rectangle from non-root part of an index structure.
	 * Called by RemoveRect. Descends tree recursively, merges
	 * branches on the way back up.
	 * 
	 * @return <code>true</code> iff record found.
	 */
	private boolean removeRectRec(Rect rect, T item, Node node, ArrayList<Node> removed) {
		assert (rect != null && node != null && removed != null);
		assert (node.level >= 0);

		if (node.isLeaf()) {
			for (int idx = 0; idx < node.count; idx++) {
				if (node.branch[idx].node == item) {
					/* Must return after this call as count has changed */
					disconnectBranch(node, idx);
					return true;
				}
			}
			return false;
		}

		/* not a leaf node */
		for (int idx = 0; idx < node.count; idx++) {

			if (!rect.overlap(node.branch[idx]))
				continue;

			Branch<Node>[] children = node.children();
			if (removeRectRec(rect, item, children[idx].node, removed)) {
				if (children[idx].node.count >= MINNODES) {
					/* child removed, just resize parent rect */
					children[idx].setCover(children[idx].node);
				} else {
					/* child removed, not enough entries in node,
					 * eliminate node */
					removed.add(children[idx].node);

					/* Must return after this call as count has changed */
					disconnectBranch(node, idx);
				}
				return true;
			}
		}
		return false;
	}

	//	/**
	//	 * Search in an index tree or subtree for all data retangles that overlap
	//	 * the argument rectangle.
	//	 */
	//	public boolean search(Node node, Rect rect, int[] found, SearchCallback<T> cb,
	//	        Object context) {
	//		if (DEBUG) {
	//			assert (node != null);
	//			assert (node.level >= 0);
	//			assert (rect != null);
	//		}
	//
	//		if (!node.isLeaf()) {
	//			Branch<Node>[] children = node.children();
	//			/* This is an internal node in the tree */
	//			for (int idx = 0; idx < node.count; idx++) {
	//
	//				if (rect.overlap(children[idx])) {
	//					if (!search(children[idx].node, rect, found, cb, context)) {
	//						/* Stop searching */
	//						return false;
	//					}
	//				}
	//			}
	//			/* Continue searching */
	//			return true;
	//		}
	//
	//		/* This is a leaf node */
	//		for (int idx = 0; idx < node.count; idx++) {
	//			if (rect.overlap(node.branch[idx])) {
	//				@SuppressWarnings("unchecked")
	//				T item = (T) node.branch[idx].node;
	//
	//				/* NOTE: There are different ways to return results. Here's
	//				 * where to modify */
	//				found[0]++;
	//				if (!cb.call(item, context)) {
	//					/* Stop searching */
	//					return false;
	//				}
	//			}
	//		}
	//
	//		/* Continue searching */
	//		return true;
	//	}
	//
	//	public boolean search(Node node, Rect rect, List<T> results) {
	//		if (DEBUG) {
	//			assert (node != null);
	//			assert (node.level >= 0);
	//			assert (rect != null);
	//		}
	//
	//		if (!node.isLeaf()) {
	//			Branch<Node>[] children = node.children();
	//			/* This is an internal node in the tree */
	//			for (int idx = 0; idx < node.count; idx++) {
	//
	//				if (rect.overlap(children[idx])) {
	//					if (!search(children[idx].node, rect, results)) {
	//						/* Stop searching */
	//						return false;
	//					}
	//				}
	//			}
	//			/* Continue searching */
	//			return true;
	//		}
	//
	//		/* This is a leaf node */
	//		for (int idx = 0; idx < node.count; idx++) {
	//			if (rect.overlap(node.branch[idx])) {
	//				@SuppressWarnings("unchecked")
	//				T item = (T) node.branch[idx].node;
	//
	//				results.add(item);
	//			}
	//		}
	//
	//		/* Continue searching */
	//		return true;
	//	}

	public void searchStack(Rect rect, SearchCb<T> cb, Object context) {
		Stack stack = stackPool.get();
		stack.push(mRoot, 0);

		O: while (!stack.empty()) {
			stack.pop();
			Node node = stack.node();

			if (node.level == 0) {
				/* is leaf node */
				for (int idx = 0; idx < node.count; idx++) {
					@SuppressWarnings("unchecked")
					Branch<T> branch[] = (Branch<T>[]) node.branch;

					if (rect.overlap(branch[idx])) {
						if (!cb.call(branch[idx].node, context))
							break O;
					}
				}
			} else {
				int idx = stack.branchIndex();

				/* Push sibling on stack for future tree walk.
				 * This is the 'fall back' node when we finish with
				 * the current level */
				for (int i = idx + 1; i < node.count; i++) {
					if (rect.overlap(node.branch[i])) {
						stack.push(node, i);
						break;
					}
				}

				/* Push first of next level to get deeper into
				 * the tree */
				node = (Node) node.branch[idx].node;
				for (int i = 0; i < node.count; i++) {
					if (rect.overlap(node.branch[i])) {
						stack.push(node, i);
						break;
					}
				}
			}
		}
		stackPool.release(stack);
	}

	public boolean searchStack(Rect rect, List<T> results) {

		@SuppressWarnings("unchecked")
		List<Object> out = (List<Object>) results;

		Stack stack = stackPool.get();
		stack.push(mRoot, 0);

		while (!stack.empty()) {
			stack.pop();
			Node node = stack.node();

			if (node.level == 0) {
				/* is leaf node */
				for (int idx = 0; idx < node.count; idx++) {
					if (rect.overlap(node.branch[idx])) {
						out.add(node.branch[idx].node);
					}
				}
			} else {
				int idx = stack.branchIndex();
				/* Push sibling on stack for future tree walk.
				 * This is the 'fall back' node when we finish with
				 * the current level */
				for (int i = idx + 1; i < node.count; i++) {
					if (rect.overlap(node.branch[i])) {
						stack.push(node, i);
						break;
					}
				}

				/* Push first of next level* to get deeper into
				 * the tree */
				node = (Node) node.branch[idx].node;
				for (int i = 0; i < node.count; i++) {
					if (rect.overlap(node.branch[i])) {
						stack.push(node, i);
						break;
					}
				}
			}
		}
		stackPool.release(stack);
		return true;
	}

	/* Max stack size. Allows almost n^32 where n is number of branches in
	 * node */
	final static int MAX_STACK = 32;

	static class StackElement {
		Node node;
		int branchIndex;
	};

	SyncPool<Stack> stackPool = new SyncPool<Stack>(10) {
		@Override
		protected Stack createItem() {
			return new Stack();
		}

		protected boolean clearItem(Stack item) {
			if (item.tos != 0) {
				item.tos = 0;
				Arrays.fill(item.nodes, null);
			}
			return true;
		};
	};

	static class Stack extends Inlist<Stack> {
		/** Top Of Stack index */
		int tos;

		final Node[] nodes;
		final int[] branchIndex;

		Stack() {
			nodes = new Node[MAX_STACK];
			branchIndex = new int[MAX_STACK];
		}

		void push(Node node, int pos) {
			nodes[tos] = node;
			branchIndex[tos] = pos;
			tos++;
			assert (tos <= MAX_STACK);
		}

		/** Pop element off iteration stack (For internal use only) */
		boolean pop() {
			assert (tos > 0);
			nodes[tos] = null;
			tos--;
			//return stack[tos];
			return tos >= 0;
		}

		Node node() {
			return nodes[tos];
		}

		int branchIndex() {
			return branchIndex[tos];
		}

		boolean empty() {
			return tos <= 0;
		}
	}

	/* Iterator is not remove safe. */
	public static class Iterator<T> implements java.util.Iterator<T> {

		/** Stack as we are doing iteration instead of recursion */
		final StackElement stack[] = new StackElement[MAX_STACK];

		/** Top Of Stack index */
		int tos;

		Iterator(Node root) {
			for (int i = 0; i < MAX_STACK; i++)
				stack[i] = new StackElement();

			push(root, 0);

			findNextData();
		}

		/** Is iterator invalid */
		boolean isNull() {
			return (tos <= 0);
		}

		/** Is iterator pointing to valid data */
		boolean isNotNull() {
			return (tos > 0);
		}

		/* Find the next data element */
		@SuppressWarnings("unchecked")
		public T next() {
			assert (isNotNull());
			StackElement curTos = stack[tos - 1];
			T result = (T) curTos.node.branch[curTos.branchIndex].node;
			curTos.branchIndex++;
			findNextData();
			return result;
		}

		/** Find the next data element in the tree (For internal use only) */
		boolean findNextData() {
			for (;;) {
				if (tos <= 0)
					return false;

				/* Copy stack top cause it may change as we use it */
				StackElement curTos = pop();

				if (curTos.node.isLeaf()) {
					/* Keep walking through data while we can */
					if (curTos.branchIndex < curTos.node.count) {

						/* There is more data, just point to the next one */
						push(curTos.node, curTos.branchIndex);
						return true;
					}
					continue;
				}
				int idx = curTos.branchIndex;
				/* No more data, so it will fall back to previous level */
				if (idx + 1 < curTos.node.count) {
					/* Push sibling on stack for future tree walk.
					 * This is the 'fall back' node when we finish with
					 * the current level */
					push(curTos.node, idx + 1);
				}

				/* Since cur node is not a leaf, push first of next level
				 * to get deeper into the tree */
				Node nextLevelnode = (Node) curTos.node.branch[idx].node;

				push(nextLevelnode, 0);

				/* If we pushed on a new leaf, exit as the data is ready
				 * at TOS */
				if (nextLevelnode.isLeaf())
					return true;
			}
		}

		/** Push node and branch onto iteration stack (For internal use only) */
		void push(Node node, int branchIndex) {
			stack[tos].node = node;
			stack[tos].branchIndex = branchIndex;
			tos++;
			assert (tos <= MAX_STACK);
		}

		/** Pop element off iteration stack (For internal use only) */
		StackElement pop() {
			assert (tos > 0);
			tos--;
			return stack[tos];
		}

		@Override
		public boolean hasNext() {
			return isNotNull();
		}

		@Override
		public void remove() {

		}
	}

	@Override
	public java.util.Iterator<T> iterator() {
		return new Iterator<T>(mRoot);
	}

}

/** Temporary data and methods to split partition */
class Partition {
	int total;
	int minFill;

	final int partition[];
	final boolean taken[];
	final int count[];
	final Rect cover[];
	final double area[];

	final Rect coverSplit;
	double coverSplitArea;

	final Branch<Object> branchBuf[];

	final double mTmpArea[];

	public Partition clear() {
		int maxnodes = branchBuf.length;
		for (int i = 0; i < maxnodes; i++) {
			taken[i] = false;
			partition[i] = -1;
		}

		count[0] = count[1] = 0;
		area[0] = area[1] = 0;

		total = maxnodes;

		return this;
	}

	@SuppressWarnings("unchecked")
	public Partition(int maxnodes, int minnodes) {
		partition = new int[maxnodes + 1];
		taken = new boolean[maxnodes + 1];
		count = new int[2];
		area = new double[2];
		branchBuf = new Branch[maxnodes + 1];
		cover = new Rect[] { new Rect(), new Rect() };
		coverSplit = new Rect();

		minFill = minnodes;

		mTmpArea = new double[maxnodes + 1];
	}

	/**
	 * Copy branches from the buffer into two nodes according to the
	 * partition.
	 * 
	 * FIXME this actually *moves* items from buffer!
	 */
	void loadNodes(Node nodeA, Node nodeB) {
		assert (nodeA != null);
		assert (nodeB != null);

		for (int idx = 0; idx < total; idx++) {
			switch (partition[idx]) {
				case 0:
					nodeA.addBranch(branchBuf[idx]);
					break;
				case 1:
					nodeB.addBranch(branchBuf[idx]);
					break;
			}
		}
	}

	/**
	 * Load branch buffer with branches from full node plus the extra
	 * branch.
	 */
	@SuppressWarnings("unchecked")
	void getBranches(Node node, Branch<?> branch) {
		assert (node != null);
		assert (branch != null);
		assert (node.count == node.branch.length);

		/* Load the branch buffer */
		for (int idx = 0; idx < node.count; idx++)
			branchBuf[idx] = (Branch<Object>) node.branch[idx];

		branchBuf[node.count] = (Branch<Object>) branch;

		/* Calculate rect containing all in the set */
		coverSplit.set(branchBuf[0]);
		for (int idx = 1, n = branchBuf.length; idx < n; idx++) {
			coverSplit.add(branchBuf[idx]);
		}
		coverSplitArea = coverSplit.calcRectVolume();

		/* init node - FIXME needed? */
		node.count = 0;
		node.level = -1;
	}

	private void classify(int idx, int group) {
		if (taken[idx])
			throw new IllegalStateException("Index already used!"
			        + idx + " " + Arrays.toString(taken));

		partition[idx] = group;
		taken[idx] = true;

		if (count[group] == 0) {
			cover[group].set(branchBuf[idx]);
		} else {
			cover[group].add(branchBuf[idx]);
		}

		area[group] = cover[group].calcRectVolume();
		count[group]++;
	}

	private void pickSeeds() {
		int seed0 = 0, seed1 = 1;

		double tmpArea[] = mTmpArea;;

		for (int idx = 0; idx < total; idx++) {
			tmpArea[idx] = branchBuf[idx].calcRectVolume();
		}

		double worst = -coverSplitArea - 1;

		for (int idxA = 0; idxA < total - 1; idxA++) {
			for (int idxB = idxA + 1; idxB < total; idxB++) {

				double waste = RTree.mergedArea(branchBuf[idxA], branchBuf[idxB])
				        - (tmpArea[idxA] + tmpArea[idxB]);

				if (waste > worst) {
					worst = waste;
					seed0 = idxA;
					seed1 = idxB;
				}
			}
		}
		classify(seed0, 0);
		classify(seed1, 1);
	}

	/**
	 * Method #0 for choosing a partition:
	 * As the seeds for the two groups, pick the two rects that would waste
	 * the most area if covered by a single rectangle, i.e. evidently the worst
	 * pair to have in the same group.
	 * Of the remaining, one at a time is chosen to be put in one of the two
	 * groups.
	 * The one chosen is the one with the greatest difference in area
	 * expansion
	 * depending on which group - the rect most strongly attracted to one
	 * group and repelled from the other.
	 * If one group gets too full (more would force other group to violate
	 * min fill requirement) then other group gets the rest.
	 * These last are the ones that can go in either group most easily.
	 */
	void choosePartition() {
		double biggestDiff;
		int group, chosen = 0, betterGroup = 0;

		pickSeeds();

		while (((count[0] + count[1]) < total)
		        && (count[0] < (total - minFill))
		        && (count[1] < (total - minFill))) {

			biggestDiff = (double) -1;
			for (int idx = 0; idx < total; idx++) {
				if (taken[idx])
					continue;

				double growth0 = RTree.mergedArea(branchBuf[idx], cover[0]) - area[0];
				double growth1 = RTree.mergedArea(branchBuf[idx], cover[1]) - area[1];

				double diff = growth1 - growth0;
				if (diff >= 0) {
					group = 0;
				} else {
					group = 1;
					diff = -diff;
				}

				if (diff > biggestDiff) {
					biggestDiff = diff;
					chosen = idx;
					betterGroup = group;
				} else if ((diff == biggestDiff)
				        && (count[group] < count[betterGroup])) {
					chosen = idx;
					betterGroup = group;
				}
			}
			classify(chosen, betterGroup);
		}

		/* If one group too full, put remaining rects in the other */
		if ((count[0] + count[1]) < total) {
			if (count[0] >= total - minFill) {
				group = 1;
			} else {
				group = 0;
			}
			for (int index = 0; index < total; index++) {
				if (!taken[index]) {
					classify(index, group);
				}
			}
		}

		assert ((count[0] + count[1]) == total);
		assert ((count[0] >= minFill) && (count[1] >= minFill));
	}

}

//	/** Minimal bounding rectangle (n-dimensional) */
//	static class Rect {
//
//		/** dimensions of bounding box */
//		double bounds[] = new double[NUMDIMS * 2];
//
//		//double xmin, ymin, xmax, ymax;
//
//		public Rect() {
//		}
//
//		public Rect(double[] min, double[] max) {
//
//			if (DEBUG) {
//				for (int index = 0; index < NUMDIMS; index++) {
//					assert (min[index] <= max[index]);
//				}
//			}
//
//			for (int axis = 0; axis < NUMDIMS; axis++) {
//				this.bounds[axis] = min[axis];
//				this.bounds[NUMDIMS + axis] = max[axis];
//			}
//		}
//
//		/**
//		 * Calculate the n-dimensional volume of a rectangle
//		 */
//		public double calcRectVolume() {
//			double volume = (double) 1;
//
//			for (int idx = 0; idx < NUMDIMS; idx++) {
//				volume *= bounds[NUMDIMS + idx] - bounds[idx];
//			}
//
//			assert (volume >= 0);
//			return volume;
//		}
//
//		/**
//		 * Decide whether two rectangles overlap.
//		 */
//		public boolean overlap(Rect other) {
//			assert (other != null);
//
//			for (int idx = 0; idx < NUMDIMS; idx++) {
//				if (bounds[idx] > other.bounds[NUMDIMS + idx] ||
//				        other.bounds[idx] > bounds[NUMDIMS + idx]) {
//					return false;
//				}
//			}
//			return true;
//		}
//
//		/**
//		 * Combine two rectangles into larger one containing both
//		 */
//		public Rect(Rect rectA, Rect rectB) {
//			assert (rectA != null && rectB != null);
//			for (int min = 0, max = NUMDIMS; min < NUMDIMS; min++, max++) {
//				bounds[min] = Math.min(rectA.bounds[min], rectB.bounds[min]);
//				bounds[max] = Math.max(rectA.bounds[max], rectB.bounds[max]);
//			}
//		}
//
//		public void add(Rect rect) {
//			for (int min = 0, max = NUMDIMS; min < NUMDIMS; min++, max++) {
//				bounds[min] = Math.min(bounds[min], rect.bounds[min]);
//				bounds[max] = Math.max(bounds[max], rect.bounds[max]);
//			}
//		}
//
//		public void set(Rect rect) {
//			for (int idx = 0; idx < NUMDIMS * 2; idx++) {
//				bounds[idx] = rect.bounds[idx];
//			}
//		}
//
//		/**
//		 * Find the smallest rectangle that includes all rectangles in branches
//		 * of a node.
//		 */
//		void setCover(Node node) {
//			assert (node != null);
//
//			set(node.branch[0]);
//			for (int idx = 1; idx < node.count; idx++) {
//				add(node.branch[idx]);
//			}
//		}
//	}