server/card_path.cpp

#include <stdio.h>
#include <algorithm>
#include <unordered_map>
#include <set>
#include <memory>
#include <atomic>
#include <thread>
#include "zlist.h"
#include "line.h"
#include "ant.h"

#include "card_path.h"
#include "visit.h"

namespace{

static const double PI=3.1416;
inline bool eq(double a,double b,double e=0.0001)
{
	return fabs(a-b)<e;
}

struct vertex:point
{
	float level;
	int sid0,sid1;
	vertex()
		:level(0)
		,sid0(0)
		,sid1(0)
	{}

	vertex(const point&pt,float level_=0,int site_id0=0,int site_id1=0)
		:point(pt)
		,level(level_)
		,sid0(site_id0)
		,sid1(site_id1)
	{
	}

	std::string to_string()const
	{
		char buf[128];
		sprintf(buf,"%.2f,%.3lf,%d,%d,%.2lf",x,y,sid0,sid1,level/PI*180);

		return buf;
	}
};

struct vertex_list;
struct base_path:std::array<int,2>
{
	int sid;
	base_path(int i1=-1,int i2=-1)
	{
		at(0)=i1;
		at(1)=i2;
	}

	bool operator<(const base_path&o)const
	{
		int c=at(0)-o.at(0);
		if(c!=0)
			return c<0;

		return at(1)-o.at(1)<0;
	}

	point  cross(const vertex_list&v_list, const base_path&o)const;
	bool   contains(const vertex_list&v_list, const point&o, double e)const;
	double arg(const vertex_list&v)const;
	int    type(const vertex_list&v)const;
	line_v as_line(const vertex_list&v)const;
	int    level(const vertex_list&v)const;
};

struct vertex_list
{
	std::vector<vertex> v;

	std::string to_string()const
	{
		std::string ret;
		ret.reserve(1024);

		char buf[128];
		for(int i=0,len=v.size();i<len;i++)
		{
			sprintf(buf,"vertex:%3d,%s\n",i,v[i].to_string().c_str());

			ret+=buf;
		}

		return std::move(ret);
	}

	void log()
	{
		printf("%s",to_string().c_str());
	}


	static unsigned hash(const point&p)
	{
		return (((((unsigned)p.x)>>2)<<16) + (((unsigned)p.y)>>2));
	}

	int find(const point&p,double x=1)const
	{
		int rc=-1;
		double t;

		for(int i=0,len=v.size();i<len;i++)
		{
			t=p.dist(v[i]);
			if(t<x)
			{
				rc=i;
				x=t;
			}
		}

		return rc;
	}

	int add(const point&p,float level=0,int sid0=0,int sid1=0)
	{
		int i=find(p);

		if(i<0)
		{
			v.push_back(vertex(p,level,sid0,sid1));
			return v.size()-1;
		}

		vertex&v0=v[i];

		v0.level=std::max(v0.level,level);

		return i;
	}

	int size()const
	{
		return v.size();
	}

	const vertex&operator[](int i)const
	{
		return v[i];
	}

