#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "portaudio.h"
#include "mad.h"

static FILE* input_fd = NULL;
static int eos;
#define INPUT_BUFFER_SIZE 2048
static unsigned char fileBuffer[INPUT_BUFFER_SIZE];
static unsigned int filePos;
static unsigned int samplesRead;

//Portaudio variables:
typedef struct
{
    short left_phase;
    short right_phase;
}
paTestData;
#define SAMPLE_RATE (44100)

//Mad variables:
static struct mad_stream Stream;
static struct mad_header Header;
static struct mad_frame Frame;
static struct mad_synth Synth;
static mad_timer_t Timer;

int swapInt32BigToHost(int arg)
{
   int i=0;
   int checkEndian = 1;
   if( 1 == *(char *)&checkEndian )
   {
      // Intel (little endian)
      i=arg;
      i=((i&0xFF000000)>>24)|((i&0x00FF0000)>>8)|((i&0x0000FF00)<<8)|((i&0x000000FF)<<24);
   }
   else
   {
      // PPC (big endian)
      i=arg;
   }
   return i;
}

/* Gets the size of the ID3v2 tag */
int ID3v2TagSize(const char *mp3path)
{
   int size;
   char sig[3];

   FILE* fp = fopen(mp3path, "rb");
   if (!fp) return 0;

   fread(sig, sizeof(char), sizeof(sig), fp);
   if (strncmp("ID3", sig, 3) != 0) {
      fclose(fp);
      return 0;
   }

   fseek(fp, 6, SEEK_SET);
   fread(&size, sizeof(unsigned int), 1, fp);
   /*
    *  The ID3 tag size is encoded with four bytes where the first bit
    *  (bit 7) is set to zero in every byte, making a total of 28 bits. The zeroed
    *  bits are ignored, so a 257 bytes long tag is represented as $00 00 02 01.
    */
   size = (unsigned int) swapInt32BigToHost((int)size);
   size = ( ( (size & 0x7f000000) >> 3 ) | ( (size & 0x7f0000) >> 2 ) | ( (size & 0x7f00) >> 1 ) | (size & 0x7f) );
   fclose(fp);
   return size;
}

/* Fill the file buffer used for decoding */
int fillFileBuffer() {
	// Find out how much to keep and how much to fill.
	const unsigned int   bytesToKeep   = Stream.bufend - Stream.next_frame;
	unsigned int         bytesToFill   = sizeof(fileBuffer) - bytesToKeep;

	// Want to keep any bytes?
	if (bytesToKeep)
		memmove(fileBuffer, fileBuffer + sizeof(fileBuffer) - bytesToKeep, bytesToKeep);

	// Read into the rest of the file buffer.
	unsigned char* bufferPos = fileBuffer + bytesToKeep;
	while (bytesToFill > 0){
		const unsigned int bytesRead = fread(bufferPos, sizeof(unsigned char), bytesToFill, input_fd);
		// EOF?
		if (bytesRead == 0){
            printf("End of file\n");
			return 2;
        }

		// Adjust where we're writing to.
		bytesToFill -= bytesRead;
		bufferPos += bytesRead;
		filePos += bytesRead;
	}
	return 0;
}

/* Decode data using libmad */
void decode() {
	while ((mad_frame_decode(&Frame, &Stream) == -1) && ((Stream.error == MAD_ERROR_BUFLEN) || (Stream.error == MAD_ERROR_BUFPTR))){
		int tmp = fillFileBuffer();
		if (tmp == 2)
            eos = 1;
		mad_stream_buffer(&Stream, fileBuffer, sizeof(fileBuffer));
	}

    mad_timer_add(&Timer, Frame.header.duration);
	mad_synth_frame(&Synth, &Frame);
}

short convertSample(mad_fixed_t sample) {
	// round
	sample += (1L << (MAD_F_FRACBITS - 16));

	// clip
	if (sample >= MAD_F_ONE)
    	sample = MAD_F_ONE - 1;
	else if (sample < -MAD_F_ONE)
	    sample = -MAD_F_ONE;

	// quantize
	return sample >> (MAD_F_FRACBITS + 1 - 16);
}

