/*
 * Copyright (c) 2024 EdgeImpulse Inc.
 *
 * Generated by Edge Impulse and licensed under the applicable Edge Impulse
 * Terms of Service. Community and Professional Terms of Service
 * (https://edgeimpulse.com/legal/terms-of-service) or Enterprise Terms of
 * Service (https://edgeimpulse.com/legal/enterprise-terms-of-service),
 * according to your product plan subscription (the “License”).
 *
 * This software, documentation and other associated files (collectively referred
 * to as the “Software”) is a single SDK variation generated by the Edge Impulse
 * platform and requires an active paid Edge Impulse subscription to use this
 * Software for any purpose.
 *
 * You may NOT use this Software unless you have an active Edge Impulse subscription
 * that meets the eligibility requirements for the applicable License, subject to
 * your full and continued compliance with the terms and conditions of the License,
 * including without limitation any usage restrictions under the applicable License.
 *
 * If you do not have an active Edge Impulse product plan subscription, or if use
 * of this Software exceeds the usage limitations of your Edge Impulse product plan
 * subscription, you are not permitted to use this Software and must immediately
 * delete and erase all copies of this Software within your control or possession.
 * Edge Impulse reserves all rights and remedies available to enforce its rights.
 *
 * Unless required by applicable law or agreed to in writing, the Software is
 * distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific language governing
 * permissions, disclaimers and limitations under the License.
 */
#ifndef _EIDSP_SPECTRAL_FILTERS_H_
#define _EIDSP_SPECTRAL_FILTERS_H_

#include <math.h>
#include "../numpy.hpp"

#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288
#endif // M_PI

namespace ei {
namespace spectral {
namespace filters {
    /**
     * The Butterworth filter has maximally flat frequency response in the passband.
     * @param filter_order Even filter order (between 2..8)
     * @param sampling_freq Sample frequency of the signal
     * @param cutoff_freq Cut-off frequency of the signal
     * @param src Source array
     * @param dest Destination array
     * @param size Size of both source and destination arrays
     */
    static void butterworth_lowpass(
        int filter_order,
        float sampling_freq,
        float cutoff_freq,
        const float *src,
        float *dest,
        size_t size)
    {
        int n_steps = filter_order / 2;
        float a = tan(M_PI * cutoff_freq / sampling_freq);
        float a2 = pow(a, 2);
        float *A = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *d1 = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *d2 = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *w0 = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *w1 = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *w2 = (float*)ei_dsp_calloc(n_steps, sizeof(float));

        // Calculate the filter parameters
        for(int ix = 0; ix < n_steps; ix++) {
            float r = sin(M_PI * ((2.0 * ix) + 1.0) / (2.0 * filter_order));
            sampling_freq = a2 + (2.0 * a * r) + 1.0;
            A[ix] = a2 / sampling_freq;
            d1[ix] = 2.0 * (1 - a2) / sampling_freq;
            d2[ix] = -(a2 - (2.0 * a * r) + 1.0) / sampling_freq;
        }

        // Apply the filter
        for (size_t sx = 0; sx < size; sx++) {
            dest[sx] = src[sx];

            for (int i = 0; i < n_steps; i++) {
                w0[i] = d1[i] * w1[i] + d2[i] * w2[i] + dest[sx];
                dest[sx] = A[i] * (w0[i] + (2.0 * w1[i]) + w2[i]);
                w2[i] = w1[i];
                w1[i] = w0[i];
            }
        }

        ei_dsp_free(A, n_steps*sizeof(float));
        ei_dsp_free(d1, n_steps*sizeof(float));
        ei_dsp_free(d2, n_steps*sizeof(float));
        ei_dsp_free(w0, n_steps*sizeof(float));
        ei_dsp_free(w1, n_steps*sizeof(float));
        ei_dsp_free(w2, n_steps*sizeof(float));
    }

    /**
     * The Butterworth filter has maximally flat frequency response in the passband.
     * @param filter_order Even filter order (between 2..8)
     * @param sampling_freq Sample frequency of the signal
     * @param cutoff_freq Cut-off frequency of the signal
     * @param src Source array
     * @param dest Destination array
     * @param size Size of both source and destination arrays
     */
    static void butterworth_highpass(
        int filter_order,
        float sampling_freq,
        float cutoff_freq,
        const float *src,
        float *dest,
        size_t size)
    {
        int n_steps = filter_order / 2;
        float a = tan(M_PI * cutoff_freq / sampling_freq);
        float a2 = pow(a, 2);
        float *A = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *d1 = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *d2 = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *w0 = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *w1 = (float*)ei_dsp_calloc(n_steps, sizeof(float));
        float *w2 = (float*)ei_dsp_calloc(n_steps, sizeof(float));

        // Calculate the filter parameters
        for (int ix = 0; ix < n_steps; ix++) {
            float r = sin(M_PI * ((2.0 * ix) + 1.0) / (2.0 * filter_order));
            sampling_freq = a2 + (2.0 * a * r) + 1.0;
            A[ix] = 1.0f / sampling_freq;
            d1[ix] = 2.0 * (1 - a2) / sampling_freq;
            d2[ix] = -(a2 - (2.0 * a * r) + 1.0) / sampling_freq;
        }

        // Apply the filter
        for (size_t sx = 0; sx < size; sx++) {
            dest[sx] = src[sx];

            for (int i = 0; i < n_steps; i++) {
                w0[i] = d1[i] * w1[i] + d2[i] * w2[i] + dest[sx];
                dest[sx] = A[i] * (w0[i] - (2.0 * w1[i]) + w2[i]);
                w2[i] = w1[i];
                w1[i] = w0[i];
            }
        }

        ei_dsp_free(A, n_steps*sizeof(float));
        ei_dsp_free(d1, n_steps*sizeof(float));
        ei_dsp_free(d2, n_steps*sizeof(float));
        ei_dsp_free(w0, n_steps*sizeof(float));
        ei_dsp_free(w1, n_steps*sizeof(float));
        ei_dsp_free(w2, n_steps*sizeof(float));
    }

} // namespace filters
} // namespace spectral
} // namespace ei

#endif // _EIDSP_SPECTRAL_FILTERS_H_
