#include "fir_filter.h"

namespace {

double sinc(const double x)
{
    if(x == 0.0) {
        return 1.0;
    }
    return std::sin(M_PI * x) / (M_PI * x);
}

} // namespace

FIR_filter::FIR_filter(int taps, double f1, double f2, char *type, char *window)
    : h(taps, 0.0), samples(taps, 0.0)
{
    this->idx = 0;
    this->taps = taps;

    std::vector<double> coeffs;
    std::vector<double> weights;
    const char *safeType = type != nullptr ? type : const_cast<char *>("");
    const char *safeWindow = window != nullptr ? window : const_cast<char *>("");

    if(std::strcmp(safeType, "lp") == 0) {
        coeffs = lowPass_coefficient(taps, f1);
    } else if(std::strcmp(safeType, "hp") == 0) {
        coeffs = highPass_coefficient(taps, f1);
    } else if(std::strcmp(safeType, "bp") == 0) {
        coeffs = bandPass_coefficient(taps, f1, f2);
    } else if(std::strcmp(safeType, "sb") == 0) {
        coeffs = bandStop_coefficient(taps, f1, f2);
    }

    if(std::strcmp(safeWindow, "hamming") == 0) {
        weights = window_hammig(taps);
    } else if(std::strcmp(safeWindow, "triangle") == 0) {
        weights = window_triangle(taps);
    } else if(std::strcmp(safeWindow, "hanning") == 0) {
        weights = window_hanning(taps);
    } else if(std::strcmp(safeWindow, "blackman") == 0) {
        weights = window_blackman(taps);
    }

    if(std::strcmp(safeWindow, "") == 0) {
        this->h = coeffs;
    } else {
        for(int n = 0; n < taps; n++) {
            this->h[n] = coeffs[n] * weights[n];
        }
    }
}

FIR_filter::~FIR_filter() = default;

std::vector<double> FIR_filter::getCoefficients()
{
    return this->h;
}

std::vector<double> FIR_filter::lowPass_coefficient(int taps, double f)
{
    std::vector<int> n(taps, 0);
    std::vector<double> coeffs(taps, 0.0);

    for(int i = 0; i < taps; i++) {
        n[i] = i - taps / 2;
    }
    for(int i = 0; i < taps; i++) {
        coeffs[i] = 2.0 * f * sinc(2.0 * f * n[i]);
    }

    return coeffs;
}

std::vector<double> FIR_filter::highPass_coefficient(int taps, double f)
{
    std::vector<int> n(taps, 0);
    std::vector<double> coeffs(taps, 0.0);

    for(int i = 0; i < taps; i++) {
        n[i] = i - taps / 2;
    }
    for(int i = 0; i < taps; i++) {
        coeffs[i] = sinc(n[i]) - 2.0 * f * sinc(2.0 * f * n[i]);
    }

    return coeffs;
}

std::vector<double> FIR_filter::bandPass_coefficient(int taps, double f1, double f2)
{
    std::vector<int> n(taps, 0);
    std::vector<double> coeffs(taps, 0.0);

    for(int i = 0; i < taps; i++) {
        n[i] = i - taps / 2;
    }
    for(int i = 0; i < taps; i++) {
        coeffs[i] = 2.0 * f1 * sinc(2.0 * f1 * n[i]) - 2.0 * f2 * sinc(2.0 * f2 * n[i]);
    }

    return coeffs;
}

std::vector<double> FIR_filter::bandStop_coefficient(int taps, double f1, double f2)
{
    std::vector<int> n(taps, 0);
    std::vector<double> coeffs(taps, 0.0);

    for(int i = 0; i < taps; i++) {
        n[i] = i - taps / 2;
    }
    for(int i = 0; i < taps; i++) {
        coeffs[i] = 2.0 * f1 * sinc(2.0 * f1 * n[i]) - 2.0 * f2 * sinc(2.0 * f2 * n[i])
                    + sinc(n[i]);
    }

    return coeffs;
}

std::vector<double> FIR_filter::window_hammig(int taps)
{
    std::vector<double> weights(taps, 0.0);
    constexpr double alpha = 0.54;
    constexpr double beta = 0.46;

    for(int i = 0; i < taps; i++) {
        weights[i] = alpha - beta * std::cos(2.0 * M_PI * i / (taps - 1));
    }

    return weights;
}

std::vector<double> FIR_filter::window_hanning(int taps)
{
    std::vector<double> weights(taps, 0.0);

    for(int i = 0; i < taps; i++) {
        const double angle = static_cast<double>(M_PI) * i / (taps - 1);
        weights[i] = std::sin(angle) * std::sin(angle);
    }

    return weights;
}

std::vector<double> FIR_filter::window_triangle(int taps)
{
    std::vector<double> weights(taps, 0.0);
    const double length = static_cast<double>(taps);

    for(int i = 0; i < taps; i++) {
        weights[i] = 1.0 - std::abs((i - ((taps - 1) / 2.0)) / (length / 2.0));
    }

    return weights;
}

std::vector<double> FIR_filter::window_blackman(int taps)
{
    std::vector<double> weights(taps, 0.0);
    constexpr double alpha0 = 0.42;
    constexpr double alpha1 = 0.5;
    constexpr double alpha2 = 0.08;

    for(int i = 0; i < taps; i++) {
        weights[i] = alpha0 - alpha1 * std::cos(2.0 * M_PI * i / (taps - 1))
                     - alpha2 * std::cos(4.0 * M_PI * i / (taps - 1));
    }

    return weights;
}

double FIR_filter::filter(double new_sample)
{
    double result = 0.0;

    this->samples[this->idx] = new_sample;
    for(int n = 0; n < this->taps; n++) {
        result += this->samples[(this->idx + n) % this->taps] * this->h[n];
    }
    this->idx = (this->idx + 1) % this->taps;

    return result;
}
