Simple Spec -- A Simple Discrete-time STFT Program.

simple_spec.c

#include <math.h>
#include <sndfile.h>
#include <string.h>
#include <assert.h>
#include <getopt.h>

#define PI 3.141592653589793
#define EPS 1e-8
#define MAX_BIN_NUM 65536
/* SHIFT_NUM should be less than or equal to BIN_NUM */
#define BUFF_SIZE (MAX_BIN_NUM * 10)

typedef struct Comp {
    /* comp of the form: a + bi */
    double a, b;
} Comp;

double pcos[2][MAX_BIN_NUM], psin[2][MAX_BIN_NUM];

typedef void (*win_func_t)(double *w, int n);
typedef void (*amp_func_t)(void);
typedef void (*init_func_t)(void);
typedef void (*finale_func_t)(void);

void rect_window(double *, int);
void hann_window(double *, int);
void hamming_window(double *, int);

void linear_amp(void);
void normalize_amp(void);
void decibel_amp(void);
void dtmf_amp(void);

void dtmf_init(void);
void dtmf_finale(void);
void common_init(void) {}
void common_finale(void) {}

int bin_num = 1024;
int sample_rate = 44100;
int resample_rate = 4410;
int shift_num = 256;
int channels = 2;

win_func_t wfunc = hann_window;
amp_func_t afunc = decibel_amp;
init_func_t ifunc = common_init;
init_func_t ffunc = common_finale;

Comp sig[MAX_BIN_NUM], freq[MAX_BIN_NUM];
double win[MAX_BIN_NUM];
short buff[BUFF_SIZE * 2];

void fft_init() {
    int i;
    for (i = 0; i < bin_num; i++)
    {
        pcos[0][i] = cos(-PI / (double)i);
        pcos[1][i] = cos(PI / (double)i);
        psin[0][i] = sin(-PI / (double)i);
        psin[1][i] = sin(PI / (double)i);
    }
}

#define comp_mul_self(c, c2) \
    do { \
        double ca = c->a; \
        c->a = ca * c2->a - c->b * c2->b; \
        c->b = c->b * c2->a + ca * c2->b; \
    } while (0)

void fft(const Comp *sig, Comp *freq, int s, int n, int inv) {
    int i, hn = n >> 1;
    Comp ep, ei;
    Comp *pi = &ei, *pp = &ep;
    ep.a = pcos[inv][hn];
    ep.b = psin[inv][hn];
    if (!hn) *freq = *sig;
    else
    {
        fft(sig, freq, s << 1, hn, inv);
        fft(sig + s, freq + hn, s << 1, hn, inv);
        pi->a = 1;
        pi->b = 0;
        for (i = 0; i < hn; i++)
        {
            Comp even = freq[i], *pe = freq + i, *po = pe + hn;
            comp_mul_self(po, pi);
            pe->a += po->a;
            pe->b += po->b;
            po->a = even.a - po->a;
            po->b = even.b - po->b;
            comp_mul_self(pi, pp);
        }
    }
}

/* Global variables for DTMF Recognition */
int key_freq[8] = {697, 770, 852, 941, 1209, 1336, 1477, 1633};
int key_char[16] = {'1', '2', '3', 'A', '4', '5', '6', 'B', '7', '8', '9', 'C', '*', '0', '#', 'D'};
int energy_range[8][2], base_range[8][2];
int hit_len = 4;
int miss_len = 1;
int alive[8], palive[8], hit_cnt[8], miss_cnt[8];
char dial_seq[1024], *dtail;
double tolerance = 0.015, thres = 0.9;

void dtmf_init() {
    double basef = resample_rate / (double)bin_num;
    int mask[MAX_BIN_NUM];
    int i, j, hb = bin_num >> 1;
    memset(mask, 0, sizeof mask);

    for (i = 0; i < 8; i++)
    {
        double kf = key_freq[i];
        double pos = kf / basef;
        int l, r;
        for (l = floor(pos) - 1; l >= 0 && fabs(l * basef - kf) / kf < tolerance; l--);
        for (r = ceil(pos) + 1; r < hb && fabs(r * basef - kf) / kf < tolerance; r++);
        l++; r--;
        for (j = l; j <= r; j++)
        {
            if (mask[j]) assert(0 && "You can not tolerate that much!");
            mask[j] = 1;
        }
        energy_range[i][0] = l;
        energy_range[i][1] = r;
    }
    for (i = 0; i < 8; i++)
    {
        int l, r;
        for (l = energy_range[i][0] - 1; l >= 0 && !mask[l]; l--);
        for (r = energy_range[i][1] + 1; r < hb && !mask[r]; r++);
        base_range[i][0] = l + 1;
        base_range[i][1] = r - 1;
    }
    for (i = 0; i < 8; i++)
    {
        int el = energy_range[i][0], er = energy_range[i][1];
        int bl = base_range[i][0], br = base_range[i][1];
        fprintf(stderr, "Tone %d Hz: %d(%.3f) ~ %d(%.3f), %d(%.3f) ~ %d(%.3f)\n",
                key_freq[i], el, el * basef, er, er * basef,
                            bl, bl * basef, br, br * basef);
    }
    memset(palive, 0, sizeof palive);
    dtail = dial_seq;
}

