#include "pch.h" // use stdafx.h in Visual Studio 2017 and earlier
#include "Astar.h"

Astar astar;

void DSU::init(int cnt)
{
	for (int i = 1; i <= cnt; i++)
		f[i] = i;
}
int DSU::getrt(int x)
{
	if (x != f[x])
		return f[x] = getrt(f[x]);
	return x;
}
void DSU::merge(int x, int y)
{
	if (x != f[x])
		x = getrt(x);
	if (y != f[y])
		y = getrt(y);
	f[x] = y;
}

bool Node::operator<(Node B) const
{
	// if(turncnt!=B.turncnt)return turncnt>B.turncnt;
	return dis > B.dis;
}
Node::Node()
{
	xs = 0;
	dis = 0;
	turncnt = 0;
}
Node::Node(int xs1, int turnc, double dis1)
{
	xs = xs1;
	dis = dis1;
	turncnt = turnc;
}

Astar::Astar()
{
	pnum = 0;
	l = bend_r = 0;
	tnum = 0;
	pclipSet = &clipSet;
}

void Astar::addBranchTree(BranchTree *pbranchTree)
{
	this->pbranchTree = pbranchTree;
}

// 初始化
void Astar::init()
{
	pnum += 2;
}

// 添加一个卡箍点
void Astar::add_clip(P &p, P &d)
{
	pnum++;
	tnum++;
	points[pnum] = p;
	points[pnum].ref = pnum;

	P dir = p - d;
	points[pnum].dx = dir.x;
	points[pnum].dy = dir.y;
	points[pnum].dz = dir.z;
	block[pnum] = 0;
}

void Astar::add_clip(Clip &cp)
{
	pnum++;
	tnum++;
	points[pnum] = cp.coord;
	block[pnum] = 0;
}

// 添加一个分支点
void Astar::add_branchPoint(BranchPoint &bp)
{
	tnum = max(tnum, clipSet.clipnum + 2);
	tnum++;
	points[tnum] = bp.coord;
	block[tnum] = 0;
}

Path Astar::search_pair(P &start, P &goal, P &start_dir, P &goal_dir, double dia)
{
	this->start = start;
	this->goal = goal;
	this->start_dir = start_dir;
	this->goal_dir = goal_dir;
	path.dia = dia;
	pnum -= 2;
	path.points.clear();
	path.wirelist.clear();
	path.dia = dia;
	search();
	return path;
}

Path Astar::search_pair(P &start, P &goal, double dia)
{
	this->start = start;
	this->goal = goal;
	path.dia = dia;

	pnum -= 2;
	path.points.clear();
	path.wirelist.clear();
	path.dia = dia;
	search();
	return path;
}

Path Astar::search_pair(Bund &bd)
{
	this->start = bd.start;
	this->goal = bd.goal;
	pnum -= 2;
	path.points.clear();
	path.inOut.clear();
	path.wirelist = bd.wirenamelist;
	path.dia = bd.dia;
	tnum = max(tnum, clipSet.clipnum + 2);
	search();
	return path;
}

