#include <vector>

#include <log.h>
#include <ant.h>
#include <loc_tool.h>
#include <message.h>

std::vector<point> loc_tool_tdoa_3_base::calc_location(std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_3_base::index() 
{
	return 5;
}

std::vector<point> loc_tool_tdoa_2_base::calc_location(std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_2_base::index() 
{
	return 3;
}

std::vector<point> loc_tool_tdoa_1_base::calc_location(std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_1_base::index() 
{
	return 1;
}

std::vector<point> loc_tool_tof_3_base::calc_location(std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_3_base::index() 
{
	return 4;
}

std::vector<point> loc_tool_tof_2_base::calc_location(std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_2_base::index() 
{
	return 2;
}


std::vector<point> loc_tool_tof_1_base::calc_location(std::vector<loc_message>&locm)
{
    int32_t last_ct = -1;
    std::vector<point> vec;
    std::vector<loc_message> lm;
    for(auto rit = locm.rbegin();rit != locm.rend();rit++)
    {
//        site s = rit->m_sit;

        if(rit->m_sit->is_path_empty() || rit->m_num_ticks == 0)
          continue;
        if(last_ct == -1)
          last_ct = rit->m_card_ct;
        else if(last_ct != rit->m_card_ct)
          continue;
		double dist_tof=rit->m_num_ticks*15.65*2.996*1e-4/rit->m_sit->m_scale;
        auto v = rit->m_sit->solving(rit->m_ant_id,dist_tof);
        lm.insert(lm.begin(),*rit);
        vec.insert(std::end(vec),std::begin(v),std::end(v));
    }
    locm.swap(lm);
	return std::move(vec);
}
int loc_tool_tof_1_base::index() 
{
	return 0;
}

loc_tool_main::loc_tool_main()
{
	set_tool(new loc_tool_tof_1_base());
	set_tool(new loc_tool_tof_2_base());
	set_tool(new loc_tool_tof_3_base());
	set_tool(new loc_tool_tdoa_1_base());
	set_tool(new loc_tool_tdoa_2_base());
	set_tool(new loc_tool_tdoa_3_base());
}

loc_tool_main::~loc_tool_main()
{
	for(auto&tool:g_tool)
		delete tool;
}

void loc_tool_main::set_tool(loc_tool*tool)
{
	int index=tool->index();
	if(g_tool[index])
	{
		delete g_tool[index];
		g_tool[index]=0;
	}
	g_tool[index]=tool;
}

std::vector<point> loc_tool_main::calc_location(std::vector<loc_message>&locm)
{
	if(locm.empty()) return {};
	int tool_index=locm[0].tool_index(),i=1,len=locm.size();
	for(;i<len;i++)
	{
		if(tool_index!=locm[i].tool_index())
			break;
	}

	if(i==len)
	{
		return std::move(g_tool[tool_index]->calc_location(locm));
	}

	//包含至少两种定位方式的基站，目前只考虑两种
	std::vector<loc_message> locm1,locm2;
	locm1.assign(locm.begin(),locm.begin()+i);

	for(;i<len;i++)
	{
		if(tool_index!=locm[i].tool_index())
			locm2.push_back(locm[i]);
		else
			locm1.push_back(locm[i]);
	}
    bool flag = false;
    if(locm2[0].tool_index() > tool_index)
        flag = true;
    std::vector<point> rc;
    if(flag && (locm2[0].m_sit->config().best_msg_cnt<=(int)locm2.size()))
    {
		int index=locm2[0].tool_index();
		rc = std::move(g_tool[index]->calc_location(locm2));
        locm.swap(locm2);
    }
    else if(locm1[0].m_sit->config().best_msg_cnt<=(int)locm1.size())
	{
	    rc = std::move(g_tool[tool_index]->calc_location(locm1));
        locm.swap(locm1);
	}
    return std::move(rc);
}

loc_tool* loc_tool_main::g_tool[6]={0,0,0,0,0,0};

/*
std::vector<point> loc_tool_tdoa_3_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_3_base::index() 
{
	return 5;
}

std::vector<point> loc_tool_tdoa_2_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_2_base::index() 
{
	return 3;
}

std::vector<point> loc_tool_tdoa_1_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_1_base::index() 
{
	return 1;
}

std::vector<point> loc_tool_tof_3_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_3_base::index() 
{
	return 4;
}

std::vector<point> loc_tool_tof_2_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_2_base::index() 
{
	return 2;
}

void loc_tool_tof_1_base::on_loc_message(ant*,const message_locinfo&m)
{
	log_info("tof1-message:site=%d,ant=%d,card=%d,ct=%d,tof=%lld,rav=%02X,acc=%02X,rssi=%d",
			m.m_site_id,m.m_ant_id,m.m_card_id,m.m_card_ct,m.m_tof,m.m_rav,m.m_acc,m.m_rssi);
}

std::vector<point> loc_tool_tof_1_base::calc_location(const std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_1_base::index() 
{
	return 0;
}

loc_tool_main::loc_tool_main()
{
	set_tool(new loc_tool_tof_1_base());
	set_tool(new loc_tool_tof_2_base());
	set_tool(new loc_tool_tof_3_base());
	set_tool(new loc_tool_tdoa_1_base());
	set_tool(new loc_tool_tdoa_2_base());
	set_tool(new loc_tool_tdoa_3_base());
}

loc_tool_main::~loc_tool_main()
{
	for(auto&tool:g_tool)
		delete tool;
}

loc_tool* loc_tool_main::get_tool(int index)
{
	if((uint32_t)index >= sizeof(g_tool)/sizeof(loc_tool*))
		return nullptr;
	
	return g_tool[index];
}

void loc_tool_main::set_tool(loc_tool*tool)
{
	int index=tool->index();
	if(g_tool[index])
	{
		delete g_tool[index];
		g_tool[index]=0;
	}
	g_tool[index]=tool;
}

void loc_tool_main::on_loc_message(ant*a, const message_locinfo&m)
{
	loc_tool*lt=get_tool(a->index());
	if(lt==nullptr)
	{
		log_warn("无法找到对应的loctool-message:site=%d,ant=%d,card=%d,ct=%d,tof=%lld,rav=%02X,acc=%02X,rssi=%d",
				m.m_site_id,m.m_ant_id,m.m_card_id,m.m_card_ct,m.m_tof,m.m_rav,m.m_acc,m.m_rssi);
		return;
	}

	lt->on_loc_message(a, m);
}

std::vector<point> loc_tool_main::calc_location(const std::vector<loc_message>&locm)
{
	if(locm.empty()) return {};
	int tool_index=locm[0].tool_index(),i=1,len=locm.size();
	for(;i<len;i++)
	{
		if(tool_index!=locm[i].tool_index())
			break;
	}

	if(i==len)
	{
		return std::move(g_tool[tool_index]->calc_location(locm));
	}

	//包含至少两种定位方式的基站，目前只考虑两种
	std::vector<loc_message> locm1,locm2;
	locm1.assign(locm.begin(),locm.begin()+i);

	for(;i<len;i++)
	{
		if(tool_index!=locm[i].tool_index())
			locm2.push_back(locm[i]);
		else
			locm1.push_back(locm[i]);
	}

	std::vector<point> rc;
	if(locm1[0].m_ant->config().best_msg_cnt<=(int)locm1.size())
	{
		rc=std::move(g_tool[tool_index]->calc_location(locm1));
	}

	if(locm2[0].m_ant->config().best_msg_cnt<=(int)locm2.size())
	{
		int index=locm2[0].tool_index();
		auto v=std::move(g_tool[index]->calc_location(locm2));
		rc.insert(rc.begin(),v.begin(),v.end());
	}

	return std::move(rc);
}
*/