#include "stdafx.h"
#include "BVH.h"
#include <cmath>
#include <vector>
#include <queue>
#include <map>
#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;

struct point
{
	double x[3];
	int id;
	point()
	{
		x[0] = x[1] = x[2] = 0;
		id = 0;
	}
	// friend bool operator < (const point &a,const point &b)
	//	{return a.x[cmpid]<b.x[cmpid];}
};

bool cmp0(const point &a, const point &b)
{
	return a.x[0] < b.x[0];
}

bool cmp1(const point &a, const point &b)
{
	return a.x[1] < b.x[1];
}

bool cmp2(const point &a, const point &b)
{
	return a.x[2] < b.x[2];
}

struct node
{
	double dis;
	int id;
	bool operator<(node b) const
	{
		return dis < b.dis || (dis == b.dis && id < b.id);
	}
	node()
	{
		dis = 0;
		id = 0;
	}
	node(int id1, double dis1)
	{
		dis = dis1;
		id = id1;
	}
};
struct tree
{
	point p;
	double mx[3], mn[3];
	int ls, rs, id;
	tree()
	{
		ls = rs = id = 0;
	}
};
struct KDtree
{
	int n, tot, root;
	double X, Y, Z;

	priority_queue<node> q;
	vector<int> vec;

	point p[MAXPointNum];
	tree t[MAXPointNum];