void convertLeftSamples(paTestData* first, paTestData* last, const mad_fixed_t* src) {
	paTestData* dst;
	for (dst = first; dst != last; ++dst){
		dst->left_phase = convertSample(*src++);
    }
}

void convertRightSamples(paTestData* first, paTestData* last, const mad_fixed_t* src) {
	paTestData* dst;
	for (dst = first; dst != last; ++dst){
		dst->right_phase = convertSample(*src++);
    }
}

/* This routine will be called by the PortAudio engine when audio is needed.
** It may called at interrupt level on some machines so don't do anything
** that could mess up the system like calling malloc() or free().
*/
static int patestCallback( const void *inputBuffer, void *outputBuffer,
                           unsigned long framesPerBuffer,
                           const PaStreamCallbackTimeInfo* timeInfo,
                           PaStreamCallbackFlags statusFlags,
                           void *userData )
{
    paTestData *destination = (paTestData*)outputBuffer;
    (void) inputBuffer; /* Prevent unused variable warning. */

    unsigned int samplesToWrite = framesPerBuffer;
    while (samplesToWrite > 0) 	{
        const unsigned int samplesAvailable = Synth.pcm.length - samplesRead;
        if (samplesAvailable > samplesToWrite) {
            convertLeftSamples(destination, destination + samplesToWrite, &Synth.pcm.samples[0][samplesRead]);
            convertRightSamples(destination, destination + samplesToWrite, &Synth.pcm.samples[1][samplesRead]);

            samplesRead += samplesToWrite;
            samplesToWrite = 0;
        }else{
            convertLeftSamples(destination, destination + samplesAvailable, &Synth.pcm.samples[0][samplesRead]);
            convertRightSamples(destination, destination + samplesAvailable, &Synth.pcm.samples[1][samplesRead]);

            samplesRead = 0;
            decode();

            destination += samplesAvailable;
            samplesToWrite -= samplesAvailable;
        }
    }

    if (!eos)
        return paContinue;
    return paComplete;
}

/* Main */
int main(int argc, char *argv[])
{
    printf("Mad and portaudio sample\n");
    printf("Copyright (c) 2011 Paolo Iommarini\n\n");

    char input[264] = "test.mp3";
    if (argc > 1)
        strcpy(input, argv[1]);
    printf("Input file: %s\n", input);

    PaError err = Pa_Initialize();
    if( err != paNoError ) goto error;
    printf("Portaudio initialized\n");

    mad_stream_init(&Stream);
	mad_header_init(&Header);
    mad_frame_init(&Frame);
    mad_synth_init(&Synth);
    mad_timer_reset(&Timer);
    printf("Mad initialized\n");

    input_fd = fopen(input, "rb");
    if (!input_fd){
        printf("Failed to open input file\n");
        goto end;
    }

    int size = ID3v2TagSize(input);
    printf("Tag size %i\n", size);
    fseek(input_fd, size, SEEK_SET);

    PaStream *stream;
    /* Open an audio I/O stream. */
    err = Pa_OpenDefaultStream( &stream,
                                0,
                                2,          /* stereo output */
                                paInt16,
                                SAMPLE_RATE,
                                INPUT_BUFFER_SIZE, /* frames per buffer, i.e. the number
                                                   of sample frames that PortAudio will
                                                   request from the callback. Many apps
                                                   may want to use
                                                   paFramesPerBufferUnspecified, which
                                                   tells PortAudio to pick the best,
                                                   possibly changing, buffer size.*/
                                patestCallback,
                                NULL );
    if( err != paNoError ) goto error;
    printf("Stream opened\n");

    err = Pa_StartStream( stream );
    if( err != paNoError ) goto error;
    printf("Stream started\n");

    printf("Waiting end of stream\n");
    while (!eos)
        Pa_Sleep(1000);

    err = Pa_StopStream( stream );
    if( err != paNoError ) goto error;

    err = Pa_Terminate();
    if( err != paNoError ) goto error;
    printf("Portaudio terminated\n");

    mad_synth_finish(&Synth);
    mad_header_finish(&Header);
    mad_frame_finish(&Frame);
    mad_stream_finish(&Stream);
    printf("Mad terminated\n");

end:
    return 0;

error:
    printf(  "PortAudio error: %s\n", Pa_GetErrorText( err ) );

    return 0;
}