void Astar::searchForPreProcess()
{
	while (!pq.empty())
		pq.pop();
	points[++pnum] = start;
	points[++pnum] = goal;
	S = pnum - 1;
	T = pnum;

	for (int i = 1; i <= 2 * tnum + 2; i++)
		vis[i] = 0, dis[i] = Node(i, 1000, 1e9), pre[i] = 0;

	dis[S * 2] = Node(S * 2, 0, 0);

	assert(pre[2 * S] == 0);

	pq.push(dis[S * 2]);
	vector<pair<int, int>> tempvec = basicEdge.getEXTNextPoint(points[T], pnum, 1);
	vector<pair<int, int>> pointsConnectedToGoal;
	for (int i = 0; i < tempvec.size(); i++)
	{
		int xs = tempvec[i].first;
		if (!bvh.iscollect(points[xs], points[T]))
			pointsConnectedToGoal.push_back(tempvec[i]);
	}
	bool testflag = true;
	while (!pq.empty())
	{
		Node node = pq.top();
		// cout<<pq.top().xs<<endl;

		pq.pop();
		while (vis[node.xs] && !pq.empty())
			node = pq.top(), pq.pop();

		vis[node.xs] = 1;

		int xs = node.xs;
		int inOut = xs % 2;
		xs /= 2;

		pair<int, int> currentNode = make_pair(xs, inOut);

		if (xs == T)
			break;

		vector<pair<int, int>> vec;
		if (xs != S)
			vec = basicEdge.getNextPoint(xs, inOut, pnum);
		else if (xs == S && nearClip[points[xs]] != make_pair(0, 0))
			vec.push_back(nearClip[points[S]]);
		else
		{
			vec = basicEdge.getEXTNextPoint(points[S], pnum, 0);
			// cout<<vec.size()<<endl;
		}

		if (nearClip[points[T]] == make_pair(0, 0))
		{
			for (int i = 0; i < pointsConnectedToGoal.size(); i++)
			{
				if (pointsConnectedToGoal[i] == currentNode)
					vec.push_back(make_pair(T, 0));
			}
		}
		else if (nearClip[points[T]] == currentNode)
			vec.push_back(make_pair(T, 0));

		for (int i = 0; i < vec.size(); i++)
		{
			int v = vec[i].first;
			int mode = vec[i].second;
			if (v < pnum - 1)
			{
				// if(!(pclipSet->transitable(v,0,path.dia)))continue;
				// if(block[v])continue;
				;
			}
			// cout<<v<<endl;
			if (v == xs)
				cout << "v==xs " << v << endl;
			/*
			if(points[xs]==points[v]){
				cout<<xs<<"  "<<v<<endl;
				cout<<points[xs].x<<"  "<<points[xs].y<<"  "<<points[xs].z<<endl;
			}
			*/

			if (points[xs] == points[v])
				continue;
			assert(xs != v);
			assert(points[xs] != points[v]);
			assert(v != pnum - 1);
			// assert(pnum==2833);
			assert(pre[2 * (pnum - 1)] == 0);
			assert(pre[2 * (pnum - 1) + 1] == 0);

			double cst = distan(points[xs], points[v], inOut, mode);
			if (points[xs].type == 1 || points[xs].type == 2)
			{
				int bid = points[xs].ref;
				BranchPoint *pb = branchPointSet.getBranchPointPointer(bid);
				cst = pb->computeWeight(points[v], mode, 0);
			}
			if (points[v].type == 1 || points[v].type == 2)
			{
				int bid = points[v].ref;
				BranchPoint *pb = branchPointSet.getBranchPointPointer(bid);
				cst = pb->computeWeight(points[xs], inOut, 1);
			}
			cst += +dis[xs * 2 + inOut].dis;

			if (dis[2 * v + mode] < Node(2 * v + mode, 0, cst))
			{
				dis[2 * v + mode] = Node(2 * v + mode, 0, cst);
				assert(v != pnum - 1);
				pre[2 * v + mode] = xs * 2 + inOut;
				pq.push(dis[2 * v + mode]);
			}
		}
	}
	int top = 0;
	int tag = T * 2;
	while (pre[tag])
	{
		st[++top] = tag;
		tag = pre[tag];
		if (testflag)
			cout << "nodeid " << tag << endl;
	}
	if (top)
	{
		st[++top] = S * 2;
	}
	for (int i = top; i > 0; i--)
	{
		int id = st[i];
		if (top > 2)
		{
			if (id / 2 != S && id / 2 != T)
			{
				if (i == top - 1)
					nearClip[points[S]] = make_pair(id / 2, id % 2);
				if (i == 2)
					nearClip[points[T]] = make_pair(id / 2, id % 2);
			}
		}
		// if(i!=top&&i!=1)pclipSet->addwire(id,dia);
		path.points.push_back(points[id / 2]);
		if (i != top && i != 1 && id / 2 < pnum)
			path.inOut.push_back(id % 2);
		else
			path.inOut.push_back(-1 - id % 2);
	}
}

