text:
#include "SapConnection.h"
#include "MemoryPool.h"
#include <boost/bind.hpp>
#include <iostream>
#include "SapLogHelper.h"
#include "SapMessage.h"
#include "Cipher.h"
#include "CommonMacro.h"

#define MAX_SAP_PAKET_LEN 1024*1024*2
const int HeartInterval = 50;
const int TimeoutInterval = 120;

boost::mutex CSapConnection::sm_mutCount;
unsigned int CSapConnection::sm_nMark = 0;

CSapConnection::CSapConnection(boost::asio::io_service &io_service, CTimerManager *pManager,ISapSession *pSession):
    m_socket(io_service),m_resolver(io_service),
	m_isConnected(false),bRunning(true),m_nQueueLen(0), m_strRemoteIp(""),m_dwRemotePort(0),
	m_isLocalDigest(false),m_isPeerDigest(false),m_isEnc(false), m_pTimerManager(pManager),m_pSession(pSession),
	m_tmHeartBeat((CTimerManager *)pManager)
{
	sm_mutCount.lock();
	m_nId = sm_nMark++;
	sm_mutCount.unlock();

    m_heartSequence=0;
    m_heartRequest.SetService(SAP_PACKET_REQUEST,0,0,0);
    m_heartRequest.SetSequence(m_heartSequence++);

    gettimeofday_a(&m_HeartBeatPoint,NULL);
    gettimeofday_a(&m_AlivePoint,NULL);
}

CSapConnection::~CSapConnection()
{
	SS_XLOG(XLOG_TRACE,"CSapConnection::%s\n", __FUNCTION__);
}

void CSapConnection::AsyncConnect(const string &strIp,unsigned int dwPort,int nTimeout,unsigned int dwLocalPort)
{
    SS_XLOG(XLOG_DEBUG,"CSapConnection::async_connect,id[%d],addr[%s:%d],timeout[%d]\n",m_nId,strIp.c_str(),dwPort,nTimeout);
    m_isConnected=false;

    char szBuf[16]={0};
    sprintf(szBuf,"%d",dwPort);
    tcp::resolver::query query(strIp,szBuf);
    m_resolver.async_resolve(query,
        MakeSapAllocHandler(m_allocReader, boost::bind(&CSapConnection::HandleResolve, shared_from_this(),
              boost::asio::placeholders::error, boost::asio::placeholders::iterator,dwLocalPort)));
}

void CSapConnection::HandleResolve(const boost::system::error_code& err,tcp::resolver::iterator endpoint_iterator,unsigned int dwLocalPort)
{
    if (!err)
    {
        if(dwLocalPort!=0)
        {
            boost::system::error_code ec;
            m_socket.open(tcp::v4(),ec);
            if(ec)
            {
                SS_SLOG(XLOG_WARNING,"CSapConnection::"<<__FUNCTION__<<",id["<<m_nId<<"],open socket error:" <<ec.message()<<"\n");
                ConnectFinish(-1);
                return;
            }
			m_socket.set_option(boost::asio::ip::tcp::acceptor::reuse_address(true),ec);
			m_socket.set_option(boost::asio::ip::tcp::no_delay(true),ec);
            m_socket.bind(tcp::endpoint(tcp::v4(),dwLocalPort),ec);
            if(ec)
            {
                SS_SLOG(XLOG_WARNING,"CSapConnection::"<<__FUNCTION__<<",id["<<m_nId<<"],bind port error:" <<ec.message()<<"\n");
                ConnectFinish(-1);
                return;
            }
        }
        tcp::endpoint endpoint = *endpoint_iterator;
        SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d],addr[%s:%d]\n",__FUNCTION__,m_nId,endpoint.address().to_string() .c_str() ,endpoint.port());
        m_socket.async_connect(endpoint,
            MakeSapAllocHandler(m_allocReader, boost::bind(&CSapConnection::HandleConnected, shared_from_this(),
            	boost::asio::placeholders::error, ++endpoint_iterator)));
    }
    else
    {
      SS_SLOG(XLOG_WARNING,"CSapConnection::"<<__FUNCTION__<<",id["<<m_nId<<"],error:" <<err.message()<<"\n");
      ConnectFinish(-2);
    }
}

void CSapConnection::HandleConnected(const boost::system::error_code& err,tcp::resolver::iterator endpoint_iterator)
{
    SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d]\n",__FUNCTION__,m_nId);
    if (!err)
    {
        gettimeofday_a(&m_HeartBeatPoint,NULL);
        gettimeofday_a(&m_AlivePoint,NULL);
        ConnectFinish(0);
        m_isConnected=true;
        SetRemoteAddr();

        AsyncRead();
    }
    else if (endpoint_iterator != tcp::resolver::iterator())
    {
      boost::system::error_code ignore_ec;
      m_socket.close(ignore_ec);
      tcp::endpoint endpoint = *endpoint_iterator;
      m_socket.async_connect(endpoint,
          MakeSapAllocHandler(m_allocReader,boost::bind(&CSapConnection::HandleConnected, shared_from_this(),
            boost::asio::placeholders::error, ++endpoint_iterator)));
    }
    else
    {
        //SS_SLOG(XLOG_WARNING,"CSapConnection::"<<__FUNCTION__<<",id["<<m_nId<<"],addr[], error:" <<err.message()<<"\n");
		SS_XLOG(XLOG_WARNING,"CSapConnection::%s, id[%d],  error:%s\n",	__FUNCTION__,m_nId,(err.message()).c_str());
        ConnectFinish(-3);
    }
}

void CSapConnection::ConnectFinish(int result)
{
	SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d],result[%d]\n",__FUNCTION__,m_nId,result);
	if(m_pSession != NULL)
	    m_pSession->OnConnectResult(m_nId,result);

}

void CSapConnection::AsyncRead()
{
    m_socket.get_io_service().post(boost::bind(&CSapConnection::AsyncReadInner, shared_from_this()));
}

void CSapConnection::AsyncReadInner()
{
    gettimeofday_a(&m_HeartBeatPoint,NULL);
    gettimeofday_a(&m_AlivePoint,NULL);
    SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d]\n",__FUNCTION__,m_nId);

	m_tmHeartBeat.Init((CTimerManager *)m_pTimerManager,60,boost::bind(&CSapConnection::DoDetectInterval,shared_from_this()),CThreadTimer::TIMER_CIRCLE);
	m_tmHeartBeat.Start();

    m_pHeader=buffer_.base();
    m_socket.async_read_some( boost::asio::buffer(buffer_.top(),buffer_.capacity()),
					MakeSapAllocHandler(m_allocReader,boost::bind(&CSapConnection::HandleRead, shared_from_this(),
							boost::asio::placeholders::error,boost::asio::placeholders::bytes_transferred)));
}

void CSapConnection::HandleRead(const boost::system::error_code& err,std::size_t dwTransferred)
{
    SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d],bytes_transferred[%d]\n",__FUNCTION__,m_nId,dwTransferred);
    if(!err)
    {
        gettimeofday_a(&m_AlivePoint,NULL);
        buffer_.inc_loc(dwTransferred);
        while(buffer_.top()>=m_pHeader+sizeof(SSapMsgHeader))
        {
            SSapMsgHeader *ptrHeader=(SSapMsgHeader *)m_pHeader;
            unsigned int packetlen=ntohl(ptrHeader->dwPacketLen);
            if(packetlen<=MAX_SAP_PAKET_LEN&&packetlen>0&&(ptrHeader->byIdentifer==SAP_PACKET_REQUEST||ptrHeader->byIdentifer==SAP_PACKET_RESPONSE))
            {
                if(buffer_.top()>=m_pHeader+packetlen)
                {
                    ReadPacketCompleted(m_pHeader,packetlen);
                    m_pHeader+=packetlen;
                }
                else
                {
                    break;
                }
            }
            else
            {
                SS_XLOG(XLOG_WARNING,"CSapConnection::%s,id[%d],packetlen[%d],identifier[%d], will close this socket\n",__FUNCTION__,m_nId,packetlen,ptrHeader->byIdentifer);
              	StopConnection();
                return;
            }
        }
        if(m_pHeader!=buffer_.base())
        {
            memmove(buffer_.base(),m_pHeader,buffer_.top()-m_pHeader);
            buffer_.reset_loc(buffer_.top()-m_pHeader);
            m_pHeader=buffer_.base();
        }
        else if(buffer_.capacity()==0)
        {
	    	m_pHeader=buffer_.base();
	    	SSapMsgHeader *ptrHeader=(SSapMsgHeader *)m_pHeader;
            unsigned int packetlen=ntohl(ptrHeader->dwPacketLen);
            buffer_.add_capacity(SAP_ALIGN(packetlen));
            m_pHeader=buffer_.base();
        }
        m_socket.async_read_some(boost::asio::buffer(buffer_.top(),buffer_.capacity()),
           MakeSapAllocHandler(m_allocReader,boost::bind(&CSapConnection::HandleRead, shared_from_this(),
                boost::asio::placeholders::error,boost::asio::placeholders::bytes_transferred)));
    }
    else
    {
        SS_SLOG(XLOG_WARNING,"CSapConnection::"<<__FUNCTION__<<",id["<<m_nId<<"],error:" <<err.message()<<"\n");
        StopConnection();
    }
}

void CSapConnection::ReadPacketCompleted(const void *pBuffer, unsigned int nLen)
{
    SSapMsgHeader *pHeader=(SSapMsgHeader *)pBuffer;
    unsigned int dwServiceId=ntohl(pHeader->dwServiceId);
    unsigned int dwMsgId=ntohl(pHeader->dwMsgId);

    if(pHeader->byIdentifer==SAP_PACKET_RESPONSE&&
        dwServiceId==0&&dwMsgId==0) //heart-beat packet
    {
        dump_sap_packet_info(pBuffer,E_SAP_IN);
    }
    else if(pHeader->byIdentifer==SAP_PACKET_RESPONSE)
    {
        ReadAvenueResponsePacketCompleted(pBuffer,nLen);
    }
	else if(pHeader->byIdentifer==SAP_PACKET_REQUEST)
	{
		ReadAvenueRequestPacketCompleted(pBuffer,nLen);
	}
    else
    {
        dump_sap_packet_notice(pBuffer,E_SAP_IN);
    }
}

void CSapConnection::ReadAvenueResponsePacketCompleted(const void *pBuffer, unsigned int nLen)
{
    if(m_isPeerDigest)
    {
        unsigned char arSignature[16];
        SSapMsgHeader *pHeader=(SSapMsgHeader *)pBuffer;
        memcpy(arSignature,pHeader->bySignature,16);

        int nDigestLen=m_strPeerKey.size();
        int nMin=(nDigestLen<16?nDigestLen:16);

        memset(pHeader->bySignature,0,16);
        memcpy(pHeader->bySignature,m_strPeerKey.c_str(),nMin);

        CCipher::Md5((const unsigned char *)pBuffer,nLen,pHeader->bySignature);
        if(memcmp(arSignature,pHeader->bySignature,16)!=0)
        {
            return;
        }
    }

    if(m_isEnc)
    {
        SSapMsgHeader *pHeader=(SSapMsgHeader *)pBuffer;
        unsigned char *ptrLoc=(unsigned char *)pBuffer+pHeader->byHeadLen;
        CCipher::AesDec(m_arEncKey,ptrLoc,nLen-pHeader->byHeadLen,ptrLoc);
    }
    dump_sap_packet_notice(pBuffer,E_SAP_IN);
	if(m_pSession != NULL)
	    m_pSession->OnReceiveSosAvenueResponse(m_nId,pBuffer,nLen,m_strRemoteIp,m_dwRemotePort);
}

void CSapConnection::ReadAvenueRequestPacketCompleted(const void *pBuffer, unsigned int nLen)
{
    if(m_isPeerDigest)
    {
        unsigned char arSignature[16];
        SSapMsgHeader *pHeader=(SSapMsgHeader *)pBuffer;
        memcpy(arSignature,pHeader->bySignature,16);

        int nDigestLen=m_strPeerKey.size();
        int nMin=(nDigestLen<16?nDigestLen:16);

        memset(pHeader->bySignature,0,16);
        memcpy(pHeader->bySignature,m_strPeerKey.c_str(),nMin);

        CCipher::Md5((const unsigned char *)pBuffer,nLen,pHeader->bySignature);
        if(memcmp(arSignature,pHeader->bySignature,16)!=0)
        {
            //ResponseFail(8,8,ntohl(pHeader->dwSequence),SAP_CODE_DEGEST_ERROR);
            return;
        }
    }

    if(m_isEnc)
    {
        SSapMsgHeader *pHeader=(SSapMsgHeader *)pBuffer;
        unsigned char *ptrLoc=(unsigned char *)pBuffer+pHeader->byHeadLen;
        CCipher::AesDec(m_arEncKey,ptrLoc,nLen-pHeader->byHeadLen,ptrLoc);
    }
    dump_sap_packet_notice(pBuffer,E_SAP_IN);
	if(m_pSession != NULL)
	    m_pSession->OnReceiveAvenueRequest(m_nId,pBuffer,nLen,m_strRemoteIp,m_dwRemotePort);
}

