/*
	inline void addPath(Path &path,bool isclips,bool isclipt){
		if(path.points.empty())return;
		addPath(path);
		int s=getid(path.points[0]);
		int t=getid(path.points[path.points.size()-1]);
		if(isclips)endpoint[s]=0;
		if(isclipt)endpoint[t]=0;
	}

	
	
	void dfs(int x,bool root,double rdis,int fa){
		vis[x]=bfs;
		bundles[bundleid].path.push_back(points[x]);
		
		if(!endpoint[x]&&edge[x].size()==1){
			cout<<"warning"<<endl;
			points[x].print("warning");

		}
		
		
		if(edge[x].size()==1){
			if(!root){
				bundles[bundleid].end_node=points[x];
				bundleid++;
				bundles[bundleid].clear();
				return;
			}
		}
		int minv=-1;
		double maxreverse=-1;
		for(int i=0;i<edge[x].size();i++){
			int y=edge[x][i].to;
			if(vis[y]!=bfs){
				double reverse=0;
				P dir=dirP(points[x]);
				if(fa!=0)reverse+=pi/2-Angle(points[x]-points[fa],dir);
			
				reverse+=pi/2-Angle(points[y]-points[x],dir);
				if(reverse<=0)reverse*=-1;
				


				if(root||maxreverse<reverse){
					maxreverse=reverse;
					minv=y;
					bundles[bundleid].wirelist=edge[x][i].wirenames;
					bundles[bundleid].d=edge[x][i].d;
				}
				if(minv==-1){
					cout<<reverse<<endl;
					cout<<maxreverse<<endl;
					cout<<points[x].isend<<endl;
					cout<<points[x].x<<"  "<<points[x].y<<"  "<<points[x].z<<endl;
					cout<<Length(dir)<<"  "<<Length(points[y]-points[x]);
					if(fa!=0)cout<<"  "<<Length(points[x]-points[fa])<<endl;
					while(1);
				}
				
			}
		}
		int thisBranchPoint=-1;
		if(minv!=-1){
			if(edge[x].size()>2){
				branchPointNum++;
				thisBranchPoint=branchPointNum;
				
				int cid=pclipSet->clipid[points[x]];

				BranchPoint bp;
				bp.id=branchPointNum;bp.position=10;
				bp.referClip=(pclipSet->c[cid]).name;
				bundles[bundleid].branchPointSet.push_back(bp);
			}
			bundles[bundleid].length+=distan1(points[x],points[minv]);
			
			dfs(minv,false,bundles[bundleid].length,x);
		}

		for(int i=0;i<edge[x].size();i++){
			int y=edge[x][i].to;
			if(vis[y]!=bfs){
				if(y!=minv){
					//bundles[bundleid].path.push_back(points[x]);
					//bundles[bundleid].start_node=points[x];
					bundles[bundleid].length=0;
					bundles[bundleid].wirelist=edge[x][i].wirenames;
					bundles[bundleid].d=edge[x][i].d;
					bundles[bundleid].startBranchPoint=thisBranchPoint;
					bundles[bundleid].start_node=points[x];
					bundles[bundleid].length+=distan(points[x],points[y]);
					dfs(y,false,bundles[bundleid].length,x);
				}
				else continue;
				
				
			}
		}
	}
	

	bool dfsh(int x){
		int huan=0;
		vis[x]=bfs;
		for(int i=0;i<edge[x].size();i++){
			int y=edge[x][i].to;
			if(vis[y]!=bfs){
				if(dfsh(y))return true;
			}
			else huan++;
		}
		if(huan>=2)return true;
		return false;
	}
	bool testhuan(){
		bfs++;
		for(int i=1;i<=cnt;i++){
			if(vis[i]!=bfs&&dfsh(i))return true;
		}
		return false;
	}
	*/

	/*

	//根据已有信息自动生成bundle
	inline void buildBundle(){
		bfs++;
		bundleid=1;
		bundles[bundleid].clear();
		for(int i=1;i<=cnt;i++){
			//cout<<"bundle size "<<bundles[bundleid].path.size()<<endl;
			if(vis[i]!=bfs&&edge[i].size()==1)dfs(i,true,0,0);
		}
	}

	
	//仅仅根据卡箍的相对位置创建近似的通道
	inline void createChannel(){
		Clip *c = pclipSet->c;
		int clipNum=pclipSet->clipnum;
		for(int i=1;i<=clipNum;i++){
			int cnt=0;
			for(int j=0;j<c[i].channelEdge.size();j++){
				int cid=c[i].channelEdge[j];
				if(pclipSet->bothConnect(i,cid))cnt++;
			}
			if(cnt==1){
				Path path;
				path.dia=5;
				int lastCid=0;
				int cid=i;
				while(cid){
					bool flag=true;
					path.points.push_back(c[cid].coord);
					for(int j=0;j<c[cid].channelEdge.size();j++){
						int cid2=c[cid].channelEdge[j];
						if(cid2==lastCid)continue;
						if(pclipSet->bothConnect(cid,cid2)){
							lastCid=cid;
							cid=cid2;
							flag=false;
							break;
						}
					}
					if(flag)break;
				}
				addPath(path);
			}
		}
		buildBundle();
		
	}


	inline void readBundle(string sourcefile){
		ifstream ifs;
		ifs.open(sourcefile.c_str(), ios::in);
		int pnum=0;
		double d=0;
		int cnt=0;
		
		while(ifs>>pnum>>d){
			cnt++;
			
			for(int i=1;i<=pnum;i++){
				P tp;
				double quality;
				ifs>>tp.x>>tp.y>>tp.z>>tp.dx>>tp.dy>>tp.dz>>quality;
				bundles[cnt].path.push_back(tp);
				bundles[cnt].dia=d;

			}
			bundles[cnt].d.push_back(d);
			if(pnum==-1)break;
		}
		int cid,bnum;
		while(ifs>>cid>>bnum){
			int bid;
			cout<<cid<<"  "<<bnum<<endl;
			for(int i=1;i<=bnum;i++){
				ifs>>bid;
				cout<<bid<<"  ";
				bundlesOfClip[cid].push_back(bid);

			}
			cout<<endl;
			
		}
		bundleid=cnt;

	}

	//把bundle信息按格式要求打印出来
	inline void printBundle(string resultfile,bool printClipName=true){
		ofstream ofs;
		ofs.open(resultfile.c_str(), ios::out);
		ofs<<bundleid-1<<endl;
		for(int i=1;i<bundleid;i++)
			bundles[i].print();
			
	}


	inline void printBundleQuality(string resultFile){
		double averageDis=0;
		int routeNum=0;
		ofstream ofs,rfs;
		ofs.open(resultFile.c_str(), ios::out);
		string testFile="test"+resultFile;
		rfs.open(testFile.c_str(), ios::out);
		for(int i=1;i<bundleid;i++){
			Bundle & bd=bundles[i];
			bd.dia=0;
			//cout<<"--------------"<<bd.d.size()<<"----------------"<<endl;

			for(int j=0;j<bd.d.size();j++){
				bd.dia+=bd.d[j]*bd.d[j];
			}
			bd.dia=sqrt(bd.dia);
			for(int j=0;j<bd.wirelist.size();j++)cout<<bd.wirelist[j]<<"  ";
			cout<<endl;
			printPath(bd.path);
			//ofs<<bd.path.size()<<endl;
			for(int j=0;j<bd.path.size()-1;j++){
				P tp=bd.path[j];
				averageDis+=distan(bd.path[j],bd.path[j+1]);
				routeNum++;
				double reverse=0;
				P dir=dirP(bd.path[j]);
				if(j!=0){
					
					reverse+=pi/2-Angle(bd.path[j]-bd.path[j-1],dir);
				}
				if(j!=bd.path.size()-1){
					reverse+=pi/2-Angle(bd.path[j+1]-bd.path[j],dir);
				}
				if(reverse<0)bd.path[j].reverse();

			}
		
			cout<<endl;

		}
		rfs<<bundleid-1<<endl;
		averageDis/=routeNum;
		for(int i=1;i<bundleid;i++){
			Bundle bd=bundles[i];
			printPath(bd.path);
			//ofs<<bd.path.size()<<endl;
			ofs<<bd.path.size()<<"  "<<bd.dia<<endl;
			rfs<<bd.dia<<endl;
			rfs<<bd.path.size()<<endl;
			
			for(int j=0;j<bd.path.size();j++){
				P tp=bd.path[j];
				ofs<<tp.x<<"  "<<tp.y<<"  "<<tp.z<<"  ";
				ofs<<tp.dx<<"  "<<tp.dy<<"  "<<tp.dz<<"  ";

				rfs<<tp.x<<" "<<tp.y<<" "<<tp.z<<" ";
				rfs<<tp.dx<<" "<<tp.dy<<" "<<tp.dz<<endl;
				if(j!=bd.path.size()-1)ofs<<distan(tp,bd.path[j+1])/averageDis<<endl;
				else ofs<<0<<endl;

			}
			ofs<<endl;

		}
		ofs<<"-1  -1"<<endl;
		for(int i=1;i<=(pclipSet->clipnum);i++){
			if(!bundlesOfClip[i].empty()){
				ofs<<i<<"  "<<bundlesOfClip[i].size()<<endl;
				for(int j=0;j<bundlesOfClip[i].size();j++){
					ofs<<bundlesOfClip[i][j]<<"  ";
				}
				ofs<<endl;
			}
		}
		
	}
	*/