void dtmf_finale() {
    *dtail = '\0';
    printf("The dailed sequence is: %s\n", dial_seq);
}

void spectral_analyzer(SNDFILE *sf) {
    short *fptr = buff;
    sf_count_t fcnt = 0, cnt = 0, bcnt = 0, bound;
    while (sf_readf_short(sf, buff + (bcnt << 1), 1))
    {
        if (!cnt)
        {
            bound = buff - fptr + ((++bcnt) << 1);
            while ((fcnt << 1) < bound)
            {
                while ((fcnt << 1) < bound && fcnt < bin_num)
                    fcnt++;
                if (fcnt == bin_num)
                {
                    int i;
                    double max = 0;
                    for (i = 0; i < bin_num; i++)
                    {
                        sig[i].a = win[i] * (fptr[i << 1] + fptr[(i << 1) + 1]) / 2.0;
                        sig[i].b = 0;
                    }
                    fft(sig, freq, 1, bin_num, 0);
                    afunc();
                    fptr += shift_num << 1;
                    bound = buff - fptr + (bcnt << 1);
                    fcnt = 0;
                }
            }
            if (bcnt == BUFF_SIZE)
            {
                /* move to the beginning */
                memmove(buff, fptr, (fcnt << 1) * sizeof(buff[0]));
                bcnt = fcnt;
                fptr = buff;
            }
        }
        /* naive resampling */
        if (++cnt == sample_rate / resample_rate)
            cnt = 0;
    }
}

int is_raw = 0;
struct option long_options[] = {
    {"win", required_argument, NULL, 'w'},
    {"bin", required_argument, NULL, 'b'},
    {"delta", required_argument, NULL, 'd'},
    {"raw", no_argument, &is_raw, 1},
    {"freq", required_argument, NULL, 'f'},
    {"amp", required_argument, NULL, 'a'},
    {"help", no_argument, NULL, 'h'},
    {0, 0, 0, 0}
};

int str_to_int(char *repr, int *flag) {
    char *endptr;
    int val = (int)strtol(repr, &endptr, 10);
    if (endptr == repr || endptr != repr + strlen(repr))
    {
        *flag = 0;
        return 0;
    }
    *flag = 1;
    return val;
}

int main(int argc, char **argv) {
    SF_INFO info;
    SNDFILE *sf;
    int opt, ind = 0;

    while ((opt = getopt_long(argc, argv, "w:b:d:rf:a:h", long_options, &ind)) != -1)
    {
        switch (opt)
        {
            case 'w': 
                if (!strcmp(optarg, "rect"))
                    wfunc = rect_window;
                else if (!strcmp(optarg, "hann"))
                    wfunc = hann_window;
                else if (!strcmp(optarg, "hamming"))
                    wfunc = hamming_window;
                else
                    return fprintf(stderr, "Invalid window: %s.\n", optarg), 1;
                break;
            case 'b':
                {
                    int flag;
                    bin_num = str_to_int(optarg, &flag);
                    if (!flag)
                        return fprintf(stderr, "Please specify an integer.\n"), 1;
                    break;
                }
            case 'd':
                {
                    int flag;
                    shift_num = str_to_int(optarg, &flag);
                    if (!flag)
                        return fprintf(stderr, "Please specify an integer.\n"), 1;
                    break;
                }
            case 'r':
                is_raw = 1;
                break;
            case 'f':
                {
                    int flag;
                    resample_rate = str_to_int(optarg, &flag);
                    if (!flag)
                        return fprintf(stderr, "Please specify an integer.\n"), 1;
                    break;
                }
            case 'a':
                if (!strcmp(optarg, "linear"))
                    afunc = linear_amp;
                else if (!strcmp(optarg, "decibel"))
                    afunc = decibel_amp;
                else if (!strcmp(optarg, "norm"))
                    afunc = normalize_amp;
                else if (!strcmp(optarg, "dtmf"))
                {
                    afunc = dtmf_amp;
                    ifunc = dtmf_init;
                    ffunc = dtmf_finale;
                    /* Default settings for DTMF */
                    bin_num = 256;
                    sample_rate = 8000;
                    resample_rate = 8000;
                    shift_num = 128;
                    channels = 1;
                }
                else
                    return fprintf(stderr, "Invalid amplifier: %s.\n", optarg), 1;
                break;
            case 'h':
                fprintf(stderr,
                        "Simple Spec -- A Simple Discrete-time STFT Program.\n\n"
                        "Usage: simple_spec [OPTION]... [FILE]\n\n"
                        "  -w, --win\t\twindow function: rect, hann, hamming\n"
                        "  -b, --bin\t\tthe width of a window\n"
                        "  -d, --delta\t\tthe shift of a window\n"
                        "  --raw\t\t\tread raw PCM16 data\n"
                        "  -f, --freq\t\tthe frequency used in resampling\n"
                        "  -a, --amp\t\tthe way of calculating amplitude: linear, decibel, norm, dtmf\n"
                        "  -h, --help\t\tshow this info\n"
                        "\nAuthor: Ted Yin <ted.sybil@gmail.com>\n");
                return 0;
            default:
                return 1;
        }
    }

    if (is_raw)
    {
        info.format = SF_FORMAT_PCM_16;
        info.samplerate = sample_rate;
        info.channels = channels;
    }
    else
        info.format = 0;
    if (optind == argc)
    {
        fprintf(stderr, "Please specify a file name.\n");
        return 1;
    }
    if (!(sf = sf_open(argv[optind], SFM_READ, &info)))
    {
        fprintf(stderr, "Failed while opening file: %s.\n", argv[optind]);
        return 1;
    }
    fprintf(stderr, "Frames: %lu\n", info.frames);
    fprintf(stderr, "Sample Rate: %d Hz\n", info.samplerate);

    sample_rate = info.samplerate;
    resample_rate = sample_rate / (sample_rate / resample_rate);

    fft_init();
    ifunc();
    wfunc(win, bin_num);
    sf_seek(sf, 0, SEEK_CUR);
    spectral_analyzer(sf);
    ffunc();
    sf_close(sf);

    return 0;
}

