#include "select_tool.h"
#include "ant.h"
#include "card_path.h"
#include "log.h"
void select_point_object::att_initiate()
{
	m_fitk.add_tool(30,5,5);
	m_fitk.add_tool(30,5,6);
	m_fitk.add_tool(30,10,10);
	m_fitk.add_tool(30,10,15);

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


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

}
loc_point select_point_object::select_solution_impl(const std::vector<point> vp,const std::vector<loc_message>&lm)
{
    bool flag=false;
    loc_point lp;
    if(vp.size()==4)
    {
        m_d.grow().reset().set_source(lm[0],lm[1]);
        flag = true;
    }
    if(vp.size()==2 && lm[0].m_card_ct - last_ct() == 1) 
    {
        m_d.grow().reset().set_source(lm[0]);
        flag = true;
    }
    if(flag)
    {
        if(!select_solution(vp,lm[0].m_sit.get(),lp))
        {
            m_cur_fit.reset();
            return lp;
        }
    }
    m_ct = lm[0].m_card_ct;
    return lp;
}

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

	return -1;
}

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

	return -1;
}
bool select_point_object::filter_by_fit(loc_point & c,const std::vector<point> & vp,const double scale)
{
    fit_result * fit = get_best_fit();
	if(fit==nullptr || m_begin==nullptr || fit->ke>2)
		return false;
    loc_point &f = *m_begin;
	std::array<solpoint,4> v;
    int cnt = vp.size();
	for(int i=0;i<cnt;i++)
	{
		v[i].set_sol(vp[i],fabs(fit->testk(c.m_time/1000.,f.dist_direct(vp[i]))));
	}
    std::sort(&v[0],&v[0]+cnt);
    double a = getA(fit,scale,v[1].score()-v[0].score());  
    if(a<0)
      return false;
	if(!filter_by_acc(c,v,a))
	{
		c.set_cl(0);
		return true; //false?
	}
    reset_fit(vp[0].dist(v[0])); 
    c[0]=v[0];
    c[1]=v[1];
    return true;

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

	double td=m_last->time_off(c);

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

	return true;
}


void select_point_object::select_one_ant(loc_point &c,const std::vector<point> & vp)
{
	int last=find_last(1);
	if(last>0)
	{
		loc_point&p=m_d(last);

		int cnt=vp.size();
		std::array<solpoint,4> res;

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

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

		c[1].set(res[1]);
		c[0].set(res[0]);
		c.inc_cl(5);
    }
}
 
