/*****************************************************************************

Perceptual Evaluation of Speech Quality (PESQ)
ITU-T Draft Recommendation P.862.
Version 1.1 - 15 November 2000.

NOTICE

The Perceptual Evaluation of Speech Quality (PESQ) algorithm and the copyright
therein is the property of British Telecommunications plc and Royal KPN NV,
and is protected by UK, US and other patents.  Permission is granted to use
PESQ for the purpose of evaluation of ITU-T recommendation P.862.  Any other
use of this software or the PESQ algorithm requires a license, which may be
obtained from:

OPTICOM GmbH
Michael Keyhl, Am Weichselgarten 7, D- 91058 Erlangen, Germany
Phone: +49 9131 691 160     Fax: +49 9131 691 325     E-mail: info@opticom.de

PsyTechnics Limited
Richard Reynolds, B54 Adastral Park, Ipswich IP5 3RE, UK
Phone: +44 1473 644 730 or +44 7730 426 251    Fax: +44 1473 645 663
E-mail: richard.reynolds@psytechnics.com

Patent-only licences should be obtained from Opticom. PsyTechnics or Opticom
can provide licences, and further information, for other PESQ products.

Further information is also available from: www.pesq.org

By using this software you acknowledge that PESQ is protected by copyright
and by patents and is being made available to you for the purpose of
evaluation of ITU-T Recommendation P.862.

You must not use PESQ for any other purpose without first obtaining a written
license from British Telecommunications plc and Royal KPN NV, from their
agents listed above.

You must not disclose, reproduce or otherwise release PESQ to any third
party without the prior written permission of British Telecommunications plc
and Royal KPN NV.

Authors:
  Antony Rix (BT) <antony.rix@psytechnics.com>
  Mike Hollier (BT)
  Andries Hekstra (KPN Research)
  John Beerends (KPN Research)

*****************************************************************************/

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include "dsp.h"

#ifndef TWOPI
  #define TWOPI   6.283185307179586f
#endif

unsigned long   FFTSwapInitialised = 0;
unsigned long   FFTLog2N;
unsigned long * FFTButter;
unsigned long * FFTBitSwap;
float         * FFTPhi;

long total_malloced = 0;

void *safe_malloc (unsigned long size) {
    void *result;
    total_malloced += size;
    result = malloc (size);
    if (result == NULL) {
        printf ("malloc failed!\n");
    }
    return result;
}

void safe_free (void *p) {
    free (p);
}


unsigned long nextpow2(unsigned long X)
{
    unsigned long C = 1;
    while( (C < ULONG_MAX) && (C < X) )
        C <<= 1;

    return C;
}

int ispow2(unsigned long X)
{
    unsigned long C = 1;
    while( (C < ULONG_MAX) && (C < X) )
        C <<= 1;
        
    return (C == X);
}

int intlog2(unsigned long X)
{
    return (int)floor( log( 1.0 * X ) / log( 2.0 ) + 0.5 );
}

void FFTInit(unsigned long N)
{
    unsigned long   C, L, K;
    float           Theta;
    float         * PFFTPhi;
    
    if( (FFTSwapInitialised != N) && (FFTSwapInitialised != 0) )
        FFTFree();

    if( FFTSwapInitialised == N )
    {
        return;
    }
    else
    {
        C = N;
        for( FFTLog2N = 0; C > 1; C >>= 1 )
            FFTLog2N++;

        C = 1;
        C <<= FFTLog2N;
        if( N == C )
            FFTSwapInitialised = N;

        FFTButter = (unsigned long *) safe_malloc( sizeof(unsigned long) * (N >> 1) );
        FFTBitSwap = (unsigned long *) safe_malloc( sizeof(unsigned long) * N );
        FFTPhi = (float *) safe_malloc( 2 * sizeof(float) * (N >> 1) );
    
        PFFTPhi = FFTPhi;
        for( C = 0; C < (N >> 1); C++ )
        {
            Theta = (TWOPI * C) / N;
            (*(PFFTPhi++)) = (float) cos( Theta );
            (*(PFFTPhi++)) = (float) sin( Theta );
        }
    
        FFTButter[0] = 0;
        L = 1;
        K = N >> 2;
        while( K >= 1 )
        {
            for( C = 0; C < L; C++ )
                FFTButter[C+L] = FFTButter[C] + K;
            L <<= 1;
            K >>= 1;
        }
    }
}

void FFTFree(void)
{
    if( FFTSwapInitialised != 0 )
    {
        safe_free( FFTButter );
        safe_free( FFTBitSwap );
        safe_free( FFTPhi );
        FFTSwapInitialised = 0;
    }
}

