server/SyncTime/SyncManager.cpp

#include "stdafx.h"
#include"SyncManager.h"
#include <deque>
#include <Eigen/Dense>

HostServer::SyncManager::SyncManager()
{
    init();
}

void HostServer::SyncManager::init()
{
	InitializeCriticalSection(&m_csSyncTime);
	//InitializeCriticalSection(&m_csCalcLiner);
    _anchors.swap(unordered_map<unsigned long long, Position>());
	//_syncTimeMsgs.clear();
	//_historySync.clear();
    _distance.swap(unordered_map<unsigned long long,unordered_map<unsigned long long, double>>());
}

void HostServer::SyncManager::analyzeSyncMsg(SyncTimeMsg &msg)
{
	EnterCriticalSection(&m_csSyncTime);

	int idx = FindSyncTimeMsg(msg.SyncNum());

	if( -1 == idx){ // 没找到
		// 如果时间同步消息的版本数量超过最大限制，则删除最早添加的消息
		if(_syncTimeMsgs.size() >= MAX_SYNCTIME_NUM){ // 删除第一个
			// 可能有泄露
			_syncTimeMsgs.pop_front();
		}
		SyncTimeMsgItem it;
		it.SyncNum = msg.SyncNum();
		it.SyncTimeMsgs[msg.LocalIdCode()] = msg;
		_syncTimeMsgs.push_back(it);

		idx = _syncTimeMsgs.size() - 1;
	}else{
		_syncTimeMsgs[idx].SyncTimeMsgs[msg.LocalIdCode()] = msg;
	}

	//
	while(_historySync.size() > MAX_SYNCTIME_NUM){
		_historySync.pop_front();
	}

    // 更新时间同步并计算
	for(auto it :_syncTimeMsgs[idx].SyncTimeMsgs)
	{
		updateSync(idx, msg.SyncNum(), it.first);
	}
	LeaveCriticalSection(&m_csSyncTime);
}

bool HostServer::SyncManager::updateSync(int idx, unsigned short SyncNum, unsigned long long localIdCode)
{
	if(-1 == idx){
		return false;
	}
    // 如果当前版本的时间同步消息未收到则返回false
	unordered_map<unsigned long long, SyncTimeMsg>::iterator itSyncTime = _syncTimeMsgs[idx].SyncTimeMsgs.find(localIdCode);
	if(itSyncTime == _syncTimeMsgs[idx].SyncTimeMsgs.end()){
		return false;
	}

    SyncTimeMsg &msg = _syncTimeMsgs[idx].SyncTimeMsgs[localIdCode];

    // 如果当前的为root，则返回true
    if(msg.SyncLevel() == 0){
        return true;
    } 

	int idx_synctime = FindSyncTime(SyncNum);

	// 如果时间同步已经计算过，则返回true
	if(-1 != idx_synctime){
		unordered_map<unsigned long long, SyncTime>::iterator itHistSync = _historySync[idx_synctime].HistSync.find(localIdCode);
		if(itHistSync != _historySync[idx_synctime].HistSync.end()){
			if(itHistSync->second.TimeDelay())
			return true;
		}
	}

    // 如果已经收到了上一级的同步消息
    if(_syncTimeMsgs[idx].SyncTimeMsgs.count(msg.UpperIdCode())){
        if(!updateSync(idx, SyncNum, msg.UpperIdCode())){
            return false;
        }
        SyncTimeMsg &upperMsg = _syncTimeMsgs[idx].SyncTimeMsgs[msg.UpperIdCode()];
        SyncTime* s = NULL;
        for(auto it(_historySync.rbegin()); it != _historySync.rend(); ++it)
        {
            if(it->SyncNum != msg.SyncNum() && it->HistSync.count(localIdCode))
            {
                s = &(it->HistSync.find(localIdCode)->second);
                break;
            }
        }

		idx_synctime = FindSyncTime(SyncNum);

		if(-1 == idx_synctime){
			SyncTimeItem it;
			it.SyncNum = msg.SyncNum();
			it.HistSync[localIdCode] = SyncTime(msg, upperMsg, s);
			_historySync.push_back(it);

			idx_synctime = _historySync.size() - 1;
		}else{
			_historySync[idx_synctime].HistSync[localIdCode] = SyncTime(msg, upperMsg, s);
		}

        // 计算时间同步
        long long upperTimeDelay = 0;
        if(_syncTimeMsgs[idx].SyncTimeMsgs[msg.UpperIdCode()].SyncLevel() != 0)
        {
            upperTimeDelay = _historySync[idx_synctime].HistSync[msg.UpperIdCode()].TimeDelay();
        }
        long long sendTime = _historySync[idx_synctime].HistSync[localIdCode].SendTime();
        long long receiveTime = _historySync[idx_synctime].HistSync[localIdCode].ReceiveTime();
        long long timeDelay = receiveTime - sendTime - _distance[localIdCode][msg.UpperIdCode()];

        timeDelay += upperTimeDelay;
		if(timeDelay > TIME_MAX){
			timeDelay -= TIME_MAX;
		}
		if(timeDelay + TIME_MAX < 0 ){
			timeDelay += TIME_MAX;
		}
        _historySync[idx_synctime].HistSync[localIdCode].TimeDelay(timeDelay);
        return true;
    }
    return false;
}