	vertex&operator[](int i)
	{
		return v[i];
	}
};

int base_path::level(const vertex_list&v)const
{
	const vertex&p0=v[at(0)];
	const vertex&p1=v[at(1)];

	return p0.level+p1.level;
}

line_v base_path::as_line(const vertex_list&v)const
{
	const vertex&p0=v[at(0)];
	const vertex&p1=v[at(1)];

	return line_v(p0,p1);
}

double  base_path::arg(const vertex_list&v)const
{
	return as_line(v).arg();
}

bool  base_path::contains(const vertex_list&v, const point&o, double e)const
{
	return as_line(v).contain(o,e);
}

point  base_path::cross(const vertex_list&v, const base_path&o)const
{
	line_v l0=as_line(v);
	line_v l1=o.as_line(v);

	return l0.crossing(l1);
}

void log_path(const std::vector<base_path>&path,const vertex_list&v_list)
{
	printf("%s\n","----------------------------------------------------------------");
	for(int i=0,len=path.size();i<len;i++)
	{
		double c=path[i].arg(v_list)/PI*180;
		printf("base_path %.6lf, %03d,(%d,%s),(%d,%s)\n",c,i,path[i][0],v_list[path[i][0]].to_string().c_str(),path[i][1],v_list[path[i][1]].to_string().c_str());
	}
}
struct handle_path :visitor<std::shared_ptr<site>>
{
	std::vector<base_path> ret;
	vertex_list v;
	handle_path()
	{
		v.add(point(0,0),0,-1);
	}
	bool visit(std::shared_ptr<site> sit)
	{
		//auto&s=sites[i];
		const auto &s = *sit;

		if(!s.have_valid_path())
			return false;

		if(s.path(0).empty()||s.path(1).empty())
			return false;
        if(s[0].size()<2)
			return false;
        line_v l000 = s[0][0][0];
        line_v l010 = s[0][1][0];
        if(l000.is_same(l010))
        {
            printf("same....%d",s.m_id);
			int p0=v.add(point::min(s.path(0),s.path(1)),0,s.m_id);
			int p1=v.add(point::max(s.path(0),s.path(1)),0,s.m_id);

		    ret.push_back(base_path(p0,p1));
			ret.back().sid=s.m_id;
        }
        else
        {
            point px = l000.line::crossing(l010);
            for(int i=0;i<2;i++)
            {
			    int p0=v.add(point::min(px,s.path(i)),0,s.m_id);
			    int p1=v.add(point::max(px,s.path(i)),0,s.m_id);

		        ret.push_back(base_path(p0,p1));
			    ret.back().sid=s.m_id;
            }
        }
        for(int i=0;i<2;i++)
        {
            if(!s[0][i][1].empty())
            {
			    int p0=v.add(point::min(s[0][i][1][0],s[0][i][1][1]),0,s.m_id);
			    int p1=v.add(point::max(s[0][i][1][0],s[0][i][1][1]),0,s.m_id);

		        ret.push_back(base_path(p0,p1));
			    ret.back().sid=s.m_id;
            }
        }
		return true;
	}
};
static std::vector<base_path> init_path(std::vector<base_path> & ret,vertex_list&v)
{
//	for(uint32_t i=0;i<sites.m_list.size();i++)
//	{
//		auto&s=sites[i];
//
//		if(!s.have_valid_path())
//			continue;
//
//		if(s.path(0).empty()||s.path(1).empty())
//			continue;
//        if(s[0].size()<2)
//            continue;
//        line_v l000 = s[0][0][0];
//        line_v l010 = s[0][1][0];
//        if(l000.is_same(l010))
//        {
//            printf("same....%d",s.m_id);
//			int p0=v.add(point::min(s.path(0),s.path(1)),0,s.m_id);
//			int p1=v.add(point::max(s.path(0),s.path(1)),0,s.m_id);
//
//		    ret.push_back(base_path(p0,p1));
//			ret.back().sid=s.m_id;
//        }
//        else
//        {
//            point px = l000.line::crossing(l010);
//            for(int i=0;i<2;i++)
//            {
//			    int p0=v.add(point::min(px,s.path(i)),0,s.m_id);
//			    int p1=v.add(point::max(px,s.path(i)),0,s.m_id);
//
//		        ret.push_back(base_path(p0,p1));
//			    ret.back().sid=s.m_id;
//            }
//        }
//        for(int i=0;i<2;i++)
//        {
//            if(!s[0][i][1].empty())
//            {
//			    int p0=v.add(point::min(s[0][i][1][0],s[0][i][1][1]),0,s.m_id);
//			    int p1=v.add(point::max(s[0][i][1][0],s[0][i][1][1]),0,s.m_id);
//
//		        ret.push_back(base_path(p0,p1));
//			    ret.back().sid=s.m_id;
//
//            }
//        }
//    }

/*

		if(x.contain(s,2.5))
		{
			int p0=v.add(point::min(s.path(0),s.path(1)),0,s.m_id);
			int p1=v.add(point::max(s.path(0),s.path(1)),0,s.m_id);

		    ret.push_back(base_path(p0,p1));
			ret.back().sid=s.m_id;
			continue;
		}

		for(int j=0;j<2;j++)
		{
			if(!s.path(j).empty() && s.dist(s.path(j))<2)
				continue;

			point p=s.path(j);
			int p0=v.add(point::min(s,p),0,s.m_id);
			int p1=v.add(point::max(s,p),0,s.m_id);
			ret.push_back(base_path(p0,p1));
			ret.back().sid=s.m_id;
		}
	}
*/
	log_path(ret,v);
    printf("++++++++++++++++++++++++++++++++++++++++++++");
	std::sort(ret.begin(),ret.end());
	log_path(ret,v);
    printf("++++++++++++++++++++++++++++++++++++++++++++");
	ret.erase(std::unique(ret.begin(),ret.end()),ret.end());
	log_path(ret,v);
    printf("+++++++++++++++++nnnnn+++++++++++++++++++++++++++");

	std::sort(ret.begin(),ret.end(),[&v](const base_path&p1,const base_path&p2){
		double arg=p1.arg(v)-p2.arg(v);

		if(fabs(arg)<0.1)
		{
			return v[p1[0]]<v[p2[0]];
		}

		return arg<0;
	});

	log_path(ret,v);

	for(int i=0,len=ret.size();i<len;i++)
	{
		line_v li=ret[i].as_line(v);
		for(int j=i+1;j<len;j++)
		{
			line_v lj=ret[j].as_line(v);
			if(!lj.is_same(li,2.5))
				continue;

			line_v ij=lj.projection(li);

			if(ij.empty())
				continue;

			point p0=point::min(v[ret[j][0]],v[ret[i][0]]);
			point p1=point::max(v[ret[j][1]],v[ret[i][1]]);

			ret[j][0]=v.add(p0,0);
			ret[j][1]=v.add(p1,0);

			ret[i][0]=0;
			ret[i][1]=0;
			break;
		}
	}

	ret.erase(std::remove_if(ret.begin(),ret.end(),[&v](const base_path&p){
		return p[0]==0||p[1]==0;
	}),ret.end());
#ifdef __DEBUG__
	std::sort(ret.begin(),ret.end(),[&v](const base_path&p1,const base_path&p2){
		double arg=p1.arg(v)-p2.arg(v);

		if(fabs(arg)<0.1)
		{
			return v[p1[0]]<v[p2[0]];
		}

		return arg<0;
	});

	log_path(ret,v);
#endif

	std::vector<std::vector<int>> p0(ret.size());
	std::vector<base_path> ret2;
	for(int i=0,len=ret.size();i<len;i++)
	{
		p0[i].push_back(ret[i][0]);
		for(int j=i+1;j<len;j++)
		{
			if(i==j) continue;

			point cr=ret[i].cross(v,ret[j]);
			if(cr.empty())
				continue;

			double arg=fabs(ret[i].as_line(v).arg()-ret[j].as_line(v).arg());
			while(arg>PI/2) 
				arg-=PI/2;

			if(arg/PI*180<5)//相交小于5度，不切分已有路径
				continue;

			int id=v.add(cr,arg,v[ret[i][0]].sid0,v[ret[j][0]].sid0);
			p0[i].push_back(id);
			p0[j].push_back(id);
		}

		p0[i].push_back(ret[i][1]);

		std::sort(p0[i].begin(),p0[i].end(),[&v](int i0,int i1){
			return v[i0]<v[i1];
		});

		auto it=std::unique(p0[i].begin(),p0[i].end());
		p0[i].erase(it,p0[i].end());

		for(int j=1,cnt=p0[i].size();j<cnt;j++)
			ret2.push_back(base_path(p0[i][j-1],p0[i][j]));
	}

	ret2.erase(std::remove_if(ret2.begin(),ret2.end(),[&v](base_path&p){
		point&p0=v[p[0]];
		point&p1=v[p[1]];

		//return p0.dist(p1)<0.1 || p0.empty() || p1.empty();
		return p0.dist(p1)<0.1;
	}),ret2.end());

	std::sort(ret2.begin(),ret2.end());
	ret2.erase(std::unique(ret2.begin(),ret2.end()),ret2.end());

/*
	ret.clear();
	for(int i=0,len=ret2.size();i<len;i++)
	{
		std::vector<line_v> tmp;
		tmp.push_back(ret2[i].as_line(v));
		for(int j=0;j<len;j++)
		{
			if(i==j) continue;

			if(ret[j].level(v)<ret[i].level(v))
				continue;

			line l1=ret2[j].as_line(v);
		}
	}

	std::sort(ret2.begin(),ret2.end(),[&v](base_path&p1,base_path&p2){
		return p1.arg(v)<p2.arg(v);
	});
	*/

#ifdef __DEBUG__
	std::sort(ret2.begin(),ret2.end(),[&v](const base_path&p1,const base_path&p2){
		double arg=p1.arg(v)-p2.arg(v);

		if(fabs(arg)<0.1)
		{
			return v[p1[0]]<v[p2[0]];
		}

		return arg<0;
	});

	log_path(ret2,v);
#endif

	return std::move(ret2);
}



#if 0
struct ghash
{
	static std::tuple<int,int> decode(unsigned h)
	{
		const uint64_t S=0x8000000000000000;

		int x=0,y=0;
		for(int i=0;i<64;i++)
		{
			x<<=1;
			y<<=1;

			if(h&S) 
				x|=1;
			h<<=1;

			if(h&S) 
				y|=1;
			h<<=1;
		}

		return std::make_tuple(x-2147483648,y-2147483648);
	}

