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


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


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

//用于搜索路径的数据结构，每种类型的线束会分配到一个
struct Astar{
	ClipSet * pclipSet;	//指向卡箍集合
	BranchTree *pbranchTree;	//指向分支树
	P points[MAXPointNum];	//点的信息，包括卡箍点，分支点和端点
	P start,goal,start_dir,goal_dir; //起点终点以及方向
	int S,T; //起点和终点的标号
	map<int, vector<int> >edge;		
	//存储每个点附近不碰撞的其他点，避免重复调用kdtree和bvh
	map<P,pair<int,int> > nearClip;	//因为端点处不能分叉，所以标记端点连接到的最近卡箍
	
	int searchNum; //搜索次数，用于调试
	int pre[MAXPointNum],vis[MAXPointNum]; //pre存储一个点的上一个点，vis存储一个点是否被访问过
	int st[MAXPointNum],block[MAXPointNum];	//st是临时栈，block标记的点不能被通过
	int pnum;	//卡箍点总数
	int tnum;   //所有点总数
	Path path;	//路径结果
	double bend_r,l;	//弯曲半径，总长度
	Node dis[MAXPointNum];		//存储距离信息
	priority_queue<Node>pq;		//优先队列
	double preBranchPointPos[MAXPointNum]; //若前一个点是1类型（同进异出）分支点，存储其最优位置

	Astar(){
		pnum=0;l=bend_r=0;tnum=0;
		pclipSet=&clipSet;
	}
	
	void addBranchTree(BranchTree *pbranchTree){
		this->pbranchTree=pbranchTree;
	}

	//初始化
	inline void init(){
		pnum+=2;
	}
	
	//添加一个卡箍点
	inline void 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;
	}
	inline void add_clip(Clip& cp){
		pnum++;tnum++;
		points[pnum]=cp.coord;
		block[pnum]=0;
	}

	//添加一个分支点
	inline void add_branchPoint(BranchPoint& bp){
		tnum=max(tnum,clipSet.clipnum+2);
		tnum++;
		points[tnum]=bp.coord;
		block[tnum]=0;
	}
	
	/*
	inline double cost(P &p1,P &p2){
		if(bvh.iscollect(p1,p2))return 1e9;
		//return distan(p1,p2);
		return angelAndLength(p1,p2);
	}
	*/


	
	
	inline Path 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;
	}
	inline Path 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;
	}
	

	inline Path 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;
	}
	
	inline void 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);
		}
		
		
	}
	

	inline void 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);
				}

			}
		}	
		
		
	}

	

	
}astar;