#include	"libmpegaud.h"

#define		LSBMASK		1
#define		MASK8		0xff0
#define		WORDSIZE	16

static int
getbyte(Bitstream *bs)
{
    BitBuffer *bf = &bs->buffer[bs->currBuffer];
    int c = bf->buffer[bs->pointer];
    bs->currentbyte++;
    if (++bs->pointer >= bf->length){
        /*
         * Swap buffers and initiate a DMA
         */
        bs->currBuffer ^= 1;
        bs->pointer = 0;
        bs->dma(bs->ifile, bf->length, bf->buffer, 0);
        bs->ifile += bf->length;
        
    }
    return c;
}

static long
getshort(Bitstream *bs)
{
  int c1, c2;
  
  if ((c1 = getbyte(bs)) < 0) return(-1);
  if ((c2 = getbyte(bs)) < 0) return(-1);
  return (c1<<8) | c2;
}

static s32
bytealign(Bitstream *bs)
{
  s32 c;
  if (bs->bitpointer == WORDSIZE-1)
    return(bs->eobitstream);
  if (bs->bitpointer > 7)
    bs->bitpointer = 7;
  else{
    if ((c = getshort(bs)) < 0) bs->eobitstream = 1;
    bs->word = (u32) c;
    bs->bitpointer = WORDSIZE-1;
  }
  return(bs->eobitstream);
}

u32
readbit(Bitstream *bs)
{
  u32	b;
  s32   c;
  b = (bs->word>>bs->bitpointer--) & LSBMASK;
  if (bs->bitpointer < 0){
    if ((c = getshort(bs)) < 0) bs->eobitstream = 1;
    bs->word = (u32) c;
    bs->bitpointer = WORDSIZE-1;
  }
  return(b);
}

u32
readbits(Bitstream *bs, u32 nbits)
{
  s32	c;
  u32	b, left;
  
/* Maximum of WORDSIZE bits can be read */

  if (nbits <= bs->bitpointer+1){
    u32 mask = (u32) (1L<<nbits) - 1;
    b = mask & (bs->word>>(bs->bitpointer-nbits+1));
    bs->bitpointer -= nbits;
    if (bs->bitpointer < 0){
      if ((c = getshort(bs)) < 0) bs->eobitstream = 1;
      bs->word = (u32) c;
      bs->bitpointer = WORDSIZE-1;
    }
    return(b);
  }
  else{
    b = bs->word & ((1<<(bs->bitpointer+1)) - 1);     
    left = nbits - bs->bitpointer - 1;
    if ((c = getshort(bs)) < 0) bs->eobitstream = 1;
    bs->word = (u32) c;
    bs->bitpointer = WORDSIZE-1;
    b = (b<<left) | readbits(bs,left);
    return(b);
  }
}

s32
skiptonextframe(Bitstream *bs, s32 framebytes)
{
  int i, c;
  /* PRINTF("Skipping framebytes %d currentbytes %d\n", framebytes, bs->currentbyte); */
  
  for (i=0; i<(framebytes-bs->currentbyte); i++)
    if((c = getbyte(bs)) < 0)
      return(-1);
  bs->currentbyte = 0;
  return(1);
}

s32
nextsyncword(Bitstream *bs, u32 sync_code, s32 nbits)
{
  int	x, y;
  static skipcount = 0;
  int first = 1;
 
  /* This searches for a byte-aligned sync-word. This is definitely
     OK, once you are in the bit-stream, but in general the "byte-alignment"
     probably should be defined by the first detected sync-word, which
     may not be byte-aligned. 

     The other problem is that the Ancillary data can contain sync-words,
     so before starting the sync-word search you should skip to the end
     of the Ancillary data. */
  
  if (bytealign(bs)) 
    return(0);
  else{
    while (1){
      if (first){
        if ((x = readbits(bs,nbits)) < 0){
	  bs->eobitstream = 1;
	  return(0);
        }
        first = 0;
      } else {
        if ((y = readbits(bs,8)) < 0){
          bs->eobitstream = 1;
          return(0);
        }
        x = (x<<8 & MASK8) | y;
      }
      if (x==sync_code)
	return(1);
      else{
	skipcount++;
      }
    }
  }
}

void
openbitstream(u8 *ptr, Bitstream *bs)
{
  s32	c;
  BitBuffer *bf = &bs->buffer[0];
  
  bs->ifile = ptr;
  bs->eobitstream = 0;
  bs->currentbyte = 0;
  bs->pointer = 0;
  bs->currBuffer = 0;
  
  /*
   * Load two buffers - wait for completion on first
   */
  bs->dma(bs->ifile, bf->length, bf->buffer, 1);
  bs->ifile += bf->length;

  bf = &bs->buffer[1];
  bs->dma(bs->ifile, bf->length, bf->buffer, 0);
  bs->ifile += bf->length;
  
  if ((c = getshort(bs)) < 0) bs->eobitstream = 1;
  
  bs->word = (u32) c;
  bs->bitpointer = WORDSIZE-1;
}