//branchTree



/*
inline void search_pair(Astar& astar,BranchTree &branchtree,P &start,P &goal,double dia,vector<P> vecp){
		astar.start=start;
		astar.goal=goal;
		astar.pnum-=2;
		astar.points[++astar.pnum]=start;
		astar.points[++astar.pnum]=goal;
	
		astar.path.points.clear();
		astar.path.dia=dia;
		astar.path.wirelist.clear();
		if(vecp.size()==0){
			astar.search();
			branchtree.addPath(astar.path);
			return;
		}
		branchtree.startRecord();
		Path tempPath=astar.search(start,vecp[0],0,1);
		tempPath.dia=dia;
		branchtree.addPath(tempPath,0,1);

		for(int i=1;i<vecp.size();i++){
			tempPath=astar.search(vecp[i-1],vecp[i],1,1);
			tempPath.dia=dia;
			branchtree.addPath(tempPath,1,1);
			
		}
		tempPath=astar.search(vecp[vecp.size()-1],goal,1,0);
		tempPath.dia=dia;
		branchtree.addPath(tempPath,1,0);
		branchtree.endRecord();

		return;
}

*/

/*
	inline void search_pair(P &start,P &goal,double dia,vector<P> vecp,int bid){
		this->start=start;
		this->goal=goal;
		pnum-=2;
		points[++pnum]=start;
		points[++pnum]=goal;
	
		path.points.clear();
		path.dia=dia;
		path.wirelist.clear();
		if(vecp.size()==0){
			search();
			pbranchTree->bundles[bid].path=path.points;
			return;
		}
		Path tempPath=search(start,vecp[0],0,1);
		vector<P> tPath=tempPath.points;
		vector<P> rPath;
		for(int j=0;j<tPath.size()-1;j++){
			int cid=pclipSet->clipid[tPath[j]];
			block[cid]=1;
			rPath.push_back(tPath[j]);
		}

		for(int i=1;i<vecp.size();i++){
			tempPath=search(vecp[i-1],vecp[i],1,1);
			tPath=tempPath.points;
			for(int j=0;j<tPath.size()-1;j++){
				int cid=pclipSet->clipid[tPath[j]];
				block[cid]=1;
				rPath.push_back(tPath[j]);
			}
			
		}
		tempPath=search(vecp[vecp.size()-1],goal,1,0);
		tPath=tempPath.points;
		for(int j=0;j<tPath.size();j++){
			int cid=pclipSet->clipid[tPath[j]];
			block[cid]=1;
			rPath.push_back(tPath[j]);
		}
		pbranchTree->bundles[bid].path=rPath;

		return;
	}

	inline Path search(P &start,P &goal,bool isclips,bool isclipt){
		while(!pq.empty())pq.pop();
		path.points.clear();
		int s=pnum-1,t=pnum;
		if(isclips){
			s=kdtree.search_by_k(start,1)[0];
		}
		if(isclipt){
			t=kdtree.search_by_k(goal,1)[0];
		}
		
	
		for(int i=1;i<=pnum;i++)vis[i]=0,dis[i]=Node(i,1000,1e9),pre[i]=0;
		


		dis[s]=Node(s,0,0);

		pq.push(dis[s]);
		vector<int> vecv=kdtree.search_by_k(points[t],20);
		vector<int> vec3;
		for(int i=0;i<vecv.size();i++){
			int xs=vecv[i];
			if(!bvh.iscollect(points[xs],points[t]))vec3.push_back(xs);
		}
		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;
			//cout<<xs<<endl;
			if(xs==t)break;
			//cout<<"stage1"<<endl;
			
			vector<int> vec;
			if(xs!=pnum-1&&!edge[xs].empty())vec=edge[xs];
			else{

				vector<int> vec1=kdtree.search_by_dis(points[xs],600);
				vector<int>	vec2=kdtree.search_by_k(points[xs],20);
				//if(cost(points[vec2[0]],points[xs])>=abdis)vec2.push_back(v);
				vec1.insert(vec1.begin(),vec2.begin(),vec2.end());
				for(int i=0;i<vec1.size();i++){
					if(!bvh.iscollect(points[xs],points[vec1[i]]))vec.push_back(vec1[i]);
				}
				if(xs!=pnum-1)edge[xs]=vec;
				
			}
			
			for(int i=0;i<vec3.size();i++){
				if(vec3[i]==xs)vec.push_back(t);
			}
			double abdis=cost(points[xs],points[t]);
			if(abdis<=600)vec.push_back(t);
			//cout<<"stage2"<<endl;
			for(int i=0;i<vec.size();i++){
				int v=vec[i];
				if(v!=t)
				if(!(pclipSet->transitable(v,2,path.dia)))continue;
				
				//if(pbranchTree->check(points[v])==false)continue;

				if(block[v])continue;
				
				double cst=distan(points[xs],points[v])+dis[xs].dis;
				int turnc=dis[xs].turncnt;
				if(BadTurn(points[xs],points[v]))turnc++;

				if(dis[v]<Node(v,turnc,cst)){
					dis[v]=Node(v,turnc,cst);
					pre[v]=xs;
					pq.push(dis[v]);
				}
			}
			//cout<<"stage3"<<endl;

			

			//cout<<"stage4"<<endl;
		}
		//cout<<"stage5"<<endl;
		//l=dis[pnum];
		int top=0;
		int tag=t;
		while(pre[tag]){
			//cout<<tag<<"  "<<pre[tag]<<endl;
			st[++top]=tag;
			tag=pre[tag];
		}
		if(top){
			st[++top]=s;
		}
		//cout<<"stage6"<<endl;
		for(int i=top;i>0;i--){
			int id=st[i];
			//if(i!=top&&i!=1)pclipSet->addwire(id,dia);
			path.points.push_back(points[id]);
		}
		return path;
		
	}


	inline void changeRoute(int bid,vector<int> cid){
		int bundleid=pbranchTree->bundleid;
		Bundle * bundles =pbranchTree->bundles;

		for(int i=1;i<=bundleid;i++){
			Bundle bd=bundles[i];
			for(int j=0;j<bd.path.size();j++){
				P tp=bd.path[j];
				int pid=pclipSet->clipid[tp];
				block[pid]=1;
			}
		}
		Bundle bd=bundles[bid];
		for(int j=0;j<bd.path.size();j++){
			P tp=bd.path[j];
			int pid=pclipSet->clipid[tp];
			block[pid]=0;
		}
		P start=bd.path[0],goal=bd.path[bd.path.size()-1];
		vector<P> vecP,sortVecP;
		for(int i=0;i<cid.size();i++)vecP.push_back(points[cid[i]]);
		pair<double,int> dis_id[100];
		P sp[100];
		for(int i=0;i<vecP.size();i++){
			dis_id[i].first=distan1(vecP[i],start)-distan1(vecP[i],goal);
			dis_id[i].second=i;
		}
		sort(dis_id,dis_id+vecP.size());
		for(int i=0;i<vecP.size();i++)sp[i]=vecP[dis_id[i].second];
		for(int i=0;i<vecP.size();i++)sortVecP.push_back(sp[i]);

		search_pair(start,goal,bd.dia,sortVecP,bid);
		
	}

	inline void changeBranchEndPoint(int cid,int newCid){

		Bundle * bundles =pbranchTree->bundles;
		int bundleid=pbranchTree->bundleid;
		vector<int> * bundlesOfClip =pbranchTree-> bundlesOfClip;


		for(int i=1;i<=bundleid;i++){
			bool flag=false;
			for(int j=0;j<bundlesOfClip[cid].size();j++){
				if(bundlesOfClip[cid][j]==i){
					flag=true;
					break;
				}
			}
			if(flag)continue;
			Bundle bd=bundles[i];
			for(int j=0;j<bd.path.size();j++){
				P tp=bd.path[j];
				int pid=pclipSet->clipid[tp];
				if(pid==newCid){
					cout<<"Error:Clip illegal"<<endl;
					return;
				}
				block[pid]=1;
			}
		}
		vector<P> vecP;
		P newClipCoord=(pclipSet->c[newCid]).coord;
		P clipCoord=(pclipSet->c[cid]).coord;
		for(int i=0;i<bundlesOfClip[cid].size();i++){
			int bid=bundlesOfClip[cid][i];
			Bundle &bd= pbranchTree->bundles[bid];
			P start=bd.path[0],goal=bd.path[bd.path.size()-1];
			if(distan1(clipCoord,start)<distan1(clipCoord,goal)){
				start=newClipCoord;
			}
			else goal=newClipCoord;
			search_pair(start,goal,bd.dia,vecP,bid);
			for(int j=0;j<bd.path.size();j++){
				P tp=bd.path[j];
				int pid=pclipSet->clipid[tp];
				block[pid]=1;
			}
		}
		bundlesOfClip[newCid]=bundlesOfClip[cid];
		bundlesOfClip[cid].clear();
		

		
		
	}




	inline void search(){
		while(!pq.empty())pq.pop();
		points[++pnum]=start;
		points[++pnum]=goal;
	
		for(int i=1;i<=2*tnum+2;i++)vis[i]=0,dis[i]=Node(i,1000,1e9),pre[i]=0;


		dis[(pnum-1)*2]=Node((pnum-1)*2,0,0);

		assert(pre[2*(pnum-1)]==0);

		pq.push(dis[(pnum-1)*2]);
		vector<int> vecv=kdtree.search_by_k(points[pnum],20);
		vector<int> vec3;
		for(int i=0;i<vecv.size();i++){
			int xs=vecv[i];
			if(!bvh.iscollect(points[xs],points[pnum]))vec3.push_back(xs);
		}
		bool testflag=false;
		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;
			
			if(xs==pnum)break;
			
			
			if(dcmp(points[xs].x-10920.50693)==0){
				cout<<"------------------------------"<<endl;
				cout<<pnum<<"  "<<xs*2+inOut<<endl;
				cout<<"------------------------------"<<endl;
				testflag=true;

			}
		


			if(pre[2*(pnum-1)])cout<<"f1.9  "<<pre[2*(pnum-1)]<<endl;
			

			
			vector<int> vec;
			if(pre[2*(pnum-1)])cout<<"f1.10  "<<pre[2*(pnum-1)]<<endl;
			if(xs!=pnum-1&&!edge[xs].empty())vec=edge[xs];
			else if(xs==pnum-1&&nearClip[points[xs]]!=0)vec.push_back(nearClip[points[xs]]);
			else{

				vector<int> vec1=kdtree.search_by_dis(points[xs],600);
				vector<int>	vec2=kdtree.search_by_k(points[xs],20);
				vec1.insert(vec1.begin(),vec2.begin(),vec2.end());
				for(int i=0;i<vec1.size();i++){
					int v=vec1[i];
					if(v==xs)continue;

					
					if(!bvh.iscollect(points[xs],points[v])){
			
						int pov=ParallelorVertical(dir[xs],dir[v]);
						if(pov==1){
							pov=ParallelorVertical(points[xs]-points[v],dir[v]);
							if(pov==2)continue;
						}
					

						vec.push_back(v);
					}
					

				}
				if(xs!=pnum-1)edge[xs]=vec;
				
			}

			if(nearClip[points[pnum]]==0){
				for(int i=0;i<vec3.size();i++){
					if(vec3[i]==xs)vec.push_back(pnum);
				}
				double abdis=cost(points[xs],points[pnum]);
				if(abdis<=600)vec.push_back(pnum);
			}
			else if(nearClip[points[pnum]]==xs)vec.push_back(pnum);

			for(int i=0;i<vec.size();i++){
				int v=vec[i];
				if(v!=pnum)
				{
					if(!(pclipSet->transitable(v,2,path.dia)))continue;
					if(block[v])continue;

				}
				if(v==xs)cout<<v<<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,0)+dis[xs*2+inOut].dis;
				double cst1=distan(points[xs],points[v],inOut,1)+dis[xs*2+inOut].dis;
				//int turnc=dis[xs].turncnt;
				//if(BadTurn(points[xs],points[v]))turnc++;

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

//Astar

if(mode=="findRoute"){
		//仅仅给出卡箍文件和起点终点坐标，简单寻路
		string clipFileName,endpointFileName,outputFileName;
		cin>>clipFileName>>endpointFileName>>outputFileName;
		read_points(clipFileName,pastar);
		kdtree.build();

		astar.init();
		bvh.read_stl("PFM_L_simple.stl");

		clipSet.computeCenter();
		ifstream iendpointFile;
		ofstream oFile;
		iendpointFile.open(endpointFileName.c_str(),ios::in);
		oFile.open(outputFileName.c_str(),ios::out);
		int pairNum;
		iendpointFile>>pairNum;
		for(int i=1;i<=pairNum;i++){
			P p1,p2;
			iendpointFile>>p1.x>>p1.y>>p1.z>>p1.dx>>p1.dy>>p1.dz;
			iendpointFile>>p2.x>>p2.y>>p2.z>>p2.dx>>p2.dy>>p2.dz;
			Path path=astar.search_pair(p1,p2,0);
			oFile<<path.points.size()<<endl;
			for(int j=0;j<path.points.size();j++){
				P pp=path.points[j];
				oFile<<pp.x<<" "<<pp.y<<" "<<pp.z<<endl;
				cout<<setprecision(10)<<"("<<pp.x<<","<<pp.y<<","<<pp.z<<")";
				if(j!=path.points.size()-1)cout<<"->";
			}
			cout<<endl;

		}
		cout<<"Finsh"<<endl;
		return 0;
	}
else if(mode=="test"){
		//，不需要任何输入，使用测试数据进行一次测试
		read_points("data_02.xml",pastar);
		read_points("data_03.xml",pastar);
		read_points("data_15.xml",pastar);

		cout<<"read points finish"<<endl;
		astar.init();
		bvh.read_stl("PFM_L_simple.stl");

		//clipset.print();
		clipSet.computeCenter();
		//clipset.printDis();
		
		//changeRoute();
		//changeBranchEndPoint();
		//return 0;
		time_t r1,r2;
		
		time(&r1);
		//testPoint();
		//char * xml="W024019B.xml";
		//produceXML(xml);

		
		string  docName="wirexml";
		vector<string> filenames;
		getAllFiles(docName,filenames);
		
		for(int i=0;i<filenames.size();i++){
			string wirebh;
			stringstream ss;
			ss<<i;
			wirebh=ss.str();
			string filename=filenames[i];
			cout<<filename<<endl;
			cout<<endl;
			string resultname="branchg"+wirebh+".txt";
			produceXML(filename.c_str(),"connector.csv",resultname);
		}
		

		
		
		time(&r2);
		cout<<"Astar time "<<(r2-r1)<<endl;
		cout<<"ALL FINISH"<<endl;
	}
/*
更改一条路径
void testPoint(){
	astar.addBranchTree(&branchtree);
	ifstream ifs;
	ifs.open("point.txt",ios::in);
	P start,goal;
	ifs>>start.x>>start.y>>start.z;
	ifs>>goal.x>>goal.y>>goal.z;
	string st[3];
	vector<P> vecp;
	start.print("start");
	goal.print("goal");
	while(ifs>>st[0]){
		for(int i=1;i<=2;i++)ifs>>st[i];
		P p;
		for(int i=0;i<3;i++)p.set(i,atof(st[i].c_str()));
		vecp.push_back(p);
		p.print("vecp");
	}
	search_pair(astar,branchtree,start,goal,10,vecp);

	branchtree.buildBundle();
	branchtree.printBundle("onebranch.txt");
	
	
}


void changeRoute(){
	astar.addBranchTree(&branchtree);
	int bid;
	int cnum;
	vector<int> clipid;
	branchtree.readBundle("onebranch3.txt");
	
	cin>>bid>>cnum;
	for(int i=1;i<=cnum;i++){
		int cid;
		cin>>cid;
		clipid.push_back(cid);
	}
	astar.changeRoute(bid,clipid);
	

	branchtree.printBundleQuality("onebranch5.txt");
	cout<<"Finish"<<endl;


}
void changeBranchEndPoint(){
	astar.addBranchTree(&branchtree);
	int cid,newCid;
	branchtree.readBundle("branchc1.txt");
	
	cin>>cid>>newCid;
	
	astar.changeBranchEndPoint(cid,newCid);
	

	branchtree.printBundleQuality("2branch.txt");
	cout<<"Finish"<<endl;


}
*/

	void produce_pair(){
	ofstream ofs;
	ofs.open("result.txt", ios::out);
	ifstream ifs;
	ifs.open("pair.txt",ios::in);
	string st[6];
	int wireid=0;
	while(ifs>>st[0]){
		for(int i=1;i<=5;i++)ifs>>st[i];
		P p1,p2;
		for(int i=0;i<3;i++)p1.set(i,atof(st[i].c_str()));
		for(int i=0;i<3;i++)p2.set(i,atof(st[i+3].c_str()));
		pmp[p1]=st[0]+" "+st[1]+" "+st[2];
		pmp[p2]=st[3]+" "+st[4]+" "+st[5];

		cout<<bvh.iscollect(p1,p2)<<endl;
		
		vector<P>vecp=astar.search_pair(p1,p2,1).points;
		for(int j=0;j<vecp.size();j++){
			P pp=vecp[j];
			ofs<<pmp[pp]<<endl;
			cout<<setprecision(10)<<"("<<pp.x<<","<<pp.y<<","<<pp.z<<")";
			if(j!=vecp.size()-1)cout<<"->";
		}
		cout<<endl;
		ofs<<"#"<<endl;
		ofs<<endl;
		
		string name,wirebh;
		stringstream ss;
		ss<<wireid;
		wirebh=ss.str();
		name="wire"+wirebh;
		Path path;
		path.points=vecp;
		path.wirelist.push_back(name);
		branchTree.addPath(path);
		
		wireid++;
	}
	basicChannel.printBundle("branch.txt");
}

	//将Bundle信息输出到文件流ofs
	void print(ofstream& ofs){

		if(head==NULL)return;
		Clip *pc=head->pc;
		double dia=pc->getDia();
		if(dcmp(dia)==0)dia=5;
		int nodeNum=0;
		BundleNode *pbn=head;
		while(pbn!=NULL){
			nodeNum++;
			pbn=pbn->next;
		}
		
		ofs<<nodeNum<<"  "<<dia<<"  "
			<<startBranchPoint()<<"  "<<endBranchPoint()<<endl;

		pbn=head;
		vector<BranchPoint* > vb;
		while(pbn!=NULL){
			if(pbn->type==0){
				P dir=pbn->pc->dir;
				string name=pbn->pc->name;
				if(name=="")name="--";
				ofs<<name<<"  "<<!(pbn->inOut)<<endl;
				
				P tp=pbn->pc->coord;
				ofs<<tp.x<<"  "<<tp.y<<"  "<<tp.z<<"  ";
				ofs<<tp.dx<<"  "<<tp.dy<<"  "<<tp.dz<<endl;
			}
			vb.insert(vb.end(),(pbn->vb).begin(),(pbn->vb).end());
			pbn=pbn->next;

		}
		ofs<<vb.size()<<endl;
		for(int j=0;j<vb.size();j++){
			ofs<<vb[j]->referClip<<"  "
				<<vb[j]->position<<endl;
		}
		ofs<<endl;

	}

	inline double distan(P &A,P &B){
	double angel=Angle(A-B,DX);
	if(angel>pi/2)angel=pi-angel;
	angel=min(angel,pi/2-angel);
	double len=distan1(A,B);

	double len1=sqrt((A.y-Ycenter)*(A.y-Ycenter)+(A.z-Zcenter)*(A.z-Zcenter));
	double len2=sqrt((B.y-Ycenter)*(B.y-Ycenter)+(B.z-Zcenter)*(B.z-Zcenter));
	if(len1<R||len2<R){
		double angel1=Angle(A-B,DX);
		if(angel1>pi/2)angel1=pi-angel1;

		double angel2=Angle(A-B,DY);
		if(angel2>pi/2)angel2=pi-angel2;

		double angel3=Angle(A-B,DZ);
		if(angel3>pi/2)angel3=pi-angel3;
		
		angel=min(angel1,min(angel2,angel3));
	}
	

	P C;
	C.x=B.dx;C.y=B.dy;C.z=B.dz;
	double angel2=Angle(A-B,C);
	if(angel2>pi/2)angel2=pi-angel2;
	if(B.isend==1||B.type)angel2=0;

	C.x=A.dx;C.y=A.dy;C.z=A.dz;
	double angel3=Angle(A-B,C);
	if(angel3>pi/2)angel3=pi-angel3;
	if(A.isend==1||A.type)angel3=0;

	

	//else if(angel>=pi/2-minAngel&&angel<=pi/2+minAngel)return false;
	/*
	P C(A.dx,A.dy,A.dz);
	double len=Dot(C,A-B)/Length(C);
	if(len>=-minDis&&len<=minDis)return false;
	*/

	//return pow(len,angel*3+1);
	return len*(angel*4+1)+300*600*(angel2+angel3)/len;

}

	inline bool BadTurn(P &A,P & B){
	return false;
	double angel=Angle(A-B,DX);
	if(angel<=minAngle||angel>=pi-minAngle)return false;
	angel=Angle(A-B,DY);
	if(angel<=minAngle||angel>=pi-minAngle)return false;
	angel=Angle(A-B,DZ);
	if(angel<=minAngle||angel>=pi-minAngle)return false;
	//else if(angel>=pi/2-minAngel&&angel<=pi/2+minAngel)return false;
	/*
	P C(A.dx,A.dy,A.dz);
	double len=Dot(C,A-B)/Length(C);
	if(len>=-minDis&&len<=minDis)return false;
	*/

	return true;

	
}