mine_server/algo/select_tool.h

#ifndef __SELECT_TOOL__H
#define __SELECT_TOOL__H
#include <mutex>
#include "loc_point.h"
#include "line.h"
#include <memory>
#include "ant.h"
#include "card_path.h"
#include "loc_message.h"
#include <zexception.h>
//#include "optimize_tool.h"

struct solpoint:point
{
	solpoint()
		:m_score(100)
	{

	}
	double m_score;

	bool operator<(const solpoint&p)const
	{
		return m_score<p.m_score;
	}

	void set_sol(const point&p,double score=100)
	{
		set(p);
		this->m_score=score;
	}

	double score()const
	{
		return m_score;
	}
};

struct push_data_point:point
{
	push_data_point()
		:ct(0)
		,valid(false)
		,stop_cnt(0)
	{

	}
	point dstp;
	int ct;
	bool valid;	// if valid
	int stop_cnt;	// if calculate speed
	loc_point lp;
};

class select_point_object;
struct select_tool
{
	std::mutex m_mtx;
	zlist<push_data_point,16> m_push_list;
	select_point_object *m_spo=nullptr;

    virtual loc_point  select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)=0;
	push_data_point getDpt()
	{
		push_data_point dpt;
		std::lock_guard<std::mutex> lock(m_mtx);
		if (m_push_list.empty())	//empty
		{
			return dpt;
		}
		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);
		}
		return dpt;
	}
	virtual ~select_tool();

};

//--------------person------solution one--------
struct select_tool_person_1:select_tool
{
	virtual loc_point  select_solution(const std::vector<point> p,const std::vector<loc_message>&lm);
	~select_tool_person_1()
	{
	}
};

struct select_tool_person_2:select_tool
{
	virtual loc_point  select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)
	{
		loc_point lp;
		return lp;
	}

};
//----------------------car----------
struct select_tool_car_1:select_tool
{
	virtual loc_point  select_solution(const std::vector<point> p,const std::vector<loc_message>&lm);
};
//---------------------drivingfaceCar
struct select_tool_drivingface_car_1:select_tool
{
	virtual loc_point  select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)
	{
		loc_point lp;
		return lp;
	}
};
//----------------------------------------
struct select_point_object
{
	select_tool * m_owner;
	public:
		select_point_object(select_tool * owner)
			:m_owner(owner)
			,m_ct(-10)
		{
			att_initiate();
		}
		// 上次ct号
		int last_ct()
        {
            return m_ct;
        }
public:
		const static int max_histime=60;		 //±£Áô×îºó60sµÄÊý¾Ý

		zlist<loc_point,128> m_d;
		line m_line;
		int	 m_ct = -10;	// 计数号
		fit_batch   m_fitk,m_fita;
		fit_result  m_cur_fit;	// 最新拟合结果
		loc_point*  m_begin;	// 起始点
		loc_point*  m_last;		// 结束点
		//static opt_tool_main m_opt_tool;

public:
		int find_last(int start);
		int find_first(int start);
		void save_k();
		void att_initiate();
		void remove_history();
		bool make_line();
		point select_solution0(std::vector<point> &vp,const double scale);
		bool select_solution(const std::vector<point> &vp,const site*sit,loc_point &p);
		loc_point  select_solution_impl(const std::vector<point> p,const std::vector<loc_message>&lm);
		bool select_pdoa_solution_impl(const std::vector<point> p, const std::vector<loc_message>& lm, loc_point& lp);
		bool select_pdoa_solution(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point&lp, const float& pdoa);
		bool select_pdoa_solution2(const std::vector<point> vp, const std::vector<loc_message>& lm, loc_point&lp, const float& pdoa);
		bool select_pdoa_1d_solution(const std::vector<point> p, const std::vector<loc_message>& lm, loc_point& lp);
		bool select_pdoa_2d_solution(const std::vector<point> p, const std::vector<loc_message>& lm, loc_point& lp);
		bool select_pdoa_3d_solution(const std::vector<point> p, const std::vector<loc_message>& lm, loc_point& lp);
		bool select_pdoa_1d_solution_original(const std::vector<point> p, const std::vector<loc_message>& lm, loc_point& lp);
		fit_result* best_fit_raw(int num_point=0,int start=0,int end=-1);
		bool filter_by_acc(loc_point&c,std::array<solpoint,4>&v,double a);
		bool filter_by_fit(loc_point & c,const std::vector<point> & vp,const double scale);
		void select_one_ant(loc_point &c,const std::vector<point> & vp);
		int select_max_solution(const float& lhs, const float& rhs, std::vector<point> vp, loc_point& lp);
		int select_min_solution(const float& lhs, const float& rhs, std::vector<point> vp, loc_point& lp);
		float get_pdoa(const double poa[], const double& offset);
		double get_speed(const loc_point& p, const point& lp, const double& offset);
		double get_distance(const int& rid, const int& lrid);
private:
		int get_algo_index(const int& idx);
public:
		virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)=0;
		virtual fit_result * get_best_fit()=0;
		virtual double getA(const fit_result * fit,const double scale,const double dt)=0;
		virtual void reset_fit(double d)=0;
		virtual bool revise_by_history(point & pt, const site*sit, int64_t m_time)=0;
		virtual ~select_point_object(){}
};

