DSPRelated.com
Online · Live + on demand
February 9-11, 2027
Price goes up Nov 1: register now to lock in today's rate
Register Now →
   Code Snippets

Goertzel Filterbank to the Implementation of a Nonuniform DFT

Miguel De Jesus Rosario ● December 14, 2010●1 comment ● Coded in Matlab 
% VectorGoertzel    Goertzel's Algorithm filter bank.
%   Realization of the Goertzel's Algorithm to compute the Nonuniform DFT 
%   of a signal(a column vector named signalw) of length Nw with sampling 
%   frecuency fs at the desired frecuencies contained in vector f. The 
%   function returns the NDFT magnitude in a vector with the same length of f.

function xk=Gfilterbank(signalw,f,fs,Nw)

% Inititialization of the different variables 
n=2;
signalw=signalw(1:Nw);

cost=cos(2*pi*f/fs);
sint=sin(2*pi*f/fs);
L=length(cost);

y1(1:L)=0;
y2(1:L)=0;
signalw=[0 0 signalw]; %Signal is delayed by two samples

% Goertzel Feedback Algorithm

while((n-2) < Nw)
    n=n+1;
    xnew(1:L)=signalw(n);
    y=xnew+2*cost.*y1-y2;
    y2=y1;
    y1=y;
end

% Goertzel Forward Algorithm

rey=y1-y2.*cost;
imy=y2.*sint;

% Magnitude Calculation

xk=abs(rey+imy*j)';

Discrete Fourier Transform

Miguel De Jesus Rosario ● December 14, 2010●3 comments ● Coded in C++ 
#include<iostream>
#include<iomanip>
#include<cmath>
using namespace std;

#define pi 3.14159265

// class complex
class complex
{
public:
	//This class member function set the values of the class members real and imag
	void setvals(double r, double i) 
	{
		real=r;
		imag=i;
	}

	//This function get the values (by reference) of the class members real and imag
	void getvals(double &r, double &i) 
	{
		r=real;
		i=imag;
	}

private: 
	/* The class members real and imag represents the real and imaginary parts 
       of a complex number, respectively.*/
	double real;
	double imag;

}; // end of class complex

//**********************************************************************************************
/* function dft receives two parameters x and N. x is the input sequence containing 
the discrete-time samples and N is the number of DFT points.  The complex type paramater
xk receives a sequence of N-points frequency samples (this is the DFT of the input sequence*/

void dft(double x[], int N, complex xk[])
{
	double r,i;
	
	for(int k=0;k<N;k++) //k is the index of the N-point frequency sequence
	{
		xk[k].setvals(0,0); //initialize the values of the counters to zero  
		
		for(int n=0;n<N;n++) //n is the index of the discrete-time sequence
		{
			xk[k].getvals(r,i); // get the values of real and imag

			/*This is the computation of the real and imaginary parts of the DFT.
			The class member function setvals is used to update the value of the accumulators
			contaning the real and imaginary parts of the DFT samples*/

			xk[k].setvals(r + x[n]*cos(2*pi*k/N*n),i + x[n]*sin(2*pi*k/N*n));	
		} //end inner for-loop

	} //end outer for-loop

}//end dft function

//************************************************************************************************
//Declaration of the main() function
int main()
{
	int size; // The number of DFT samples 
	double r,i; // r = real part of the DFT; i = imaginary part of the DFT 

cout<<"Enter the desired number of DFT samples: ";
cin>>size;

double *x = new double[size];  //dynamic array x represent the discrete-time samples
complex *xk = new complex[size]; // dynamic array xk represent the DFT samples

cout<<"\nEnter the sequence x[n]: ";
for(int h=0;h<size;h++)
{
	cin>>x[h];
}

//Computation of the DFT of x
dft(x, size, xk);

system("CLS"); //clear screen

cout<<"Discrete Fourier Transform: "<<endl<<endl;

for(int h=0;h<size;h++)
{
	cout.precision(3); // display three (3) decimal places
	xk[h].getvals(r,i); //get the DFT samples values
	cout<<"xk["<<h<<"] = "<<fixed<< r <<" + "<<fixed<< i <<"j"<<endl;  // print the DFT samples
}

cout<<endl;

return 0;
}// end main function

Discrete Wavelet Transform - Chain processing (Linear Convolution)

