server/pointSmooth/car.cpp

#include "stdafx.h"
#include "car.h"
#include "ProcessRemodule.h"

NAMESPACE_POINT_BEGIN(NAMESPACE_POINT)
	base_card::base_card()
		:m_id(0)
		,m_ct(-1)
		,mutex(0)

	{
		base_card_initiate();
	}
	base_card::base_card(std::shared_ptr<card_interface> &c,int id)
		:m_id(id)
		,m_ct(-1)
		,mutex(0)
		,m_card(c)
	{
		base_card_initiate();
	}

	void base_card::base_card_initiate()
	{
		m_sol_p=m_sol;

		m_fitk.add_tool(30,5,5);
		m_fitk.add_tool(30,5,6);
		m_fitk.add_tool(30,10,10);
		m_fitk.add_tool(30,10,15);

		m_fitk.add_tool(20,5,6);
		m_fitk.add_tool(30,4,6);
		m_fitk.add_tool(40,4,6);


		m_fita.add_tool(15,5,5);
		m_begin=m_last=0;

		m_filter_man_count = 0;

		//site*m_current_site=nullptr;

		//record of last fitting data
		m_last_fit_valid = false;
		m_last_fit_time_sec = 0;
		m_last_fit_k = 0;
		m_last_fit_kb = 0;
		m_last_fit_ka = 0;
		m_last_fit_xo = 0;
		m_last_fit_yo = 0;
		m_last_fit_md_x = 0;
		m_last_fit_md_y = 0;

		m_last_fit_nearest_time_sec = 0;	//latest time_sec within estimation span
		m_last_time_sec = 0;

		//record of last receiving time   comment it out if using timer
		m_last_receive_time_sec=0;
		m_last_receive_ct=0;
		m_last_receive_sit=nullptr;

		//parameters
		m_fit_differ=4;
		m_pos_differ=8;

		m_simple_rec_kx = 0;
		m_simple_rec_ky = 0;

		//turning
		reset_turning();

		smooth_initial_setting = false;
		smooth_speed = 0;
		smooth_speed_presentation = 0;
		smooth_speed_presentation_cnt = 0;
		smooth_last_time_sec = 0;
		smooth_line_reset = false;

		smooth_halt_condition = 0;	//if halting
		smooth_halt_count = 0;	//halting count

		//his
		his_reset();
	}

	int base_card::last_ct()const
	{
		return m_ct;
	}

	void base_card::reset_sol()
	{
		m_sol_p=m_sol;
	}

	int base_card::sol_count()const
	{
		return m_sol_p-m_sol;
	}

	void base_card::on_tof_data(const site*sit,const loc_info&li)
	{
		if(sit->is_path_empty() || li.m_tof==0)
			return ;
		if(! m_d.empty() && li.m_loc_time<m_d(0).m_time)
		{
			//printf("time backwards!!!\n");
			return;
		}

		//estimate_missing_point_by_history(sit, mtc, m_last_receive_ct);

		//debug_print_syslog(0,"[sit_scale on_tof_data]%f",sit->m_scale);
		double dist_tof=li.m_tof*15.65*2.996*1e-4/sit->m_scale;
		//the same ct,then calc
		if(last_ct()==li.ct()) 
		{
			sit->solving(&m_sol_p,li.ant_id(),dist_tof);
			m_d.grow().reset().set_source(m_id,sit->m_id,m_last_li,li);
			select_solution(sit);
			//计算多次天线之间的测量距离
			sit->count_ant_dist(dist_tof, m_last_li.m_tof*15.65*2.996*1e-4/sit->m_scale);
		}   
		else 
		{
			if(sol_count()==2 && li.m_card_ct - m_last_li.m_card_ct ==1)
			{
				m_d.grow().reset().set_source(m_id,m_last_site->m_id,m_last_li);
				select_solution(m_last_site);
			}
			reset_sol();
			sit->solving(&m_sol_p,li.ant_id(),dist_tof);
			m_last_li=li;
			m_last_site=sit;
		}

		m_ct=li.ct();
	}

	void base_card::remove_history()
	{
		loc_point&b=m_d(0);

		//如果队列大小大于120个元素
		//或者队列元素大于2个以上，其中第一个元素和第二个元素的时间差大于max_histime（此处为60）秒
		if(m_d.size()>120 || (m_d.size()>2 && m_d(1).time_off(b)>max_histime))
		{
			m_d.skip_if([&b,this](loc_point&p){ return p.time_off(b)>max_histime;});
			m_fitk.reset_data();
			m_fita.reset_data();
			m_cur_fit.reset();

			m_begin=m_last=nullptr;
			int idx=find_first(0);
			if(idx<0) 
				return;

			m_begin=&m_d[idx];

			idx=find_last(1);
			m_last=&m_d(idx);

			double dist=0,dist2=0;
			for(int len=min(15,m_d.size()-1),i=len;i>0;i--)
			{
				if(!m_d(i).cl())
					continue;

				//implemented by li
				int lastID1=find_last(i+1);
				int lastID2=-1;
				if(lastID1!=-1)
					lastID2=find_last(lastID1+1);
				if(lastID2!=-1)
				{
					double t0=m_d(i).m_time/1000., t1=m_d(lastID1).m_time/1000., t2=m_d(lastID2).m_time/1000.;
					double d0=m_begin->loc_dist(m_d(i)[0]), 
						   d1=m_begin->loc_dist(m_d(lastID1)[0]), 
						   d2=m_begin->loc_dist(m_d(lastID2)[0]);
					double k1=(d1-d0)/(t1-t0), k2=(d2-d1)/(t2-t1);
					if(t0-t1<5 && t1-t2<5 && fabs(k2-k1)<0.5)
					{
						double tbegin = t0-3;
						while(tbegin<t1+0.5)
							tbegin=tbegin+1;
						double tk=(d0-d1)/(t0-t1), tb=d1-tk*t1;		//d1+(d0-d1)/(t0-t1)*(t-t1)
						for(double ti=tbegin;ti<t0;ti=ti+1)
						{
							m_fitk.add(ti, tk*ti+tb);
						}
					}
				}

				dist=m_begin->loc_dist(m_d(i)[0]);
				m_fitk.add(m_d(i).m_time/1000.,dist);

				if(i==len)
					continue;

				m_fita.add(m_d(i).m_time/1000.,dist-dist2);
				dist2=dist;
			}

			save_k();
		}
	}

	void base_card::select_solution(const site*sit)
	{
		if(! select_solution_impl(sit))
		{
			m_cur_fit.reset();
		}
	}

	bool base_card::select_solution_impl(const site*sit)
	{
		if (CT_PERSON == m_card->getCardType())
		{
			return select_solution_impl_person(sit);
		}
		
		remove_history();

		if(!solution0(sit))
		{
			reset_sol();
			return false;
		}

		point pt=select_solution0(sit);
		reset_sol();

		debug_print_syslog(0,"[lemon_pt]%s,x:%f,y:%f",m_card->cardId().c_str(),pt.x,pt.y);
		if(m_d.empty())
		{
			debug_print_syslog(0,"[m_d.empty() is true] %s,ct: %d",m_card->cardId().c_str(),m_ct);
			return false;
		}

		//将pt点插入到队首位置
		m_d(0).set(pt);
		if(!m_d(0).empty())
			m_d(0).m_dist=sit->dist_direct(pt);
		else
		{
			m_d(0).set_cl(0);
			if(m_last) m_d(0).m_dist=0.01*m_last->m_dist>0?1:-1;	
		}

		if(pt.empty())
		{
			debug_print_syslog(0,"[pt.empty() is true] %s,ct: %d",m_card->cardId().c_str(),m_ct);
			return false;
		}

		//计算分站与计算出点之间距离
		m_d(0).m_dist1=sit->dist_direct(pt);
		double speed=0;
		int stat=1;
		int revise_flag = 0;

		//只有车卡才平滑
		if (m_card->smoothFlag())
		{
			revise_flag=1;
			pt=revise_by_history(pt, sit, m_d(0).m_time, revise_flag);
		}
		else
		{
			put_single_loc_point(pt, sit, m_d(0).m_ct, m_d(0));
			debug_print_syslog(0,"revise_by_history %s,flag:%d,(%.2f,%.2f)",m_card->cardId().c_str(),revise_flag,pt.x,pt.y);

		}
		//定位结果是否在路径上
		if(!card_path::inst().is_at_path(pt))
		{
			m_d(0).set_cl(0);
			debug_print_syslog(0,"[is_at_path() is false] %s,ct: %d",m_card->cardId().c_str(),m_ct);
			return false;
		}

		if(m_d(0).empty())
		{
			debug_print_syslog(0,"[m_d(0).empty() is true] %s,ct: %d",m_card->cardId().c_str(),m_ct);
			return false;
		}

		m_last=&m_d(0);

		if(m_line.empty() && m_d.size()>=2 && !make_line())
		{
			debug_print_syslog(0,"[m_line.empty() is true] %s,ct: %d",m_card->cardId().c_str(),m_ct);
			return false;
		}

		//if(!m_line.contain(m_d(0),0.1))
		if(!m_line.contain(m_d(0),0.01) || (m_begin && !m_line.contain(m_begin->m_sol[0], 0.01)))
		{
			m_fitk.reset_data();
			m_fita.reset_data();
			m_begin=m_last=nullptr;

			int i0=find_last(1);
			if(i0==-1)
			{
				debug_print_syslog(0,"[i0==-1] %s,ct: %d",m_card->cardId().c_str(),m_ct);
				return false;
			}

			std::vector<point> path=card_path::inst().find_path(m_d(i0),m_d(0));

			m_d.skip(m_d.size()-i0);

			if(path.empty())
			{
				m_line.clear();
				debug_print_syslog(0,"[path.empty() is true] %s,ct: %d",m_card->cardId().c_str(),m_ct);
				return false;
			}

			m_line.set(path.back(),m_d(0));
		}

		if(!m_begin)
		{
			int idx=find_first(0);
			if(idx>=0) m_begin=&m_d[idx];
		}

		if(!m_last)
		{
			int idx=find_last(0);
			if(idx>=0) m_last=&m_d(idx);
		}

		if(m_begin && m_d(0).cl())
		{
			//implemented by li
			int lastID1=find_last(1);
			int lastID2=-1;
			if(lastID1!=-1)
				lastID2=find_last(lastID1+1);
			if(lastID2!=-1)
			{
				double t0=m_last->m_time/1000., t1=m_d(lastID1).m_time/1000., t2=m_d(lastID2).m_time/1000.;
				double d0=m_begin->loc_dist(*m_last), d1=m_begin->loc_dist(m_d(lastID1)), d2=m_begin->loc_dist(m_d(lastID2));
				double k1=(d1-d0)/(t1-t0), k2=(d2-d1)/(t2-t1);
				if(t0-t1<5 && t1-t2<5 && fabs(k2-k1)<0.5)
				{
					double tbegin = t0-3;
					while(tbegin<t1+0.5)
						tbegin=tbegin+1;
					double tk=(d0-d1)/(t0-t1), tb=d1-tk*t1;		//d1+(d0-d1)/(t0-t1)*(t-t1)
					for(double ti=tbegin;ti<t0;ti=ti+1)
					{
						m_fitk.add(ti, tk*ti+tb);
					}
				}
			}

			m_d(0).m_area=m_fitk.add(m_last->m_time/1000.,
				m_begin->loc_dist(*m_last));	//printf("log:%f\n",f.dist(m_d(0)));

			if(m_d.size()>1)
			{
				int pre=find_last(1);
				if(pre>0) 
				{
					m_fita.add(m_d(0).m_time/1000.,m_begin->loc_dist(m_d(0))-m_begin->loc_dist(m_d(pre)));
				}
			}
		}

//		m_fitk.log();
//		m_fita.log();
#ifdef _SMOOTH
		//m_d(0).set(pt);
		//计算分站到这个点距离
		m_d(0).m_dist=sit->dist_direct(pt);
#endif
		//取出队首位置的坐标数据更新到业务模块
		loc_point lp=m_d(0);
		lp.set(pt);
		lp.m_speed=speed;
		lp.m_stat=stat;
		if(revise_flag==0)
		{
			//将结果更新到业务系统
			m_card->push_optimized_data(lp);
			//debug_print_syslog(0,"[on_tof_data]cardid: %s,ct: %d,x: %.2f,y: %.2f,pass: %d,speed: %.3f,acc: %.2f",m_card->cardId().c_str(),lp.m_ct,lp.x,lp.y,pass,speed,acc);
		}

		save_k();

		m_d(0).debug_out();
		//debug_print_syslog(0,"[lemon_smooth_log]t=%lld,sit=%d,card=%d,ct=%d,cred=%d,tof1=%d,tof2=%d,dist=%f,dir=0,pt=(%f,%f),rsp=%d,acc=%.2f,dist1=%.2f,dist2=%.2f\n",
		//	lp.m_time, lp.m_sid, lp.m_cid, lp.m_ct, lp.m_cred_level, 
		//	lp.m_tof[0], lp.m_tof[1],lp.m_dist,  lp.x,
		//	lp.y ,(int)(lp.m_rsp[0]+lp.m_rsp[1])/2,lp.m_acc,
		//	lp.m_dist1,lp.m_dist2
		//	);
		return true;
	}

	bool base_card::select_solution_impl_person(const site* sit)
	{
		remove_history();

		if(!solution0(sit))
		{
			reset_sol();
			return false;
		}
		
		int t_ct = m_ct;

		point pt=select_solution0(sit);
		reset_sol();

		if(m_d.empty())
		{
			return false;
		}

		//将pt点插入到队首位置
		m_d(0).set(pt);
		if(!m_d(0).empty())
			m_d(0).m_dist=sit->dist_direct(pt);
		else
		{
			m_d(0).set_cl(0);
			if(m_last) m_d(0).m_dist=0.01*m_last->m_dist>0?1:-1;	
		}

		if(pt.empty())
		{
			return false;
		}

		//计算分站与计算出点之间距离
		int revise_flag = 0;

		put_single_loc_point(pt, sit, m_d(0).m_ct, m_d(0));

		//定位结果是否在路径上
		if(!card_path::inst().is_at_path(pt))
		{
			m_d(0).set_cl(0);
			return false;
		}

		if(m_d(0).empty())
			return false;

		m_last=&m_d(0);

		if(m_line.empty() && m_d.size()>=2 && !make_line())
			return false;

		//点不在直线上
		if(!m_line.contain(m_d(0),0.01) || (m_begin && !m_line.contain(m_begin->m_sol[0], 0.01)))
		{
			m_fitk.reset_data();
			m_fita.reset_data();
			m_begin=m_last=nullptr;

			int i0=find_last(1);
			if(i0==-1)
				return false;

			std::vector<point> path=card_path::inst().find_path(m_d(i0),m_d(0));

			m_d.skip(m_d.size()-i0);

			if(path.empty())
			{
				m_line.clear();
				return false;
			}

			m_line.set(path.back(),m_d(0));
		}

		if(!m_begin)
		{
			int idx=find_first(0);
			if(idx>=0) m_begin=&m_d[idx];
		}

		if(!m_last)
		{
			int idx=find_last(0);
			if(idx>=0) m_last=&m_d(idx);
		}

		if(m_begin && m_d(0).cl())
		{
			//implemented by li
			int lastID1=find_last(1);
			int lastID2=-1;
			if(lastID1!=-1)
				lastID2=find_last(lastID1+1);
			if(lastID2!=-1)
			{
				double t0=m_last->m_time/1000., t1=m_d(lastID1).m_time/1000., t2=m_d(lastID2).m_time/1000.;
				double d0=m_begin->loc_dist(*m_last), d1=m_begin->loc_dist(m_d(lastID1)), d2=m_begin->loc_dist(m_d(lastID2));
				double k1=(d1-d0)/(t1-t0), k2=(d2-d1)/(t2-t1);
				if(t0-t1<5 && t1-t2<5 && fabs(k2-k1)<0.5)
				{
					double tbegin = t0-3;
					while(tbegin<t1+0.5)
						tbegin=tbegin+1;
					double tk=(d0-d1)/(t0-t1), tb=d1-tk*t1;		//d1+(d0-d1)/(t0-t1)*(t-t1)
					for(double ti=tbegin;ti<t0;ti=ti+1)
					{
						m_fitk.add(ti, tk*ti+tb);
					}
				}
			}

			m_d(0).m_area=m_fitk.add(m_last->m_time/1000.,m_begin->loc_dist(*m_last));

			if(m_d.size()>1)
			{
				int pre=find_last(1);
				if(pre>0) 
				{
					m_fita.add(m_d(0).m_time/1000.,m_begin->loc_dist(m_d(0))-m_begin->loc_dist(m_d(pre)));
				}
			}
		}

#ifdef _SMOOTH
		//计算分站到这个点距离
		m_d(0).m_dist=sit->dist_direct(pt);
#endif
		//取出队首位置的坐标数据更新到业务模块
		loc_point lp=m_d(0);
		lp.set(pt);
		//在将结果更新到业务之前先判断结果是否正确
		//将结果更新到业务系统
		m_card->push_optimized_data(lp);

		save_k();

		return true;
	}

	void base_card::save_k()
	{
		m_d(0).m_acc=0;
		const fit_result*fk=best_fit_raw(0,4);
		if(!fk)
		{
			m_cur_fit.reset();
			return;
		}

		m_cur_fit=*fk;

		fit_result&r=m_cur_fit;
		card_fit*fa=&m_fita[0];
		if(fa->is_valid() && fa->ke<0.1 && fk->k*fa->k<0)
		{
			double dk=fa->k*fa->num_point;
			r.ka=fa->k;
			if((fk->k+dk)*fk->k<0)
				r.k=0;
			else
				r.k+=dk;

			double y=fk->k*m_fitk(0).x+fk->kb;
			r.kb=y-m_fitk(0).x*r.k;
		}
	}

	int base_card::find_last(int start)
	{
		for(int i=start,len=m_d.size();i<len;i++)
		{
			if(m_d(i).cl()>0)
				return i;
		}

		return -1;
	}

	int base_card::find_first(int start)
	{
		for(int i=start,len=m_d.size();i<len;i++)
		{
			if(m_d[i].cl()>0)
				return i;
		}

		return -1;
	}

	bool base_card::make_line()
	{
		int i0=-1,i1=-1;

		if(-1==(i0=find_last(0)))
			return false;

		if(-1==(i1=find_last(i0+1)))
			return false;

		m_line.set(m_d(i0),m_d(i1));

		return true;
	}

	point base_card::select_solution0(const site* sit)
	{
		loc_point&c=m_d(0);
		if(m_d.size()==1)
		{
			//first point ,only accpet two ants data.
			if(c.cl()>0)
			{
				c[1]=m_sol[1];
				return c[0]=m_sol[0];
			}

			m_d.skip(1);
			return point(0,0);
		}

		//two ants.
		if(c.cl()>0 && sol_count()==2 && m_d(0).m_card_type==2)//m_card->smoothFlag() )
		{
			debug_print_syslog(0,"[1.one ant push data] %s,ct: %d",m_card->cardId().c_str(),m_ct);
			c[0]=m_sol[0];
			c[1]=m_sol[1];
			c.inc_cl(50);
		}
		else if(filter_by_fit(sit))
		{
			debug_print_syslog(0,"[2.one ant push data] %s,ct: %d",m_card->cardId().c_str(),m_ct);
			c.inc_cl(40);
		}
//		else if(filter_by_speed(sit))
//		{
//			c.inc_cl(20);
//		}
		else if(c.cl()>0 && sol_count()==2)
		{
			debug_print_syslog(0,"[3.one ant push data] %s,ct: %d",m_card->cardId().c_str(),m_ct);
			c[0]=m_sol[0];
			c[1]=m_sol[1];
			c.inc_cl(10);
		}
		else //one ant
		{
			debug_print_syslog(0,"[4.one ant push data] %s,ct: %d",m_card->cardId().c_str(),m_ct);
			int last=find_last(1);
			if(last>0)
			{
				loc_point&p=m_d(last);

				int cnt=sol_count();
				std::array<solpoint,4> res;

				//find the shortest dis
				for(int i=0;i<cnt;i++) 
					res[i].set_sol(m_sol[i],m_sol[i].dist(p[0]));

				std::sort(&res[0],&res[0]+cnt);

				c[1].set(res[1]);
				c[0].set(res[0]);
				c.inc_cl(5);
			}
		}

		return c[0];
	}

	fit_result* base_card::best_fit_raw(int num_point,int start,int last)
	{
		card_fit*fit=nullptr;

		start=max(start,0);
		last =min(last,m_fitk.tool_size());
		if(last==-1)
			last=m_fitk.tool_size();

		for(int i=start;i<last;i++)
		{
			if(!m_fitk[i].is_valid())
				continue;

			if(m_fitk[i].num_point<num_point)
				continue;

			if(fit==nullptr)
			{
				fit=&m_fitk[i];
				continue;
			}

			if(fit->ke>m_fitk[i].ke)
			{
				fit=&m_fitk[i];
			}
		}

		return fit;
	}


	const fit_result* base_card::best_fit()const
	{
		if(m_cur_fit.k==0 && m_cur_fit.ke==0)
			return nullptr;

		return &m_cur_fit;
	}

	bool base_card::filter_by_speed(const site *sit)
	{
		static double MAX_SPEED[]={0,8/(3.6*sit->m_scale),40/(3.6*sit->m_scale)};

		loc_point&c=m_d(0);
		if(c.m_card_type==2||m_begin==nullptr||m_last==nullptr)
			return false;

		int ctype=c.m_card_type==2?2:1;

		loc_point&e=*m_last;

		int cnt=sol_count();
		std::array<solpoint,4> v;

		int vc=0;

		double time=(c.m_time-e.m_time)/1000.;
		for(int i=0;i<cnt;i++)
		{
			double dist=e.loc_dist(m_sol[i]);
			double speed=dist/time;

			if(fabs(speed)>MAX_SPEED[ctype])
				continue;

			v[vc++].set_sol(m_sol[i],speed);
		}

		if(vc==0) return true;
		if(vc==1)
		{
			c[0]=v[0];
			c[1]=v[0];
			return true;
		}

		std::sort(&v[0],&v[0]+vc,[](const solpoint&l,const solpoint&r){
			return fabs(l.m_score)<fabs(r.m_score);
		});

		while(vc>1)
		{
			int pre=find_last(1);
			pre=find_last(pre+1);

			if(pre<0)
				break;

			double dist=m_d(pre).loc_dist(e);
			double time=(e.m_time-m_d(pre).m_time)/1000.;
			double speed=dist/time;

			int ac=0;
			for(int i=0;i<vc;i++)
			{
				double a=fabs(v[i].m_score-speed)/time;
				if(a>2.5)
					continue;

				v[ac++].m_score=a;
			}

			vc=ac;

			if(vc==0)
				return true;

			if(vc==1)
			{
				c[0]=v[0];
				c[1]=v[0];
				return true;
			}

			std::sort(&v[0],&v[0]+vc);

			if(c.m_card_type==1 && v[0].m_score<0.1)
			{
				c[0]=v[0];
				c[1]=v[0];
				return true;
			}

			break;
		}

		return false;
	}
	void base_card::log(const char * str)
	{
		if (m_card->getCardType()==5||m_card->getCardType()==4)
			debug_print_syslog(0,"%s %s",str,m_card->cardId().c_str());
	}
	bool base_card::filter_by_fit(const site *sit)
	{
		loc_point&f=*m_begin;
		loc_point&c=m_d(0);

		fit_result*fit=c.m_card_type==2?best_fit_raw(5):best_fit_raw(4,4);
//		fit_result*fit=best_fit_raw();

		if(fit==nullptr || m_begin==nullptr || fit->ke>2)
			return false;

		int cnt=sol_count();
		std::array<solpoint,4> v;

		for(int i=0;i<cnt;i++)
		{
			v[i].set_sol(m_sol[i],fabs(fit->testk(c.m_time/1000.,f.dist_direct(m_sol[i]))));
		}

		std::sort(&v[0],&v[0]+cnt);

		double a=2.5;

		if(c.m_card_type==2 && fabs(fit->k)<10/(3.6*sit->m_scale)  && fabs(fit->k)>1/(3.6*sit->m_scale)  && v[1].score()-v[0].score()<20)
		{
			a=1;
		}

		if(c.m_card_type==1 && fabs(fit->k)<2/(3.6*sit->m_scale)  && fabs(fit->k)>0.5/(3.6*sit->m_scale)  && v[1].score()-v[0].score()<10)
		{
			a=0.3;
		}

		if(c.m_card_type==2 && fabs(fit->k)<=1/(3.6*sit->m_scale)  && v[1].score()-v[0].score()<20)
		{
			return false;
		}

		//if(c.m_card_type==1 && fabs(fit->k)<=0.5/(3.6*sit->m_scale)  && v[1].score()-v[0].score()<8)
		//{
		//	return false;
		//}

		if(!filter_by_acc(c,v,a))
		{
			c.set_cl(0);
			log("filter_by_acc");
			return true; //false?
		}

		assert(m_last);

		//revise
		if(c.m_card_type==1)// && c.cl()>0 && sol_count()==2)
		{
			if(m_sol[0].dist(v[0])>1)
				m_filter_man_count++;
			else 
				m_filter_man_count = 0;
			if(m_filter_man_count==3)
			{
				m_fitk.reset_data();
				m_fita.reset_data();
				m_cur_fit.reset();
				m_begin=m_last=nullptr;
				m_filter_man_count = 0;
			}
		}

		c[0]=v[0];
		c[1]=v[1];

		return true;
	}
	bool base_card::filter_by_acc(loc_point&c,std::array<solpoint,4>&v,double a)
	{
		if(!m_last->is_same_site(c))
			return true;

		double td=m_last->time_off(c);

		if(v[0].score()>a*td*td)
			return false;

		return true;
	}