	static uint64_t encode(int64_t x, int64_t y)
	{
		return encode_(x+2147483648,y+2147483648);
	}

public: //test
	static void test_code(int64_t x,int64_t y)
	{
		uint64_t h=ghash::encode(x,y);
		auto t=ghash::decode(h);

		printf("src x=%X,y=%X hash=%X,check x=%X,y=%X\n",x,y,h,std::get<0>(t),std::get<1>(t));
	}

	static void test()
	{
		for(int i=0;i<10;i++)
		{
			test_code((4<<i)-1,(4<<i)-1);
			test_code((4<<i)-1,(4<<i));
			test_code((4<<i)-1,(4<<i)-1);
			test_code((4<<i),(4<<i)-1);
		}
	}
private:
	static unsigned encode_(unsigned short x, unsigned short y)
	{
		const unsigned S=0x8000;
		unsigned r=0;
		for(int i=0;i<16;i++)
		{
			r<<=2;
			if(x&S)
			{
				r|=(y&S)?3:2;
			}
			else
			{
				if(y&S) r|=1;
			}

			x<<=1;
			y<<=1;
		}
		
		return r;
	}
};
#endif


struct geolist
{
	std::unordered_map<uint64_t,int> geo2path;
public:

	static uint64_t encode(double x0,double y0)
	{
		uint64_t x=(uint32_t)(x0*10);
		uint64_t y=(uint32_t)(y0*10);

		return (x<<32)|y;
	}