void FFT(float * x, unsigned long N)
{
    unsigned long   Cycle, C, S, NC;
    unsigned long   Step    = N >> 1;
    unsigned long   K1, K2;
    register float  R1, I1, R2, I2;
    float           ReFFTPhi, ImFFTPhi;

    if( N > 1 )
    {
        FFTInit( N );
    
        for( Cycle = 1; Cycle < N; Cycle <<= 1, Step >>= 1 )
        {
            K1 = 0;
            K2 = Step << 1;
    
            for( C = 0; C < Cycle; C++ )
            {
                NC = FFTButter[C] << 1;
                ReFFTPhi = FFTPhi[NC];
                ImFFTPhi = FFTPhi[NC+1];
                for( S = 0; S < Step; S++ )
                {
                    R1 = x[K1];
                    I1 = x[K1+1];
                    R2 = x[K2];
                    I2 = x[K2+1];
                    
                    x[K1++] = R1 + ReFFTPhi * R2 + ImFFTPhi * I2;
                    x[K1++] = I1 - ImFFTPhi * R2 + ReFFTPhi * I2;
                    x[K2++] = R1 - ReFFTPhi * R2 - ImFFTPhi * I2;
                    x[K2++] = I1 + ImFFTPhi * R2 - ReFFTPhi * I2;
                }
                K1 = K2;
                K2 = K1 + (Step << 1);
            }
        }
    
        NC = N >> 1;
        for( C = 0; C < NC; C++ )
        {
            FFTBitSwap[C] = FFTButter[C] << 1;
            FFTBitSwap[C+NC] = 1 + FFTBitSwap[C];
        }
        for( C = 0; C < N; C++ )
            if( (S = FFTBitSwap[C]) != C )
            {
                FFTBitSwap[S] = S;
                K1 = C << 1;
                K2 = S << 1;
                R1 = x[K1];
                x[K1++] = x[K2];
                x[K2++] = R1;
                R1 = x[K1];
                x[K1] = x[K2];
                x[K2] = R1;
            }
    }
}

void IFFT(float * x, unsigned long N)
{
    unsigned long   Cycle, C, S, NC;
    unsigned long   Step    = N >> 1;
    unsigned long   K1, K2;
    register float  R1, I1, R2, I2;
    float           ReFFTPhi, ImFFTPhi;

    if( N > 1 )
    {
        FFTInit( N );
    
        for( Cycle = 1; Cycle < N; Cycle <<= 1, Step >>= 1 )
        {
            K1 = 0;
            K2 = Step << 1;
    
            for( C = 0; C < Cycle; C++ )
            {
                NC = FFTButter[C] << 1;
                ReFFTPhi = FFTPhi[NC];
                ImFFTPhi = FFTPhi[NC+1];
                for( S = 0; S < Step; S++ )
                {
                    R1 = x[K1];
                    I1 = x[K1+1];
                    R2 = x[K2];
                    I2 = x[K2+1];
                    
                    x[K1++] = R1 + ReFFTPhi * R2 - ImFFTPhi * I2;
                    x[K1++] = I1 + ImFFTPhi * R2 + ReFFTPhi * I2;
                    x[K2++] = R1 - ReFFTPhi * R2 + ImFFTPhi * I2;
                    x[K2++] = I1 - ImFFTPhi * R2 - ReFFTPhi * I2;
                }
                K1 = K2;
                K2 = K1 + (Step << 1);
            }
        }
    
        NC = N >> 1;
        for( C = 0; C < NC; C++ )
        {
            FFTBitSwap[C] = FFTButter[C] << 1;
            FFTBitSwap[C+NC] = 1 + FFTBitSwap[C];
        }
        for( C = 0; C < N; C++ )
            if( (S = FFTBitSwap[C]) != C )
            {
                FFTBitSwap[S] = S;
                K1 = C << 1;
                K2 = S << 1;
                R1 = x[K1];
                x[K1++] = x[K2];
                x[K2++] = R1;
                R1 = x[K1];
                x[K1] = x[K2];
                x[K2] = R1;
            }
    
        NC = N << 1;
        for( C = 0; C < NC; )
            x[C++] /= N;
    }
}

void RealFFT(float *x, unsigned long N) 
{
    float            *y;
    unsigned long    i;

    y = (float *) safe_malloc (2 * N * sizeof (float));

    for (i = 0; i < N; i++) {
        y [2 * i] = x [i];
        y [2 * i + 1] = 0.0f;
    }

    FFT (y, N);

    for (i = 0; i <= N / 2; i++) {
        x [2 * i] = y [2 * i];
        x [2 * i + 1] = y [2 * i + 1];
    }    
    
    safe_free (y);
}

