import config, filter

"""
quadratic spline wavelet with one vanishing moment and compact support
[Mallat&Zhong, 1992]
"""
"""
indata, instep, n - input data
"""

double h0, h1, h2, h3
double g0, g1
static bool got_constants

Filter *def mallat_qsw_get_filter():
    Filter *f = ALLOCATE0(sizeof *f)
    f->hn = 4
    f->gn = 2
    f->h = malloc(f->hn * sizeof(V))
    f->g = malloc(f->gn * sizeof(V))

    mallat_qsw_init()
    f->h[0] = h0
    f->h[1] = h1
    f->h[2] = h2
    f->h[3] = h3
    f->g[0] = g0
    f->g[1] = g1
    
    return f

def mallat_qsw_init():
    double s = sqrt(2)

    h0 = 0.125 * s
    h1 = 0.375 * s
    h2 = 0.375 * s
    h3 = 0.125 * s

    g0 = 0.5 * s
    g1 = -0.5 * s

    got_constants = true

def qsw_step_unscaled(V const *indata, int n, instep,
    V *outlow, int lowstep, V *outhigh, int highstep):
    """
    Performs wavelet transform, without downsampling.
    """

    double S = sqrt(2)
    double H[] = {0.125 * S, 0.375 * S, 0.375 * S, 0.125 * S}
    double G[]= {0.5 * S, -0.5 * S}

    V const *s = indata

    # Highpass filter (with cyclic border handling)

    int hi = 0
    outhigh[hi] = s[0] * G[0] + s[instep * (n - 1)] * G[1]
    hi += highstep
    for int i = 1; i < n; i++:
        outhigh[hi] = s[instep * i] * G[0] + s[instep * (i - 1)] * G[1]
        hi += highstep

    # Lowpass filter (with cyclic border handling)

    int li = 0
    outlow[li] = s[instep] * H[0] + s[0] * H[1] + s[instep * (n - 1)] * H[2] + s[instep * (n - 2)] * H[3]
    li += lowstep
    outlow[li] = s[instep * 2] * H[0] + s[instep] * H[1] + s[0] * H[2] + s[instep * (n - 1)] * H[3]
    li += lowstep
    int ii = 2 * instep
    for int i = 2; i < n - 1; i++:
        outlow[li] = s[ii + instep] * H[0] + s[ii] * H[1] + s[ii - instep] * H[2] + s[ii - 2 * instep] * H[3]
        li += lowstep
        ii += instep
    outlow[li] = s[0] * H[0] + s[instep * (n - 1)] * H[1] + s[instep * (n - 2)] * H[2] + s[instep * (n - 3)] * H[3]
    li += lowstep

def qsw_step_scaled(V const *indata, int n, instep,
    V *outlow, int lowstep, V *outhigh, int highstep,
    int scale):
    """
    This version has a dyadic scale parameter for the filters.
    """

    double S = sqrt(2)
    double Hu[] = {0.125 * S, 0.375 * S, 0.375 * S, 0.125 * S}
    double Gu[]= {0.5 * S, -0.5 * S}

    int hn = 4 * (1 << scale)
    int gn = 2 * (1 << scale)

    double H[hn]
    double G[gn]

    for int i = 0; i < hn; i++: H[i] = 0
    H[0 << scale] = Hu[0]
    H[1 << scale] = Hu[1]
    H[2 << scale] = Hu[2]
    H[3 << scale] = Hu[3]

    for int i = 0; i < gn; i++: G[i] = 0
    G[0 << scale] = Gu[0]
    G[1 << scale] = Gu[1]

    V const *s = indata

    # Highpass filter (with cyclic border handling)

    int hi = 0
    for int i = 0; i < n; i++:
        outhigh[hi] = 0
        for int j = 0; j < gn; j++:
            outhigh[hi] += s[instep * ((unsigned)(i + gn / 2 - j) % n)] * G[j]
        hi += highstep

    # Lowpass filter (with cyclic border handling)

    int li = 0
    for int i = 0; i < n; i++:
        outlow[li] = 0
        for int j = 0; j < hn; j++:
            outlow[li] += s[instep * ((unsigned)(i + hn / 2 - j) % n)] * H[j]
        li += lowstep

def qsw_transform_no_key(V const *indata, int n, instep,
    V *outlow, int lowstep, V *outhigh, int highstep, char const *key):
    V templow[n], temphigh[n]
    qsw_step_unscaled(indata, n, instep, templow, 1, temphigh, 1)
    V *lo = outlow
    V *hi = outhigh
    for int i = 0; i < n; i += 2:
        *lo = (templow[i] + templow[i + 1]) * 0.5
        *hi = (temphigh[i] + temphigh[i + 1]) * 0.5
        lo += lowstep
        hi += highstep

V *def qsw(V const *s, int n, uint32_t which):
    # n = 512
    #
    """
    'which' is the subband to get. The highest bit tells the depth, the
    remaining bits select the subband.
    For example 9, binary 1010, means third decomposition depth, and the
    remaining 010 means for low, then high, then again lowpass.
    (Starting with high bits, going left to low bits.)

    Since our signals have 512 samples, we have at most 9 subdivision.

    Returns an array with twice as many signals as dyadic steps, with
    alternating (full-length) wavelet transformation results for each scale,
    always lowpass followed by highpass.

    """
    #int l = ceil(log2(n));
    V temp[n]

    uint32_t l = 1

    while (1u << l) <= which:
        l++
    l -= 1
    which &= (1 << l) - 1
    V const *p = s
    int read = 1 << (l - 1)
    for uint32_t i = 0; i < l; i++:
        V result_lo[n]
        V result_hi[n]
        qsw_step_scaled(p, n, 1, result_lo, 1, result_hi, 1, i)
        if which & read:
            printf("H")
            memcpy(temp, result_hi, n * sizeof(V))
        else:
            printf("L")
            memcpy(temp, result_lo, n * sizeof(V))
        p = temp
        read >>= 1
    if l > 0: printf("\n")

    void *r = ALLOCATE(n * sizeof(V))
    memcpy(r, p, n * sizeof(V))
    return r