SortedList.hpp

/***************************************************************************
  tag: Peter Soetens  Wed Jan 18 14:11:39 CET 2006  SortedList.hpp

                        SortedList.hpp -  description
                           -------------------
    begin                : Wed January 18 2006
    copyright            : (C) 2006 Peter Soetens
    email                : peter.soetens@mech.kuleuven.be

 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU General Public                   *
 *   License as published by the Free Software Foundation;                 *
 *   version 2 of the License.                                             *
 *                                                                         *
 *   As a special exception, you may use this file as part of a free       *
 *   software library without restriction.  Specifically, if other files   *
 *   instantiate templates or use macros or inline functions from this     *
 *   file, or you compile this file and link it with other files to        *
 *   produce an executable, this file does not by itself cause the         *
 *   resulting executable to be covered by the GNU General Public          *
 *   License.  This exception does not however invalidate any other        *
 *   reasons why the executable file might be covered by the GNU General   *
 *   Public License.                                                       *
 *                                                                         *
 *   This library 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 General Public             *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/


#ifndef ORO_SORTED_LIST_HPP
#define ORO_SORTED_LIST_HPP

#include "os/CAS.hpp"
#include <boost/shared_ptr.hpp>
#include "MemoryPool.hpp"

namespace RTT
{
    template<class DataType_>
    class SortedList
    {
    public:
        typedef DataType_ DataType;
    protected:
        struct NodeType
        {
            typedef NodeType* NodeType_sptr;
            DataType key;
            NodeType_sptr next;

            NodeType(const DataType& data)
                : key(data)
            {}

            NodeType()
                : key()
            {}
        };

        typedef NodeType Node;
        typedef typename NodeType::NodeType_sptr Node_sptr;

        MemoryPool<Node> mpool;

        Node_sptr head;
        Node_sptr tail;

        Node_sptr search(const DataType& search_key, Node_sptr& left_node)
        {
            Node_sptr left_node_next, right_node;
        search_again:
            do {
                Node_sptr t = this->head;
                Node_sptr t_next = this->head->next;

                do {
                    if (!is_marked_reference(t_next)) {
                        left_node = t;
                        left_node_next = t_next;
                    }
                    t = get_unmarked_reference(t_next);
                    if (t == this->tail)
                        break;
                    t_next = t->next;
                } while (is_marked_reference(t_next) || (t->key < search_key));
                right_node = t;

                if (left_node_next == right_node)
                    if ((right_node != this->tail) && is_marked_reference(right_node->next))
                        goto search_again;
                    else
                        return right_node;

                if (OS::CAS(&(left_node->next), left_node_next, right_node)) {
                    mpool.deallocate( get_unmarked_reference(left_node_next) );
                    if ((right_node != this->tail) && is_marked_reference(right_node->next))
                        goto search_again;
                    else
                        return right_node;
                }
            } while (true);
        }

        bool is_marked_reference(Node* n)
        {
            unsigned int v = reinterpret_cast<unsigned int>(n);
            return (v%2) == 1;
        }

        Node* get_unmarked_reference(Node* n)
        {
            unsigned int v = reinterpret_cast<unsigned int>(n);
            return reinterpret_cast<Node*>( v & -2 ); // is ..11110
        }

        Node* get_marked_reference(Node* n)
        {
            unsigned int v = reinterpret_cast<unsigned int>(n);
            return reinterpret_cast<Node*>( v | 1);  // is ...00001
        }
    public:

        SortedList()
        {
            mpool.reserve();
            mpool.reserve();
            head = mpool.allocate();
            tail = mpool.allocate();

            head->next = tail;
        }

        ~SortedList()
        {
            mpool.deallocate(head);
            mpool.deallocate(tail);
        }

        void reserve(size_t n = 1)
        { size_t i(0); while (i++ < n) mpool.reserve(); }

        void shrink(size_t n = 1)
        { size_t i(0); while (i++ < n) mpool.shrink(); }

        bool empty() const
        {
            return head->next == tail;
        }

        bool insert(const DataType& key)
        {
            Node_sptr new_node( mpool.allocate() );
            Node_sptr right_node, left_node;

            new_node->key = key;
            do {
                right_node = this->search(key, left_node);
                if ((right_node != tail) && (right_node->key == key )) {
                    mpool.deallocate( new_node );
                    return false;
                }
                new_node->next = right_node;
                if (OS::CAS(&(left_node->next), right_node, new_node))
                    return true;
            } while (true);
        }

        bool erase(const DataType& search_key)
        {
            Node_sptr right_node, right_node_next, left_node;

            do {
                right_node = search(search_key, left_node);
                if (( right_node == tail) || !(right_node->key == search_key) )
                    return false;
                right_node_next = right_node->next;
                if (!is_marked_reference(right_node_next))
                    if (OS::CAS( &(right_node->next), right_node_next, get_marked_reference(right_node_next)))
                        break;
            } while(true);
            if (!OS::CAS(&(left_node->next), right_node, right_node_next))
                right_node = search(right_node->key, left_node);
            else
                mpool.deallocate( get_unmarked_reference(right_node) );
            return true;
        }

        bool hasKey(const DataType& search_key)
        {
            Node_sptr right_node, left_node;

            right_node = search( search_key, left_node);
            if ((right_node == tail) || !(right_node->key == search_key))
                return false;
            else
                return true;
        }

        template<class Function>
        void applyOnData(const Function& f )
        {
            // start at head, go until tail. This is similar to search().
            Node_sptr t = this->head;
            Node_sptr t_next = this->head->next;

            // Marked fields may be traversed !
            // thus go on until you hit the tail.
            do {
                // need to unmark always to be sure.
                t = get_unmarked_reference(t_next);
                if (t == this->tail)
                    return;
                if (!is_marked_reference(t_next)) {
                    // The applying conseptually starts here. If t_next is now concurrently erased,
                    // f() was already in progress of execution, so it continues.
                    f( t->key );
                }
                t_next = t->next;
            } while ( true );

        }

        template<class Function>
        void applyOnCopy(const Function& f )
        {
            // start at head, go until tail. This is similar to search().
            Node_sptr t;
            Node_sptr t_next = this->head->next;

            // Marked fields may be traversed !
            // thus go on until you hit the tail.
            do {
                // need to unmark always to be sure.
                t = get_unmarked_reference(t_next);
                if (t == this->tail)
                    return;
                // make the copy
                DataType d(t->key);
                if (!is_marked_reference(t_next)) {
                    // The applying conseptually starts here. If t_next is now concurrently erased,
                    // f() was already in progress of execution, so it continues.
                    f( d );
                }
                t_next = t->next;
            } while ( true );

        }
    };

}

#endif