void CSapConnection::WriteData(const void *pBuffer,int nLen,const char* pSignatureKey,unsigned int dwTimeout)
{
    SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d]\n",__FUNCTION__,m_nId);
    dump_sap_packet_notice(pBuffer,E_SAP_OUT);
    SSapMsgHeader *pHeader=(SSapMsgHeader *)pBuffer;

	if(m_nQueueLen>MAX_QUEUE_FULL)
     {
        SS_XLOG(XLOG_ERROR,"CSapConnection::%s,queueLen too many[%d], will discard,identifer[%d],target addr[%s:%d]\n",
            __FUNCTION__,m_nQueueLen,pHeader->byIdentifer,m_strRemoteIp.c_str(),m_dwRemotePort);
        if(pHeader->byIdentifer==SAP_PACKET_REQUEST&&m_pSession!=NULL)
        {
            SSapMsgHeader stSendFailResponse;
            stSendFailResponse.byIdentifer=SAP_PACKET_RESPONSE;
            stSendFailResponse.byHeadLen=sizeof(SSapMsgHeader);
            stSendFailResponse.dwPacketLen=htonl(sizeof(SSapMsgHeader));
            stSendFailResponse.byVersion=0x1;
            stSendFailResponse.dwCode=htonl(ERROR_SOS_QUEUE_FULL);

            stSendFailResponse.dwServiceId=pHeader->dwServiceId;
            stSendFailResponse.dwMsgId=pHeader->dwMsgId;
            stSendFailResponse.dwSequence=pHeader->dwSequence;
			m_pSession->OnReceiveSosAvenueResponse(m_nId,&stSendFailResponse,sizeof(stSendFailResponse),m_strRemoteIp,m_dwRemotePort);
        }
        return;
     }
    /*first enc, then digest*/
    int nFactLen=nLen;
    unsigned int nLeft=0x10-(nLen-pHeader->byHeadLen)&0x0f;

    if(m_isEnc)
    {
        nFactLen=nLen+nLeft;
    }

    void *wrBuffer=malloc(nFactLen);
    memcpy(wrBuffer,pBuffer,nLen);
    pHeader=(SSapMsgHeader *)wrBuffer;

    if(m_isEnc)
    {
    	if(nLeft>0)
            memset((char *)wrBuffer+nLen,0,nLeft);

        unsigned char *pbyInBlk=(unsigned char *)wrBuffer+pHeader->byHeadLen;
        CCipher::AesEnc(m_arEncKey,pbyInBlk,nFactLen-pHeader->byHeadLen,pbyInBlk);
        pHeader->dwPacketLen=htonl(nFactLen);
    }

    SS_XLOG(XLOG_TRACE,"CSapConnection::%s,id[%d],m_isLocalDigest[%d],m_isEnc[%d],key[%s]\n",__FUNCTION__,m_nId,m_isLocalDigest,m_isEnc,m_strLocalKey.c_str());
    if(pSignatureKey != NULL && strlen(pSignatureKey) > 0)
    {
        int nDigestLen=strlen(pSignatureKey);
        int nMin=(nDigestLen<16?nDigestLen:16);
        memcpy(pHeader->bySignature,pSignatureKey,nMin);
        CCipher::Md5((unsigned char *)wrBuffer,nFactLen,pHeader->bySignature);
    }

    WriteDataInner(wrBuffer,nFactLen,dwTimeout);
}

void CSapConnection::WriteDataEx(const void *pBuffer,int nLen, const void* pExHead, int nExLen,const char* pSignatureKey,unsigned int dwTimeout)
{
     SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d], nLen[%d],nExLen[%d]\n",__FUNCTION__,m_nId,nLen,nExLen);

	 SSapMsgHeader *pHeader=(SSapMsgHeader *)pBuffer;

	 if(m_nQueueLen>MAX_QUEUE_FULL)
     {
        SS_XLOG(XLOG_ERROR,"CSapConnection::%s,queueLen too many[%d], will discard,identifer[%d],target addr[%s:%d]\n",
            __FUNCTION__,m_nQueueLen,pHeader->byIdentifer,m_strRemoteIp.c_str(),m_dwRemotePort);
        if(pHeader->byIdentifer==SAP_PACKET_REQUEST&&m_pSession!=NULL)
        {
            SSapMsgHeader stSendFailResponse;
            stSendFailResponse.byIdentifer=SAP_PACKET_RESPONSE;
            stSendFailResponse.byHeadLen=sizeof(SSapMsgHeader);
            stSendFailResponse.dwPacketLen=htonl(sizeof(SSapMsgHeader));
            stSendFailResponse.byVersion=0x1;
            stSendFailResponse.dwCode=htonl(ERROR_SOS_QUEUE_FULL);

            stSendFailResponse.dwServiceId=pHeader->dwServiceId;
            stSendFailResponse.dwMsgId=pHeader->dwMsgId;
            stSendFailResponse.dwSequence=pHeader->dwSequence;
            m_pSession->OnReceiveSosAvenueResponse(m_nId,&stSendFailResponse,sizeof(stSendFailResponse),m_strRemoteIp,m_dwRemotePort);
        }
        return;
     }
/*first enc, then digest*/
    int nFactLen=nLen+nExLen;
    unsigned int nLeft=0x10-(nLen-pHeader->byHeadLen)&0x0f;

    if(m_isEnc)
    {
        nFactLen=nLen+nExLen+nLeft;
    }

	 SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d], nLen[%d],nExLen[%d],nFactLen[%d]\n",__FUNCTION__,m_nId,nLen,nExLen,nFactLen);

	void *wrBuffer=malloc(nFactLen);
	memcpy(wrBuffer,pBuffer,pHeader->byHeadLen);
	if (nExLen > 0)
		memcpy((char *)wrBuffer+pHeader->byHeadLen, pExHead, nExLen);
	memcpy((char *)wrBuffer+pHeader->byHeadLen+nExLen, (char *)pBuffer+pHeader->byHeadLen, nLen-pHeader->byHeadLen);

    pHeader=(SSapMsgHeader *)wrBuffer;
    pHeader->byHeadLen += nExLen;
    pHeader->dwPacketLen = htonl(ntohl(pHeader->dwPacketLen)+nExLen);

    dump_sap_packet_notice(wrBuffer,E_SAP_OUT);
    if(m_isEnc)
    {
    	if(nLeft>0)
    	{
            memset((char *)wrBuffer+nExLen+nLen,0,nLeft);
    	}

        unsigned char *pbyInBlk=(unsigned char *)wrBuffer+pHeader->byHeadLen;
        CCipher::AesEnc(m_arEncKey,pbyInBlk,nFactLen-pHeader->byHeadLen,pbyInBlk);
        pHeader->dwPacketLen=htonl(nFactLen);
    }

	if(pSignatureKey != NULL && strlen(pSignatureKey) > 0)
	{
		int nDigestLen=strlen(pSignatureKey);
		int nMin=(nDigestLen<16?nDigestLen:16);
		memcpy(pHeader->bySignature,pSignatureKey,nMin);
		CCipher::Md5((unsigned char *)wrBuffer,nFactLen,pHeader->bySignature);
	}

    WriteDataInner(wrBuffer,nFactLen,dwTimeout);
}

void CSapConnection::WriteDataInner(const void *pBuffer,int nLen,unsigned int dwTimeout)
{
    SSapLenMsg msg;
    msg.nLen=nLen;
    msg.pBuffer=pBuffer;

    bool write_in_progress = !m_queue.empty();
    m_queue.push_back(msg);
	m_nQueueLen++;
    if (!write_in_progress)
    {
        boost::asio::async_write(m_socket, boost::asio::buffer(m_queue.front().pBuffer,m_queue.front().nLen),
                  MakeSapAllocHandler(m_allocWrite,boost::bind(&CSapConnection::HandleWrite, shared_from_this(),
                    boost::asio::placeholders::error)));
    }
}

void CSapConnection::HandleWrite(const boost::system::error_code& err)
{
    SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d]\n",__FUNCTION__,m_nId);
    SSapMsgHeader *pHeader=NULL;
    unsigned int dwServiceId=0;
    unsigned int dwMsgId=0;
    if(err)
    {
        SS_SLOG(XLOG_WARNING,"CSapConnection::"<<__FUNCTION__<<",id["<<m_nId<<"],error:" <<err.message()<<"\n");
        while(!m_queue.empty())
        {
            SSapMsgHeader *pHeader=(SSapMsgHeader *)(m_queue.front().pBuffer);
            unsigned int dwServiceId=ntohl(pHeader->dwServiceId);
            unsigned int dwMsgId=ntohl(pHeader->dwMsgId);
            if(!(dwServiceId==0&&dwMsgId==0))
            {
                if(pHeader->byIdentifer==SAP_PACKET_REQUEST && m_pSession!=NULL)
                {
                    SSapMsgHeader stSendFailResponse;
                    stSendFailResponse.byIdentifer=SAP_PACKET_RESPONSE;
                    stSendFailResponse.byHeadLen=sizeof(SSapMsgHeader);
                    stSendFailResponse.dwPacketLen=htonl(sizeof(SSapMsgHeader));
                    stSendFailResponse.byVersion=0x1;
                    stSendFailResponse.dwCode=htonl(ERROR_SOS_SEND_FAIL);

                    stSendFailResponse.dwServiceId=pHeader->dwServiceId;
                    stSendFailResponse.dwMsgId=pHeader->dwMsgId;
                    stSendFailResponse.dwSequence=pHeader->dwSequence;
                    m_pSession->OnReceiveSosAvenueResponse(m_nId,&stSendFailResponse,sizeof(stSendFailResponse),m_strRemoteIp,m_dwRemotePort);
                }
                free((void *)(m_queue.front().pBuffer));
            }
            m_queue.pop_front();
			m_nQueueLen--;
        }
        HandleStop();
        return;
    }

    pHeader=(SSapMsgHeader *)(m_queue.front().pBuffer);
    dwServiceId=ntohl(pHeader->dwServiceId);
    dwMsgId=ntohl(pHeader->dwMsgId);
    if(!(dwServiceId==0&&dwMsgId==0))
    {
        free((void *)(m_queue.front().pBuffer));
    }

    m_queue.pop_front();
	m_nQueueLen--;
    if (!m_queue.empty())
    {
        boost::asio::async_write(m_socket,
              boost::asio::buffer(m_queue.front().pBuffer,m_queue.front().nLen),
              MakeSapAllocHandler(m_allocWrite,boost::bind(&CSapConnection::HandleWrite, shared_from_this(),
                boost::asio::placeholders::error)));
    }
}

void CSapConnection::DoDetectInterval()
{
    struct timeval_a now;
    gettimeofday_a(&now,NULL);

    struct timeval_a interval,endtime;
      interval.tv_sec=HeartInterval;
    interval.tv_usec=0;
    timeradd(&m_HeartBeatPoint,&interval,&endtime);
    if(timercmp(&now,&endtime,>=))
	{
        m_heartRequest.SetSequence(m_heartSequence++);
        dump_sap_packet_info(m_heartRequest.GetBuffer(),E_SAP_OUT);
        WriteDataInner(m_heartRequest.GetBuffer(),m_heartRequest.GetLen());
        m_HeartBeatPoint=now;
    }

    interval.tv_sec=TimeoutInterval;
    interval.tv_usec=0;
    timeradd(&m_AlivePoint,&interval,&endtime);
    if(timercmp(&now,&endtime,>))
    {
        SS_XLOG(XLOG_WARNING,"CSapConnection::%s,id[%d] timeout, will reconnect.alive[%ld.%06ld] now[%ld.%06ld]\n",
            __FUNCTION__,m_nId,m_AlivePoint.tv_sec,m_AlivePoint.tv_usec,
            now.tv_sec,now.tv_usec);
        StopConnection();
    }
}

void CSapConnection::Close()
{
    SS_XLOG(XLOG_DEBUG,"CSapConnection::%s,id[%d]\n",__FUNCTION__,m_nId);
    bRunning=false;
    HandleStop();
}

void CSapConnection::HandleStop()
{
    SS_XLOG(XLOG_TRACE,"CSapConnection::%s,id[%d]\n",__FUNCTION__,m_nId);
    boost::system::error_code ignore_ec;
    m_socket.close(ignore_ec);
}

void CSapConnection::StopConnection()
{
	SS_XLOG(XLOG_TRACE,"CSapConnection::%s,id[%d]\n",__FUNCTION__,m_nId);
    boost::system::error_code ignore_ec;
    m_socket.close(ignore_ec);
    bRunning=false;
	m_isConnected = false;
	if(m_pSession != NULL)
	    m_pSession->OnPeerClose(m_nId);

	m_tmHeartBeat.Stop();

}

void CSapConnection::SetRemoteAddr()
{
    try{
        m_strRemoteIp=m_socket.remote_endpoint().address().to_string();
        m_dwRemotePort=m_socket.remote_endpoint().port();
    }
    catch(boost::system::system_error & ec){
        SS_SLOG(XLOG_WARNING,"CSapConnection::SetRemoteAddr,code:" <<ec.what()<<"\n");
        m_strRemoteIp="";
        m_dwRemotePort=0;
        return;
    }
}

void CSapConnection::SetLocalDigest(const string &strDigest)
{
    m_isLocalDigest=true;
	m_strLocalKey=strDigest;
}

void CSapConnection::SetPeerDigest(const string &strDigest)
{
    m_isPeerDigest=true;
    m_strPeerKey=strDigest;
}

void CSapConnection::SetEncKey(unsigned char arKey[16])
{
    m_isEnc=true;
	memcpy(m_arEncKey,arKey,16);
}

