/* synfilt.cpp

	Synthesis Filter implementation */

/* -- 03/20/97 --
 *  compute_new_v() -- reoptimized with the assumption that constant offsets
 *    to memory are free.  Common subexpression were redone for better
 *    optimization.
 *  compute_pcm_samples() -- reoptimized with constant offsets.
 *
 *  -- Conrad Wei-Li Song (conradsong@mail.utexas.edu)
 */

/*
 *  @(#) synthesis_filter.cc 1.14, last edit: 6/21/94 11:22:20
 *  @(#) Copyright (C) 1993, 1994 Tobias Bading (bading@cs.tu-berlin.de)
 *  @(#) Berlin University of Technology
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program 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 General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/*
 *  Changes from version 1.1 to 1.2:
 *    - compute_new_v() uses a 32 point fast cosine transform as described by
 *      Byeong Gi Lee in IEEE Transactions ASSP-32 Part 2, August 1984,
 *      "A New Algorithm to Compute the Discrete Cosine Transform"
 *      instead of the matrix-vector multiplication in V1.1
 *    - loop unrolling done in compute_pcm_samples()
 *    - if ULAW is defined, the synthesis filter does a downsampling
 *      to 8 kHz by dropping samples and ignoring subbands above 4 kHz
 */

#include <math.h>
#include "all.h"
#include "synfilt.h"

#define MY_PI 3.14159265358979323846

#include "scalfact.h"

#ifdef __WIN32__
#pragma warning (disable: 4244 4305)
#endif

static const real cos1_64  = (real)(1.0 / (2.0 * cos(MY_PI / 64.0)));
static const real cos3_64  = (real)(.0 / (2.0 * cos(MY_PI * 3.0  / 64.0)));
static const real cos5_64  = (real)(1.0 / (2.0 * cos(MY_PI * 5.0  / 64.0)));
static const real cos7_64  = (real)(1.0 / (2.0 * cos(MY_PI * 7.0  / 64.0)));
static const real cos9_64  = (real)(1.0 / (2.0 * cos(MY_PI * 9.0  / 64.0)));
static const real cos11_64 = (real)(1.0 / (2.0 * cos(MY_PI * 11.0 / 64.0)));
static const real cos13_64 = (real)(1.0 / (2.0 * cos(MY_PI * 13.0 / 64.0)));
static const real cos15_64 = (real)(1.0 / (2.0 * cos(MY_PI * 15.0 / 64.0)));
static const real cos17_64 = (real)(1.0 / (2.0 * cos(MY_PI * 17.0 / 64.0)));
static const real cos19_64 = (real)(1.0 / (2.0 * cos(MY_PI * 19.0 / 64.0)));
static const real cos21_64 = (real)(1.0 / (2.0 * cos(MY_PI * 21.0 / 64.0)));
static const real cos23_64 = (real)(1.0 / (2.0 * cos(MY_PI * 23.0 / 64.0)));
static const real cos25_64 = (real)(1.0 / (2.0 * cos(MY_PI * 25.0 / 64.0)));
static const real cos27_64 = (real)(1.0 / (2.0 * cos(MY_PI * 27.0 / 64.0)));
static const real cos29_64 = (real)(1.0 / (2.0 * cos(MY_PI * 29.0 / 64.0)));
static const real cos31_64 = (real)(1.0 / (2.0 * cos(MY_PI * 31.0 / 64.0)));
static const real cos1_32  = (real)(1.0 / (2.0 * cos(MY_PI        / 32.0)));
static const real cos3_32  = (real)(1.0 / (2.0 * cos(MY_PI * 3.0  / 32.0)));
static const real cos5_32  = (real)(1.0 / (2.0 * cos(MY_PI * 5.0  / 32.0)));
static const real cos7_32  = (real)(1.0 / (2.0 * cos(MY_PI * 7.0  / 32.0)));
static const real cos9_32  = (real)(1.0 / (2.0 * cos(MY_PI * 9.0  / 32.0)));
static const real cos11_32 = (real)(1.0 / (2.0 * cos(MY_PI * 11.0 / 32.0)));
static const real cos13_32 = (real)(1.0 / (2.0 * cos(MY_PI * 13.0 / 32.0)));
static const real cos15_32 = (real)(1.0 / (2.0 * cos(MY_PI * 15.0 / 32.0)));
static const real cos1_16  = (real)(1.0 / (2.0 * cos(MY_PI        / 16.0)));
static const real cos3_16  = (real)(1.0 / (2.0 * cos(MY_PI * 3.0  / 16.0)));
static const real cos5_16  = (real)(1.0 / (2.0 * cos(MY_PI * 5.0  / 16.0)));
static const real cos7_16  = (real)(1.0 / (2.0 * cos(MY_PI * 7.0  / 16.0)));
static const real cos1_8   = (real)(1.0 / (2.0 * cos(MY_PI        / 8.0)));
static const real cos3_8   = (real)(1.0 / (2.0 * cos(MY_PI * 3.0  / 8.0)));
static const real cos1_4   = (real)(1.0 / (2.0 * cos(MY_PI / 4.0)));


