/******************************************************************************/
/* Copyright 1993 MBARI                                                       */
/******************************************************************************/
/* Summary  : Functions to filter sensor data                                 */
/* Filename : filterFP.c (from Andy Pearce's filter.c)                        */
/* Author   : Janice Tarrant                                                  */
/* Project  : Tiburon                                                         */
/* Version  : Version 1.0                                                     */
/* Created  : 06/29/93                                                        */
/* Modified :                                                                 */
/* Archived :                                                                 */
/******************************************************************************/
/* Modification History :                                                     */
/* $Header: filterFP.c,v 1.1 97/12/04 15:28:01 oreilly Exp $
 * $Log:        filterFP.c,v $
 * Revision 1.1  97/12/04  15:28:01  15:28:01  oreilly (Thomas C. O'Reilly)
 * Initial revision
 *
 *
 */
/******************************************************************************/

#include <mbariTypes.h>         /* MBARI style guide type declarations        */
#include "filterFP.h"           /* floating point filter definitions          */


/******************************************************************************/
/* Function : initIIRFilter                                                   */
/* Purpose  : Initializes the Infinite Impulse Response filter.               */
/* Inputs   : Raw data values pointer, filtered data values pointer, gain     */
/*            pointer, max value.                                             */
/* Outputs  : None.                                                           */
/******************************************************************************/
    Void
initIIRFilter(Flt32 raw[], Flt32 filtered[], Nat16 *gain, Nat16 maxValue)
{
    Int16 i;

    for (i = 0; i < IIR_RAW; i++)
        raw[i] = 0.0;

    for (i = 0; i < IIR_FILT; i++)
        filtered[i] = 0.0;

    for (i = 14; i >= 0; i--)
        if (maxValue >= (1 << i))
        {
            *gain = 14 - i;
            return;
        }

} /* initIIRFilter */


/******************************************************************************/
/* Function : IIFFilterFP                                                     */
/* Purpose  : Infinite Impulse Response Filter.                               */
/* Inputs   : New sample value, raw data values pointer, filtered data values */
/*            pointer.                                                        */
/* Outputs  : None.                                                           */
/******************************************************************************/
    Flt32
IIRFilterFP(Flt32 newRaw, Flt32 raw[], Flt32 filtered[])
{
    Flt32 filtValue;
    const Flt64 coeff[] = { 1.25, -0.5, 0.0625, 0.125, 0.0625 };

    filtValue = (Flt32) ( coeff[0] * (Flt64) filtered[0] +
                          coeff[1] * (Flt64) filtered[1] +
                          coeff[2] * (Flt64) newRaw +
                          coeff[3] * (Flt64) raw[0] +
                          coeff[4] * (Flt64) raw[1] );

    raw[1] = raw[0];                    /* Shuffle Raw Data Value */
    raw[0] = newRaw;

    filtered[1] = filtered[0];          /* Shuffle Filtered Data Value */
    filtered[0] = filtValue;

    return(filtValue);

} /* IIRfilterFP() */