void CSapConnection::dump_sap_packet_notice(const void *pBuffer,ESapDirectType type)
{
    if(IsEnableLevel(SAP_STACK_MODULE,XLOG_NOTICE))
    {
        string strPacket;
        char szPeer[128]={0};
        char szId[16]={0};
        sprintf(szId,"%d",m_nId);
        sprintf(szPeer,"%s:%d",m_strRemoteIp.c_str() ,m_dwRemotePort);

        if(type==E_SAP_IN)
        {
            strPacket=string("Read Sap Command from id[")+szId+"] addr["+szPeer+"]\n";
        }
        else
        {
            strPacket=string("Write Sap Command to id[")+szId+"] addr["+szPeer+"]\n";
        }

        SSapMsgHeader *pHeader=(SSapMsgHeader *)pBuffer;

        unsigned char *pChar=(unsigned char *)pBuffer;
        unsigned int nPacketlen=ntohl(pHeader->dwPacketLen);
        int nLine=nPacketlen>>3;
        int nLast=(nPacketlen&0x7);
        int i=0;
        for(i=0;i<nLine;i++)
        {
            char szBuf[128]={0};
            unsigned char * pBase=pChar+(i<<3);
            snprintf(szBuf,127,"                [%2d]    %02x %02x %02x %02x    %02x %02x %02x %02x    ......\n",
                i,*(pBase),*(pBase+1),*(pBase+2),*(pBase+3),*(pBase+4),*(pBase+5),*(pBase+6),*(pBase+7));
            strPacket+=szBuf;
        }

        unsigned char * pBase=pChar+(i<<3);
        if(nLast>0)
        {
            for(int j=0;j<8;j++)
            {
                char szBuf[128]={0};
                if(j==0)
                {
                    sprintf(szBuf,"                [%2d]    %02x ",i,*(pBase+j));
                    strPacket+=szBuf;
                }
                else if(j<nLast&&j==3)
                {
                    sprintf(szBuf,"%02x    ",*(pBase+j));
                    strPacket+=szBuf;
                }
                else if(j>=nLast&&j==3)
                {
                    strPacket+="      ";
                }
                else if(j<nLast)
                {
                    sprintf(szBuf,"%02x ",*(pBase+j));
                    strPacket+=szBuf;
                }
                else
                {
                    strPacket+="   ";
                }

            }
            strPacket+="   ......\n";
        }
        SS_XLOG(XLOG_NOTICE,strPacket.c_str());
    }
}
void CSapConnection::dump_sap_packet_info(const void *pBuffer,ESapDirectType type)
{
    if(IsEnableLevel(SAP_STACK_MODULE,XLOG_INFO))
    {
        string strPacket;
        char szPeer[64]={0};
        char szId[16]={0};
        sprintf(szId,"%d",m_nId);
        sprintf(szPeer,"%s:%d",m_strRemoteIp.c_str() ,m_dwRemotePort);

        if(type==E_SAP_IN)
        {
            strPacket=string("Read Sap Command from id[")+szId+"] addr["+szPeer+"]\n";
        }
        else
        {
            strPacket=string("Write Sap Command to id[")+szId+"] addr["+szPeer+"]\n";
        }

        SSapMsgHeader *pHeader=(SSapMsgHeader *)pBuffer;

        unsigned char *pChar=(unsigned char *)pBuffer;
        unsigned int nPacketlen=ntohl(pHeader->dwPacketLen);
        int nLine=nPacketlen>>3;
        int nLast=(nPacketlen&0x7);
        int i=0;
        for(i=0;i<nLine;i++)
        {
            char szBuf[128]={0};
            unsigned char * pBase=pChar+(i<<3);
            snprintf(szBuf,127,"                [%2d]    %02x %02x %02x %02x    %02x %02x %02x %02x    ......\n",
                i,*(pBase),*(pBase+1),*(pBase+2),*(pBase+3),*(pBase+4),*(pBase+5),*(pBase+6),*(pBase+7));
            strPacket+=szBuf;
        }

        unsigned char * pBase=pChar+(i<<3);
        if(nLast>0)
        {
            for(int j=0;j<8;j++)
            {
                char szBuf[128]={0};
                if(j==0)
                {
                    sprintf(szBuf,"                [%2d]    %02x ",i,*(pBase+j));
                    strPacket+=szBuf;
                }
                else if(j<nLast&&j==3)
                {
                    sprintf(szBuf,"%02x    ",*(pBase+j));
                    strPacket+=szBuf;
                }
                else if(j>=nLast&&j==3)
                {
                    strPacket+="      ";
                }
                else if(j<nLast)
                {
                    sprintf(szBuf,"%02x ",*(pBase+j));
                    strPacket+=szBuf;
                }
                else
                {
                    strPacket+="   ";
                }

            }
            strPacket+="   ......\n";
        }
        SS_XLOG(XLOG_INFO,strPacket.c_str());
    }
}

max_stars_repo_name: shrewdlin/BPE

max_issues_repo_name: shrewdlin/BPE

max_forks_repo_name: shrewdlin/BPE


text:
#pragma comment(lib,"winmm.lib")
#pragma comment(lib,"wsock32.lib")

#include "rtklib/rtklib.h"
#include <Eigen/Dense>

#include "utilities.h"
#include "WeightLeastSquare.h"
#include "SelectEphemeris.h"

#include "shadowMatching.h"

//#define TARGETWEEK 1997
//#define STARTTIME 218012
//#define ENDTIME   218014

std::string main_directory = "result\\";
std::string data_name = "v2v107M";

//read the rinex obx and nav(ephemeris) using rtklib function
extern int readobsnav(gtime_t ts, gtime_t te, double ti, char **infile,
	const int *index, int n, const prcopt_t *prcopt,
	obs_t *obs, nav_t *nav, sta_t *sta)
{
	int nepoch = 0;            /* number of observation epochs */
	int i, j, ind = 0, nobs = 0, rcv = 1;
	printf("%d %d %d %d\n", index[0], index[1], index[2], index[3]); // to read two obs file
	obs->data = NULL; obs->n = obs->nmax = 0;
	nav->eph = NULL; nav->n = nav->nmax = 0;
	nav->geph = NULL; nav->ng = nav->ngmax = 0;
	nav->seph = NULL; nav->ns = nav->nsmax = 0;

	for (i = 0; i < n; i++) {

		if (index[i] != ind) {
			if (obs->n > nobs) rcv++;
			ind = index[i]; nobs = obs->n;
		}
		//read rinex obs and nav file
		if (readrnxt(infile[i], rcv, ts, te, ti, prcopt->rnxopt[rcv <= 1 ? 0 : 1], obs, nav,
			rcv <= 2 ? sta + rcv - 1 : NULL) < 0) {
			return 0;
		}
	}

	if (obs->n <= 0) {
		return 0;
	}

	nepoch = sortobs(obs);

	/*
	if (nav->n<=0&&nav->ng<=0&&nav->ns<=0) {
	return 0;
	}
	*/
	// delete duplicated ephemeris
	//uniqnav(nav);

	// set time span for progress display
	if (ts.time == 0 || te.time == 0)
	{
		for (i = 0; i < obs->n; i++)
			if (obs->data[i].rcv == 1)
				break;
		for (j = obs->n - 1; j >= 0; j--)
			if (obs->data[j].rcv == 1)
				break;
		if (i < j) {
			if (ts.time == 0) ts = obs->data[i].time;
			if (te.time == 0) te = obs->data[j].time;
			settspan(ts, te);
		}
	}

	return 1;
}

//read the precise ephemeris (not used in 3D-GNSS code)
static void readpreceph(char **infile, int n, const prcopt_t *prcopt,
	nav_t *nav, sbs_t *sbs, lex_t *lex)
{
	seph_t seph0 = { 0 };
	int i;
	//    char *ext;

	trace(3, "readpreceph: n=%d\n", n);

	nav->ne = nav->nemax = 0;
	nav->nc = nav->ncmax = 0;
	sbs->n = sbs->nmax = 0;
	lex->n = lex->nmax = 0;

	/* read precise ephemeris files */
	for (i = 0; i < n; i++) {
		if (strstr(infile[i], "%r") || strstr(infile[i], "%b")) continue;
		readsp3(infile[i], nav, 0);
	}
	/* read precise clock files */
	for (i = 0; i < n; i++) {
		if (strstr(infile[i], "%r") || strstr(infile[i], "%b")) continue;
		readrnxc(infile[i], nav);
	}
	/* read sbas message files */
	for (i = 0; i < n; i++) {
		if (strstr(infile[i], "%r") || strstr(infile[i], "%b")) continue;
		sbsreadmsg(infile[i], prcopt->sbassatsel, sbs);
	}
	/* read lex message files */
	for (i = 0; i < n; i++) {
		if (strstr(infile[i], "%r") || strstr(infile[i], "%b")) continue;
		lexreadmsg(infile[i], 0, lex);
	}
	/* allocate sbas ephemeris */
	nav->ns = nav->nsmax = NSATSBS * 2;
	if (!(nav->seph = (seph_t *)malloc(sizeof(seph_t)*nav->ns))) {
		showmsg("error : sbas ephem memory allocation");
		trace(1, "error : sbas ephem memory allocation");
		return;
	}
	for (i = 0; i < nav->ns; i++) nav->seph[i] = seph0;

}