const real SynthesisFilter::d[512] = {
	// Note: These values are not in the same order
	// as in Annex 3-B.3 of the ISO/IEC DIS 11172-3
	0.000000000f, -0.000442505f,  0.003250122f, -0.007003784f,
	0.031082153f, -0.078628540f,  0.100311279f, -0.572036743f,
	1.144989014f,  0.572036743f,  0.100311279f,  0.078628540f,
	0.031082153f,  0.007003784f,  0.003250122f,  0.000442505f,
	-0.000015259f, -0.000473022f,  0.003326416f, -0.007919312f,
	0.030517578f, -0.084182739f,  0.090927124f, -0.600219727f,
	1.144287109f,  0.543823242f,  0.108856201f,  0.073059082f,
	0.031478882f,  0.006118774f,  0.003173828f,  0.000396729f,
  -0.000015259f, -0.000534058f,  0.003387451f, -0.008865356f,
	0.029785156f, -0.089706421f,  0.080688477f, -0.628295898f,
	1.142211914f,  0.515609741f,  0.116577148f,  0.067520142f,
	 0.031738281f,  0.005294800f,  0.003082275f,  0.000366211f,
	-0.000015259f, -0.000579834f,  0.003433228f, -0.009841919f,
	 0.028884888f, -0.095169067f,  0.069595337f, -0.656219482f,
	1.138763428f,  0.487472534f,  0.123474121f,  0.061996460f,
   0.031845093f,  0.004486084f,  0.002990723f,  0.000320435f,
  -0.000015259f, -0.000625610f,  0.003463745f, -0.010848999f,
	 0.027801514f, -0.100540161f,  0.057617188f, -0.683914185f,
	1.133926392f,  0.459472656f,  0.129577637f,  0.056533813f,
	0.031814575f,  0.003723145f,  0.002899170f,  0.000289917f,
  -0.000015259f, -0.000686646f,  0.003479004f, -0.011886597f,
	0.026535034f, -0.105819702f,  0.044784546f, -0.711318970f,
	1.127746582f,  0.431655884f,  0.134887695f,  0.051132202f,
	0.031661987f,  0.003005981f,  0.002792358f,  0.000259399f,
	-0.000015259f, -0.000747681f,  0.003479004f, -0.012939453f,
	0.025085449f, -0.110946655f,  0.031082153f, -0.738372803f,
   1.120223999f,  0.404083252f,  0.139450073f,  0.045837402f,
   0.031387329f,  0.002334595f,  0.002685547f,  0.000244141f,
  -0.000030518f, -0.000808716f,  0.003463745f, -0.014022827f,
   0.023422241f, -0.115921021f,  0.016510010f, -0.765029907f,
	1.111373901f,  0.376800537f,  0.143264771f,  0.040634155f,
   0.031005859f,  0.001693726f,  0.002578735f,  0.000213623f,
	-0.000030518f, -0.000885010f,  0.003417969f, -0.015121460f,
	0.021575928f, -0.120697021f,  0.001068115f, -0.791213989f,
   1.101211548f,  0.349868774f,  0.146362305f,  0.035552979f,
	0.030532837f,  0.001098633f,  0.002456665f,  0.000198364f,
	-0.000030518f, -0.000961304f,  0.003372192f, -0.016235352f,
   0.019531250f, -0.125259399f, -0.015228271f, -0.816864014f,
	 1.089782715f,  0.323318481f,  0.148773193f,  0.030609131f,
	0.029937744f,  0.000549316f,  0.002349854f,  0.000167847f,
	-0.000030518f, -0.001037598f,  0.003280640f, -0.017349243f,
	0.017257690f, -0.129562378f, -0.032379150f, -0.841949463f,
   1.077117920f,  0.297210693f,  0.150497437f,  0.025817871f,
   0.029281616f,  0.000030518f,  0.002243042f,  0.000152588f,
  -0.000045776f, -0.001113892f,  0.003173828f, -0.018463135f,
	0.014801025f, -0.133590698f, -0.050354004f, -0.866363525f,
	1.063217163f,  0.271591187f,  0.151596069f,  0.021179199f,
	0.028533936f, -0.000442505f,  0.002120972f,  0.000137329f,
	-0.000045776f, -0.001205444f,  0.003051758f, -0.019577026f,
	0.012115479f, -0.137298584f, -0.069168091f, -0.890090942f,
	 1.048156738f,  0.246505737f,  0.152069092f,  0.016708374f,
	0.027725220f, -0.000869751f,  0.002014160f,  0.000122070f,
  -0.000061035f, -0.001296997f,  0.002883911f, -0.020690918f,
   0.009231567f, -0.140670776f, -0.088775635f, -0.913055420f,
	 1.031936646f,  0.221984863f,  0.151962280f,  0.012420654f,
   0.026840210f, -0.001266479f,  0.001907349f,  0.000106812f,
  -0.000061035f, -0.001388550f,  0.002700806f, -0.021789551f,
	0.006134033f, -0.143676758f, -0.109161377f, -0.935195923f,
   1.014617920f,  0.198059082f,  0.151306152f,  0.008316040f,
	0.025909424f, -0.001617432f,  0.001785278f,  0.000106812f,
	-0.000076294f, -0.001480103f,  0.002487183f, -0.022857666f,
	 0.002822876f, -0.146255493f, -0.130310059f, -0.956481934f,
	0.996246338f,  0.174789429f,  0.150115967f,  0.004394531f,
   0.024932861f, -0.001937866f,  0.001693726f,  0.000091553f,
  -0.000076294f, -0.001586914f,  0.002227783f, -0.023910522f,
	-0.000686646f, -0.148422241f, -0.152206421f, -0.976852417f,
   0.976852417f,  0.152206421f,  0.148422241f,  0.000686646f,
	0.023910522f, -0.002227783f,  0.001586914f,  0.000076294f,
	-0.000091553f, -0.001693726f,  0.001937866f, -0.024932861f,
	-0.004394531f, -0.150115967f, -0.174789429f, -0.996246338f,
   0.956481934f,  0.130310059f,  0.146255493f, -0.002822876f,
   0.022857666f, -0.002487183f,  0.001480103f,  0.000076294f,
	-0.000106812f, -0.001785278f,  0.001617432f, -0.025909424f,
  -0.008316040f, -0.151306152f, -0.198059082f, -1.014617920f,
   0.935195923f,  0.109161377f,  0.143676758f, -0.006134033f,
   0.021789551f, -0.002700806f,  0.001388550f,  0.000061035f,
  -0.000106812f, -0.001907349f,  0.001266479f, -0.026840210f,
	-0.012420654f, -0.151962280f, -0.221984863f, -1.031936646f,
	0.913055420f,  0.088775635f,  0.140670776f, -0.009231567f,
	 0.020690918f, -0.002883911f,  0.001296997f,  0.000061035f,
	-0.000122070f, -0.002014160f,  0.000869751f, -0.027725220f,
  -0.016708374f, -0.152069092f, -0.246505737f, -1.048156738f,
   0.890090942f,  0.069168091f,  0.137298584f, -0.012115479f,
	0.019577026f, -0.003051758f,  0.001205444f,  0.000045776f,
	-0.000137329f, -0.002120972f,  0.000442505f, -0.028533936f,
  -0.021179199f, -0.151596069f, -0.271591187f, -1.063217163f,
   0.866363525f,  0.050354004f,  0.133590698f, -0.014801025f,
   0.018463135f, -0.003173828f,  0.001113892f,  0.000045776f,
  -0.000152588f, -0.002243042f, -0.000030518f, -0.029281616f,
  -0.025817871f, -0.150497437f, -0.297210693f, -1.077117920f,
	 0.841949463f,  0.032379150f,  0.129562378f, -0.017257690f,
	 0.017349243f, -0.003280640f,  0.001037598f,  0.000030518f,
  -0.000167847f, -0.002349854f, -0.000549316f, -0.029937744f,
  -0.030609131f, -0.148773193f, -0.323318481f, -1.089782715f,
	0.816864014f,  0.015228271f,  0.125259399f, -0.019531250f,
   0.016235352f, -0.003372192f,  0.000961304f,  0.000030518f,
	-0.000198364f, -0.002456665f, -0.001098633f, -0.030532837f,
	-0.035552979f, -0.146362305f, -0.349868774f, -1.101211548f,
	 0.791213989f, -0.001068115f,  0.120697021f, -0.021575928f,
	0.015121460f, -0.003417969f,  0.000885010f,  0.000030518f,
  -0.000213623f, -0.002578735f, -0.001693726f, -0.031005859f,
	-0.040634155f, -0.143264771f, -0.376800537f, -1.111373901f,
   0.765029907f, -0.016510010f,  0.115921021f, -0.023422241f,
   0.014022827f, -0.003463745f,  0.000808716f,  0.000030518f,
  -0.000244141f, -0.002685547f, -0.002334595f, -0.031387329f,
  -0.045837402f, -0.139450073f, -0.404083252f, -1.120223999f,
   0.738372803f, -0.031082153f,  0.110946655f, -0.025085449f,
	0.012939453f, -0.003479004f,  0.000747681f,  0.000015259f,
	-0.000259399f, -0.002792358f, -0.003005981f, -0.031661987f,
  -0.051132202f, -0.134887695f, -0.431655884f, -1.127746582f,
	0.711318970f, -0.044784546f,  0.105819702f, -0.026535034f,
   0.011886597f, -0.003479004f,  0.000686646f,  0.000015259f,
	-0.000289917f, -0.002899170f, -0.003723145f, -0.031814575f,
	-0.056533813f, -0.129577637f, -0.459472656f, -1.133926392f,
   0.683914185f, -0.057617188f,  0.100540161f, -0.027801514f,
	0.010848999f, -0.003463745f,  0.000625610f,  0.000015259f,
  -0.000320435f, -0.002990723f, -0.004486084f, -0.031845093f,
  -0.061996460f, -0.123474121f, -0.487472534f, -1.138763428f,
	0.656219482f, -0.069595337f,  0.095169067f, -0.028884888f,
	0.009841919f, -0.003433228f,  0.000579834f,  0.000015259f,
	-0.000366211f, -0.003082275f, -0.005294800f, -0.031738281f,
  -0.067520142f, -0.116577148f, -0.515609741f, -1.142211914f,
	0.628295898f, -0.080688477f,  0.089706421f, -0.029785156f,
	0.008865356f, -0.003387451f,  0.000534058f,  0.000015259f,
  -0.000396729f, -0.003173828f, -0.006118774f, -0.031478882f,
  -0.073059082f, -0.108856201f, -0.543823242f, -1.144287109f,
	0.600219727f, -0.090927124f,  0.084182739f, -0.030517578f,
	0.007919312f, -0.003326416f,  0.000473022f,  0.000015259f
};