HostServer::Position HostServer::SyncManager::analyzeTagMsg(TagMsg &tagMsg, unsigned long long tagId, unsigned short tagNum)
{
    _tagRecords[tagId][tagNum].push_back(tagMsg);
    // 获取预估的时间差
    unordered_map<unsigned long long, long long> times;
    if(!_tagRecords.size() || !_tagRecords[tagId].size() || _tagRecords[tagId][tagNum].size() != 4)
    {
        return Position(DBL_MAX, DBL_MAX, DBL_MAX);
    }

    for(auto tag : _tagRecords[tagId][tagNum])
    {
        auto res = calTimeByLinar(tag);
        if(res == LLONG_MAX) return Position(DBL_MAX, DBL_MAX, DBL_MAX);
        times[tag.StationIdCode] = res;
    }

    // 初始化矩阵
    auto begin = times.begin();
    auto it = begin;
    it++;

    return Position(DBL_MAX, DBL_MAX, DBL_MAX);
}

unsigned long long HostServer::SyncManager::calTimeByLinar(TagMsg &tag)
{
	//EnterCriticalSection(&m_csCalcLiner);
	EnterCriticalSection(&m_csSyncTime);
    // 获取历史记录中与此tag最近的两条
    deque<SyncTime> hisSync;
    //ofstream fout("test/linar_513.txt",ios::app);
    int i = 0;
	int idx = FindSyncTimeMsg(tag.SyncNum);
	if(-1 != idx){
		unordered_map<unsigned long long, SyncTimeMsg>::iterator it = _syncTimeMsgs[idx].SyncTimeMsgs.find(tag.StationIdCode);
		if(it != _syncTimeMsgs[idx].SyncTimeMsgs.end()){
			if(it->second.SyncLevel() == 0){
				//LeaveCriticalSection(&m_csCalcLiner);
				LeaveCriticalSection(&m_csSyncTime);
				return tag.ReceiveTime;
			}
		}
	}
	
	int idx_sync = -1;
    while(hisSync.size() < 2 && i < MAX_CALCLINER_NUM)
    {
        auto syncNum = tag.SyncNum - i;
		idx_sync = FindSyncTime(syncNum);
		if(-1 != idx_sync){
			if(_historySync[idx_sync].HistSync.count(tag.StationIdCode)){
				hisSync.push_front(_historySync[idx_sync].HistSync[tag.StationIdCode]);
			}
		}
	
        //if(_historySync.count(syncNum) && _historySync[syncNum].count(tag.StationIdCode))
        //{
        //    // return (tag.ReceiveTime + (_historySync[syncNum][tag.StationIdCode].IsRTimeOverflow() ? TIME_MAX : 0) - _historySync[syncNum][tag.StationIdCode].TimeDelay()) & TIME_MAX;
        //    hisSync.push_front(_historySync[syncNum][tag.StationIdCode]);
        //}
        i++;
    }
    // 如果满足条件的历史记录不足两个则返回
    if(hisSync.size() < 2){
		//LeaveCriticalSection(&m_csCalcLiner);
		LeaveCriticalSection(&m_csSyncTime);
		return LLONG_MAX;
	}

    // 计算预估值
    long long y1(hisSync.at(0).ReceiveTime() - hisSync.at(0).TimeDelay()),y2(hisSync.at(1).ReceiveTime() - hisSync.at(1).TimeDelay());
    long long x1(hisSync.at(0).RealReceiveTime()), x2(hisSync.at(1).RealReceiveTime()), x3(tag.ReceiveTime);

	LeaveCriticalSection(&m_csSyncTime);

    unsigned long long res;
    if(x1 > x2)
    {
        x2 += TIME_MAX;
    }
    if(x2 > x3)
    {
        x3 += TIME_MAX;
    }

	if(y1 < 0){ // 理论y值不能小于0
		y1 += TIME_MAX;
	}

	if(y2 < 0 ){
		y2 += TIME_MAX;
	}

	if(y1 > y2){
		y2 += TIME_MAX;
	}else if(y2-y1 > TIME_MAX){
		y2 -=TIME_MAX;
	}

    Eigen::Matrix3d a;
    a << x1 ,1 , 0,
         x2 ,1 , 0,
         x3, 1, -1;

    Eigen::Vector3d b(y1, y2, 0);
    Eigen::Vector3d X = a.colPivHouseholderQr().solve(b);
    
    res = X(2);
    res &= TIME_MAX;

    return res;
}

