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

#include "interpol.h"
#include "util.h"

void interpol_init(struct interpol_state *s, int frac, int radius) {
    int l;
    assert(s && frac);

    memset(s, 0, sizeof(struct interpol_state));

    s->frac = frac;

    s->radius = radius;
    s->coeff = malloc((l = s->radius*frac)*sizeof(float));
    s->coeff_valid = malloc(l*sizeof(int));

    assert(s->coeff_valid && s->coeff);

    memset(s->coeff_valid, 0, l*sizeof(int));
}

void interpol_done(struct interpol_state *s) {
    assert(s);
    free(s->coeff);
    free(s->coeff_valid);
}

static float sinc(struct interpol_state *s, int x) {
    assert(s);

    if (x == 0)
        return 1;

    if (x < 0)
        x = -x;

    if (x >= s->frac * s->radius)
        return 0;

    if (!s->coeff_valid[x]) {
        float fx = ((float) x)/s->frac;
        float hanning = .5*(1+cos(M_PI * fx / s->radius));
        float _sinc = sin(M_PI * fx) / M_PI / fx;

        /*fprintf(stderr, "%2.10f %2.10f %2.10f\n", fx, hanning, _sinc);*/

        assert(_sinc <= 1);
        assert(_sinc >= -1);
        assert(hanning >= 0);
        assert(hanning <= 1);

        
        s->coeff[x] = hanning * _sinc;
        s->coeff_valid[x] = 1;
    }

    return s->coeff[x];
}

float interpol_get(struct interpol_state *s, float *p, int l, float x) {
    int i, j, z, d, n;
    float sum;

    assert(s && p && l);

    i = (int) roundf(x*s->frac);          /* x in units of 1/fracs */
    z = i/s->frac;                       /* index of sample left of x */
    j = z*s->frac;                       /* index of sample left of x in units of 1/fracs */
    d = i-j;

    sum = 0;
    
    for (n = -s->radius; n <= s->radius; n++) {
        int k = z + n;                /* sample of current sinc pulse */

        if (k < 0 || k >= l)
            fprintf(stderr, "interpol.c: index out of bound: %i not in 0...%i\n", k, l);
        else {
/*
            char txt[2];
            snprintf(txt, sizeof(txt), "%f", p[k]);
            fprintf(stderr, "%i %i\r", k, l);
*/          
            sum += p[k] * sinc(s, -n*s->frac + d);
        }
    }

    return sum;
}

#if (TEST == 5)

int main() {
    struct interpol_state s;
    int x;
    int frac = 9;

    interpol_init(&s, frac);
    
    for (x = -10*frac; x <= 10*frac; x ++) {
        fprintf(stderr, "%2.5f -> %2.5f\n", (float)x/frac, sinc(&s, x));
    }

    interpol_done(&s);

    return 0;
}

#endif

#if (TEST == 6)

int main() {
    struct interpol_state s;
    float buf[1024], x;
    int i, frac = 37;

    interpol_init(&s, frac);
    
    for (i = 0; i < 1024; i++)
        buf[i] = 1;

    for (x = 0; x < 1024; x += 0.1) {
        float y = interpol_get(&s, buf, 1024, x);
        //loop_write(1, &y, sizeof(float));
        fprintf(stderr, "%2.10f => %2.10f\n", x, y);
    }
    
    interpol_done(&s);

    return 0;
}

#endif