point select_point_object::select_solution0(std::vector<point> &tvp,const double scale)
{
   //log_info("lemon test 2 :cardid:%d sit:%f sitid:%d",m_d(0).m_cid,scale,m_d(0).m_sid);
    loc_point&c=m_d(0);
	if(m_d.size()==1)
	{
		//first point ,only accpet two ants good data.
		if(c.cl()>0 && tvp.size()<4)
		{
			c[1]=tvp[1];
			return c[0]=tvp[0];
		}

		m_d.skip(1);
		return point(0,0);
	}
    select_solution1(c,tvp,scale);
    return c[0];
}
bool select_point_object::select_solution(const std::vector<point> &vp,const site*sit,loc_point &p)
{
   //log_info("lemon test 1 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
   remove_history(); 
   std::vector<point> tvp(vp.begin(),vp.end());
   if(vp.size()==4)
   {
       int c=0;
       std::array<solpoint,4> res;
       for(int i=0;i<2;i++)
	   {
            int x = i+2;
			double d=vp[i].dist(vp[x]);
			if(d<sit->ant_dist()*3)
			{
				res[c++].set_sol(vp[i].middle(vp[x]),d);
			}
			else
			{
				res[c++].set_sol(vp[i]);
				res[c++].set_sol(vp[x]);
			}
		}

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

        tvp.clear();
		for(int i=0;i<c;i++)
          tvp.push_back(res[i]);

		m_d(0).inc_cl(10);
   }
   point pt = select_solution0(tvp,sit->m_scale);
	if(pt.empty() || m_d.empty())
		return false;
	m_d(0).set(pt);
    if(!m_d(0).empty())
		m_d(0).m_dist=sit->dist_direct(pt);
	else
	{
		m_d(0).set_cl(0);
		if(m_last) m_d(0).m_dist=0.01*m_last->m_dist>0?1:-1;
	}

    //
	//std_info("revise_by_history:::%llu",m_d(0).m_time);
    bool fg=revise_by_history(pt,sit,m_d(0).m_time);
	if(!fg)
	{
		log_warn("out of site path:t=%ld,sit=%d,card_id=%d,ct=%d,tof1=%d,tof2=%d,pt=(%f,%f)\n", m_d(0).m_time, m_d(0).m_sid,m_d(0).m_cid,m_d(0).m_ct, m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
	
	}

    if(!card_path::inst().is_at_path(pt))
	{
		m_d(0).set_cl(0);

		log_warn("out of path:t=%ld,sit=%d,card_id=%d,ct=%d,tof1=%d,tof2=%d,pt=(%f,%f)\n", m_d(0).m_time, m_d(0).m_sid,m_d(0).m_cid,m_d(0).m_ct, m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
		return false;
	}


	m_last=&m_d(0);

	if(m_line.empty() && m_d.size()>=2 && !make_line())
		return false;
    if(!m_line.contain(m_d(0),0.01) || (m_begin && !m_line.contain(m_begin->m_sol[0], 0.01)))
    {
		m_fitk.reset_data();
		m_fita.reset_data();
		m_begin=m_last=nullptr;

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

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

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

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

		m_line.set(path.back(),m_d(0));
	} 
    if(!m_begin)
	{
		int idx=find_first(0);
		if(idx>=0) m_begin=&m_d[idx];
	}

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

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

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

   p.set(pt);
   save_k();
   p.m_useless=fg;

   return true;

}

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

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

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

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

	return true;
}

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

	if(m_d.size()>120 || (m_d.size()>2 && m_d(1).time_off(b)>max_histime))
	{
		m_d.skip_if([&b,this](loc_point&p){ return p.time_off(b)>max_histime;});
		m_fitk.reset_data();
		m_fita.reset_data();
		m_cur_fit.reset();
		m_begin=m_last=nullptr;
		int idx=find_first(0);
		if(idx<0) 
			return;
		m_begin=&m_d[idx];
		idx=find_last(1);
		m_last=&m_d(idx);

		double dist=0,dist2=0;
		for(int len=std::min(15,m_d.size()-1),i=len;i>0;i--)
		{
			if(!m_d(i).cl())
				continue;
			int lastID1=find_last(i+1);
			int lastID2=-1;
			if(lastID1!=-1)
				lastID2=find_last(lastID1+1);
			if(lastID2!=-1)
			{
				double t0=m_d(i).m_time/1000., t1=m_d(lastID1).m_time/1000., t2=m_d(lastID2).m_time/1000.;
				double d0=m_begin->loc_dist(m_d(i)[0]), d1=m_begin->loc_dist(m_d(lastID1)[0]), 
														d2=m_begin->loc_dist(m_d(lastID2)[0]);
				double k1=(d1-d0)/(t1-t0), k2=(d2-d1)/(t2-t1);
				if(t0-t1<5 && t1-t2<5 && fabs(k2-k1)<0.5)
				{
					double tbegin = t0-3;
					while(tbegin<t1+0.5)
						tbegin=tbegin+1;
					double tk=(d0-d1)/(t0-t1), tb=d1-tk*t1;		//d1+(d0-d1)/(t0-t1)*(t-t1)
					for(double ti=tbegin;ti<t0;ti=ti+1)
					{
						//std_info("add remove..");
						m_fitk.add(ti, tk*ti+tb);
					}
				}
			}

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

			if(i==len)
				continue;

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

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

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

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


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

	start=std::max(start,0);
	last  =std::min(last,m_fitk.tool_size());
	if(last==-1)
		last=m_fitk.tool_size();
	//std_info("best_fit_raw :%d:%d",start,last);
	for(int i=start;i<last;i++)
	{
		if(!m_fitk[i].is_valid())
			continue;

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

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

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

	return fit;
}

select_tool::~select_tool()
{
	if(m_spo !=nullptr)
	  delete m_spo;
}

loc_point select_tool_person_1::select_solution(const std::vector<point> vp,const std::vector<loc_message>&lm)
{
    loc_point lp;
    //select point.
    if(lm[0].tool_index() == 0)  
    {
		if(m_spo==nullptr)
			m_spo = new person_point_filter(this); 
        lp=m_spo->select_solution_impl(vp,lm);
    }
    else if (lm[0].tool_index() == 2)
    {
        //for now..
        //m_spo = new person_point_filter(); 
        lp=m_spo->select_solution_impl(vp,lm);
    }
    else
    {}
    return lp;
}

loc_point select_tool_car_1::select_solution(const std::vector<point> vp,const std::vector<loc_message>&lm)
{
    loc_point lp;
    //select point.
    if(lm[0].tool_index() == 0)  
    {
		if(m_spo==nullptr)
			m_spo = new car_point_filter(this); 
        lp=m_spo->select_solution_impl(vp,lm);
    }
    else if (lm[0].tool_index() == 2)
    {
        //for now..
        //m_spo = new car_point_filter(); 
        lp=m_spo->select_solution_impl(vp,lm);
    }
    else
    {}
    return lp;
}


select_tool_manage * select_tool_manage::instance()
{
    static select_tool_manage stm;
    return &stm;
}