	KDtree()
	{
		X = Y = Z = 0;
		n = tot = root = 0;
	}
	inline double dis(tree &x)
	{
		double P = (x.p.x[0] - X) * (x.p.x[0] - X);
		double Q = (x.p.x[1] - Y) * (x.p.x[1] - Y);
		double O = (x.p.x[2] - Z) * (x.p.x[2] - Z);
		return P + Q + O;
	}
	inline double mndis(tree &x)
	{
		double P = (x.mn[0] - X) * (x.mn[0] - X);
		double M = (x.mx[0] - X) * (x.mx[0] - X);
		if (X >= x.mn[0] && X <= x.mx[0])
			P = M = 0;
		double Q = (x.mn[1] - Y) * (x.mn[1] - Y);
		double N = (x.mx[1] - Y) * (x.mx[1] - Y);
		if (Y >= x.mn[1] && Y <= x.mx[1])
			Q = N = 0;
		double O = (x.mn[2] - Z) * (x.mn[2] - Z);
		double U = (x.mx[2] - Z) * (x.mx[2] - Z);
		if (Z >= x.mn[2] && Z <= x.mx[2])
			O = U = 0;
		return min(P, M) + min(Q, N) + min(O, U);
	}
	inline void update(int x)
	{
		if (!x)
			return;
		int l = t[x].ls, r = t[x].rs;
		if (l)
			t[x].mn[0] = min(t[x].mn[0], t[l].mn[0]),
			t[x].mn[1] = min(t[x].mn[1], t[l].mn[1]),
			t[x].mn[2] = min(t[x].mn[2], t[l].mn[2]),
			t[x].mx[0] = max(t[x].mx[0], t[l].mx[0]),
			t[x].mx[1] = max(t[x].mx[1], t[l].mx[1]),
			t[x].mx[2] = max(t[x].mx[2], t[l].mx[2]);
		if (r)
			t[x].mn[0] = min(t[x].mn[0], t[r].mn[0]),
			t[x].mn[1] = min(t[x].mn[1], t[r].mn[1]),
			t[x].mn[2] = min(t[x].mn[2], t[r].mn[2]),
			t[x].mx[0] = max(t[x].mx[0], t[r].mx[0]),
			t[x].mx[1] = max(t[x].mx[1], t[r].mx[1]),
			t[x].mx[2] = max(t[x].mx[2], t[r].mx[2]);
	}
	void query(int x)
	{
		if (!x)
			return;
		double res = dis(t[x]);
		if (res < q.top().dis || (res == q.top().dis && t[x].id < q.top().id))
			q.pop(), q.push(node(t[x].id, res));
		int l = t[x].ls, r = t[x].rs;
		double ld = 0, rd = 0;
		if (l)
			ld = mndis(t[l]);
		if (r)
			rd = mndis(t[r]);
		// cout<<x<<"  "<<ld<<"  "<<rd<<"    "<<res<<"  "<<q.top().id<<" "<<q.top().dis<<endl;
		if (ld < rd)
		{
			if (ld <= q.top().dis)
				query(l);
			if (rd <= q.top().dis)
				query(r);
		}
		else
		{
			if (rd <= q.top().dis)
				query(r);
			if (ld <= q.top().dis)
				query(l);
		}
	}
	void queryd(int x, double d)
	{
		if (!x)
			return;
		double res = dis(t[x]);
		// cout<<t[x].p.id<<"  "<<res<<endl;
		if (res <= d * d)
			vec.push_back(t[x].p.id);
		int l = t[x].ls, r = t[x].rs;
		double ld = 0, rd = 0;
		if (l)
			ld = mndis(t[l]);
		if (r)
			rd = mndis(t[r]);
		// cout<<x<<"  "<<ld<<"  "<<rd<<"    "<<res<<"  "<<q.top().id<<" "<<q.top().dis<<endl;
		if (ld < rd)
		{
			if (ld <= d * d)
				queryd(l, d);
			if (rd <= d * d)
				queryd(r, d);
		}
		else
		{
			if (rd <= d * d)
				queryd(r, d);
			if (ld <= d * d)
				queryd(l, d);
		}
	}
	inline void add(P &s)
	{
		n++;
		p[n].x[0] = s.x;
		p[n].x[1] = s.y;
		p[n].x[2] = s.z;
		p[n].id = n;
	}
	void build(int &x, int l, int r, int k)
	{
		if (l > r)
			return;
		x = ++tot;
		int mid = (l + r) >> 1;
		// cout<<"stage1  "<<x<<endl;
		if (k == 0)
			nth_element(p + l, p + mid, p + r + 1, cmp0);
		else if (k == 1)
			nth_element(p + l, p + mid, p + r + 1, cmp1);
		else if (k == 2)
			nth_element(p + l, p + mid, p + r + 1, cmp2);
		// cout<<"stage2"<<endl;
		t[x].p = p[mid];
		t[x].id = t[x].p.id;
		t[x].mn[0] = t[x].mx[0] = t[x].p.x[0];
		t[x].mn[1] = t[x].mx[1] = t[x].p.x[1];
		t[x].mn[2] = t[x].mx[2] = t[x].p.x[2];
		build(t[x].ls, l, mid - 1, (k + 1) % 3);
		build(t[x].rs, mid + 1, r, (k + 1) % 3);
		// cout<<"stage3"<<endl;
		update(x);
		// cout<<x<<"  "<<t[x].ls<<"  "<<t[x].rs<<"           "<<t[x].mx[0]<<" "<<t[x].mx[1]<<" "<<t[x].mx[2]<<endl;
	}
	inline void build()
	{
		cout << root << "  " << n << endl;
		build(root, 1, n, 0);
	}

	// 返回离s最近的k个点的编号
	inline vector<int> search_by_k(P &s, int k)
	{
		X = s.x;
		Y = s.y;
		Z = s.z;
		while (q.size())
			q.pop();
		for (int j = 1; j <= k; j++)
			q.push(node(0, 1e9));
		query(root);
		vector<int> veck;
		while (q.size())
		{
			if (q.top().id != 0)
				veck.push_back(q.top().id);
			q.pop();
		}
		return veck;
	}

	// 返回距离s点d以内的所有点的编号
	inline vector<int> search_by_dis(P &s, double d)
	{
		X = s.x;
		Y = s.y;
		Z = s.z;
		vec.clear();
		queryd(root, d);
		return vec;
	}
} kdtree;