/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License, Version 1.0 only
 * (the "License").  You may not use this file except in compliance
 * with the License.
 *
 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
 * or http://www.opensolaris.org/os/licensing.
 * See the License for the specific language governing permissions
 * and limitations under the License.
 *
 * When distributing Covered Code, include this CDDL HEADER in each
 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
 * If applicable, add the following below this CDDL HEADER, with the
 * fields enclosed by brackets "[]" replaced with your own identifying
 * information: Portions Copyright [yyyy] [name of copyright owner]
 *
 * CDDL HEADER END
 */
/*
 * Copyright (c) 1992-2001 by Sun Microsystems, Inc.
 * All rights reserved.
 */

#include <memory.h>
#include <stddef.h>
#include <sys/types.h>
#include <Fir.h>

extern "C" {
        char *bcopy(char *, char *, int);
        char *memmove(char *, char *, int);
}

#define BCOPY(src, dest, num) memmove(dest, src, num)

/*
 * convolve()
 * returns the convolution of coef[length] and in_buf[length]:
 *
 * convolution = coef[0] * in_buf[length - 1] +
 *               coef[1] * in_buf[length - 2] +
 *               ...
 *               coef[length - 1] * in_buf[0]
 */
double
convolve(
        double  *coefs,
        double  *in_buf,
        int     length)
{
        if (length <= 0)
                return (0.0);
        else {
                in_buf += --length;
                double sum = *coefs * *in_buf;
                while (length--)
                        sum += *++coefs * *--in_buf;
                return (sum);
        }
}

void                            // convert short to double
short2double(
        double *out,
        short *in,
        int size)
{
        while (size-- > 0)
                *out++ = (double)*in++;
}

short
double2short(double in)                 // limit double to short
{
        if (in <= -32768.0)
                return (-32768);
        else if (in >= 32767.0)
                return (32767);
        else
                return ((short)in);
}

void Fir::                              // update state with data[size]
updateState(
        double  *data,
        int     size)
{
        if (size >= order)
                memcpy(state, data + size - order, order * sizeof (double));
        else {
                int old = order - size;
                BCOPY((char *)(state + size), (char *)state,
                    old * sizeof (double));
                memcpy(state + order - size, data, size * sizeof (double));
        }
}

void Fir::
update_short(
        short   *in,
        int     size)
{
        double *in_buf = new double[size];
        short2double(in_buf, in, size);
        updateState(in_buf, size);
        delete[] in_buf;
}

void Fir::
resetState(void)                        // reset state to all zero
{
        for (int i = 0; i < order; i++)
                state[i] = 0.0;
}

Fir::
Fir(void)
{
}

Fir::
Fir(int order_in): order(order_in)      // construct Fir object
{
        state = new double[order];
        resetState();
        coef = new double[order + 1];
        delay = (order + 1) >> 1;       // assuming symmetric FIR
}

Fir::
~Fir()                                  // destruct Fir object
{
        delete coef;
        delete state;
}

int Fir::
getOrder(void)                          // returns filter order
{
        return (order);
}

int Fir::
getNumCoefs(void)                       // returns number of filter coefficients
{
        return (order + 1);
}

void Fir::
putCoef(double *coef_in)                // copy coef_in in filter coefficients
{
        memcpy(coef, coef_in, (order + 1) * sizeof (double));
}

void Fir::
getCoef(double *coef_out)               // returns filter coefs in coef_out
{
        memcpy(coef_out, coef, (order + 1) * sizeof (double));
}

int Fir::               // filter in[size], and updates the state.
filter_noadjust(
        short   *in,
        int     size,
        short   *out)
{
        if (size <= 0)
                return (0);

        double *in_buf = new double[size];
        short2double(in_buf, in, size);         // convert short input to double
        int     i;
        int     init_size = (size <= order)? size : order;
        int     init_order = order;
        double  *state_ptr = state;
        short   *out_ptr = out;

        // the first "order" outputs need state in convolution
        for (i = 1; i <= init_size; i++)
                *out_ptr++ = double2short(convolve(coef, in_buf, i) +
                    convolve(coef + i, state_ptr++, init_order--));

        // starting from "order + 1"th output, state is no longer needed
        state_ptr = in_buf;
        while (i++ <= size)
                *out_ptr++ =
                    double2short(convolve(coef, state_ptr++, order + 1));
        updateState(in_buf, size);
        delete[] in_buf;
        return (out_ptr - out);
}

int Fir::
getFlushSize(void)
{
        int group_delay = (order + 1) >> 1;
        return ((delay < group_delay)? group_delay - delay : 0);
}

int Fir::
flush(short *out)               // zero input response of Fir
{
        int num = getFlushSize();
        if (num > 0) {
                short *in = new short[num];
                memset(in, 0, num * sizeof (short));
                num = filter_noadjust(in, num, out);
                delete[] in;
        }
        return (num);
}

/*
 * filter() filters in[size] with filter delay adjusted to 0
 *
 * All FIR filters introduce a delay of "order" samples between input and
 * output sequences. Most FIR filters are symmetric filters to keep the
 * linear phase responses. For those FIR fitlers the group delay is
 * "(order + 1) / 2". So filter_nodelay adjusts the group delay in the
 * output sequence such that the output is aligned with the input and
 * direct comparison between them is possible.
 *
 * The first call of filter returns "size - group_delay" output samples.
 * After all the input samples have been filtered, filter() needs
 * to be called with size = 0 to get the residual output samples to make
 * the output sequence the same length as the input.
 *
 */

int Fir::
filter(
        short   *in,
        int     size,
        short   *out)
{
        if ((size <= 0) || (in == NULL))
                return (flush(out));
        else if (delay <= 0)
                return (filter_noadjust(in, size, out));
        else if (size <= delay) {
                update_short(in, size);
                delay -= size;
                return (0);
        } else {
                update_short(in, delay);
                in += delay;
                size -= delay;
                delay = 0;
                return (filter_noadjust(in, size, out));
        }
}