/* main function */
int main(int argc, char * argv[])
{

#ifdef STARTTIME
	gtime_t ts = gpst2time(TARGETWEEK, STARTTIME);
#else
	gtime_t ts = { 0 }; ts.time = 0;							// start time (0:all)
#endif

#ifdef 	ENDTIME
	gtime_t te = gpst2time(TARGETWEEK, ENDTIME);
#else
	gtime_t te = { 0 }; te.time = 0;							// end time (0:all)
#endif

		/* Configuration of RTKLIB ------------------------*/
	int n = 0, i, stat;

	double ti = 0.0;						// processing interval  (s) (0:all)
	double tu = 0.0;						// unit time (s) (0:all)

	/* options */
	prcopt_t prcopt = prcopt_default;	// processing option
	solopt_t solopt = solopt_default;	// output solution option
	filopt_t filopt = { "" };	            // file option

	prcopt.mode = 2;				// 0: SPP 1: DGNSS 2:Kinematic RTK
	prcopt.navsys = SYS_ALL;
	prcopt.nf = 1;						// frequency (1:L1,2:L1+L2,3:L1+L2+L5)
	prcopt.soltype = 0;					// 0:forward,1:backward,2:combined
	prcopt.elmin = 10.0*D2R;				// elevation mask (rad)
	prcopt.tidecorr = 0;					// earth tide correction (0:off,1-:on)
	prcopt.posopt[4] = 0;               // use RAIM FDE (qmo)
	prcopt.tropopt = TROPOPT_SAAS;        // troposphere option: Saastamoinen model
	prcopt.ionoopt = IONOOPT_BRDC;		// ionosphere option: Broad cast
	prcopt.sateph = EPHOPT_BRDC;			// ephemeris option: broadcast ephemeris

	prcopt.modear = 1;					// AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold)

	// HKSC
	prcopt.rb[0] = -2414266.9197;           // base position for relative mode {x,y,z} (ecef) (m)
	prcopt.rb[1] = 5386768.9868;			// base position for relative mode {x,y,z} (ecef) (m)
	prcopt.rb[2] = 2407460.0314;			// base position for relative mode {x,y,z} (ecef) (m)

	solopt.outopt = 1;					// output processing options (0:no,1:yes)
	solopt.timef = 0;						// time format (0:sssss.s,1:yyyy/mm/dd hh:mm:ss.s)
	solopt.timeu = 3;						// time digits under decimal point
	solopt.sep[0] = ',';					// field separator
	solopt.sstat = 0;						// solution statistics level (0:off,1:states,2:residuals)
	solopt.trace = 0;						// debug trace level (0:off,1-5:debug)
	solopt.sstat = 0;						// get the solution file
	solopt.posf = SOLF_XYZ;
	solopt.height = 0;

	char *rov = "", *base = "";
	char infile_[5][1024] = { "" }, *infile[5];
	char outfile[1024];

	shadowMatching SM_;

	/* set input files */
	for (i = 0; i < 5; i++) infile[i] = infile_[i];
	strcpy(infile[n++], "data\\v2v107M2.obs");
	strcpy(infile[n++], "data\\hksc107m.18n");
	strcpy(infile[n++], "data\\hksc107m.18g");

	//strcpy(infile[n++], "data\\TSTb120L.obs");
	//strcpy(infile[n++], "data\\hksc1200.18n");
	//strcpy(infile[n++], "data\\hksc1200.18g");

	/**************************************************/

	/*starting to read the obs and nav rinex data*/
	int index[4] = { 0 };

	obs_t obss = { 0 };          /* observation data */
	nav_t navs = { 0 };          /* navigation data */
	sta_t stas[MAXRCV];      /* station infomation */
	lex_t lexs = { 0 };          /* lex messages */

	if (prcopt.mode == PMODE_KINEMA || prcopt.mode == PMODE_DGPS) {
		index[1] = 1;
		index[2] = 2;
		index[3] = 0;
	}

	// Step1: rinex obs and nav to variable obss and navs! @@@
	if (!readobsnav(ts, te, ti, infile, index, n, &prcopt, &obss, &navs, stas)) return 0;
	printf("obss.n = %d\n", obss.n);
	printf("navs.n navs.ng = %d %d\n", navs.n, navs.ng);

	// show raw measurement obss nav

	/*Initializa the output file in RTKLIB format*/
	FILE * fp;
	//std::string rtklibName2 = main_directory + data_name + ".pos";
	std::string rtklibName2 = main_directory + "SM_" + ".pos";
	fopen_s(&fp, rtklibName2.c_str(), "w");
	solopt.posf = SOLF_LLH;
	if (fp)
	{
		outheader_Q(fp, infile, n, &prcopt, &solopt, &obss);
		fclose(fp);
	}
	/*---------------------------*/

	/**************************************************/

	/* Step2: starting to use obs and nav rinex data @@@*/

	int i_obs, j_obs; // (i_obs,j_obs) are the (start,end) of rover obs.
	//for (i_obs = 0; i_obs<obss.n; i_obs++)
	//	printf("%d %d \t", i_obs, obss.data[i_obs].rcv);
	for (i_obs = 0; i_obs < obss.n; i_obs++)
		if (obss.data[i_obs].rcv == 1)
			break;

	for (j_obs = obss.n - 1; j_obs >= 0; j_obs--)
		if (obss.data[j_obs].rcv == 1)
			break;

	bool bFindSameData = false;

	int IndexStart = i_obs, IndexEnd = 0;
	int idx_epoch = 0;
	// note that the for is not every epoch of gps time
	for (int idx = i_obs; idx < j_obs + 1; idx++)
	{
		if (idx == obss.n + 1)
		{
			break;
		}

		int current_week1, current_week2;
		double current_tow1, current_tow2;
		current_tow1 = time2gpst(obss.data[idx].time, &current_week1);
		current_tow2 = time2gpst(obss.data[idx + 1].time, &current_week2);

		if ((current_week1 != current_week2) || fabs(current_tow1 - current_tow2) >0.49) // assuming 1Hz data rate. (if data rate higher than 2 Hz this does not valid)
		{
			IndexEnd = idx; // for current epoch
			bFindSameData = true;
		}
		//printf("IndexStart = %d, IndexEnd = %d\n", IndexStart, IndexEnd);

		/* Step3:find the measurements at the same epoch @@@*/

		if (bFindSameData)
		{
			idx_epoch++;
			int current_week;
			double current_tow;
			current_tow = time2gpst(obss.data[IndexStart].time, &current_week);
			printf("current_week = %4d - current_tow = %6.0f\n", current_week, current_tow);

			/* Step3.1:construct measurement matrix  @@@*/
			Eigen::MatrixXd eAllMeasurement; // (n,3) PRN CNO Pseudorange
			eAllMeasurement = ConstructMeasurementMatrix(IndexStart, IndexEnd, &obss);

			/* Step3.2: Select Ephemeris*/
			int target_idx[100], target_glo_idx[100]; // maximun number of satellite is 100. (GPS and GLONASS)
			int iNumValidSV = 0, iNumValidSV_glo = 0;
			SelectEphemeris(&navs, current_week, current_tow, target_idx, target_glo_idx, iNumValidSV, iNumValidSV_glo);

			/* Step3.3: caculating satellite positions @@@*/
			// caculating satellite positions (I suggest to make a structure about satellite position related information)
			Eigen::MatrixXd eAllSVPositions; // (n,5) prn, sx, sy, sz,
			eAllSVPositions = SatellitePosition(&navs, current_week, current_tow, target_idx, target_glo_idx, iNumValidSV, iNumValidSV_glo);

			/* Step3.4: WLS @@@
			initial guess for shadow Matching is availalble,
			*/
			Eigen::MatrixXd  eWLSSolutionECEF; // 6 unknowns with two clock bias variables
			eWLSSolutionECEF = WeightLeastSquare(eAllSVPositions, eAllMeasurement);

			// example of coordinate transformation from ECEF to LLH (please see more in rtklib.h. hint:F12 can trace back the function)
			Eigen::MatrixXd eWLSSolutionLLH(3, 1);
			double pos[3], r[3];
			for (int idx = 0; idx < 3; idx++) {
				pos[idx] = eWLSSolutionECEF(idx);
				//printf("%d %f %f\n", idx, eWLSSolutionECEF(idx), pos[idx]);
			}
			ecef2pos(pos, r);
			for (int idx = 0; idx < 3; idx++) {
				eWLSSolutionLLH(idx) = r[idx];
				//printf("%d %f %f\n", idx, eWLSSolutionLLH(idx), r[idx]);
			}

			// example to calculate azimuth and angle 2018/05/17 by LT Hsu
			Eigen::MatrixXd eELAZAngles;
			eELAZAngles = CalcELAZAngle(eWLSSolutionLLH, eAllSVPositions);

			// Add by Weisong WEN
			clock_t begin_time = clock();
			vector<shadowMatching::particle> particlesMain;
			//cout << "eAllMeasurement.size-> " << eAllMeasurement.rows() << "  eELAZAngles.size->  " << eELAZAngles.rows() << "\n";
			MatrixXd llh_;
			MatrixXd ecef_;
			for (int index = 0; index < 2; index++)
			{
				particlesMain = SM_.generateParticles(eWLSSolutionECEF, 50, 2, eAllMeasurement, eELAZAngles);
				llh_ = SM_.weightSM_();
				ecef_ = SM_.llh2ecef(llh_);
				SM_.particles.clear();

			}
			std::cout << "shadow matching  result: \n" << llh_ << "\n\n";
			std::cout << "one circle  used  time ---------------------------------------------------------------------------------------------> " << float(clock() - begin_time) / CLOCKS_PER_SEC << "\n\n";
			//Output as RTKLIB format
			fopen_s(&fp, rtklibName2.c_str(), "a");
			if (fp)
			{
				//fprintf(fp, "%4d, %9.3f,  %11.9f, %12.9f,  %9.4f, %d\n", current_week, current_tow, eWLSSolutionLLH(0)*R2D, eWLSSolutionLLH(1)*R2D, eWLSSolutionLLH(2),1);
				fprintf(fp, "%4d, %9.3f,  %11.9f, %12.9f,  %9.4f, %d\n", current_week, current_tow, llh_(1), llh_(0), llh_(2), 1);
				fclose(fp);
			}

			// End of an epoch
			IndexStart = idx + 1; // don't touch this InderStart
			//printf("\n");
			bFindSameData = false;

			// coordinate convertion.

		}

	}
	/**************************************************/

	//shadowMatching SM_;
	system("pause");
	return 0;
}

/* dummy application functions for shared library ----------------------------*/
extern int showmsg(char *format, ...) {
	va_list arg;
	char buff[1024];
	if (*format) {
		va_start(arg, format);
		vsprintf(buff, format, arg);
		va_end(arg);
		printf("%s\n", buff);
	}
	return 0;
}
extern void settspan(gtime_t ts, gtime_t te) {}
extern void settime(gtime_t time) {}

ext: cpp

max_stars_repo_name: StanleyHusai/mapping


text:
//
//  Mesh.hpp
//  DOT
//
//  Created by Minchen Li on 8/30/17.
//

#ifndef Mesh_hpp
#define Mesh_hpp

#include <Eigen/Eigen>

#include <igl/massmatrix.h>

#include <set>
#include <array>

#include "Types.hpp"

namespace DOT{

    enum Primitive
    {
        P_GRID,
        P_SQUARE,
        P_RECTANGLE,
        P_SPIKES,
        P_SHARKEY,
        P_CYLINDER,
        P_INPUT
    };

    // duplicate the vertices and edges of a mesh to separate its triangles,
    // adjacent triangles in the original mesh will have a cohesive edge structure to
    // indicate the connectivity
    template<int dim>
    class Mesh{
    public: // owned data
        Eigen::MatrixXd V_rest; // duplicated rest vertex coordinates in 3D
        Eigen::MatrixXd V; // duplicated vertex coordinates, the dimension depends on the search space
        Eigen::MatrixXi F; // reordered triangle draw list (0, 1, 2, ...), indices based on V

    public: // owned features
        Eigen::SparseMatrix<double> massMatrix; // V.rows() wide
        double density, m_YM, m_PR;
        Eigen::VectorXd u, lambda;
        Eigen::VectorXd triArea; // triangle rest area
        double surfaceArea;
        double avgEdgeLen;
        std::set<int> fixedVert; // for linear solve
        std::vector<bool> isFixedVert;
        Eigen::Matrix<double, 2, 3> bbox;
        Eigen::VectorXd triWeight; // for weighted stencil
        std::vector<std::vector<int>> borderVerts_primitive;
        std::vector<Eigen::Matrix<double, dim, dim>> restTriInv;
        std::vector<bool> m_isBoundaryVert;

        // indices for fast access
        std::vector<std::set<int>> vNeighbor;
        std::vector<std::set<std::pair<int, int>>> vFLoc;

    public: // constructor
        // default constructor that doesn't do anything
        Mesh(void);

        // initialize from a triangle mesh, V will be constructed from UV_mesh in 2D,
        // V_mesh will be used to initialize restShape
        Mesh(const Eigen::MatrixXd& V_mesh, const Eigen::MatrixXi& F_mesh,
                     const Eigen::MatrixXd& Vt_mesh,
                     double YM, double PR, double rho);

        Mesh(Primitive primitive, double size, int elemAmt,
                     double YM, double PR, double rho);

    public: // API
        void computeMassMatrix(const igl::MassMatrixType type = igl::MASSMATRIX_TYPE_VORONOI);
        void computeFeatures(bool multiComp = false, bool resetFixedV = false);
        void resetFixedVert(const std::set<int>& p_fixedVert = std::set<int>());
        void addFixedVert(int vI);
        void addFixedVert(const std::vector<int>& p_fixedVert);
        void removeFixedVert(int vI);

        void setLameParam(double YM, double PR);

        bool checkInversion(int triI, bool mute) const;
        bool checkInversion(bool mute = false, const std::vector<int>& triangles = std::vector<int>()) const;

        void saveAsMesh(const std::string& filePath, bool scaleUV = false,
                        const Eigen::MatrixXi& SF = Eigen::MatrixXi()) const;

        void constructSubmesh(const Eigen::VectorXi& triangles,
                              Mesh& submesh,
                              std::map<int, int>& globalVIToLocal,
                              std::map<int, int>& globalTriIToLocal) const;

    public: // helper function
        bool isBoundaryVert(int vI) const;
        void computeBoundaryVert(const Eigen::MatrixXi& SF);
    };

}

#endif /* Mesh_hpp */

ext: hpp

max_stars_repo_name: liminchen/DOT

max_issues_repo_name: liminchen/DOT

max_forks_repo_name: liminchen/DOT


text:
/**
   $ g++ -I/path/to/boost sieve.cpp -o sieve && sieve 10000000
 */
#include <inttypes.h> // uintmax_t
#include <limits>
#include <cmath>
#include <iostream>
#include <sstream>
#include <vector>

#include <boost/lambda/lambda.hpp>

int main(int argc, char *argv[])
{
  using namespace std;
  using namespace boost::lambda;

  int limit = 10000;
  if (argc == 2) {
    stringstream ss(argv[--argc]);
    ss >> limit;

    if (limit < 1 or ss.fail()) {
      cerr << "USAGE:\n  sieve LIMIT\n\nwhere LIMIT in the range [1, "
	   << numeric_limits<int>::max() << ")" << endl;
      return 2;
    }
  }

  // print primes less then 100
  primesupto(100, cout << _1 << " ");
  cout << endl;

  // find number of primes less then limit and their sum
  int count = 0;
  uintmax_t sum = 0;
  primesupto(limit, (var(sum) += _1, var(count) += 1));

  cout << "limit sum pi(n)\n"
       << limit << " " << sum << " " << count << endl;
}

max_stars_repo_name: djgoku/RosettaCodeData

max_issues_repo_name: djgoku/RosettaCodeData

max_forks_repo_name: djgoku/RosettaCodeData


text:
#include <iostream>
#include <signal.h>
#include <cstdlib>
#include <boost/program_options.hpp>
#include <thread>
#include <atomic>
#include <experimental/filesystem>
#include <chrono>

#include <sdm/types.hpp>
#include <sdm/config.hpp>
#include <sdm/common.hpp>
#include <sdm/parser/parser.hpp>
#include <sdm/public/algorithm.hpp>
#include <sdm/algorithms.hpp>
#include <sdm/worlds.hpp>
#include <sdm/core/state/private_occupancy_state.hpp>
#include <sdm/utils/value_function/qfunction/pwlc_qvalue_function.hpp>

using namespace sdm;
using namespace std;
namespace po = boost::program_options;

double MAX_RUNNING_TIME = 1800;
bool quit_flag = false;
std::string name = "result";
std::shared_ptr<Algorithm> algorithm;

void leave()
{
    algorithm->logging();
    std::cout << "\n"
              << config::LOG_SDMS << "Exit process" << std::endl;
    algorithm = nullptr;
    exit(0);
}

void handler(int)
{
    leave();
}

