Change implementation for a bit more clear

1 files changed, 65 insertions, 57 deletions

diff --git a/src/pqf.c b/src/pqf.c
index 50657cc..f46c48b 100644
--- a/src/pqf.c
+++ b/src/pqf.c
@@ -16,30 +16,28 @@
  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  */
 
-/*
- * Musepack pqf implementation was used as reference.
- * see svn.musepack.net
- */
-
 #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* m;
+    float* cos_tab;
+    uint16_t cos_tab_mask;
 
     uint16_t proto_sz;
     uint16_t subbands_num;
     uint16_t subband_size;
 
-    uint16_t proto_step;
+    uint16_t pos;
 };
 
 static inline uint16_t get_frame_sz(pqf_a_ctx_t ctx)
@@ -53,59 +51,69 @@ static void init(pqf_a_ctx_t ctx)
     const float* const t = ctx->proto;
     uint16_t subbands_num = ctx->subbands_num; 
     float* c = ctx->c;
-    float* m = ctx->m;
     uint16_t dsb = subbands_num * 2;
-    uint16_t proto_step = ctx->proto_step;
 
-    for (i = 0; i < subbands_num; i++) {
-        for (j = 0; j < subbands_num; j++) {
-            m[i * subbands_num + j] = (float)cos(((2 * i + 1) * j & 127) * M_PI / (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 < proto_step; i++) {
-        for (j = 0; j < dsb; j++) {
-            if (i & 1) {
-                c[j * proto_step + i] = 1 * t[j + dsb * i];
-            } else {
-                c[j * proto_step + i] = -1 * t[j + dsb * i];
-            }
-        }
+
+    for (i = 0; i < ctx->proto_sz; i++) {
+        float sign = i & (1 << 6) ? -1.0 : 1.0;
+        c[i] = t[i] * sign;
     }
 }
 
-static void vectoring(const float* x, float* y, pqf_a_ctx_t ctx)
+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 subbands_num = ctx->subbands_num;
     uint16_t dsb = ctx->subbands_num << 1;
-    uint16_t proto_step = ctx->proto_step;
+    uint16_t proto_sz = ctx->proto_sz;
+    uint16_t mask = dsb - 1;
 
-    for (i = 0; i < dsb; i++, c += proto_step, x += 1, y += 1 ) {
-        y[0] = 0;
-        for (j = 0; j < proto_step; j++) {
-            y[0] += c[j] * x[j * dsb];
-        }
+    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 matrixing(const float* y, float* samples, pqf_a_ctx_t ctx)
+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;
-    const float* mi = ctx->m;
 
-    for (i = 0; i < subbands_num; i++, mi += subbands_num, samples += subband_size) {
-        for (j = 0, k = subbands_num / 2; j < subbands_num / 2; j++, k--) {
-            samples[0] += (y[j] + y[subbands_num - j]) * mi[k];
+    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);
         }
-        for (j = 1; j < subbands_num / 2; j++) {
-            samples[0] += (y[subbands_num + j] - y[subbands_num * 2 - j]) * mi[subbands_num/2  + j];
-        }
-	samples[0] += y[subbands_num/2];
+
+        samples[0] = ((i + 1) & 2) ? -resp : resp;
     }
 }
 
@@ -154,9 +162,9 @@ pqf_status_t pqf_create_a_ctx(uint16_t subband_sz, uint16_t subbands_num, uint16
     ctx->proto_sz = proto_sz;
     ctx->subbands_num = subbands_num;
     ctx->subband_size = subband_sz;
-    ctx->proto_step = proto_sz / (subbands_num << 1);
+    ctx->pos = 0;
 
-    ctx->buf = (float*)malloc(sizeof(float) * buf_sz);
+    ctx->buf = (float*)calloc(buf_sz, sizeof(float));
 
     if (!ctx->buf)
         goto nomem_buf;
@@ -182,10 +190,10 @@ pqf_status_t pqf_create_a_ctx(uint16_t subband_sz, uint16_t subbands_num, uint16
     if (!ctx->c)
         goto nomem_c;
 
-    ctx->m = (float*)malloc(sizeof(float) * subbands_num * subbands_num);
+    ctx->cos_tab = (float*)calloc(subbands_num * subbands_num * 2, sizeof(float));
 
-    if (!ctx->c)
-        goto nomem_m;
+    if (!ctx->cos_tab)
+        goto nomem_cos_tab;
 
     a_init(ctx);
 
@@ -193,7 +201,7 @@ pqf_status_t pqf_create_a_ctx(uint16_t subband_sz, uint16_t subbands_num, uint16
 
     return PQF_SUCCESS;
 
-nomem_m:
+nomem_cos_tab:
     free(ctx->c);
 
 nomem_c:
@@ -228,7 +236,6 @@ uint16_t pqf_get_subbands_num(pqf_a_ctx_t ctx)
 
 void pqf_free_a_ctx(pqf_a_ctx_t ctx)
 {
-    free(ctx->m);
     free(ctx->y);
     free(ctx->c);
     free(ctx->proto);
@@ -241,8 +248,8 @@ void pqf_do_analyse(pqf_a_ctx_t ctx, const float* in, float* out)
     float* y = ctx->y;
 
     float* x;
-    const float* pcm;
     uint16_t n, i;
+    uint16_t pos = ctx->pos;
 
     float* buf = ctx->buf;
     uint16_t subband_size = ctx->subband_size;
@@ -250,18 +257,19 @@ void pqf_do_analyse(pqf_a_ctx_t ctx, const float* in, float* out)
     uint16_t frame_sz = subband_size * subbands_num;
     uint16_t extra_sz = ctx->proto_sz - subbands_num;
 
-    memcpy(buf + frame_sz, buf, extra_sz * sizeof(float));
-    x = buf + frame_sz;
+    for (n = 0; n < subband_size; n++) {
+        vectoring2(buf, pos, y, ctx);
+        matrixing2(y, &out[n], ctx);
 
-    pcm = in + (subbands_num - 1);
-
-    for (n = 0; n < subband_size; n++, pcm += subbands_num * 2) {
-        x -= subbands_num;
-        for (i = 0; i < subbands_num; i++) {
-            x[i] = *pcm--;
+	for (i = 0; i < subbands_num; i++) {
+	    buf[i + pos] = in[n * subbands_num + i];
         }
-        vectoring(x, y, ctx);
-        matrixing (y, &out[n], ctx);
+
+	pos = pos + subbands_num;
+	if (pos >= ctx->proto_sz) {
+		pos = 0;
+	}
     }
+    ctx->pos = pos;
 }