/*************************************************
 *                   bwf.cpp
 *
 *************************************************/

#include <stdlib.h>
#include <stdio.h>
#include <string.h>    
#include <math.h>
#include "bwf.h"

using namespace std;

void BWF_n1::create(double tau, double fc, BWFilterType hp){
    /* Binding */
    tau_ = tau;
    fc_ = fc;
    b0_ = 2.0 * PI * fc_ * tau_;
    a1_ = exp(-b0_);
    y_ = 0.0;
    hp_ = hp;
}


double BWF_n1::pass(double x){
    y_ = a1_ * y_ + b0_ * x;
        
    if (hp_ == HP)
	return	(x-y_);
    else
	return	y_;
}


void BWF_n1::reset(){
    y_ = 0.0;
}


//thetaは次元によって変わる
void BWF_n2::create(double tau, double fc, double theta, BWFilterType hp){
    /* Binding */
    tau_ = tau;
    fc_ = fc;
    theta_ = theta;
    
    ss_ = 2.0 * PI * fc_ * tau_ * sin(theta_);
    cs_ = 2.0 * PI * fc_ * tau_ * cos(theta_);
    a1_ = 2.0 * exp(-ss_) * cos(cs_);
    a2_ = - exp(-2.0 * ss_);
    b1_ = (2.0 * PI * fc_ * tau_ * exp(-ss_) * sin(cs_)) / cos(theta_);
    
    x1_ = 0.0;
    y1_ = 0.0;
    y2_ = 0.0;
    hp_ = hp;
}

double BWF_n2::pass(double x){
    double tmp_y;
    tmp_y = y1_;
    y1_ = b1_*x1_ + a1_*y1_ + a2_*y2_;
    //y1_ = b1_*x + a1_*y1_ + a2_*y2_;
    y2_ = tmp_y;
    
    x1_ = x;
    
    if(hp_ == HP)
        return	(x - y1_);
    else 
        return	y1_;
}


void BWF_n2::reset(){
    x1_ = 0.0;
    y1_ = 0.0;
    y2_ = 0.0;
}


BWF::BWF(double tau, double fc, int dim, BWFilterType hp){
    this->create(tau, fc, dim, hp);
}


BWF::~BWF(){
    if(n1_==1)
        delete n1_filter_;
    if(n2_>0)
        delete[] n2_filters_;
}

void BWF::create(double tau, double fc, int dim, BWFilterType hp){
    double theta;
    
    /* Binding */
    tau_ = tau;
    fc_ = fc;
    dim_ = dim;
    n1_ = dim_%2;
    n2_ = dim_/2;
    fnum_ = n1_ + n2_;
    hp_ = hp;
    offset_ = 0.0;

    if(n1_ == 1){
        n1_filter_ = new BWF_n1;
        n1_filter_->create(tau,fc,hp);
    }

    if(n2_ > 0){
        n2_filters_ = new BWF_n2[n2_];
        for (int i=0; i<n2_; i++) {
            theta = (PI / (2.0 * dim_)) + (PI / dim_) * i;
            n2_filters_[i].create(tau_, fc_, theta, hp_);
        }}
}

double BWF::pass(double x){
    x = x - offset_;
    if(n1_==1)
        x = n1_filter_->pass(x);
    if(n2_>0){
        for(int i=0; i<n2_; i++)
            x = n2_filters_[i].pass(x);
    }
    //return (x + offset_);
    return x;
}

void BWF::reset(double offset){
    offset_ = offset;
    if(n1_==1)
        n1_filter_->reset();
    if(n2_>0){
        for(int i=0; i<n2_; i++)
            n2_filters_[i].reset();
    }
}


