function h = CIC(w, R, N, D)
    //w -> Normalized frequncy point (1/Fs)
    //R -> CIC Filter Ratio
    //N -> CIC Filter Cascaded Sections
    //D -> Decimation Ratio
    
    //Symmetric response handling, around 0.5Fs
    if(w >= 0.5) then
        w = 1 - w;
    end
    
    w = w / D;
    h = abs(1/R*(sin(2*%pi*w*R/2) / sin(2*%pi*w/2)))^N;         //CIC Response
endfunction

function h = InverseCIC(w, R, N, D)
    //w -> Normalized frequncy point (1/Fs)
    //R -> CIC Filter Ratio
    //N -> CIC Filter Cascaded Sections
    //D -> Decimation Ratio
    
    //Symmetric response handling, around 0.5Fs
    if(w >= 0.5) then
        w = 1 - w;
    end
    
    w = w / D;
    if(w ~= 0) then
        h = abs(R*(sin(2*%pi*w*1/2) / sin(2*%pi*w*R/2)))^N;      //Inverse CIC Response
        //h = abs(M*(sin(2*%pi*1/(2*M)) / sin(2*%pi*w*1/2)));  //CIC Response
    else
        h = 1;
    end
    
    //Limit inverse response, can grow to infinite...
    if(h > 100) then
        h = 100;
    end
endfunction


function h = LowPassWindow(w, f0, B)
    //w -> Normalized frequncy point (1/Fs)
    //f0 -> Cut-off frequency
    //B -> Transition bandwidth
    
    //Symmetric response handling, around 0.5Fs
    if(w >= 0.5) then
        w = 1 - w;
    end
    
    if(w <= f0) then
        h = 1;
    elseif(w <= (f0 + B)) then
        h = (1 - ((w - f0) / B));
    else
        h = 0;
    end
endfunction

function [c,w,hk] = FrequencySampling(h, n, t, s)
    //h -> Desired frequency response function
    //n -> Number of coefficents to generate
    //t -> Sampling Type: either 1 or 2
    //s -> Symmetry: either 1 for symmetric or 2 for antisymmetric
    
    //Based on: https://webusers.i3s.unice.fr/~deneire/filt_cours_4.pdf
    
    //Check if even or odd number of coeficients
    ft = 1;
    even = %t;
    if(modulo(n, 2) == 0) then
        even = %t;
        if(s == 1) then
            ft = 2;
        else
            ft = 4;
        end
    else
        even = %f;
        if(s == 1) then
            ft = 1;
        else
            ft = 3;
        end
    end
    
    //Sample symmetric points
    for k = 0:(n-1)
        if(t == 1) then
            //Type 1 sampling
            wk(k+1) = k/n;
        else
            //Type 2 sampling
            wk(k+1) = (k + 0.5)/n;
        end
        p(k+1) = -2*%pi*wk(k+1);
        
        w(k+1) = exp(%i*p(k+1));
        hk(k+1) = h(wk(k+1));
    end
    
    //Calculate coeficients: Type I and II (symmetric coeficients)
    if(ft == 1 || ft == 2) then
        for i = 0:(n-1)
            if(even == %t) then
                ep = ((n/2) - 1);
            else
                ep = ((n-1) / 2);
            end
            sumHk = 0;
            for k = 1:ep
                //sumHk = sumHk + hk(k+1)*exp(%i*p(k+1))*exp(%i*2*%pi*k/N*i);
                co = cos(%pi*k*(1+2*i) / n);
                //co = cos(p(k+1) + 2*%pi*k*i/n);
                sumHk = sumHk + ((-1)^k * hk(k+1) * co);
            end
            c(i+1) = 1/n * (2 * sumHk + hk(1));
        end
    elseif(ft == 3) then
        //Calculate coeficients: Type III
        for i = 0:(n-1)
            sumHk = 0;
            for k = 1:((n-1) / 2)
                si = sin((%pi*k*(1 + 2*i)) / n);
                sumHk = sumHk + ((-1)^(k + 1) * hk(k+1) * si);            
            end
            c(i+1) = 2/n * sumHk;
        end
    elseif(ft == 4) then
        //Calculate coeficients: Type IV
        for i = 0:(n-1)
            sumHk = 0;
            for k = 1:((n/2) - 1)
                si = sin((%pi*k*(1 + 2*i)) / n);
                sumHk = sumHk + ((-1)^k * hk(k+1) * si);            
            end
            c(i+1) = 1/n * ((-1)^(n/2 + i) * hk(n/2) + 2*sumHk);
        end
    end
endfunction

