server/SyncTime/SyncManager.cpp

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

#include <fstream>  
#include <iostream>  
#include <direct.h>
#include <io.h>

#include "../ProcessRemodule.h"
#include "../log_process_module.h"
#include "./../system_basic_info/SystemAnalysis.h"

#pragma warning(disable: 4244)

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

void HostServer::SyncManager::init()
{
	InitializeCriticalSectionAndSpinCount(&m_csSyncTime, 4000);
	//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>>());
	logDir = ".\\synclog\\";

	if(0 != _access(logDir.c_str(), 0)){
		_mkdir(logDir.c_str());
	}

	isOutputLog = false;
}

void HostServer::SyncManager::analyzeSyncMsg(SyncTimeMsg &msg)
{
    LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_10);
	EnterCriticalSection(&m_csSyncTime);
	int idx = FindSyncTimeMsg(msg.RootIdCode(), msg.SyncNum());

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

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

	//
	while(_historySync[msg.RootIdCode()].size() > MAX_SYNCTIME_NUM){
		_historySync[msg.RootIdCode()].pop_front();
	}

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

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

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

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

	int idx_synctime = FindHisSyncTime(rootIdCode, SyncNum); 

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

    // 如果已经收到了上一级的同步消息
    if(_syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.count(msg.UpperIdCode())){
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_77);
        if(!updateSync(rootIdCode, idx, SyncNum, msg.UpperIdCode())){
			LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_78);
            return false;
        }

        SyncTimeMsg &upperMsg = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs[msg.UpperIdCode()];
        SyncTime* s = NULL;
        for(auto it(_historySync[rootIdCode].rbegin()); it != _historySync[rootIdCode].rend(); ++it)
        {
            if(it->SyncNum != msg.SyncNum() && it->HistSync.count(localIdCode))
            {
                s = &(it->HistSync.find(localIdCode)->second);
                break;
            }
        }

		idx_synctime = FindHisSyncTime(rootIdCode, SyncNum);

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

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

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

        timeDelay += upperTimeDelay;
		
		// 从不同的Upper来的同步数据，跨周期判断可能不正确，将时间差控制在一个周期内
		while(timeDelay > TIME_MAX){
			timeDelay -= TIME_MAX;
		}
		while(timeDelay + TIME_MAX < 0 ){
			timeDelay += TIME_MAX;
		}
        _historySync[rootIdCode][idx_synctime].HistSync[localIdCode].TimeDelay(timeDelay);


		//debug_print_syslog(0, "Delay info, syncnum:%d, station id: 0x%x, timeDelay:%I64u", 
		//                 SyncNum, localIdCode, timeDelay);

		if(isOutputLog){
			char filename[100];
			char temp[200]; 

			long long aa = timeDelay;
			if(aa < 0){
				aa += TIME_MAX;
			}
			int bb = (receiveTime > TIME_MAX) ? 1 : 0;
			int cc = (sendTime > TIME_MAX) ? 1 : 0;
			int dd = msg.UpperIdCode() >> 8;

			sprintf_s(filename, "%s\\%d.log", logDir.c_str(), localIdCode>>8);
			ofstream outfile(filename, ofstream::app); 
			//sprintf(temp, "%d, %d, %I64d, %I64d\r", SyncNum, dd, receiveTime, timeDelay );
			sprintf_s(temp, "sn:%d, up:%d, rec:%I64d, snd:%I64d, del:%I64d, crt:%I64d, r_crs:%d, s_crs:%d\r", SyncNum, dd, receiveTime, sendTime, timeDelay, aa, bb, cc); 
			outfile << temp;
		}

		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_69);
        return true;
    }else{
		LOCATION_SYSTEM_BRANCH(LOCATION_SYSTEM_BRANCH_110);
	}
    return false;
}

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);

	unsigned long long rootIdCode = tag.SyncRootIdCode;

    int i = 0;
	int idx = FindSyncTimeMsg(rootIdCode, tag.SyncNum);
	if(-1 != idx){
		unordered_map<unsigned long long, SyncTimeMsg>::iterator it = _syncTimeMsgs[rootIdCode][idx].SyncTimeMsgs.find(tag.StationIdCode);
		if(it != _syncTimeMsgs[rootIdCode][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 = FindHisSyncTime(rootIdCode, syncNum);
		if(-1 != idx_sync){
			if(_historySync[rootIdCode][idx_sync].HistSync.count(tag.StationIdCode)){
				hisSync.push_front(_historySync[rootIdCode][idx_sync].HistSync[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;
	}

	// y1 = k*x1 + b;
	// y2 = k*x2 + b;

	// k = (x1-x2)/(y1-y2);
	// b = y1 - k*x1; 

    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 long long rootIdCode, unsigned short SyncNum )
{
	int idx = -1;
	for(int i = _syncTimeMsgs[rootIdCode].size() - 1; i >= 0; i--){
		if(_syncTimeMsgs[rootIdCode][i].SyncNum == SyncNum ){
			return i;
		}
	}
	return idx;
}

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