spectrum.cpp

#include <common.hpp>
#include <vector>
#include <cmath>
#include <cstdio>

struct spectrumdraw_interface_t
{
    int32_t const* samples;
    unsigned int samplecount;
    unsigned int samplescale;
    unsigned int channelcount;
    unsigned int samplerate;
    unsigned int bitmapwidth;
    float freqbase;
    unsigned int freqdivision; // number of frequencies per octave
    unsigned int freqcount;
    uint8_t* bitmap;
};

namespace
{
namespace spectrumdraw_private
{
    float const pi = 3.14159265359f;
    int const batchsize = 1;
    // 1 // 300
    // 2 // 258
    // 16 // 261
    // 660 // 276

    int abs( int x )
    {
        return x * ( x >> 31 );
    }

    class filter_t
    {
    private:
        struct state_t
        {
            float a;
            float b;
        };

    private:
        float m_res_aa;
        float m_res_ab;
        float m_res_au;
        float m_res_ba;
        float m_res_bb;
        float m_res_bu;
        float m_res_yb;
        float m_res_ybsqr;
        state_t m_state[ 2 ];
        float m_powerbuf;
        size_t m_powersamplecount;

    public:
        filter_t() = default;
        ~filter_t() = default;

        void init( int sr, float freq );
        void feed( float u );
        float getpowermean();
    };

    void filter_t::init( int sr, float freq )
    {
        float xi = 0.03f;
        float sp = 1.0f / sr;
        float T = 1 / ( 2 * pi * freq );
        float scale = exp( - sp * xi / T );
        if( xi < 1 )
        {
            float xisqrt = sqrt( 1 - xi*xi );
            float phase = sp * xisqrt / T;
            float phasecosv = cos( phase );
            float phasesinv = sin( phase ) / xisqrt;
            m_res_aa = scale * ( phasecosv + xi * phasesinv );
            m_res_ba = - scale * phasesinv;
            m_res_ab = scale * phasesinv;
            m_res_bb = scale * ( phasecosv - xi * phasesinv );
            m_res_au = ( 1 - scale * ( phasecosv + xi * phasesinv ) );
            m_res_bu = scale * phasesinv;
            m_res_yb = 2 * xi;
        }
        else if( xi > 1 )
        {
            float xisqrt = sqrt( xi*xi - 1 );
            float phase = sp * xisqrt / T;
            float scale2 = exp( - sp * ( xi + xisqrt ) / T );
            float phasecoshv = cosh( phase );
            float phasesinhv = sinh( phase ) / xisqrt;
            float phaseexpv = exp( 2 * phase );
            m_res_aa = scale * ( phasecoshv + xi * phasesinhv );
            m_res_ba = - scale * phasesinhv;
            m_res_ab = scale * phasesinhv;
            m_res_bb = scale * ( phasecoshv - xi * phasesinhv );
            m_res_au = - 0.5 * scale2 * ( ( phaseexpv - 1 ) * xi +
                ( phaseexpv + 1 - 2/scale2 ) * xisqrt ) / xisqrt;
            m_res_bu = scale * phasesinhv;
            m_res_yb = 2 * xi;
        }
        else
        {
            m_res_aa = scale * ( T + sp ) / T;
            m_res_ba = - scale * sp / T;
            m_res_ab = scale * sp / T;
            m_res_bb = scale * ( T - sp ) / T;
            m_res_au = scale * ( ( exp( sp / T ) - 1 ) * T - sp ) / T;
            m_res_bu = scale * sp / T;
            m_res_yb = 2;
        }
        m_res_ybsqr = m_res_yb*m_res_yb;
        for( auto& state : m_state )
        {
            state.a = 0;
            state.b = 0;
        }
        m_powerbuf = 0;
        m_powersamplecount = 0;
    }

    void filter_t::feed( float u )
    {
        float na = 0;
        float nb = 0;
        for( auto& state : m_state )
        {
            na = m_res_aa * state.a + m_res_ab * state.b + m_res_au * u;
            nb = m_res_ba * state.a + m_res_bb * state.b + m_res_bu * u;
            state.a = na;
            state.b = nb;
            u = m_res_yb * nb;
        }
        m_powerbuf += ( na*na + nb*nb ) * m_res_ybsqr;
        m_powersamplecount += 1;
    }

    float filter_t::getpowermean()
    {
        if( m_powersamplecount == 0 )
        {
            return 0;
        }
        float ret = m_powerbuf / m_powersamplecount;
        m_powerbuf = 0;
        m_powersamplecount = 0;
        return ret;
    }

