server/pointSmooth/car.h

#ifndef __CAR_HPP__
#define __CAR_HPP__

#include "pointSmooth/point.h"
#include "zlist.h"
#include "zstream.h"
#include "message_file.h"
#include "message.h"
#include "ant.h"
#include "loc_point.h"
#include "line_fit.h"
#include "line.h"
#include "card_path.h"

#include <stdio.h>
#include <list>
#include <algorithm>
#include <deque>
#include <map>
#include <memory>
#include "atomic_lock.h"
#include "cacti/kernel/Mutex.h"
#include "his_location.h"
#include "traffic_info.h"
#define _SMOOTH

NAMESPACE_POINT_BEGIN(NAMESPACE_POINT)

class card_interface
{
public:
	card_interface()
		:_isMotionless(false)
		,_count(0)
	{

	}
	 virtual int push_optimized_data(loc_point&lp) = 0;
	 virtual int getCardType() = 0;
	 virtual bool smoothFlag() = 0;
	 virtual const std::string &cardId() = 0;
	 virtual const uint64_t CtTime()=0;
	 virtual const uint64_t get_last_recv_time()=0;
	 virtual const int getX() = 0;
	 virtual const int getY() = 0;
	 virtual void do_alarm(const std::size_t ow)=0;
	 virtual void PrintString(const std::string &&str) = 0;
	 virtual bool getBigCarFlag() = 0;
	 virtual double getv() = 0;
	 virtual int getAreaId() = 0;
	 virtual int getAcc() =0;
	 inline int getCount()
	 {
		 return _count;
	 }
public:
	bool _isMotionless;
	int _count;
	std::vector<std::string> m_v;
	std::map<std::string,short> m_m;
	std::list<std::shared_ptr<TrafficLightGroup>> m_arrTraLightGroup;
	std::size_t size()
	{   
		return m_arrTraLightGroup.size();
	}  
	void motionlessFlag()
	{
		if (0 == getAcc())
			_count ++;
		else
			_count = 0;
	}
	void clear()
	{   
		m_arrTraLightGroup.clear();
	}   
	void pop_front()
	{   
		m_arrTraLightGroup.pop_front();
	}   
	void pop_back()
	{   
		m_arrTraLightGroup.pop_back();
	}   
	void push_back(std::shared_ptr<TrafficLightGroup> tlg)
	{   
		//std::lock_guard<std::mutex> guard(m_mtx);
		m_arrTraLightGroup.push_back(tlg);
	}   
	void push_front(std::shared_ptr<TrafficLightGroup> tlg)
	{   
		//std::lock_guard<std::mutex> guard(m_mtx);
		m_arrTraLightGroup.push_front(tlg);
	}   
	void put(std::shared_ptr<TrafficLightGroup> tlg)
	{   
		if(size()>2)
			return;
		//std::lock_guard<std::mutex> guard(m_mtx);
		m_arrTraLightGroup.push_back(tlg);
		double x = getX();
		double y = getY();
		m_arrTraLightGroup.sort([&](std::shared_ptr<TrafficLightGroup>  one ,std::shared_ptr<TrafficLightGroup> two){
			double d1 = one->dist(x,y);
			double d2 = two->dist(x,y);
			return d1 < d2; 
		}); 
	}   
	std::shared_ptr<TrafficLightGroup> front()
	{
		//std::lock_guard<std::mutex> guard(m_mtx);
		return m_arrTraLightGroup.front();
	}
	std::shared_ptr<TrafficLightGroup> back()
	{
		//std::lock_guard<std::mutex> guard(m_mtx);
		return m_arrTraLightGroup.back();
	}
	bool find(std::shared_ptr<TrafficLightGroup> t)
	{
		for(auto x:m_arrTraLightGroup)
			if(x==t)
				return true;
		return false;
	}

