You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
203 lines
4.0 KiB
203 lines
4.0 KiB
//--------------Includes-----------------
|
|
#include<stdio.h>
|
|
#include<stdlib.h>
|
|
#include<strings.h>
|
|
#include"heap.h"
|
|
#include"error.h"
|
|
#include"ostypes.h"
|
|
#include"printf.h"
|
|
|
|
//--------------Functions----------------
|
|
|
|
/** Gives the index of the left child of a node.
|
|
*
|
|
* @param iind Index of the node itself.
|
|
* @retval Index of the left child of the node.
|
|
*/
|
|
static inline uint8_t osHeapChildL(uint8_t iind)
|
|
{
|
|
return (2*iind + 1);
|
|
}
|
|
|
|
/** Gives the index of the right child of a node.
|
|
*
|
|
* @param iind Index of the node itself.
|
|
* @retval Index of the left child of the node.
|
|
*/
|
|
static inline uint8_t osHeapChildR(uint8_t iind)
|
|
{
|
|
return (2*iind + 2);
|
|
}
|
|
|
|
/** Gives the index of the parent of a node.
|
|
*
|
|
* @param iind Index of the node itself.
|
|
* @retval Index of the left child of the node.
|
|
*/
|
|
static inline uint8_t osHeapParent(uint8_t iind)
|
|
{
|
|
return (iind-1)/2;
|
|
}
|
|
|
|
/** Gives the size of the heap.
|
|
*
|
|
* @param ioarray Array where the heap is stored.
|
|
* @retval Size of the heap.
|
|
*/
|
|
static inline uint8_t osHeapSize(osHeapNode_t* ioarray)
|
|
{
|
|
int i;
|
|
for(i=0; i<HEAP_SIZE; i++)
|
|
{
|
|
if(ioarray[i] == NULL)
|
|
return i;
|
|
}
|
|
return i;
|
|
}
|
|
|
|
/** Delete one element in the heap.
|
|
*
|
|
* @param ioarray Array where the heap is stored.
|
|
* @retval Index of the left child of the node.
|
|
*/
|
|
static inline void osHeapDelete(osHeapNode_t* ioarray, uint8_t ii)
|
|
{
|
|
ioarray[ii] = NULL;
|
|
}
|
|
|
|
/** Swap two elements in the heap.
|
|
*
|
|
* @param ioarray Array where the heap is stored.
|
|
* @param ia Index where element a is stored.
|
|
* @param ib Index where element b is stored.
|
|
*/
|
|
static inline void osHeapSwap(osHeapNode_t* ioarray, uint8_t ia, uint8_t ib)
|
|
{
|
|
osHeapNode_t z;
|
|
z = ioarray[ia];
|
|
ioarray[ia] = ioarray[ib];
|
|
ioarray[ib] = z;
|
|
}
|
|
|
|
void osHeapInit(osHeapNode_t* ioarray)
|
|
{
|
|
for(int i=0; i<HEAP_SIZE; i++)
|
|
{
|
|
osHeapDelete(ioarray, i);
|
|
}
|
|
}
|
|
|
|
void osHeapHeapify(osHeapNode_t* ioarray, uint8_t iind)
|
|
{
|
|
uint8_t left = osHeapChildL(iind);
|
|
uint8_t right = osHeapChildR(iind);
|
|
uint8_t largest = 0;
|
|
if( ioarray[left] != NULL )
|
|
{
|
|
if( ioarray[left]->priority > ioarray[iind]->priority )
|
|
largest = left;
|
|
}
|
|
else
|
|
largest = iind;
|
|
if( ioarray[right] != NULL )
|
|
{
|
|
if( ioarray[right]->priority > ioarray[largest]->priority )
|
|
largest = right;
|
|
}
|
|
if( largest != iind)
|
|
{
|
|
osHeapSwap(ioarray, iind, largest);
|
|
osHeapHeapify(ioarray, largest);
|
|
}
|
|
}
|
|
|
|
void osHeapBuild(osHeapNode_t* ioarray)
|
|
{
|
|
for(int i = (osHeapSize(ioarray)/2); i >= 0; i--)
|
|
{
|
|
osHeapHeapify(ioarray, i);
|
|
}
|
|
}
|
|
|
|
uint8_t osHeapMaximum(osHeapNode_t* ioarray, osHeapNode_t* iomax)
|
|
{
|
|
if(osHeapSize(ioarray) > 0)
|
|
{
|
|
if(iomax != NULL)
|
|
(*iomax) = ioarray[0];
|
|
return 1;
|
|
}
|
|
else
|
|
/* Heap is empty. */
|
|
return 0;
|
|
}
|
|
|
|
uint8_t osHeapExtractMaximum(osHeapNode_t* ioarray, osHeapNode_t* iomax)
|
|
{
|
|
if(osHeapSize(ioarray) > 0)
|
|
{
|
|
if(iomax == NULL)
|
|
{
|
|
THROW_ERROR(E_NULL_FORBIDDEN);
|
|
return 0;
|
|
}
|
|
(*iomax) = ioarray[0];
|
|
osHeapSwap(ioarray, 0, osHeapSize(ioarray)-1);
|
|
osHeapDelete(ioarray, osHeapSize(ioarray)-1);
|
|
osHeapHeapify(ioarray,0);
|
|
return 1;
|
|
}
|
|
else
|
|
/* Heap is empty. */
|
|
return 0;
|
|
}
|
|
|
|
uint8_t osHeapInsert(osHeapNode_t* ioarray, osHeapNode_t x)
|
|
{
|
|
uint8_t i = 0;
|
|
if( osHeapSize(ioarray) < HEAP_SIZE)
|
|
{
|
|
ioarray[osHeapSize(ioarray)] = x;
|
|
i = osHeapSize(ioarray)-1;
|
|
while( (i>0) && (ioarray[osHeapParent(i)]->priority < ioarray[i]->priority) )
|
|
{
|
|
osHeapSwap(ioarray, i, osHeapParent(i));
|
|
i = osHeapParent(i);
|
|
}
|
|
return 1;
|
|
}
|
|
else
|
|
{
|
|
THROW_ERROR(E_HEAP_OVERLFOW);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
void osHeapPrintS(osHeapNode_t* ioarray)
|
|
{
|
|
if( osHeapIsEmpty(ioarray) )
|
|
return;
|
|
for(int i=0; i<HEAP_SIZE; i++)
|
|
{
|
|
if(ioarray[i] == 0)
|
|
continue;
|
|
#ifdef X86_TEST_ENVIRONMENT
|
|
printf("%d ", ioarray[i]->priority);
|
|
#else
|
|
osPrintf("%d ", ioarray[i]->priority);
|
|
#endif
|
|
}
|
|
#ifdef X86_TEST_ENVIRONMENT
|
|
printf("\n");
|
|
#else
|
|
osPrintf("\n\r");
|
|
#endif
|
|
}
|
|
|
|
uint8_t osHeapIsEmpty(osHeapNode_t* ioarray)
|
|
{
|
|
if(osHeapSize(ioarray) > 0)
|
|
return 0;
|
|
else
|
|
return 1;
|
|
}
|
|
|