David Valencia ● December 5, 2010●1 comment ● Coded in Matlab 
% ----------------------------------------------------
% Title: Discrete Wavelet Transform
% Author: David Valencia
% UPIITA IPN 2010
%
% Posted in DSPrelated.com
% http://www.dsprelated.com/showcode/47.php
%
% Computes the Discrete Wavelet Transform of
% n levels (and its branches) from a n sample input.
% Base filters can be of any lenght.
%
% Generalized DWT filter bank computed via the
% chain processing method (linear convolution)
%
% Dependencies: 
%   formfilters.m - http://www.dsprelated.com/showcode/44.php
%   formfiltersdwt.m - http://www.dsprelated.com/showcode/45.php
%   upsample2.m - http://www.dsprelated.com/showcode/10.php
%   recorreder.m - http://www.dsprelated.com/showcode/43.php
%   getsincdwt.m - http://www.dsprelated.com/showcode/46.php
%
% Revisions:
%       v1.0a Commented and translated in English
% ----------------------------------------------------

close all; clear all; clc;

% ######################################

disp('== CHAIN PROCESSING DWT ==')

% Filter parameters
typeofbasis = 'o';
typbior = 'bior2.2';
typor = 'db2';

if(typeofbasis == 'b')
    [Rf,Df] = biorwavf(typbior);
    [h0,h1,g0,g1] = biorfilt(Df,Rf);
elseif (typeofbasis == 'o')
    [h0,h1,g0,g1] = wfilters(typor);
end;

% Input values, change as needed
N = 16;
x = (1:N);
L = length(h0);

figure(1);
subplot(2,1,1);
stem(x);
xlabel('Original signal');

% Filter bank parameters, change as needed
n_stages = 3; %Of the low-pass part
% Checked for the minimal case: DWT = DWPT n_stages = 1
n_branches = n_stages + 1; %Of the low-pass part
niter = 300; %Number of iterations

% Define input buffer dimensions
hx = formfilters(n_stages, 1, h0, h1);
lhx = length(hx);

% Input buffer vector
inputbuffer = zeros(lhx,1);

% Synthesis input buffer
sbuffer = zeros(n_branches,lhx);

% This vector will store the length of each branch filter
Lfiltros = zeros(n_branches, 1);

% This vector will store the decimate factor for each branch
dec_factors = zeros(n_branches, 1);

% This vector will store the synchronization factor for each branch
sinc_factors = zeros(n_branches, 1);

% Cycle to get the synchronization factors
for j = 0:n_branches-1
    try
        sinc_factors(j+1) = getsincdwt(n_stages, j+1, L);
    catch error
        disp('ERROR: Error, experimentation is needed for this case');
        disp('The results may not be correct');
        % return;
    end
end

%% Filter matrices formation
% This matrix will store each of the branch filters as vectors
H = zeros(n_branches, lhx); %Malloc
G = zeros(n_branches, lhx); %Malloc

for i = 0:n_stages
    
    if i >= n_stages-1
        dec_factors(i+1) = 2^n_stages;
    else
        dec_factors(i+1) = 2^(i+1);
    end
    
    hx = fliplr(formfiltersdwt(n_stages,n_stages+1-i,h0,h1));
    gx = fliplr(formfiltersdwt(n_stages,n_stages+1-i,g0,g1));
    lhx = length(hx);
    lgx = length(gx);
    Lfiltros(i+1) = lhx;
    H(i+1,1:lhx) = hx;
    G(i+1,1:lhx) = gx;
end

% Debug code
disp('Dimensiones de los filtros')
Lfiltros
disp('Factores de diezmado')
dec_factors
disp('Matriz de filtros análisis: ');
H
disp('Matriz de filtros síntesis: ');
G

chanbufftestigo = zeros(n_branches,1);
outputbuffer = zeros(1,niter);