/*
 * 人员定位坐标过滤类
 * 
 * */
struct person_point_filter:select_point_object
{
	person_point_filter(select_tool * owner)
		:select_point_object(owner)
		 ,m_filter_man_count(0)
	{}
	int m_filter_man_count=0;	// 过滤计数
	// 重置拟合
	virtual void reset_fit(double d)
	{
		if(d>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;
		}

	}
	
	virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)
	{
		if(filter_by_fit(c,vp,scale))
		{
			c.inc_cl(40);
		}
		else if(c.cl()>0 && vp.size()==2)
		{
			c[0]=vp[0];
			c[1]=vp[1];
			c.inc_cl(10);
		}
		else
		{
			select_one_ant(c,vp);
		}
		loc_point &refc=m_d(1);
		if(c.m_type==4 &&  c.cl()<=20 && refc.is_same_site(c) && refc.time_off(c)<5 ){    
			logn_info(3,"{test}first point:site=%d,card=%d,(%f,%f) else:(%f,%f),cred:%d,size:%d",c.m_sid,c.m_cid,c[0].x,c[0].y,c[1].x,c[1].y,c.cl(),vp.size());
			if(const auto & site_ptr=sit_list::instance()->get(c.m_sid)){
				double d1=0,d2=0,d3=0;
				d1=refc.m_dist1;
				d2=site_ptr->dist_direct(c[0]);
				d3=site_ptr->dist_direct(c[1]);
				if(d1*d2<0 && d1*d3>0){
					logn_info(3,"{test}first point:site=%d,card=%d,(%f,%f) else:(%f,%f),cred:%d,size:%d",c.m_sid,c.m_cid,c[0].x,c[0].y,c[1].x,c[1].y,c.cl(),vp.size());
					point t=c[0];
					c[0]=c[1];
					c[1]=t;
					c.set_cl(0);
				}
				else if(d1*d2<0 && d1*d3<0)
					c[0].set(refc);
			}
		}    
	}

	virtual fit_result * get_best_fit()
	{
		return best_fit_raw(4,4);
	}
	
	virtual double getA(const fit_result * fit,const double scale,const double dt)
	{
		double a=2.5;
		if(fabs(fit->k)<2/(3.6*scale)  && fabs(fit->k)>0.5/(3.6*scale)  && dt<10)
		{
			a=0.3;
		}
		return a;
	}

	virtual bool revise_by_history(point & pt, const site*sit, int64_t timestamp)
	{
		if(!card_path::inst().is_at_path(pt))	//if point not on the path
			return false;

		point dstp = sit->get_dstp(pt);
		if(dstp.empty())
		{
			log_warn("person.dstp empty,");
		}

		push_data_point dp;
		dp.dstp.set(dstp);
		dp.ct       = m_d(0).m_ct;
		dp.valid    = true;
		dp.stop_cnt = 5;
		dp.lp       = m_d(0);
		dp.set(pt);
		{	//don't delete the braces
			std::lock_guard<std::mutex> lock(m_owner->m_mtx);
			m_owner->m_push_list.clear();
			m_owner->m_push_list.push(dp);
			//dp.lp.debug_out("person");
		}

		return true;
	}
};

struct car_point_filter:select_point_object
{
	car_point_filter(select_tool * owner)
		:select_point_object(owner)
		 ,m_last_fit_valid(false)
		 ,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_time_sec(0)
		 ,m_last_fit_nearest_time_sec(0)
		 ,m_last_time_sec(0)
		 ,m_if_turning(false)
	{}
	bool m_last_fit_valid = false;
	double m_last_fit_k ;
	double m_last_fit_kb;
	double m_last_fit_ka;
	double m_last_fit_xo ;
	double m_last_fit_yo;
	double m_last_fit_md_x;
	double m_last_fit_md_y;

	double m_last_fit_time_sec;	//x(0) of latest valid fit
	double m_last_fit_nearest_time_sec;
	double m_last_time_sec=0;
	bool m_if_turning=false;
	point m_turning_pt;
	point m_turning_ept;
	double m_simple_rec_kx,m_simple_rec_ky;
	const double m_fit_differ=4;
	const double m_pos_differ=8;