void Astar::search()
{
	searchNum++;
	cout << "searchNum " << searchNum << endl;

	while (!pq.empty())
		pq.pop();
	points[++pnum] = start;
	points[++pnum] = goal;
	S = pnum - 1;
	T = pnum;

	for (int i = 1; i <= 2 * tnum + 2; i++)
		vis[i] = 0, dis[i] = Node(i, 1000, 1e9), pre[i] = 0;

	dis[S * 2] = Node(S * 2, 0, 0);

	assert(pre[2 * S] == 0);

	pq.push(dis[S * 2]);
	vector<pair<int, int>> tempvec = basicEdge.getEXTNextPoint(points[T], pnum, 1);
	vector<pair<int, int>> pointsConnectedToGoal;
	for (int i = 0; i < tempvec.size(); i++)
	{
		int xs = tempvec[i].first;
		if (!bvh.iscollect(points[xs], points[T]))
			pointsConnectedToGoal.push_back(tempvec[i]);
	}
	bool testflag = true;
	int whilenum = 0;
	while (!pq.empty())
	{
		Node node = pq.top();
		// cout<<pq.top().xs<<endl;
		whilenum++;
		if (searchNum == 105)
			cout << "whilenum " << whilenum << endl;

		pq.pop();
		while (vis[node.xs] && !pq.empty())
			node = pq.top(), pq.pop();

		vis[node.xs] = 1;

		int xs = node.xs;
		int inOut = xs % 2;
		xs /= 2;

		pair<int, int> currentNode = make_pair(xs, inOut);

		if (xs == T)
			break;

		vector<pair<int, int>> vec;
		if (xs != S)
			vec = basicEdge.getNextPoint(xs, inOut, pnum);
		else if (xs == S && nearClip[points[xs]] != make_pair(0, 0))
			vec.push_back(nearClip[points[S]]);
		else
		{
			vec = basicEdge.getEXTNextPoint(points[S], pnum, 0);
			// cout<<vec.size()<<endl;
		}

		if (nearClip[points[T]] == make_pair(0, 0))
		{
			for (int i = 0; i < pointsConnectedToGoal.size(); i++)
			{
				if (pointsConnectedToGoal[i] == currentNode)
					vec.push_back(make_pair(T, 0));
			}
		}
		else if (nearClip[points[T]] == currentNode)
			vec.push_back(make_pair(T, 0));

		for (int i = 0; i < vec.size(); i++)
		{
			int v = vec[i].first;
			int mode = vec[i].second;
			if (v < pnum - 1)
			{
				// if(!(pclipSet->transitable(v,0,path.dia)))continue;
				// if(block[v])continue;
				;
			}
			// cout<<v<<endl;
			if (v == xs)
				cout << "v==xs " << v << "  " << points[v].type << endl;

			if (vis[v * 2 + mode])
				continue;
			/*
			if(points[xs]==points[v]){
				cout<<xs<<"  "<<v<<endl;
				cout<<points[xs].x<<"  "<<points[xs].y<<"  "<<points[xs].z<<endl;
			}
			*/

			if (points[xs] == points[v])
				continue;
			assert(xs != v);
			assert(points[xs] != points[v]);
			assert(v != pnum - 1);
			// assert(pnum==2833);
			assert(pre[2 * (pnum - 1)] == 0);
			assert(pre[2 * (pnum - 1) + 1] == 0);

			double cst = distan(points[xs], points[v], inOut, mode);

			bool updatePreBranchPoint = false;
			bool updateNextBranchPoint = false;
			double tempPos1 = 0, tempPos2 = 0;

				// 特殊处理分支点
			if (points[xs].type == 1 || points[xs].type == 2)
			{
				int bid = points[xs].ref;
				if (inOut == 0)
				{
					BranchPoint *pb = branchPointSet.getBranchPointPointer(bid);
					cst = (pb->Dis) / 2;
				}
				else
				{
					BranchPoint *pb = branchPointSet.getBranchPointPointer(bid);
					cst = pb->computeWeight(points[v], mode, 0);
					updatePreBranchPoint = true;
					tempPos1 = pb->t;
				}
			}
			if (points[v].type == 1 || points[v].type == 2)
			{
				int bid = points[v].ref;
				BranchPoint *pb = branchPointSet.getBranchPointPointer(bid);
				if (mode == 1)
				{

					cst = (pb->Dis) / 2;
				}
				else
				{
					cst = pb->computeWeight(points[xs], inOut, 1);
					updateNextBranchPoint = true;
					tempPos2 = pb->t;
				}
			}
			cst += dis[xs * 2 + inOut].dis;

			if (dis[2 * v + mode] < Node(2 * v + mode, 0, cst))
			{
				if (updatePreBranchPoint)
				{
					this->preBranchPointPos[2 * v + mode] = tempPos1;
				}
				if (updateNextBranchPoint)
				{
					int bid = points[v].ref;
					BranchPoint *pb = branchPointSet.getBranchPointPointer(bid);
					pb->recordPos(0);
				}

				dis[2 * v + mode] = Node(2 * v + mode, 0, cst);
				assert(v != pnum - 1);
				pre[2 * v + mode] = xs * 2 + inOut;
				pq.push(dis[2 * v + mode]);
			}
		}
	}
	int top = 0;
	int tag = T * 2;
		while (pre[tag]) // pre[tag] 值为0导致无结果，
	{
		st[++top] = tag;
		tag = pre[tag];
		if (testflag)
		{
			cout << "nodeid  " << tag << "  ";
			if (tag < (pnum - 1) * 2)
				cout << basicChannel.clipType(tag / 2) << endl;
			else
				cout << -1 << endl;
		}
	}
	if (top)
	{
		st[++top] = S * 2;
	}
	vector<double> prePos;
	for (int i = top; i > 0; i--)
	{
		int id = st[i];
		if (top > 2)
		{
			if (id / 2 != S && id / 2 != T)
			{
				if (i == top - 1)
					nearClip[points[S]] = make_pair(id / 2, id % 2);
				if (i == 2)
					nearClip[points[T]] = make_pair(id / 2, id % 2);
			}
		}
		// if(i!=top&&i!=1)pclipSet->addwire(id,dia);
		path.points.push_back(points[id / 2]);
		if (i != top && i != 1 && id / 2 < pnum)
			path.inOut.push_back(id % 2);
		else
		{
			path.inOut.push_back(-1 - id % 2);
		}
		prePos.push_back(preBranchPointPos[id]);
	}
	for (int i = 0; i < path.points.size(); i++)
	{
		P &p = path.points[i];
		if (p.type == 1 || p.type == 2)
		{
			int mode = -1 - path.inOut[i];
			int bid = p.ref;
			BranchPoint *pb = branchPointSet.getBranchPointPointer(bid);
			pb->mode = mode;
			if (mode == 1)
			{
				pb->t = prePos[i + 1];
				pb->recordPos(1);
			}
		}
	}
}