SynthesisFilter::SynthesisFilter(uint32 channelnumber, real factor)
{
  channel = channelnumber;
  scalefactor = factor;

  reset();
}

void SynthesisFilter::reset()
{
  register real *floatp, *floatp2;

  // initialize v1[] and v2[]:
  for (floatp = v1 + 512, floatp2 = v2 + 512; floatp > v1; )
	 *--floatp = *--floatp2 = 0.0;

  // initialize samples[]:
  for (floatp = samples + 32; floatp > samples; )
	 *--floatp = 0.0;

  actual_v = v1;
  actual_write_pos = 15;
}

void SynthesisFilter::compute_new_v()
{
	real new_v[32];         // new V[0-15] and V[33-48] of Figure 3-A.2 in ISO DIS 11172-3
	real p[16];
	real pp[16];

	// compute new values via a fast cosine transform:
	{
	register real *x1 = samples;

	p[0] = x1[0] + x1[31];
	p[1] = x1[1] + x1[30];
	p[2] = x1[2] + x1[29];
	p[3] = x1[3] + x1[28];
	p[4] = x1[4] + x1[27];
	p[5] = x1[5] + x1[26];
	p[6] = x1[6] + x1[25];
	p[7] = x1[7] + x1[24];
	p[8] = x1[8] + x1[23];
	p[9] = x1[9] + x1[22];
	p[10] = x1[10] + x1[21];
	p[11] = x1[11] + x1[20];
	p[12] = x1[12] + x1[19];
	p[13] = x1[13] + x1[18];
	p[14] = x1[14] + x1[17];
	p[15] = x1[15] + x1[16];
	}

	{
	pp[0] = p[0] + p[15];
	pp[1] = p[1] + p[14];
	pp[2] = p[2] + p[13];
	pp[3] = p[3] + p[12];
	pp[4] = p[4] + p[11];
	pp[5] = p[5] + p[10];
	pp[6] = p[6] + p[9];
	pp[7] = p[7] + p[8];
	pp[8] = (p[0] - p[15]) * cos1_32;
	pp[9] = (p[1] - p[14]) * cos3_32;
	pp[10] = (p[2] - p[13]) * cos5_32;
	pp[11] = (p[3] - p[12]) * cos7_32;
	pp[12] = (p[4] - p[11]) * cos9_32;
	pp[13] = (p[5] - p[10]) * cos11_32;
	pp[14] = (p[6] - p[9]) * cos13_32;
	pp[15] = (p[7] - p[8]) * cos15_32;
	}

	{
	p[0] = pp[0] + pp[7];
	p[1] = pp[1] + pp[6];
	p[2] = pp[2] + pp[5];
	p[3] = pp[3] + pp[4];
	p[4] = (pp[0] - pp[7]) * cos1_16;
	p[5] = (pp[1] - pp[6]) * cos3_16;
	p[6] = (pp[2] - pp[5]) * cos5_16;
	p[7] = (pp[3] - pp[4]) * cos7_16;
	p[8] = pp[8] + pp[15];
	p[9] = pp[9] + pp[14];
	p[10] = pp[10] + pp[13];
	p[11] = pp[11] + pp[12];
	p[12] = (pp[8] - pp[15]) * cos1_16;
	p[13] = (pp[9] - pp[14]) * cos3_16;
	p[14] = (pp[10] - pp[13]) * cos5_16;
	p[15] = (pp[11] - pp[12]) * cos7_16;
	}

	{
	pp[0] = p[0] + p[3];
	pp[1] = p[1] + p[2];
	pp[2] = (p[0] - p[3]) * cos1_8;
	pp[3] = (p[1] - p[2]) * cos3_8;
	pp[4] = p[4] + p[7];
	pp[5] = p[5] + p[6];
	pp[6] = (p[4] - p[7]) * cos1_8;
	pp[7] = (p[5] - p[6]) * cos3_8;
	pp[8] = p[8] + p[11];
	pp[9] = p[9] + p[10];
	pp[10] = (p[8] - p[11]) * cos1_8;
	pp[11] = (p[9] - p[10]) * cos3_8;
	pp[12] = p[12] + p[15];
	pp[13] = p[13] + p[14];
	pp[14] = (p[12] - p[15]) * cos1_8;
	pp[15] = (p[13] - p[14]) * cos3_8;
	}

	{
	p[0] = pp[0] + pp[1];
	p[1] = (pp[0] - pp[1]) * cos1_4;
	p[2] = pp[2] + pp[3];
	p[3] = (pp[2] - pp[3]) * cos1_4;
	p[4] = pp[4] + pp[5];
	p[5] = (pp[4] - pp[5]) * cos1_4;
	p[6] = pp[6] + pp[7];
	p[7] = (pp[6] - pp[7]) * cos1_4;
	p[8] = pp[8] + pp[9];
	p[9] = (pp[8] - pp[9]) * cos1_4;
	p[10] = pp[10] + pp[11];
	p[11] = (pp[10] - pp[11]) * cos1_4;
	p[12] = pp[12] + pp[13];
	p[13] = (pp[12] - pp[13]) * cos1_4;
	p[14] = pp[14] + pp[15];
	p[15] = (pp[14] - pp[15]) * cos1_4;
	}

	{