priority_q.h

nRF24E1 sample sensor node code

/* 

*   Copyright 2003 Gilbert (Gang) Chen, Boleslaw K. Szymanski and
*   Rensselaer Polytechnic Institute. All worldwide rights reserved.
*   A license to use, copy, modify and distribute this software for
*   non-commercial research purposes only is hereby granted, provided
*   that this copyright notice and accompanying disclaimer is not
*   modified or removed from the software.
*
*   DISCLAIMER: The software is distributed "AS IS" without any
*   express or implied warranty, including but not limited to, any
*   implied warranties of merchantability or fitness for a particular
*   purpose or any warranty of non-infringement of any current or
*   pending patent rights. The authors of the software make no
*   representations about the suitability of this software for any
*   particular purpose. The entire risk as to the quality and
*   performance of the software is with the user. Should the software
*   prove defective, the user assumes the cost of all necessary
*   servicing, repair or correction. In particular, neither Rensselaer
*   Polytechnic Institute, nor the authors of the software are liable
*   for any indirect, special, consequential, or incidental damages
*   related to the software, to the maximum extent the law permits.

*/

#ifndef PRIORITY_QUEUE_H
#define PRIORITY_QUEUE_H

/*
  Five Priority Queues:

  SimpleQueue: Double Linked list
  GuardedQueue: Derived from SimpleQueue, checks before EnQueue() and Delete()
  ErrorQueue: Derived from SimpleQueue, only correct half of the time (for debugging)
  HeadQueue: Implicit Heap
  CalendarQueue: The fastest

  Last Modified: Nov 18, 2002 by Gilbert Chen 

*/

template < class ITEM >
class SimpleQueue 
{
public:
    SimpleQueue() :m_head(NULL) {};
    void EnQueue(ITEM*);
    ITEM* DeQueue();
    void Delete(ITEM*);
    ITEM* NextEvent() const { return m_head; };
    const char* GetName();
protected:
    ITEM* m_head;
};

template <class ITEM>
const char* SimpleQueue<ITEM>::GetName()
{
    static const char* name = "SimpleQueue";
    return name;
}

template <class ITEM>
void SimpleQueue<ITEM>::EnQueue(ITEM* item)
{
    if( m_head==NULL || item->time < m_head->time )
    {
        if(m_head!=NULL)m_head->prev=item;
        item->next=m_head;
        m_head=item;
	item->prev=NULL;
        return;
    }
    
    ITEM* i=m_head;
    while( i->next!=NULL && item->time > i->next->time)
        i=i->next;
    item->next=i->next;
    if(i->next!=NULL)i->next->prev=item;
    i->next=item;
    item->prev=i;

}

template <class ITEM>
ITEM* SimpleQueue<ITEM> ::DeQueue()
{
    if(m_head==NULL)return NULL;
    ITEM* item = m_head;
    m_head=m_head->next;
    if(m_head!=NULL)m_head->prev=NULL;
    return item;
}

template <class ITEM>
void SimpleQueue<ITEM>::Delete(ITEM* item)
{
    if(item==NULL) return;

    if(item==m_head)
    {
        m_head=m_head->next;
	if(m_head!=NULL)m_head->prev=NULL;
    }
    else
    {
        item->prev->next=item->next;
        if(item->next!=NULL)
	    item->next->prev=item->prev;
    }

}

template <class ITEM>
class GuardedQueue : public SimpleQueue<ITEM>
{
 public:
    void Delete(ITEM*);
    void EnQueue(ITEM*);
    bool Validate(const char*);
};
template <class ITEM>
void GuardedQueue<ITEM>::EnQueue(ITEM* item)
{
    ITEM* i=SimpleQueue<ITEM>::m_head;
    while(i!=NULL)
    {
	if(i==item)
	{
	    pthread_printf("queue error: item %f(%p) is already in the queue\n",item->time,item);
	}
	i=i->next;
    }
    SimpleQueue<ITEM>::EnQueue(item);
}

template <class ITEM>
void GuardedQueue<ITEM>::Delete(ITEM* item)
{
    ITEM* i=SimpleQueue<ITEM>::m_head;
    while(i!=item&&i!=NULL)i=i->next;
    if(i==NULL)
	pthread_printf("error: cannot find the to-be-deleted event %f(%p)\n",item->time,item);
    else
        SimpleQueue<ITEM>::Delete(item);
}

template <class ITEM>
bool GuardedQueue<ITEM>::Validate(const char* s)
{
    char out[1000],buff[100];

    ITEM* i=SimpleQueue<ITEM>::m_head;
    bool qerror=false;

    sprintf(out,"queue error %s : ",s);
    while(i!=NULL)
    {
	sprintf(buff,"%f ",i->time);
	strcat(out,buff);
	if(i->next!=NULL)
	    if(i->next->prev!=i)
	    {
		qerror=true;
		sprintf(buff," {broken} ");
		strcat(out,buff);
	    }
	if(i==i->next)
	{
	    qerror=true;
	    sprintf(buff,"{loop}");
	    strcat(out,buff);
	    break;
	}
	i=i->next;
    }
    if(qerror)
	printf("%s\n",out);
    return qerror;
}

