priority_q.h

nRF24E1 sample sensor node code

}

template <class ITEM>
void HeapQueue<ITEM>::Delete(ITEM* item)
{
    int i=item->pos;

    num_of_elems--;
    elems[i]=elems[num_of_elems];
    elems[i]->pos=i;
    SiftDown(i);
    PercolateUp(i);
}



#define CQ_MAX_SAMPLES 25

template <class ITEM>
class CalendarQueue 
{
public:
        CalendarQueue();
        const char* GetName();
        ~CalendarQueue();
        void enqueue(ITEM*);
        ITEM* dequeue();
	void EnQueue(ITEM*);
	ITEM* DeQueue();
	ITEM* NextEvent() const { return m_head;}
        void Delete(ITEM*);
private:
        long last_bucket,number_of_buckets;
        double bucket_width;
        
        void ReSize(long);
        double NewWidth();

        ITEM ** buckets;
        long total_number;
        double bucket_top;
        long bottom_threshold;
        long top_threshold;
        double last_priority;
        bool resizable;

	ITEM* m_head;
	char m_name[100];
};


template <class ITEM>
const char* CalendarQueue<ITEM> :: GetName()
{
    sprintf(m_name,"Calendar Queue (bucket width: %.2e, size: %ld) ",
	    bucket_width,number_of_buckets);
    return m_name;
}
template <class ITEM>
CalendarQueue<ITEM>::CalendarQueue()
{
        long i;
        
        number_of_buckets=16;
        bucket_width=1.0;
        bucket_top=bucket_width;
        total_number=0;
        last_bucket=0;
        last_priority=0.0;
        top_threshold=number_of_buckets*2;
        bottom_threshold=number_of_buckets/2-2;
        resizable=true;
        
        buckets= new ITEM*[number_of_buckets];
        for(i=0;i<number_of_buckets;i++)
                buckets[i]=NULL;
	m_head=NULL;

}
template <class ITEM>
CalendarQueue<ITEM>::~CalendarQueue()
{
        delete [] buckets;
}
template <class ITEM>
void CalendarQueue<ITEM>::ReSize(long newsize)
{
        long i;
        ITEM** old_buckets=buckets;
        long old_number=number_of_buckets;
        
        resizable=false;
        bucket_width=NewWidth();
        buckets= new ITEM*[newsize];
        number_of_buckets=newsize;
        for(i=0;i<newsize;i++)
                buckets[i]=NULL;
        last_bucket=0;
        total_number=0;

	//printf("Resize to width=%e and size=%ld\n",bucket_width,newsize);
        
        ITEM *item;
        for(i=0;i<old_number;i++)
        {
                while(old_buckets[i]!=NULL)
                {
                        item=old_buckets[i];
                        old_buckets[i]=item->next;
                        enqueue(item);
                }
        }
        resizable=true;
        delete[] old_buckets;
        number_of_buckets=newsize;
        top_threshold=number_of_buckets*2;
        bottom_threshold=number_of_buckets/2-2;
        bucket_top=bucket_width*((long)(last_priority/bucket_width)+1)+bucket_width*0.5;
        last_bucket = long(last_priority/bucket_width) % number_of_buckets;

}
template <class ITEM>
ITEM* CalendarQueue<ITEM>::DeQueue()
{
    ITEM* head=m_head;
    m_head=dequeue();
    return head;
}
template <class ITEM>
ITEM* CalendarQueue<ITEM>::dequeue()
{
        long i;
        for(i=last_bucket;;)
        {
                if(buckets[i]!=NULL&&buckets[i]->time<bucket_top)
                {
                        ITEM * item=buckets[i];
                        buckets[i]=buckets[i]->next;
                        total_number--;
                        last_bucket=i;
                        last_priority=item->time;
                        
                        if(resizable&&total_number<bottom_threshold)
                                ReSize(number_of_buckets/2);
                        item->next=NULL;
                        return item;
                }
                else
                {
                        i++;
                        if(i==number_of_buckets)i=0;
                        bucket_top+=bucket_width;
                        if(i==last_bucket)
                                break;
                }
        }
        
        /* Start Direct Search */
        long smallest;
        for(smallest=0;smallest<number_of_buckets;smallest++)
                if(buckets[smallest]!=NULL)break;

        if(smallest >= number_of_buckets)
	{
	    last_priority=bucket_top;
	    return NULL;
	}

        for(i=smallest+1;i<number_of_buckets;i++)
        {
                if(buckets[i]==NULL)
                        continue;
                else
                        if(buckets[i]->time<buckets[smallest]->time)
                                smallest=i;
        }
        ITEM * item=buckets[smallest];
        buckets[smallest]=buckets[smallest]->next;
        total_number--;
        last_bucket=smallest;
        last_priority=item->time;
        bucket_top=bucket_width*((long)(last_priority/bucket_width)+1)+bucket_width*0.5;
        item->next=NULL;
        return item;
}
template <class ITEM>
void CalendarQueue<ITEM>::EnQueue(ITEM* item)
{
    //printf("Enqueue %f\n",item->time);
    if(m_head==NULL)
    {
	m_head=item;
	return;
    }
    if(m_head->time>item->time)
    {
	enqueue(m_head);
	m_head=item;
    }
    else
	enqueue(item);
}
template <class ITEM>
void CalendarQueue<ITEM>::enqueue(ITEM* item)
{
    long i;
    if(item->time<last_priority)
    {
        i=(long)(item->time/bucket_width);
        last_priority=item->time;
        bucket_top=bucket_width*(i+1)+bucket_width*0.5;
        i=i%number_of_buckets;
	last_bucket=i;
    }
    else
    {
        i=(long)(item->time/bucket_width);
        i=i%number_of_buckets;
    }

        
        /*Insert into buckets[i] */

        if(buckets[i]==NULL||item->time<buckets[i]->time)
        {
                item->next=buckets[i];
                buckets[i]=item;
        }
        else
        {

	    ITEM* pos=buckets[i];
                while(pos->next!=NULL&&item->time>pos->next->time)
                {
                        pos=pos->next;
                }
                item->next=pos->next;
                pos->next=item;
        }
        total_number++;
        if(resizable&&total_number>top_threshold)
                ReSize(number_of_buckets*2);
}
template <class ITEM>
void CalendarQueue<ITEM>::Delete(ITEM* item)
{
    if(item==m_head)
    {
	m_head=dequeue();
	return;
    }
    long j;
    j=(long)(item->time/bucket_width);
    j=j%number_of_buckets;
        
    /*remove from buckets[j] */

    // the address of the pointer that points
    // to the current item

    ITEM** p = &buckets[j];
    // the current item
    ITEM* i=buckets[j];
    
    while(i!=NULL)
    {
        if(i==item)
	{ /* got it */
            (*p)=item->next;
            total_number--;
            if(resizable&&total_number<bottom_threshold)
                ReSize(number_of_buckets/2);
            return;
        }
        p=&(i->next);
        i=i->next;
    }   
}
template <class ITEM>
double CalendarQueue<ITEM>::NewWidth()
{
        long i, nsamples;
        
        if(total_number<2) return 1.0;
        if(total_number<=5)
                nsamples=total_number;
        else
                nsamples=5+total_number/10;
        if(nsamples>CQ_MAX_SAMPLES) nsamples=CQ_MAX_SAMPLES;
        
        long _last_bucket=last_bucket;
        double _bucket_top=bucket_top;
        double _last_priority=last_priority;
        
        double AVG[CQ_MAX_SAMPLES],avg1=0,avg2=0;
        ITEM* list,*next,*item;
        
        list=dequeue(); 
        long real_samples=0;
        while(real_samples<nsamples)
        {
                item=dequeue();
                if(item==NULL)
                {
                        item=list;
                        while(item!=NULL)
                        {
                                next=item->next;
                                enqueue(item);
                                item=next;      
                        }

                        last_bucket=_last_bucket;
                        bucket_top=_bucket_top;
                        last_priority=_last_priority;

                        
                        return 1.0;
                }
                AVG[real_samples]=item->time-list->time;
                avg1+=AVG[real_samples];
                if(AVG[real_samples]!=0.0)
                        real_samples++;
                item->next=list;
                list=item;
        }
        item=list;
        while(item!=NULL)
        {
                next=item->next;
                enqueue(item);
                item=next;      
        }
        
        last_bucket=_last_bucket;
        bucket_top=_bucket_top;
        last_priority=_last_priority;
        
        avg1=avg1/(double)(real_samples-1);
        avg1=avg1*2.0;
        
        //printf("avg1: %le\n",avg1);
        long count=0;
        for(i=0;i<real_samples-1;i++)
        {
                if(AVG[i]<avg1&&AVG[i]!=0)
                {
                        avg2+=AVG[i];
                        count++;
                }
        }
        if(count==0||avg2==0)   return 1.0;
        
        avg2 /= (double) count;
        avg2 *= 3.0;
        
        return avg2;
}

#endif /*PRIORITY_QUEUE_H*/