//
//  firmath.c
//  FIR_rect_win
//
//  Created by Jacky Jack on 04/07/2021.
//

#include "firmath.h"

/*
void window_func_rect(double *w, int N1) {
    int j;
    
    for (j=0;j<N1;j++) {
        w[j] = 1.0;
    }
}
*/

void fir_init(fir_t *fir, uint32_t taps) {
    int i;
    for (i=0;i<taps;i++) {
        fir->fir_coef[i] = (double)0.0;
    }
    fir->num_taps = taps;
}


void firw_init(firw_t *fir_win, uint32_t taps) {
    int i;
    for (i=0;i<taps;i++) {
        fir_win->win_coef[i] = (double)0.0;
    }
    fir_win->num_taps = taps;
}


double math_sinx(double x) {
    if (x>-1.0E-5 && x < 1.0E-5) return (1.0);
    return sin(x)/x;
}

void fir_sinc_lp(fir_t *fir, double omega_c) {
    int i;
    double arg=0.0;
    //should set 2fc?!
    printf("omega_c=%f\n",omega_c);
    for (i=0;i<fir->num_taps;i++) {
        arg = (double)i-(double)(fir->num_taps-1)/2.0;
        fir->fir_coef[i] = omega_c * math_sinx(omega_c*arg*M_PI);
    }
}

void fir_sinc_hp(fir_t *fir, double omega_c) {
    int i;
    double arg=0.0;
    int even = fir->num_taps%2 ? 0: 1;
    
    //should set 2fc?!
    printf("omega_c=%f\n",omega_c);
    if (even) {
        for (i=0;i<fir->num_taps;i++) {
            arg = (double)i-(double)(fir->num_taps-1)/2.0;
            if (arg == 0.0) {
                fir->fir_coef[i] = 0.0;
            } else {
                //from IOWA
                fir->fir_coef[i] = cos(omega_c*arg*M_PI)/M_PI/arg + cos(arg*M_PI);
            }
        }
    } else {
        for (i=0;i<fir->num_taps;i++) {
            arg = (double)i-(double)(fir->num_taps-1)/2.0;
            fir->fir_coef[i] = math_sinx(arg*M_PI)-omega_c * math_sinx(omega_c*arg*M_PI);
        }
    }
}

void fir_sinc_bp(fir_t *fir, double omega_c1, double omega_c2) {
    int i;
    double arg=0.0;
    //should set 2fc?!
    printf("omega_c1=%f\n",omega_c1);
    printf("omega_c2=%f\n",omega_c2);
    for (i=0;i<fir->num_taps;i++) {
        arg = (double)i-(double)(fir->num_taps-1)/2.0;
        if (arg==0.0) {
            fir->fir_coef[i] = 0.0;
        } else {
            //fir->fir_coef[i] = math_sinx(arg*M_PI) - omega_c2 * math_sinx(omega_c2*arg*M_PI) - omega_c1 * math_sinx(omega_c1*arg*M_PI);
            fir->fir_coef[i] = (cos(omega_c1*arg*M_PI) - cos(omega_c2*arg*M_PI))/M_PI/arg;
        }
    }
}

void fir_sinc_bs(fir_t *fir, double omega_c1, double omega_c2) {
    int i;
    double arg=0.0;
    //should set 2fc?!
    printf("omega_c1=%f\n",omega_c1);
    printf("omega_c2=%f\n",omega_c2);
    for (i=0;i<fir->num_taps;i++) {
        arg = (double)i-(double)(fir->num_taps-1)/2.0;
        if (arg==0.0) {
            fir->fir_coef[i] = 0.0;
        } else {
            //fir->fir_coef[i] = math_sinx(arg*M_PI) - omega_c2 * math_sinx(omega_c2*arg*M_PI) - omega_c1 * math_sinx(omega_c1*arg*M_PI);
            fir->fir_coef[i] = math_sinx(arg*M_PI)  - omega_c2*math_sinx(omega_c2*arg*M_PI) - omega_c1*math_sinx(omega_c1*arg*M_PI);
        }
    }
}

void fir_convolute(fir_t *fir,firw_t *firw) {
    int i=0;
    for (i=0;i<fir->num_taps;i++) {
        fir->fir_coef[i] = fir->fir_coef[i]*firw->win_coef[i];
    }
}