	int find(double x,double y)const
	{
		uint64_t h=encode(x,y);

		auto it=geo2path.find(h);

		if(it==geo2path.end())
			return -1;

		return it->second;
	}

	void add(double x,double y,int id)
	{
		uint64_t h=encode(x,y);
		geo2path.insert(std::make_pair(h,id));
	}

	int size()
	{
		return geo2path.size();
	}

	void print()
	{
		for(auto it=geo2path.begin();it!=geo2path.end();++it)
		{
			//std::cout<<it->first<<"\n";
            printf("%ld\n",it->first);
		}
	}

	geolist()
	{
	}

	~geolist()
	{
	}
};

struct graph
{
	vertex_list v;
	std::vector<std::vector<int>>  d;   //索引为起始顶点，数组内容为目标点
	std::vector<std::array<int,2>> l;	//bast_path list
	geolist geo;

	graph()
	{}

	void init(const vertex_list&v_,const std::vector<base_path>&bp)
	{
		for(auto&p:bp)
		{
			int from=v.add(v_[p[0]]);
			int to=v.add(v_[p[1]]);

			l.push_back({from,to});

			int id=l.size()-1;

			line_v lv(v[from],v[to]);

			printf("line:%s\n",lv.to_string().c_str());

			double cos=lv.cos_k();
			double sin=lv.sin_k();

			double x,y;
			for(double r=0,len=lv.length();r<len+0.1;r+=0.05)
			{
				x=lv[0].x+r*cos;
				y=lv[0].y+r*sin;

//				printf("x=%lf,y=%lf\n",x,y);

				for(int i=-1;i<=1;i++)
				for(int j=-1;j<=1;j++)
				{
					geo.add(x+i/10.,y+j/10.,id);
				}
			}

			add(from,to);
		}
	}