#if 0
	bool filter_gt50m()
	{
		static double max_speed=10.0/1000.;
		loc_point&c=m_d(0);

		if(!c.b_50m())
			return false;

		int last=std::min(6,m_d.size()-1);
		while(c.m_time-m_d(last).m_time>8000)
			--last;

		if(last<3) 
			return false;

		int cnt=sol_count();
		std::array<point,4> v;
		for(int j=1;j<=last;j++)
		{
			double t=c.m_time-m_d(j).m_time;
			if(m_d(j).empty())
				continue;

			for(int i=0;i<cnt;i++)
			{
				double v0=m_d(j).dist(m_sol[i])/t;
				if(v0>max_speed)
					continue;
				v[i].y=i;
				v[i].x-=1;

				if(m_d(j).m_sid!=c.m_sid)//切换过分站之后更靠谱
					v[i].x-=3;

				if(j==last)
					continue;

				double v1=m_d(j).dist(m_d(last))/(m_d(j).m_time-m_d(last).m_time);
				v[i].x-=10-fabs(v1-v0)*1000;
			}
		}

		std::sort(&v[0],&v[0]+cnt);

		if(v[0].x<-3)
		{
			c[0]=m_sol[(int)v[0].y];
			c[1]=m_sol[(int)v[1].y];

			return true;
		}

		return false;
	}
