#include <string.h>
#include "mpeg_demux.h"







PSDemux::PSDemux(int _stream, int _substream)
{
  stream = _stream;
  substream = _substream;
}

void
PSDemux::reset()
{
  parser.reset();
}

void
PSDemux::set(int _stream, int _substream)
{
  stream = _stream;
  substream = _substream;
  parser.reset();
}

size_t
PSDemux::demux(uint8_t *_buf, size_t _size)
{
  uint8_t *end = _buf + _size;
  uint8_t *read_buf  = _buf;
  uint8_t *write_buf = _buf;
  size_t len;

  while (read_buf < end)
  {
    if (parser.payload_size)
    {
      len = MIN(parser.payload_size, size_t(end - read_buf));

      
      if ((stream && stream != parser.stream) || 
          (stream && substream && substream != parser.substream))
      {
        parser.payload_size -= len;
        read_buf += len;
        continue;
      }

      
      memmove(write_buf, read_buf, len);
      parser.payload_size -= len;
      read_buf += len;
      write_buf += len;
    }
    else
      parser.parse(&read_buf, end);
  }

  return write_buf - _buf;
}







PSParser::PSParser()
{
  packets = 0;
  errors = 0;
  reset();
}

void
PSParser::reset()
{
  scanner.set_standard(SYNCMASK_PS);
  state = state_sync;
  data_size = 0;

  subheader = 0;

  header_size = 0;
  payload_size = 0;

  stream = 0;
  substream = 0;
}

bool
PSParser::is_audio()
{
  return (((stream    & 0xe0) == 0xc0) ||   
          ((substream & 0xf8) == 0x80) ||   
          ((substream & 0xf8) == 0x88) ||   
          ((substream & 0xf8) == 0xA0));    
}

Speakers 
PSParser::spk()
{
  
  static const int nch2mask[8] = 
  {
    MODE_MONO, 
    MODE_STEREO,
    MODE_3_1,
    MODE_QUADRO,
    MODE_3_2, 
    MODE_5_1,
    0, 0
  };

  if      ((stream    & 0xe0) == 0xc0) return Speakers(FORMAT_MPA, 0, 0);   
  else if ((substream & 0xf8) == 0x80) return Speakers(FORMAT_AC3, 0, 0);   
  else if ((substream & 0xf8) == 0x88) return Speakers(FORMAT_DTS, 0, 0);   
  else if ((substream & 0xf8) == 0xA0)                                 
  {
    
    
    int format, mask, sample_rate;
    sample_t level;

    switch (subheader[4] >> 6)
    {
      case 0: format = FORMAT_PCM16_BE; level = 32767;    break;
      case 2: format = FORMAT_PCM24_BE; level = 8388607;  break;
      default: return spk_unknown;
    }

    mask = nch2mask[subheader[4] & 7];
    if (!mask) return spk_unknown;

    switch ((subheader[4] >> 4) & 3)
    {
      case 0: sample_rate = 48000; break;
      case 1: sample_rate = 96000; break;
      default: return spk_unknown;
    }

    return Speakers(format, mask, sample_rate, level);

  }
  else
    
    return spk_unknown;
}

size_t 
PSParser::parse(uint8_t **buf, uint8_t *end)
{
  size_t len;
  size_t required_size = 0;

  #define REQUIRE(bytes)       \
  if (data_size < (bytes))     \
  {                            \
    required_size = (bytes);   \
    continue;                  \
  }

  #define DROP                 \
  {                            \
    REQUIRE(6);                \
    data_size = 6 + (header[4] << 8) + header[5] - data_size; \
    state = state_drop;        \
    continue;                  \
  }

  #define SYNC                 \
  {                            \
    data_size = 0;             \
    state = state_sync;        \
    required_size = 4;         \
    continue;                  \
  }

  #define RESYNC(bytes)        \
  {                            \
    data_size -= bytes;        \
    memmove(header, header + bytes, data_size); \
    state = state_sync;        \
    required_size = 4;         \
    continue;                  \
  }

  
  
  

  header_size = 0;
  payload_size = 0;

  while (1)
  {
    
    

    if (data_size < required_size)
    {
      len = MIN(size_t(end - *buf), required_size - data_size);
      memcpy(header + data_size, *buf, len);
      data_size += len;
      *buf += len;

      if (data_size < required_size)
        return 0;
    }

    switch (state)
    {
      
      
      

      case state_drop:
      {
        len = MIN(size_t(end - *buf), data_size);
        data_size -= len;
        *buf += len;

        if (data_size)
          return 0;
        else 
          SYNC;
      } 

      
      
      

      case state_sync:
      {
        REQUIRE(4);

        if (!scanner.get_sync(header))
        {
          size_t gone = scanner.scan(header, header + 4, data_size - 4);
          data_size -= gone;
          memmove(header + 4, header + 4 + gone, data_size);

          if (!scanner.get_sync(header))
          {
            gone = scanner.scan(header, *buf, end - *buf);
            *buf += gone;
            if (!scanner.get_sync(header))
              return 0;
          }
        }

        
        if (header[0] != 0 || header[1] != 0 || header[2] != 1)
          RESYNC(1);

        
        if (header[3] < 0xb9)
          RESYNC(1);

        
        if (header[3] == 0xb9)
          RESYNC(4);

        state = state_header;
        
      } 

      
      
      

      case state_header:
      {
        
        

        switch (header[3])
        {
          case 0xba: 
          {
            REQUIRE(12);
            if ((header[4] & 0xf0) == 0x20)
            {
              
              SYNC;
            }
            else if ((header[4] & 0xc0) == 0x40) 
            {
              
              REQUIRE(14);
              REQUIRE(14 + size_t(header[13] & 7));
              SYNC;
            } 
            else
            {
              
              errors++;
              RESYNC(1);
            }
          }

          case 0xbb: 
          case 0xbc: 
          case 0xbe: 
            DROP;

        } 

        
        if ((header[3] & 0xf0) == 0xf0)
          DROP;

        
        

        REQUIRE(7);
        size_t pos = 6;
        if (header[3] != 0xbf) 
        {
          if ((header[pos] & 0xc0) == 0x80)
          {
            
            REQUIRE(9);
            pos = header[8] + 9;
            REQUIRE(pos);
          } 
          else 
          {
            
            while (header[pos] == 0xff && pos < data_size)
              pos++;

            if (pos == data_size)
              if (data_size < 24)
                REQUIRE(pos+1)
              else
              {
                
                errors++; 
                RESYNC(1);
              }

            if ((header[pos] & 0xc0) == 0x40)
            {
              pos += 2;
              REQUIRE(pos+1);
            }

            if ((header[pos] & 0xf0) == 0x20)
              pos += 5;
            else if ((header[pos] & 0xf0) == 0x30)
              pos += 10;
            else 
              pos++;

            REQUIRE(pos);
          }
        } 

        
        

        stream = header[3];
        if (stream == 0xbd) 
        {
          pos++;
          REQUIRE(pos);

          substream = header[pos-1];
          subheader = header + pos;

          
          if ((substream & 0xf0) == 0x80)
            REQUIRE(pos + 3);
          
          if ((substream & 0xf0) == 0xa0)
            REQUIRE(pos + 6);
        }
        else
        {
          substream = 0;
          subheader = 0;
        }

        
        
        

        packets++;
        header_size = data_size;
        payload_size = 6 + (header[4] << 8) + header[5] - data_size;

        data_size = 0;
        state = state_sync;

        return payload_size;
        
      } 

    } 
  } 
}