int solve(int argv, char **args)
{
    // Handle interrupt signal
    signal(SIGINT, &handler);

    try
    {
        std::string world, algo_name, formalism, upper_bound, lower_bound, ub_init, lb_init, type_sampling;
        int trials, memory;
        number horizon, seed, batch_size, freq_update_lb, freq_update_ub, state_type;
        double error, discount, granularity_start, granularity_end, rate_start, rate_end, rate_decay, eps_start, eps_end, eps_decay;
        double p_b, p_o, p_c;
        bool store_actions, store_states;
        unsigned long long num_samples;

        int freq_pruning_v1 = 1, freq_pruning_v2 = 1;
        std::string type_of_resolution_v1, type_of_resolution_v2, type_of_pruning_v1, type_of_pruning_v2;

        po::options_description options("Options");
        options.add_options()
        ("help", "produce help message")
        ("test", "test the policy found")
        ("save", "save the policy found");

        po::options_description config("Configuration");
        config.add_options()
        ("algorithm,a", po::value<string>(&algo_name)->default_value("hsvi"), "the algorithm to use")
        ("world,w", po::value<string>(&world)->default_value("mabc.dpomdp"), "the world to be solved")
        ("formalism,f", po::value<string>(&formalism)->default_value("OccupancyMDP"), "the formalism to use")
        ("horizon,h", po::value<number>(&horizon)->default_value(5), "the planning horizon")
        ("discount,d", po::value<double>(&discount)->default_value(1.0), "the discount factor")
        ("memory,m", po::value<int>(&memory)->default_value(-1), "the memory for history")
        ("error,e", po::value<double>(&error)->default_value(0.001), "the error")
        ("trials,t", po::value<int>(&trials)->default_value(100000), "the maximum number of trials")
        ("seed,s", po::value<number>(&seed)->default_value(1), "the seed")
        ("batch_size,b", po::value<number>(&batch_size)->default_value(0), "the batch size used in rl")
        ("name,n", po::value<std::string>(&name)->default_value(""), "the name of the experiment")
        ("compression", po::value<number>(&state_type)->default_value(0), "The type of occupancy state (COMPRESSED, ONE_STEP, UNCOMPRESSED)")
        ("store_states", po::value<bool>(&store_states)->default_value(true), "If true, store the macro states.")
        ("store_actions", po::value<bool>(&store_actions)->default_value(true), "If true, store the macro actions.")
        ("p_c", po::value<double>(&p_c)->default_value(config::PRECISION_COMPRESSION), "The precision of the compression.")
        ("p_b", po::value<double>(&p_b)->default_value(config::PRECISION_BELIEF), "The precision of beliefs.")
        ("p_o", po::value<double>(&p_o)->default_value(config::PRECISION_OCCUPANCY_STATE), "The precision of occupancy states.")
        ("time_max", po::value<double>(&MAX_RUNNING_TIME)->default_value(1800), "The maximum running time.");

        po::options_description hsvi_config("HSVI configuration");
        hsvi_config.add_options()
        ("lower_bound", po::value<string>(&lower_bound)->default_value("tabular"), "the lower bound representation")
        ("upper_bound", po::value<string>(&upper_bound)->default_value("tabular"), "the upper bound representation")
        ("lb_init", po::value<string>(&lb_init)->default_value("Min"), "the lower bound initialization method")
        ("ub_init", po::value<string>(&ub_init)->default_value("Max"), "the upper bound initialization method")
        ("freq_update_lb", po::value<number>(&freq_update_lb)->default_value(1), "the update frequency of the lower bound.")
        ("freq_update_ub", po::value<number>(&freq_update_ub)->default_value(1), "the update frequency of the upper bound.")
        ("lb_type_of_resolution", po::value<string>(&type_of_resolution_v1)->default_value("IloIfThen"), "the type of resolution for the lower bound (ex: 'BigM:100' or 'IloIfThen' for LP)")
        ("ub_type_of_resolution", po::value<string>(&type_of_resolution_v2)->default_value("IloIfThen"), "the type of resolution for the upper bound (ex: 'BigM:100' or 'IloIfThen' for LP)")
        ("lb_freq_pruning", po::value<int>(&freq_pruning_v1)->default_value(1), "the pruning frequency for the first value function.")
        ("ub_freq_pruning", po::value<int>(&freq_pruning_v2)->default_value(1), "the pruning frequency for the second value function .")
        ("lb_type_of_pruning", po::value<string>(&type_of_pruning_v1)->default_value("none"), "the pruning type for the lower bound (ex: 'bounded', 'pairwise', 'none'")
        ("ub_type_of_pruning", po::value<string>(&type_of_pruning_v2)->default_value("none"), "the pruning type for the upper bound (ex: 'iterative', 'global', 'none'");

        po::options_description pbvi_config("PBVI configuration");
        pbvi_config.add_options()
        ("num_samples", po::value<unsigned long long>(&num_samples)->default_value(10), "the number of sample to generate in the algorithm")
        ("type_sampling", po::value<string>(&type_sampling)->default_value(""), "the type of sampling process")
        ("value_function", po::value<string>(&lower_bound), "the lower bound representation")
        ("vf_init", po::value<string>(&lb_init), "the lower bound initialization method")
        ("freq_update", po::value<number>(&freq_update_lb), "the update frequency of the lower bound.")
        ("type_of_resolution", po::value<string>(&type_of_resolution_v1), "the type of resolution for the lower bound (ex: 'BigM:100' or 'IloIfThen' for LP)")
        ("freq_pruning", po::value<int>(&freq_pruning_v1), "the pruning frequency for the first value function.")
        ("type_of_pruning", po::value<string>(&type_of_pruning_v1), "the pruning type for the lower bound (ex: 'bounded', 'pairwise', 'none'");

        po::options_description qlearning_config("Q-learning configuration");
        qlearning_config.add_options()
        ("qvalue_function", po::value<string>(&lower_bound), "the q-value function representation")
        ("eps_start", po::value<double>(&eps_start)->default_value(1.0), "the starting epsilon")
        ("eps_end", po::value<double>(&eps_end)->default_value(0.001), "the final epsilon")
        ("eps_decay", po::value<double>(&eps_decay)->default_value(-1), "the decaying epsilon factor (default to linear)")
        ("rate_start", po::value<double>(&rate_start)->default_value(1.0), "the starting learning rate")
        ("rate_end", po::value<double>(&rate_end)->default_value(0.001), "the final learning rate")
        ("rate_decay", po::value<double>(&rate_decay)->default_value(10000), "the decaying factor")
        ("rate_decay_start_time", po::value<double>(&QLearning::RATE_DECAY_START_TIME)->default_value(43200), "the decaying factor")
        ("rate_decay_duration", po::value<double>(&QLearning::DURATION_RATE_DECAY)->default_value(43200), "the decaying factor")
        ("q_init", po::value<string>(&lb_init), "the q-value function initialization method")
        ("g_start", po::value<double>(&granularity_start)->default_value(oPWLCQ::GRANULARITY_START), "The granularity...")
        ("g_end", po::value<double>(&granularity_end)->default_value(oPWLCQ::GRANULARITY_END), "The granularity...")
        ;

        po::options_description byg_config("Bayesian game solver configuration");
        byg_config.add_options()
        ("player_id", po::value<number>(&batch_size), "the identifier of player");

        po::options_description visible("\nUsage:\tsdms-solve [CONFIGS]\n\tSDMStudio solve [CONFIGS]\n\nSolve a problem with specified algorithms and configurations.");
        visible.add(options).add(config).add(hsvi_config).add(pbvi_config).add(qlearning_config).add(byg_config);

        po::options_description config_file_options;
        config_file_options.add(config);

        po::variables_map vm;
        try
        {
            po::store(po::command_line_parser(argv, args).options(visible).run(), vm);
            po::notify(vm);
            if (vm.count("help"))
            {
                std::cout << visible << std::endl;
                return sdm::SUCCESS;
            }
            if (discount >= 1 && !horizon)
            {
                std::cerr << config::LOG_SDMS << "Invalid hyperparameters (discount must be less than 1. for infinite horizon)" << std::endl;
                return sdm::ERROR_IN_COMMAND_LINE;
            }
        }
        catch (po::error &e)
        {
            std::cerr << "ERROR: " << e.what() << std::endl;
            std::cerr << visible << std::endl;
            return sdm::ERROR_IN_COMMAND_LINE;
        }

        // Set the seed
        common::global_urng().seed(seed);

        // Set precisions
        Belief::PRECISION = p_b;
        OccupancyState::PRECISION = p_o;
        PrivateOccupancyState::PRECISION_COMPRESSION = p_c;
        oPWLCQ::GRANULARITY_START = granularity_start;
        oPWLCQ::GRANULARITY_END = granularity_end;

        common::logo();

        // Build algorithm
        algorithm = sdm::algo::make(algo_name,
                                    world,
                                    formalism,
                                    horizon,
                                    discount,
                                    error,
                                    trials,
                                    MAX_RUNNING_TIME,
                                    name,
                                    memory,
                                    (StateType)state_type,
                                    store_states,
                                    store_actions,
                                    batch_size,
                                    num_samples,
                                    type_sampling,
                                    rate_start,
                                    rate_end,
                                    rate_decay,
                                    eps_start,
                                    eps_end,
                                    eps_decay,
                                    lower_bound,
                                    lb_init,
                                    freq_update_lb,
                                    type_of_resolution_v1,
                                    freq_pruning_v1,
                                    type_of_pruning_v1,
                                    upper_bound,
                                    ub_init,
                                    freq_update_ub,
                                    type_of_resolution_v2,
                                    freq_pruning_v2,
                                    type_of_pruning_v2);

        // Initialize algorithm
        algorithm->initialize();

        // Solve the problem
        algorithm->solve();

        if (vm.count("test"))
        {
            algorithm->test();
        }
        if (vm.count("save"))
        {
            algorithm->save();
        }
    }
    catch (std::exception &e)
    {
        std::cerr << "Unhandled Exception reached the top of main: " << e.what() << std::endl;
        return sdm::ERROR_UNHANDLED_EXCEPTION;
    }

    return sdm::SUCCESS;
}

#ifndef __main_program__
#define __main_program__
int main(int argv, char **args)
{

    // return solve(argv, args);
    return solve(argv, args);
}
#endif

max_stars_repo_name: SDMStudio/sdms

max_issues_repo_name: SDMStudio/sdms

max_forks_repo_name: SDMStudio/sdms


text: #include <boost/compute/image/image_object.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text: #include <boost/preprocessor/repeat_from_to_2nd.hpp>


text:
/*
 * Copyright (c) 2012 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */

// VS2015 fix atomic alignment and require an acknowledgement from developer
#include <boost/predef.h>
#if BOOST_COMP_MSVC
#  if (BOOST_COMP_MSVC >= BOOST_VERSION_NUMBER(14, 0, 0))
#    define _ENABLE_ATOMIC_ALIGNMENT_FIX
#  endif
#endif

#include <qi/assert.hpp>
#include <qi/log.hpp>
#include "log_p.hpp"
#include <qi/os.hpp>
#include <list>
#include <map>
#include <cstring>
#include <iomanip>
#include <iostream>

#include <qi/application.hpp>
#include <qi/atomic.hpp>
#include <qi/log/consoleloghandler.hpp>

#include <boost/thread/thread.hpp>
#include <boost/thread/condition_variable.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/thread/locks.hpp>
#include <boost/program_options.hpp>
#include <boost/unordered_map.hpp>
#include <boost/algorithm/string.hpp>

#include <boost/lockfree/queue.hpp>
#include <boost/function.hpp>
#include <boost/predef.h>
#include <boost/utility/string_ref.hpp>
#include <boost/function_output_iterator.hpp>

#ifdef WITH_SYSTEMD
#include <qi/log/journaldloghandler.hpp>
#endif

#ifdef ANDROID
# include <qi/log/androidloghandler.hpp>
#endif

#if BOOST_OS_WINDOWS
# include <shlwapi.h>
# pragma comment(lib, "shlwapi.lib")
# include <boost/locale.hpp>
# include <boost/filesystem.hpp>
#else
# include <fnmatch.h>
#endif

#define RTLOG_BUFFERS (128)

#define CAT_SIZE 64
#define FILE_SIZE 128
#define FUNC_SIZE 64
#define LOG_SIZE 2048

qiLogCategory("qi.log");

namespace qi {
  namespace detail {

    std::string logline(LogContext                         context,
                        const qi::Clock::time_point        date,
                        const qi::SystemClock::time_point  systemDate,
                        const char                        *category,
                        const char                        *msg,
                        const char                        *file,
                        const char                        *fct,
                        const int                          line,
                        const qi::LogLevel                 verb)
    {
      std::stringstream logline;

      if (context & qi::LogContextAttr_Verbosity)
        logline << qi::log::logLevelToString(verb) << " ";
      if (context & qi::LogContextAttr_ShortVerbosity)
        logline << qi::log::logLevelToString(verb, false) << " ";
      if (context & qi::LogContextAttr_Date)
        logline << qi::detail::dateToString(date) << " ";
      if (context & qi::LogContextAttr_SystemDate)
        logline << qi::detail::dateToString(systemDate) << " ";
      if (context & qi::LogContextAttr_Tid)
        logline << qi::detail::tidToString() << " ";
      if (context & qi::LogContextAttr_Category)
        logline << category << ": ";
      if (context & qi::LogContextAttr_File) {
        logline << file;
        if (line != 0)
          logline << "(" << line << ")";
        logline << " ";
      }
      if (context & qi::LogContextAttr_Function)
        logline << fct << "() ";
      if (context & qi::LogContextAttr_Return)
        logline << std::endl;
      if (msg)
      {
        boost::algorithm::trim_right_copy_if(std::ostream_iterator<char>(logline),
                                             boost::string_ref(msg), &isNewLine);
      }
      logline << std::endl;

      return logline.str();
    }

