CDSPBlockConvolver.h

Go to the documentation of this file.

//$ nobt
//$ nocpp

#ifndef R8B_CDSPBLOCKCONVOLVER_INCLUDED
#define R8B_CDSPBLOCKCONVOLVER_INCLUDED

#include "CDSPFIRFilter.h"
#include "CDSPProcessor.h"

namespace r8b {

class CDSPBlockConvolver : public CDSPProcessor
{
public:
    CDSPBlockConvolver( CDSPFIRFilter& aFilter, const int aUpFactor,
        const int aDownFactor, const double PrevLatency = 0.0,
        const bool aDoConsumeLatency = true )
        : Filter( &aFilter )
        , UpFactor( aUpFactor )
        , DownFactor( aDownFactor )
        , DoConsumeLatency( aDoConsumeLatency )
        , BlockLen2( 2 << Filter -> getBlockLenBits() )
    {
        R8BASSERT( UpFactor > 0 );
        R8BASSERT( DownFactor > 0 );
        R8BASSERT( PrevLatency >= 0.0 );

        int fftinBits;
        UpShift = getBitOccupancy( UpFactor ) - 1;

        if(( 1 << UpShift ) == UpFactor )
        {
            fftinBits = Filter -> getBlockLenBits() + 1 - UpShift;
            PrevInputLen = ( Filter -> getKernelLen() - 1 + UpFactor - 1 ) /
                UpFactor;

            InputLen = BlockLen2 - PrevInputLen * UpFactor;
        }
        else
        {
            UpShift = -1;
            fftinBits = Filter -> getBlockLenBits() + 1;
            PrevInputLen = Filter -> getKernelLen() - 1;
            InputLen = BlockLen2 - PrevInputLen;
        }

        OutOffset = ( Filter -> isZeroPhase() ? Filter -> getLatency() : 0 );
        LatencyFrac = Filter -> getLatencyFrac() + PrevLatency * UpFactor;
        Latency = (int) LatencyFrac;
        const int InLatency = Latency + Filter -> getLatency() - OutOffset;
        LatencyFrac -= Latency;
        LatencyFrac /= DownFactor;

        Latency += InputLen + Filter -> getLatency();

        int fftoutBits;
        InputDelay = 0;
        UpSkipInit = 0;
        DownSkipInit = 0;
        DownShift = getBitOccupancy( DownFactor ) - 1;

        if(( 1 << DownShift ) == DownFactor )
        {
            fftoutBits = Filter -> getBlockLenBits() + 1 - DownShift;

            if( DownFactor > 1 )
            {
                if( UpShift > 0 )
                {
                    // This case never happens in practice due to mutual
                    // exclusion of "power of 2" DownFactor and UpFactor
                    // values.

                    R8BASSERT( UpShift == 0 );
                }
                else
                {
                    // Make sure InputLen is divisible by DownFactor.

                    const int ilc = InputLen & ( DownFactor - 1 );
                    PrevInputLen += ilc;
                    InputLen -= ilc;
                    Latency -= ilc;

                    // Correct InputDelay for input and filter's latency.

                    const int lc = InLatency & ( DownFactor - 1 );

                    if( lc > 0 )
                    {
                        InputDelay = DownFactor - lc;
                    }

                    if( !DoConsumeLatency )
                    {
                        Latency /= DownFactor;
                    }
                }
            }
        }
        else
        {
            fftoutBits = Filter -> getBlockLenBits() + 1;
            DownShift = -1;

            if( !DoConsumeLatency && DownFactor > 1 )
            {
                DownSkipInit = Latency % DownFactor;
                Latency /= DownFactor;
            }
        }

        fftin = new CDSPRealFFTKeeper( fftinBits );

        if( fftoutBits == fftinBits )
        {
            fftout = fftin;
        }
        else
        {
            ffto2 = new CDSPRealFFTKeeper( fftoutBits );
            fftout = ffto2;
        }

        WorkBlocks.alloc( BlockLen2 * 2 + PrevInputLen );
        CurInput = &WorkBlocks[ 0 ];
        CurOutput = &WorkBlocks[ BlockLen2 ]; // CurInput and
            // CurOutput are memory-aligned.
        PrevInput = &WorkBlocks[ BlockLen2 * 2 ];

        clear();

        R8BCONSOLE( "CDSPBlockConvolver: flt_len=%i in_len=%i io=%i/%i "
            "fft=%i/%i latency=%i\n", Filter -> getKernelLen(), InputLen,
            UpFactor, DownFactor, (*fftin) -> getLen(), (*fftout) -> getLen(),
            getLatency() );
    }

    virtual ~CDSPBlockConvolver()
    {
        Filter -> unref();
    }