%% MAIN CYCLE
for i = 1:niter %General FOR (for iterations)
    i % Print iteration number
    % Shifting of input buffer (sequentially)
    
    inputbuffer = recorreder(inputbuffer,1);
    if i<=N
        inputbuffer(1)=x(i);
    else
        inputbuffer(1)=0;
    end
    
    inputbuffer %Print buffer (debug)
    
    %% Analyisis stage (h0 and h1)
    % The following cycle will select each of the branches for processing
    % If the iteration counter is a multiply of the decimation factor,
    % the convolution is calculated, otherwise, zeros are sent to the
    % channel
    
    y = zeros(n_branches,1); %Clear the output buffer
    
    for j = 0:n_branches-1
        fprintf('\nBranch: %d, Decimating factor: %d\n',j+1,dec_factors(j+1));
        fprintf('\nFilter length: %d\n',Lfiltros(j+1));
        if mod(i,dec_factors(j+1)) == 1
            fprintf('i = %d is a multiply of the factor: %d\n',i,dec_factors(j+1));
            tempfilt = H(j+1,1:Lfiltros(j+1));
            % If the current iteration (clock cycle) is a multiply of the
            % factor, the convolution is computed
            for k=1:Lfiltros(j+1) %convolution
                y(j+1,1) = y(j+1,1) + inputbuffer(k)*tempfilt(k);
            end;
        end
        disp('Results in the channel');
        chanbuff(:,1) = y %Debug printing
    end
    
    %% Synthesis stage (g0 and g1)
    
    % Shifting and interpolation of the synthesis buffer
    for j = 1:n_branches
        sbuffer(j,:) = recorreder(sbuffer(j,:),1);
        % Interpolation of the synthesis stage inputs
        if mod(i,dec_factors(j)) == 1
            
            fprintf('Inserting sample to the synthesis branch %d with dec_factor %d\n',j,dec_factors(j));
            sbuffer(j,1) = chanbuff(j,1);
        else
            fprintf('Inserting a zero to the synthesis branch %d with dec_factor %d\n',j,dec_factors(j));
            sbuffer(j,1) = 0;
        end
    end
    
    disp('Synthesis buffer matrix');
    sbuffer
    
    xnbuff = zeros(n_branches,1);
    
    for j=0:n_branches-1
        fprintf('Branch: %d , dec_factor = %d',j+1,dec_factors(j+1));
        
        % === BRANCH SYNCHRONIZATION ===
        % The branches (except the two last ones down the bank filter)
        % will only take a part of the vector that corresponds with their
        % channel, taking 'Lx' elements, being Lx the number of coefficient
        % that their filter has. An offset is applied
        
        if j < n_branches - 2
            [m,n] = size(sbuffer);
            tempsinth = sbuffer(j+1,n-Lfiltros(j+1)+1:n) %TESTING
        else % The lowest branches take the whole vector
            tempsinth = sbuffer(j+1,1+sinc_factors(j+1):Lfiltros(j+1)+sinc_factors(j+1))
        end
        
        for k = 1 : Lfiltros(j+1) %Convolution
            xnbuff(j+1,1) = xnbuff(j+1,1) + tempsinth(k)*G(j+1,k);
        end
    end
    
    xnbuff
    outputbuffer(i) = sum(xnbuff);
    disp('Output buffer: ');
    if i > N
        disp(outputbuffer(1,i-N+1:i))
        figure(1);
        subplot(2,1,2);
        stem(outputbuffer(1,i-N+1:i));        
        xlabel('Recovered signal');
    else
        disp(outputbuffer(1,1:N))
        figure(1);
        subplot(2,1,2);
        stem(outputbuffer(1,1:N));
        xlabel('Recovered signal');        
    end
    pause; 
end

% END OF PROGRAM >>>

getsincdwt.m - Get DWT synchronization for chain-processing

David Valencia ● December 5, 2010 ● Coded in Matlab 
% UPIITA IPN 2010
% Valencia Pesqueira José David

% Function used to obtain the synchronization factor for each
% branch in the DWT chain-processing program

function [x] = getsincdwt(n_etapas, rama, Lfiltrobase)

switch(n_etapas)
    case 1 %One stage, two-branches. DWPT equivalent
        x = 0;
        
    case 2  %2 stages, 3 branches
        switch(rama)
            case 1 % High-pass branch
                x = (Lfiltrobase-1)*2;
            otherwise
                x = 0;
        end
        
    case 3 %3 branches, 4 stages
        switch(rama)
            case 1 % High-pass branch
                x = (Lfiltrobase-1)*2;
            case 2
                x = 10;
            otherwise
                x = 0;
        end
end

% Experimental ANALISIS
% 2 etapas
% lfiltro - sinc_factor experimental
% 
% 4 - 6       (4-1)*2 = 6
% 6 - 10	    (6-1)*2 = 10
% 8 - 14	    (8-1)*2 = 7

formfiltersdwt.m DWT filter generator adaptor

David Valencia ● December 5, 2010 ● Coded in Matlab 
% By David Valencia
% As posted in DSPRelated.com
% at http://www.dsprelated.com/showcode/45.php
% Used for the DWT chain processing program

function [hx] = formfiltersdwt(n_stages,branch,h0,h1)
if(branch==1)
    hx=formfilters(n_stages,branch,h0,h1);
elseif(branch==2)
    hx=formfilters(n_stages,2,h0,h1);
else
    hx=formfilters(n_stages-branch+2,2,h0,h1);
end

% The code line that is changed in formfilters.m for DWT is:
%p = mod(2^(n_stages-1-i),p);

DWT filter generator - formfilters.m (for chain processing)