    std::string logline(LogContext             context,
                        const qi::os::timeval  systemDate,
                        const char            *category,
                        const char            *msg,
                        const char            *file,
                        const char            *fct,
                        const int              line,
                        const qi::LogLevel     verb)
    {
      return logline(context & ~qi::LogContextAttr_Date, // hide date
                     qi::ClockTimePoint(), // date, not printed
                     qi::SystemClockTimePoint(qi::Seconds(systemDate.tv_sec) +
                                              qi::MicroSeconds(systemDate.tv_usec)),
                     category, msg, file, fct, line, verb);
    }

    std::string csvheader()
    {
      std::ostringstream cvsheader;
      cvsheader << "VERBOSITYID,";
      cvsheader << "VERBOSITY,";
      cvsheader << "SVERBOSITY,";
      cvsheader << "DATE,";
      cvsheader << "SYSTEM_DATE,";
      cvsheader << "THREAD_ID,";
      cvsheader << "CATEGORY,";
      cvsheader << "FILE,";
      cvsheader << "LINE,";
      cvsheader << "FUNCTION,";
      cvsheader << "MSG" << std::endl;

      return cvsheader.str();
    }

    std::string csvline(const qi::Clock::time_point date,
                        const qi::SystemClock::time_point systemDate,
                        const char        *category,
                        const char        *msg,
                        const char        *file,
                        const char        *fct,
                        const int          line,
                        const qi::LogLevel verb)
    {
      std::ostringstream csvline;
      csvline << verb << ",";
      csvline << qi::log::logLevelToString(verb) << ",";
      csvline << qi::log::logLevelToString(verb, false) << ",";
      csvline << qi::detail::dateToString(date) << ",";
      csvline << qi::detail::dateToString(systemDate) << ",";
      csvline << qi::detail::tidToString() << ",";

      csvline << "\"";
      csvline << category;
      csvline << "\"";
      csvline << ",";

      csvline << "\"";
      csvline << file;
      csvline << "\"";
      csvline << ",";

      if (line != 0)
        csvline << line;
      csvline << ",";

      csvline << "\"";
      csvline << fct << "()";
      csvline << "\"";
      csvline << ",";

      csvline << "\"";

      if (msg)
      {
        std::string msgStr(msg);
        boost::algorithm::replace_all(msgStr, "\"", "\"\"");
        boost::algorithm::trim_right_copy_if(std::ostream_iterator<char>(csvline), msgStr,
                                             &isNewLine);
      }
      csvline << "\"" << std::endl;

      return csvline.str();
    }

    const std::string dateToString(const qi::os::timeval date)
    {
      std::stringstream ss;
      ss << date.tv_sec << "."
        << std::setw(6) << std::setfill('0') << date.tv_usec;

      return ss.str();
    }

    const std::string tidToString()
    {
      int tid = qi::os::gettid();
      std::stringstream ss;
      ss << tid;

      return ss.str();
    }

    bool isNewLine(char c)
    {
      return c == '\n' || c == '\r';
    }
  }

  namespace log {

    using privateLog = struct sPrivateLog
    {
      qi::LogLevel               _logLevel;
      char                       _category[CAT_SIZE];
      char                       _file[FILE_SIZE];
      char                       _function[FUNC_SIZE];
      int                        _line;
      char                       _log[LOG_SIZE];
      qi::Clock::time_point       _date;
      qi::SystemClock::time_point _systemDate;
    };

    class Log
    {
    public:
      inline Log();
      inline ~Log();

      struct Handler
      {
        qi::log::Handler func;
        unsigned int index; // index of this handler in category levels
      };

      void run();
      void printLog();
      // Invoke handlers who enabled given level/category
      void dispatch_unsynchronized(const qi::LogLevel,
                                   const qi::Clock::time_point date,
                                   const qi::SystemClock::time_point systemDate,
                                   const char*,
                                   const char*,
                                   const char*,
                                   const char*,
                                   int);
      void dispatch_unsynchronized(const qi::LogLevel level,
                                   const qi::Clock::time_point date,
                                   const qi::SystemClock::time_point systemDate,
                                   detail::Category& category,
                                   const char* log,
                                   const char* file,
                                   const char* function,
                                   int line);
      Handler* logHandler(SubscriberId id);

      void setSynchronousLog(bool sync);
    public:
      bool                       LogInit;
      boost::thread              LogThread;
      boost::mutex               LogWriteLock;
      boost::mutex               LogHandlerLock;
      boost::condition_variable  LogReadyCond;
      bool                       SyncLog;
      bool                       AsyncLogInit;

      boost::lockfree::queue<privateLog*>     logs;

      using LogHandlerMap = std::map<std::string, Handler>;
      LogHandlerMap logHandlers;

      qi::Atomic<int> nextIndex;
    };

    // If we receive a setLevel with a globbing category, we must keep it
    // in mind, in case a new category that matches the glob is created.
    struct GlobRule
    {
      GlobRule(std::string t, unsigned int i, qi::LogLevel l)
      : target(t)
      , id(i)
      , level(l)
      {}

      bool matches(const std::string& n) const
      {
        return os::fnmatch(target, n);
      }

      std::string  target;       // glob target
      unsigned int id;           // listener id or -1 for all
      qi::LogLevel level;
    };

    static std::vector<GlobRule> _glGlobRules;

    // categories must be accessible at static init: cannot go in Log class
    using CategoryMap = std::map<std::string, detail::Category*>;
    inline CategoryMap& _categories()
    {
      static CategoryMap* _glCategories;
      QI_ONCE(_glCategories = new CategoryMap);
      return *_glCategories;
    }

    // protects globs and categories, both the map and the per-category vector
    inline boost::recursive_mutex& _mutex()
    {
      static boost::recursive_mutex* _glMutex;
      QI_ONCE(_glMutex = new boost::recursive_mutex());
      return *_glMutex;
    }

    static int                    _glContext = 0;
    static bool                   _glInit    = false;
    static LogColor               _glColorWhen = LogColor_Auto;

    static Log                   *LogInstance = nullptr;
    static privateLog             LogBuffer[RTLOG_BUFFERS];
    static volatile unsigned long LogPush = 0;

    namespace detail {

      // return a log handler on windows, an invalid boost::function otherwise
      Handler makeWindowsDebuggerOutputLogHandler()
      {
#if BOOST_OS_WINDOWS && !defined(NDEBUG)
        auto winDebuggerOutputLogHandler = [](const qi::LogLevel verb, const qi::Clock::time_point date,
            const qi::SystemClock::time_point systemDate,
            const char* category, const char* msg,
            const char* file, const char* fct, const int line)
        {
          if (!IsDebuggerPresent()) // we need to check on each call in case the debuger is attached on the fly
            return;

          // We force long log level names in this specific case.
          auto context = (log::context() | LogContextAttr_Verbosity) & ~LogContextAttr_ShortVerbosity;
          auto logline = qi::detail::logline(context, date, systemDate, category, msg, file, fct, line, verb);

          const auto processId = std::to_wstring(GetCurrentProcessId());

          // Add the process name to help with multi-process debugging:
          static const auto processName = [&] {

            using Path = boost::filesystem::path;

            static const auto getProcessPath = [&] {
              const Path processPath = __argv ? Path{ __argv[0] } : Path{ __wargv[0] };

              // Before providing the process name, we log in the debug ouptut the complete command line
              // so that is is possible to identify processes in multi-process debugging when using several same process names
              // but different arguments:
              {
                std::wstringstream launchMessage;
                launchMessage << L"#### QI Process Logs Tracking Begin (PID=" << processId
                              << L") - Command Line: \n  " << processPath;
                for (int idx = 1; idx < __argc; ++idx)
                {
                  launchMessage << L' ';
                  if (__argv)
                    launchMessage << (__argv[idx] ? __argv[idx] : "<null-argument?>");
                  else
                    launchMessage << (__wargv[idx] ? __wargv[idx] : L"<null-argument?>");
                }
                launchMessage << L'\n';
                OutputDebugStringW(launchMessage.str().c_str());
              }

              return processPath;
            };

            return getProcessPath().filename().wstring() + L'(' + processId + L") > ";

          }();

          // We receive UTF-8 so we want to be able to display unicode characters in this output too:
          auto wlogline = processName + boost::locale::conv::utf_to_utf<wchar_t>(logline.c_str(), logline.c_str() + logline.size());
          OutputDebugStringW(wlogline.c_str());
        };
        return winDebuggerOutputLogHandler;
#else
        return Handler{};
#endif
      }

    }

    static ConsoleLogHandler *_glConsoleLogHandler = nullptr;

    namespace env {
      namespace QI_DEFAULT_LOGHANDLER {
        char const * const name = "QI_DEFAULT_LOGHANDLER";
        namespace value {
          char const * const none     = "none";
          char const * const stdOut   = "stdout";
          char const * const logger   = "logger";
          char const * const debugger = "debugger";
        }
      }
    }

    namespace detail {

      /// Creates and registers the default log handler according to QI_DEFAULT_LOGHANDLER
      /// environment variable, libqi WITH_SYSTEMD build option and the target OS platform.

      /// See documentation of the public function calling this private one: qi::log::init().
      void createAndInstallDefaultHandler(qi::LogLevel verb)
      {
        using namespace qi::log::env;
        auto handler = qi::os::getenv(QI_DEFAULT_LOGHANDLER::name);
        if (handler.empty())
        {
          handler =
#if defined(ANDROID) || defined(WITH_SYSTEMD)
              QI_DEFAULT_LOGHANDLER::value::logger;
#else
              QI_DEFAULT_LOGHANDLER::value::stdOut;
#endif
        }
        const auto invalidId = static_cast<SubscriberId>(-1);
        auto id = invalidId;
        QI_ASSERT(! handler.empty());
        if (handler == QI_DEFAULT_LOGHANDLER::value::stdOut){
          _glConsoleLogHandler = new ConsoleLogHandler;
          id = addHandler("consoleloghandler",
                          boost::bind(&ConsoleLogHandler::log,
                                      _glConsoleLogHandler,
                                      _1, _2, _3, _4, _5, _6, _7, _8),
                          verb);
          QI_ASSERT(id == 0 || id == invalidId);
        }
        else if (handler == QI_DEFAULT_LOGHANDLER::value::logger)
        {
#ifdef ANDROID
          id = addHandler("androidloghandler", makeAndroidLogHandler(), verb);
#elif defined(WITH_SYSTEMD)
          id = addHandler("journaldloghandler", makeJournaldLogHandler(), verb);
#endif
          QI_ASSERT(id == 0 || id == invalidId);
        }
        else if (handler == QI_DEFAULT_LOGHANDLER::value::debugger)
        {
          auto h = makeWindowsDebuggerOutputLogHandler();
          if (h)
          {
            id = addHandler("winDebuggerOutputLogHandler", std::move(h), verb);
          }
          QI_ASSERT(id == 0 || id == invalidId);
        }
        else if (handler == QI_DEFAULT_LOGHANDLER::value::none)
        {
          QI_ASSERT(id == invalidId);
        }
        else
        {
          QI_ASSERT(id == invalidId);
          std::cerr << "qi.log: bad value for " << QI_DEFAULT_LOGHANDLER::name
                    << " environment variable: \"" << handler << "\"."
                    << " Possible values are: \"\","
                    << " \"" << QI_DEFAULT_LOGHANDLER::value::none     << "\","
                    << " \"" << QI_DEFAULT_LOGHANDLER::value::stdOut   << "\","
                    << " \"" << QI_DEFAULT_LOGHANDLER::value::logger   << "\","
                    << " \"" << QI_DEFAULT_LOGHANDLER::value::debugger << "\".\n";
        }
        if (id == invalidId)
        {
          std::cerr << "qi.log: failed to register \"" << handler
                    << "\" log handler. Log messages will be lost until a"
                       " log handler is added.\n";
        }
      }

      void destroyDefaultHandler()
      {
#ifndef ANDROID
        if(_glConsoleLogHandler)
        {
          delete _glConsoleLogHandler;
          _glConsoleLogHandler = nullptr;
        }
#endif
      }
    } // namespace detail

    namespace detail {

      void log(const qi::LogLevel    verb,
               CategoryType          category,
               const char           *categoryStr,
               const char           *msg,
               const char           *file,
               const char           *fct,
               const int             line);

      // This pattern allows to continue logging at static destruction time
      // even if the static FormatMap is destroyed
      class FormatMap: public boost::unordered_map<std::string, boost::format>
      {
      public:
        FormatMap(bool& ward)
        : ward_(ward)
        {
          ward_ = true;
        }

        ~FormatMap()
        {
          ward_ = false;
        }

      private:
        bool& ward_;
      };

      boost::format getFormat(const std::string& s)
      {
        static bool map_ok(false);
        static FormatMap map(map_ok);
        if (map_ok)
        {
          static boost::mutex mutex;
          boost::mutex::scoped_lock lock(mutex);
          FormatMap::iterator i = map.find(s);
          if (i == map.end())
          {
            boost::format& result = map[s]; // creates with default ctor
            result.parse(s);
            result.exceptions(boost::io::no_error_bits);
            return result;
          }
          else
            return i->second;
        }
        else
          {
            boost::format result = boost::format(s);
            result.exceptions(boost::io::no_error_bits);
            return result;
          }
      }
    }