	std::vector<line_v>  find_possible_path(const point&from,double dist) const
	{
		std::vector<line_v> ret;

		int start_pt_id=v.find(from,dist);
		if(start_pt_id==-1)
			return std::move(ret);

		point start_pt=v[start_pt_id];

		for(uint32_t i=0;i<d[start_pt_id].size();i++)
		{
			ret.push_back(line_v(start_pt,v[d[start_pt_id][i]]));
		}

		return std::move(ret);
	}

	void add(int from,int to)
	{
		if((int)d.size()<=from) d.resize(from+1);
		if((int)d.size()<=to) d.resize(to+1);

		d[from].push_back(to);
		d[to].push_back(from);
	}

	bool find(std::vector<int>&ret,int from,std::array<int,2>&to,std::set<int>&ex,int level)
	{
		if(level--<0)
			return false;

		ret.push_back(from);

		for(int p:d[from]) //在当前路径
		{
			if(ex.find(p)!=ex.end()) //找过
				continue;

			if(p==to[0]||p==to[1])//找到
				return true;
//			{
//				ret.push_back(p);
//				return true;
//			}
		}

		ex.insert(from);

		for(int p:d[from])//在下级路径
		{
			if(ex.find(p)!=ex.end())//找过
				continue;
			if(find(ret,p,to,ex,level)) //找到
			{
				return true;
			}
		}

		ret.pop_back();
		return false;
	}

	bool is_at_path(const point&pt)const
	{
		return geo.find(pt.x,pt.y)>=0;
	}

	std::vector<point> find(const point&from,const point&to)
	{
		int f=geo.find(from.x,from.y);
		int t=geo.find(to.x,to.y);

		if(f<0 || t<0 || f==t)
			return {};
#if 0
		std::vector<point> rc[2];
		for(int i=0;i<2;i++)
		{
			std::vector<int> ret;
			std::set<int> ex({l[f][1-i]});
			if(find(ret,l[f][i],l[t],ex,2))
			{
				for(auto id:ret)
					rc[i].push_back(v[id]);
			}
		}
		if(!rc[0].empty() && (rc[1].empty() || rc[0].size() < rc[1].size()))
			return std::move(rc[0]);
		else if(!rc[1].empty())
			return std::move(rc[1]);
#endif
		std::vector<int> ret;
		int size=v.size();
		std::vector<double> mat(size+1,-1);
		std::vector<double> trace(size+1,-1);
		std::vector<bool> val(size+2,false);
		mat[l[t][0]]=to.dist(v[l[t][0]]);
		mat[l[t][1]]=to.dist(v[l[t][1]]);
		int f0=l[f][0],f1=l[f][1];
		for(int i=0;i<2;i++)
		{
			if(mat[f0]!=-1 && mat[f1]!=-1)
				break;
			for(int j=0;j<size;j++)
				if(mat[j]!=-1)val[j]=true;
			for(int j=0;j<size;j++)
			{
				if(val[j]==false)continue;
				for(unsigned int k=0;k<d[j].size();k++)
				{
					double dist=v[j].dist(v[d[j][k]]);
					if(mat[d[j][k]]==-1 || mat[j]+dist < mat[d[j][k]])
					{
						mat[d[j][k]] = mat[j]+dist;
						trace[d[j][k]] = j;
					}
				}
			}
		}
		mat[f0]=mat[f0]+from.dist(v[l[f][0]]);
		mat[f1]=mat[f1]+from.dist(v[l[f][1]]);
		std::vector<point> rc;
		if(mat[f0]!=-1 && (mat[f1]==-1 || mat[f0] < mat[f1]))
		{
			int temp=f0;
			while(temp!=-1)
			{
				rc.push_back(v[temp]);
				temp=trace[temp];
			}
			return std::move(rc);
		}
		else if(mat[f1]!=-1)
		{
			int temp=f1;
			while(temp!=-1)
			{
				rc.push_back(v[temp]);
				temp=trace[temp];
			}
			return std::move(rc);
		}
		return {};
	}
};


std::atomic<int> g_init_flag(0);
graph g_graph;

}//namespace