    virtual int getLatency() const
    {
        return( DoConsumeLatency ? 0 : Latency );
    }

    virtual double getLatencyFrac() const
    {
        return( LatencyFrac );
    }

    virtual int getMaxOutLen( const int MaxInLen ) const
    {
        R8BASSERT( MaxInLen >= 0 );

        return(( MaxInLen * UpFactor + DownFactor - 1 ) / DownFactor );
    }

    virtual void clear()
    {
        memset( &PrevInput[ 0 ], 0, PrevInputLen * sizeof( double ));

        if( DoConsumeLatency )
        {
            LatencyLeft = Latency;
        }
        else
        {
            LatencyLeft = 0;

            if( DownShift > 0 )
            {
                memset( &CurOutput[ 0 ], 0, ( BlockLen2 >> DownShift ) *
                    sizeof( double ));
            }
            else
            {
                memset( &CurOutput[ BlockLen2 - OutOffset ], 0, OutOffset *
                    sizeof( double ));

                memset( &CurOutput[ 0 ], 0, ( InputLen - OutOffset ) *
                    sizeof( double ));
            }
        }

        memset( CurInput, 0, InputDelay * sizeof( double ));

        InDataLeft = InputLen - InputDelay;
        UpSkip = UpSkipInit;
        DownSkip = DownSkipInit;
    }

    virtual int process( double* ip, int l0, double*& op0 )
    {
        R8BASSERT( l0 >= 0 );
        R8BASSERT( UpFactor / DownFactor <= 1 || ip != op0 || l0 == 0 );

        double* op = op0;
        int l = l0 * UpFactor;
        l0 = 0;

        while( l > 0 )
        {
            const int Offs = InputLen - InDataLeft;

            if( l < InDataLeft )
            {
                InDataLeft -= l;

                if( UpShift >= 0 )
                {
                    memcpy( &CurInput[ Offs >> UpShift ], ip,
                        ( l >> UpShift ) * sizeof( double ));
                }
                else
                {
                    copyUpsample( ip, &CurInput[ Offs ], l );
                }

                copyToOutput( Offs - OutOffset, op, l, l0 );
                break;
            }

            const int b = InDataLeft;
            l -= b;
            InDataLeft = InputLen;
            int ilu;

            if( UpShift >= 0 )
            {
                const int bu = b >> UpShift;
                memcpy( &CurInput[ Offs >> UpShift ], ip,
                    bu * sizeof( double ));

                ip += bu;
                ilu = InputLen >> UpShift;
            }
            else
            {
                copyUpsample( ip, &CurInput[ Offs ], b );
                ilu = InputLen;
            }

            const int pil = (int) ( PrevInputLen * sizeof( double ));
            memcpy( &CurInput[ ilu ], PrevInput, pil );
            memcpy( PrevInput, &CurInput[ ilu - PrevInputLen ], pil );

            (*fftin) -> forward( CurInput );

            if( UpShift > 0 )
            {
                #if R8B_FLOATFFT
                    mirrorInputSpectrum( (float*) CurInput );
                #else // R8B_FLOATFFT
                    mirrorInputSpectrum( CurInput );
                #endif // R8B_FLOATFFT
            }

            if( Filter -> isZeroPhase() )
            {
                (*fftout) -> multiplyBlocksZ( Filter -> getKernelBlock(),
                    CurInput );
            }
            else
            {
                (*fftout) -> multiplyBlocks( Filter -> getKernelBlock(),
                    CurInput );
            }

            if( DownShift > 0 )
            {
                const int z = BlockLen2 >> DownShift;

                #if R8B_FLOATFFT
                    float* const kb = (float*) Filter -> getKernelBlock();
                    float* const p = (float*) CurInput;
                #else // R8B_FLOATFFT
                    const double* const kb = Filter -> getKernelBlock();
                    double* const p = CurInput;
                #endif // R8B_FLOATFFT

                p[ 1 ] = kb[ z ] * p[ z ] - kb[ z + 1 ] * p[ z + 1 ];
            }

            (*fftout) -> inverse( CurInput );

            copyToOutput( Offs - OutOffset, op, b, l0 );

            double* const tmp = CurInput;
            CurInput = CurOutput;
            CurOutput = tmp;
        }

        return( l0 );
    }

private:
    CDSPFIRFilter* Filter; 
    CPtrKeeper< CDSPRealFFTKeeper* > fftin; 
    CPtrKeeper< CDSPRealFFTKeeper* > ffto2; 
    CDSPRealFFTKeeper* fftout; 
    int UpFactor; 
    int DownFactor; 
    bool DoConsumeLatency; 
    int BlockLen2; 
    int OutOffset; 
    int PrevInputLen; 
    int InputLen; 
    int Latency; 
    double LatencyFrac; 
    int UpShift; 
    int DownShift; 
    int InputDelay; 
    CFixedBuffer< double > WorkBlocks; 
    double* PrevInput; 
    double* CurInput; 
    double* CurOutput; 
    int InDataLeft; 
    int LatencyLeft; 
    int UpSkip; 
    int UpSkipInit; 
    int DownSkip; 
    int DownSkipInit; 

