server-l-zmg/ant.h

#ifndef _ANT_LIST_HPP_
#define _ANT_LIST_HPP_

#include <math.h>
#include <array>
#include <deque>
#include <tuple>
#include <memory>
#include <algorithm>
#include <sstream>
#include "log.h"
#include "line.h"
#include "point.h"
#include "write-copy.h"

struct path
{
    std::array<line_v,2>	m_line; 
    std::array<double,2>    m_slope;
    path()
    {
        for(auto &d:m_slope)
            d=0;
    }
    std::string to_str() const
    {
        std::stringstream ss;
        for(int i=0;i<2;i++)
        {
            ss<<"line:" <<m_line[i].to_string()<<"slope:"<<m_slope[i]<< " cos:"<<m_line[i].cos()<<" sin:"<<m_line[i].sin()<<" | ";
        }
        return ss.str();
    }
    bool vaild() const
    {
        return !m_line[0].empty();
    }
    line_v & operator[](int i)
    {
        return m_line[i];
    }
    const line_v & operator[](int i) const 
    {
        return m_line[i];
    }
   
};
//?
struct algo_config
{
	const char*desc;
	int min_msg_cnt;
	int best_msg_cnt;
	double min_wait_time;
	double max_wait_time;
};
//
struct ant :point
{
	std::vector<path> m_path;
    path & operator[](int i)
    {
        return m_path[i];
    }
    const path & operator[](int i) const
    {
        return m_path[i];
    }
    size_t size() const
    {
        return m_path.size();
    }
    std::vector<point> getsol(const double &dist) const
    {
        std::vector<point> v;
        for(const auto & p : m_path)
        {
            double d  = dist;
            if(p.vaild())
            {
                point pt;
                if(dist <= p.m_line[0].length() || (dist > p.m_line[0].length() && p.m_line[1].empty()))        
                {
                    d += d*p.m_slope[0];
                    pt = point(p.m_line[0][0].x + d*p.m_line[0].cos() , p.m_line[0][0].y + d*p.m_line[0].sin());
                }
                else
                {
                    d -= p.m_line[0].length();
                    d += d*p.m_slope[1];
                    pt = point(p.m_line[1][0].x+d*p.m_line[1].cos(),p.m_line[1][0].y+d*p.m_line[1].sin());
                }
                v.push_back(pt);
                std_info("get_sol:x:%.2f,y:%.2f",pt.x,pt.y);
            }
            else
                std_error("%s","ant::getsol empty path..");
        }
        return std::move(v);
    }

};

struct site:point
{
	static algo_config g_config[];
	int      m_algo;			//TOF:0,TDOA:1
	int      m_num_dims;	    //1维:0,2维:1,3维:2
    double  m_scale = 2.0;  // 地图比例尺

	point    m_position;
	int index()const;
	const algo_config&config()const;

	site(int id=-1);

	mutable double m_height=1.5;
	int m_id;
	bool m_path_empty;
	std::array<ant,2> m_ant;

	mutable double m_ant_dist=0;
	mutable double m_ant_dist_sum_new=0;
	mutable int m_ant_dist_cnt_new=0;

    point get_dstp(const point pt) const 
    {
        point tmp;
        for(const auto & p : m_ant[0].m_path)
        {
            for(int i=0;i<2;i++)
            {
                if(!p[i].empty())
                {
                    if(p[i].contain(pt,0.01))
                    {
                        //if(i==0)
                         // return *this;
                        //else
                          tmp = p[i][0];
                    }
                }
            }
        }
        return tmp;
    }

	void count_ant_dist(double dist_tof1, double dist_tof2)const
	{
		if(dist_tof1<10 || dist_tof2<10)
			return;
		double dist = fabs(dist_tof1 - dist_tof2);
		if(dist>5)
			return;
		m_ant_dist_sum_new += dist;
		m_ant_dist_cnt_new++;
		
		if(m_ant_dist_cnt_new >= 2500)
		{
			m_ant_dist = m_ant_dist_sum_new / m_ant_dist_cnt_new;
			m_ant_dist_sum_new = 0;
			m_ant_dist_cnt_new = 0;
		}
	}
    void swap()
    {
       auto v0 =  m_ant[0].m_path;
       auto v1 =  m_ant[1].m_path;
       std::copy (std::begin(v0),std::end(v0),std::back_inserter(m_ant[1].m_path));
       std::copy (std::begin(v1),std::end(v1),std::back_inserter(m_ant[0].m_path));
    }
	double ant_dist()const
	{
		return m_ant[0].dist(m_ant[1]);
	}
	
	bool is_path_empty()const
	{
        return m_path_empty;
	}

	bool have_valid_path()const
	{
        return m_id != -1 && ant_dist() > 0.1;
	}

	std::string to_string()const
	{
        std::stringstream ss;
        ss<<"site_id:"<<m_id<<"x:"<<x<<" y: "<<y;
        for(const auto a:m_ant)
        {  
            ss<<"<";
            for(const auto p:a.m_path)
            {
                ss<<"{"<<p.to_str()<<"}";
            }
            ss<<">";
        }
        return ss.str();
	}

	const point&path(int i)const
	{
        static point p;
        if(i>=(int)m_ant[0].m_path.size())
            return p ;
		return m_ant[0].m_path[i].m_line[0][1];
	}

    std::vector<point> solving(int ant_id, double dist)const
	{
        const ant &a = m_ant[ant_id];
        if(dist<50 && dist>0)
		{
			if(dist<m_height)
			{
				m_height=dist;
				dist=0;				
			}
			else
			{
				dist=sqrt(dist*dist-m_height*m_height);
			}
		}
        return std::move(a.getsol(dist));
   	}
    ant operator[](int i)
    {
        return m_ant[i];
    }
    const ant operator[](int i) const
    {
        return m_ant[i];
    }
};


struct sit_list:single_base<sit_list,int,std::shared_ptr<site>>
{
	void load(const char*ant_file,const char*path_file)
	{
		read_sit_list(ant_file);
		read_ant_path(path_file);
	}

	void load_from_db()
	{
		load("data_reader_antenna.txt","path_tof.txt");
	}

	void read_sit_list(const char*fname);
	void read_ant_path(const char*fname);
};

struct loc_message
{
	site     m_sit;
	uint64_t m_num_ticks; //tof时间片m_tof或tdoa相对root时间
	uint64_t m_loc_time;
	uint32_t m_card_id;
	int32_t	 m_card_ct;
	int8_t   m_card_type;
	int8_t   m_ant_id;
	int16_t  m_rav;
	int16_t  m_acc;
	uint16_t m_sync_ct;
	uint16_t m_rssi;


	loc_message();
	loc_message(site s,uint64_t num_ticks,uint64_t timestamp,
			uint32_t cardid,int32_t ct,int8_t type,int8_t antid,	
			int16_t rav,int16_t acc,uint16_t sync_ct,uint16_t rssi)
		:m_sit(s)
		 ,m_num_ticks(num_ticks)
         ,m_loc_time(timestamp)
        ,m_card_id(cardid)
        ,m_card_ct(ct)
        ,m_card_type(type)
        ,m_ant_id(antid)
        ,m_rav(rav)
        ,m_acc(acc)
        ,m_sync_ct(sync_ct)
        ,m_rssi(rssi)
	{}
	int tool_index()const;
};

#endif