void hann_window(double *w, int n) {
    int i;
    for (i = 0; i < n; i++)
        w[i] = 0.5 * (1 - cos(2 * PI * i / (n - 1)));
}

void hamming_window(double *w, int n) {
    int i;
    for (i = 0; i < n; i++)
        w[i] = 0.53836 - 0.46164 * cos(2 * PI * i / (n - 1));
}

void rect_window(double *w, int n) {
    int i;
    for (i = 0; i < n; i++)
        w[i] = 1;
}

void linear_amp(void) {
    int hb = bin_num >> 1, i;
    double a, b;
    for (i = 0; i < hb; i++)
    {
        a = freq[i].a;
        b = freq[i].b;
        printf("%.6f ", sqrt(a * a + b * b));
    }
    puts("");
}

void dtmf_amp(void) {
    int state[8];
    int hb = bin_num >> 1, i, j, flag;
    double e0 = 0, e, a, b;
    double dis[8];
    memset(state, 0, sizeof state);
    for (i = 0; i < hb; i++)
    {
        double a = freq[i].a;
        double b = freq[i].b;
        b = sqrt(a * a + b * b);
        e0 += b * b;
    }
    e0 /= hb;
    for (i = 0; i < 8; i++)
    {
        int el = energy_range[i][0], er = energy_range[i][1];
        e = 0;
        for (j = el; j <= er; j++)
        {
            a = freq[j].a;
            b = freq[j].b;
            e += a * a + b * b;  /* V^2 */
        }
        e /= er - el + 1;
        if (e0 > EPS && e / e0 > 10)
        { /* hit */
            dis[i] = e;
            if (++hit_cnt[i] == hit_len)
            {
                hit_cnt[i] = 0;
                miss_cnt[i] = 0;
                alive[i] = 1;
            }
        }
        else
        {
            dis[i] = 0;
            if (++miss_cnt[i] == miss_len)
            {
                hit_cnt[i] = 0;
                miss_cnt[i] = 0;
                alive[i] = 0;
            }
        }
    }
    flag = 0;
    for (i = 0; i < 8; i++)
        flag += alive[i];
    if (flag >= 2)
    {
        double mhi = 0, mlo = 0;
        int hi = -1, lo = -1;
        flag = 0;
        for (i = 0; i < 8; i++)
            if (alive[i] != palive[i])
            {
                flag = 1;
                break;
            }
        if (flag)
        {
            for (i = 0; i < 4; i++)
                if (alive[i] && dis[i] > mhi)
                { 
                    hi = i * 4;
                    mhi = dis[i];
                }
            for (i = 4; i < 8; i++)
                if (alive[i] && dis[i] > mlo)
                { 
                    lo = i - 4;
                    mlo = dis[i];
                }
            if (hi > -1 && lo > -1)
            {
                *dtail++ = key_char[hi + lo];
                printf("Dialed: %c\n", key_char[hi + lo]);
            }
        }
    }
    memmove(palive, alive, sizeof alive);
    for (i = 0; i < 8; i++)
        printf("%c", alive[i] ? '*' : '.');
    puts("");
}

void normalize_amp(void) {
    double res[MAX_BIN_NUM], max = 0;
    int hb = bin_num >> 1, i;
    for (i = 0; i < hb; i++)
    {
        double a = freq[i].a;
        double b = freq[i].b;
        res[i] = sqrt(a * a + b * b);
        if (res[i] > max)
            max = res[i];
    }
    if (max < EPS)
    {
        for (i = 0; i < hb; i++)
            printf("%.6f ", 0.0);
    }
    else
    {
        for (i = 0; i < hb; i++)
            printf("%.6f ", res[i] / max);
    }
    puts("");
}

void decibel_amp(void) {
    double res[MAX_BIN_NUM], max = 0;
    int hb = bin_num >> 1, i;
    for (i = 0; i < hb; i++)
    {
        double a = freq[i].a;
        double b = freq[i].b;
        b = a * a + b * b;
        b = b < EPS ? 0 : 10 * log10(b);
        printf("%.6f ", b);
    }
    puts("");
}