	bool  operator[](const std::string &s)
	{
		if (m_m[s]<15)
		{
			m_m[s]++;
			return false;
		}
		else
		{
			return true;
		}
	}
	const bool operator[](const std::string &s) const
	{
		auto it = m_m.find(s);
		if (it != m_m.end())
		{
			if(it->second<15)
				return false;
			else
			{
				return true;
			}
		}
		return false;
	}
};
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 base_card
{
public:
	const static int max_histime=60;		 //保留最后60s的数据
	zlist<loc_point,128> m_d;
	line m_line;
	int	 m_id,m_ct;

	point m_sol[4];
	point*m_sol_p;

	loc_info m_last_li;
	const site*m_last_site;

	fit_batch   m_fitk,m_fita;
	fit_result  m_cur_fit;
	loc_point*  m_begin;
	loc_point*  m_last;

	int m_filter_man_count;	// in filter_by_fit

	// implemented by li
	//const site*m_current_site;
	//record of last fitting data
	bool m_last_fit_valid;

	fit_result m_last_fit;

	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;
	double m_last_fit_nearest_time_sec;	//latest time_sec within estimation span
	double m_last_time_sec;

	//record of last receiving time   comment it out if using timer
	double m_last_receive_time_sec;
	double m_last_receive_ct;
	const site*m_last_receive_sit;

	//parameters
	double m_fit_differ;
	double m_pos_differ;

	// turning
	bool m_if_turning;
	point m_turning_pt, m_turning_ept;

	// smooth the dist----test
	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;

	// convert between pt and dist //
	double m_simple_rec_kx ;
	double m_simple_rec_ky ;
	std::shared_ptr<card_interface> m_card;

	// 1-20 implemented
	std::atomic<int> mutex;
	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;

	// history location
	double his_speed;
	double his_speed_presentation;
	double his_last_time;
	point his_last_pt;
	line his_line;

public:
	base_card(); 
	base_card(std::shared_ptr<card_interface> &c,int id=-1);
	void log(const char * );
	void base_card_initiate();
	int last_ct()const;
	void reset_sol();
	int sol_count()const;

	//point door
	void on_tof_data(const site*sit,const loc_info&li);
	//remove_history.
	void remove_history();

	//chose one point 
	void select_solution(const site*sit);
	bool select_solution_impl(const site*sit);
	bool select_solution_impl_person(const site* sit);//人卡选点处理
	void save_k();
	//find m_d last parameter that is not empty.
	int find_last(int start);
	//find m_d first para  that is not empty/
	int find_first(int start);
	//make a line between first and last this is not empty/
	bool make_line();
	//select point..
	point select_solution0(const site * sit);

	bool filter_by_fit(const site*sit);
	bool filter_by_speed(const site * sit);
	bool filter_by_acc(loc_point&c,std::array<solpoint,4>&v,double a);

	fit_result* best_fit_raw(int num_point=0,int start=0,int end=-1);
	const fit_result* best_fit()const;
//	void solution0();
	bool solution0(const site*sit);
	point revise_by_history(point pt, const site*sit, int64_t m_time, int &revise_flag);

	double estimate_point_by_history(const site*sit, double m_time_sec);
	void estimate_missing_point_by_history(const site*sit, double m_time_sec, int m_ct);
	double convert_pt_to_dist(point &pt, const site*sit);

	point convert_dist_to_pt(double dist, const site*sit);

	// turning
	void reset_turning();
	void detect_turning(point &mpt, const site*sit);
	double calc_turning_angle(point &a, point &b);
	void turning_mapping(point &rpt, const site*sit);

	void smooth_reset();
	bool smooth_initiate(point &pt, double t, const site*sit);
	void smooth_dist_car(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr);
	void smooth_dist_man(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr);
	void smooth_set_loc_point(double t, int ct, const site*sit, loc_point *lp);
	void generate_list(point &pt, const site*sit, bool is_whole_list);
	void put_single_loc_point(point &pt, const site*sit, int ct, loc_point &lp);
	void put_loc_point(point &pt, const site*sit, int ct, loc_point &lp);
	loc_point grab_loc_point();

	void his_reset();
	std::vector<his_location> make_up_more_points_for_history(point &pt, uint64_t t,const site *sit);
};

typedef std::map<std::string,std::shared_ptr<base_card>>  Card_List;

class base_card_list
{
public:
	typedef cacti::RecursiveMutex::ScopedLock ScopedLock;
	~base_card_list()
	{
		m_cardlist.clear();
	}
	void erase(std::string cardid)
	{
		ScopedLock lp(m_monitor);
		Card_List::iterator iter = m_cardlist.find(cardid);
		if (iter != m_cardlist.end())
		{
			m_cardlist.erase(iter);
		}
	}
	void insert(std::string cardid,std::shared_ptr<base_card>& c)
	{
		ScopedLock lp(m_monitor);
		m_cardlist.insert(std::make_pair(cardid,c));
	}
	const std::shared_ptr<base_card>  operator [](std::string cardid) const
	{
		Card_List::const_iterator iter = m_cardlist.find(cardid);
		if (iter != m_cardlist.end())
		{
			return iter->second;
		}
		else
		{
			return nullptr;
		}
	}
	std::shared_ptr<base_card> operator [](std::string cardid)
	{
		ScopedLock lp(m_monitor);
		Card_List::iterator iter = m_cardlist.find(cardid);
		if (iter != m_cardlist.end())
		{
			return iter->second;
		}
		else
		{
			return nullptr;
		}
	}

private:
	Card_List m_cardlist;
	cacti::RecursiveMutex m_monitor;
};


#if 0
int main()
{
	std::unique_ptr<sit_list> sites(new sit_list());

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

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

	fflush(stdout);

	//	message_file mf(stdin);
	message_file mf("../raw_s_20171214.log.01");

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


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

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

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

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

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

	return 0;
}
#endif 
NAMESPACE_POINT_END(NAMESPACE_POINT)
#endif