David Valencia ● December 5, 2010 ● Coded in Matlab 
% Created by José David Valencia Pesqueira
% UPIITA-IPN 2010
% For the DWT chain-processing example
% as posted in DSPrelated.com
% http://www.dsprelated.com/showcode/44.php
%
function [hx] = formfilters(n_stages,branch,h0,h1)
p = branch;

% Seed vector
hx = 0;
hx(1) = 1;

switch n_stages
    case 1
        if mod(branch,2) ~= 0
            hx = h0;
        else
            hx = h1;
        end
    case 2
        switch branch
            case 1
                hx = conv(h0,upsample2(h0,2));
            case 2
                hx = conv(h0,upsample2(h1,2));
            case 3
                hx = conv(h1,upsample2(h0,2));
            case 4
                hx = conv(h1,upsample2(h1,2));
            otherwise
                beep;
                fprintf('\nFor a 2-stage bank there cannot be a fifth branch');
        end
        
    otherwise
        
        for i=0:n_stages-2
            q = floor(p /(2^(n_stages-1-i)));
            if (q == 1)
                hx = conv(hx,upsample2(h1,2^i));
            else
                hx = conv(hx,upsample2(h0,2^i));
            end
            % p = mod(p,2^(n_stages-1-i)); %For DWPT
            p = mod(2^(n_stages-1-i),p); %For DWT
        end
        
        t = mod(branch,2);
        if(t == 1)
            hx = conv(hx,upsample2(h0,2^(n_stages-1)));
        else
            hx = conv(hx,upsample2(h1,2^(n_stages-1)));
        end
             
end

recorreder.m - Vector shifting made simple

David Valencia ● December 5, 2010 ● Coded in Matlab 
function [xd] = recorreder(x,N)
% This function shifts a vector by N positions to the right
% As posted in DSPrelated.com
% http://www.dsprelated.com/showcode/43.php
%
lx = length(x);
if N>lx
    fprintf('N has to be smaller than the vector length');
    return;
else
    xd(N+1:lx) = x(1:lx-N);
end;

Fixed Point FIR Filter

Shawn Stevenson ● December 3, 2010●1 comment ● Coded in C 
#include <stdio.h>
#include <stdint.h>

//////////////////////////////////////////////////////////////
//  Filter Code Definitions
//////////////////////////////////////////////////////////////

// maximum number of inputs that can be handled
// in one function call
#define MAX_INPUT_LEN   80
// maximum length of filter than can be handled
#define MAX_FLT_LEN     63
// buffer to hold all of the input samples
#define BUFFER_LEN      (MAX_FLT_LEN - 1 + MAX_INPUT_LEN)

// array to hold input samples
static int16_t insamp[ BUFFER_LEN ];

// FIR init
void firFixedInit( void )
{
    memset( insamp, 0, sizeof( insamp ) );
}

// store new input samples
int16_t *firStoreNewSamples( int16_t *inp, int length )
{
    // put the new samples at the high end of the buffer
    memcpy( &insamp[MAX_FLT_LEN - 1], inp,
            length * sizeof(int16_t) );
    // return the location at which to apply the filtering
    return &insamp[MAX_FLT_LEN - 1];
}

// move processed samples
void firMoveProcSamples( int length )
{
    // shift input samples back in time for next time
    memmove( &insamp[0], &insamp[length],
            (MAX_FLT_LEN - 1) * sizeof(int16_t) );
}

// the FIR filter function
void firFixed( int16_t *coeffs, int16_t *input, int16_t *output,
       int length, int filterLength )
{
    int32_t acc;     // accumulator for MACs
    int16_t *coeffp; // pointer to coefficients
    int16_t *inputp; // pointer to input samples
    int n;
    int k;

    // apply the filter to each input sample
    for ( n = 0; n < length; n++ ) {
        // calculate output n
        coeffp = coeffs;
        inputp = &input[n];
        // load rounding constant
        acc = 1 << 14;
        // perform the multiply-accumulate
        for ( k = 0; k < filterLength; k++ ) {
            acc += (int32_t)(*coeffp++) * (int32_t)(*inputp--);
        }
        // saturate the result
        if ( acc > 0x3fffffff ) {
            acc = 0x3fffffff;
        } else if ( acc < -0x40000000 ) {
            acc = -0x40000000;
        }
        // convert from Q30 to Q15
        output[n] = (int16_t)(acc >> 15);
    }
}

//////////////////////////////////////////////////////////////
//  Test program
//////////////////////////////////////////////////////////////

// bandpass filter centred around 1000 Hz
// sampling rate = 8000 Hz
// gain at 1000 Hz is about 1.13

