#include <stdio.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include <inttypes.h>

#include "resample.h"
#include "interpol.h"

#define SINC_RADIUS 10

static int lcd(int a, int b) {
    assert(a >= 1 && b >= 1);

    while (a != b) {
        while (a > b) {
            a = a-b;
        }

        while (b > a) {
            b = b-a;
        }
    }

    return a;
}

void resample_init(struct resample_state *s) {
    assert(s);
    memset(s, 0, sizeof(struct resample_state));
    s->sfreq = s->dfreq = -1;
}

void resample_done(struct resample_state *s) {
    assert(s);
    interpol_done(&s->interpol);
}

void resample_get(struct resample_state *s, struct qbuf *sq, struct qbuf *dq, int sfreq, int dfreq) {
    int di;
    float x = 0;
    float *sp, *dp;
    size_t sl, dl;
    
    assert(s && sq && dq && sfreq > 0 && dfreq > 0);

    if (sfreq != s->sfreq || dfreq != s->dfreq) {
        interpol_done(&s->interpol);
        interpol_init(&s->interpol, dfreq/lcd(sfreq,dfreq), SINC_RADIUS);
        s->sfreq = sfreq;
        s->dfreq = dfreq;
    }

    sp = qbuf_pull(sq, &sl);
    dp = qbuf_push(dq, &dl);

    assert(sp && dp && sl && dl);

    sl /= sizeof(float);
    dl /= sizeof(float);
    
    di = 0;
    while (di < dl) {
        x = (float) di*sfreq/dfreq + s->delta;

        if (x + SINC_RADIUS + 1 > sl)
            break;
        
        *(dp++) = interpol_get(&s->interpol, sp, sl, x);
        di++;
    }
    
    qbuf_push_validate(dq, di*sizeof(float));

    if (x >= SINC_RADIUS) {
        int rn;
        rn = (int) (x - SINC_RADIUS);
        assert(rn <= sl);
        s->delta = x - rn;
        fprintf(stderr, "foo\n");
        qbuf_pull_invalidate(sq, rn*sizeof(float));
        fprintf(stderr, "bar\n");
    }
}

#if (TEST == 7)

#define BUFSIZE (10*1024)

static void sinbuf(float *buf, int l) {
    static int i = 0;

    for (; l > 0; l--, i = (i+1 == 100) ? 0 : i+1)
        *(buf++) = sin(i*2*M_PI/100);
}

static void convbuf(float *buf, int l) {
    int16_t *p = (int16_t*) buf;
    for (; l > 0; l--, p+=2) {
        float v = ((float) *p + (float) *(p+1))/0x7FFF;
        *(buf++) = (v < -1) ? -1 : ((v > 1) ? 1 : v);
    }
            
}


int main(int argc, char *argv[]) {
    struct resample_state resample;
    struct qbuf qi, qo;
    
    if (argc > 1) {
        stdin = fopen(argv[1], "r");
        assert(stdin);
    }

    qbuf_init(&qi, BUFSIZE);
    qbuf_init(&qo, BUFSIZE);
    resample_init(&resample);
    
    while (!feof(stdin)) {
        int c;
        float *ip, *up;
        size_t il, ol;
        
        fprintf(stderr, "LOOP!\n");

        ip = qbuf_push(&qi, &il);
        
        c = fread(ip, sizeof(float), il/sizeof(float), stdin);
        convbuf(ip, c);
        qbuf_push_validate(&qi, c*sizeof(float));

        resample_get(&resample, &qi, &qo, 44100, 8000);

        up = qbuf_pull(&qo, &ol);
        fwrite(up, sizeof(float), ol/sizeof(float), stdout);
        qbuf_pull_invalidate(&qo, ol);
    }

    resample_done(&resample);
    qbuf_done(&qi);
    qbuf_done(&qo);
    
    return 0;
}

#endif