#endif

	//判断距离最近的两个解
	bool base_card::solution0(const site*sit)
	{
		if(sol_count()!=4)
			return true;

		int c=0;
		std::array<solpoint,4> res;

		for(int i=0;i<2;i++)
		{
			double d=m_sol[i].dist(m_sol[i+2]);

			//add by zhuyf 2018/06/27
			//将判断条件改为根据两天线间的距离与实际距离的差是否符合指定误差范围
			/*double l = 0.0;
			double d1 = 0.0, d2 = 0.0;

			d1 = m_d(0).m_tof[0]*15.65*2.996*1e-4/sit->m_scale;
			d2 = m_d(0).m_tof[1]*15.65*2.996*1e-4/sit->m_scale;

			if (d1 < 10.0 || d2 < 10.0)
			{
			d1 = sqrt(d1*d1 - 1.5*1.5);
			d2 = sqrt(d2*d2 - 1.5*1.5);
			}
			l = fabs(d1 - d2);

			bool condition = false;
			if (fabs(l - sit->ant_dist()) < 0.2*sit->m_scale)
			{
				condition = true;
				debug_print_syslog(0,"[condition failed push data]cardid: %s,ct: %d",m_card->cardId().c_str(),m_ct);
			}*/

			if(d<sit->ant_dist()*3)
			//if(condition)
			{
				res[c++].set_sol(m_sol[i].middle(m_sol[i+2]),d);
			}
			else
			{
				res[c++].set_sol(m_sol[i]);
				res[c++].set_sol(m_sol[i+2]);
			}
		}

		std::sort(&res[0],&res[0]+c);

//		printf("dist=%.5f, %.5f\n",res[0].dist,res[1].dist);

		for(int i=0;i<c;i++)
			m_sol[i]=res[i];

		m_sol_p=m_sol+c;
		//if(c>=2 && res[0].m_score*3>res[1].m_score)
		//{
		//}
		//else if(c<=2)
		//{
			m_d(0).inc_cl(10);
		//}

		return true;
	}

	point base_card::revise_by_history(point pt, const site*sit, int64_t m_time, int &revise_flag)	//m_time - 1000*ct
	{
		if(m_line.empty() || !m_line.contain(m_d(0),0.1))
		{
			//m_current_site = sit;
			m_last_fit_valid = false;
			m_last_fit_time_sec = 0;
			m_last_fit_k = 0;
			m_last_fit_kb = 0;
			m_last_fit_ka = 0;
			m_last_fit_xo = 0;
			m_last_fit_yo = 0;
			m_last_fit_nearest_time_sec = 0;
			m_last_time_sec = 0;

			//turning
			reset_turning();

			//create the list
			generate_list(pt, sit, false);	//generate single point

		// smooth the dist----test
#ifdef _CAR_TEST
			point smooth_location;
			if(!m_if_turning)
			{
				point tmp;
				tmp.set(sit->x,sit->y);
				sit->mapping(tmp,smooth_location);
			}
			else
				smooth_location.set(m_turning_pt);
			smooth_dist_car(pt, m_time/1000., m_d(0).m_ct, sit, smooth_location,&m_d(0));
#endif
			return pt;
		}

#ifdef _CAR_TEST
		// print missing point
		if(m_last_fit_valid && m_last_receive_sit!=nullptr && m_last_receive_time_sec - m_last_fit_time_sec < 20 && fabs(m_last_fit_k) > 0.5)
		{
			double mt1 = m_last_receive_time_sec;
			double mt2 = m_d(0).m_time / 1000.;
			double mtc = mt1;
			printf("begin print missing point %f ~ %f\n",mt1, mt2);
			if(mt2-mt1 > 1.5)
			{
				mtc = mtc + 1;
				while(mt2-mtc > 0.5)
				{
					if(mtc-mt1>12)
					{
						break;
					}
					estimate_missing_point_by_history(m_last_receive_sit, mtc, m_last_receive_ct);
					mtc = mtc + 1;
				}

			}
			//printf("end print missing point\n");
		}
		m_last_receive_time_sec = m_time / 1000.;
		m_last_receive_ct = m_d(0).m_ct;
		m_last_receive_sit = sit;
		// comment out if using timer
#endif
		// convert pt to distance
		double dist = convert_pt_to_dist(pt, sit);
		double m_time_sec = m_time / 1000.;	//second
		//if(m_time_sec - m_last_fit_nearest_time_sec > 30)m_last_fit_valid = false;
		if(m_time_sec - m_last_fit_time_sec > 60)m_last_fit_valid = false;

		// update acc
		//m_accumulate_acc = m_d(0).m_acc;

		// choose data by fit
		const fit_result*fit=best_fit();
		bool if_change_fit=false;
		if(fit!=nullptr && fit->ke<=1 && m_time_sec - m_fitk.x(0) <= 15 && fabs(fit->k) < m_pos_differ)
		{							//printf("change fit time:%f,%f,%f\n",m_time_sec, fit->d.x(0), m_time_sec - fit->d.x(0));
			
			// put m_acccumulate_acc into consideration
			// fit->k - m_last_fit_k < m_accumulate_acc
			if(m_last_fit_valid == true && m_last_fit_k * fit->k > -0.6)
			//if((sit->dist(pt)<20 ||m_last_fit_k * fit->k > -0.6))
			{	//if point is too near the sit: do not not judge the backwards

				double est1 = estimate_point_by_history(sit, m_last_fit_time_sec);
				double est2 = fit->k * (m_time_sec-fit->xo) + fit->kb + fit->yo;
				//printf("change fit:1(%f,%f),2(%f,%f),differ:(%f,%f)\n",
				//	m_last_fit_nearest_time_sec,est1,m_time_sec,est2,m_time_sec-m_last_fit_nearest_time_sec,est2-est1);
				//if(fabs(est1-est2)>40)printf("change fit:%f,%f,%f\n",fabs(est1-est2),est1,est2);
				if(fabs(est1-est2)< (m_time_sec - m_last_fit_time_sec) * 5) // large jump is not allowed
					if_change_fit=true;
			}
			else if(m_last_fit_valid==false)
				if_change_fit=true;
		}
		if(if_change_fit)
		{
			m_last_fit = *fit;
			m_last_fit_valid = true;
			m_last_fit_time_sec = m_fitk.x(0);
			m_last_fit_k = fit->k;
			m_last_fit_kb = fit->kb;
			m_last_fit_ka = fit->ka;
			m_last_fit_xo = fit->xo;
			m_last_fit_yo = fit->yo;
			m_last_fit_nearest_time_sec = m_fitk.x(0);
			
			int fidx=find_first(0);
			if(fidx<0)
			{
				m_last_fit_md_x = 0;
				m_last_fit_md_y = 0;
			}
			else{
				loc_point&f=m_d[fidx];
				m_last_fit_md_x = f.m_sol[0].x;
				m_last_fit_md_y = f.m_sol[0].y;
			}

			//printf("change line\n");
		}

		// revise
		double estimate_dist = estimate_point_by_history(sit, m_time_sec);
		//if(m_last_fit_valid)printf("e:%f,d:%f,fit differ:%f;t-lt:%f\n",
		//	estimate_dist,dist,estimate_dist - dist,m_time_sec - m_last_fit_time_sec);
		if(m_last_fit_valid && m_time_sec - m_last_fit_time_sec < 20)
		{
			if(fabs(m_last_fit_k) > 0.5 && fabs(estimate_dist-dist)>m_fit_differ)
				dist=estimate_dist;
			else if(fabs(m_last_fit_k) <= 0.5 && fabs(estimate_dist-dist)>m_fit_differ * 2)
				dist=estimate_dist;
			else revise_flag = 0;
		}
		else m_last_fit_nearest_time_sec = m_time_sec;
		m_last_time_sec = m_time_sec;

		// convert the estimated dist to pt
		point mpt = convert_dist_to_pt(dist, sit);

		// judging turning
		detect_turning(mpt, sit);

		//create the list
		if(m_last_fit_valid && m_time/1000. - m_last_fit_time_sec < 20 && fabs(m_last_fit_k) > 0.5)
			generate_list(mpt, sit, true);	//generate the whole list
		else
			generate_list(mpt, sit, false);	//generate single point

		//turning map
		turning_mapping(mpt, sit);

		// smooth the dist----test
#ifdef _CAR_TEST
		point smooth_location;
		if(!m_if_turning)
		{
			point tmp;
			tmp.set(sit->x,sit->y);
			sit->mapping(tmp,smooth_location);
		}
		else
			smooth_location.set(m_turning_pt);
		smooth_dist_car(mpt, m_time_sec, m_d(0).m_ct, sit, smooth_location, &m_d(0));
#endif

		return mpt;
	}

	// estimate point dist
	double base_card::estimate_point_by_history(const site*sit, double m_time_sec)
	{

		//m_time, m_sid, m_cid
		double estimate_dist = m_last_fit_k * (m_time_sec-m_last_fit_xo) + m_last_fit_kb + m_last_fit_yo;
		point pt(m_last_fit_md_x + estimate_dist * m_simple_rec_kx, m_last_fit_md_y + estimate_dist * m_simple_rec_ky);

		//printf("cts:%f, ts:%f, k:%f, b:%f, xo:%f, yo:%f, (%f,%f)",
			//m_time_sec ,m_last_fit_time_sec,m_last_fit_k,m_last_fit_kb,m_fitk.xo,m_fitk.yo, pt.x, pt.y);

		int fidx=find_first(0);	
		if(fidx>=0)
		{
			loc_point&f=m_d[fidx];
			estimate_dist = f.loc_dist(pt);
		}
		else estimate_dist = 0;

		return estimate_dist;
	}

	// timer up missing point
	void base_card::estimate_missing_point_by_history(const site*sit, double m_time_sec, int m_ct)
	{
		loc_point m_lp;
		m_lp.m_time=(int64_t)(m_time_sec * 1000);
		m_lp.m_sid = sit->m_id;
		m_lp.m_cid = m_d(0).m_cid;
		//printf("cts:%f, ts:%f, k:%f, b:%f, xo:%f, yo:%f\n",
		//	m_time_sec ,m_last_fit_time_sec,m_last_fit_k,m_last_fit_kb,m_fitk.xo,m_fitk.yo);

		//m_time, m_sid, m_cid
		double estimate_dist = estimate_point_by_history(sit, m_time_sec);
	//	printf("missing, t:%f,d:%f\n",m_time_sec,estimate_dist);
		point m_p = convert_dist_to_pt(estimate_dist, sit);

		turning_mapping(m_p,sit);
		if(!card_path::inst().is_at_path(m_p))	//if point not on the path
		{	
			printf("out of path:(%f,%f)\n",m_p.x, m_p.y);
			return;
		}

#ifdef _CAR_TEST
		point smooth_location;
		if(!m_if_turning)
		{
			point tmp;
			tmp.set(sit->x,sit->y);
			sit->mapping(tmp,smooth_location);
		}
		else
			smooth_location.set(m_turning_pt);
		smooth_dist_car(m_p, m_time_sec, m_ct, sit, smooth_location, &m_lp);
#endif

		m_lp.set(m_p);
		m_lp.m_dist=sit->dist_direct(m_p);
		m_lp.debug_out();
		m_lp.x=m_lp.m_smooth_x;
		m_lp.y=m_lp.m_smooth_y;
		//push m_lp
	}

	double base_card::convert_pt_to_dist(point &pt, const site*sit)
	{
		double dist=0;
		int fidx=find_first(0);
		if(fidx>=0)
		{
			loc_point&f=m_d[fidx];
			//dist = f.dist(pt) * (pt<f?-1:1);
			dist = f.loc_dist(pt);
			if(dist!=0)
			{
				m_simple_rec_kx = (pt.x - f.m_sol[0].x) / dist;
				m_simple_rec_ky = (pt.y - f.m_sol[0].y) / dist;
			}
		}
		if(fidx<0 || dist==0)
		{
			m_simple_rec_kx = 0;
			m_simple_rec_ky = 0;
		}
			//printf("convert_pt_to_dist:(%f,%f),%f\n",pt.x,pt.y,dist);
		//double dist = sit->dist_direct(pt);
		//if(dist == 0)return 0;
		//m_simple_rec_kx = (pt.x - (*sit).x) / dist;
		//m_simple_rec_ky = (pt.y - (*sit).y) / dist;
		return dist;
	}

	point base_card::convert_dist_to_pt(double dist, const site*sit)
	{
		int fidx=find_first(0);
		if(fidx<0)return point(0, 0);
		loc_point&f=m_d[fidx];
		//printf("convert_dist_to_pt:dist:%f,x:%f,y:%f,reckx:%f,recky:%f,f.x:%f,f.y%f\n",
			//dist,f.x + dist * m_simple_rec_kx,f.y + dist * m_simple_rec_ky,
			//m_simple_rec_kx,m_simple_rec_ky,f.x,f.y);
		return point(f.m_sol[0].x + dist * m_simple_rec_kx, f.m_sol[0].y + dist * m_simple_rec_ky);
		//return point((*sit).x + dist * m_simple_rec_kx, (*sit).y + dist * m_simple_rec_ky);
	}

	void base_card::reset_turning()
	{
		m_if_turning=false;
		m_turning_pt.set(0,0);
		m_turning_ept.set(0,0);
	}

	void base_card::detect_turning(point &mpt, const site*sit)
	{
		if(m_if_turning)return;

		//IMPORTANT: only car-1121 and car-1136 have accurate rav values currently. May delete this sentence in the future.
		//if(m_id!=1121 && m_id!=1136)return;

		double detect_area = 4;
		double detect_angle = 15;	//15
		double detect_para = 0.25;	//0.25

		// check angle
		double angle=-m_d(0).m_rav;   // right+ left-

		if(fabs(angle)>180)return;		// invalid data
		if(fabs(angle)<detect_angle || !m_last_fit_valid || fabs(m_last_fit_k)<0.01)return;

		// find turning point
		std::vector<line_v> turning_list=card_path::inst().find_possible_path(mpt, detect_area);;

		//angle1
		int fidx=find_first(0);
		if(fidx<0)return;
		double dist=m_d[fidx].loc_dist(mpt);
		point pt1;
		pt1.set(m_d[fidx].m_sol[0]);

		double angle1;
		if(m_last_fit_k * dist>0)
			angle1=calc_turning_angle(pt1, mpt);
		else
			angle1=calc_turning_angle(mpt, pt1);
		if(angle1<0)return;

		//finding
		for(unsigned int i=0;i<turning_list.size();i++)
		{
			line_v &l=turning_list[i];
			double angle2=calc_turning_angle(l.v[0], l.v[1]);
			double delta=angle1-angle2;
			if(delta>180)delta=delta-360;
			if(delta<-180)delta=delta+360;
			if(fabs(delta)<5)continue;
			if(fabs(delta)>175)continue;

			if(angle*delta>0 && fabs(angle)>fabs(delta)*detect_para)
			{
				printf("turning:(%.5f,%.5f)(%.5f,%.5f)(%.5f,%.5f),a1:%f,a2:%f,delta:%f,angle:%f\n",
					pt1.x,pt1.y,l.v[0].x,l.v[0].y,l.v[1].x,l.v[1].y,angle1,angle2,delta,angle);
				m_if_turning=true;
				m_turning_pt.set(l.v[0]);
				m_turning_ept.set(l.v[1]);
				break;
			}
		}
	}
	double base_card::calc_turning_angle(point &a, point &b)
	{
		if(fabs(a.x-b.x)<0.001)
		{
			if(fabs(a.y-b.y)<0.001)return -1;
			return b.y>a.y?90:270;
		}
		double angle=std::atan((b.y-a.y)/(b.x-a.x))*180/3.1415926;
		if(a.x>b.x)angle=angle+180;
		if(angle<0)angle=angle+360;
		return angle;
	}
	void base_card::turning_mapping(point &rpt,const site*sit)
	{
		if(!m_if_turning)return;

		point tmp;
		tmp.set(sit->x,sit->y);
		point sit_location;		//projection
		sit->mapping(tmp,sit_location);

		double dist1=sit_location.dist(rpt);
		double dist2=sit_location.dist(m_turning_pt);
		double dist3=m_turning_pt.dist(rpt);
		if(dist1<=dist2 || dist1<=dist3)
		{
			if(dist2-dist1>3||dist1<=dist3)
			{
				printf("reset turning\n");
				reset_turning();	// may encounter problems
			}
			return;
		}
		if(dist3>10)dist3=10;	// turning distance no more than 10

		double turning_x,turning_y;
		double dist4=m_turning_pt.dist(m_turning_ept);
		turning_x=m_turning_pt.x + dist3 * (m_turning_ept.x - m_turning_pt.x) / dist4;
		turning_y=m_turning_pt.y + dist3 * (m_turning_ept.y - m_turning_pt.y) / dist4;

		/*printf("turning mapping:(%.2f,%.2f)->(%.2f,%.2f),d1:%f,d2:%f,d3:%f\n",
			rpt.x,rpt.y,turning_x,turning_y,dist1,dist2,dist3);*/

		rpt.set(turning_x,turning_y);
	}

	void base_card::smooth_reset()
	{
		smooth_initial_setting=false;
		smooth_speed=0;							//smoothed speed
		//smooth_speed_presentation=0;
		smooth_speed_presentation_cnt=0;
		smooth_last_position=point(0,0);		//last position of smoothed point
		smooth_last_true_position=point(0,0);	//last position of true point
		smooth_last_time_sec=0;					//last time second
		smooth_halt_condition = false;
		smooth_halt_count=0;
	}
	bool base_card::smooth_initiate(point &pt, double t, const site*sit)
	{
		smooth_initial_setting=true;
		smooth_speed=0;
		//smooth_speed_presentation=0;
		smooth_speed_presentation_cnt=0;
		smooth_last_position = pt;
		smooth_last_true_position = pt;
		smooth_last_time_sec = t;
		smooth_halt_condition=false;
		smooth_halt_count=0;
		smooth_halt_position=pt;
		smooth_halt_position_plus=pt;
		smooth_halt_position_minus=pt;
		return true;
	}
	void base_card::smooth_dist_car(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp)
	{
		point init_pt(pt.x, pt.y);
		if(smooth_line.empty() || !smooth_line.contain(pt,0.1) || smooth_halt_count>6)
		{
			if(!smooth_line_reset)
			{
				if(!smooth_line.empty() && !smooth_line.contain(pt,0.1) && !smooth_last_true_position.empty())
				{
					std::vector<point> path=card_path::inst().find_path(smooth_last_true_position, pt);
					if(!path.empty() && smooth_last_true_position.dist(path[0])>200)
						path.clear();
					debug_print_syslog(0,"generating critical point in smooth(car):(%.2f,%.2f)->(%.2f,%.2f)\n",
        			 	smooth_last_true_position.x, smooth_last_true_position.y, pt.x, pt.y);
					if(!path.empty())
					{
						point critical_point=path[0];
						debug_print_syslog(0,"critical point generated in smooth(car):pt=(%.2f,%.2f),(%.2f,%.2f)->(%.2f,%.2f)\n",
        					    critical_point.x, critical_point.y, smooth_last_true_position.x, smooth_last_true_position.y,
        					    pt, pt.y);
						init_pt.set(critical_point);
					}
				}
				smooth_reset();
				smooth_line_reset=true;
			}
			else 
			{
				std::vector<point> path=card_path::inst().find_path(smooth_last_true_position, pt);
				if(!path.empty() && smooth_last_true_position.dist(path[0])>200)
					path.clear();
				if(path.empty())
				{
					smooth_line.set(smooth_last_true_position, pt);
					smooth_line_reset=false;
				}
				else
				{
					smooth_reset();
				}
			}
		}
		else
			smooth_line_reset=false;

		if(!smooth_initial_setting)
		{
			smooth_initiate(init_pt, t, sit);
		}
		else
		{
			double current_dist = dstp.dist_direct(pt);
			double last_position = dstp.dist_direct(smooth_last_position);
			double last_true_position = dstp.dist_direct(smooth_last_true_position);
			double rec_kx=0;
			double rec_ky=0;
			if(current_dist!=0)
			{
				rec_kx = (pt.x - dstp.x)/current_dist;
				rec_ky = (pt.y - dstp.y)/current_dist;
			}
			double next_dist = last_position + smooth_speed * (t-smooth_last_time_sec);
			double max_span = 200;
				//printf("smooth dist:%f,%f,%f\n",next_dist,current_dist,next_dist-current_dist);
			if(fabs(next_dist-current_dist)<max_span && t - smooth_last_time_sec < 10)
			{
				double new_speed = (current_dist-last_true_position) / (t - smooth_last_time_sec);

				// judge halting
				if(fabs(new_speed)<0.1)smooth_halt_count++;
				else{
					smooth_halt_count=0;
				}
				if(!smooth_halt_condition && smooth_halt_count>=3 && fabs(smooth_speed) < 0.2)
				{
					smooth_halt_condition=true;
					smooth_halt_position=smooth_last_position;
					smooth_halt_position_plus=pt;
					smooth_halt_position_minus=pt;
				}

				// handle speed
				if(smooth_halt_condition)
				{
					double halt_position = dstp.dist_direct(smooth_halt_position);
					double halt_position_plus = dstp.dist_direct(smooth_halt_position_plus);
					double halt_position_minus = dstp.dist_direct(smooth_halt_position_minus);
					if(halt_position_plus<current_dist)halt_position_plus=current_dist;
					if(halt_position_minus>current_dist)halt_position_minus=current_dist;
					smooth_halt_position_plus = point(dstp.x + halt_position_plus * rec_kx, dstp.y + halt_position_plus * rec_ky);
					smooth_halt_position_minus = point(dstp.x + halt_position_minus * rec_kx, dstp.y + halt_position_minus * rec_ky);
					if(fabs(halt_position_plus - halt_position_minus)>1)
					{
						smooth_halt_condition=false;
						last_position = halt_position;
						smooth_speed = 0;
						//printf("smooth stop halting\n");
					}
				}
				else
				{
					if(fabs(smooth_speed)<1e-6 || _isnan(smooth_speed))
					{
						smooth_speed=new_speed;
						if(smooth_speed>2.5)smooth_speed=2.5;
						if(smooth_speed<-2.5)smooth_speed=-2.5;
					}
					else
					{
						double speed_differ = fabs(new_speed-smooth_speed);
						if(speed_differ>1)
							new_speed=smooth_speed +1*(new_speed>smooth_speed?1:-1);
						smooth_speed = smooth_speed * 0.4 + new_speed * 0.6;
					}

					if(fabs(smooth_speed_presentation)<1e-6 ||_isnan(smooth_speed_presentation))
					{
						smooth_speed_presentation=fabs(smooth_speed);
					}
					else
						smooth_speed_presentation = smooth_speed_presentation * 0.4 + fabs(smooth_speed) * 0.6;

					//printf(",%f,%f\n",new_speed,smooth_speed);

					// must obey speed direction 
					if(smooth_speed * (current_dist-last_position) > 0)
					{
						last_position = last_position+smooth_speed*(t-smooth_last_time_sec);
						if(smooth_speed * (current_dist-last_position) < 0)
						{
							last_position = current_dist;
						}
						smooth_speed_presentation_cnt=0;
					}
					else
					{
						if(smooth_speed_presentation_cnt<3)
							smooth_speed_presentation_cnt++;
						else
							smooth_speed_presentation=0;
					}
					if(fabs(smooth_speed)<0.1)smooth_speed_presentation=0;

					double revise_para = 0.2;
					if(fabs(smooth_speed) < 0.01 || smooth_speed * (current_dist-last_position) < 0)
						revise_para=0;

					last_position=last_position+(current_dist-last_position)*revise_para;
				}
				smooth_last_position = point(dstp.x + last_position * rec_kx, dstp.y + last_position * rec_ky);
				smooth_last_true_position = pt;
				smooth_last_time_sec = t;
			}
			else
			{
				smooth_reset();
				smooth_initiate(pt, t, sit);
			}
		}

		if(m_lp != nullptr)//m_time,m_ct,x,y,m_speed,m_stat
		{
			smooth_set_loc_point(t, ct, sit, m_lp);
		}
	
	}
	void base_card::smooth_dist_man(point &pt, double t, int ct, const site*sit, point dstp,loc_point *m_lp)
	{
		if(smooth_line.empty() || !smooth_line.contain(pt,0.1))
		{
			if(!smooth_line_reset)
			{
				smooth_reset();
				smooth_line_reset=true;
			}
			else 
			{
				std::vector<point> path=card_path::inst().find_path(smooth_last_true_position, pt);
				if(!path.empty() && smooth_last_true_position.dist(path[0])>200)
					path.clear();
				if(path.empty())
				{
					smooth_line.set(smooth_last_true_position, pt);
					smooth_line_reset=false;
				}
				else
				{
					smooth_reset();
				}
			}
		}
		else
			smooth_line_reset=false;

		if(!smooth_initial_setting)
		{
			smooth_initiate(pt, t, sit);
		}
		else
		{
			double current_dist = dstp.dist_direct(pt);
			double last_true_position = dstp.dist_direct(smooth_last_true_position);
			double max_span = 100;
			if(fabs(current_dist-last_true_position)<max_span && t - smooth_last_time_sec < 10)
			{
				//计算人卡速度
				double new_speed = (current_dist-last_true_position) / (t - smooth_last_time_sec);

				double speed_differ = fabs(new_speed-smooth_speed);
				if(speed_differ>1)new_speed=smooth_speed +1*(new_speed>smooth_speed?1:-1);
				smooth_speed = smooth_speed * 0.4 + new_speed * 0.6;
				smooth_last_true_position = pt;
				smooth_last_position = pt;
				smooth_last_time_sec = t;
				if(fabs(smooth_speed_presentation)<1e-6 ||_isnan(smooth_speed_presentation))
				{
					smooth_speed_presentation=fabs(smooth_speed);
				}
				else
					smooth_speed_presentation = smooth_speed_presentation * 0.4 + fabs(smooth_speed) * 0.6;
				if(fabs(smooth_speed)<0.1)smooth_speed_presentation=0;
			}
			else
			{
				smooth_reset();
				smooth_initiate(pt, t, sit);
			}
		}

		if(m_lp != nullptr)//m_time,m_ct,x,y,m_speed,m_stat
		{
			smooth_set_loc_point(t, ct, sit, m_lp);
		}
	}
	void base_card::smooth_set_loc_point(double t, int ct, const site*sit, loc_point *lp)
	{
		point pt;
		if(smooth_halt_condition)
			pt.set(smooth_halt_position);
		else
			pt.set(smooth_last_position);

		lp->m_dist2=sit->dist_direct(pt);
		lp->m_smooth_x=pt.x;
		lp->m_smooth_y=pt.y;
		lp->m_time=(int64_t)(t*1000);
		lp->m_ct=ct;
		if(smooth_halt_condition)
		{
			lp->m_speed=0;
			lp->m_stat=0;
		}
		else
		{
			lp->m_speed=smooth_speed_presentation * (3.6*sit->m_scale) ; //(m/s) to (km/h)
			if(smooth_speed<0)
				lp->m_speed = -lp->m_speed;
			if(_isnan(lp->m_speed))
				lp->m_speed=0;
			lp->m_stat=1;
			//if(fabs(smooth_speed_presentation) < 0.1)
			//	lp->m_speed=0;
		}
	}
	void base_card::generate_list(point &pt, const site*sit, bool is_whole_list)
	{
		if(is_whole_list)
		{
			put_loc_point(pt, sit, m_d(0).m_ct, m_d(0));
		}
		else
		{
			put_single_loc_point(pt, sit, m_d(0).m_ct, m_d(0));
		}
	}

	void base_card::put_single_loc_point(point &pt, const site*sit, int ct, loc_point &lp)
	{
		point rpt;
		rpt.set(pt);
		turning_mapping(rpt,sit);

		if(!card_path::inst().is_at_path(rpt))	//if point not on the path
		{	
			//lp.debug_out();
			/*printf("out of path:t=%ld,sit=%d,card=%d,ct=%d,"
			"tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
			m_d(0).m_time, m_d(0).m_sid, m_d(0).m_cid,m_d(0).m_ct,
			m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);*/
			debug_print_syslog(0,"[not on path----]cardid: %s,ct: %d,x: %.3f, y: %.3f.\n",m_card->cardId().c_str(), m_ct,rpt.x,rpt.y);
			return;
		}

		point dstp;		//projection
		if(!m_if_turning)
		{
			point tmp;
			tmp.set(sit->x,sit->y);
			sit->mapping(tmp,dstp);
		}
		else
		{
			dstp.set(m_turning_pt);
		}

		push_data_point dp;
		dp.sit=sit;
		dp.dstp.set(dstp);
		dp.ct=ct;
		dp.valid=true;
		dp.stop_cnt=5;
		dp.lp=lp;
		dp.set(rpt);
		{	//don't delete the braces
			atomic_lock lock(mutex);
			m_push_list.clear();
			m_push_list.push(dp);
		}
	}

	void base_card::put_loc_point(point &pt, const site*sit, int ct, loc_point &lp)
	{
		point rpt;
		rpt.set(pt);
		turning_mapping(rpt,sit);

		if(!card_path::inst().is_at_path(pt))	//if point not on the path
		{	
			lp.debug_out();
			/*printf("out of path:t=%ld,sit=%d,card=%d,ct=%d,"
            "tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
            m_d(0).m_time, m_d(0).m_sid, m_d(0).m_cid,m_d(0).m_ct,
            m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);*/
			return;
		}

		point dstp;		//projection
		if(!m_if_turning)
		{
			point tmp;
			tmp.set(sit->x,sit->y);
			sit->mapping(tmp,dstp);
		}
		else
		{
			dstp.set(m_turning_pt);
		}

		int size = 0;
		push_data_point dp[13];
		dp[size].sit=sit;
		dp[size].dstp.set(dstp);
		dp[size].ct=ct;
		dp[size].valid=true;
		dp[size].stop_cnt=5;
		dp[size].lp=lp;
		dp[size].set(rpt);
		double missing_time = m_d(0).m_time/1000.;
		size++;
		for(;size<13;size++)
		{
			dp[size].sit=sit;
			dp[size].dstp.set(dstp);
			dp[size].ct = ct;
			dp[size].valid=true;
			dp[size].stop_cnt=5;
			double mt = missing_time + size;
			double missing_dist = estimate_point_by_history(sit, mt);
			point missing_point = convert_dist_to_pt(missing_dist, sit);

			if(!card_path::inst().is_at_path(missing_point))	//if point not on the path
			{	
				break;
			}
			turning_mapping(missing_point,sit);	//turning

			dp[size].set(missing_point);
			dp[size].lp.set(missing_point);
			dp[size].lp.m_time=(int64_t)(missing_time * 1000);
			dp[size].lp.m_sid = sit->m_id;
			dp[size].lp.m_cid = m_d(0).m_cid;
		}
		{	//don't delete the braces
			atomic_lock lock(mutex);
			m_push_list.clear();
			for(int i=0;i<size;i++)
			{
				m_push_list.push(dp[i]);
			}
		}
	}
	loc_point base_card::grab_loc_point()
	{
		loc_point lp;
		push_data_point dpt;
		{	//don't delete the braces
			atomic_lock lock(mutex);
			if (m_push_list.empty())	//empty
			{
				return lp;
			}
			else if(m_push_list.size()==1)		//size=1
			{
				dpt = m_push_list[0];
				m_push_list[0].valid=false;
				if(m_push_list[0].stop_cnt>5)m_push_list[0].stop_cnt=5;
				if(m_push_list[0].stop_cnt>0)m_push_list[0].stop_cnt--;
			}
			else{			//size>1
				dpt = m_push_list[0];
				m_push_list.skip(1);
			}
		}
		point pt;
		pt.set(dpt);
		
		double current_t=time(NULL);
		if(dpt.valid)
		{
			if (m_card->smoothFlag())
			{
				smooth_dist_car(pt, current_t, dpt.ct, dpt.sit, dpt.dstp, &lp);
				lp.set(lp.m_smooth_x,lp.m_smooth_y);
			}
			else
			{
				smooth_dist_man(pt, current_t, dpt.ct, dpt.sit, dpt.dstp, &lp);
				lp.set(pt);
			}
			dpt.lp.m_dist2=lp.m_dist2;
			dpt.lp.m_time=lp.m_time;
			dpt.lp.m_ct=lp.m_ct;	
			dpt.lp.set(lp);
			dpt.lp.m_dist=dpt.sit->dist_direct(dpt);
			dpt.lp.debug_out();
		}
		else
		{
			//if(dpt.stop_cnt<=0)smooth_line.clear();
			smooth_set_loc_point(current_t, dpt.ct, dpt.sit, &lp);
			if (m_card->smoothFlag())
			{
				lp.set(lp.m_smooth_x,lp.m_smooth_y);
				if(dpt.stop_cnt<=0)
				{
					lp.m_speed=0;
					lp.m_stat=0;
				}
			}
			else
			{
				if(dpt.stop_cnt<=0)
				{
					lp.m_speed=0;
					lp.m_stat=0;
				}
				lp.set(pt);
			}
		}
		return lp;
	}

	void base_card::his_reset()
	{
		his_speed = 0;
		his_speed_presentation = 0;
		his_last_time = 0;
		his_last_pt.set(0, 0);
		his_line.clear();
	}

	std::vector<his_location> base_card::make_up_more_points_for_history(point &pt, uint64_t t,const site * sit)
	{
		//debug_print_syslog(0,"[sit_scale make_up_more_points_for_history]%f",sit->m_scale);
		std::vector<his_location> retv;
		if (sit->is_path_empty())
		{
			return std::move(retv);
		}
		double RESET_SPEED_TIME=2;
		double checkTime=3;	// car card
		double speedChangeLimit=1;
		if(!m_card->smoothFlag())	// man card	// IMPORTANT:uncomment when transplant
		{
			speedChangeLimit=0.5;
			checkTime=5;
		}
		double cur_time = t / 1000.;
		if(his_last_pt.empty() || cur_time - his_last_time > 120)
		{
			if(!his_last_pt.empty())
				retv.push_back(his_location(his_last_pt, uint64_t((his_last_time + RESET_SPEED_TIME)*1000), 0, false)) ;
			his_reset();
			his_last_time = cur_time;
			his_last_pt.set(pt);
			retv.push_back(his_location(pt, t, 0, false));
			return std::move(retv);
		}
		if(his_last_pt==pt)return std::move(retv);

		//line check: get critical point and total length
		double sum_len = 0;
		std::vector<point> path;
		std::vector<double> sec_len; 
		if(his_line.empty() || !his_line.contain(pt,0.1))
		{
			path=card_path::inst().find_path(his_last_pt, pt);
			if(!path.empty() && his_last_pt.dist(path[0])>200)
				path.clear();
			if(path.empty())
			{
				his_line.set(his_last_pt, pt);
				sum_len = his_last_pt.dist_direct(pt);
				sec_len.push_back(fabs(sum_len));
			}
			else
			{
				his_line.set(path.back(), pt);
				point tmp = his_last_pt;
				for(std::vector<point>::iterator i=path.begin();i!=path.end();i++)
				{
					point cur = *i;
					double len = tmp.dist(cur);
					sec_len.push_back(len);
					sum_len += len;
					tmp = cur;
				}
				double len = tmp.dist(pt);
				sec_len.push_back(len);
				sum_len += len;
			} 
		}
		else
		{
			sum_len = his_last_pt.dist_direct(pt);
			sec_len.push_back(fabs(sum_len));
		}

		//update his_speed
		double cur_speed = sum_len / (cur_time - his_last_time);
		double new_his_speed;

		if(fabs(his_speed)<1e-6 || _isnan(his_speed))
		{
			new_his_speed=cur_speed;
			if(new_his_speed>25/(3.6*sit->m_scale))new_his_speed=25/(3.6*sit->m_scale);
			else if(new_his_speed<-25/(3.6*sit->m_scale))new_his_speed=-25/(3.6*sit->m_scale);
		}
		else
		{
			if(!path.empty())
			{
				double end_d = (*path.rbegin()).dist_direct(pt);
				if(fabs(end_d)>=1e-6)
				{
					double speed_sign = fabs(end_d) / end_d;
					cur_speed = fabs(cur_speed) * speed_sign; 
					his_speed = fabs(his_speed) * speed_sign;
				}
			}
			if(cur_time - his_last_time < 4)
			{
				double tmp_cur = cur_speed;
				double speed_differ = fabs(tmp_cur-his_speed);
				if(speed_differ>speedChangeLimit)
					tmp_cur = his_speed + speedChangeLimit*(tmp_cur>his_speed?1:-1);
				new_his_speed = his_speed * 0.4 + tmp_cur * 0.6;
			}
			else
			{
				new_his_speed = cur_speed;
				if(new_his_speed>25/(3.6*sit->m_scale))new_his_speed=25/(3.6*sit->m_scale);
				else if(new_his_speed<-25/(3.6*sit->m_scale))new_his_speed=-25/(3.6*sit->m_scale);
			}
		}

		if(_isnan(new_his_speed))
			new_his_speed=0;

		if(fabs(new_his_speed)<0.1)
		{
			if(his_speed!=0)
			{
				double halt_time = cur_time-his_last_time>RESET_SPEED_TIME?his_last_time+RESET_SPEED_TIME:cur_time;
				retv.push_back(his_location(his_last_pt, uint64_t(halt_time*1000), 0, false));
			}
			his_last_time = cur_time;
			his_speed = 0;
			return std::move(retv);
		}

		//speed check
		double speed;
		if(fabs(his_speed) > fabs(cur_speed) * 1.5 && cur_time - his_last_time > checkTime)
			speed = fabs(his_speed);
		else
			speed = fabs(cur_speed);
		
		//insert vector
		point tmp_pt = his_last_pt;
		double tmp_t = his_last_time;
		std::vector<double>::iterator len_i=sec_len.begin();
		for(std::vector<point>::iterator path_i=path.begin();path_i!=path.end();path_i++,len_i++)
		{
			double len=*len_i;
			point tmp_pt2 = *path_i;
			double tmp_t2 = tmp_t + len / speed;
			if(tmp_t2 - tmp_t > checkTime)
			{
				retv.push_back(his_location(tmp_pt, uint64_t(tmp_t*1000), 
					tmp_pt2, uint64_t(tmp_t2*1000), new_his_speed*(3.6*sit->m_scale),false));
			}
			retv.push_back(his_location(tmp_pt2, uint64_t(tmp_t2*1000), new_his_speed*(3.6*sit->m_scale),true));

			tmp_pt = tmp_pt2;
			tmp_t = tmp_t2;
		}
		double len=*len_i;
		double end_time_sec = tmp_t + len / speed;
		if(end_time_sec - tmp_t > checkTime)
		{
			retv.push_back(his_location(tmp_pt, uint64_t(tmp_t*1000), 
				pt, uint64_t(end_time_sec*1000), new_his_speed*(3.6*sit->m_scale),false));
		}
		retv.push_back(his_location(pt, uint64_t(end_time_sec*1000), new_his_speed*(3.6*sit->m_scale),false));

		if(cur_time - end_time_sec > checkTime)
			retv.push_back(his_location(pt, uint64_t((end_time_sec + RESET_SPEED_TIME)*1000), 0,false));

		//update history
		his_last_time = cur_time;
		his_last_pt.set(pt);
		his_speed = new_his_speed;
		return std::move(retv);
	}