#define FILTER_LEN  63
int16_t coeffs[ FILTER_LEN ] =
{
 -1468, 1058,   594,   287,    186,  284,   485,   613,
   495,   90,  -435,  -762,   -615,   21,   821,  1269,
   982,    9, -1132, -1721,  -1296,    1,  1445,  2136,
  1570,    0, -1666, -2413,  -1735,   -2,  1770,  2512,
  1770,   -2, -1735, -2413,  -1666,    0,  1570,  2136,
  1445,    1, -1296, -1721,  -1132,    9,   982,  1269,
   821,   21,  -615,  -762,   -435,   90,   495,   613,
   485,  284,   186,   287,    594, 1058, -1468
};

// Moving average (lowpass) filter of length 8
// There is a null in the spectrum at 1000 Hz

#define FILTER_LEN_MA   8
int16_t coeffsMa[ FILTER_LEN_MA ] =
{
    32768/8, 32768/8, 32768/8, 32768/8,
    32768/8, 32768/8, 32768/8, 32768/8
};

// number of samples to read per loop
#define SAMPLES   80

int main( void )
{
    int size;
    int16_t input[SAMPLES];
    int16_t output[SAMPLES];
    int16_t *inp;
    FILE   *in_fid;
    FILE   *out_fid;
    FILE   *out_fid2;

    // open the input waveform file
    in_fid = fopen( "input.pcm", "rb" );
    if ( in_fid == 0 ) {
        printf("couldn't open input.pcm");
        return;
    }

    // open the output waveform files
    out_fid = fopen( "outputFixed.pcm", "wb" );
    if ( out_fid == 0 ) {
        printf("couldn't open outputFixed.pcm");
        return;
    }
    out_fid2 = fopen( "outputFixedMa.pcm", "wb" );
    if ( out_fid == 0 ) {
        printf("couldn't open outputFixedMa.pcm");
        return;
    }

    // initialize the filter
    firFixedInit();

    // process all of the samples
    do {
        // read samples from file
        size = fread( input, sizeof(int16_t), SAMPLES, in_fid );
        // store new samples in working array
        inp = firStoreNewSamples( input, size );

        // apply each filter
        firFixed( coeffs, inp, output, size, FILTER_LEN );
        fwrite( output, sizeof(int16_t), size, out_fid );

        firFixed( coeffsMa, inp, output, size, FILTER_LEN_MA );
        fwrite( output, sizeof(int16_t), size, out_fid2 );

        // move processed samples
        firMoveProcSamples( size );
    } while ( size != 0 );

    fclose( in_fid );
    fclose( out_fid );
    fclose( out_fid2 );

    return 0;
}

Echo Filter

Miguel De Jesus Rosario ● December 1, 2010●3 comments ● Coded in Matlab 
% function echo_filter:
%     1) computes and returns the impulse response h of the LTI system (echo filter)
%     2) Filter the input signal.  The output signal is yecho. 
%
% INPUTS:
% signal = input signal
% times = vector containing the time delays (in seconds) of the repetitions(echoes)
% attenuations = vector containing the respective attenuations of the echoes
% fs = sampling frequency of the input signal.
%
% OUTPUTS:
% yecho = output signal (passed through the echo filter)
% h = impulse response of the echo filter.

function [yecho, h] = echo_filter(signal, times, attenuations, fs)

h(1)=1; % This is the first coefficient of the echo filter

% Computing the impulse response h
for i=1:length(times),
    samples(i) = times(i)*fs; % Calculating the sample-times (N = t*fs)
    h(floor(samples(i))) = attenuations(i); % Impulse response coeficients
end

% #########################################################################
% You may use the following implementation instead of the illustrated
% above:

%       samples = times*fs;
%       h(floor(samples)) = attenuations;

% In this case, the implementation is done without a for-loop.
% #########################################################################

yecho = filter(h,1,signal(:,1)); % filtering of the input signal results in a signal with echoes

% You may test the filter using the Matlab signal "splat" (write load splat
% in the Matlab command window).  Use function sound() in order to
% listening both, the input and the output, signals.

Fixed-point Math Library

Jordan ● November 30, 2010 ● Coded in C 
/* **********************************************************************
 *
 * Fixed Point Math Library
 *
 * **********************************************************************
 *
 * qmath.h
 *
 * Alex Forencich
 * alex@alexelectronics.com
 *
 * Jordan Rhee
 * rhee.jordan@gmail.com
 *
 * IEEE UCSD
 * http://ieee.ucsd.edu
 *
 * **********************************************************************/

#ifndef _QMATH_H_
#define _QMATH_H_

