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

	}
	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)
	{
		//log_info("lemon test revise preson:cardid:%d sit:%d sitid:%d  %0X",m_d(0).m_cid,sit->m_id,m_d(0).m_sid,sit);
		if(!card_path::inst().is_at_path(pt))	//if point not on the path
          return true;

		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)
	{
		
		//log_info("lemon test 4 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
		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.set(sit->x,sit->y);
		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;


		//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 put_single_loc_point(point &pt, const site*sit, int ct, loc_point &lp)
	{
		//log_info("lemon test 6 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
		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=l,ct=%d,"
		//				"tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
		//				m_d(0).m_time, m_d(0).m_sid,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)
		{
			//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 .");
		}
	}
	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)
	{
		//log_info("lemon test 5 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
		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=0,ct=%d,"
		//				"tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
		//				m_d(0).m_time, m_d(0).m_sid,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)
		{
			dstp = sit->get_dstp(pt);
			if(dstp.empty())
			  log_error("dstp.empty()..");
			//dstp.set(sit->x,sit->y);
		}
		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;
			//dp[size].lp.m_cid = m_d(0).m_cid;
		}
		{
			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]);
				//dp[i].lp.debug_out("push_list");
			}
		}
	}

	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];		
			//printf("find_first:(%.2f,%.2f)(%.2f,%.2f)\n",f.m_sol[0].x,f.m_sol[0].y,pt.x,pt.y);
			//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;
	}

	virtual bool revise_by_history(point & pt, const site*sit, int64_t timestamp)
	{

		//log_info("lemon test 3 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
		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_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_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;
			}

			// update acc
			//m_accumulate_acc=0

			//printf("change line------------, k=%f,ke=%f\n",fit->k,fit->ke);
		}

		// revise
		double estimate_dist = estimate_point_by_history(sit, m_time_sec);
		//printf("revise:est:%f, d:%f, fitvalid:%d, timesecdiffer:%f\n",
		//estimate_dist, dist, m_last_fit_valid, 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 flag = true;
			//m_last_fit_nearest_time_sec = m_time_sec;	//need more tests to uncomment this sentence
		}
		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;

		//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];

			// 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;
			//printf("angle:%f, delta:%f. mul:%f\n",angle, delta, delta*detect_para);
			// 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;
			//if(fabs(smooth_speed_presentation) < 0.1)
			//	lp->m_speed=0;
		}
	}

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

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

    }
};
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));
			//set.reset(new select_tool_drivingface_car_1());
			//smt.reset(new smooth_tool_drivingface_car_1(set));
        }
		else
		{
			THROW_EXCEPTION(no_tool,"未定义%s的策略,需要在config.ini中定义",s.c_str());
		}
   }
   static select_tool_manage * instance();
};

#endif