void HostServer::SyncManager::updateDistance(unsigned int localId, unsigned char localAntNum, unsigned int upperId, unsigned char uppderAntNum, double d)
{
    unsigned long long lId = SyncHelper::parseId(localId, localAntNum);
    unsigned long long uId = SyncHelper::parseId(upperId, uppderAntNum);
    if(_anchors.count(lId) == 0)
    {
        _anchors[lId] = Position();
    }
    if(_anchors.count(uId) == 0) 
    {
        _anchors[uId] = Position();
    }
    _distance[lId][uId] = d;
    _distance[uId][lId] = d;

    // 删除所有版本的消息记录
	_historySync.clear();
	_syncTimeMsgs.clear();
    //_historySync.swap(unordered_map<unsigned short, unordered_map<unsigned long long, SyncTime>>());
    //_syncTimeMsgs.swap(unordered_map<unsigned short, unordered_map<unsigned long long, SyncTimeMsg>>());
}

void HostServer::SyncManager::updateAnchor(unsigned int localId, unsigned char localAntNum, double x, double y, double z)
{
    deleteAnchor(localId, localAntNum);

    unsigned long long lId = SyncHelper::parseId(localId, localAntNum);
    _anchors[lId] = Position( x, y, z);
}

void HostServer::SyncManager::deleteAnchor(unsigned int localId, unsigned char localAntNum)
{
    unsigned long long lId = SyncHelper::parseId(localId, localAntNum);
    auto it = _anchors.find(lId);
    if(it == _anchors.end()) return;

    // 删除anchor
    _anchors.erase(it);

    // 删除与此anchor相关的距离信息
    _distance.erase(_distance.find(lId));
    for(auto it(_distance.begin()); it != _distance.end(); it++)
    {
        it->second.erase(it->second.find(lId));
        if(it->second.size() == 0)
        {
            it = _distance.erase(it);
        }
    }

    // 删除所有版本的消息记录
	_historySync.clear();
	_syncTimeMsgs.clear();
    //_historySync.swap(unordered_map<unsigned short, unordered_map<unsigned long long, SyncTime>>());
    //_syncTimeMsgs.swap(unordered_map<unsigned short, unordered_map<unsigned long long, SyncTimeMsg>>());
}

HostServer::SyncManager::~SyncManager()
{
	//DeleteCriticalSection(&m_csCalcLiner);
	DeleteCriticalSection(&m_csSyncTime);
}

int HostServer::SyncManager::FindSyncTimeMsg( unsigned short SyncNum )
{
	int idx = -1;
	for(int i = _syncTimeMsgs.size() - 1; i >= 0; i--){
		if(_syncTimeMsgs[i].SyncNum == SyncNum ){
			return i;
		}
	}
	return idx;
}

int HostServer::SyncManager::FindSyncTime( unsigned short SyncNum )
{
	int idx = -1;
	for(int i = _historySync.size() - 1; i >= 0; i--){
		if(_historySync[i].SyncNum == SyncNum ){
			return i;
		}
	}
	return idx;
}