void RealIFFT(float *x, unsigned long N)
{

    float            *y;
    unsigned long    i;

    y = (float *) safe_malloc (2 * N * sizeof (float));

    for (i = 0; i <= N / 2; i++) {
        y [2 * i] = x [2 * i];
        y [2 * i + 1] = x [2 * i + 1];
    }    
    for (i = N / 2 + 1; i < N; i++) {
        int j = N - i;
        y [2 * i] = x [2 * j];
        y [2 * i + 1] = -x [2 * j + 1];
    }    
    
    IFFT (y, N);
    
    for (i = 0; i < N; i++) {
        x [i] = y [2 * i];
    }

    safe_free (y);
}

unsigned long FFTNXCorr(
  float * x1, unsigned long n1,
  float * x2, unsigned long n2,
  float * y )
{
    register float  r1, i1;
    float         * tmp1;
    float         * tmp2;
    long            C, D, Nx, Ny;

    Nx = nextpow2( max(n1, n2) );
    tmp1 = (float *) safe_malloc(sizeof(float) * (2 * Nx + 2));
    tmp2 = (float *) safe_malloc(sizeof(float) * (2 * Nx + 2));

    for( C = n1 - 1; C >= 0; C-- )
    {
        tmp1[C] = *(x1++);
    }
    for( C = n1; C < 2 * Nx; C++ )
        tmp1[C] = 0.0;

    RealFFT( tmp1, 2*Nx );
    
    for( C = 0; C < (long) n2; C++ )
    {
        tmp2[C] = x2[C];
    }
    for( C = n2; C < 2 * Nx; C++ )
        tmp2[C] = 0.0;
    
    RealFFT( tmp2, 2*Nx );

    for( C = 0; C <= Nx; C++ )
    {
        D = C << 1; r1 = tmp1[D]; i1 = tmp1[1 + D];
        tmp1[D] = r1 * tmp2[D] - i1 * tmp2[1 + D];
        tmp1[1 + D] = r1 * tmp2[1 + D] + i1 * tmp2[D];
    }

    RealIFFT( tmp1, 2*Nx );
    Ny = n1 + n2 - 1;
    for( C = 0; C < Ny; C++ )
        y[C] = tmp1[C];
    
    safe_free( tmp1 );
    safe_free( tmp2 );

    return Ny;
}

void IIRsos(
    float * x, unsigned long Nx,
    float b0, float b1, float b2, float a1, float a2,
    float * tz1, float * tz2 )
{
    register float z0;
    register float z1;
    register float z2;

    if( tz1 == NULL ) z1 = 0.0f; else z1 = *tz1;
    if( tz2 == NULL ) z2 = 0.0f; else z2 = *tz2;
    
    if( (a1 != 0.0f) || (a2 != 0.0f) )
    {
        if( (b1 != 0.0f) || (b2 != 0.0f) )
        {
            while( (Nx) > 0 )
            {
                Nx--;
                                z0 = (*x) - a1 * z1 - a2 * z2;
                *(x++) = b0 * z0 + b1 * z1 + b2 * z2;
                z2 = z1;
                z1 = z0;
            }
        }
        else
        {
            if( b0 != 1.0f )
            {
                while( (Nx) > 0 )
                                {
                                        Nx--;
                    z0 = (*x) - a1 * z1 - a2 * z2;
                    *(x++) = b0 * z0;
                    z2 = z1;
                    z1 = z0;
                }
            }
            else
            {
                while( (Nx) > 0 )
                {
                    Nx--;
                    z0 = (*x) - a1 * z1 - a2 * z2;
                    *(x++) = z0;
                    z2 = z1;
                    z1 = z0;
                }
            }
        }
    }
    else
    {
        if( (b1 != 0.0f) || (b2 != 0.0f) )
        {
            while( (Nx) > 0 )
            {
                Nx--;
                z0 = (*x);
                *(x++) = b0 * z0 + b1 * z1 + b2 * z2;
                z2 = z1;
                z1 = z0;
            }
        }
        else
        {
            if( b0 != 1.0f )
            {
                while( (Nx) > 0 )
                {
                    Nx--;
                    *x = b0 * (*x);
                    x++;
                }
            }
        }
    }

    if( tz1 != NULL ) (*tz1) = z1;
    if( tz2 != NULL ) (*tz2) = z2;
}

void IIRFilt(
    float * h, unsigned long Nsos, float * z,
    float * x, unsigned long Nx, float * y )
{
    unsigned long C;

    if( y == NULL )
        y = x;
    else
    {
        for( C = 0; C < Nx; C++ )
            y[C] = x[C];
    }


    for( C = 0; C < Nsos; C++ )
    {
        if( z != NULL )
        {
            IIRsos( y, Nx, h[0], h[1], h[2], h[3], h[4], z, z+1 );
            z += 2;
        }
        else
            IIRsos( y, Nx, h[0], h[1], h[2], h[3], h[4], NULL, NULL );
        h += 5;
    }
}
/* END OF FILE */