function [c, smp, rms] = CICCompFilter(m, d, fc, bp, n)
    //Arguments:
    //m -> CIC Filter Ratio
    //d -> Decimation Ratio
    //fc -> Low Pass cut-off frequency
    //n -> Number of coefficients/taps
    //Returns:
    //c -> FIR filter coefficients
    //smp -> FIR frequency sample points
    //rms -> FIR filter accuracy as RMS    
    
    plotData = %t;
    safeCSV = %f;
    
    function h = CombinedFilter(w)
        h = InverseCIC(w, m, 1, d) * LowPassWindow(w, fc, bp);
    endfunction
    
    fr = linspace(0, 0.5, 512)';
    for i = 1:length(fr)
        xmCIC(i) = CIC(fr(i), m, 1, d);
        xmInvCIC(i) = InverseCIC(fr(i), m, 1, d);
        xmLP(i) = LowPassWindow(fr(i), fc, bp);
        xmComb(i) = CombinedFilter(fr(i));
    end
    
    //Mirror to get otherside
    xmCIC = [xmCIC; flipdim(xmCIC,1)];
    xmInvCIC = [xmInvCIC; flipdim(xmInvCIC,1)];
    xmLP = [xmLP; flipdim(xmLP,1)];
    xmComb = [xmComb; flipdim(xmComb,1)];
    
    //Actual frequncy sampling to design the compensation FIR filter
    [c, smp, hks] = FrequencySampling(CombinedFilter, n, 1, 1);
    
    //Calculate sample position
    smp_x = atan(imag(smp), real(smp)) / (2*%pi);
    for i = 1:length(smp_x)
        if(smp_x(i) < 0) then
            smp_x(i) = 1 + smp_x(i);
        end
    end
    
   //Scale frequency from 0->0.5 to 0->1
    fr = linspace(0, 1, 1024)';
    //Scale obtained FIR response from 0->0.5 to 0->1
    [xm, fr2] = frmag(c, 512);
    xm = [xm flipdim(xm,2)];
    //Calculate global response (CIC´+ Compensation)
    xmNew = xm .* xmCIC';
    
    //Calculate filter error in RMS
    sumRMS = 0;
    //for i = (length(fr) * 0.05):(length(fr) * 0.3)
    for i = 1:(length(fr) * 0.5)
        sumRMS = sumRMS + (xmComb(i) - xm(i))^2;
    end
    rms = sqrt(sumRMS / (length(fr) / 2));
    //disp(rms);
    
    //Plot graphs in Window
    if(plotData == %t) then
        //Plot graphs in Window
        scf(0);
        clf(0);
        
        //Plot CIC filter response
        
        p = plot('nn', fr, xmCIC);
        p.foreground = color(128,0,255);
        //Plot inverse CIC filter response
        p = plot('nn', fr, xmInvCIC, ":");
        p.foreground = color(128,0,255);
        //Plot LPF window
        p = plot('nn', fr, xmLP, ":");
        p.foreground = color(255,255,255);
        //Plot ideal combined frequency response
        p = plot('nn', fr, xmComb, "g");
        p.foreground = color(0,255,0);
        
        //Plot actual designed filter response
        p = plot('nn', fr, xm');
        p.foreground = color(255,0,0);
        
        //Draw sample points on top
        p = plot('nn', smp_x, hks, 'x');
        p.mark_foreground = color(255,0,0);
        
        //Draw Global Response, CIC with compensation
        p = plot('nn', fr, xmNew');
        p.foreground = color(0,0,255);
        
        //Format plot3
        title("CIC Compensation Filter", 'fontname', "helvetica bold", 'fontsize', 6);
        a = get("current_axes");
        a.data_bounds = [0;0.5;0;1.5];
        a.x_ticks = tlist(["ticks" "locations", "labels"], [0.00; 0.05; 0.10; 0.15; 0.20; 0.25; 0.30; 0.35; 0.40; 0.45; 0.50], ['0.00'; '0.05'; '0.10'; '0.15'; '0.20'; '0.25'; '0.30'; '0.35'; '0.40'; '0.45'; '0.50']);
        a.y_ticks = tlist(["ticks" "locations", "labels"], [1.20; 1.00; 0.8; 0.60; 0.40; 0.20; 0.00], ['1.20'; '1.00'; '0.80'; '0.60'; '0.40'; '0.20'; '0.00']);
        a.sub_ticks = [0, 0];
        a.font_size = 4;
        a.font_style = 8;
        a.tight_limits = "on";
        xlabel("Frequency (1/Fs)", 'fontname', "helvetica bold", 'fontsize', 4);
        ylabel("Gain (x1)", 'fontname', "helvetica bold", 'fontsize', 4);
        xgrid();
        
        l = legend(['CIC';'Inverse CIC';'LPF Window';'Desired Response';'Compensation Filter';'FIR Samples';'Global Response'], 'in_lower_left');
        l.line_width = 0.05;
        l.background = color(0,0,0);
        l.foreground = color(255,255,255);
        l.font_color = color(255,255,255);
        l.font_size = 3;
        l.font_style = 8;
        
        //Syling
        hf = gcf();
        ha = gca();
        hf.background = color(0,0,0);           //Outer Area
        ha.background = color(0,0,0);           //Inner Area
        ha.foreground = color(255,255,255);     //Axes Rectangle
        ha.thickness = 4;                       //Line thickness
        ha.font_color = color(255,255,255);     //Ticks
        ha.grid = [color(255,255,255), color(255,255,255)];    //Grid
        ha.title.font_foreground = color(255,255,255);         //Title
        ha.x_label.font_foreground = color(255,255,255);       //x-Label
        ha.y_label.font_foreground = color(255,255,255);       //y-Label
        ha.children.children.thickness = 4;     //Line thickness
        
        xgrid(color(255,255,255), 1, 8);
    end
    
    //Export/save data
    if(safeCSV == %t) then
        fileName = "FIR_BPF_Frequency_Sampling_Coefficient_N=" + string(N) + ".csv";
        disp(fileName);
        csvWrite(c, fileName, ",", ".");
        fileName = "FIR_BPF_Frequency_Sampling_Point_N=" + string(N) + ".csv";
        disp(fileName);
        csvWrite(cat(2, x, hks), fileName, ",", ".");
        fileName = "FIR_BPF_Frequency_Sampling_Response_N=" + string(N) + ".csv";
        disp(fileName);
        csvWrite(cat(2, fr, xm', xmNew'), fileName, ",", ".");
    end
endfunction