    namespace detail {
      void Category::setLevel(SubscriberId sub, qi::LogLevel level)
      {
        boost::recursive_mutex::scoped_lock lock(_mutex());
        if (levels.size() <= sub)
        {
          bool willUseDefault = (levels.size() < sub);
          levels.resize(sub + 1, LogLevel_Info);
          if (willUseDefault)
          { // should not happen
            // cannot qilog here or deadlock
            std::cerr << "Default level for category " << name
              << " will be used for subscriber " << sub
              << ", use setVerbosity() after adding the subscriber"
              << std::endl;
          }
        }
        levels[sub] = level;
        maxLevel = *std::max_element(levels.begin(), levels.end());
      }
    }

    // check and apply existing glob if they match given category
    static void checkGlobs(detail::Category* cat)
    {
      boost::recursive_mutex::scoped_lock lock(_mutex());
      for (unsigned i=0; i<_glGlobRules.size(); ++i) {
        GlobRule& g = _glGlobRules[i];
        if (g.matches(cat->name))
          cat->setLevel(g.id, g.level);
      }
    }

    // apply a globbing rule to existing categories
    static void applyGlob(const GlobRule& g)
    {
      boost::recursive_mutex::scoped_lock lock(_mutex());
      CategoryMap& c = _categories();
      for (CategoryMap::iterator it = c.begin(); it != c.end(); ++it)
      {
        QI_ASSERT(it->first == it->second->name);
        if (g.matches(it->first)) {
          detail::Category* cat = it->second;
          checkGlobs(cat);
        }
      }
    }

    // Check if globRule replaces an existiing one, then replace or append
    static void mergeGlob(const GlobRule& p)
    {
      boost::recursive_mutex::scoped_lock lock(_mutex());
      for (unsigned i=0; i<_glGlobRules.size(); ++i)
      {
        GlobRule& c = _glGlobRules[i];
        if (p.target == c.target && p.id == c.id)
        {
          c = p;
          return;
        }
      }
      _glGlobRules.push_back(p);
    }

    static class DefaultLogInit
    {
    public:
      DefaultLogInit()
      {
        _glInit = false;
        const std::string logLevel = qi::os::getEnvParam<std::string>("QI_LOG_LEVEL", "info");
        const int context = qi::os::getEnvParam<int>("QI_LOG_CONTEXT", 30);
        _glContext = context;
        const std::string rules = qi::os::getEnvParam<std::string>("QI_LOG_FILTERS", std::string());
        if (!rules.empty())
          addFilters(rules);
        qi::log::init(stringToLogLevel(logLevel.c_str()), context);
      }

      ~DefaultLogInit()
      {
        qi::log::destroy();
      }
    } synchLog;

    void Log::printLog()
    {
      privateLog* pl = nullptr;

      boost::recursive_mutex::scoped_lock lock(_mutex(), boost::defer_lock);
      boost::mutex::scoped_lock lockHandlers(LogInstance->LogHandlerLock, boost::defer_lock);
      boost::lock(lock, lockHandlers);
      while (logs.pop(pl))
      {
        dispatch_unsynchronized(pl->_logLevel, pl->_date, pl->_systemDate, pl->_category, pl->_log,
                                pl->_file, pl->_function, pl->_line);
      }
    }

    void Log::dispatch_unsynchronized(const qi::LogLevel level,
                                      const qi::Clock::time_point date,
                                      const qi::SystemClock::time_point systemDate,
                                      const char* category,
                                      const char* log,
                                      const char* file,
                                      const char* function,
                                      int line)
    {
      dispatch_unsynchronized(level, date, systemDate, *addCategory(category), log, file, function,
                              line);
    }

    void Log::dispatch_unsynchronized(const qi::LogLevel level,
                                      const qi::Clock::time_point date,
                                      const qi::SystemClock::time_point systemDate,
                                      detail::Category& category,
                                      const char* log,
                                      const char* file,
                                      const char* function,
                                      int line)
    {
      if (!logHandlers.empty())
      {
        LogHandlerMap::iterator it;
        for (it = logHandlers.begin(); it != logHandlers.end(); ++it)
        {
          Handler& h = it->second;
          unsigned int index = h.index;
          if (category.levels.size() <= index || category.levels[index] >= level)
            h.func(level, date, systemDate, category.name.c_str(), log, file, function, line);
        }
      }
    }

    void Log::run()
    {
      while (LogInit)
      {
        {
          boost::mutex::scoped_lock lock(LogWriteLock);
          LogReadyCond.wait(lock, []{ return !LogInstance->logs.empty(); });
        }

        printLog();
      }
    }

    void Log::setSynchronousLog(bool sync)
    {
      SyncLog = sync;
      if (!SyncLog && !AsyncLogInit)
      {
        AsyncLogInit = true;
        LogThread = boost::thread(&Log::run, this);
      }
    }

    inline Log::Log() :
      SyncLog(true),
      AsyncLogInit(false)
      , logs(50)
    {
      LogInit = true;
    }

    inline Log::~Log()
    {
      if (!LogInit)
        return;
      LogInit = false;

      if (AsyncLogInit)
      {
        LogThread.interrupt();
        LogThread.join();

        printLog();
      }
    }

    static void my_strcpy(char *dst, const char *src, int len) {
      if (!src)
        src = "(null)";
#ifdef _MSV_VER
      strncpy_s(dst, len, src, _TRUNCATE);
#else
      strncpy(dst, src, len);
      dst[len - 1] = 0;
#endif
    }

    static void doInit(qi::LogLevel verb) {
      //if init has already been called, we are set here. (reallocating all globals
      // will lead to racecond)
      if (_glInit)
        return;

      LogInstance = new Log;
      detail::createAndInstallDefaultHandler(verb);
      _glInit = true;
    }

    void init(qi::LogLevel verb,
              int ctx,
              bool synchronous)
    {
      QI_ONCE(doInit(verb));

      setContext(ctx);

      setSynchronousLog(synchronous);
    }

    void destroy()
    {
      if (!_glInit)
        return;
      _glInit = false;
      LogInstance->printLog();
      detail::destroyDefaultHandler();
      delete LogInstance;
      LogInstance = nullptr;
    }

    void flush()
    {
      if (_glInit)
        LogInstance->printLog();
    }

    void log(const qi::LogLevel    verb,
             CategoryType          category,
             const std::string&    msg,
             const char           *file,
             const char           *fct,
             const int             line)
    {
      if (!isVisible(category, verb))
        return;

      ::qi::log::detail::log(verb, category, category->name.c_str(), msg.c_str(), file, fct, line);
    }

    void log(const qi::LogLevel    verb,
             const char           *categoryStr,
             const char           *msg,
             const char           *file,
             const char           *fct,
             const int             line)
    {
      if (!isVisible(categoryStr, verb))
        return;

      ::qi::log::detail::log(verb, NULL, categoryStr, msg, file, fct, line);
    }

    void detail::log(const qi::LogLevel    verb,
                     CategoryType          category,
                     const char           *categoryStr,
                     const char           *msg,
                     const char           *file,
                     const char           *fct,
                     const int             line)
    {
      if (!LogInstance)
        return;
      if (!LogInstance->LogInit)
        return;

      qi::Clock::time_point date = qi::Clock::now();
      qi::SystemClock::time_point systemDate = qi::SystemClock::now();
      if (LogInstance->SyncLog)
      {
        boost::recursive_mutex::scoped_lock lock(_mutex(), boost::defer_lock);
        boost::mutex::scoped_lock lockHandlers(LogInstance->LogHandlerLock, boost::defer_lock);
        boost::lock(lock, lockHandlers);
        if (category)
          LogInstance->dispatch_unsynchronized(verb, date, systemDate, *category, msg, file, fct,
                                               line);
        else
          LogInstance->dispatch_unsynchronized(verb, date, systemDate, categoryStr, msg, file, fct,
                                               line);
      }
      else
      {
        int tmpRtLogPush = ++LogPush % RTLOG_BUFFERS;
        privateLog* pl = &(LogBuffer[tmpRtLogPush]);

        pl->_logLevel = verb;
        pl->_line = line;
        pl->_date = date;
        pl->_systemDate = systemDate;

        my_strcpy(pl->_category, categoryStr, CAT_SIZE);
        my_strcpy(pl->_file, file, FILE_SIZE);
        my_strcpy(pl->_function, fct, FUNC_SIZE);
        my_strcpy(pl->_log, msg, LOG_SIZE);
        LogInstance->logs.push(pl);
        LogInstance->LogReadyCond.notify_one();
      }
    }

    Log::Handler* Log::logHandler(SubscriberId id)
    {
       boost::mutex::scoped_lock l(LogInstance->LogHandlerLock);
       LogHandlerMap::iterator it;
       for (it = logHandlers.begin(); it != logHandlers.end(); ++it)
       {
         if (it->second.index == id)
           return &it->second;
       }
       return  nullptr;
    }

    void adaptLogFuncHandler(
        logFuncHandler handler,
        const qi::LogLevel verb,
        const qi::Clock::time_point,
        const qi::SystemClock::time_point systemDate,
        const char *category,
        const char *msg,
        const char *file,
        const char *fct,
        const int line)
    {
      handler(verb, qi::os::timeval(systemDate.time_since_epoch()), category,
              msg, file, fct, line);
    }

    SubscriberId addHandler(const std::string& name, Handler fct,
                            qi::LogLevel defaultLevel)
    {
      if (!LogInstance)
        return -1;
      boost::mutex::scoped_lock l(LogInstance->LogHandlerLock);
      unsigned int id = ++LogInstance->nextIndex;
      --id; // no postfix ++ on atomic
      Log::Handler h;
      h.index = id;
      h.func = fct;
      LogInstance->logHandlers[name] = h;
      setLogLevel(defaultLevel, id);
      return id;
    }

    SubscriberId addLogHandler(const std::string& name, logFuncHandler fct,
                               qi::LogLevel defaultLevel)
    {
      return addHandler(name,
          boost::bind(adaptLogFuncHandler,
                      fct, _1, _2, _3, _4, _5, _6, _7, _8),
                      defaultLevel);
    }

    void removeHandler(const std::string& name)
    {
      if (!LogInstance)
        return;
      boost::mutex::scoped_lock l(LogInstance->LogHandlerLock);
      LogInstance->logHandlers.erase(name);
    }

    void removeLogHandler(const std::string& name)
    {
      removeHandler(name);
    }

    qi::LogLevel stringToLogLevel(const char* verb)
    {
      std::string v(verb);
      if (v == "silent" || v == "0")
        return qi::LogLevel_Silent;
      if (v == "fatal" || v == "1")
        return qi::LogLevel_Fatal;
      if (v == "error" || v == "2")
        return qi::LogLevel_Error;
      if (v == "warning" || v == "3")
        return qi::LogLevel_Warning;
      if (v == "info" || v == "4")
        return qi::LogLevel_Info;
      if (v == "verbose" || v == "5")
        return qi::LogLevel_Verbose;
      if (v == "debug" || v == "6")
        return qi::LogLevel_Debug;
      return qi::LogLevel_Info;
    }

    const char *logLevelToString(const qi::LogLevel level, bool verbose)
    {
      static const char *sverb[] = {
        "[SILENT]", // never shown
        "[F]",
        "[E]",
        "[W]",
        "[I]",
        "[V]",
        "[D]"
      };
      static const char *verb[] = {
        "[SILENT]", // never shown
        "[FATAL]",
        "[ERROR]",
        "[WARN ]",
        "[INFO ]",
        "[VERB ]",
        "[DEBUG]"
      };
      QI_ASSERT(level >= 0 && level <= qi::LogLevel_Debug);
      if (verbose)
        return verb[level];
      return sverb[level];
    }

    qi::LogLevel logLevel(SubscriberId sub)
    {
      CategoryType cat = addCategory("*");
      if (sub < cat->levels.size())
        return cat->levels[sub];
      return LogLevel_Info;
    }

    void setContext(int ctx)
    {
      _glContext = ctx;
      qiLogVerbose() << "Context set to " << _glContext;
    }

    int context()
    {
      return _glContext;
    }

    void setColor(LogColor color)
    {
      _glColorWhen = color;
#ifndef ANDROID
      if(_glConsoleLogHandler)
      {
        _glConsoleLogHandler->updateColor();
      }
#endif
    }

    LogColor color()
    {
      return _glColorWhen;
    }

    void setSynchronousLog(bool sync)
    {
      LogInstance->setSynchronousLog(sync);
    }

    CategoryType addCategory(const std::string& name)
    {
      boost::recursive_mutex::scoped_lock lock(_mutex());
      CategoryMap& c = _categories();
      CategoryMap::iterator i = c.find(name);
      if (i == c.end())
      {
        detail::Category* res = new detail::Category(name);
        c[name] = res;
        checkGlobs(res);
        return res;
      }
      else
        return i->second;
    }

    bool isVisible(const std::string& category, qi::LogLevel level)
    {
      return log::isVisible(addCategory(category), level);
    }

    void enableCategory(const std::string& cat, SubscriberId sub)
    {
      addFilter(cat, logLevel(sub), sub);
    }

    void disableCategory(const std::string& cat, SubscriberId sub)
    {
      addFilter(cat, LogLevel_Silent, sub);
    }

    void addFilter(const std::string& catName, qi::LogLevel level, SubscriberId sub)
    {
      qiLogVerbose() << "addFilter(cat=" << catName << ", level=" << (int)level << ", sub=" << (int)sub << ")";
      if (catName.find('*') != catName.npos)
      {
        GlobRule rule(catName, sub, level);
        mergeGlob(rule);
        applyGlob(rule);
      }
      else
      {
        CategoryType cat = addCategory(catName);
        cat->setLevel(sub, level);
        GlobRule rule(catName, sub, level);
        mergeGlob(rule);
      }
    }