    void copyUpsample( double*& ip0, double* op, int l0 )
    {
        int b = min( UpSkip, l0 );

        if( b > 0 )
        {
            l0 -= b;
            UpSkip -= b;
            *op = 0.0;
            op++;
            b--;

            while( b > 0 )
            {
                *op = 0.0;
                op++;
                b--;
            }
        }

        double* ip = ip0;
        int l = l0 / UpFactor;
        int lz = l0 - l * UpFactor;
        const int upf = UpFactor;

        if( upf == 3 )
        {
            while( l > 0 )
            {
                op[ 0 ] = *ip;
                op[ 1 ] = 0.0;
                op[ 2 ] = 0.0;
                ip++;
                op += upf;
                l--;
            }
        }
        else
        if( upf == 5 )
        {
            while( l > 0 )
            {
                op[ 0 ] = *ip;
                op[ 1 ] = 0.0;
                op[ 2 ] = 0.0;
                op[ 3 ] = 0.0;
                op[ 4 ] = 0.0;
                ip++;
                op += upf;
                l--;
            }
        }
        else
        {
            while( l > 0 )
            {
                op[ 0 ] = *ip;
                int j;

                for( j = 1; j < upf; j++ )
                {
                    op[ j ] = 0.0;
                }

                ip++;
                op += upf;
                l--;
            }
        }

        if( lz > 0 )
        {
            *op = *ip;
            op++;
            ip++;
            UpSkip = UpFactor - lz;

            while( lz > 1 )
            {
                *op = 0.0;
                op++;
                lz--;
            }
        }

        ip0 = ip;
    }

    void copyToOutput( int Offs, double*& op0, int b, int& l0 )
    {
        if( Offs < 0 )
        {
            if( Offs + b <= 0 )
            {
                Offs += BlockLen2;
            }
            else
            {
                copyToOutput( Offs + BlockLen2, op0, -Offs, l0 );
                b += Offs;
                Offs = 0;
            }
        }

        if( LatencyLeft > 0 )
        {
            if( LatencyLeft >= b )
            {
                LatencyLeft -= b;
                return;
            }

            Offs += LatencyLeft;
            b -= LatencyLeft;
            LatencyLeft = 0;
        }

        const int df = DownFactor;

        if( DownShift > 0 )
        {
            int Skip = Offs & ( df - 1 );

            if( Skip > 0 )
            {
                Skip = df - Skip;
                b -= Skip;
                Offs += Skip;
            }

            if( b > 0 )
            {
                b = ( b + df - 1 ) >> DownShift;
                memcpy( op0, &CurOutput[ Offs >> DownShift ],
                    b * sizeof( double ));

                op0 += b;
                l0 += b;
            }
        }
        else
        {
            if( df > 1 )
            {
                const double* ip = &CurOutput[ Offs + DownSkip ];
                int l = ( b + df - 1 - DownSkip ) / df;
                DownSkip += l * df - b;

                double* op = op0;
                l0 += l;
                op0 += l;

                while( l > 0 )
                {
                    *op = *ip;
                    op++;
                    ip += df;
                    l--;
                }
            }
            else
            {
                memcpy( op0, &CurOutput[ Offs ], b * sizeof( double ));
                op0 += b;
                l0 += b;
            }
        }
    }

    template< class T >
    void mirrorInputSpectrum( T* const p )
    {
        const int bl1 = BlockLen2 >> UpShift;
        const int bl2 = bl1 + bl1;
        int i;

        for( i = bl1 + 2; i < bl2; i += 2 )
        {
            p[ i ] = p[ bl2 - i ];
            p[ i + 1 ] = -p[ bl2 - i + 1 ];
        }

        p[ bl1 ] = p[ 1 ];
        p[ bl1 + 1 ] = 0.0;
        p[ 1 ] = p[ 0 ];

        for( i = 1; i < UpShift; i++ )
        {
            const int z = bl1 << i;
            memcpy( &p[ z ], p, z * sizeof( T ));
            p[ z + 1 ] = 0.0;
        }
    }
};

} // namespace r8b

#endif // R8B_CDSPBLOCKCONVOLVER_INCLUDED