src/lib/doslib/ext/faad/ps_dec.c

   1 /*
   2 ** FAAD2 - Freeware Advanced Audio (AAC) Decoder including SBR decoding
   3 ** Copyright (C) 2003-2005 M. Bakker, Nero AG, http://www.nero.com
   4 **
   5 ** This program is free software; you can redistribute it and/or modify
   6 ** it under the terms of the GNU General Public License as published by
   7 ** the Free Software Foundation; either version 2 of the License, or
   8 ** (at your option) any later version.
   9 **
  10 ** This program is distributed in the hope that it will be useful,
  11 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 ** GNU General Public License for more details.
  14 **
  15 ** You should have received a copy of the GNU General Public License
  16 ** along with this program; if not, write to the Free Software
  17 ** Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  18 **
  19 ** Any non-GPL usage of this software or parts of this software is strictly
  20 ** forbidden.
  21 **
  22 ** The "appropriate copyright message" mentioned in section 2c of the GPLv2
  23 ** must read: "Code from FAAD2 is copyright (c) Nero AG, www.nero.com"
  24 **
  25 ** Commercial non-GPL licensing of this software is possible.
  26 ** For more info contact Nero AG through Mpeg4AAClicense@nero.com.
  27 **
  28 ** $Id: ps_dec.c,v 1.16 2009/01/26 22:32:31 menno Exp $
  29 **/
  30
  31 #include "common.h"
  32
  33 #ifdef PS_DEC
  34
  35 #include <stdlib.h>
  36 #include "ps_dec.h"
  37 #include "pstables.h"
  38
  39 /* constants */
  40 #define NEGATE_IPD_MASK            (0x1000)
  41 #define DECAY_SLOPE                FRAC_CONST(0.05)
  42 #define COEF_SQRT2                 COEF_CONST(1.4142135623731)
  43
  44 /* tables */
  45 /* filters are mirrored in coef 6, second half left out */
  46 static const real_t p8_13_20[7] =
  47 {
  48     FRAC_CONST(0.00746082949812),
  49     FRAC_CONST(0.02270420949825),
  50     FRAC_CONST(0.04546865930473),
  51     FRAC_CONST(0.07266113929591),
  52     FRAC_CONST(0.09885108575264),
  53     FRAC_CONST(0.11793710567217),
  54     FRAC_CONST(0.125)
  55 };
  56
  57 static const real_t p2_13_20[7] =
  58 {
  59     FRAC_CONST(0.0),
  60     FRAC_CONST(0.01899487526049),
  61     FRAC_CONST(0.0),
  62     FRAC_CONST(-0.07293139167538),
  63     FRAC_CONST(0.0),
  64     FRAC_CONST(0.30596630545168),
  65     FRAC_CONST(0.5)
  66 };
  67
  68 static const real_t p12_13_34[7] =
  69 {
  70     FRAC_CONST(0.04081179924692),
  71     FRAC_CONST(0.03812810994926),
  72     FRAC_CONST(0.05144908135699),
  73     FRAC_CONST(0.06399831151592),
  74     FRAC_CONST(0.07428313801106),
  75     FRAC_CONST(0.08100347892914),
  76     FRAC_CONST(0.08333333333333)
  77 };
  78
  79 static const real_t p8_13_34[7] =
  80 {
  81     FRAC_CONST(0.01565675600122),
  82     FRAC_CONST(0.03752716391991),
  83     FRAC_CONST(0.05417891378782),
  84     FRAC_CONST(0.08417044116767),
  85     FRAC_CONST(0.10307344158036),
  86     FRAC_CONST(0.12222452249753),
  87     FRAC_CONST(0.125)
  88 };
  89
  90 static const real_t p4_13_34[7] =
  91 {
  92     FRAC_CONST(-0.05908211155639),
  93     FRAC_CONST(-0.04871498374946),
  94     FRAC_CONST(0.0),
  95     FRAC_CONST(0.07778723915851),
  96     FRAC_CONST(0.16486303567403),
  97     FRAC_CONST(0.23279856662996),
  98     FRAC_CONST(0.25)
  99 };
 100
 101 #ifdef PARAM_32KHZ
 102 static const uint8_t delay_length_d[2][NO_ALLPASS_LINKS] = {
 103     { 1, 2, 3 } /* d_24kHz */,
 104     { 3, 4, 5 } /* d_48kHz */
 105 };
 106 #else
 107 static const uint8_t delay_length_d[NO_ALLPASS_LINKS] = {
 108     3, 4, 5 /* d_48kHz */
 109 };
 110 #endif
 111 static const real_t filter_a[NO_ALLPASS_LINKS] = { /* a(m) = exp(-d_48kHz(m)/7) */
 112     FRAC_CONST(0.65143905753106),
 113     FRAC_CONST(0.56471812200776),
 114     FRAC_CONST(0.48954165955695)
 115 };
 116
 117 static const uint8_t group_border20[10+12 + 1] =
 118 {
 119     6, 7, 0, 1, 2, 3, /* 6 subqmf subbands */
 120     9, 8,             /* 2 subqmf subbands */
 121     10, 11,           /* 2 subqmf subbands */
 122     3, 4, 5, 6, 7, 8, 9, 11, 14, 18, 23, 35, 64
 123 };
 124
 125 static const uint8_t group_border34[32+18 + 1] =
 126 {
 127      0,  1,  2,  3,  4,  5,  6,  7,  8,  9,  10, 11, /* 12 subqmf subbands */
 128      12, 13, 14, 15, 16, 17, 18, 19,                 /*  8 subqmf subbands */
 129      20, 21, 22, 23,                                 /*  4 subqmf subbands */
 130      24, 25, 26, 27,                                 /*  4 subqmf subbands */
 131      28, 29, 30, 31,                                 /*  4 subqmf subbands */
 132      32-27, 33-27, 34-27, 35-27, 36-27, 37-27, 38-27, 40-27, 42-27, 44-27, 46-27, 48-27, 51-27, 54-27, 57-27, 60-27, 64-27, 68-27, 91-27
 133 };
 134
 135 static const uint16_t map_group2bk20[10+12] =
 136 {
 137     (NEGATE_IPD_MASK | 1), (NEGATE_IPD_MASK | 0),
 138     0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19
 139 };
 140
 141 static const uint16_t map_group2bk34[32+18] =
 142 {
 143     0,  1,  2,  3,  4,  5,  6,  6,  7, (NEGATE_IPD_MASK | 2), (NEGATE_IPD_MASK | 1), (NEGATE_IPD_MASK | 0),
 144     10, 10, 4,  5,  6,  7,  8,  9,
 145     10, 11, 12, 9,
 146     14, 11, 12, 13,
 147     14, 15, 16, 13,
 148     16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33
 149 };
 150
 151 /* type definitions */
 152 typedef struct
 153 {
 154     uint8_t frame_len;
 155     uint8_t resolution20[3];
 156     uint8_t resolution34[5];
 157
 158     qmf_t *work;
 159     qmf_t **buffer;
 160     qmf_t **temp;
 161 } hyb_info;
 162
 163 /* static function declarations */
 164 static void ps_data_decode(ps_info *ps);
 165 static hyb_info *hybrid_init(uint8_t numTimeSlotsRate);
 166 static void channel_filter2(hyb_info *hyb, uint8_t frame_len, const real_t *filter,
 167                             qmf_t *buffer, qmf_t **X_hybrid);
 168 static void INLINE DCT3_4_unscaled(real_t *y, real_t *x);
 169 static void channel_filter8(hyb_info *hyb, uint8_t frame_len, const real_t *filter,
 170                             qmf_t *buffer, qmf_t **X_hybrid);
 171 static void hybrid_analysis(hyb_info *hyb, qmf_t X[32][64], qmf_t X_hybrid[32][32],
 172                             uint8_t use34, uint8_t numTimeSlotsRate);
 173 static void hybrid_synthesis(hyb_info *hyb, qmf_t X[32][64], qmf_t X_hybrid[32][32],
 174                              uint8_t use34, uint8_t numTimeSlotsRate);
 175 static int8_t delta_clip(int8_t i, int8_t min, int8_t max);
 176 static void delta_decode(uint8_t enable, int8_t *index, int8_t *index_prev,
 177                          uint8_t dt_flag, uint8_t nr_par, uint8_t stride,
 178                          int8_t min_index, int8_t max_index);
 179 static void delta_modulo_decode(uint8_t enable, int8_t *index, int8_t *index_prev,
 180                                 uint8_t dt_flag, uint8_t nr_par, uint8_t stride,
 181                                 int8_t and_modulo);
 182 static void map20indexto34(int8_t *index, uint8_t bins);
 183 #ifdef PS_LOW_POWER
 184 static void map34indexto20(int8_t *index, uint8_t bins);
 185 #endif
 186 static void ps_data_decode(ps_info *ps);
 187 static void ps_decorrelate(ps_info *ps, qmf_t X_left[38][64], qmf_t X_right[38][64],
 188                            qmf_t X_hybrid_left[32][32], qmf_t X_hybrid_right[32][32]);
 189 static void ps_mix_phase(ps_info *ps, qmf_t X_left[38][64], qmf_t X_right[38][64],
 190                          qmf_t X_hybrid_left[32][32], qmf_t X_hybrid_right[32][32]);
 191
 192 /*  */
 193
 194
 195 static hyb_info *hybrid_init(uint8_t numTimeSlotsRate)
 196 {
 197     uint8_t i;
 198
 199     hyb_info *hyb = (hyb_info*)faad_malloc(sizeof(hyb_info));
 200
 201     hyb->resolution34[0] = 12;
 202     hyb->resolution34[1] = 8;
 203     hyb->resolution34[2] = 4;
 204     hyb->resolution34[3] = 4;
 205     hyb->resolution34[4] = 4;
 206
 207     hyb->resolution20[0] = 8;
 208     hyb->resolution20[1] = 2;
 209     hyb->resolution20[2] = 2;
 210
 211     hyb->frame_len = numTimeSlotsRate;
 212
 213     hyb->work = (qmf_t*)faad_malloc((hyb->frame_len+12) * sizeof(qmf_t));
 214     memset(hyb->work, 0, (hyb->frame_len+12) * sizeof(qmf_t));
 215
 216     hyb->buffer = (qmf_t**)faad_malloc(5 * sizeof(qmf_t*));
 217     for (i = 0; i < 5; i++)
 218     {
 219         hyb->buffer[i] = (qmf_t*)faad_malloc(hyb->frame_len * sizeof(qmf_t));
 220         memset(hyb->buffer[i], 0, hyb->frame_len * sizeof(qmf_t));
 221     }
 222
 223     hyb->temp = (qmf_t**)faad_malloc(hyb->frame_len * sizeof(qmf_t*));
 224     for (i = 0; i < hyb->frame_len; i++)
 225     {
 226         hyb->temp[i] = (qmf_t*)faad_malloc(12 /*max*/ * sizeof(qmf_t));
 227     }
 228
 229     return hyb;
 230 }
 231
 232 static void hybrid_free(hyb_info *hyb)
 233 {
 234     uint8_t i;
 235
 236         if (!hyb) return;
 237
 238     if (hyb->work)
 239         faad_free(hyb->work);
 240
 241     for (i = 0; i < 5; i++)
 242     {
 243         if (hyb->buffer[i])
 244             faad_free(hyb->buffer[i]);
 245     }
 246     if (hyb->buffer)
 247         faad_free(hyb->buffer);
 248
 249     for (i = 0; i < hyb->frame_len; i++)
 250     {
 251         if (hyb->temp[i])
 252             faad_free(hyb->temp[i]);
 253     }
 254     if (hyb->temp)
 255         faad_free(hyb->temp);
 256
 257         faad_free(hyb);
 258 }
 259
 260 /* real filter, size 2 */
 261 static void channel_filter2(hyb_info *hyb, uint8_t frame_len, const real_t *filter,
 262                             qmf_t *buffer, qmf_t **X_hybrid)
 263 {
 264     uint8_t i;
 265
 266     for (i = 0; i < frame_len; i++)
 267     {
 268         real_t r0 = MUL_F(filter[0],(QMF_RE(buffer[0+i]) + QMF_RE(buffer[12+i])));
 269         real_t r1 = MUL_F(filter[1],(QMF_RE(buffer[1+i]) + QMF_RE(buffer[11+i])));
 270         real_t r2 = MUL_F(filter[2],(QMF_RE(buffer[2+i]) + QMF_RE(buffer[10+i])));
 271         real_t r3 = MUL_F(filter[3],(QMF_RE(buffer[3+i]) + QMF_RE(buffer[9+i])));
 272         real_t r4 = MUL_F(filter[4],(QMF_RE(buffer[4+i]) + QMF_RE(buffer[8+i])));
 273         real_t r5 = MUL_F(filter[5],(QMF_RE(buffer[5+i]) + QMF_RE(buffer[7+i])));
 274         real_t r6 = MUL_F(filter[6],QMF_RE(buffer[6+i]));
 275         real_t i0 = MUL_F(filter[0],(QMF_IM(buffer[0+i]) + QMF_IM(buffer[12+i])));
 276         real_t i1 = MUL_F(filter[1],(QMF_IM(buffer[1+i]) + QMF_IM(buffer[11+i])));
 277         real_t i2 = MUL_F(filter[2],(QMF_IM(buffer[2+i]) + QMF_IM(buffer[10+i])));
 278         real_t i3 = MUL_F(filter[3],(QMF_IM(buffer[3+i]) + QMF_IM(buffer[9+i])));
 279         real_t i4 = MUL_F(filter[4],(QMF_IM(buffer[4+i]) + QMF_IM(buffer[8+i])));
 280         real_t i5 = MUL_F(filter[5],(QMF_IM(buffer[5+i]) + QMF_IM(buffer[7+i])));
 281         real_t i6 = MUL_F(filter[6],QMF_IM(buffer[6+i]));
 282
 283         /* q = 0 */
 284         QMF_RE(X_hybrid[i][0]) = r0 + r1 + r2 + r3 + r4 + r5 + r6;
 285         QMF_IM(X_hybrid[i][0]) = i0 + i1 + i2 + i3 + i4 + i5 + i6;
 286
 287         /* q = 1 */
 288         QMF_RE(X_hybrid[i][1]) = r0 - r1 + r2 - r3 + r4 - r5 + r6;
 289         QMF_IM(X_hybrid[i][1]) = i0 - i1 + i2 - i3 + i4 - i5 + i6;
 290     }
 291 }
 292
 293 /* complex filter, size 4 */
 294 static void channel_filter4(hyb_info *hyb, uint8_t frame_len, const real_t *filter,
 295                             qmf_t *buffer, qmf_t **X_hybrid)
 296 {
 297     uint8_t i;
 298     real_t input_re1[2], input_re2[2], input_im1[2], input_im2[2];
 299
 300     for (i = 0; i < frame_len; i++)
 301     {
 302         input_re1[0] = -MUL_F(filter[2], (QMF_RE(buffer[i+2]) + QMF_RE(buffer[i+10]))) +
 303             MUL_F(filter[6], QMF_RE(buffer[i+6]));
 304         input_re1[1] = MUL_F(FRAC_CONST(-0.70710678118655),
 305             (MUL_F(filter[1], (QMF_RE(buffer[i+1]) + QMF_RE(buffer[i+11]))) +
 306             MUL_F(filter[3], (QMF_RE(buffer[i+3]) + QMF_RE(buffer[i+9]))) -
 307             MUL_F(filter[5], (QMF_RE(buffer[i+5]) + QMF_RE(buffer[i+7])))));
 308
 309         input_im1[0] = MUL_F(filter[0], (QMF_IM(buffer[i+0]) - QMF_IM(buffer[i+12]))) -
 310             MUL_F(filter[4], (QMF_IM(buffer[i+4]) - QMF_IM(buffer[i+8])));
 311         input_im1[1] = MUL_F(FRAC_CONST(0.70710678118655),
 312             (MUL_F(filter[1], (QMF_IM(buffer[i+1]) - QMF_IM(buffer[i+11]))) -
 313             MUL_F(filter[3], (QMF_IM(buffer[i+3]) - QMF_IM(buffer[i+9]))) -
 314             MUL_F(filter[5], (QMF_IM(buffer[i+5]) - QMF_IM(buffer[i+7])))));
 315
 316         input_re2[0] = MUL_F(filter[0], (QMF_RE(buffer[i+0]) - QMF_RE(buffer[i+12]))) -
 317             MUL_F(filter[4], (QMF_RE(buffer[i+4]) - QMF_RE(buffer[i+8])));
 318         input_re2[1] = MUL_F(FRAC_CONST(0.70710678118655),
 319             (MUL_F(filter[1], (QMF_RE(buffer[i+1]) - QMF_RE(buffer[i+11]))) -
 320             MUL_F(filter[3], (QMF_RE(buffer[i+3]) - QMF_RE(buffer[i+9]))) -
 321             MUL_F(filter[5], (QMF_RE(buffer[i+5]) - QMF_RE(buffer[i+7])))));
 322
 323         input_im2[0] = -MUL_F(filter[2], (QMF_IM(buffer[i+2]) + QMF_IM(buffer[i+10]))) +
 324             MUL_F(filter[6], QMF_IM(buffer[i+6]));
 325         input_im2[1] = MUL_F(FRAC_CONST(-0.70710678118655),
 326             (MUL_F(filter[1], (QMF_IM(buffer[i+1]) + QMF_IM(buffer[i+11]))) +
 327             MUL_F(filter[3], (QMF_IM(buffer[i+3]) + QMF_IM(buffer[i+9]))) -
 328             MUL_F(filter[5], (QMF_IM(buffer[i+5]) + QMF_IM(buffer[i+7])))));
 329
 330         /* q == 0 */
 331         QMF_RE(X_hybrid[i][0]) =  input_re1[0] + input_re1[1] + input_im1[0] + input_im1[1];
 332         QMF_IM(X_hybrid[i][0]) = -input_re2[0] - input_re2[1] + input_im2[0] + input_im2[1];
 333
 334         /* q == 1 */
 335         QMF_RE(X_hybrid[i][1]) =  input_re1[0] - input_re1[1] - input_im1[0] + input_im1[1];
 336         QMF_IM(X_hybrid[i][1]) =  input_re2[0] - input_re2[1] + input_im2[0] - input_im2[1];
 337
 338         /* q == 2 */
 339         QMF_RE(X_hybrid[i][2]) =  input_re1[0] - input_re1[1] + input_im1[0] - input_im1[1];
 340         QMF_IM(X_hybrid[i][2]) = -input_re2[0] + input_re2[1] + input_im2[0] - input_im2[1];
 341
 342         /* q == 3 */
 343         QMF_RE(X_hybrid[i][3]) =  input_re1[0] + input_re1[1] - input_im1[0] - input_im1[1];
 344         QMF_IM(X_hybrid[i][3]) =  input_re2[0] + input_re2[1] + input_im2[0] + input_im2[1];
 345     }
 346 }
 347
 348 static void INLINE DCT3_4_unscaled(real_t *y, real_t *x)
 349 {
 350     real_t f0, f1, f2, f3, f4, f5, f6, f7, f8;
 351
 352     f0 = MUL_F(x[2], FRAC_CONST(0.7071067811865476));
 353     f1 = x[0] - f0;
 354     f2 = x[0] + f0;
 355     f3 = x[1] + x[3];
 356     f4 = MUL_C(x[1], COEF_CONST(1.3065629648763766));
 357     f5 = MUL_F(f3, FRAC_CONST(-0.9238795325112866));
 358     f6 = MUL_F(x[3], FRAC_CONST(-0.5411961001461967));
 359     f7 = f4 + f5;
 360     f8 = f6 - f5;
 361     y[3] = f2 - f8;
 362     y[0] = f2 + f8;
 363     y[2] = f1 - f7;
 364     y[1] = f1 + f7;
 365 }
 366
 367 /* complex filter, size 8 */
 368 static void channel_filter8(hyb_info *hyb, uint8_t frame_len, const real_t *filter,
 369                             qmf_t *buffer, qmf_t **X_hybrid)
 370 {
 371     uint8_t i, n;
 372     real_t input_re1[4], input_re2[4], input_im1[4], input_im2[4];
 373     real_t x[4];
 374
 375     for (i = 0; i < frame_len; i++)
 376     {
 377         input_re1[0] =  MUL_F(filter[6],QMF_RE(buffer[6+i]));
 378         input_re1[1] =  MUL_F(filter[5],(QMF_RE(buffer[5+i]) + QMF_RE(buffer[7+i])));
 379         input_re1[2] = -MUL_F(filter[0],(QMF_RE(buffer[0+i]) + QMF_RE(buffer[12+i]))) + MUL_F(filter[4],(QMF_RE(buffer[4+i]) + QMF_RE(buffer[8+i])));
 380         input_re1[3] = -MUL_F(filter[1],(QMF_RE(buffer[1+i]) + QMF_RE(buffer[11+i]))) + MUL_F(filter[3],(QMF_RE(buffer[3+i]) + QMF_RE(buffer[9+i])));
 381
 382         input_im1[0] = MUL_F(filter[5],(QMF_IM(buffer[7+i]) - QMF_IM(buffer[5+i])));
 383         input_im1[1] = MUL_F(filter[0],(QMF_IM(buffer[12+i]) - QMF_IM(buffer[0+i]))) + MUL_F(filter[4],(QMF_IM(buffer[8+i]) - QMF_IM(buffer[4+i])));
 384         input_im1[2] = MUL_F(filter[1],(QMF_IM(buffer[11+i]) - QMF_IM(buffer[1+i]))) + MUL_F(filter[3],(QMF_IM(buffer[9+i]) - QMF_IM(buffer[3+i])));
 385         input_im1[3] = MUL_F(filter[2],(QMF_IM(buffer[10+i]) - QMF_IM(buffer[2+i])));
 386
 387         for (n = 0; n < 4; n++)
 388         {
 389             x[n] = input_re1[n] - input_im1[3-n];
 390         }
 391         DCT3_4_unscaled(x, x);
 392         QMF_RE(X_hybrid[i][7]) = x[0];
 393         QMF_RE(X_hybrid[i][5]) = x[2];
 394         QMF_RE(X_hybrid[i][3]) = x[3];
 395         QMF_RE(X_hybrid[i][1]) = x[1];
 396
 397         for (n = 0; n < 4; n++)
 398         {
 399             x[n] = input_re1[n] + input_im1[3-n];
 400         }
 401         DCT3_4_unscaled(x, x);
 402         QMF_RE(X_hybrid[i][6]) = x[1];
 403         QMF_RE(X_hybrid[i][4]) = x[3];
 404         QMF_RE(X_hybrid[i][2]) = x[2];
 405         QMF_RE(X_hybrid[i][0]) = x[0];
 406
 407         input_im2[0] =  MUL_F(filter[6],QMF_IM(buffer[6+i]));
 408         input_im2[1] =  MUL_F(filter[5],(QMF_IM(buffer[5+i]) + QMF_IM(buffer[7+i])));
 409         input_im2[2] = -MUL_F(filter[0],(QMF_IM(buffer[0+i]) + QMF_IM(buffer[12+i]))) + MUL_F(filter[4],(QMF_IM(buffer[4+i]) + QMF_IM(buffer[8+i])));
 410         input_im2[3] = -MUL_F(filter[1],(QMF_IM(buffer[1+i]) + QMF_IM(buffer[11+i]))) + MUL_F(filter[3],(QMF_IM(buffer[3+i]) + QMF_IM(buffer[9+i])));
 411
 412         input_re2[0] = MUL_F(filter[5],(QMF_RE(buffer[7+i]) - QMF_RE(buffer[5+i])));
 413         input_re2[1] = MUL_F(filter[0],(QMF_RE(buffer[12+i]) - QMF_RE(buffer[0+i]))) + MUL_F(filter[4],(QMF_RE(buffer[8+i]) - QMF_RE(buffer[4+i])));
 414         input_re2[2] = MUL_F(filter[1],(QMF_RE(buffer[11+i]) - QMF_RE(buffer[1+i]))) + MUL_F(filter[3],(QMF_RE(buffer[9+i]) - QMF_RE(buffer[3+i])));
 415         input_re2[3] = MUL_F(filter[2],(QMF_RE(buffer[10+i]) - QMF_RE(buffer[2+i])));
 416
 417         for (n = 0; n < 4; n++)
 418         {
 419             x[n] = input_im2[n] + input_re2[3-n];
 420         }
 421         DCT3_4_unscaled(x, x);
 422         QMF_IM(X_hybrid[i][7]) = x[0];
 423         QMF_IM(X_hybrid[i][5]) = x[2];
 424         QMF_IM(X_hybrid[i][3]) = x[3];
 425         QMF_IM(X_hybrid[i][1]) = x[1];
 426
 427         for (n = 0; n < 4; n++)
 428         {
 429             x[n] = input_im2[n] - input_re2[3-n];
 430         }
 431         DCT3_4_unscaled(x, x);
 432         QMF_IM(X_hybrid[i][6]) = x[1];
 433         QMF_IM(X_hybrid[i][4]) = x[3];
 434         QMF_IM(X_hybrid[i][2]) = x[2];
 435         QMF_IM(X_hybrid[i][0]) = x[0];
 436     }
 437 }
 438
 439 static void INLINE DCT3_6_unscaled(real_t *y, real_t *x)
 440 {
 441     real_t f0, f1, f2, f3, f4, f5, f6, f7;
 442
 443     f0 = MUL_F(x[3], FRAC_CONST(0.70710678118655));
 444     f1 = x[0] + f0;
 445     f2 = x[0] - f0;
 446     f3 = MUL_F((x[1] - x[5]), FRAC_CONST(0.70710678118655));
 447     f4 = MUL_F(x[2], FRAC_CONST(0.86602540378444)) + MUL_F(x[4], FRAC_CONST(0.5));
 448     f5 = f4 - x[4];
 449     f6 = MUL_F(x[1], FRAC_CONST(0.96592582628907)) + MUL_F(x[5], FRAC_CONST(0.25881904510252));
 450     f7 = f6 - f3;
 451     y[0] = f1 + f6 + f4;
 452     y[1] = f2 + f3 - x[4];
 453     y[2] = f7 + f2 - f5;
 454     y[3] = f1 - f7 - f5;
 455     y[4] = f1 - f3 - x[4];
 456     y[5] = f2 - f6 + f4;
 457 }
 458
 459 /* complex filter, size 12 */
 460 static void channel_filter12(hyb_info *hyb, uint8_t frame_len, const real_t *filter,
 461                              qmf_t *buffer, qmf_t **X_hybrid)
 462 {
 463     uint8_t i, n;
 464     real_t input_re1[6], input_re2[6], input_im1[6], input_im2[6];
 465     real_t out_re1[6], out_re2[6], out_im1[6], out_im2[6];
 466
 467     for (i = 0; i < frame_len; i++)
 468     {
 469         for (n = 0; n < 6; n++)
 470         {
 471             if (n == 0)
 472             {
 473                 input_re1[0] = MUL_F(QMF_RE(buffer[6+i]), filter[6]);
 474                 input_re2[0] = MUL_F(QMF_IM(buffer[6+i]), filter[6]);
 475             } else {
 476                 input_re1[6-n] = MUL_F((QMF_RE(buffer[n+i]) + QMF_RE(buffer[12-n+i])), filter[n]);
 477                 input_re2[6-n] = MUL_F((QMF_IM(buffer[n+i]) + QMF_IM(buffer[12-n+i])), filter[n]);
 478             }
 479             input_im2[n] = MUL_F((QMF_RE(buffer[n+i]) - QMF_RE(buffer[12-n+i])), filter[n]);
 480             input_im1[n] = MUL_F((QMF_IM(buffer[n+i]) - QMF_IM(buffer[12-n+i])), filter[n]);
 481         }
 482
 483         DCT3_6_unscaled(out_re1, input_re1);
 484         DCT3_6_unscaled(out_re2, input_re2);
 485
 486         DCT3_6_unscaled(out_im1, input_im1);
 487         DCT3_6_unscaled(out_im2, input_im2);
 488
 489         for (n = 0; n < 6; n += 2)
 490         {
 491             QMF_RE(X_hybrid[i][n]) = out_re1[n] - out_im1[n];
 492             QMF_IM(X_hybrid[i][n]) = out_re2[n] + out_im2[n];
 493             QMF_RE(X_hybrid[i][n+1]) = out_re1[n+1] + out_im1[n+1];
 494             QMF_IM(X_hybrid[i][n+1]) = out_re2[n+1] - out_im2[n+1];
 495
 496             QMF_RE(X_hybrid[i][10-n]) = out_re1[n+1] - out_im1[n+1];
 497             QMF_IM(X_hybrid[i][10-n]) = out_re2[n+1] + out_im2[n+1];
 498             QMF_RE(X_hybrid[i][11-n]) = out_re1[n] + out_im1[n];
 499             QMF_IM(X_hybrid[i][11-n]) = out_re2[n] - out_im2[n];
 500         }
 501     }
 502 }
 503
 504 /* Hybrid analysis: further split up QMF subbands
 505  * to improve frequency resolution
 506  */
 507 static void hybrid_analysis(hyb_info *hyb, qmf_t X[32][64], qmf_t X_hybrid[32][32],
 508                             uint8_t use34, uint8_t numTimeSlotsRate)
 509 {
 510     uint8_t k, n, band;
 511     uint8_t offset = 0;
 512     uint8_t qmf_bands = (use34) ? 5 : 3;
 513     uint8_t *resolution = (use34) ? hyb->resolution34 : hyb->resolution20;
 514
 515     for (band = 0; band < qmf_bands; band++)
 516     {
 517         /* build working buffer */
 518         memcpy(hyb->work, hyb->buffer[band], 12 * sizeof(qmf_t));
 519
 520         /* add new samples */
 521         for (n = 0; n < hyb->frame_len; n++)
 522         {
 523             QMF_RE(hyb->work[12 + n]) = QMF_RE(X[n + 6 /*delay*/][band]);
 524             QMF_IM(hyb->work[12 + n]) = QMF_IM(X[n + 6 /*delay*/][band]);
 525         }
 526
 527         /* store samples */
 528         memcpy(hyb->buffer[band], hyb->work + hyb->frame_len, 12 * sizeof(qmf_t));
 529
 530
 531         switch(resolution[band])
 532         {
 533         case 2:
 534             /* Type B real filter, Q[p] = 2 */
 535             channel_filter2(hyb, hyb->frame_len, p2_13_20, hyb->work, hyb->temp);
 536             break;
 537         case 4:
 538             /* Type A complex filter, Q[p] = 4 */
 539             channel_filter4(hyb, hyb->frame_len, p4_13_34, hyb->work, hyb->temp);
 540             break;
 541         case 8:
 542             /* Type A complex filter, Q[p] = 8 */
 543             channel_filter8(hyb, hyb->frame_len, (use34) ? p8_13_34 : p8_13_20,
 544                 hyb->work, hyb->temp);
 545             break;
 546         case 12:
 547             /* Type A complex filter, Q[p] = 12 */
 548             channel_filter12(hyb, hyb->frame_len, p12_13_34, hyb->work, hyb->temp);
 549             break;
 550         }
 551
 552         for (n = 0; n < hyb->frame_len; n++)
 553         {
 554             for (k = 0; k < resolution[band]; k++)
 555             {
 556                 QMF_RE(X_hybrid[n][offset + k]) = QMF_RE(hyb->temp[n][k]);
 557                 QMF_IM(X_hybrid[n][offset + k]) = QMF_IM(hyb->temp[n][k]);
 558             }
 559         }
 560         offset += resolution[band];
 561     }
 562
 563     /* group hybrid channels */
 564     if (!use34)
 565     {
 566         for (n = 0; n < numTimeSlotsRate; n++)
 567         {
 568             QMF_RE(X_hybrid[n][3]) += QMF_RE(X_hybrid[n][4]);
 569             QMF_IM(X_hybrid[n][3]) += QMF_IM(X_hybrid[n][4]);
 570             QMF_RE(X_hybrid[n][4]) = 0;
 571             QMF_IM(X_hybrid[n][4]) = 0;
 572
 573             QMF_RE(X_hybrid[n][2]) += QMF_RE(X_hybrid[n][5]);
 574             QMF_IM(X_hybrid[n][2]) += QMF_IM(X_hybrid[n][5]);
 575             QMF_RE(X_hybrid[n][5]) = 0;
 576             QMF_IM(X_hybrid[n][5]) = 0;
 577         }
 578     }
 579 }
 580
 581 static void hybrid_synthesis(hyb_info *hyb, qmf_t X[32][64], qmf_t X_hybrid[32][32],
 582                              uint8_t use34, uint8_t numTimeSlotsRate)
 583 {
 584     uint8_t k, n, band;
 585     uint8_t offset = 0;
 586     uint8_t qmf_bands = (use34) ? 5 : 3;
 587     uint8_t *resolution = (use34) ? hyb->resolution34 : hyb->resolution20;
 588
 589     for(band = 0; band < qmf_bands; band++)
 590     {
 591         for (n = 0; n < hyb->frame_len; n++)
 592         {
 593             QMF_RE(X[n][band]) = 0;
 594             QMF_IM(X[n][band]) = 0;
 595
 596             for (k = 0; k < resolution[band]; k++)
 597             {
 598                 QMF_RE(X[n][band]) += QMF_RE(X_hybrid[n][offset + k]);
 599                 QMF_IM(X[n][band]) += QMF_IM(X_hybrid[n][offset + k]);
 600             }
 601         }
 602         offset += resolution[band];
 603     }
 604 }
 605
 606 /* limits the value i to the range [min,max] */
 607 static int8_t delta_clip(int8_t i, int8_t min, int8_t max)
 608 {
 609     if (i < min)
 610         return min;
 611     else if (i > max)
 612         return max;
 613     else
 614         return i;
 615 }
 616
 617 //int iid = 0;
 618
 619 /* delta decode array */
 620 static void delta_decode(uint8_t enable, int8_t *index, int8_t *index_prev,
 621                          uint8_t dt_flag, uint8_t nr_par, uint8_t stride,
 622                          int8_t min_index, int8_t max_index)
 623 {
 624     int8_t i;
 625
 626     if (enable == 1)
 627     {
 628         if (dt_flag == 0)
 629         {
 630             /* delta coded in frequency direction */
 631             index[0] = 0 + index[0];
 632             index[0] = delta_clip(index[0], min_index, max_index);
 633
 634             for (i = 1; i < nr_par; i++)
 635             {
 636                 index[i] = index[i-1] + index[i];
 637                 index[i] = delta_clip(index[i], min_index, max_index);
 638             }
 639         } else {
 640             /* delta coded in time direction */
 641             for (i = 0; i < nr_par; i++)
 642             {
 643                 //int8_t tmp2;
 644                 //int8_t tmp = index[i];
 645
 646                 //printf("%d %d\n", index_prev[i*stride], index[i]);
 647                 //printf("%d\n", index[i]);
 648
 649                 index[i] = index_prev[i*stride] + index[i];
 650                 //tmp2 = index[i];
 651                 index[i] = delta_clip(index[i], min_index, max_index);
 652
 653                 //if (iid)
 654                 //{
 655                 //    if (index[i] == 7)
 656                 //    {
 657                 //        printf("%d %d %d\n", index_prev[i*stride], tmp, tmp2);
 658                 //    }
 659                 //}
 660             }
 661         }
 662     } else {
 663         /* set indices to zero */
 664         for (i = 0; i < nr_par; i++)
 665         {
 666             index[i] = 0;
 667         }
 668     }
 669
 670     /* coarse */
 671     if (stride == 2)
 672     {
 673         for (i = (nr_par<<1)-1; i > 0; i--)
 674         {
 675             index[i] = index[i>>1];
 676         }
 677     }
 678 }
 679
 680 /* delta modulo decode array */
 681 /* in: log2 value of the modulo value to allow using AND instead of MOD */
 682 static void delta_modulo_decode(uint8_t enable, int8_t *index, int8_t *index_prev,
 683                                 uint8_t dt_flag, uint8_t nr_par, uint8_t stride,
 684                                 int8_t and_modulo)
 685 {
 686     int8_t i;
 687
 688     if (enable == 1)
 689     {
 690         if (dt_flag == 0)
 691         {
 692             /* delta coded in frequency direction */
 693             index[0] = 0 + index[0];
 694             index[0] &= and_modulo;
 695
 696             for (i = 1; i < nr_par; i++)
 697             {
 698                 index[i] = index[i-1] + index[i];
 699                 index[i] &= and_modulo;
 700             }
 701         } else {
 702             /* delta coded in time direction */
 703             for (i = 0; i < nr_par; i++)
 704             {
 705                 index[i] = index_prev[i*stride] + index[i];
 706                 index[i] &= and_modulo;
 707             }
 708         }
 709     } else {
 710         /* set indices to zero */
 711         for (i = 0; i < nr_par; i++)
 712         {
 713             index[i] = 0;
 714         }
 715     }
 716
 717     /* coarse */
 718     if (stride == 2)
 719     {
 720         index[0] = 0;
 721         for (i = (nr_par<<1)-1; i > 0; i--)
 722         {
 723             index[i] = index[i>>1];
 724         }
 725     }
 726 }
 727
 728 #ifdef PS_LOW_POWER
 729 static void map34indexto20(int8_t *index, uint8_t bins)
 730 {
 731     index[0] = (2*index[0]+index[1])/3;
 732     index[1] = (index[1]+2*index[2])/3;
 733     index[2] = (2*index[3]+index[4])/3;
 734     index[3] = (index[4]+2*index[5])/3;
 735     index[4] = (index[6]+index[7])/2;
 736     index[5] = (index[8]+index[9])/2;
 737     index[6] = index[10];
 738     index[7] = index[11];
 739     index[8] = (index[12]+index[13])/2;
 740     index[9] = (index[14]+index[15])/2;
 741     index[10] = index[16];
 742
 743     if (bins == 34)
 744     {
 745         index[11] = index[17];
 746         index[12] = index[18];
 747         index[13] = index[19];
 748         index[14] = (index[20]+index[21])/2;
 749         index[15] = (index[22]+index[23])/2;
 750         index[16] = (index[24]+index[25])/2;
 751         index[17] = (index[26]+index[27])/2;
 752         index[18] = (index[28]+index[29]+index[30]+index[31])/4;
 753         index[19] = (index[32]+index[33])/2;
 754     }
 755 }
 756 #endif
 757
 758 static void map20indexto34(int8_t *index, uint8_t bins)
 759 {
 760     index[0] = index[0];
 761     index[1] = (index[0] + index[1])/2;
 762     index[2] = index[1];
 763     index[3] = index[2];
 764     index[4] = (index[2] + index[3])/2;
 765     index[5] = index[3];
 766     index[6] = index[4];
 767     index[7] = index[4];
 768     index[8] = index[5];
 769     index[9] = index[5];
 770     index[10] = index[6];
 771     index[11] = index[7];
 772     index[12] = index[8];
 773     index[13] = index[8];
 774     index[14] = index[9];
 775     index[15] = index[9];
 776     index[16] = index[10];
 777
 778     if (bins == 34)
 779     {
 780         index[17] = index[11];
 781         index[18] = index[12];
 782         index[19] = index[13];
 783         index[20] = index[14];
 784         index[21] = index[14];
 785         index[22] = index[15];
 786         index[23] = index[15];
 787         index[24] = index[16];
 788         index[25] = index[16];
 789         index[26] = index[17];
 790         index[27] = index[17];
 791         index[28] = index[18];
 792         index[29] = index[18];
 793         index[30] = index[18];
 794         index[31] = index[18];
 795         index[32] = index[19];
 796         index[33] = index[19];
 797     }
 798 }
 799
 800 /* parse the bitstream data decoded in ps_data() */
 801 static void ps_data_decode(ps_info *ps)
 802 {
 803     uint8_t env, bin;
 804
 805     /* ps data not available, use data from previous frame */
 806     if (ps->ps_data_available == 0)
 807     {
 808         ps->num_env = 0;
 809     }
 810
 811     for (env = 0; env < ps->num_env; env++)
 812     {
 813         int8_t *iid_index_prev;
 814         int8_t *icc_index_prev;
 815         int8_t *ipd_index_prev;
 816         int8_t *opd_index_prev;
 817
 818         int8_t num_iid_steps = (ps->iid_mode < 3) ? 7 : 15 /*fine quant*/;
 819
 820         if (env == 0)
 821         {
 822             /* take last envelope from previous frame */
 823             iid_index_prev = ps->iid_index_prev;
 824             icc_index_prev = ps->icc_index_prev;
 825             ipd_index_prev = ps->ipd_index_prev;
 826             opd_index_prev = ps->opd_index_prev;
 827         } else {
 828             /* take index values from previous envelope */
 829             iid_index_prev = ps->iid_index[env - 1];
 830             icc_index_prev = ps->icc_index[env - 1];
 831             ipd_index_prev = ps->ipd_index[env - 1];
 832             opd_index_prev = ps->opd_index[env - 1];
 833         }
 834
 835 //        iid = 1;
 836         /* delta decode iid parameters */
 837         delta_decode(ps->enable_iid, ps->iid_index[env], iid_index_prev,
 838             ps->iid_dt[env], ps->nr_iid_par,
 839             (ps->iid_mode == 0 || ps->iid_mode == 3) ? 2 : 1,
 840             -num_iid_steps, num_iid_steps);
 841 //        iid = 0;
 842
 843         /* delta decode icc parameters */
 844         delta_decode(ps->enable_icc, ps->icc_index[env], icc_index_prev,
 845             ps->icc_dt[env], ps->nr_icc_par,
 846             (ps->icc_mode == 0 || ps->icc_mode == 3) ? 2 : 1,
 847             0, 7);
 848
 849         /* delta modulo decode ipd parameters */
 850         delta_modulo_decode(ps->enable_ipdopd, ps->ipd_index[env], ipd_index_prev,
 851             ps->ipd_dt[env], ps->nr_ipdopd_par, 1, 7);
 852
 853         /* delta modulo decode opd parameters */
 854         delta_modulo_decode(ps->enable_ipdopd, ps->opd_index[env], opd_index_prev,
 855             ps->opd_dt[env], ps->nr_ipdopd_par, 1, 7);
 856     }
 857
 858     /* handle error case */
 859     if (ps->num_env == 0)
 860     {
 861         /* force to 1 */
 862         ps->num_env = 1;
 863
 864         if (ps->enable_iid)
 865         {
 866             for (bin = 0; bin < 34; bin++)
 867                 ps->iid_index[0][bin] = ps->iid_index_prev[bin];
 868         } else {
 869             for (bin = 0; bin < 34; bin++)
 870                 ps->iid_index[0][bin] = 0;
 871         }
 872
 873         if (ps->enable_icc)
 874         {
 875             for (bin = 0; bin < 34; bin++)
 876                 ps->icc_index[0][bin] = ps->icc_index_prev[bin];
 877         } else {
 878             for (bin = 0; bin < 34; bin++)
 879                 ps->icc_index[0][bin] = 0;
 880         }
 881
 882         if (ps->enable_ipdopd)
 883         {
 884             for (bin = 0; bin < 17; bin++)
 885             {
 886                 ps->ipd_index[0][bin] = ps->ipd_index_prev[bin];
 887                 ps->opd_index[0][bin] = ps->opd_index_prev[bin];
 888             }
 889         } else {
 890             for (bin = 0; bin < 17; bin++)
 891             {
 892                 ps->ipd_index[0][bin] = 0;
 893                 ps->opd_index[0][bin] = 0;
 894             }
 895         }
 896     }
 897
 898     /* update previous indices */
 899     for (bin = 0; bin < 34; bin++)
 900         ps->iid_index_prev[bin] = ps->iid_index[ps->num_env-1][bin];
 901     for (bin = 0; bin < 34; bin++)
 902         ps->icc_index_prev[bin] = ps->icc_index[ps->num_env-1][bin];
 903     for (bin = 0; bin < 17; bin++)
 904     {
 905         ps->ipd_index_prev[bin] = ps->ipd_index[ps->num_env-1][bin];
 906         ps->opd_index_prev[bin] = ps->opd_index[ps->num_env-1][bin];
 907     }
 908
 909     ps->ps_data_available = 0;
 910
 911     if (ps->frame_class == 0)
 912     {
 913         ps->border_position[0] = 0;
 914         for (env = 1; env < ps->num_env; env++)
 915         {
 916             ps->border_position[env] = (env * ps->numTimeSlotsRate) / ps->num_env;
 917         }
 918         ps->border_position[ps->num_env] = ps->numTimeSlotsRate;
 919     } else {
 920         ps->border_position[0] = 0;
 921
 922         if (ps->border_position[ps->num_env] < ps->numTimeSlotsRate)
 923         {
 924             for (bin = 0; bin < 34; bin++)
 925             {
 926                 ps->iid_index[ps->num_env][bin] = ps->iid_index[ps->num_env-1][bin];
 927                 ps->icc_index[ps->num_env][bin] = ps->icc_index[ps->num_env-1][bin];
 928             }
 929             for (bin = 0; bin < 17; bin++)
 930             {
 931                 ps->ipd_index[ps->num_env][bin] = ps->ipd_index[ps->num_env-1][bin];
 932                 ps->opd_index[ps->num_env][bin] = ps->opd_index[ps->num_env-1][bin];
 933             }
 934             ps->num_env++;
 935             ps->border_position[ps->num_env] = ps->numTimeSlotsRate;
 936         }
 937
 938         for (env = 1; env < ps->num_env; env++)
 939         {
 940             int8_t thr = ps->numTimeSlotsRate - (ps->num_env - env);
 941
 942             if (ps->border_position[env] > thr)
 943             {
 944                 ps->border_position[env] = thr;
 945             } else {
 946                 thr = ps->border_position[env-1]+1;
 947                 if (ps->border_position[env] < thr)
 948                 {
 949                     ps->border_position[env] = thr;
 950                 }
 951             }
 952         }
 953     }
 954
 955     /* make sure that the indices of all parameters can be mapped
 956      * to the same hybrid synthesis filterbank
 957      */
 958 #ifdef PS_LOW_POWER
 959     for (env = 0; env < ps->num_env; env++)
 960     {
 961         if (ps->iid_mode == 2 || ps->iid_mode == 5)
 962             map34indexto20(ps->iid_index[env], 34);
 963         if (ps->icc_mode == 2 || ps->icc_mode == 5)
 964             map34indexto20(ps->icc_index[env], 34);
 965
 966         /* disable ipd/opd */
 967         for (bin = 0; bin < 17; bin++)
 968         {
 969             ps->aaIpdIndex[env][bin] = 0;
 970             ps->aaOpdIndex[env][bin] = 0;
 971         }
 972     }
 973 #else
 974     if (ps->use34hybrid_bands)
 975     {
 976         for (env = 0; env < ps->num_env; env++)
 977         {
 978             if (ps->iid_mode != 2 && ps->iid_mode != 5)
 979                 map20indexto34(ps->iid_index[env], 34);
 980             if (ps->icc_mode != 2 && ps->icc_mode != 5)
 981                 map20indexto34(ps->icc_index[env], 34);
 982             if (ps->ipd_mode != 2 && ps->ipd_mode != 5)
 983             {
 984                 map20indexto34(ps->ipd_index[env], 17);
 985                 map20indexto34(ps->opd_index[env], 17);
 986             }
 987         }
 988     }
 989 #endif
 990
 991 #if 0
 992     for (env = 0; env < ps->num_env; env++)
 993     {
 994         printf("iid[env:%d]:", env);
 995         for (bin = 0; bin < 34; bin++)
 996         {
 997             printf(" %d", ps->iid_index[env][bin]);
 998         }
 999         printf("\n");
1000     }
1001     for (env = 0; env < ps->num_env; env++)
1002     {
1003         printf("icc[env:%d]:", env);
1004         for (bin = 0; bin < 34; bin++)
1005         {
1006             printf(" %d", ps->icc_index[env][bin]);
1007         }
1008         printf("\n");
1009     }
1010     for (env = 0; env < ps->num_env; env++)
1011     {
1012         printf("ipd[env:%d]:", env);
1013         for (bin = 0; bin < 17; bin++)
1014         {
1015             printf(" %d", ps->ipd_index[env][bin]);
1016         }
1017         printf("\n");
1018     }
1019     for (env = 0; env < ps->num_env; env++)
1020     {
1021         printf("opd[env:%d]:", env);
1022         for (bin = 0; bin < 17; bin++)
1023         {
1024             printf(" %d", ps->opd_index[env][bin]);
1025         }
1026         printf("\n");
1027     }
1028     printf("\n");
1029 #endif
1030 }
1031
1032 /* decorrelate the mono signal using an allpass filter */
1033 static void ps_decorrelate(ps_info *ps, qmf_t X_left[38][64], qmf_t X_right[38][64],
1034                            qmf_t X_hybrid_left[32][32], qmf_t X_hybrid_right[32][32])
1035 {
1036     uint8_t gr, n, m, bk;
1037     uint8_t temp_delay;
1038     uint8_t sb, maxsb;
1039     const complex_t *Phi_Fract_SubQmf;
1040     uint8_t temp_delay_ser[NO_ALLPASS_LINKS];
1041     real_t P_SmoothPeakDecayDiffNrg, nrg;
1042     real_t P[32][34];
1043     real_t G_TransientRatio[32][34] = {{0}};
1044     complex_t inputLeft;
1045
1046
1047     /* chose hybrid filterbank: 20 or 34 band case */
1048     if (ps->use34hybrid_bands)
1049     {
1050         Phi_Fract_SubQmf = Phi_Fract_SubQmf34;
1051     } else{
1052         Phi_Fract_SubQmf = Phi_Fract_SubQmf20;
1053     }
1054
1055     /* clear the energy values */
1056     for (n = 0; n < 32; n++)
1057     {
1058         for (bk = 0; bk < 34; bk++)
1059         {
1060             P[n][bk] = 0;
1061         }
1062     }
1063
1064     /* calculate the energy in each parameter band b(k) */
1065     for (gr = 0; gr < ps->num_groups; gr++)
1066     {
1067         /* select the parameter index b(k) to which this group belongs */
1068         bk = (~NEGATE_IPD_MASK) & ps->map_group2bk[gr];
1069
1070         /* select the upper subband border for this group */
1071         maxsb = (gr < ps->num_hybrid_groups) ? ps->group_border[gr]+1 : ps->group_border[gr+1];
1072
1073         for (sb = ps->group_border[gr]; sb < maxsb; sb++)
1074         {
1075             for (n = ps->border_position[0]; n < ps->border_position[ps->num_env]; n++)
1076             {
1077 #ifdef FIXED_POINT
1078                 uint32_t in_re, in_im;
1079 #endif
1080
1081                 /* input from hybrid subbands or QMF subbands */
1082                 if (gr < ps->num_hybrid_groups)
1083                 {
1084                     RE(inputLeft) = QMF_RE(X_hybrid_left[n][sb]);
1085                     IM(inputLeft) = QMF_IM(X_hybrid_left[n][sb]);
1086                 } else {
1087                     RE(inputLeft) = QMF_RE(X_left[n][sb]);
1088                     IM(inputLeft) = QMF_IM(X_left[n][sb]);
1089                 }
1090
1091                 /* accumulate energy */
1092 #ifdef FIXED_POINT
1093                 /* NOTE: all input is scaled by 2^(-5) because of fixed point QMF
1094                  * meaning that P will be scaled by 2^(-10) compared to floating point version
1095                  */
1096                 in_re = ((abs(RE(inputLeft))+(1<<(REAL_BITS-1)))>>REAL_BITS);
1097                 in_im = ((abs(IM(inputLeft))+(1<<(REAL_BITS-1)))>>REAL_BITS);
1098                 P[n][bk] += in_re*in_re + in_im*in_im;
1099 #else
1100                 P[n][bk] += MUL_R(RE(inputLeft),RE(inputLeft)) + MUL_R(IM(inputLeft),IM(inputLeft));
1101 #endif
1102             }
1103         }
1104     }
1105
1106 #if 0
1107     for (n = 0; n < 32; n++)
1108     {
1109         for (bk = 0; bk < 34; bk++)
1110         {
1111 #ifdef FIXED_POINT
1112             printf("%d %d: %d\n", n, bk, P[n][bk] /*/(float)REAL_PRECISION*/);
1113 #else
1114             printf("%d %d: %f\n", n, bk, P[n][bk]/1024.0);
1115 #endif
1116         }
1117     }
1118 #endif
1119
1120     /* calculate transient reduction ratio for each parameter band b(k) */
1121     for (bk = 0; bk < ps->nr_par_bands; bk++)
1122     {
1123         for (n = ps->border_position[0]; n < ps->border_position[ps->num_env]; n++)
1124         {
1125             const real_t gamma = COEF_CONST(1.5);
1126
1127             ps->P_PeakDecayNrg[bk] = MUL_F(ps->P_PeakDecayNrg[bk], ps->alpha_decay);
1128             if (ps->P_PeakDecayNrg[bk] < P[n][bk])
1129                 ps->P_PeakDecayNrg[bk] = P[n][bk];
1130
1131             /* apply smoothing filter to peak decay energy */
1132             P_SmoothPeakDecayDiffNrg = ps->P_SmoothPeakDecayDiffNrg_prev[bk];
1133             P_SmoothPeakDecayDiffNrg += MUL_F((ps->P_PeakDecayNrg[bk] - P[n][bk] - ps->P_SmoothPeakDecayDiffNrg_prev[bk]), ps->alpha_smooth);
1134             ps->P_SmoothPeakDecayDiffNrg_prev[bk] = P_SmoothPeakDecayDiffNrg;
1135
1136             /* apply smoothing filter to energy */
1137             nrg = ps->P_prev[bk];
1138             nrg += MUL_F((P[n][bk] - ps->P_prev[bk]), ps->alpha_smooth);
1139             ps->P_prev[bk] = nrg;
1140
1141             /* calculate transient ratio */
1142             if (MUL_C(P_SmoothPeakDecayDiffNrg, gamma) <= nrg)
1143             {
1144                 G_TransientRatio[n][bk] = REAL_CONST(1.0);
1145             } else {
1146                 G_TransientRatio[n][bk] = DIV_R(nrg, (MUL_C(P_SmoothPeakDecayDiffNrg, gamma)));
1147             }
1148         }
1149     }
1150
1151 #if 0
1152     for (n = 0; n < 32; n++)
1153     {
1154         for (bk = 0; bk < 34; bk++)
1155         {
1156 #ifdef FIXED_POINT
1157             printf("%d %d: %f\n", n, bk, G_TransientRatio[n][bk]/(float)REAL_PRECISION);
1158 #else
1159             printf("%d %d: %f\n", n, bk, G_TransientRatio[n][bk]);
1160 #endif
1161         }
1162     }
1163 #endif
1164
1165     /* apply stereo decorrelation filter to the signal */
1166     for (gr = 0; gr < ps->num_groups; gr++)
1167     {
1168         if (gr < ps->num_hybrid_groups)
1169             maxsb = ps->group_border[gr] + 1;
1170         else
1171             maxsb = ps->group_border[gr + 1];
1172
1173         /* QMF channel */
1174         for (sb = ps->group_border[gr]; sb < maxsb; sb++)
1175         {
1176             real_t g_DecaySlope;
1177             real_t g_DecaySlope_filt[NO_ALLPASS_LINKS];
1178
1179             /* g_DecaySlope: [0..1] */
1180             if (gr < ps->num_hybrid_groups || sb <= ps->decay_cutoff)
1181             {
1182                 g_DecaySlope = FRAC_CONST(1.0);
1183             } else {
1184                 int8_t decay = ps->decay_cutoff - sb;
1185                 if (decay <= -20 /* -1/DECAY_SLOPE */)
1186                 {
1187                     g_DecaySlope = 0;
1188                 } else {
1189                     /* decay(int)*decay_slope(frac) = g_DecaySlope(frac) */
1190                     g_DecaySlope = FRAC_CONST(1.0) + DECAY_SLOPE * decay;
1191                 }
1192             }
1193
1194             /* calculate g_DecaySlope_filt for every m multiplied by filter_a[m] */
1195             for (m = 0; m < NO_ALLPASS_LINKS; m++)
1196             {
1197                 g_DecaySlope_filt[m] = MUL_F(g_DecaySlope, filter_a[m]);
1198             }
1199
1200
1201             /* set delay indices */
1202             temp_delay = ps->saved_delay;
1203             for (n = 0; n < NO_ALLPASS_LINKS; n++)
1204                 temp_delay_ser[n] = ps->delay_buf_index_ser[n];
1205
1206             for (n = ps->border_position[0]; n < ps->border_position[ps->num_env]; n++)
1207             {
1208                 complex_t tmp, tmp0, R0;
1209
1210                 if (gr < ps->num_hybrid_groups)
1211                 {
1212                     /* hybrid filterbank input */
1213                     RE(inputLeft) = QMF_RE(X_hybrid_left[n][sb]);
1214                     IM(inputLeft) = QMF_IM(X_hybrid_left[n][sb]);
1215                 } else {
1216                     /* QMF filterbank input */
1217                     RE(inputLeft) = QMF_RE(X_left[n][sb]);
1218                     IM(inputLeft) = QMF_IM(X_left[n][sb]);
1219                 }
1220
1221                 if (sb > ps->nr_allpass_bands && gr >= ps->num_hybrid_groups)
1222                 {
1223                     /* delay */
1224
1225                     /* never hybrid subbands here, always QMF subbands */
1226                     RE(tmp) = RE(ps->delay_Qmf[ps->delay_buf_index_delay[sb]][sb]);
1227                     IM(tmp) = IM(ps->delay_Qmf[ps->delay_buf_index_delay[sb]][sb]);
1228                     RE(R0) = RE(tmp);
1229                     IM(R0) = IM(tmp);
1230                     RE(ps->delay_Qmf[ps->delay_buf_index_delay[sb]][sb]) = RE(inputLeft);
1231                     IM(ps->delay_Qmf[ps->delay_buf_index_delay[sb]][sb]) = IM(inputLeft);
1232                 } else {
1233                     /* allpass filter */
1234                     uint8_t m;
1235                     complex_t Phi_Fract;
1236
1237                     /* fetch parameters */
1238                     if (gr < ps->num_hybrid_groups)
1239                     {
1240                         /* select data from the hybrid subbands */
1241                         RE(tmp0) = RE(ps->delay_SubQmf[temp_delay][sb]);
1242                         IM(tmp0) = IM(ps->delay_SubQmf[temp_delay][sb]);
1243
1244                         RE(ps->delay_SubQmf[temp_delay][sb]) = RE(inputLeft);
1245                         IM(ps->delay_SubQmf[temp_delay][sb]) = IM(inputLeft);
1246
1247                         RE(Phi_Fract) = RE(Phi_Fract_SubQmf[sb]);
1248                         IM(Phi_Fract) = IM(Phi_Fract_SubQmf[sb]);
1249                     } else {
1250                         /* select data from the QMF subbands */
1251                         RE(tmp0) = RE(ps->delay_Qmf[temp_delay][sb]);
1252                         IM(tmp0) = IM(ps->delay_Qmf[temp_delay][sb]);
1253
1254                         RE(ps->delay_Qmf[temp_delay][sb]) = RE(inputLeft);
1255                         IM(ps->delay_Qmf[temp_delay][sb]) = IM(inputLeft);
1256
1257                         RE(Phi_Fract) = RE(Phi_Fract_Qmf[sb]);
1258                         IM(Phi_Fract) = IM(Phi_Fract_Qmf[sb]);
1259                     }
1260
1261                     /* z^(-2) * Phi_Fract[k] */
1262                     ComplexMult(&RE(tmp), &IM(tmp), RE(tmp0), IM(tmp0), RE(Phi_Fract), IM(Phi_Fract));
1263
1264                     RE(R0) = RE(tmp);
1265                     IM(R0) = IM(tmp);
1266                     for (m = 0; m < NO_ALLPASS_LINKS; m++)
1267                     {
1268                         complex_t Q_Fract_allpass, tmp2;
1269
1270                         /* fetch parameters */
1271                         if (gr < ps->num_hybrid_groups)
1272                         {
1273                             /* select data from the hybrid subbands */
1274                             RE(tmp0) = RE(ps->delay_SubQmf_ser[m][temp_delay_ser[m]][sb]);
1275                             IM(tmp0) = IM(ps->delay_SubQmf_ser[m][temp_delay_ser[m]][sb]);
1276
1277                             if (ps->use34hybrid_bands)
1278                             {
1279                                 RE(Q_Fract_allpass) = RE(Q_Fract_allpass_SubQmf34[sb][m]);
1280                                 IM(Q_Fract_allpass) = IM(Q_Fract_allpass_SubQmf34[sb][m]);
1281                             } else {
1282                                 RE(Q_Fract_allpass) = RE(Q_Fract_allpass_SubQmf20[sb][m]);
1283                                 IM(Q_Fract_allpass) = IM(Q_Fract_allpass_SubQmf20[sb][m]);
1284                             }
1285                         } else {
1286                             /* select data from the QMF subbands */
1287                             RE(tmp0) = RE(ps->delay_Qmf_ser[m][temp_delay_ser[m]][sb]);
1288                             IM(tmp0) = IM(ps->delay_Qmf_ser[m][temp_delay_ser[m]][sb]);
1289
1290                             RE(Q_Fract_allpass) = RE(Q_Fract_allpass_Qmf[sb][m]);
1291                             IM(Q_Fract_allpass) = IM(Q_Fract_allpass_Qmf[sb][m]);
1292                         }
1293
1294                         /* delay by a fraction */
1295                         /* z^(-d(m)) * Q_Fract_allpass[k,m] */
1296                         ComplexMult(&RE(tmp), &IM(tmp), RE(tmp0), IM(tmp0), RE(Q_Fract_allpass), IM(Q_Fract_allpass));
1297
1298                         /* -a(m) * g_DecaySlope[k] */
1299                         RE(tmp) += -MUL_F(g_DecaySlope_filt[m], RE(R0));
1300                         IM(tmp) += -MUL_F(g_DecaySlope_filt[m], IM(R0));
1301
1302                         /* -a(m) * g_DecaySlope[k] * Q_Fract_allpass[k,m] * z^(-d(m)) */
1303                         RE(tmp2) = RE(R0) + MUL_F(g_DecaySlope_filt[m], RE(tmp));
1304                         IM(tmp2) = IM(R0) + MUL_F(g_DecaySlope_filt[m], IM(tmp));
1305
1306                         /* store sample */
1307                         if (gr < ps->num_hybrid_groups)
1308                         {
1309                             RE(ps->delay_SubQmf_ser[m][temp_delay_ser[m]][sb]) = RE(tmp2);
1310                             IM(ps->delay_SubQmf_ser[m][temp_delay_ser[m]][sb]) = IM(tmp2);
1311                         } else {
1312                             RE(ps->delay_Qmf_ser[m][temp_delay_ser[m]][sb]) = RE(tmp2);
1313                             IM(ps->delay_Qmf_ser[m][temp_delay_ser[m]][sb]) = IM(tmp2);
1314                         }
1315
1316                         /* store for next iteration (or as output value if last iteration) */
1317                         RE(R0) = RE(tmp);
1318                         IM(R0) = IM(tmp);
1319                     }
1320                 }
1321
1322                 /* select b(k) for reading the transient ratio */
1323                 bk = (~NEGATE_IPD_MASK) & ps->map_group2bk[gr];
1324
1325                 /* duck if a past transient is found */
1326                 RE(R0) = MUL_R(G_TransientRatio[n][bk], RE(R0));
1327                 IM(R0) = MUL_R(G_TransientRatio[n][bk], IM(R0));
1328
1329                 if (gr < ps->num_hybrid_groups)
1330                 {
1331                     /* hybrid */
1332                     QMF_RE(X_hybrid_right[n][sb]) = RE(R0);
1333                     QMF_IM(X_hybrid_right[n][sb]) = IM(R0);
1334                 } else {
1335                     /* QMF */
1336                     QMF_RE(X_right[n][sb]) = RE(R0);
1337                     QMF_IM(X_right[n][sb]) = IM(R0);
1338                 }
1339
1340                 /* Update delay buffer index */
1341                 if (++temp_delay >= 2)
1342                 {
1343                     temp_delay = 0;
1344                 }
1345
1346                 /* update delay indices */
1347                 if (sb > ps->nr_allpass_bands && gr >= ps->num_hybrid_groups)
1348                 {
1349                     /* delay_D depends on the samplerate, it can hold the values 14 and 1 */
1350                     if (++ps->delay_buf_index_delay[sb] >= ps->delay_D[sb])
1351                     {
1352                         ps->delay_buf_index_delay[sb] = 0;
1353                     }
1354                 }
1355
1356                 for (m = 0; m < NO_ALLPASS_LINKS; m++)
1357                 {
1358                     if (++temp_delay_ser[m] >= ps->num_sample_delay_ser[m])
1359                     {
1360                         temp_delay_ser[m] = 0;
1361                     }
1362                 }
1363             }
1364         }
1365     }
1366
1367     /* update delay indices */
1368     ps->saved_delay = temp_delay;
1369     for (m = 0; m < NO_ALLPASS_LINKS; m++)
1370         ps->delay_buf_index_ser[m] = temp_delay_ser[m];
1371 }
1372
1373 #ifdef FIXED_POINT
1374 #define step(shift) \
1375     if ((0x40000000l >> shift) + root <= value)       \
1376     {                                                 \
1377         value -= (0x40000000l >> shift) + root;       \
1378         root = (root >> 1) | (0x40000000l >> shift);  \
1379     } else {                                          \
1380         root = root >> 1;                             \
1381     }
1382
1383 /* fixed point square root approximation */
1384 static real_t ps_sqrt(real_t value)
1385 {
1386     real_t root = 0;
1387
1388     step( 0); step( 2); step( 4); step( 6);
1389     step( 8); step(10); step(12); step(14);
1390     step(16); step(18); step(20); step(22);
1391     step(24); step(26); step(28); step(30);
1392
1393     if (root < value)
1394         ++root;
1395
1396     root <<= (REAL_BITS/2);
1397
1398     return root;
1399 }
1400 #else
1401 #define ps_sqrt(A) sqrt(A)
1402 #endif
1403
1404 static const real_t ipdopd_cos_tab[] = {
1405     FRAC_CONST(1.000000000000000),
1406     FRAC_CONST(0.707106781186548),
1407     FRAC_CONST(0.000000000000000),
1408     FRAC_CONST(-0.707106781186547),
1409     FRAC_CONST(-1.000000000000000),
1410     FRAC_CONST(-0.707106781186548),
1411     FRAC_CONST(-0.000000000000000),
1412     FRAC_CONST(0.707106781186547),
1413     FRAC_CONST(1.000000000000000)
1414 };
1415
1416 static const real_t ipdopd_sin_tab[] = {
1417     FRAC_CONST(0.000000000000000),
1418     FRAC_CONST(0.707106781186547),
1419     FRAC_CONST(1.000000000000000),
1420     FRAC_CONST(0.707106781186548),
1421     FRAC_CONST(0.000000000000000),
1422     FRAC_CONST(-0.707106781186547),
1423     FRAC_CONST(-1.000000000000000),
1424     FRAC_CONST(-0.707106781186548),
1425     FRAC_CONST(-0.000000000000000)
1426 };
1427
1428 static real_t magnitude_c(complex_t c)
1429 {
1430 #ifdef FIXED_POINT
1431 #define ps_abs(A) (((A) > 0) ? (A) : (-(A)))
1432 #define ALPHA FRAC_CONST(0.948059448969)
1433 #define BETA  FRAC_CONST(0.392699081699)
1434
1435     real_t abs_inphase = ps_abs(RE(c));
1436     real_t abs_quadrature = ps_abs(IM(c));
1437
1438     if (abs_inphase > abs_quadrature) {
1439         return MUL_F(abs_inphase, ALPHA) + MUL_F(abs_quadrature, BETA);
1440     } else {
1441         return MUL_F(abs_quadrature, ALPHA) + MUL_F(abs_inphase, BETA);
1442     }
1443 #else
1444     return sqrt(RE(c)*RE(c) + IM(c)*IM(c));
1445 #endif
1446 }
1447
1448 static void ps_mix_phase(ps_info *ps, qmf_t X_left[38][64], qmf_t X_right[38][64],
1449                          qmf_t X_hybrid_left[32][32], qmf_t X_hybrid_right[32][32])
1450 {
1451     uint8_t n;
1452     uint8_t gr;
1453     uint8_t bk = 0;
1454     uint8_t sb, maxsb;
1455     uint8_t env;
1456     uint8_t nr_ipdopd_par;
1457     complex_t h11, h12, h21, h22;
1458     complex_t H11, H12, H21, H22;
1459     complex_t deltaH11, deltaH12, deltaH21, deltaH22;
1460     complex_t tempLeft;
1461     complex_t tempRight;
1462     complex_t phaseLeft;
1463     complex_t phaseRight;
1464     real_t L;
1465     const real_t *sf_iid;
1466     uint8_t no_iid_steps;
1467
1468     if (ps->iid_mode >= 3)
1469     {
1470         no_iid_steps = 15;
1471         sf_iid = sf_iid_fine;
1472     } else {
1473         no_iid_steps = 7;
1474         sf_iid = sf_iid_normal;
1475     }
1476
1477     if (ps->ipd_mode == 0 || ps->ipd_mode == 3)
1478     {
1479         nr_ipdopd_par = 11; /* resolution */
1480     } else {
1481         nr_ipdopd_par = ps->nr_ipdopd_par;
1482     }
1483
1484     for (gr = 0; gr < ps->num_groups; gr++)
1485     {
1486         bk = (~NEGATE_IPD_MASK) & ps->map_group2bk[gr];
1487
1488         /* use one channel per group in the subqmf domain */
1489         maxsb = (gr < ps->num_hybrid_groups) ? ps->group_border[gr] + 1 : ps->group_border[gr + 1];
1490
1491         for (env = 0; env < ps->num_env; env++)
1492         {
1493             if (ps->icc_mode < 3)
1494             {
1495                 /* type 'A' mixing as described in 8.6.4.6.2.1 */
1496                 real_t c_1, c_2;
1497                 real_t cosa, sina;
1498                 real_t cosb, sinb;
1499                 real_t ab1, ab2;
1500                 real_t ab3, ab4;
1501
1502                 /*
1503                 c_1 = sqrt(2.0 / (1.0 + pow(10.0, quant_iid[no_iid_steps + iid_index] / 10.0)));
1504                 c_2 = sqrt(2.0 / (1.0 + pow(10.0, quant_iid[no_iid_steps - iid_index] / 10.0)));
1505                 alpha = 0.5 * acos(quant_rho[icc_index]);
1506                 beta = alpha * ( c_1 - c_2 ) / sqrt(2.0);
1507                 */
1508
1509                 //printf("%d\n", ps->iid_index[env][bk]);
1510
1511                 /* calculate the scalefactors c_1 and c_2 from the intensity differences */
1512                 c_1 = sf_iid[no_iid_steps + ps->iid_index[env][bk]];
1513                 c_2 = sf_iid[no_iid_steps - ps->iid_index[env][bk]];
1514
1515                 /* calculate alpha and beta using the ICC parameters */
1516                 cosa = cos_alphas[ps->icc_index[env][bk]];
1517                 sina = sin_alphas[ps->icc_index[env][bk]];
1518
1519                 if (ps->iid_mode >= 3)
1520                 {
1521                     if (ps->iid_index[env][bk] < 0)
1522                     {
1523                         cosb =  cos_betas_fine[-ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1524                         sinb = -sin_betas_fine[-ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1525                     } else {
1526                         cosb = cos_betas_fine[ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1527                         sinb = sin_betas_fine[ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1528                     }
1529                 } else {
1530                     if (ps->iid_index[env][bk] < 0)
1531                     {
1532                         cosb =  cos_betas_normal[-ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1533                         sinb = -sin_betas_normal[-ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1534                     } else {
1535                         cosb = cos_betas_normal[ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1536                         sinb = sin_betas_normal[ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1537                     }
1538                 }
1539
1540                 ab1 = MUL_C(cosb, cosa);
1541                 ab2 = MUL_C(sinb, sina);
1542                 ab3 = MUL_C(sinb, cosa);
1543                 ab4 = MUL_C(cosb, sina);
1544
1545                 /* h_xy: COEF */
1546                 RE(h11) = MUL_C(c_2, (ab1 - ab2));
1547                 RE(h12) = MUL_C(c_1, (ab1 + ab2));
1548                 RE(h21) = MUL_C(c_2, (ab3 + ab4));
1549                 RE(h22) = MUL_C(c_1, (ab3 - ab4));
1550             } else {
1551                 /* type 'B' mixing as described in 8.6.4.6.2.2 */
1552                 real_t sina, cosa;
1553                 real_t cosg, sing;
1554
1555                 /*
1556                 real_t c, rho, mu, alpha, gamma;
1557                 uint8_t i;
1558
1559                 i = ps->iid_index[env][bk];
1560                 c = (real_t)pow(10.0, ((i)?(((i>0)?1:-1)*quant_iid[((i>0)?i:-i)-1]):0.)/20.0);
1561                 rho = quant_rho[ps->icc_index[env][bk]];
1562
1563                 if (rho == 0.0f && c == 1.)
1564                 {
1565                     alpha = (real_t)M_PI/4.0f;
1566                     rho = 0.05f;
1567                 } else {
1568                     if (rho <= 0.05f)
1569                     {
1570                         rho = 0.05f;
1571                     }
1572                     alpha = 0.5f*(real_t)atan( (2.0f*c*rho) / (c*c-1.0f) );
1573
1574                     if (alpha < 0.)
1575                     {
1576                         alpha += (real_t)M_PI/2.0f;
1577                     }
1578                     if (rho < 0.)
1579                     {
1580                         alpha += (real_t)M_PI;
1581                     }
1582                 }
1583                 mu = c+1.0f/c;
1584                 mu = 1+(4.0f*rho*rho-4.0f)/(mu*mu);
1585                 gamma = (real_t)atan(sqrt((1.0f-sqrt(mu))/(1.0f+sqrt(mu))));
1586                 */
1587
1588                 if (ps->iid_mode >= 3)
1589                 {
1590                     uint8_t abs_iid = abs(ps->iid_index[env][bk]);
1591
1592                     cosa = sincos_alphas_B_fine[no_iid_steps + ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1593                     sina = sincos_alphas_B_fine[30 - (no_iid_steps + ps->iid_index[env][bk])][ps->icc_index[env][bk]];
1594                     cosg = cos_gammas_fine[abs_iid][ps->icc_index[env][bk]];
1595                     sing = sin_gammas_fine[abs_iid][ps->icc_index[env][bk]];
1596                 } else {
1597                     uint8_t abs_iid = abs(ps->iid_index[env][bk]);
1598
1599                     cosa = sincos_alphas_B_normal[no_iid_steps + ps->iid_index[env][bk]][ps->icc_index[env][bk]];
1600                     sina = sincos_alphas_B_normal[14 - (no_iid_steps + ps->iid_index[env][bk])][ps->icc_index[env][bk]];
1601                     cosg = cos_gammas_normal[abs_iid][ps->icc_index[env][bk]];
1602                     sing = sin_gammas_normal[abs_iid][ps->icc_index[env][bk]];
1603                 }
1604
1605                 RE(h11) = MUL_C(COEF_SQRT2, MUL_C(cosa, cosg));
1606                 RE(h12) = MUL_C(COEF_SQRT2, MUL_C(sina, cosg));
1607                 RE(h21) = MUL_C(COEF_SQRT2, MUL_C(-cosa, sing));
1608                 RE(h22) = MUL_C(COEF_SQRT2, MUL_C(sina, sing));
1609             }
1610
1611             /* calculate phase rotation parameters H_xy */
1612             /* note that the imaginary part of these parameters are only calculated when
1613                IPD and OPD are enabled
1614              */
1615             if ((ps->enable_ipdopd) && (bk < nr_ipdopd_par))
1616             {
1617                 int8_t i;
1618                 real_t xy, pq, xypq;
1619
1620                 /* ringbuffer index */
1621                 i = ps->phase_hist;
1622
1623                 /* previous value */
1624 #ifdef FIXED_POINT
1625                 /* divide by 4, shift right 2 bits */
1626                 RE(tempLeft)  = RE(ps->ipd_prev[bk][i]) >> 2;
1627                 IM(tempLeft)  = IM(ps->ipd_prev[bk][i]) >> 2;
1628                 RE(tempRight) = RE(ps->opd_prev[bk][i]) >> 2;
1629                 IM(tempRight) = IM(ps->opd_prev[bk][i]) >> 2;
1630 #else
1631                 RE(tempLeft)  = MUL_F(RE(ps->ipd_prev[bk][i]), FRAC_CONST(0.25));
1632                 IM(tempLeft)  = MUL_F(IM(ps->ipd_prev[bk][i]), FRAC_CONST(0.25));
1633                 RE(tempRight) = MUL_F(RE(ps->opd_prev[bk][i]), FRAC_CONST(0.25));
1634                 IM(tempRight) = MUL_F(IM(ps->opd_prev[bk][i]), FRAC_CONST(0.25));
1635 #endif
1636
1637                 /* save current value */
1638                 RE(ps->ipd_prev[bk][i]) = ipdopd_cos_tab[abs(ps->ipd_index[env][bk])];
1639                 IM(ps->ipd_prev[bk][i]) = ipdopd_sin_tab[abs(ps->ipd_index[env][bk])];
1640                 RE(ps->opd_prev[bk][i]) = ipdopd_cos_tab[abs(ps->opd_index[env][bk])];
1641                 IM(ps->opd_prev[bk][i]) = ipdopd_sin_tab[abs(ps->opd_index[env][bk])];
1642
1643                 /* add current value */
1644                 RE(tempLeft)  += RE(ps->ipd_prev[bk][i]);
1645                 IM(tempLeft)  += IM(ps->ipd_prev[bk][i]);
1646                 RE(tempRight) += RE(ps->opd_prev[bk][i]);
1647                 IM(tempRight) += IM(ps->opd_prev[bk][i]);
1648
1649                 /* ringbuffer index */
1650                 if (i == 0)
1651                 {
1652                     i = 2;
1653                 }
1654                 i--;
1655
1656                 /* get value before previous */
1657 #ifdef FIXED_POINT
1658                 /* dividing by 2, shift right 1 bit */
1659                 RE(tempLeft)  += (RE(ps->ipd_prev[bk][i]) >> 1);
1660                 IM(tempLeft)  += (IM(ps->ipd_prev[bk][i]) >> 1);
1661                 RE(tempRight) += (RE(ps->opd_prev[bk][i]) >> 1);
1662                 IM(tempRight) += (IM(ps->opd_prev[bk][i]) >> 1);
1663 #else
1664                 RE(tempLeft)  += MUL_F(RE(ps->ipd_prev[bk][i]), FRAC_CONST(0.5));
1665                 IM(tempLeft)  += MUL_F(IM(ps->ipd_prev[bk][i]), FRAC_CONST(0.5));
1666                 RE(tempRight) += MUL_F(RE(ps->opd_prev[bk][i]), FRAC_CONST(0.5));
1667                 IM(tempRight) += MUL_F(IM(ps->opd_prev[bk][i]), FRAC_CONST(0.5));
1668 #endif
1669
1670 #if 0 /* original code */
1671                 ipd = (float)atan2(IM(tempLeft), RE(tempLeft));
1672                 opd = (float)atan2(IM(tempRight), RE(tempRight));
1673
1674                 /* phase rotation */
1675                 RE(phaseLeft) = (float)cos(opd);
1676                 IM(phaseLeft) = (float)sin(opd);
1677                 opd -= ipd;
1678                 RE(phaseRight) = (float)cos(opd);
1679                 IM(phaseRight) = (float)sin(opd);
1680 #else
1681
1682                 // x = IM(tempLeft)
1683                 // y = RE(tempLeft)
1684                 // p = IM(tempRight)
1685                 // q = RE(tempRight)
1686                 // cos(atan2(x,y)) = y/sqrt((x*x) + (y*y))
1687                 // sin(atan2(x,y)) = x/sqrt((x*x) + (y*y))
1688                 // cos(atan2(x,y)-atan2(p,q)) = (y*q + x*p) / ( sqrt((x*x) + (y*y)) * sqrt((p*p) + (q*q)) );
1689                 // sin(atan2(x,y)-atan2(p,q)) = (x*q - y*p) / ( sqrt((x*x) + (y*y)) * sqrt((p*p) + (q*q)) );
1690
1691                 xy = magnitude_c(tempRight);
1692                 pq = magnitude_c(tempLeft);
1693
1694                 if (xy != 0)
1695                 {
1696                     RE(phaseLeft) = DIV_R(RE(tempRight), xy);
1697                     IM(phaseLeft) = DIV_R(IM(tempRight), xy);
1698                 } else {
1699                     RE(phaseLeft) = 0;
1700                     IM(phaseLeft) = 0;
1701                 }
1702
1703                 xypq = MUL_R(xy, pq);
1704
1705                 if (xypq != 0)
1706                 {
1707                     real_t tmp1 = MUL_R(RE(tempRight), RE(tempLeft)) + MUL_R(IM(tempRight), IM(tempLeft));
1708                     real_t tmp2 = MUL_R(IM(tempRight), RE(tempLeft)) - MUL_R(RE(tempRight), IM(tempLeft));
1709
1710                     RE(phaseRight) = DIV_R(tmp1, xypq);
1711                     IM(phaseRight) = DIV_R(tmp2, xypq);
1712                 } else {
1713                     RE(phaseRight) = 0;
1714                     IM(phaseRight) = 0;
1715                 }
1716
1717 #endif
1718
1719                 /* MUL_F(COEF, REAL) = COEF */
1720                 IM(h11) = MUL_R(RE(h11), IM(phaseLeft));
1721                 IM(h12) = MUL_R(RE(h12), IM(phaseRight));
1722                 IM(h21) = MUL_R(RE(h21), IM(phaseLeft));
1723                 IM(h22) = MUL_R(RE(h22), IM(phaseRight));
1724
1725                 RE(h11) = MUL_R(RE(h11), RE(phaseLeft));
1726                 RE(h12) = MUL_R(RE(h12), RE(phaseRight));
1727                 RE(h21) = MUL_R(RE(h21), RE(phaseLeft));
1728                 RE(h22) = MUL_R(RE(h22), RE(phaseRight));
1729             }
1730
1731             /* length of the envelope n_e+1 - n_e (in time samples) */
1732             /* 0 < L <= 32: integer */
1733             L = (real_t)(ps->border_position[env + 1] - ps->border_position[env]);
1734
1735             /* obtain final H_xy by means of linear interpolation */
1736             RE(deltaH11) = (RE(h11) - RE(ps->h11_prev[gr])) / L;
1737             RE(deltaH12) = (RE(h12) - RE(ps->h12_prev[gr])) / L;
1738             RE(deltaH21) = (RE(h21) - RE(ps->h21_prev[gr])) / L;
1739             RE(deltaH22) = (RE(h22) - RE(ps->h22_prev[gr])) / L;
1740
1741             RE(H11) = RE(ps->h11_prev[gr]);
1742             RE(H12) = RE(ps->h12_prev[gr]);
1743             RE(H21) = RE(ps->h21_prev[gr]);
1744             RE(H22) = RE(ps->h22_prev[gr]);
1745
1746             RE(ps->h11_prev[gr]) = RE(h11);
1747             RE(ps->h12_prev[gr]) = RE(h12);
1748             RE(ps->h21_prev[gr]) = RE(h21);
1749             RE(ps->h22_prev[gr]) = RE(h22);
1750
1751             /* only calculate imaginary part when needed */
1752             if ((ps->enable_ipdopd) && (bk < nr_ipdopd_par))
1753             {
1754                 /* obtain final H_xy by means of linear interpolation */
1755                 IM(deltaH11) = (IM(h11) - IM(ps->h11_prev[gr])) / L;
1756                 IM(deltaH12) = (IM(h12) - IM(ps->h12_prev[gr])) / L;
1757                 IM(deltaH21) = (IM(h21) - IM(ps->h21_prev[gr])) / L;
1758                 IM(deltaH22) = (IM(h22) - IM(ps->h22_prev[gr])) / L;
1759
1760                 IM(H11) = IM(ps->h11_prev[gr]);
1761                 IM(H12) = IM(ps->h12_prev[gr]);
1762                 IM(H21) = IM(ps->h21_prev[gr]);
1763                 IM(H22) = IM(ps->h22_prev[gr]);
1764
1765                 if ((NEGATE_IPD_MASK & ps->map_group2bk[gr]) != 0)
1766                 {
1767                     IM(deltaH11) = -IM(deltaH11);
1768                     IM(deltaH12) = -IM(deltaH12);
1769                     IM(deltaH21) = -IM(deltaH21);
1770                     IM(deltaH22) = -IM(deltaH22);
1771
1772                     IM(H11) = -IM(H11);
1773                     IM(H12) = -IM(H12);
1774                     IM(H21) = -IM(H21);
1775                     IM(H22) = -IM(H22);
1776                 }
1777
1778                 IM(ps->h11_prev[gr]) = IM(h11);
1779                 IM(ps->h12_prev[gr]) = IM(h12);
1780                 IM(ps->h21_prev[gr]) = IM(h21);
1781                 IM(ps->h22_prev[gr]) = IM(h22);
1782             }
1783
1784             /* apply H_xy to the current envelope band of the decorrelated subband */
1785             for (n = ps->border_position[env]; n < ps->border_position[env + 1]; n++)
1786             {
1787                 /* addition finalises the interpolation over every n */
1788                 RE(H11) += RE(deltaH11);
1789                 RE(H12) += RE(deltaH12);
1790                 RE(H21) += RE(deltaH21);
1791                 RE(H22) += RE(deltaH22);
1792                 if ((ps->enable_ipdopd) && (bk < nr_ipdopd_par))
1793                 {
1794                     IM(H11) += IM(deltaH11);
1795                     IM(H12) += IM(deltaH12);
1796                     IM(H21) += IM(deltaH21);
1797                     IM(H22) += IM(deltaH22);
1798                 }
1799
1800                 /* channel is an alias to the subband */
1801                 for (sb = ps->group_border[gr]; sb < maxsb; sb++)
1802                 {
1803                     complex_t inLeft, inRight;
1804
1805                     /* load decorrelated samples */
1806                     if (gr < ps->num_hybrid_groups)
1807                     {
1808                         RE(inLeft) =  RE(X_hybrid_left[n][sb]);
1809                         IM(inLeft) =  IM(X_hybrid_left[n][sb]);
1810                         RE(inRight) = RE(X_hybrid_right[n][sb]);
1811                         IM(inRight) = IM(X_hybrid_right[n][sb]);
1812                     } else {
1813                         RE(inLeft) =  RE(X_left[n][sb]);
1814                         IM(inLeft) =  IM(X_left[n][sb]);
1815                         RE(inRight) = RE(X_right[n][sb]);
1816                         IM(inRight) = IM(X_right[n][sb]);
1817                     }
1818
1819                     /* apply mixing */
1820                     RE(tempLeft) =  MUL_C(RE(H11), RE(inLeft)) + MUL_C(RE(H21), RE(inRight));
1821                     IM(tempLeft) =  MUL_C(RE(H11), IM(inLeft)) + MUL_C(RE(H21), IM(inRight));
1822                     RE(tempRight) = MUL_C(RE(H12), RE(inLeft)) + MUL_C(RE(H22), RE(inRight));
1823                     IM(tempRight) = MUL_C(RE(H12), IM(inLeft)) + MUL_C(RE(H22), IM(inRight));
1824
1825                     /* only perform imaginary operations when needed */
1826                     if ((ps->enable_ipdopd) && (bk < nr_ipdopd_par))
1827                     {
1828                         /* apply rotation */
1829                         RE(tempLeft)  -= MUL_C(IM(H11), IM(inLeft)) + MUL_C(IM(H21), IM(inRight));
1830                         IM(tempLeft)  += MUL_C(IM(H11), RE(inLeft)) + MUL_C(IM(H21), RE(inRight));
1831                         RE(tempRight) -= MUL_C(IM(H12), IM(inLeft)) + MUL_C(IM(H22), IM(inRight));
1832                         IM(tempRight) += MUL_C(IM(H12), RE(inLeft)) + MUL_C(IM(H22), RE(inRight));
1833                     }
1834
1835                     /* store final samples */
1836                     if (gr < ps->num_hybrid_groups)
1837                     {
1838                         RE(X_hybrid_left[n][sb])  = RE(tempLeft);
1839                         IM(X_hybrid_left[n][sb])  = IM(tempLeft);
1840                         RE(X_hybrid_right[n][sb]) = RE(tempRight);
1841                         IM(X_hybrid_right[n][sb]) = IM(tempRight);
1842                     } else {
1843                         RE(X_left[n][sb])  = RE(tempLeft);
1844                         IM(X_left[n][sb])  = IM(tempLeft);
1845                         RE(X_right[n][sb]) = RE(tempRight);
1846                         IM(X_right[n][sb]) = IM(tempRight);
1847                     }
1848                 }
1849             }
1850
1851             /* shift phase smoother's circular buffer index */
1852             ps->phase_hist++;
1853             if (ps->phase_hist == 2)
1854             {
1855                 ps->phase_hist = 0;
1856             }
1857         }
1858     }
1859 }
1860
1861 void ps_free(ps_info *ps)
1862 {
1863     /* free hybrid filterbank structures */
1864     hybrid_free(ps->hyb);
1865
1866     faad_free(ps);
1867 }
1868
1869 ps_info *ps_init(uint8_t sr_index, uint8_t numTimeSlotsRate)
1870 {
1871     uint8_t i;
1872     uint8_t short_delay_band;
1873
1874     ps_info *ps = (ps_info*)faad_malloc(sizeof(ps_info));
1875     memset(ps, 0, sizeof(ps_info));
1876
1877     ps->hyb = hybrid_init(numTimeSlotsRate);
1878     ps->numTimeSlotsRate = numTimeSlotsRate;
1879
1880     ps->ps_data_available = 0;
1881
1882     /* delay stuff*/
1883     ps->saved_delay = 0;
1884
1885     for (i = 0; i < 64; i++)
1886     {
1887         ps->delay_buf_index_delay[i] = 0;
1888     }
1889
1890     for (i = 0; i < NO_ALLPASS_LINKS; i++)
1891     {
1892         ps->delay_buf_index_ser[i] = 0;
1893 #ifdef PARAM_32KHZ
1894         if (sr_index <= 5) /* >= 32 kHz*/
1895         {
1896             ps->num_sample_delay_ser[i] = delay_length_d[1][i];
1897         } else {
1898             ps->num_sample_delay_ser[i] = delay_length_d[0][i];
1899         }
1900 #else
1901         /* THESE ARE CONSTANTS NOW */
1902         ps->num_sample_delay_ser[i] = delay_length_d[i];
1903 #endif
1904     }
1905
1906 #ifdef PARAM_32KHZ
1907     if (sr_index <= 5) /* >= 32 kHz*/
1908     {
1909         short_delay_band = 35;
1910         ps->nr_allpass_bands = 22;
1911         ps->alpha_decay = FRAC_CONST(0.76592833836465);
1912         ps->alpha_smooth = FRAC_CONST(0.25);
1913     } else {
1914         short_delay_band = 64;
1915         ps->nr_allpass_bands = 45;
1916         ps->alpha_decay = FRAC_CONST(0.58664621951003);
1917         ps->alpha_smooth = FRAC_CONST(0.6);
1918     }
1919 #else
1920     /* THESE ARE CONSTANTS NOW */
1921     short_delay_band = 35;
1922     ps->nr_allpass_bands = 22;
1923     ps->alpha_decay = FRAC_CONST(0.76592833836465);
1924     ps->alpha_smooth = FRAC_CONST(0.25);
1925 #endif
1926
1927     /* THESE ARE CONSTANT NOW IF PS IS INDEPENDANT OF SAMPLERATE */
1928     for (i = 0; i < short_delay_band; i++)
1929     {
1930         ps->delay_D[i] = 14;
1931     }
1932     for (i = short_delay_band; i < 64; i++)
1933     {
1934         ps->delay_D[i] = 1;
1935     }
1936
1937     /* mixing and phase */
1938     for (i = 0; i < 50; i++)
1939     {
1940         RE(ps->h11_prev[i]) = 1;
1941         IM(ps->h12_prev[i]) = 1;
1942         RE(ps->h11_prev[i]) = 1;
1943         IM(ps->h12_prev[i]) = 1;
1944     }
1945
1946     ps->phase_hist = 0;
1947
1948     for (i = 0; i < 20; i++)
1949     {
1950         RE(ps->ipd_prev[i][0]) = 0;
1951         IM(ps->ipd_prev[i][0]) = 0;
1952         RE(ps->ipd_prev[i][1]) = 0;
1953         IM(ps->ipd_prev[i][1]) = 0;
1954         RE(ps->opd_prev[i][0]) = 0;
1955         IM(ps->opd_prev[i][0]) = 0;
1956         RE(ps->opd_prev[i][1]) = 0;
1957         IM(ps->opd_prev[i][1]) = 0;
1958     }
1959
1960     return ps;
1961 }
1962
1963 /* main Parametric Stereo decoding function */
1964 uint8_t ps_decode(ps_info *ps, qmf_t X_left[38][64], qmf_t X_right[38][64])
1965 {
1966     qmf_t X_hybrid_left[32][32] = {{0}};
1967     qmf_t X_hybrid_right[32][32] = {{0}};
1968
1969     /* delta decoding of the bitstream data */
1970     ps_data_decode(ps);
1971
1972     /* set up some parameters depending on filterbank type */
1973     if (ps->use34hybrid_bands)
1974     {
1975         ps->group_border = (uint8_t*)group_border34;
1976         ps->map_group2bk = (uint16_t*)map_group2bk34;
1977         ps->num_groups = 32+18;
1978         ps->num_hybrid_groups = 32;
1979         ps->nr_par_bands = 34;
1980         ps->decay_cutoff = 5;
1981     } else {
1982         ps->group_border = (uint8_t*)group_border20;
1983         ps->map_group2bk = (uint16_t*)map_group2bk20;
1984         ps->num_groups = 10+12;
1985         ps->num_hybrid_groups = 10;
1986         ps->nr_par_bands = 20;
1987         ps->decay_cutoff = 3;
1988     }
1989
1990     /* Perform further analysis on the lowest subbands to get a higher
1991      * frequency resolution
1992      */
1993     hybrid_analysis((hyb_info*)ps->hyb, X_left, X_hybrid_left,
1994         ps->use34hybrid_bands, ps->numTimeSlotsRate);
1995
1996     /* decorrelate mono signal */
1997     ps_decorrelate(ps, X_left, X_right, X_hybrid_left, X_hybrid_right);
1998
1999     /* apply mixing and phase parameters */
2000     ps_mix_phase(ps, X_left, X_right, X_hybrid_left, X_hybrid_right);
2001
2002     /* hybrid synthesis, to rebuild the SBR QMF matrices */
2003     hybrid_synthesis((hyb_info*)ps->hyb, X_left, X_hybrid_left,
2004         ps->use34hybrid_bands, ps->numTimeSlotsRate);
2005
2006     hybrid_synthesis((hyb_info*)ps->hyb, X_right, X_hybrid_right,
2007         ps->use34hybrid_bands, ps->numTimeSlotsRate);
2008
2009     return 0;
2010 }
2011
2012 #endif
2013