/****************************************************************************/
/* Copyright 1992 - 1994 MBARI                                              */
/****************************************************************************/
/* Summary  : Median Filter Algorithm for VxWorks                           */
/* Filename : median.c                                                      */
/* Author   : Andrew Pearce                                                 */
/* Project  : New ROV                                                       */
/* Version  : Version 1.0                                                   */
/* Created  : 02/07/94                                                      */
/* Modified : 02/07/94                                                      */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/* $Header:$
 * $Log:$
 */
/****************************************************************************/

#include <vxWorks.h>                /* vxWorks system declarations          */

#include "mbariTypes.h"             /* MBARI style guide type declarations  */
#include "mbariConst.h"       	    /* Miscellaneous constants              */
#include "medianFilter.h"	    /* Filter Data Types and functions      */

    Void
initMedianFilter( MedianFilter *filter, Int16 samples )
{
    Int16 i;
   
    filter->initialized = FALSE;
    filter->samples = 0;
    filter->nextPtr = 0;

    if ( (filter->values = (Flt32 *) malloc(sizeof(Flt32) * samples))
	== (Flt32 *) NULL)
	return;

    if ( (filter->sorted = (Flt32*) malloc(sizeof(Flt32) * samples))
	== (Flt32 *) NULL)
    {
	free(filter->values);
	return;
    } /* if */

    for (i = 0; i < samples; i++)
	filter->values[i] = filter->sorted[i] = 0.0;

    filter->samples = samples;
} /* initMedianFilter() */

    Flt32
medianFilter( MedianFilter *filter, Flt32 newValue )
{
    Reg Int16 i, j, samples;
    Reg Flt32 a;
    
    samples = filter->samples;

    if (samples == 0)	        /* Filter did not initialize correctly      */
	return (newValue);

                                /* Initialize: Fill array with new value    */
    if (filter->initialized == FALSE)
    {
	for (i = 0; i < samples; i++)
	    filter->values[i] = filter->sorted[i] = newValue;

	filter->initialized = TRUE;
	return (newValue);
    } /* if */

                                /* Add new value to value buffer ring       */
    filter->values[filter->nextPtr++] = newValue;

    if (filter->nextPtr >= samples)
	filter->nextPtr = 0;	/* Adjust run input pointer                 */
   
                                /* Copy values into sort array for sorting  */
    for (i = 0; i < samples; i++)
	filter->sorted[i] = filter->values[i];
    
                                /* Sort array of values into ascending order*/
    for (j = 1; j < samples; j++)
    {
	i = j - 1;
	a = filter->sorted[j];
	while (filter->sorted[i] > a)
	{
	    filter->sorted[i+1] = filter->sorted[i];

	    if (--i < 0)
		break;
	} /* while */

	filter->sorted[i+1] = a;
    } /* for */

                                 /* Extract middle value and return it       */
    return (filter->sorted[ (samples-1) / 2 ]);
} /* medianFilter() */