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

#include "v17mod.h"

#define SAMPLE_RATE 8000
#define CARRIER_FREQ 1800
#define BAUD_RATE 2400
#define SYMBOL_INTERVAL ((float) SAMPLE_RATE/BAUD_RATE)
#define SINC_RADIUS 5
#define SYMBOLS_MAX 1024

static const float v17_7k2_x[] = {
    0.6, -0.2, 0.6, -0.6,
    -0.6, 0.2, -0.6, 0.6,
    -0.2, 0.6, -0.2, 0.2,
    0.2, -0.6, 0.2, -0.2
};

static const float v17_7k2_y[] = {
    -0.6, 0.6, 0.2, -0.6,
    0.6, -0.6, -0.2, 0.6,
    0.2, -0.2, -0.6, 0.2,
    -0.2, 0.2, 0.6, -0.2
};

void v17mod_init(struct v17mod_state *s) {
    size_t l;
    int i;
    
    assert(s);
    memset(s, 0, sizeof(*s));

    s->xsymbols = malloc(l = (sizeof(float)*SYMBOLS_MAX));
    s->ysymbols = malloc(l);

    s->n_symbols = SINC_RADIUS*2;

    for (i = 0; i < s->n_symbols; i++)
        s->xsymbols[i] = s->ysymbols[i] = 0.0;
}

void v17mod_done(struct v17mod_state *s) {
    assert(s);
    free(s->xsymbols);
    free(s->ysymbols);
}

static const float sinc_table[] = {
    0.0000, -0.0000, -0.0002, -0.0005, -0.0009,
   -0.0014, -0.0018, -0.0020, -0.0019, -0.0013,
    0.0000,  0.0019,  0.0042,  0.0068,  0.0093,
    0.0113,  0.0124,  0.0120,  0.0098,  0.0058,
   -0.0000, -0.0073, -0.0153, -0.0234, -0.0303,
   -0.0350, -0.0365, -0.0340, -0.0270, -0.0155,
    0.0000,  0.0183,  0.0377,  0.0562,  0.0716,
    0.0815,  0.0838,  0.0771,  0.0605,  0.0344,
   -0.0000, -0.0403, -0.0832, -0.1243, -0.1589,
   -0.1821, -0.1893, -0.1767, -0.1415, -0.0824,
    0.0000,  0.1035,  0.2241,  0.3561,  0.4926,
    0.6261,  0.7488,  0.8533,  0.9330,  0.9830,
    1.0000,  0.9830,  0.9330,  0.8533,  0.7488,
    0.6261,  0.4926,  0.3561,  0.2241,  0.1035,
    0.0000, -0.0824, -0.1415, -0.1767, -0.1893,
   -0.1821, -0.1589, -0.1243, -0.0832, -0.0403,
   -0.0000,  0.0344,  0.0605,  0.0771,  0.0838,
    0.0815,  0.0716,  0.0562,  0.0377,  0.0183,
    0.0000, -0.0155, -0.0270, -0.0340, -0.0365,
   -0.0350, -0.0303, -0.0234, -0.0153, -0.0073,
   -0.0000,  0.0058,  0.0098,  0.0120,  0.0124,
    0.0113,  0.0093,  0.0068,  0.0042,  0.0019,
    0.0000, -0.0013, -0.0019, -0.0020, -0.0018,
   -0.0014, -0.0009, -0.0005, -0.0002, -0.0000,
   -0.0000
};

static float sinc(float x) {
    int i;
    assert(sizeof(sinc_table)/sizeof(float) == 121);

    i = (int) (60+x*10+.5);

    if (i < 0)
        return 0;
    if (i > 120)
        return 0;

    return sinc_table[i];
}


void v17mod_push(struct v17mod_state *s, const uint8_t *p, int l) {
    int i, j;

    assert(s && p && l);
    assert(s->n_symbols+l <= SYMBOLS_MAX);

    for (i = 0, j = (s->i_symbols+s->n_symbols) % SYMBOLS_MAX; i < l; i++, j = (j+1) % SYMBOLS_MAX) {
        assert(p[i] < 16);
        s->xsymbols[j] = v17_7k2_x[p[i]];
        s->ysymbols[j] = v17_7k2_y[p[i]];
    }

    s->n_symbols += l;
}

inline static float symbol_at(struct v17mod_state *s, float *a, int i) {
    int j = s->i_symbols+i;

    assert(i >= 0);
    assert(i < s->n_symbols);

    while(j >= SYMBOLS_MAX)
        j -= SYMBOLS_MAX;

    assert(j>=0);
    assert(j<SYMBOLS_MAX);
    
    return a[j];
}

static float fsignal(struct v17mod_state *s, int i) {
    float sum, d;
    int n;
    float *symbols;
    
    assert(s && i >= 0 && i <= 1);

    symbols = (i == 0) ? s->xsymbols : s->ysymbols;
    sum = 0;
    d = s->sample - s->symbol_index*SAMPLE_RATE/BAUD_RATE;
    
    for (n = -SINC_RADIUS; n < SINC_RADIUS; n++) {
        float a,b;
        a = symbol_at(s, symbols, n+SINC_RADIUS);
        b = sinc(-n + d*BAUD_RATE/SAMPLE_RATE);
        sum +=  a*b; 
    }
    assert(sum<=10);
    assert(sum >=-10);
    
    return sum;
}

int v17mod_pull(struct v17mod_state *s, float *p, int l) {
    assert(s && p && l && p && l);

    int n_samples = 0;
    
    for (;;) {
        float x;
        int nsi;

        if (s->sample % 800 == 0) 
             fprintf(stderr, "seconds: %i; samples: %i; symbols: %i          \r", s->seconds, s->sample, s->symbol_index); 
        
        if (!l) {
            fprintf(stderr, "v17_mod(): destination buffer too short\n");
            return -1;
        }

        nsi = (int) ((float) s->sample*BAUD_RATE/SAMPLE_RATE);
        if (nsi != s->symbol_index) {
            s->i_symbols++;
            s->n_symbols--;

            while (s->i_symbols >= SYMBOLS_MAX)
                s->i_symbols -= SYMBOLS_MAX;
            
            s->symbol_index = nsi;
        }

        if (s->n_symbols < SINC_RADIUS*2)
            return n_samples;

        if (s->sample == SAMPLE_RATE) {
            assert(s->symbol_index == BAUD_RATE);

            s->symbol_index = s->sample = 0;
            s->seconds++;
        }
        
        
        x = (s->sample % SAMPLE_RATE) * M_PI*2 * CARRIER_FREQ / SAMPLE_RATE;
        
        *(p++) = fsignal(s, 0)*sin(x) + fsignal(s, 1)*cos(x);
        l--;

        s->sample++;
        n_samples++;
    }
}


#if (TEST == 3)

int main() {
    fprintf(stderr, "%f\n", sinc(1.0/3));

    return 0;
}

#endif