    float clampf( float c )
    {
        return ( fabs( c ) - fabs( c - 1.0f ) + 1.0f ) * 0.5f;
    }

    // cc 0 1 2 3 4 5 6
    // R 0 0 0 0<<1 1 1
    // G 0 0<<1 1 1>>0<<1
    // B 0<<1 1>>0 0 0<<1
    // black < blue < cyan < green < yellow < red < white
    void setpixel( uint8_t* pixel, float c )
    {
        float cc = clampf( c ) * 6.0f;
        float d = fmod( cc, 1.0f );
        if( cc < 1 )
        {
            pixel[ 0 ] = 0;
            pixel[ 1 ] = 0;
            pixel[ 2 ] = 255 * d;
            pixel[ 3 ] = 255;
        }
        else if( cc < 2 )
        {
            pixel[ 0 ] = 0;
            pixel[ 1 ] = 255 * d;
            pixel[ 2 ] = 255;
            pixel[ 3 ] = 255;
        }
        else if( cc < 3 )
        {
            pixel[ 0 ] = 0;
            pixel[ 1 ] = 255;
            pixel[ 2 ] = 255 * ( 1 - d );
            pixel[ 3 ] = 255;
        }
        else if( cc < 4 )
        {
            pixel[ 0 ] = 255 * d;
            pixel[ 1 ] = 255;
            pixel[ 2 ] = 0;
            pixel[ 3 ] = 255;
        }
        else if( cc < 5 )
        {
            pixel[ 0 ] = 255;
            pixel[ 1 ] = 255 * ( 1 - d );
            pixel[ 2 ] = 0;
            pixel[ 3 ] = 255;
        }
        else if( cc < 6 )
        {
            pixel[ 0 ] = 255;
            pixel[ 1 ] = 255 * d;
            pixel[ 2 ] = pixel[ 1 ];
            pixel[ 3 ] = 255;
        }
        else
        {
            pixel[ 0 ] = 255;
            pixel[ 1 ] = 255;
            pixel[ 2 ] = 255;
            pixel[ 3 ] = 255;
        }
    }
}
}

using namespace spectrumdraw_private;

extern "C"
__declspec( dllexport )
void spectrumdraw_main( spectrumdraw_interface_t const* interface )
{
    filter_t filters[ batchsize ];
    int samplereduction = interface->samplecount / interface->bitmapwidth;
    size_t bitmapstride = interface->bitmapwidth * 4;
    for(
        unsigned ystart = 0;
        ystart < interface->freqcount;
        ystart += batchsize )
    {
        printf( "%i/%i\n", ystart, interface->freqcount );
        for(
            unsigned y = ystart;
            y < ( ystart + batchsize ) && y < interface->freqcount;
            y += 1 )
        {
            float frequency = interface->freqbase
                * pow( 2.0f, ( float )y / interface->freqdivision );
            filters[ y - ystart ].init( interface->samplerate, frequency );
        }
        int32_t const* currentsample =
            interface->samples;
        int32_t const* sampleend =
            interface->samples +
            interface->samplecount * interface->channelcount;
        float ampfactor =
            1 / ( ( float )interface->channelcount * interface->samplescale );
        int powersamplecount = 0;
        uint8_t* currentpixel =
            interface->bitmap;
        while( currentsample != sampleend )
        {
            float value;
            if( interface->channelcount == 1 )
            {
                value = currentsample[ 0 ];
                currentsample += 1;
            }
            else if( interface->channelcount == 2 )
            {
                value = currentsample[ 0 ] + currentsample[ 1 ];
                currentsample += 2;
            }
            else
            {
                ASSERT( false );
                value = currentsample[ 0 ];
            }
            value *= ampfactor;
            for(
                unsigned y = ystart;
                y < ( ystart + batchsize ) && y < interface->freqcount;
                y += 1 )
            {
                filters[ y - ystart ].feed( value );
            }
            powersamplecount += 1;
            if( powersamplecount == samplereduction )
            {
                for(
                    unsigned y = ystart;
                    y < ( ystart + batchsize ) && y < interface->freqcount;
                    y += 1 )
                {
                    setpixel(
                        currentpixel + bitmapstride * y,
                        filters[ y - ystart ].getpowermean() );
                }
                currentpixel += 4;
                powersamplecount = 0;
            }
        }
    }
}