	virtual void reset_fit(double d){}
	virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)
	{
		//two ants.
		if(c.cl()>0 && vp.size()==2)//m_card->smoothFlag() )
		{
			c[0]=vp[0];
			c[1]=vp[1];
			c.inc_cl(50);
		}
		else if(filter_by_fit(c,vp,scale))
		{
			c.inc_cl(40);
		}
		else
		{
			select_one_ant(c,vp);
		}
	}

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

		return &m_cur_fit;
	}

	virtual fit_result * get_best_fit()
	{
		return best_fit_raw(5);
	}
	
	virtual double getA(const fit_result * fit,const double scale,const double dt)
	{
		double a=2.5;
		if(fabs(fit->k)<10/(3.6*scale)  && fabs(fit->k)>1/(3.6*scale)  && dt<20)
		{
			a=1;
		}
		if(fabs(fit->k)<=1/(3.6*scale)  && dt<20)
		{
			a=-1;
		}
		return a;
	}

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

	void 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 turning_mapping(point &rpt,const site*sit)
	{
		if(!m_if_turning)return;

		point sit_location;		//projection
		sit_location = sit->get_dstp(m_turning_pt);
		if(sit_location.empty())
		{
			log_error("%d get_dstp point error.....",sit->m_id);
		}
		double dist1=sit_location.dist(rpt);
		double dist2=sit_location.dist(m_turning_pt);
		double dist3=m_turning_pt.dist(rpt);	// dist1 is supposed to be = dist2+dist3
		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;

		rpt.set(turning_x,turning_y);
	}

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

		point dstp;		//projection
		if(!m_if_turning)
		{
			//dstp.set(sit->x,sit->y);
			dstp = sit->get_dstp(pt);
			if(dstp.empty())
			{
				log_error("error.here....here.");
			}
		}
		else
		{
			dstp.set(m_turning_pt);
		}

		push_data_point dp;
		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
			std::lock_guard<std::mutex> lock(m_owner->m_mtx);
			m_owner->m_push_list.clear();
			m_owner->m_push_list.push(dp);
			//dp.lp.debug_out("single point .");
		}
	}

	/* 根据历史数据估计定位点
	 *
	 * param
	 *	const site* sit		分站
	 *	double m_time_sec	秒数
	 * return
	 *	
	 *
	 */
	double estimate_point_by_history(const site*sit, double m_time_sec)
	{
		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);

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

	/*
	 * 转换距离为坐标点
	 * */
	point 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];
		return point(f.m_sol[0].x + dist * m_simple_rec_kx, f.m_sol[0].y + dist * m_simple_rec_ky);
	}

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

		point dstp;		//projection
		if(!m_if_turning)
		{
			dstp = sit->get_dstp(pt);
			if(dstp.empty())
				log_error("dstp.empty()..");
		}
		else
		{
			dstp.set(m_turning_pt);
		}

		int size = 0;
		push_data_point dp[13];
		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].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;
		}
		{
			std::lock_guard<std::mutex> lock(m_owner->m_mtx);
			m_owner->m_push_list.clear();
			for(int i=0;i<size;i++)
			{
				m_owner->m_push_list.push(dp[i]);
			}
		}
	}

	/*
	 * 转换点为距离
	 * */
	double 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.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;
		}
		return dist;
	}

	virtual bool revise_by_history(point & pt, const site*sit, int64_t timestamp)
	{
		bool flag =false;
		if(m_line.empty() || !m_line.contain(m_d(0),0.1))
		{
			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;

			reset_turning();

			generate_list(pt, sit, false);

			return true;
		}

		// convert pt to distance
		double dist = convert_pt_to_dist(pt, sit);
		double m_time_sec = timestamp / 1000.;	//second
		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)
		{	
			// 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 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;
				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_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;
			}
		}

		// revise
		double estimate_dist = estimate_point_by_history(sit, m_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 flag = true;
		}
		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_accumulate_acc<-10)generate(mpt, sit,false); generate single point
		if(m_last_fit_valid && timestamp/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);

		pt = mpt;

		return flag; 
	}

	// 检测掉头或转弯
	void detect_turning(point &mpt, const site*sit)
	{
		if(m_if_turning)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];

			// get map angle
			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;
			// turning angle must be correct
			if(angle*delta>0 && fabs(angle)>fabs(delta)*detect_para)
			{
				log_info("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 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;
	}

};

//---------------------smooth
struct smooth_tool
{
	std::unique_ptr<select_tool>& m_st;	// 选择工具类
	bool smooth_initial_setting;		// 平滑初始化状态
	double smooth_speed;				// 平滑速度
	double smooth_speed_presentation;
	int smooth_speed_presentation_cnt;
	point smooth_last_position;			// 上一个平滑位置
	double smooth_last_time_sec;		// 上一个平滑时间
	point smooth_last_true_position;	// 上一个真实位置