void card_path::init()
{
    //Ensure only ont thread can init path.
	int expect=0;
	if(g_init_flag.compare_exchange_strong(expect,1))
	{
		handle_path hp;
		//std::vector<base_path> opath=init_path(sites,v_list);
		sit_list::instance()->accept(hp);
		std::vector<base_path> opath=init_path(hp.ret,hp.v);
		
		g_graph.init(hp.v,opath);

		++g_init_flag;
	}
    //if != 2 then wait here until init over.
	while(g_init_flag.load()!=2)
	{
		std::this_thread::sleep_for (std::chrono::seconds(1));
	}
}


card_path&card_path::inst()
{
	static card_path path;
	return path;
}

std::vector<point> card_path::find_path(const point&from,const point&to)const
{
	return g_graph.find(from,to);
}

bool card_path::is_at_path(const point&pt)const
{
	return g_graph.is_at_path(pt);
}

std::vector<line_v>  card_path::find_possible_path(const point&from,double dist) const
{
	return std::move(g_graph.find_possible_path(from,dist));
}

#ifdef __DEBUG__

void test_at_path(card_path&cp,const point&pt)
{
	if(cp.is_at_path(pt))
		printf("test (%.3lf,%.3lf) is-at-path:true\n",pt.x,pt.y);
	else
		printf("test (%.3lf,%.3lf) is-at-path:false\n",pt.x,pt.y);
}

void test_find_path(card_path&cp,const point&p1,const point&p2)
{
	printf("\nfind-path:  from=(%.3lf,%.3lf),to=(%.3lf,%.3lf)\n",p1.x,p1.y,p2.x,p2.y);
	std::vector<point> rc=cp.find_path(p1,p2);
	for(uint32_t i=0;i<rc.size();i++)
		printf("x=%.3lf,y=%.3lf\n",rc[i].x,rc[i].y);
}

void test_find_poss_path(const card_path&cp ,const point&from)
{
	printf("\nfind-poss_path:  from=(%.3lf,%.3lf)\n",from.x,from.y);
	std::vector<line_v> rc=cp.find_possible_path(from,3);
	for(uint32_t i=0;i<rc.size();i++)
	{
		printf("poss path from=(%.3lf,%.3lf) to=(%.3lf,%.3lf)\n", rc[i][0].x,rc[i][0].y, rc[i][1].x,rc[i][1].y);
	}
}

int main()
{
//	std::unique_ptr<sit_list> sites(new sit_list());
	sit_list::instance()->load("data_reader_antenna.txt","path_tof.txt");

	card_path cp;

	cp.init();
#if 0
	{
		test_at_path(cp,point(4773.104654,-104.0887));

		test_at_path(cp,point(4727,-74.8));
		test_at_path(cp,point(4727,-75.0));
		test_at_path(cp,point(4727,-75.2));
		test_at_path(cp,point(4727,-75.8));
		test_at_path(cp,point(4727,-90));
		test_at_path(cp,point(4727,-89.6));
		test_at_path(cp,point(4727,-89.8));
		test_at_path(cp,point(4727,-90));
		test_at_path(cp,point(4727,-90.2));
		test_at_path(cp,point(4727,-90.4));
	}
#endif
	//test_at_path(cp,point(4726.90,376.85));
	test_find_path(cp,point(-227.6,174.9),point(-201.3,175.5));
	test_find_path(cp,point(-227.625219,174.945670),point(-170.085722,176.101574));
	test_find_path(cp,point(-393.1944,262.5324),point(-366.0754,274.1253));
	//test_find_path(cp,point(4637.000000,-75),point(4727,135));
#if 1
	{
		test_find_path(cp,point(4637.000000,-75),point(4727,135));
		test_find_path(cp,point(4637.000000,-75),point(4727,120));
		test_find_path(cp,point(4700,-75),point(4727,120));
		test_find_path(cp,point(4725,-75),point(4727,135));
		test_find_path(cp,point(4727,-89.7),point(6144.3,-523));
	}
#endif
#if 0
	{
		test_find_poss_path(cp, point(4727,-89));
		test_find_poss_path(cp, point(4727,-90));

		test_find_poss_path(cp, point(4724,-75));
		test_find_poss_path(cp, point(4725,-75));
		test_find_poss_path(cp, point(4726,-75));
		test_find_poss_path(cp, point(4727,-75));
		test_find_poss_path(cp, point(4728,-75));
		test_find_poss_path(cp, point(4727,-7));

		test_find_poss_path(cp, point(4726.6,33));
	}
#endif
	return 0;
}
#endif