#include "xparameters.h"
#include "xio.h"
//#include "pcmu_l.h"
#include "types.h"

#define BIAS 0x84   /* define the add-in bias for 16 bit samples */
#define CLIP 32635

void audio_int_handler(void * poop){
  int linear_data;
  unsigned char ulaw;
  unsigned char packet_data;
  short sample_to_write;

  linear_data = XIo_In32(0xFEFE0000);
  ulaw = l16_to_pcmu(linear_data);

  //  packet_data = packet[54 + pkt_cntr];
  //if(pkt_cntr < 160){
  //  pkt_cntr++;
  // }

  sample_to_write = (short) (pcmu_to_l16(ulaw));
  //print("\r\n");
  //putnum(linear_data);
  XIo_Out32(0xFEFE2222,(sample_to_write));
}

/*
* Convert 16 bit linear sample to 8-bit ulaw sample.
*/
unsigned char l16_to_pcmu(short sample)
{
  static short exp_lut[256] = 
  {0, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3,
   4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,
   5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
   5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
   6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
   6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
   6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
   6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
   7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
   7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
   7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
   7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
   7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
   7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
   7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
   7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7};
  int sign, exponent, mantissa;
  unsigned char ulawbyte;

  /* Get the sample into sign-magnitude. */
  sign = (sample >> 8) & 0x80;      /* set aside the sign */
  if (sign) sample = -sample;       /* get magnitude */
  if (sample > CLIP) sample = CLIP; /* clip the magnitude */

  /* Convert from 16 bit linear to ulaw. */
  sample = sample + BIAS;
  exponent = exp_lut[( sample >> 7 ) & 0xFF];
  mantissa = ( sample >> ( exponent + 3 ) ) & 0x0F;
  ulawbyte = ~ ( sign | ( exponent << 4 ) | mantissa );
#ifdef ZEROTRAP
  if (ulawbyte == 0) ulawbyte = 0x02;  /* optional CCITT trap */
#endif

  return ulawbyte;
} /* l16_to_pcmu */

/*
* Convert input of 8-bit ulaw to signed 16-bit linear sample.
*/
int16 pcmu_to_l16(unsigned int ulawbyte)
{
  static int exp_lut[8] = { 0, 132, 396, 924, 1980, 4092, 8316, 16764 };
  int sign, exponent, mantissa, sample;

  ulawbyte = ~ ulawbyte;
  sign = ( ulawbyte & 0x80 );
  exponent = ( ulawbyte >> 4 ) & 0x07;
  mantissa = ulawbyte & 0x0F;
  sample = exp_lut[exponent] + ( mantissa << ( exponent + 3 ) );
  if (sign) sample = -sample;

  return sample;
} /* pcmu_to_l16 */

int main(){

  microblaze_register_handler(audio_int_handler, (void *)0);
  microblaze_enable_interrupts();

  while(1){
  }

}
