Nubert Lautsprecher, HiFi- und Surround-Elektronik

Code: Alles auswählen

/*
 *   Restrictions:
 *   - can only handle 2x 16 bit data
 *   - many, many other
 *
 *   Ungetestet, Unvollständig, Unbedienbar, Quick&Dirty
 *   Ohne GPL-Header, dafür mit Kommentaren 
 *   (verbietet die GNU-Religion eigentlich Kommentare neben dem Vater-Unser im Header?)
 */

/*
 *   Design Guide
 *   ~~~~~~~~~~~~
 *   Der Filter ist ein sogenannter DFT-Filter mit Double Overlap. In der Literatur habe ich darüber noch nichts gefunden.
 *   DFT-Filter sind keine LTI-Systeme. Sie sind nicht TI, sind aber L. Um die NTI-Eigenschaft zu mildern, verwendet man
 *   überlappende Blocks und "fenstert" diese gewöhnlich vor der Analyse (damit die Funktion glatt wird und keine Stufen
 *   am Rand hat, was zu frequenzmäßigem Ausbluten führt) und nach der Re-Synthese (damit es keine Sprungstellen durch
 *   nicht perfekte Anschlußstellen kommt, die zu deutlichen Knacksern führen. Typischerweise nimmt man für beide Fenste-
 *   rungen die gleiche Funktion. Damit keine Seitenbänder auftreten, müssen die Fenster perfekt wieder zusammensetzbar sein.
 *   Bei Single Overlap führt das zur Forderung, daß w1^2 + w2^2 = 1 sein muß, bei Double Overlap zu w1^2 + w2^2 + w3^2 = 1.
 *   Single Overlap stellt die gleichen Forderungen der MDCT, daher sind auch die dort üblichen Fensterfunktionen verwendbar
 *   (Sinus, KBD, CosSin). Double Overlap stellt andere Forderungen, u.a. geht sehr gut das Sinus^2-Fenster, was sehr gute
 *   Eigenschaften aufweist. Nachteilig ist eine DFT der Breite 3*2^n, die nicht so schnell mal hingekritzelt ist.
 *
 *   Der Kern des Programms (neben dem garstigen Zeug, was immer noch rundherum erledigt werden muß) besteht darin:
 *     - WINDOW_INCREMENT neue Samples aus der Quell-Datei zu lesen   
 *     - insgesamt die letzten 3*WINDOW_INCREMENT Samples als Block zu betrachten und unter zweimaliger Verwendung der 
 *       Filterfunktion (siehe oben) zu filtern
 *     - die Blocks wieder zusammenzusetzen
 *     - die bei jedem Durchlauf enstehenden WINDOW_INCREMENT Samples in die Zieldatei zu schreiben
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#ifndef M_PI
# define M_PI 3.1415926535897932384626433832795
#endif


// Wichtige Parameter einer WAVE-Datei
typedef struct {
    double         samplefrequency ;
    unsigned int   channels        ;
    unsigned int   bits            ;
    unsigned long  samples         ;
    // ...
} waveinfo_t ;

// Komplexe Zahl mit 32 bit Genauigkeit (man muß ja nicht übertreiben
typedef struct {
    float  re ;
    float  im ;
} complex_float_t ;


// #include <fftw3.h>
void  fft_forward ( complex_float_t* fft_buffer, unsigned int len ) ; 
void  fft_bckward ( complex_float_t* fft_buffer, unsigned int len ) ; 

void  fft_forward ( complex_float_t* fft_buffer, unsigned int len ) {}
void  fft_bckward ( complex_float_t* fft_buffer, unsigned int len ) {}

// Fortschritt pro Arbeitsschritt, Fenstergröße ist derzeitig dreimal so groß
#define WINDOW_INCREMENT     16384
#define CHANNELS                 2      // 2.0 only


// Lesen und Analyse eines WAVE-Headers, Lesezeiger steht DANACH am Anfang der PCM-Daten
// Bei Fehler wird -1 zurückgegeben
// Streamingfähig, d.h. kein fseek() benutzt
// Analyse ist nicht vollständig und nicht idiotensicher (Division durch 0 und Überlauf bei fehlerhaften Dateien möglich)
static int
Read_Header ( waveinfo_t* const info, FILE* fp )
{
    unsigned char   Header1 [    20 ] ;
    unsigned char   Header2 [ 65536 ] ;
    unsigned char   Header3 [     8 ] ;
    unsigned long   ChunkLen ;

    fread ( Header1, 1,       20, fp ) ;

    if ( Header1 [ 0] != 'R' ) return -1 ;
    if ( Header1 [ 1] != 'I' ) return -1 ;
    if ( Header1 [ 2] != 'F' ) return -1 ;
    if ( Header1 [ 3] != 'F' ) return -1 ;
    if ( Header1 [ 8] != 'W' ) return -1 ;
    if ( Header1 [ 9] != 'A' ) return -1 ;
    if ( Header1 [10] != 'V' ) return -1 ;
    if ( Header1 [11] != 'E' ) return -1 ;
    if ( Header1 [12] != 'f' ) return -1 ;
    if ( Header1 [13] != 'm' ) return -1 ;
    if ( Header1 [14] != 't' ) return -1 ;
    if ( Header1 [15] != ' ' ) return -1 ;
    if ( Header1 [18] !=   0 ) return -1 ;
    if ( Header1 [19] !=   0 ) return -1 ;

    ChunkLen = Header1 [16] + 256 * Header1 [17];
    if ( ChunkLen < 16 )       return -1 ;
    fread ( Header2, 1, ChunkLen, fp ) ;
    fread ( Header3, 1,        8, fp ) ;

    if ( Header2 [ 0] != 0x01 ) return -1 ;
    if ( Header2 [ 1] != 0x00 ) return -1 ;

    info -> channels        = Header2 [ 2] + 256 * Header2 [ 3] ;
    info -> samplefrequency = Header2 [ 4] + 256 * Header2 [ 5] + 65536 * Header2 [ 6] + 16777216 * Header2 [ 7] ;
    info -> bits            = Header2 [14] + 256 * Header2 [15] ;
    info -> samples         = Header3 [ 4] + 256 * Header3 [ 5] + 65536 * Header3 [ 6] + 16777216 * Header3 [ 7] ;
    info -> samples        /= ( info -> bits + 7 ) / 8 * info -> channels ;

    return 0 ;
}


// Schreiben eines 44 Byte Standard-WAVE-Header, danach müssen noch die PCM-Daten geschrieben werden
// Auch hier fehlen noch ein paar Sicherheitsabfragen
static int
Write_Header ( waveinfo_t  info, FILE* fp )
{
    unsigned char   Header [44] ;
    unsigned long   word32 ;
    unsigned char*  p          = Header ;
    unsigned int    Bytes      = (info . bits + 7) / 8 ;
    double          pcmdatalen = (double) info . channels * Bytes * info . samples ;

    *p++ = 'R';
    *p++ = 'I';
    *p++ = 'F';
    *p++ = 'F';                                   // "RIFF" label

    word32 = pcmdatalen + (44 - 8) < 0xFFFFFF00  ?  (unsigned long)pcmdatalen + (44 - 8)  :  (unsigned long)0xFFFFFF00 ;
    *p++ = (unsigned char) (word32 >>  0);
    *p++ = (unsigned char) (word32 >>  8);
    *p++ = (unsigned char) (word32 >> 16);
    *p++ = (unsigned char) (word32 >> 24);        // Size of the next chunk

    *p++ = 'W';
    *p++ = 'A';
    *p++ = 'V';
    *p++ = 'E';                                   // "WAVE" label

    *p++ = 'f';
    *p++ = 'm';
    *p++ = 't';
    *p++ = ' ';                                   // "fmt " label

    *p++ = 0x10;
    *p++ = 0x00;
    *p++ = 0x00;
    *p++ = 0x00;                                  // length of the PCM data declaration = 2+2+4+4+2+2

    *p++ = 0x01;
    *p++ = 0x00;                                  // ACM type 0x0001 = uncompressed linear PCM

    word32 = info . channels ;
    *p++ = (unsigned char) (word32 >> 0);
    *p++ = (unsigned char) (word32 >> 8);              // Channels

    word32 = (unsigned long) ( info . samplefrequency + 0.5 ) ;
    *p++ = (unsigned char) (word32 >>  0);
    *p++ = (unsigned char) (word32 >>  8);
    *p++ = (unsigned char) (word32 >> 16);
    *p++ = (unsigned char) (word32 >> 24);               // Sample frequency

    word32 *= Bytes * info . channels ;
    *p++ = (unsigned char) (word32 >>  0);
    *p++ = (unsigned char) (word32 >>  8);
    *p++ = (unsigned char) (word32 >> 16);
    *p++ = (unsigned char) (word32 >> 24);               // Bytes per second in the data stream

    word32 = Bytes * info . channels ;
    *p++ = (unsigned char) (word32 >>  0);
    *p++ = (unsigned char) (word32 >>  8);               // Bytes per sample time

    word32 = info . bits ;
    *p++ = (unsigned char) (word32 >> 0);
    *p++ = (unsigned char) (word32 >> 8);         // Bits per single sample

    *p++ = 'd';
    *p++ = 'a';
    *p++ = 't';
    *p++ = 'a';                                   // "data" label

    word32 = pcmdatalen < 0xFFFFFF00  ?  (unsigned long)pcmdatalen  :  (unsigned long)0xFFFFFF00 ;
    *p++ = (unsigned char) (word32 >>  0);
    *p++ = (unsigned char) (word32 >>  8);
    *p++ = (unsigned char) (word32 >> 16);
    *p++ = (unsigned char) (word32 >> 24);               // Größe der rohen PCM-Daten

    fwrite ( Header, 1, sizeof Header, fp ) ;
    return 0 ;
}


static int
round_to_next_int ( double x )
{
    return (int) floor ( x + 0.5 ) ;
}

// Lesen der Frequenzgangdatei
// Aufbau:
//   Frequenz  Pegel
//   Frequenz  Pegel  Phase
//   # Kommentar
// Frequenzwerte müssen numerisch sortiert sein, Pegel ist in dB, Phase in Grad
static void
Read_Config ( FILE* fp, complex_float_t* frequency_response, unsigned int len, double sample_frequency )
{
    char           line [ 512 ];
    static double  fr [ 32768 ] ;
    static double  le [ 32768 ] ;
    static double  ph [ 32768 ] ;
    float          re ;
    float          im ;
    double         freq  ;
    double         level ;
    double         phase ;
    unsigned int   i ;
    int            j ;
    int            max = 0 ;

    fr [max] = 0. ;
    le [max] = 1. ;
    ph [max] = 0. ;
    max++ ;

    while ( fgets ( line, sizeof line, fp ) != NULL ) {
        switch ( line [0] ) {
        case '#' :
        case ';' :
        case '!' :
        case '\n':
            break;
        default:
            switch ( sscanf ( line, "%f%f%f", &freq, &level, &phase ) ) {
            case 2:
                fr [max]  = freq ;
                le [max]  = pow ( 10., 0.05 * level ) ;
                ph [max]  = 0. ;
                max++ ;
                break ;
            case 3:
                fr [max]  = freq ;
                le [max]  = pow ( 10., 0.05 * level ) ;
                ph [max]  = phase * ( M_PI / 180. ) ;
                // Analytische Fortsetzung
                ph [max] += 2. * M_PI * round_to_next_int ( ( ph [max-1] - ph [max] ) / ( 2. * M_PI ) ) ;
                max++ ;
                break ;
            default:
                fprintf ( stderr, "Vermurkste Zeile: %s", line ) ;
                break ;
            }
        }
    }
    fr [max] = 1.e37f ;
    le [max] = le [max-1] ;
    ph [max] = ph [max-1] ;

    j = 0 ;
    for ( i = 0; i <= len/2; i++ ) {

        freq = sample_frequency * i / len ;
        while ( fr [j + 1] <= freq )
            j++ ;

        level = ( le[j] * (fr[j+1] - freq) + le[j+1] * (freq - fr[j]) ) / ( fr[j+1] - fr[j] ) ;
        phase = ( ph[j] * (fr[j+1] - freq) + ph[j+1] * (freq - fr[j]) ) / ( fr[j+1] - fr[j] ) ;
        re    = (float) ( level * cos (phase) ) ;
        im    = (float) ( level * sin (phase) ) ;

        frequency_response [    + i] . re = +re ;
        frequency_response [    + i] . im = +im ;
        if ( i != 0 ) {
            frequency_response [len - i] . re = +re ;
            frequency_response [len - i] . im = -im ;       // Ist das richtig? Grübel?
        }
    }
}


// Frequenzgang anwenden auf fft_buffer
static void
Apply_Frequency_Response ( complex_float_t* fft_buffer, complex_float_t* frequency_response, unsigned int len )
{
    float  re ;
    float  im ;

    for ( ; len--; fft_buffer++, frequency_response++ ) {
        re             = fft_buffer->re * frequency_response->re - fft_buffer->im * frequency_response->im ;
        im             = fft_buffer->re * frequency_response->im + fft_buffer->im * frequency_response->re ;
        fft_buffer->re = re ;
        fft_buffer->im = im ;
    }
}


// Fensterfunktion berechnen und tabellieren
static void 
Init_Window ( float* Window, unsigned int len )
{
    double        x ;
    unsigned int  i ;

    for ( i = 0; i < len; i++ ) {
        x          = ( i + 0.5 ) / len - 0.5 ;
        x          = cos ( M_PI * x ) ;
        Window [i] = (float) ( x * x * sqrt (8. / 9) ) ;
    }

    // Fenstertest: Die Fenster kombiniert müssen wieder 1.000000 ergeben
    // Fensterfunktion wird vor der Analyse und Nach der Synthese ausgeführt, daher muß a² + b² + c² = 1 ergeben (und nicht a + b + c = 1 !)
    for ( i = 0; i < len/3; i++ ) {
        x = Window [i] * Window [i] + Window [i + len/3] * Window [i + len/3] + Window [i + len/3*2] * Window [i + len/3*2] ;
        fprintf ( stderr, "%5u %8.6f\n", i, x ) ;
    }
}


// Gleitkommawert auf 16 bit runden
// bei Übersteuerungen sollte sich noch der höchste Wert gemerkt werden
static short 
Quantize ( double x )
{
    int  r = (int) floor ( x + 0.5 ) ;
    if ( r != (short) r )
        r = ( r >> 31 ) ^ 0x7FFF ;
    return r ;
}


// Holly Main
int 
main ( int argc, char** argv ) 
{
    static complex_float_t  fft_buffer         [ 3 * WINDOW_INCREMENT ] ;
    static complex_float_t  frequency_response [ 3 * WINDOW_INCREMENT ] ;
    static float            fft_window         [ 3 * WINDOW_INCREMENT ] ;
    static float            input              [ CHANNELS ] [ 3 * WINDOW_INCREMENT ] ;
    static float            output             [ CHANNELS ] [ 3 * WINDOW_INCREMENT ] ;
    static short            input_quant        [ WINDOW_INCREMENT ] [ CHANNELS ] ;
    static short            output_quant       [ WINDOW_INCREMENT ] [ CHANNELS ] ;
    FILE*                   fp_input  ;
    FILE*                   fp_output ;
    FILE*                   fp_config ;
    waveinfo_t              waveinfo ;        
    unsigned long           samples_to_read ;
    unsigned long           i ;
    int                     j ;
    int                     k ;
//  fftw_complex            X_freq         ;
//  fftw_plan               X_time_to_freq ;
//  fftw_plan               X_freq_to_time ;

//  X_freq         = fftw_malloc          ( sizeof (fftw_complex) * SAMPLES_FREQ ) ;
//  X_time_to_freq = fftw_plan_dft_c2c_1d ( SAMPLES_MONO, samp_in , freq, FFTW_ESTIMATE ) ;
//  X_freq_to_time = fftw_plan_dft_c2c_1d ( SAMPLES_MONO, freq, samp_out, FFTW_ESTIMATE ) ;

//  fftw_execute ( X_time_to_freq ) ;
//  fftw_execute ( X_freq_to_time ) ;

    Init_Window  ( fft_window, 3 * WINDOW_INCREMENT ) ;

    memset ( frequency_response, 0, sizeof frequency_response ) ;
    memset ( input             , 0, sizeof input              ) ;
    memset ( output            , 0, sizeof output             ) ;

    // Dateien aufmachen, analysieren
    if ( argc != 3 ) {
        fprintf ( stderr, "Usage: %s Input.wav Output.wav Configfile\n", argv[1] ) ;
        return 1 ;
    }

    if ( ( fp_input = fopen ( argv[1], "rb" ) ) == NULL ) {
        fprintf ( stderr, "%s: Can't open '%s'\n", argv[0], argv[1] );
        return 2 ;
    }

    if ( strcmp ( argv[1], argv[2] ) == 0 ) {
        fprintf ( stderr, "%s: Input and Output file are identical: '%s' = '%s'\n", argv[0], argv[1], argv[2] );
        return 3 ;
    }

    if ( ( fp_output = fopen ( argv[2], "wb" ) ) == NULL ) {
        fprintf ( stderr, "%s: Can't create '%s'\n", argv[0], argv[2] );
        return 4 ;
    }

    if ( Read_Header ( &waveinfo, fp_input ) ) {
        fprintf ( stderr, "%s: Analysis of WAVE header failed.\n", argv[0], argv[3] );
        return 5 ;
    }

    if ( ( fp_config = fopen ( argv[3], "r" ) ) == NULL ) {
        fprintf ( stderr, "%s: Can't read config file '%s'\n", argv[0], argv[3] );
        return 6 ;
    }

    Read_Config ( fp_config, frequency_response, 3 * WINDOW_INCREMENT, waveinfo.samplefrequency ) ;
    fclose      ( fp_config ) ;

    // Letzten Vorbereitungen
    Write_Header ( waveinfo, fp_output ) ;

    // Block für Block bearbeiten
    for ( i = 0; i < waveinfo.samples; i += WINDOW_INCREMENT ) {
        fprintf ( stderr, "%4.1f sec (%2.0f%%)\r", i / waveinfo . samplefrequency, 100. * i / waveinfo.samples ) ;

        // neue Samples holen
        samples_to_read = waveinfo.samples - i ;
        if ( samples_to_read > WINDOW_INCREMENT )
            samples_to_read = WINDOW_INCREMENT ;

        memset  ( input_quant , 0           , sizeof input_quant         ) ;
        fread   ( input_quant , 2 * CHANNELS, samples_to_read, fp_input  ) ;

        // Samples bearbeiten
        for ( k = 0; k < CHANNELS; k++ ) {

            // Kanalweise umsortieren, letzten 3*WINDOW_INCREMENT Samples dabei verfügbar halten
            memmove ( input [k] + 0, input [k] + WINDOW_INCREMENT, sizeof(**input) * 2 * WINDOW_INCREMENT ) ;
            for ( j = 0; j < WINDOW_INCREMENT; j++ )
                input [k] [ 2 * WINDOW_INCREMENT + j ] = input_quant [j] [k] ;

            // Fenstern
            for ( j = 0; j < 3 * WINDOW_INCREMENT; j++ ) {
                fft_buffer [j].re = input [k] [j] * fft_window [j] ;
                fft_buffer [j].im = 0. ;
            }

            // Transformieren, Filter, Rücktransformieren
            fft_forward              ( fft_buffer, 3 * WINDOW_INCREMENT ) ; 
            Apply_Frequency_Response ( fft_buffer, frequency_response, 3 * WINDOW_INCREMENT ) ;
            fft_bckward              ( fft_buffer, 3 * WINDOW_INCREMENT ) ; 

            // Noch mal Fenstern und dabei zusammenfassen
            for ( j = 0; j < 3 * WINDOW_INCREMENT; j++ ) {
                output [k] [j] += fft_buffer [j].re * fft_window [j] ;
            }

            // Quantisieren
            for ( j = 0; j < WINDOW_INCREMENT; j++ )
                output_quant [j] [k] = Quantize ( output [k] [j] / WINDOW_INCREMENT ) ;                             // Stimmt der Pegel für fftw ???
            memmove ( output [k] + 0, output [k] + WINDOW_INCREMENT, sizeof(**output) * 2 * WINDOW_INCREMENT ) ;
        }
            
        // Und wieder schreiben
        fwrite  ( output_quant, 4, samples_to_read, fp_output ) ;  // derzeitig verzögert der Filter um 2 * WINDOW_INCREMENT
    }

    // Und alles wieder zumachen
    fprintf ( stderr, "Ready.                \n" ) ;
    fclose ( fp_input  ) ;
    fclose ( fp_output ) ;

    return 0 ;
}