	line smooth_line;					// 平滑线
	bool smooth_line_reset=false;	//if line reset

	bool smooth_halt_condition;	//if halting
	int smooth_halt_count;	//halting count
	point smooth_halt_position;	//position while begin halting
	point smooth_halt_position_plus;
	point smooth_halt_position_minus;

	smooth_tool()=default;
	smooth_tool(std::unique_ptr<select_tool>&st)
		:m_st(st)
		 ,smooth_initial_setting(false)
		 ,smooth_speed(0)
		 ,smooth_speed_presentation(0)
		 ,smooth_speed_presentation_cnt(0)
		 ,smooth_last_time_sec(0)
		 ,smooth_halt_condition(0)
		 ,smooth_halt_count(0)
	{}
	
	void 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(std::isnan(lp->m_speed))
				lp->m_speed=0;
			lp->m_stat=1;
		}
	}

	void smooth_reset()
	{
		smooth_initial_setting = false;
		smooth_speed = 0;							//smoothed speed
		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 smooth_initiate(point &pt, double t, const site*sit)
	{
		smooth_initial_setting=true;
		smooth_speed=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;
	}
	
	virtual void set(point &pt,loc_point &lp)=0;
	loc_point smooth_strategy()
	{
		loc_point lp;
		push_data_point dpt = m_st->getDpt();
		if(dpt.empty())	
			return lp;
		point pt;
		pt.set(dpt);
		double current_t = time(NULL);
		const auto & sit = sit_list::instance()->get(dpt.lp.m_sid);
		if(dpt.valid)
		{
			smooth_dist(pt, current_t, dpt.ct, sit.get(), dpt.dstp, &lp);
			set(pt,lp);

			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 = sit->dist_direct(dpt);
			dpt.lp.m_speed = lp.m_speed;
			dpt.lp.m_stat = lp.m_stat;
			dpt.lp.debug_out("get_point");
		}
		else
		{
			smooth_set_loc_point(current_t, dpt.ct, sit.get(), &lp);
			if(dpt.stop_cnt<=0)
			{
				lp.m_speed = 0;
				lp.m_stat = 0;
			}
			set(pt, lp);
		}
		return lp;
	}
	virtual void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr)=0;
	virtual ~smooth_tool(){}
};

struct smooth_tool_person_1:smooth_tool
{
	smooth_tool_person_1(std::unique_ptr<select_tool>&m)
		:smooth_tool(m)
	{}
	virtual void set(point &pt,loc_point &lp)
	{
		lp.set(pt);
	}
	void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr)
	{
		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 || std::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);
		}

	}
};

struct smooth_tool_car_1:smooth_tool
{
	smooth_tool_car_1(std::unique_ptr<select_tool>&m)
		:smooth_tool(m)
	{}
	virtual void set(point &pt,loc_point &lp)
	{
		lp.set(lp.m_smooth_x,lp.m_smooth_y);
	}
	void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr)
	{
		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();
					log_info("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];
						log_info("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.x, 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;
			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;
					}
				}
				else
				{
					if(fabs(smooth_speed)<1e-6 || std::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 || std::isnan(smooth_speed_presentation))
					{
						smooth_speed_presentation=fabs(smooth_speed);
					}
					else
						smooth_speed_presentation = smooth_speed_presentation * 0.4 + fabs(smooth_speed) * 0.6;

					// 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);
		}

	}
};

struct smooth_tool_drivingface_car_1:smooth_tool
{
	smooth_tool_drivingface_car_1(std::unique_ptr<select_tool>&m)
		:smooth_tool(m)
	{}
	virtual void set(point&p,loc_point&lp){}
	virtual void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr){}
};
//---------------------------------

DEF_EXCEPTION (no_tool);
struct select_tool_manage
{
	void create_tool(const std::string &s,std::unique_ptr <select_tool> &set,std::unique_ptr <smooth_tool> &smt)
	{
		if(!s.compare(std::string{"PS_1"}))
		{
			set.reset(new select_tool_person_1());
			smt.reset(new smooth_tool_person_1(set));
		}
		else if(!s.compare(std::string{"PS_2"}))
		{
			set.reset(new select_tool_person_2());
			smt.reset(new smooth_tool_person_1(set));
		}
		else if(!s.compare(std::string{"CS_1"}))
		{
			set.reset(new select_tool_car_1());
			smt.reset(new smooth_tool_car_1(set));
		}
		else if(!s.compare(std::string{"WS_1"}))
		{
			set.reset(new select_tool_person_1());
			smt.reset(new smooth_tool_person_1(set));
		}
		else
		{
			THROW_EXCEPTION(no_tool,"未定义%s的策略,需要在config.ini中定义",s.c_str());
		}
	}
	static select_tool_manage * instance();
};

#endif