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

BranchTree branchTree;
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]) {
		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);
			}

		}
	}


}