voted_tracking.h

// This file is part of snark, a generic and flexible library
// for robotics research.
//
// Copyright (C) 2011 The University of Sydney
//
// snark is free software; 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.
//
// snark 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 Lesser General Public License
// for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with snark. If not, see <http://www.gnu.org/licenses/>.

#ifndef SNARK_PERCEPTION_ALGORITHMS_ALGORITHMS_VOTEDTRACKING_HEADER_GUARD_
#define SNARK_PERCEPTION_ALGORITHMS_ALGORITHMS_VOTEDTRACKING_HEADER_GUARD_

#include <stdlib.h>
#include <algorithm>
#include <map>
#include <boost/optional.hpp>

namespace snark { 
    
template < typename It, typename Id, typename F >
Id voted_tracking( It begin, It end, F GetPreviousId, Id vacantId );

namespace impl {

template < typename P > inline bool comparePairs( const P& i, const P& j ) { return i.second < j.second; }
    
} // namespace impl {

template < typename It, typename Id, typename F >
inline Id voted_tracking( It begin, It end, F getPreviousId, Id vacantId )
{
    typedef std::map< Id, std::size_t > Map;
    typedef std::pair< Id, std::size_t > Pair;
    Map ids;
    for( It it = begin; it != end; ++it )
    {
        boost::optional< Id > previous = getPreviousId( it );
        if( !previous ) { continue; }
        std::pair< typename Map::iterator, bool > p = ids.insert( Pair( *previous, 0 ) );
        if( !p.second ) { ++p.first->second; }
    }
    return ids.empty() ? vacantId
                       : std::max_element( ids.begin(), ids.end(), impl::comparePairs< Pair > )->first;
}
    
} // namespace snark { 

#endif // #ifndef SNARK_PERCEPTION_ALGORITHMS_ALGORITHMS_VOTEDTRACKING_HEADER_GUARD_