template <class ITEM>
class ErrorQueue : public SimpleQueue<ITEM>
{
 public:
    ITEM* DeQueue(double);
    const char* GetName();
};

template <class ITEM>
const char* ErrorQueue<ITEM>::GetName()
{
    static const char* name = "ErrorQueue";
    return name;
}

template <class ITEM>
ITEM* ErrorQueue<ITEM> ::DeQueue(double stoptime)
{
    //Validate("Before DeQueue()");

    if(drand48()>0.5)
	return SimpleQueue<ITEM>::DeQueue();

    int s=0;
    ITEM* e;
    e=SimpleQueue<ITEM>::m_head;
    while(e!=NULL&&e->time<stoptime)
    {
	s++;
	e=e->next;
    }
    e=SimpleQueue<ITEM>::m_head;
    s=(int)(s*drand48());
    while(s!=0)
    {
	e=e->next;
	s--;
    }
    Delete(e);
    return e;
}

template < class ITEM >
class HeapQueue 
{
public:
        HeapQueue();
        ~HeapQueue();
        void EnQueue(ITEM*);
        ITEM* DeQueue();
        void Delete(ITEM*);
	const char* GetName();
	ITEM* NextEvent() const { return num_of_elems?elems[0]:NULL; };
private:
        void SiftDown(int);
        void PercolateUp(int);
	void Validate(const char*);
        
        ITEM** elems;
        int num_of_elems;
        int curr_max;
};

template <class ITEM>
const char* HeapQueue<ITEM>::GetName()
{
    static const char* name = "HeapQueue";
    return name;
}

template <class ITEM>
void HeapQueue<ITEM>::Validate(const char* s)
{
    int i,j;
    char out[1000],buff[100];
    for(i=0;i<num_of_elems;i++)
	if(  ((2*i+1)<num_of_elems&&elems[i]->time>elems[2*i+1]->time) ||
	     ((2*i+2)<num_of_elems&&elems[i]->time>elems[2*i+2]->time) )
	{
	    sprintf(out,"queue error %s : ",s);
	    for(j=0;j<num_of_elems;j++)
	    {
		if(i!=j)
		    sprintf(buff,"%f(%d) ",elems[j]->time,j);
		else
		    sprintf(buff,"{%f(%d)} ",elems[j]->time,j);
		strcat(out,buff);
	    }
	    printf("%s\n",out);
	}
}
template <class ITEM>
HeapQueue<ITEM>::HeapQueue()
{
        curr_max=16;
        elems=new ITEM*[curr_max];
        num_of_elems=0;
}
template <class ITEM>
HeapQueue<ITEM>::~HeapQueue()
{
        delete [] elems;
}
template <class ITEM>
void HeapQueue<ITEM>::SiftDown(int node)
{
        if(num_of_elems<=1) return;
        int i=node,k,c1,c2;
        ITEM* temp;
        
        do{
                k=i;
                c1=c2=2*i+1;
                c2++;
                if(c1<num_of_elems && elems[c1]->time < elems[i]->time)
                        i=c1;
                if(c2<num_of_elems && elems[c2]->time < elems[i]->time)
                        i=c2;
                if(k!=i)
                {
                        temp=elems[i];
                        elems[i]=elems[k];
                        elems[k]=temp;
			elems[k]->pos=k;
			elems[i]->pos=i;
                }
        }while(k!=i);
}
template <class ITEM>
void HeapQueue<ITEM>::PercolateUp(int node)
{
        int i=node,k,p;
        ITEM* temp;
        
        do{
                k=i;
		if( (p=(i+1)/2) != 0)
                {
                        --p;
                        if(elems[i]->time < elems[p]->time)
                        {
                                i=p;
                                temp=elems[i];
                                elems[i]=elems[k];
                                elems[k]=temp;
				elems[k]->pos=k;
				elems[i]->pos=i;
                        }
                }
        }while(k!=i);
}
template <class ITEM>
void HeapQueue<ITEM>::EnQueue(ITEM* item)
{
        if(num_of_elems>=curr_max)
        {
                curr_max*=2;
                ITEM** buffer=new ITEM*[curr_max];
                for(int i=0;i<num_of_elems;i++)
                        buffer[i]=elems[i];
                delete[] elems;
                elems=buffer;
        }
        
        elems[num_of_elems]=item;
	elems[num_of_elems]->pos=num_of_elems;
	num_of_elems++;
        PercolateUp(num_of_elems-1);
}

template <class ITEM>
ITEM* HeapQueue<ITEM>::DeQueue()
{
        if(num_of_elems<=0)return NULL;
        
        ITEM* item=elems[0];
	num_of_elems--;
        elems[0]=elems[num_of_elems];
	elems[0]->pos=0;
        SiftDown(0);
        return item;