    std::vector<std::string> categories()
    {
      boost::recursive_mutex::scoped_lock lock(_mutex());
      std::vector<std::string> res;
      CategoryMap& c = _categories();
      for (CategoryMap::iterator it = c.begin(); it != c.end(); ++it)
        res.push_back(it->first);
      return res;
    }

    void setLogLevel(qi::LogLevel level, SubscriberId sub)
    {
      boost::recursive_mutex::scoped_lock lock(_mutex());
      // Check if there is already a '*' rule, replace it if so
      bool found = false;
      for (unsigned i=0; i<_glGlobRules.size(); ++i)
      {
        if (_glGlobRules[i].target == "*" && _glGlobRules[i].id == sub)
        {
          _glGlobRules[i].level = level;
          found = true;
          break;
        }
      }
      if (!found)
      {
        // Prepend the rule
        GlobRule rule("*", sub, level);
        // Insert the rule with initial '*' rule set, ordered by subscriber id
        // to avoid spurious unset-verbosity warning
        std::vector<GlobRule>::iterator insertIt = _glGlobRules.begin();
        while (insertIt != _glGlobRules.end()
          && insertIt->target == "*" && insertIt->id < sub)
          ++insertIt;
        _glGlobRules.insert(insertIt, rule);
      }
      // Then reprocess all categories
      CategoryMap& c = _categories();
      for (CategoryMap::iterator it = c.begin(); it != c.end(); ++it)
        checkGlobs(it->second);
    }

    namespace detail {
      std::vector<std::tuple<std::string, qi::LogLevel>> parseFilterRules(
          const std::string &rules) {
        std::vector<std::tuple<std::string, qi::LogLevel>> res;
        // See doc in header for format
        size_t pos = 0;
        while (true)
        {
          if (pos >= rules.length())
            break;
          size_t next = rules.find(':', pos);
          std::string token;
          if (next == rules.npos)
            token = rules.substr(pos);
          else
            token = rules.substr(pos, next-pos);
          if (token.empty())
          {
            pos = next + 1;
            continue;
          }
          if (token[0] == '+')
            token = token.substr(1);
          size_t sep = token.find('=');
          if (sep != token.npos)
          {
            std::string sLevel = token.substr(sep+1);
            std::string cat = token.substr(0, sep);
            qi::LogLevel level = stringToLogLevel(sLevel.c_str());
            res.emplace_back(cat, level);
          }
          else
          {
            if (token[0] == '-')
              res.emplace_back(token.substr(1), LogLevel_Silent);
            else
              res.emplace_back(token, LogLevel_Debug);
          }
          if (next == rules.npos)
            break;
          pos = next+1;
        }
        return res;
      }
    }

    void addFilters(const std::string& rules, SubscriberId sub)
    {
      std::string cat;
      qi::LogLevel level;
      for (auto &&p: detail::parseFilterRules(rules))
      {
        std::tie(cat, level) = std::move(p);
        addFilter(cat, level, sub);
      }
    }

    static void _setLogLevel(const std::string &level)
    {
      setLogLevel(stringToLogLevel(level.c_str()), 0u);
    }

    static void _setColor(const std::string &color)
    {
      if (color == "always")
        setColor(LogColor_Always);
      else if (color == "never")
        setColor(LogColor_Never);
      else
        setColor(LogColor_Auto);
    }

    static void _setFilters(const std::string &filters)
    {
      addFilters(filters);
    }

    static const std::string contextLogOption = ""
        "Show context logs, it's a bit field (add the values below):\n"
        " 1  : Verbosity\n"
        " 2  : ShortVerbosity\n"
        " 4  : SystemDate\n"
        " 8  : ThreadId\n"
        " 16 : Category\n"
        " 32 : File\n"
        " 64 : Function\n"
        " 128: EndOfLine\n"
        " 256: Date\n"
        "some useful values for context are:\n"
        " 26 : (verb+threadId+cat)\n"
        " 30 : (verb+threadId+date+cat)\n"
        " 126: (verb+threadId+date+cat+file+fun)\n"
        " 254: (verb+threadId+date+cat+file+fun+eol)\n"
        "Can be set with env var QI_LOG_CONTEXT";

    static const std::string levelLogOption = ""
        "Change the log minimum level: [0-6] (default:4)\n"
        " 0: silent\n"
        " 1: fatal\n"
        " 2: error\n"
        " 3: warning\n"
        " 4: info\n"
        " 5: verbose\n"
        " 6: debug\n"
        "Can be set with env var QI_LOG_LEVEL";

    static const std::string filterLogOption = ""
        "Set log filtering options.\n"

        " Colon separated list of rules.\n"
        " Each rule can be:\n"
        "  - +CAT      : enable category CAT\n"
        "  - -CAT      : disable category CAT\n"
        "  - CAT=level : set category CAT to level\n"
        " Each category can include a '*' for globbing.\n"
        "Can be set with env var QI_LOG_FILTERS\n"
        "Example: 'qi.*=debug:-qi.foo:+qi.foo.bar' (all qi.* logs in info, remove all qi.foo logs except qi.foo.bar)";

    _QI_COMMAND_LINE_OPTIONS(
      "Logging options",
      ("qi-log-context",     value<int>()->notifier(&setContext), contextLogOption.c_str())
      ("qi-log-synchronous", bool_switch()->notifier(boost::bind(&setSynchronousLog, true)),  "Activate synchronous logs.")
      ("qi-log-level",       value<std::string>()->notifier(&_setLogLevel), levelLogOption.c_str())
      ("qi-log-color",       value<std::string>()->notifier(&_setColor), "Tell if we should put color or not in log (auto, always, never).")
      ("qi-log-filters",     value<std::string>()->notifier(&_setFilters), filterLogOption.c_str())
    )

    // deprecated
    qi::LogLevel verbosity(SubscriberId sub)
    {
      return logLevel(sub);
    }

    // deprecated
    void setVerbosity(qi::LogLevel level, SubscriberId sub)
    {
      setLogLevel(level, sub);
    }

    // deprecated
    void setVerbosity(const std::string& rules, SubscriberId sub)
    {
      addFilters(rules, sub);
    }

    // deprecated
    void setCategory(const std::string& catName, qi::LogLevel level, SubscriberId sub)
    {
      addFilter(catName, level, sub);
    }

  } // namespace log
} // namespace qi

max_stars_repo_name: arntanguy/libqi

max_issues_repo_name: arntanguy/libqi

max_forks_repo_name: arntanguy/libqi


text:
#include <stdio.h>
#include <iostream>

#include <Eigen/Geometry>

#include "../libfovis/refine_motion_estimate.hpp"

#define dump(v) std::cerr << #v << " : " << (v) << "\n"
#define dumpT(v) std::cerr << #v << " : " << (v).transpose() << "\n"

using namespace fovis;

static inline Eigen::Isometry3d
isometryFromXYZRollPitchYaw(const Eigen::Matrix<double, 6, 1>& params)
{
  Eigen::Isometry3d result;

  double roll = params(3), pitch = params(4), yaw = params(5);
  double halfroll = roll / 2;
  double halfpitch = pitch / 2;
  double halfyaw = yaw / 2;
  double sin_r2 = sin(halfroll);
  double sin_p2 = sin(halfpitch);
  double sin_y2 = sin(halfyaw);
  double cos_r2 = cos(halfroll);
  double cos_p2 = cos(halfpitch);
  double cos_y2 = cos(halfyaw);

  Eigen::Quaterniond quat(
    cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2,
    sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2,
    cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2,
    cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2);

  result.setIdentity();
  result.translate(params.head<3>());
  result.rotate(quat);

  return result;
}

static inline Eigen::Vector3d
isometryGetRollPitchYaw(const Eigen::Isometry3d& M)
{
  Eigen::Quaterniond q(M.rotation());
  double roll_a = 2 * (q.w()*q.x() + q.y()*q.z());
  double roll_b = 1 - 2 * (q.x()*q.x() + q.y()*q.y());
  double pitch_sin = 2 * (q.w()*q.y() - q.z()*q.x());
  double yaw_a = 2 * (q.w()*q.z() + q.x()*q.y());
  double yaw_b = 1 - 2 * (q.y()*q.y() + q.z()*q.z());

  return Eigen::Vector3d(atan2(roll_a, roll_b),
      asin(pitch_sin),
      atan2(yaw_a, yaw_b));
}

int main(int argc, char** argv)
{

  int num_points = 9;

  Eigen::Matrix<double, 4, Eigen::Dynamic> points_xyz(4, num_points);
  Eigen::Matrix<double, 4, Eigen::Dynamic> transformed_xyz(4, num_points);
  Eigen::Matrix<double, 2, Eigen::Dynamic> ref_projections(2, num_points);

  double tx    = 1.0;
  double ty    = 1.5;
  double tz    = 0.5;
  double roll  = 10 * (M_PI / 180);
  double pitch =  1 * (M_PI / 180);
  double yaw   =  5 * (M_PI / 180);
  //	tx = 0;
  //	ty = 0;
  //	tz = 0;
  //	roll = 0;
  //	pitch = 0;
  //	yaw = 1 * (M_PI / 180);

  Eigen::Matrix<double, 6, 1> params;
  params << tx, ty, tz, roll, pitch, yaw;
  Eigen::Isometry3d true_motion = isometryFromXYZRollPitchYaw(params);
  double fx = 528;
  double cx = 320;
  double cy = 240;

  Eigen::Matrix<double, Eigen::Dynamic, 2> tmp(num_points, 2);
  tmp <<
    0, 0,
    320, 0,
    640, 0,
    0, 240,
    320, 240,
    640, 240,
    0, 480,
    320, 480,
    640, 480;
  ref_projections = tmp.transpose();
  double depths[] = {
    1, 0.5, 0.75,
    100, 1, 0.75,
    0.75, 0.5, 1,
  };

  Eigen::Matrix<double, 3, 4> K;
  K << fx, 0, cx, 0,
    0, fx, cy, 0,
    0, 0, 1, 0;

  for(int i=0; i<num_points; i++) {
    points_xyz(0, i) = depths[i] * (ref_projections(0, i) - cx) / fx;
    points_xyz(1, i) = depths[i] * (ref_projections(1, i) - cy) / fx;
    points_xyz(2, i) = depths[i];
    points_xyz(3, i) = 1;

    Eigen::Vector3d uvw = K * points_xyz.col(i);
    uvw(0) /= uvw(2);
    uvw(1) /= uvw(2);
    uvw(2) = 1;

    transformed_xyz.col(i) = true_motion.inverse().matrix() * points_xyz.col(i);

    Eigen::Vector4d t = transformed_xyz.col(i);
    Eigen::Vector4d p = points_xyz.col(i);

    printf("%3d : %6.2f %6.2f  ->  %6.2f %6.2f %6.2f  -> %7.3f %7.3f %7.3f\n",
        i,
        uvw(0), uvw(1),
        p(0), p(1), p(2),
        t(0), t(1), t(2));
  }
  printf("=======\n");

  Eigen::Isometry3d initial_estimate;
  initial_estimate.setIdentity();
  Eigen::Vector3d initial_rpy = isometryGetRollPitchYaw(initial_estimate);
  Eigen::Vector3d initial_trans = initial_estimate.translation();

  Eigen::Isometry3d estimate = refineMotionEstimate(transformed_xyz,
      ref_projections,
      fx, cx, cy,
      initial_estimate, 6);

  Eigen::Matrix<double, 3, 4> P = K * estimate.matrix();

  Eigen::Vector3d estimated_rpy = isometryGetRollPitchYaw(estimate);
  Eigen::Vector3d estimated_trans = estimate.translation();
  printf("       Estimate   True    Initial\n"
      "tx:    %6.2f %6.2f %6.2f\n"
      "ty:    %6.2f %6.2f %6.2f\n"
      "tz:    %6.2f %6.2f %6.2f\n"
      "roll:  %6.2f %6.2f %6.2f\n"
      "pitch: %6.2f %6.2f %6.2f\n"
      "yaw:   %6.2f %6.2f %6.2f\n",
      estimated_trans(0), tx, initial_trans(0),
      estimated_trans(1), ty, initial_trans(1),
      estimated_trans(2), tz, initial_trans(2),
      estimated_rpy(0) * 180 / M_PI, roll * 180 / M_PI,  initial_rpy(0) * 180 / M_PI,
      estimated_rpy(1) * 180 / M_PI, pitch * 180 / M_PI, initial_rpy(1) * 180 / M_PI,
      estimated_rpy(2) * 180 / M_PI, yaw * 180 / M_PI,   initial_rpy(2) * 180 / M_PI);

  // compute reprojection error
  for(int i=0; i<num_points; i++) {
    Eigen::Vector3d uvw = P * transformed_xyz.col(i);
    double u = uvw(0) / uvw(2);
    double v = uvw(1) / uvw(2);
    double ref_u = ref_projections(0, i);
    double ref_v = ref_projections(1, i);
    double err_u = u - ref_u;
    double err_v = v - ref_v;

    printf("%3d  %6.1f %6.1f -> %6.1f %6.1f (%6.2f %6.2f)\n", i, ref_u, ref_v, u, v, err_u, err_v);
  }

  return 0;
}