path: root/src/pqf.c

/*
 * This file is part of libpqf.
 *
 * libpqf is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * AtracDEnc is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with AtracDEnc; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 */

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

#include "libpqf.h"

#define COS_T(x) (ctx->cos_tab[(x) & ctx->cos_tab_mask])

struct pqf_a_ctx {
    float* buf;
    float* proto;
    float* y;
    float* c;
    float* cos_tab;
    uint16_t cos_tab_mask;

    uint16_t proto_sz;
    uint16_t subbands_num;
    uint16_t subband_size;

    uint16_t pos;
};

static inline uint16_t get_frame_sz(pqf_a_ctx_t ctx)
{
    return ctx->subbands_num * ctx->subband_size;
}

static void init(pqf_a_ctx_t ctx)
{
    uint16_t i, j;
    const float* const t = ctx->proto;
    uint16_t subbands_num = ctx->subbands_num; 
    float* c = ctx->c;
    uint16_t dsb = subbands_num * 2;

    ctx->cos_tab[0] = 1.0;
    ctx->cos_tab[subbands_num * subbands_num / 2] = 0;
    ctx->cos_tab[subbands_num * subbands_num] = -ctx->cos_tab[0];
    for (i = 1; i < (subbands_num * subbands_num / 2); i++) {
        ctx->cos_tab[i]   = cos(M_PI * i / (subbands_num * subbands_num));
        ctx->cos_tab[(subbands_num * subbands_num) - i] = -ctx->cos_tab[i];
        ctx->cos_tab[subbands_num * subbands_num + i] = -ctx->cos_tab[i];
        ctx->cos_tab[subbands_num * subbands_num * 2 - i] = +ctx->cos_tab[i];
    }
    ctx->cos_tab_mask = subbands_num * subbands_num * 2 - 1;


    for (i = 0; i < ctx->proto_sz; i++) {
        float sign = i & (1 << 6) ? -1.0 : 1.0;
        c[i] = t[i] * sign;
    }
}

static void vectoring2(const float* x, uint16_t pos, float* y, pqf_a_ctx_t ctx)
{
    uint16_t i, j;
    uint16_t k = 0;

    const float* c = ctx->c;
    uint16_t dsb = ctx->subbands_num << 1;
    uint16_t proto_sz = ctx->proto_sz;
    uint16_t mask = dsb - 1;

    memset(y, 0, sizeof(float) * dsb);

    for (k = 0, i = pos, j = 0;
            i < proto_sz; k = (k + 1) & mask, i++, j++) {
        y[k] += x[i] * c[j];
    }

    for (i = 0; i < pos; k = (k + 1) & mask, i++, j++) {
        y[k] += x[i] * c[j];
    }

}

static void matrixing2(float* y, float* samples, pqf_a_ctx_t ctx)
{
    uint16_t i, j, k;

    uint16_t subbands_num = ctx->subbands_num;
    uint16_t subband_size = ctx->subband_size;

    for (k = subbands_num / 2; k < subbands_num; k++)
        y[k] = y[k] - y[(subbands_num - 1) - k];
    for (k = subbands_num; k < (subbands_num + subbands_num / 2); k++)
        y[k] = y[k] + y[(subbands_num * 3 - 1) - k];
    
    for (i = 0; i < subbands_num; i++, samples += subband_size) {
        float resp = 0;
        for (j = subbands_num / 2; j < (subbands_num + subbands_num / 2); j++) {
            int s = (2 * i + 1) * (2 * (j + subbands_num / 2) + 1);
            resp += y[j] * COS_T(s << 3);
        }

        samples[0] = ((i + 1) & 2) ? -resp : resp;
    }
}

static void a_init(pqf_a_ctx_t ctx)
{
    init(ctx);
}

static pqf_status_t check_params(uint16_t subband_sz, uint16_t subbands_num, uint16_t proto_sz)
{
    if (!subbands_num || subbands_num > 128) {
        return PQF_WRONG_SUBBANDS_NUM;
    }

    if (!proto_sz || proto_sz < subbands_num) {
        return PQF_WRONG_SUBBAND_SZ;
    }

    if (!subband_sz || subband_sz > (0x7fff / subbands_num)) {
        return PQF_FRAME_TOO_LONG;
    }

    return PQF_SUCCESS;
}