NAMESPACE_POINT_END(NAMESPACE_POINT)
/*
std::unique_ptr<sit_list> sites(new sit_list());
inline const char* now(char*date_str,uint64_t time)
{
	time_t ntime=time/1000;
	struct tm buff;
	const struct tm*t=localtime_r(&ntime, &buff);

int main()
{
	sites->load("data_reader_antenna.txt","dat_reader_path_tof.txt");

	card_path::init(*sites);

	printf("%s\n",sites->to_string().c_str());

	fflush(stdout);
#ifdef _SMOOTH
	message_file mf(stdin);	//../raw_s_20171201.log.02
#else
	message_file mf(stdin);
#endif

	uint64_t ms;
	char buf[2048];
	std::unique_ptr<std::array<card,2000>> cards(new std::array<card,2000>());
	for(int i=0,len=cards->size();i<len;i++)
		cards->at(i).m_id=i;

	int len;
	while((len=mf.get_line(&ms,buf,sizeof(buf))))
	{
		loc_package lp(ms,buf);
		for(int i=0,ct=lp.count();i<ct;i++)
		{
			loc_info li=lp.get(i);
			if(li.m_card_type!=2) continue;

//			if(li.m_card_id!=1222) continue;

			card&c=cards->at(li.m_card_id);

			printf("t=%ld,sit=%d,ant=%d,card=%d,ct=%d,tof=%d,rav=%d,acc=%d,rsp=%.1f\n"
					,li.m_loc_time,lp.m_sit_id, li.m_ant_id, c.m_id ,li.m_card_ct,li.m_tof
					,li.m_rav,li.m_acc,li.m_card_sp );

			c.on_tof_data(&(*sites)[lp.m_sit_id],li);
		}
	}

	return 0;
}
*/