server/select_tool.h

#ifndef __SELECT_TOOL__H
#define __SELECT_TOOL__H
#include "loc_point.h"
#include "line.h"
#include <memory>
#include "ant.h"
#include "card_path.h"
class loc_message;

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 select_point_object
{
public:
    select_point_object()
        :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;
	struct push_data_point:point
	{
		const site *sit;
		point dstp;
		int ct;
		bool valid;	// if valid
		int stop_cnt;	// if calculate speed
		loc_point lp;
	};
	zlist<push_data_point,16> m_push_list;

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()
        :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)
    {
        point dstp = sit->get_dstp(pt);
        if(dstp.empty())
            std_error("error.here....");

		push_data_point dp;
		dp.sit=sit;
		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
			m_push_list.clear();
			m_push_list.push(dp);
		}
        return true;
    }

};
struct car_point_filter:select_point_object
{
    car_point_filter()
        :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.set(sit->x,sit->y);
        sit_location = sit->get_dstp(m_turning_pt);
        if(sit_location.empty())
          std_error("get_dstp point error.....");
		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)
	{
		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())
              std_error("error.here....");
		}
		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
			m_push_list.clear();
			m_push_list.push(dp);
		}
	}
	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();
			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())
              std_error("error.here....");
			//dstp.set(sit->x,sit->y);
		}
		else
		{
			dstp.set(m_turning_pt);
		}

		int size = 0;
		push_data_point dp[10];
		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<10;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;
		}
        {
			m_push_list.clear();
			for(int i=0;i<size;i++)
			{
				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];		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)
    {
        std_info("revise_____-before:%f,%f,%llu",pt.x,pt.y,timestamp);
        bool flag =false;
		if(m_line.empty() || !m_line.contain(m_d(0),0.1))
		{
			std_info("m_line .empty() ......");
			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)
		  std_info("fit != nullptr...m_time_sec:%f,fitK.x():%f",m_time_sec,m_fitk.x(0));
		else 
		  std_info("fit ==nullptr...");
		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)
		{
			std_info("here ....m_time_sec:%llu,m_last_fit_time_sec:%llu",m_time_sec,m_last_fit_time_sec);
			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;
        std_info("revise_____-end:%f,%f,useless:%s",pt.x,pt.y,flag?"true":"false");


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

};
struct select_tool
{
    select_point_object *m_spo=nullptr;
    virtual loc_point  select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)=0;
    virtual ~select_tool()
    {
        if(m_spo !=nullptr)
          delete m_spo;
    }
};

//--------------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;
    }
};
//---------------------smooth
struct smooth_tool
{
    std::shared_ptr<select_tool> m_st=nullptr;
    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;	//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::shared_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 smooth_strategy() = 0;
	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::shared_ptr<select_tool> m)
        :smooth_tool(m)
    {}
    virtual void smooth_strategy(){}
	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
		{
            printf("lemon2");
			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::shared_ptr<select_tool> m)
        :smooth_tool(m)
    {}
    virtual void smooth_strategy(){}
	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();
					printf("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];
						printf("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::shared_ptr<select_tool> m)
        :smooth_tool(m)
    {}
    virtual void smooth_strategy()
    {}
	virtual void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr){}
};
//---------------------------------
struct select_tool_manage
{
   void create_tool(const std::string &s,std::shared_ptr<select_tool> &set,std::shared_ptr<smooth_tool> &smt)
   {
        if(!s.compare(std::string{"person1"}))
        {
            set=std::make_shared<select_tool_person_1>();
            smt=std::make_shared<smooth_tool_person_1>(set);
        }
        else if(!s.compare(std::string{"person2"}))
        {
            set=std::make_shared<select_tool_person_2>();
            smt=std::make_shared<smooth_tool_person_1>(set);
        }
        else if(!s.compare(std::string{"car1"}))
        {
            set=std::make_shared<select_tool_car_1>();
            smt=std::make_shared<smooth_tool_car_1>(set);
        }
        else if(!s.compare(std::string{"drivingface1"}))
        {
            set=std::make_shared<select_tool_drivingface_car_1>();
            smt=std::make_shared<smooth_tool_drivingface_car_1>(set);
        }
   }
   static select_tool_manage * instance();
};

#endif