pqf_status_t pqf_create_a_ctx(uint16_t subband_sz, uint16_t subbands_num, uint16_t proto_sz, const float* proto, pqf_a_ctx_t* ctx_result)
{
    uint16_t i = 0;

    uint16_t frame_sz = subband_sz * subbands_num;
    uint16_t extra_sz = proto_sz - subbands_num;
    uint16_t buf_sz = frame_sz + extra_sz;
    pqf_status_t status;

    if (ctx_result == NULL)
        return PQF_CONTRACT_VIOLATION;

    if ((status = check_params(subband_sz, subbands_num, proto_sz)) != PQF_SUCCESS)
        return status;

    pqf_a_ctx_t ctx = (pqf_a_ctx_t)malloc(sizeof(struct pqf_a_ctx));

    if (!ctx)
        return PQF_NOMEM;

    ctx->proto_sz = proto_sz;
    ctx->subbands_num = subbands_num;
    ctx->subband_size = subband_sz;
    ctx->pos = 0;

    ctx->buf = (float*)calloc(buf_sz, sizeof(float));

    if (!ctx->buf)
        goto nomem_buf;

    for (i = 0; i < buf_sz; i++) {
        ctx->buf[i] = 0.0;
    }

    ctx->proto = (float*)malloc(sizeof(float) * proto_sz);

    if (!ctx->proto)
        goto nomem_proto;

    memcpy(ctx->proto, proto, sizeof(float) * proto_sz);

    ctx->y = (float*)malloc(sizeof(float) * subbands_num * 2);

    if (!ctx->y)
        goto nomem_y;

    ctx->c = (float*)malloc(sizeof(float) * proto_sz);

    if (!ctx->c)
        goto nomem_c;

    ctx->cos_tab = (float*)calloc(subbands_num * subbands_num * 2, sizeof(float));

    if (!ctx->cos_tab)
        goto nomem_cos_tab;

    a_init(ctx);

    *ctx_result = ctx;

    return PQF_SUCCESS;

nomem_cos_tab:
    free(ctx->c);

nomem_c:
    free(ctx->y);

nomem_y:
    free(ctx->proto);

nomem_proto:
    free(ctx->buf);

nomem_buf:
    free(ctx);

    return PQF_NOMEM;
}

uint16_t pqf_get_frame_sz(pqf_a_ctx_t ctx)
{
    return get_frame_sz(ctx);
}

uint16_t pqf_get_subband_sz(pqf_a_ctx_t ctx)
{
    return ctx->subband_size;
}

uint16_t pqf_get_subbands_num(pqf_a_ctx_t ctx)
{
    return ctx->subbands_num;
}

void pqf_free_a_ctx(pqf_a_ctx_t ctx)
{
    free(ctx->y);
    free(ctx->c);
    free(ctx->proto);
    free(ctx->buf);
    free(ctx);
}

void pqf_do_analyse(pqf_a_ctx_t ctx, const float* in, float* out)
{
    float* y = ctx->y;

    float* x;
    uint16_t n, i;
    uint16_t pos = ctx->pos;

    float* buf = ctx->buf;
    uint16_t subband_size = ctx->subband_size;
    uint16_t subbands_num = ctx->subbands_num;
    uint16_t frame_sz = subband_size * subbands_num;
    uint16_t extra_sz = ctx->proto_sz - subbands_num;

    for (n = 0; n < subband_size; n++) {
        vectoring2(buf, pos, y, ctx);
        matrixing2(y, &out[n], ctx);

	for (i = 0; i < subbands_num; i++) {
	    buf[i + pos] = in[n * subbands_num + i];
        }

	pos = pos + subbands_num;
	if (pos >= ctx->proto_sz) {
		pos = 0;
	}
    }
    ctx->pos = pos;
}