using System;
using System.Collections.Generic;
using UnityEngine;

namespace Mtree
{
	public class KDTree
	{
		private KDTreeNode root;

		public KDTree(Vector3[] points, Vector3 t, string TreeName)
		{
			if (points.Length == 0)
			{
				points = new Vector3[1] { t };
			}
			root = new KDTreeNode(points[0]);
			for (int i = 1; i < points.Length; i++)
			{
				AddPoint(points[i]);
			}
		}

		public KDTree()
		{
			root = new KDTreeNode(Vector3.zero);
		}

		public void AddPoint(Vector3 point)
		{
			root.AddPoint(point, 0);
		}

		private KDTreeNode BuildFromPointsRec(Vector3[] points, int axis, int start, int end)
		{
			Debug.Log(start + " " + end);
			if (start == end - 1)
			{
				Debug.Log(start);
				return new KDTreeNode(points[start]);
			}
			int num = UnityEngine.Random.Range(start, end - 1);
			KDTreeNode kDTreeNode = new KDTreeNode(points[num]);
			axis = (axis + 1) % 3;
			kDTreeNode.left = BuildFromPointsRec(points, axis, start, num);
			kDTreeNode.right = BuildFromPointsRec(points, axis, num, end);
			return kDTreeNode;
		}

		public Vector3 FindClosest(Vector3 point)
		{
			float currBestDist = float.PositiveInfinity;
			Vector3 currBest = root.position;
			root.FindClosestRec(point, ref currBestDist, ref currBest, 0);
			return currBest;
		}

		public List<Vector3> RadiusSearch(Vector3 center, float radius)
		{
			List<Vector3> neighbours = new List<Vector3>();
			root.RadiusSearchRec(center, radius * radius, ref neighbours, 0);
			return neighbours;
		}

		private int SelectMedian(Vector3[] points, int axis, int start, int end)
		{
			float[] array = new float[end - start];
			for (int i = start; i < end; i++)
			{
				array[i - start] = points[i][axis];
			}
			int[] array2 = new int[array.Length];
			for (int j = 0; j < array2.Length; j++)
			{
				array2[j] = j;
			}
			Array.Sort(array, array2);
			return array2[array2.Length / 2];
		}
	}
}