source: pacpussensors/trunk/Vislab/lib3dv/detail/device_impl.hpp@ 140

#ifndef LIB3DV_DETAIL_ENDPOINT_IMPL_HPP
#define LIB3DV_DETAIL_ENDPOINT_IMPL_HPP

/* lib3dv/detail/endpoint_impl.hpp
 *
 * Copyright (C) 2013 VisLab
 *
 * This file is part of lib3dv; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, see <http://www.gnu.org/licenses/>.
 */

#include "protocol.h"

#include <boost/shared_ptr.hpp>

#include <algorithm>
#include <iterator>

template<typename C, typename F, typename H>
typename C::mapped_type lib3dv::detail::device_impl::init_reassembly_container(C& container, F& fragments_container, const H& data_info_header, uint32_t key)
{
    typedef typename C::mapped_type::element_type data_type;
    typedef typename F::mapped_type fragments_type;

    boost::shared_ptr<data_type> data = boost::shared_ptr<data_type>(new data_type());
    container[key] = data;

    data->m_header = data_info_header;
    data->m_buffer.resize(data_info_header.m_size);

    // the following is needed to handle out-of-order image fragments arriving before the corresponding image header
    const fragments_type& data_fragments = fragments_container[key];

    if(!data_fragments.empty())
    {
        data->m_fragments.resize(std::max(data_fragments.front()->m_total_fragments, 1U)); // it's time to initialize the bitset

        for(size_t f = 0; f < data_fragments.size(); ++f)
        {
            std::copy(data_fragments[f]->m_buffer.begin(), data_fragments[f]->m_buffer.end(), data->m_buffer.data() + data_fragments[f]->m_header.m_offset);
            data->m_fragments.set(data_fragments[f]->m_fragment);
        }

        if(data->m_fragments.count() == data->m_fragments.size())
            container.erase(key);
        else
                data.reset();
    }
    else
        data.reset();


    fragments_container.erase(key);

    return data;
}

template<typename C, typename F>
typename C::mapped_type lib3dv::detail::device_impl::fill_reassembly_container(C& container, F& fragments_container, size_t bytes_received, uint32_t key, uint32_t fragment, uint32_t total_fragments)
{
    typedef typename C::mapped_type::element_type data_type;
    typedef typename F::mapped_type::value_type::element_type fragments_type;

    const protocol::data_header* raw_data_header = reinterpret_cast<const protocol::data_header*>(m_data_payload.c_array() + sizeof(protocol::packet_header));
    const uint8_t* payload_data = m_data_payload.c_array() + sizeof(protocol::packet_header) + sizeof(protocol::data_header);
    uint32_t payload_size = static_cast<uint32_t>(bytes_received - (sizeof(protocol::packet_header) + sizeof(protocol::data_header)));

    if(m_log_level == 1)
    {
        std::cout << "°";
        std::cout.flush();
    }

    boost::shared_ptr<data_type> data;

    typename C::iterator dd = container.find(key);

    // we check that the data info header has already been received
    if(dd != container.end())
        data = dd->second;

    if(data && (!data->m_buffer.empty() || !data->m_header.m_size))
    {
        if(!data->m_header.m_size)
        {
                container.erase(key);
            return data;
        }


        std::copy(payload_data, payload_data + payload_size, data->m_buffer.data() + raw_data_header->m_offset);

        if(data->m_fragments.empty()) // the first data fragment has been received, it's time to initialize the bitset
            data->m_fragments.resize(std::max(total_fragments,1U));

        data->m_fragments.set(fragment);

        if(data->m_fragments.count() == data->m_fragments.size())
        {                             
            container.erase(key);
            return data;
        }
    }
    else // the fragment arrived before the corresponding header, we keep it aside for later processing
    {
        boost::shared_ptr<fragments_type> fragments = boost::shared_ptr<fragments_type>(new fragments_type());
        fragments->m_header = *raw_data_header;
        fragments->m_buffer.resize(payload_size);
        fragments->m_fragment = fragment;
        fragments->m_total_fragments = total_fragments;
        std::copy(payload_data, payload_data + payload_size, fragments->m_buffer.data());
        fragments_container[key].push_back(fragments);
    }
    
    return boost::shared_ptr<data_type>();
}

template<typename C, typename F>
void lib3dv::detail::device_impl::cleanup_reassembly_container(C& container, F& fragments_container, size_t max_size)
{
    typedef typename C::iterator container_iterator_type;
    typedef typename F::iterator fragments_container_iterator_type;
    
    if(m_log_level > 3){
        std::cout << "container size "<<container.size()<< " ";
        if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_image_type> > >::type::value){
             std::cout << "image!" << std::endl;
        }
        if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_terrain_type> > >::type::value){
             std::cout << "terrain!" << std::endl;
        }
        if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_obstacles_type> > >::type::value){
             std::cout << "obstacle!" << std::endl;
        }
        if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_motion_type> > >::type::value){
             std::cout << "motion!" << std::endl;
        }
        if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_candidate_type> > >::type::value){
             std::cout << "classification!" << std::endl;
        }

    }

    if(container.size() > max_size)
    {
        std::cout << "[WW] lib3dv: dropping " << container.size() - max_size << " objects ";
        if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_image_type> > >::type::value){
             std::cout << "image!" << std::endl;
        }
        if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_terrain_type> > >::type::value){
             std::cout << "terrain!" << std::endl;
        }
        if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_obstacles_type> > >::type::value){
             std::cout << "obstacle!" << std::endl;
        }
        if(boost::is_same<C,std::map<uint32_t, boost::shared_ptr<raw_motion_type> > >::type::value){
             std::cout << "motion!" << std::endl;
        }
        container_iterator_type bb = container.begin();
        container_iterator_type ee = container.begin();
        std::advance(ee, container.size() - max_size);

        container.erase(bb, ee);
    }
    
    if(fragments_container.size() > max_size)
    {
        std::cout << "[WW] lib3dv: dropping " << fragments_container.size() - max_size << " object fragments!" << std::endl;

        fragments_container_iterator_type bb = fragments_container.begin();
        fragments_container_iterator_type ee = fragments_container.begin();
        std::advance(ee, fragments_container.size() - max_size);

        fragments_container.erase(bb, ee);
    }
}

#endif