#ifdef __cplusplus
extern "C" {
#endif

#ifndef INLINE
#ifdef _MSC_VER
#define INLINE __inline
#else
#define INLINE inline
#endif /* _MSC_VER */
#endif /* INLINE */

/*
 * Default fractional bits. This precision is used in the routines
 * and macros without a leading underscore. 
 * For example, if you are mostly working with values that come from
 * a 10-bit A/D converter, you may want to choose 21. This leaves 11
 * bits for the whole part, which will help avoid overflow in addition.
 * On ARM, bit shifts require a single cycle, so all fracbits 
 * require the same amount of time to compute and there is no advantage
 * to selecting fracbits that are a multiple of 8.
 */
#define	FIXED_FRACBITS 24

#define FIXED_RESOLUTION (1 << FIXED_FRACBITS)
#define FIXED_INT_MASK (0xffffffffL << FIXED_FRACBITS)
#define FIXED_FRAC_MASK (~FIXED_INT_MASK)

// square roots
#define FIXED_SQRT_ERR 1 << (FIXED_FRACBITS - 10)

// fixedp2a
#define FIXED_DECIMALDIGITS 6

typedef long fixedp;

// conversions for arbitrary fracbits
#define _short2q(x, fb)			((fixedp)((x) << (fb)))
#define _int2q(x, fb)			((fixedp)((x) << (fb)))
#define _long2q(x, fb)			((fixedp)((x) << (fb)))
#define _float2q(x, fb)			((fixedp)((x) * (1 << (fb))))
#define _double2q(x, fb)		((fixedp)((x) * (1 << (fb))))

// conversions for default fracbits
#define short2q(x)			_short2q(x, FIXED_FRACBITS)
#define int2q(x)			_int2q(x, FIXED_FRACBITS)
#define long2q(x)			_long2q(x, FIXED_FRACBITS)
#define float2q(x)			_float2q(x, FIXED_FRACBITS)
#define double2q(x)			_double2q(x, FIXED_FRACBITS)

// conversions for arbitrary fracbits
#define _q2short(x, fb)		((short)((x) >> (fb)))
#define _q2int(x, fb)		((int)((x) >> (fb)))
#define _q2long(x, fb)		((long)((x) >> (fb)))
#define _q2float(x, fb)		((float)(x) / (1 << (fb)))
#define _q2double(x, fb)	((double)(x) / (1 << (fb)))

// conversions for default fracbits
#define q2short(x)			_q2short(x, FIXED_FRACBITS)
#define q2int(x)			_q2int(x, FIXED_FRACBITS)
#define q2long(x)			_q2long(x, FIXED_FRACBITS)
#define q2float(x)			_q2float(x, FIXED_FRACBITS)
#define q2double(x)			_q2double(x, FIXED_FRACBITS)

// evaluates to the whole (integer) part of x
#define qipart(x)			q2long(x)

// evaluates to the fractional part of x
#define qfpart(x)			((x) & FIXED_FRAC_MASK)

/*
 * Constants
 */
#define _QPI      3.1415926535897932384626433832795
#define QPI      double2q(_QPI)
#define _Q2PI     6.283185307179586476925286766559
#define Q2PI     double2q(_Q2PI)
#define _QPIO2    1.5707963267948966192313216916398
#define QPIO2    double2q(_QPIO2)
#define _QPIO4    0.78539816339744830961566084581988
#define QPIO4    double2q(_QPIO4)
#define _QLN_E    2.71828182845904523536
#define QLN_E    double2q(_QLN_E)
#define _QLN_10   2.30258509299404568402
#define QLN_10   double2q(_QLN_10)
#define _Q1OLN_10 0.43429448190325182765
#define Q1OLN_10 double2q(_Q1OLN_10)

// Both operands in addition and subtraction must have the same fracbits.
// If you need to add or subtract fixed point numbers with different
// fracbits, then use q2q to convert each operand beforehand.
#define qadd(a, b) ((a) + (b))
#define qsub(a, b) ((a) - (b))

/**
 * q2q - convert one fixed point type to another
 * x - the fixed point number to convert
 * xFb - source fracbits
 * yFb - destination fracbits
 */
static INLINE fixedp q2q(fixedp x, int xFb, int yFb)
{
	if(xFb == yFb) {
		return x;
	} else if(xFb < yFb) {
		return x << (yFb - xFb);
	} else {
		return x >> (xFb - yFb);
	}
}

/**
 * Multiply two fixed point numbers with arbitrary fracbits
 * x - left operand
 * y - right operand
 * xFb - number of fracbits for X
 * yFb - number of fracbits for Y
 * resFb - number of fracbits for the result
 * 
 */
#define _qmul(x, y, xFb, yFb, resFb) ((fixedp)(((long long)(x) * (long long)(y)) >> ((xFb) + (yFb) - (resFb))))

/**
 * Fixed point multiply for default fracbits.
 */
#define qmul(x, y) _qmul(x, y, FIXED_FRACBITS, FIXED_FRACBITS, FIXED_FRACBITS)

/**
 * divide
 * shift into 64 bits and divide, then truncate
 */
#define _qdiv(x, y, xFb, yFb, resFb) ((fixedp)((((long long)x) << ((xFb) + (yFb) - (resFb))) / y))

/**
 * Fixed point divide for default fracbbits.
 */
#define qdiv(x, y) _qdiv(x, y, FIXED_FRACBITS, FIXED_FRACBITS, FIXED_FRACBITS)

/**
 * Invert a number (x^-1) for arbitrary fracbits
 */
#define _qinv(x, xFb, resFb) ((fixedp)((((long long)1) << (xFb + resFb)) / x))

/**
 * Invert a number with default fracbits.
 */
#define qinv(x) _qinv(x, FIXED_FRACBITS, FIXED_FRACBITS);

/**
 * Modulus. Modulus is only defined for two numbers of the same fracbits
 */
#define qmod(x, y) ((x) % (y))

/**
 * Absolute value. Works for any fracbits.
 */
#define qabs(x) (((x) < 0) ? (-x) : (x))

/**
 * Floor for arbitrary fracbits
 */
#define _qfloor(x, fb) ((x) & (0xffffffff << (fb)))

/**
 * Floor for default fracbits
 */
#define qfloor(x) _qfloor(x, FIXED_FRACBITS)

/**
 * ceil for arbitrary fracbits.
 */
static INLINE fixedp _qceil(fixedp x, int fb)
{
	// masks off fraction bits and adds one if there were some fractional bits
	fixedp f = _qfloor(x, fb);
	if (f != x) return qadd(f, _int2q(1, fb));
	return x;
}

/**
 * ceil for default fracbits
 */
#define qceil(x) _qceil(x, FIXED_FRACBITS)

/**
 * square root for default fracbits
 */
fixedp qsqrt(fixedp p_Square);

/**
 * log (base e) for default fracbits
 */
fixedp qlog( fixedp p_Base );

/**
 * log base 10 for default fracbits
 */
fixedp qlog10( fixedp p_Base );

/**
 * exp (e to the x) for default fracbits
 */
fixedp qexp(fixedp p_Base);

/**
 * pow for default fracbits
 */
fixedp qpow( fixedp p_Base, fixedp p_Power );

/**
 * sine for default fracbits
 */
fixedp qsin(fixedp theta);

/**
 * cosine for default fracbits
 */
fixedp qcos(fixedp theta);

/**
 * tangent for default fracbits
 */
fixedp qtan(fixedp theta);

/**
 * fixedp2a - converts a fixed point number with default fracbits to a string
 */
char *q2a(char *buf, fixedp n);

#ifdef __cplusplus
}	// extern C
#endif

