/*
   File: subband_layer_2.cc
*/

#include <stdio.h>
#include <stdlib.h>
#include <fstream.h>
#ifdef IRIX
#include <dmedia/audio.h>
#endif
#ifdef SOLARIS
#include <sys/audioio.h>
#endif

#include "athread.hh"

#include "error.hh"
#include "debug.hh"
#include "util.hh"

#include "sync.hh"
#include "mpeg2const.hh"
#include "mpeg2buff.hh"

#include "astream.hh"
#include "crc.hh"
#include "header.hh"
#include "obuffer.hh"
#include "synthesis_filter.hh"
#include "subband.hh"
#include "subband_layer_1.hh"
#include "subband_layer_2.hh"
#include "scalefactors.hh"


// data taken from ISO/IEC DIS 11172, Annexes 3-B.2[abcd] and 3-B.4:

// subbands 0-2 in tables 3-B.2a and 2b: (index is allocation)
static const uint32 table_ab1_codelength[16] =
  // bits per codeword
{ 0, 5, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 };

static const uint32 table_ab1_quantizationsteps[16] =
  // number of steps if grouped, 0 if ungrouped
{ 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

static const real table_ab1_factor[16] =
  // factor for requantization: (real)sample * factor - 1.0 gives requantized sample
{ 0.0, 1.0/2.0, 1.0/4.0, 1.0/8.0, 1.0/16.0, 1.0/32.0, 1.0/64.0,
  1.0/128.0, 1.0/256.0, 1.0/512.0, 1.0/1024.0, 1.0/2048.0,
  1.0/4096.0, 1.0/8192.0, 1.0/16384.0, 1.0/32768.0 };

static const real table_ab1_c[16] =
  // factor c for requantization from table 3-B.4
{ 0.0,           1.33333333333, 1.14285714286, 1.06666666666, 1.03225806452,
  1.01587301587, 1.00787401575, 1.00392156863, 1.00195694716, 1.00097751711,
  1.00048851979, 1.00024420024, 1.00012208522, 1.00006103888, 1.00003051851,
  1.00001525902 };
static const real table_ab1_d[16] =
  // addend d for requantization from table 3-B.4
{ 0.0,           0.50000000000, 0.25000000000, 0.12500000000, 0.06250000000,
  0.03125000000, 0.01562500000, 0.00781250000, 0.00390625000, 0.00195312500,
  0.00097656250, 0.00048828125, 0.00024414063, 0.00012207031, 0.00006103516,
  0.00003051758 };

// subbands 3-... tables 3-B.2a and 2b:
static const uint32 table_ab234_quantizationsteps[16] =
{ 0, 3, 5, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

// subbands 3-10 in tables 3-B.2a and 2b:
static const uint32 table_ab2_codelength[16] =
{ 0, 5, 7, 3, 10, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 16 };
static const real table_ab2_factor[16] =
{ 0.0, 1.0/2.0, 1.0/4.0, 1.0/4.0, 1.0/8.0, 1.0/8.0, 1.0/16.0,
  1.0/32.0, 1.0/64.0, 1.0/128.0, 1.0/256.0, 1.0/512.0,
  1.0/1024.0, 1.0/2048.0, 1.0/4096.0, 1.0/32768.0 };
static const real table_ab2_c[16] =
{ 0.0,           1.33333333333, 1.60000000000, 1.14285714286, 1.77777777777,
  1.06666666666, 1.03225806452, 1.01587301587, 1.00787401575, 1.00392156863,
  1.00195694716, 1.00097751711, 1.00048851979, 1.00024420024, 1.00012208522,
  1.00001525902 };
static const real table_ab2_d[16] =
{ 0.0,           0.50000000000, 0.50000000000, 0.25000000000, 0.50000000000,
  0.12500000000, 0.06250000000, 0.03125000000, 0.01562500000, 0.00781250000,
  0.00390625000, 0.00195312500, 0.00097656250, 0.00048828125, 0.00024414063,
  0.00003051758 };

// subbands 11-22 in tables 3-B.2a and 2b:
static const uint32 table_ab3_codelength[8] = { 0, 5, 7, 3, 10, 4, 5, 16 };
static const real table_ab3_factor[8] =
{ 0.0, 1.0/2.0, 1.0/4.0, 1.0/4.0, 1.0/8.0, 1.0/8.0, 1.0/16.0, 1.0/32768.0 };
static const real table_ab3_c[8] =
{ 0.0,           1.33333333333, 1.60000000000, 1.14285714286, 1.77777777777,
  1.06666666666, 1.03225806452, 1.00001525902 };
static const real table_ab3_d[8] =
{ 0.0,           0.50000000000, 0.50000000000, 0.25000000000, 0.50000000000,
  0.12500000000, 0.06250000000, 0.00003051758 };

// subbands 23-... in tables 3-B.2a and 2b:
static const uint32 table_ab4_codelength[4] = { 0, 5, 7, 16 };
static const real table_ab4_factor[8] = { 0.0, 1.0/2.0, 1.0/4.0, 1.0/32768.0 };
static const real table_ab4_c[4] = { 0.0, 1.33333333333, 1.60000000000, 1.00001525902 };
static const real table_ab4_d[4] = { 0.0, 0.50000000000, 0.50000000000, 0.00003051758 };

// subbands in tables 3-B.2c and 2d:
static const uint32 table_cd_codelength[16] =
{ 0, 5, 7, 10, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 };
static const uint32 table_cd_quantizationsteps[16] =
{ 0, 3, 5, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
static const real table_cd_factor[16] =
{ 0.0, 1.0/2.0, 1.0/4.0, 1.0/8.0, 1.0/8.0, 1.0/16.0, 1.0/32.0, 1.0/64.0,
  1.0/128.0, 1.0/256.0, 1.0/512.0, 1.0/1024.0, 1.0/2048.0, 1.0/4096.0,
  1.0/8192.0, 1.0/16384.0 };
static const real table_cd_c[16] =
{ 0.0,           1.33333333333, 1.60000000000, 1.77777777777, 1.06666666666,
  1.03225806452, 1.01587301587, 1.00787401575, 1.00392156863, 1.00195694716,
  1.00097751711, 1.00048851979, 1.00024420024, 1.00012208522, 1.00006103888,
  1.00003051851 };
static const real table_cd_d[16] =
{ 0.0,           0.50000000000, 0.50000000000, 0.50000000000, 0.12500000000,
  0.06250000000, 0.03125000000, 0.01562500000, 0.00781250000, 0.00390625000,
  0.00195312500, 0.00097656250, 0.00048828125, 0.00024414063, 0.00012207031,
  0.00006103516 };



/**********************/	// used for single channel mode
/*** Standard Class ***/	// and in derived class for intensity
/**********************/	// stereo mode

SubbandLayer2::SubbandLayer2(uint32 subbandnumber){
  this->subbandnumber=subbandnumber;
  groupnumber=samplenumber=0;
}


uint32 SubbandLayer2::get_allocationlength (Header *header){
  uint32 channel_bitrate = header->bitrate_index ();

  // calculate bitrate per channel:
  if (header->mode () != single_channel)
    if (channel_bitrate == 4)
      channel_bitrate = 1;
    else
      channel_bitrate -= 4;

  if (channel_bitrate == 1 || channel_bitrate == 2)
    // table 3-B.2c or 3-B.2d
    if (subbandnumber <= 1)
      return 4;
    else
      return 3;
  else
    // tables 3-B.2a or 3-B.2b
    if (subbandnumber <= 10)
      return 4;
    else if (subbandnumber <= 22)
      return 3;
    else
      return 2;
}


uint32 SubbandLayer2::prepare_sample_reading(Header *header, uint32 allocation, bool *grouping,
	 uint32 *quantizationsteps, real *factor, uint32 *codelength, real *c, real *d){
  uint32 channel_bitrate = header->bitrate_index ();

  // calculate bitrate per channel:
  if (header->mode () != single_channel)
    if (channel_bitrate == 4)  channel_bitrate = 1;
    else                       channel_bitrate -= 4;

  *grouping = False;
  if (channel_bitrate == 1 || channel_bitrate == 2){
    // table 3-B.2c or 3-B.2d
    if ((*quantizationsteps=table_cd_quantizationsteps[allocation])!=0)
      *grouping=True;
    *factor=table_cd_factor[allocation];
    *codelength=table_cd_codelength[allocation];
    *c=table_cd_c[allocation];
    *d=table_cd_d[allocation];
  }
  else {
    // tables 3-B.2a or 3-B.2b
    if (subbandnumber <= 2){
      if ((*quantizationsteps=table_ab1_quantizationsteps[allocation])!=0)
        *grouping = True;
      *factor = table_ab1_factor[allocation];
      *codelength = table_ab1_codelength[allocation];
      *c = table_ab1_c[allocation];
      *d = table_ab1_d[allocation];
    }
    else {
      if ((*quantizationsteps=table_ab234_quantizationsteps[allocation])!=0)
        *grouping = True;
      if (subbandnumber <= 10){
        *factor = table_ab2_factor[allocation];
        *codelength = table_ab2_codelength[allocation];
        *c = table_ab2_c[allocation];
        *d = table_ab2_d[allocation];
      }
      else if (subbandnumber <= 22){
        *factor = table_ab3_factor[allocation];
        *codelength = table_ab3_codelength[allocation];
        *c = table_ab3_c[allocation];
        *d = table_ab3_d[allocation];
      }
      else {
        *factor = table_ab4_factor[allocation];
        *codelength = table_ab4_codelength[allocation];
        *c = table_ab4_c[allocation];
        *d = table_ab4_d[allocation];
      }
    }
  }
  
  return channel_bitrate;
}


void SubbandLayer2::read_allocation(AudioStream *stream, Header *header, Crc16 *crc){
  register uint32 length = get_allocationlength (header);
  allocation = stream->get_bits (length);
  if (crc)
    crc->add_bits (allocation, length);
}


void SubbandLayer2::read_scalefactor_selection(AudioStream *stream, Crc16 *crc){
  if (allocation){
    scfsi = stream->get_bits (2);
    if (crc) crc->add_bits (scfsi, 2);
  }
}


void SubbandLayer2::read_scalefactor(AudioStream *stream, Header *header){
  if (allocation){
    switch (scfsi){
      case 0:
	scalefactor1 = stream->get_bits (6);
	scalefactor2 = stream->get_bits (6);
	scalefactor3 = stream->get_bits (6);
	break;
      case 1:
	scalefactor1 = scalefactor2 = stream->get_bits (6);
	scalefactor3 = stream->get_bits (6);
	break;
      case 2:
	scalefactor1 = scalefactor2 = scalefactor3 = stream->get_bits (6);
	break;
      case 3:
	scalefactor1 = stream->get_bits (6);
	scalefactor2 = scalefactor3 = stream->get_bits (6);
	break;
    }
    if (scalefactor1 == 63 || scalefactor2 == 63 || scalefactor3 == 63)
      cerr << "WARNING: stream contains an illegal scalefactor!\n";	// MPEG-stream is corrupted!

    prepare_sample_reading (header, allocation, &grouping, &quantizationsteps,
			    &factor, &codelength, &c, &d);
  }
}


bool SubbandLayer2::read_sampledata(AudioStream *stream){
  if (allocation)
    if (grouping){
      uint32 samplecode = stream->get_bits (codelength);
#ifdef DEBUG
      if (samplecode == (1 << codelength) - 1)
        cerr << "WARNING: stream contains an illegal subband sample!\n";  // MPEG-stream is corrupted!
#endif
      samples[0] = real (samplecode % quantizationsteps) * factor - 1.0;
      samplecode /= quantizationsteps;
      samples[1] = real (samplecode % quantizationsteps) * factor - 1.0;
      samplecode /= quantizationsteps;
      samples[2] = real (samplecode % quantizationsteps) * factor - 1.0;
    }
    else {
      samples[0] = real (stream->get_bits (codelength)) * factor - 1.0;
#ifdef DEBUG
      if (samples[0] == (1 << codelength) - 1)
        cerr << "WARNING: stream contains an illegal subband sample!\n";  // MPEG-stream is corrupted!
#endif
      samples[1] = real (stream->get_bits (codelength)) * factor - 1.0;
#ifdef DEBUG
      if (samples[1] == (1 << codelength) - 1)
        cerr << "WARNING: stream contains an illegal subband sample!\n";  // MPEG-stream is corrupted!
#endif
      samples[2] = real (stream->get_bits (codelength)) * factor - 1.0;
#ifdef DEBUG
      if (samples[2] == (1 << codelength) - 1)
        cerr << "WARNING: stream contains an illegal subband sample!\n";  // MPEG-stream is corrupted!
#endif
    }

  samplenumber = 0;
  return (++groupnumber == 12) ? True : False;
}


bool SubbandLayer2::put_next_sample(e_channels channels,
				     SynthesisFilter *filter1, SynthesisFilter*){
#ifdef DEBUG
  if (samplenumber >= 3){
    // don't call put_next_sample if the previvious call returned True;
    cerr << "Illegal call to SubbandLayer2::put_next_sample()!\n";
    exit (1);
  }
#endif
  if (allocation && channels != right){
    register real sample = (samples[samplenumber] + d) * c;
    if (groupnumber <= 4)
      sample *= scalefactors[scalefactor1];
    else if (groupnumber <= 8)
      sample *= scalefactors[scalefactor2];
    else
      sample *= scalefactors[scalefactor3];
#ifdef DEBUG
    if (sample < -1.0 || sample > 1.0)
      cerr << "WARNING: rescaled subband sample is not in [-1.0, 1.0]\n";
      // this should never occur
#endif
    if (sample < -1.0E-7 || sample > 1.0E-7)
      filter1->input_sample (sample, subbandnumber);
  }

  return (++samplenumber == 3) ? True : False;
}



/******************************/
/*** Intensity Stereo Class ***/
/******************************/

SubbandLayer2IntensityStereo::SubbandLayer2IntensityStereo (uint32 subbandnumber)
: SubbandLayer2 (subbandnumber){
}


void SubbandLayer2IntensityStereo::read_scalefactor_selection (AudioStream *stream, Crc16 *crc){
  if (allocation){
    scfsi = stream->get_bits (2);
    channel2_scfsi = stream->get_bits (2);
    if (crc){
      crc->add_bits (scfsi, 2);
      crc->add_bits (channel2_scfsi, 2);
    }
  }
}


void SubbandLayer2IntensityStereo::read_scalefactor (AudioStream *stream, Header *header){
  if (allocation){
    SubbandLayer2::read_scalefactor (stream, header);
    switch (channel2_scfsi)
    {
      case 0:
	channel2_scalefactor1 = stream->get_bits (6);
	channel2_scalefactor2 = stream->get_bits (6);
	channel2_scalefactor3 = stream->get_bits (6);
	break;
      case 1:
	channel2_scalefactor1 = channel2_scalefactor2 = stream->get_bits (6);
	channel2_scalefactor3 = stream->get_bits (6);
	break;
      case 2:
	channel2_scalefactor1 = channel2_scalefactor2 =
	channel2_scalefactor3 = stream->get_bits (6);
	break;
      case 3:
	channel2_scalefactor1 = stream->get_bits (6);
	channel2_scalefactor2 = channel2_scalefactor3 = stream->get_bits (6);
	break;
    }
    if (channel2_scalefactor1 == 63 || channel2_scalefactor2 == 63 || channel2_scalefactor3 == 63)
      cerr << "WARNING: stream contains an illegal scalefactor!\n";	// MPEG-stream is corrupted!
  }
}


bool SubbandLayer2IntensityStereo::put_next_sample (e_channels channels,
	SynthesisFilter *filter1, SynthesisFilter *filter2){
#ifdef DEBUG
  if (samplenumber >= 3)
  {
    // don't call put_next_sample if the previvious call returned True;
    cerr << "Illegal call to SubbandLayer2::put_next_sample()!\n";
    exit (1);
  }
#endif
  if (allocation)
  {
    register real sample = (samples[samplenumber] + d) * c;
    if (channels == both)
    {
      register float sample2 = sample;
      if (groupnumber <= 4)
      {
	sample *= scalefactors[scalefactor1];
	sample2 *= scalefactors[channel2_scalefactor1];
      }
      else if (groupnumber <= 8)
      {
	sample *= scalefactors[scalefactor2];
	sample2 *= scalefactors[channel2_scalefactor2];
      }
      else
      {
	sample *= scalefactors[scalefactor3];
	sample2 *= scalefactors[channel2_scalefactor3];
      }
#ifdef DEBUG
      if (sample < -1.0 || sample > 1.0 || sample2 < -1.0 || sample2 > 1.0)
	cerr << "WARNING: rescaled subband sample is not in [-1.0, 1.0]\n";
	// this should never occur
#endif
      if (sample < -1.0E-7 || sample > 1.0E-7)
	filter1->input_sample (sample, subbandnumber);
      if (sample2 < -1.0E-7 || sample2 > 1.0E-7)
	filter2->input_sample (sample2, subbandnumber);
    }
    else if (channels == left)
    {
      if (groupnumber <= 4)
	sample *= scalefactors[scalefactor1];
      else if (groupnumber <= 8)
	sample *= scalefactors[scalefactor2];
      else
	sample *= scalefactors[scalefactor3];
#ifdef DEBUG
      if (sample < -1.0 || sample > 1.0)
	cerr << "WARNING: rescaled subband sample is not in [-1.0, 1.0]\n";
	// this should never occur
#endif
      if (sample < -1.0E-7 || sample > 1.0E-7)
	filter1->input_sample (sample, subbandnumber);
    }
    else
    {
      if (groupnumber <= 4)
	sample *= scalefactors[channel2_scalefactor1];
      else if (groupnumber <= 8)
	sample *= scalefactors[channel2_scalefactor2];
      else
	sample *= scalefactors[channel2_scalefactor3];
#ifdef DEBUG
      if (sample < -1.0 || sample > 1.0)
	cerr << "WARNING: rescaled subband sample is not in [-1.0, 1.0]\n";
	// this should never occur
#endif
      if (sample < -1.0E-7 || sample > 1.0E-7)
	filter1->input_sample (sample, subbandnumber);
    }
  }

  if (++samplenumber == 3)
    return True;
  else
    return False;
}



/********************/
/*** Stereo Class ***/
/********************/

SubbandLayer2Stereo::SubbandLayer2Stereo (uint32 subbandnumber)
: SubbandLayer2 (subbandnumber)
{
}


void SubbandLayer2Stereo::read_allocation (AudioStream *stream, Header *header, Crc16 *crc)
{
  uint32 length = get_allocationlength (header);
  allocation = stream->get_bits (length);
  channel2_allocation = stream->get_bits (length);
  if (crc)
  {
    crc->add_bits (allocation, length);
    crc->add_bits (channel2_allocation, length);
  }
}


void SubbandLayer2Stereo::read_scalefactor_selection (AudioStream *stream, Crc16 *crc)
{
  if (allocation)
  {
    scfsi = stream->get_bits (2);
    if (crc)
      crc->add_bits (scfsi, 2);
  }
  if (channel2_allocation)
  {
    channel2_scfsi = stream->get_bits (2);
    if (crc)
      crc->add_bits (channel2_scfsi, 2);
  }
}


void SubbandLayer2Stereo::read_scalefactor (AudioStream *stream, Header *header)
{
  SubbandLayer2::read_scalefactor (stream, header);
  if (channel2_allocation)
  {
    switch (channel2_scfsi)
    {
      case 0:
	channel2_scalefactor1 = stream->get_bits (6);
	channel2_scalefactor2 = stream->get_bits (6);
	channel2_scalefactor3 = stream->get_bits (6);
	break;
      case 1:
	channel2_scalefactor1 = channel2_scalefactor2 = stream->get_bits (6);
	channel2_scalefactor3 = stream->get_bits (6);
	break;
      case 2:
	channel2_scalefactor1 = channel2_scalefactor2 =
	channel2_scalefactor3 = stream->get_bits (6);
	break;
      case 3:
	channel2_scalefactor1 = stream->get_bits (6);
	channel2_scalefactor2 = channel2_scalefactor3 = stream->get_bits (6);
	break;
    }
    if (channel2_scalefactor1 == 63 || channel2_scalefactor2 == 63 || channel2_scalefactor3 == 63)
      cerr << "WARNING: stream contains an illegal scalefactor!\n";	// MPEG-stream is corrupted!

    prepare_sample_reading (header, channel2_allocation, &channel2_grouping,
			    &channel2_quantizationsteps, &channel2_factor,
			    &channel2_codelength, &channel2_c, &channel2_d);
  }
}


bool SubbandLayer2Stereo::read_sampledata (AudioStream *stream){
  bool returnvalue = SubbandLayer2::read_sampledata (stream);

  if (channel2_allocation)
    if (channel2_grouping)
    {
      uint32 samplecode = stream->get_bits (channel2_codelength);
#ifdef DEBUG
      if (samplecode == (1 << channel2_codelength) - 1)
	cerr << "WARNING: stream contains an illegal subband sample!\n";  // MPEG-stream is corrupted!
#endif
      channel2_samples[0] = real (samplecode % channel2_quantizationsteps) * channel2_factor - 1.0;
      samplecode /= channel2_quantizationsteps;
      channel2_samples[1] = real (samplecode % channel2_quantizationsteps) * channel2_factor - 1.0;
      samplecode /= channel2_quantizationsteps;
      channel2_samples[2] = real (samplecode % channel2_quantizationsteps) * channel2_factor - 1.0;
    }
    else
    {
      channel2_samples[0] = real (stream->get_bits (channel2_codelength)) * channel2_factor - 1.0;
#ifdef DEBUG
      if (channel2_samples[0] == (1 << channel2_codelength) - 1)
	cerr << "WARNING: stream contains an illegal subband sample!\n";  // MPEG-stream is corrupted!
#endif
      channel2_samples[1] = real (stream->get_bits (channel2_codelength)) * channel2_factor - 1.0;
#ifdef DEBUG
      if (channel2_samples[1] == (1 << channel2_codelength) - 1)
	cerr << "WARNING: stream contains an illegal subband sample!\n";  // MPEG-stream is corrupted!
#endif
      channel2_samples[2] = real (stream->get_bits (channel2_codelength)) * channel2_factor - 1.0;
#ifdef DEBUG
      if (channel2_samples[2] == (1 << channel2_codelength) - 1)
	cerr << "WARNING: stream contains an illegal subband sample!\n";  // MPEG-stream is corrupted!
#endif
    }
  return returnvalue;
}


bool SubbandLayer2Stereo::put_next_sample (e_channels channels,
					   SynthesisFilter *filter1, SynthesisFilter *filter2)
{
  bool returnvalue = SubbandLayer2::put_next_sample (channels, filter1, filter2);
  if (channel2_allocation && channels != left)
  {
    register real sample = (channel2_samples[samplenumber - 1] + channel2_d) * channel2_c;
    if (groupnumber <= 4)
      sample *= scalefactors[channel2_scalefactor1];
    else if (groupnumber <= 8)
      sample *= scalefactors[channel2_scalefactor2];
    else
      sample *= scalefactors[channel2_scalefactor3];
#ifdef DEBUG
    if (sample < -1.0 || sample > 1.0)
      cerr << "WARNING: rescaled subband sample is not in [-1.0, 1.0]\n";
      // this should never occur
#endif
    if (sample < -1.0E-7 || sample > 1.0E-7)
      if (channels == both)
	filter2->input_sample (sample, subbandnumber);
      else
	filter1->input_sample (sample, subbandnumber);
  }
  return returnvalue;
}
