/*-
 *  Copyright 2020 Beuth Hochschule für Technik Berlin, Hochschule für Technik Stuttgart
 * 
 *  This file is part of CityDoctor2.
 *
 *  CityDoctor2 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.
 *
 *  CityDoctor2 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 CityDoctor2.  If not, see <https://www.gnu.org/licenses/>.
 */
package de.hft.stuttgart.citydoctor2.math.graph;

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

import de.hft.stuttgart.citydoctor2.datastructure.Vertex;
import de.hft.stuttgart.citydoctor2.math.Vector3d;

/**
 * KD-Tree implementation to filter out duplicate points
 * 
 * @author Matthias Betz
 *
 */
public class KDTree {

	private static final int K = 3;

	private Vertex location;
	private int axis;

	private KDTree left;
	private KDTree right;

	public KDTree() {
		axis = 0;
	}

	public KDTree(Vertex median) {
		this(median, 0);
	}

	private KDTree(Vertex median, int depth) {
		location = median;
		axis = depth % K;
	}

	public void add(Vertex node) {
		if (location == null) {
			// root node is empty, set as root
			location = node;
			return;
		}
		if (node.getCoordinate(axis) < location.getCoordinate(axis)) {
			// left tree
			if (left == null) {
				// insert node
				left = new KDTree(node, axis + 1);
			} else {
				// search next tree
				left.add(node);
			}
		} else {
			// right tree
			if (right == null) {
				// insert node
				right = new KDTree(node, axis + 1);
			} else {
				// search next tree
				right.add(node);
			}
		}
	}

	public Vertex getFirstNodeInRadius(Vertex node, double radius) {
		// construct box:
		Vector3d leftBotFront = node.plus(-radius);

		Vector3d topRightBehind = node.plus(radius);

		double radiusSquare = radius * radius;
		for (Vertex point : getNodesInRange(leftBotFront, topRightBehind, new ArrayList<>())) {
			if (point.getDistanceSquare(node) <= radiusSquare) {
				return point;
			}
		}

		return null;
	}

	public List<Vertex> getNodesInRange(Vertex node, double radius) {
		// construct box:
		Vector3d leftBotFront = node.copyVector();
		leftBotFront.plus(-radius);

		Vector3d topRightBehind = node.copyVector();
		topRightBehind.plus(radius);

		List<Vertex> nodesInRangeBox = getNodesInRange(leftBotFront, topRightBehind, new ArrayList<>());
		List<Vertex> nodesInRange = new ArrayList<>();
		double radiusSquare = radius * radius;
		for (Vertex v : nodesInRangeBox) {
			if (v.getDistanceSquare(node) <= radiusSquare) {
				nodesInRange.add(v);
			}
		}
		return nodesInRange;
	}

	private List<Vertex> getNodesInRange(Vector3d bbLeft, Vector3d bbRight, List<Vertex> result) {
		boolean added = false;
		if (location == null) {
			return result;
		}
		double split = location.getCoordinate(axis);
		if (bbLeft.getCoordinate(axis) <= split) {
			if (bbRight.getCoordinate(axis) >= split) {
				// potential result
				result.add(location);
				added = true;
			}
			if (left != null) {
				left.getNodesInRange(bbLeft, bbRight, result);
			}
		}
		if (bbRight.getCoordinate(axis) >= split) {
			if (!added && bbLeft.getCoordinate(axis) <= split) {
				result.add(location);
			}
			if (right != null) {
				right.getNodesInRange(bbLeft, bbRight, result);
			}
		}
		return result;
	}

	public List<Vertex> getVertices() {
		List<Vertex> vertices = new ArrayList<>();
		getVertices(vertices);
		return vertices;
	}

	private void getVertices(List<Vertex> vertices) {
		vertices.add(location);
		if (left != null) {
			left.getVertices(vertices);
		}
		if (right != null) {
			right.getVertices(vertices);
		}
	}

}