#endif /* _QMATH_H_ */

/* **********************************************************************
 *
 * Fixed Point Math Library
 *
 * **********************************************************************
 *
 * qmath.c
 *
 * Alex Forencich
 * alex@alexelectronics.com
 *
 * Jordan Rhee
 * rhee.jordan@gmail.com
 *
 * IEEE UCSD
 * http://ieee.ucsd.edu
 *
 * **********************************************************************/

#include "qmath.h"

/**
 * square root
 */
fixedp qsqrt(fixedp p_Square)
{
	fixedp   res;
	fixedp   delta;
	fixedp   maxError;

	if ( p_Square <= 0 )
		return 0;

	/* start at half */
	res = (p_Square >> 1);

	/* determine allowable error */
	maxError =  qmul(p_Square, FIXED_SQRT_ERR);

	do
	{
		delta =  (qmul( res, res ) - p_Square);
		res -=  qdiv(delta, ( res << 1 ));
	}
	while ( delta > maxError || delta < -maxError );

	return res;
}

/**
 * log (base e)
 */
fixedp qlog( fixedp p_Base )
{
    fixedp w = 0;
	fixedp y = 0;
	fixedp z = 0;
	fixedp num = int2q(1);
	fixedp dec = 0;

	if ( p_Base == int2q(1) )
		return 0;

	if ( p_Base == 0 )
		return 0xffffffff;

	for ( dec=0 ; qabs( p_Base ) >= int2q(2) ; dec += int2q(1) )
		p_Base = qdiv(p_Base, QLN_E);

	p_Base -= int2q(1);
	z = p_Base;
	y = p_Base;
	w = int2q(1);

	while ( y != y + w )
	{
		z = 0 - qmul( z , p_Base );
		num += int2q(1);
		w = qdiv( z , num );
		y += w;
	}

	return y + dec;
}