void firw_rect(firw_t *fir_win) {
    int i;
    int n1 = fir_win->num_taps/2;
    int even = fir_win->num_taps%2 ? 0: 1;
    
    if (even) n1+=1;
    
    for (i=0;i<n1;i++) {
        fir_win->win_coef[i] = 1.0;
    }
    
    for (i=0;i<n1;i++) {
        fir_win->win_coef[fir_win->num_taps-1-i] =  fir_win->win_coef[i];
    }
    
}


void firw_hamming(firw_t *fir_win) {
    int i;
    double dN=0;
    double arg=0;
    //double p=0;
    //double arg=0;
    
    int n1 = fir_win->num_taps/2;
    int even = fir_win->num_taps%2 ? 0: 1;
    if (!even) n1+=1;
    dN = fir_win->num_taps +1;
    
    for (i=0;i<n1;i++) {
        //arg = (double)i-(double)(fir_win->num_taps-1)/2.0;
        //fir_win->win_coef[i] =  0.54+(1-0.54)*cos(2*M_PI*arg/n1);
        
        //fir_win->win_coef[i] =  0.54+(1-0.54)*cos(2*M_PI*i/n1);
        arg = (double)i-(double)(fir_win->num_taps-1)/2.0;
        fir_win->win_coef[i] = 0.54+0.46*cos(2*M_PI*arg/fir_win->num_taps);
        
    }
    //fold
    for (i=0;i<n1;i++) {
        //arg = (double)i-(double)(fir_win->num_taps-1)/2.0;
        //fir_win->win_coef[i] =  0.54+(1-0.54)*cos(2*M_PI*arg/n1);
        
        fir_win->win_coef[fir_win->num_taps-1-i] =  fir_win->win_coef[i];
    }
    
}


void firw_hanning(firw_t *fir_win) {
    int i;
    double dN=0;
    double arg=0;
    
    int n1 = fir_win->num_taps/2;
    int even = fir_win->num_taps%2 ? 0: 1;
    if (!even) n1+=1;
    dN = fir_win->num_taps +1;
    
    for (i=0;i<n1;i++) {
        arg = (double)i-(double)(fir_win->num_taps-1)/2.0;
        fir_win->win_coef[i] = 0.5+(1-0.5)*cos(2*M_PI*arg/fir_win->num_taps);
        
    }
    //fold
    for (i=0;i<n1;i++) {
        fir_win->win_coef[fir_win->num_taps-1-i] =  fir_win->win_coef[i];
    }
}

void firw_blackman(firw_t *firw) {
    int i;
    double dN=0;
    double arg=0;
    
    int n1 = firw->num_taps/2;
    int even = firw->num_taps%2 ? 0: 1;
    if (!even) n1+=1;
    dN = firw->num_taps +1;
    
    for (i=0;i<n1;i++) {
        arg = (double)i-(double)(firw->num_taps-1)/2.0;
        /*
        firw->win_coef[i] = 0.42
        +0.50*cos(2*M_PI*arg/(firw->num_taps))
        +0.08*cos(4*M_PI*arg/(firw->num_taps));
        */
        firw->win_coef[i] = 0.42
        +0.50*cos(2*M_PI*arg/(firw->num_taps-1))
        +0.08*cos(4*M_PI*arg/(firw->num_taps-1));
        
    }
    //fold
    for (i=0;i<n1;i++) {
        firw->win_coef[firw->num_taps-1-i] =  firw->win_coef[i];
    }
}

void fir_print_matlab(fir_t *fir) {
    int i;
    printf("f=[\n");
    for (i=0;i<fir->num_taps;i++) {
        printf("%f\n",fir->fir_coef[i]);
    }
    printf("];\n");
}

void firw_print_matlab(firw_t *firw) {
    int i;
    printf("w=[\n");
    for (i=0;i<firw->num_taps;i++) {
        printf("%f\n",firw->win_coef[i]);
    }
    printf("];\n");
}


void fir_print_c(fir_t *fir) {
    int i;
    printf("double f=[\n");
    for (i=0;i<fir->num_taps;i++) {
        printf("%f,\n",fir->fir_coef[i]);
    }
    printf("];\n");
}