/**
 * log base 10
 */
fixedp qlog10( fixedp p_Base )
{
    // ln(p_Base)/ln(10)
    // more accurately, ln(p_Base) * (1/ln(10))
    return qmul(qlog(p_Base), Q1OLN_10);
}

/**
 * exp (e to the x)
 */
fixedp qexp(fixedp p_Base)
{
	fixedp w;
	fixedp y;
	fixedp num;

	for ( w=int2q(1), y=int2q(1), num=int2q(1) ; y != y+w ; num += int2q(1) )
	{
		w = qmul(w, qdiv(p_Base, num));
		y += w;
	}

	return y;
}

/**
 * pow
 */
fixedp qpow( fixedp p_Base, fixedp p_Power )
{
	if ( p_Base < 0 && qmod(p_Power, int2q(2)) != 0 )
		return - qexp( qmul(p_Power, qlog( -p_Base )) );
	else
		return qexp( qmul(p_Power, qlog(qabs( p_Base ))) );
}

/**
 * sinx, internal sine function
 */
static fixedp sinx(fixedp x)
{
	fixedp xpwr;
	long xftl;
	fixedp xresult;
	short positive;
	int i;

	xresult = 0;
	xpwr = x;
	xftl = 1;
	positive = -1;

	// Note: 12! largest for long
	for (i = 1; i < 7; i+=2)
	{
		if ( positive )
			xresult += qdiv(xpwr, long2q(xftl));
		else
			xresult -= qdiv(xpwr, long2q(xftl));

		xpwr = qmul(x, xpwr);
		xpwr = qmul(x, xpwr);
		xftl *= i+1;
		xftl *= i+2;
		positive = !positive;
	}

	return xresult;
}

/**
 * sine
 */
fixedp qsin(fixedp theta)
{
	fixedp xresult;
	short bBottom = 0;
	//static fixed xPI = XPI;
	//static fixed x2PI = X2PI;
	//static fixed xPIO2 = XPIO2;

	fixedp x = qmod(theta, Q2PI);
	if ( x < 0 )
		x += Q2PI;

	if ( x > QPI )
	{
		bBottom = -1;
		x -= QPI;
	}

	if ( x <= QPIO2 )
		xresult = sinx(x);
	else
		xresult = sinx(QPIO2-(x-QPIO2));

	if ( bBottom )
		return -xresult;

	return xresult;
}

/**
 * cosine
 */
fixedp qcos(fixedp theta)
{
	return qsin(theta + QPIO2);
}

/**
 * tangent
 */
fixedp qtan(fixedp theta)
{
	return qdiv(qsin(theta), qcos(theta));
}

/**
 * q2a - converts a fixed point number to a string
 */
char *q2a(char *buf, fixedp n)
{
	long ipart = qipart(n);
	long fpart = qfpart(n);
	long intdigits = 0;

	int i = 0;
	int j = 0;
	int d = 0;

	int offset = 0;

	long v;
	long t;
	long st = 0;

	if (n != 0)
	{
		intdigits = qipart(qceil(qlog10(qabs(n))));
	}

	if (intdigits < 1) intdigits = 1;

	offset = intdigits - 1;

	if (n < 0)
	{
		buf[0] = '-';
		offset++;
		n = -n;
		ipart = -ipart;
		fpart = -fpart;
	}

	// integer portion

	for (i = 0; i < intdigits; i++)
	{
		j = offset - i;
		d = ipart % 10;
		ipart = ipart / 10;
		buf[j] = '0' + d;
	}

	// decimal point

	buf[offset + 1] = '.';

	// fractional portion

	v = 5;
	t = FIXED_RESOLUTION >> 1;

	for (i = 0; i < FIXED_DECIMALDIGITS - 1; i++)
	{
		v = (v << 1) + (v << 3);
	}

	for (i = 0; i < FIXED_FRACBITS; i++)
	{
		if (fpart & t)
		{
			st += v;
		}
		v = v >> 1;
		t = t >> 1;
	}

	offset += FIXED_DECIMALDIGITS + 1;

	for (i = 0; i < FIXED_DECIMALDIGITS; i++)
	{
		j = offset - i;
		d = st % 10;
		st = st / 10;
		buf[j] = '0' + d;
	}

	// ending zero
	buf[offset + 1] = '\0';

	return buf;
}