#include "GlobHdr.h"
#include "Header.h"
#include "FStream.h"
#include "Log.h"

#include <stdlib.h>
#include <string.h>

#include "kernel.h"
#include "swis.h"

static _kernel_oserror Err_StillHandler =
{0, "Vorbis Stream(s) still in use."};
static _kernel_oserror err_config = {0, "Invalid Vorbis configuration code."};
static _kernel_oserror err_notogg = {0, "Data doesn't seem to be an Ogg bitstream."};
static _kernel_oserror err_notvorbis = {0, "Ogg bitstream doesn't seem to contain Vorbis data."};
static _kernel_oserror err_badhdr = {0, "Ogg bitstream contains a corrupted header."};
static _kernel_oserror err_badpacket = {0, "Ogg bitstream contains a corrupted packet."};
static _kernel_oserror Err_TooManyStreamHandlers =
{0, "Maximum number of streams reached."};
static _kernel_oserror Err_Bad_Stream_Handle =
{0, "Incorrect Vorbis stream handle."};
static _kernel_oserror err_memory = {0, "Out of memory."};

#ifdef MAKEABS
static int oggb_size=128*1024;
static int sndb_size=256*1024;
#else
static int oggb_size=128*1024;
static int sndb_size=1024*1024/2;
#endif

_kernel_oserror* Streams_Init(GlobHdr* g, void* pw)
{
	memset(&g->streamsptr, 0, sizeof(g->streamsptr[0])*max_streams);

#ifdef MAKEABS
	IGNORE(pw);
	return NULL;
#else
	return _swix(OS_AddCallBack, _INR(0,1), streams_callback_veneer, pw);
#endif
}

void Streams_CloseAll(GlobHdr* g)
{
	int i;

	for (i = 0; i < max_streams; i++)
	{
		if (g->streamsptr[i])
		{
			_kernel_swi_regs r;

			r.r[0] = i;
			swi_StreamClose(g, &r);
		}
	}
}

_kernel_oserror* Streams_Finalize(GlobHdr* g, void* pw)
{
	int i;

	for (i = 0; i < max_streams; i++)
	{
		if (g->streamsptr[i])
			return &Err_StillHandler;
	}

#ifdef MAKEABS
	IGNORE(pw);
#else
	// Remove possible pending CallAfter
	_swix(OS_RemoveTickerEvent, _INR(0,1), streams_callevery_veneer, pw);

	// Remove possible pending Callback
	if (g->flags & glb_flag_callback)
	{
		_swix(OS_RemoveCallBack, _INR(0,1), streams_callback_veneer, pw);
		g->flags &= ~glb_flag_callback;
	}
#endif

	return NULL;
}

#ifndef MAKEABS
_kernel_oserror* Streams_CalledEvery(_kernel_swi_regs* r, void* pw)
{
	GlobHdr* g = Glb;

	IGNORE(r);

	g->flags |= glb_flag_callback;
	_swix(OS_AddCallBack, _INR(0,1), streams_callback_veneer, pw);

	return NULL;
}

_kernel_oserror* Streams_Callback(_kernel_swi_regs* r, void* pw)
{
	GlobHdr* g = Glb;
	int i;

	IGNORE(r);

	for (i = 0; i < max_streams; i++)
	{
		if (g->streamsptr[i] && !(g->streamsptr[i]->conf.flags & Stream_Flag_ForegroundProcess))
		{
			Stream_BackgroundProcess(g->streamsptr[i], 0);
		}
	}

	g->flags &= ~glb_flag_callback;

	_swix(OS_CallAfter, _INR(0,2), 2, streams_callevery_veneer, pw);

	return NULL;
}
#endif

static int freesndbuf(sndbuf* sndb, int channels, int interlaced)
{
	int n;

	n = sndb->start - sndb->free;
	if (n <= 0) n += (sndb->last - sndb->data) + 1;

	if (!interlaced) n *= channels;

	return n;
}

static int usedoutbuf(doutbuf* b, int channels, int interlaced)
{
	int n;

	n = b->real_free - b->real_start;
	if (n < 0) n += b->real_size;

	if (interlaced) n /= channels;

	return n;
}

static void bs_init(bytebuf* bs, byte* data, int size)
{
	bs->start = data;
	bs->free = data;
	bs->data = data;
	bs->last = bs->data + size - 1;
	bs->size = size;
	bs->count = 0;
}

static int bs_fixcount(bytebuf* bs)
{
	int count = bs->free - bs->start;
	if (count < 0) count += bs->size;
	bs->count = count;
	return count;
}

static _kernel_oserror* createdynamicareas(stream* S, dinbuf* in)
{
#ifdef MAKEABS
	IGNORE(in);
	S->inb = malloc(oggb_size+16*4);
	S->inb->size = oggb_size;
#else
	_kernel_oserror* err;

	if (!in)
	{
		err = _swix(OS_DynamicArea, _INR(0,8)|_OUT(1)|_OUT(3)
			, 0, -1, oggb_size+16*4, -1, 0x80
			, oggb_size+16*4, 0, 0, "Vorbis (Input)"
			, &S->inb_area, &S->inb);
		if (err) return err;

		S->inb->size = oggb_size;
	}
	else S->inb = in;
#endif

#ifdef MAKEABS
	S->outb = malloc(sndb_size*2+16*4);
#else
	err = _swix(OS_DynamicArea, _INR(0,8)|_OUT(1)|_OUT(3)
		, 0, -1, sndb_size*2+16*4, -1, 0x80
		, sndb_size*2+16*4, 0, 0, "Vorbis (Output)"
		, &S->outb_area, &S->outb);
	if (err)
	{
		_swix(OS_DynamicArea, _INR(0,1), 1, S->inb_area);
		return err;
	}
#endif
	memset(S->outb, 0, sizeof(*S->outb));
	S->outb->real_size = sndb_size;
	S->outb->size = sndb_size;

	return NULL;
}

static void removedynamicareas(stream* S)
{
#ifdef MAKEABS
	free(S->inb);
	free(S->outb);
#else
	if (S->inb_area) _swix(OS_DynamicArea, _IN(0)|_IN(1), 1, S->inb_area);
	if (S->outb_area) _swix(OS_DynamicArea, _IN(0)|_IN(1), 1, S->outb_area);
#endif
}

static int Vorbis_New(vorbis_stream** pv, int serialno)
{
	int result = 0;
	vorbis_stream* V;

	V = _ogg_malloc(sizeof(*V));
	if (!V) return OV_EMEMORY;

	*pv = V;
	memset(V, 0, sizeof(*V));

	V->object = Stream_Object_OggHdr;

	if (result >= 0) result = ogg_stream_init(&V->os, serialno);

	vorbis_dsp_clear(&V->vd);
	if (result >= 0) result = vorbis_info_init(&V->vi);
	if (result >= 0) result = vorbis_comment_init(&V->vc);
	if (result >= 0) result = vorbis_block_init(&V->vd, &V->vb);

	return result;
}

static void Vorbis_Destroy(vorbis_stream** pv)
{
	vorbis_stream* V = *pv;

	if (!V) return;
	*pv = NULL;

	vorbis_block_clear(&V->vb);
	vorbis_dsp_clear(&V->vd);
	vorbis_comment_clear(&V->vc);
	vorbis_info_clear(&V->vi);  // must be called last

	ogg_stream_clear(&V->os);
	_ogg_free(V);
}

static void Vorbis_Reset(vorbis_stream* V)
{
	if (!V) return;

	if (V->object == Stream_Object_Packet)
	{
		vorbis_block_clear(&V->vb);
		vorbis_block_init(&V->vd, &V->vb);
	}
	ogg_stream_reset(&V->os);
}

static void stream_clearinput(stream* S, int bbuftoo)
{
	int ioff = _kernel_irqs_disabled();

	S->decode = Stream_Decode_ReadPage;
	ogg_sync_reset(&S->oy);

	if (!ioff) _kernel_irqs_off();

	bs_init(&S->oggb, S->inb->data, S->inb->size);

	if (bbuftoo)
	{
		S->inb->real_start = 0;
		S->inb->real_free = 0;
		S->inb->finished = 0;
	}

	S->outb->finished = 0;
	if (!ioff) _kernel_irqs_on();
}

static void stream_clearoutput(stream* S)
{
	int ioff = _kernel_irqs_disabled();

	if (!ioff) _kernel_irqs_off();
	S->sndb.data = S->outb->data;
	S->sndb.start = S->outb->data;
	S->sndb.free = S->sndb.data;
	S->sndb.last = S->sndb.data + S->outb->size - 1;

	S->outb->real_start = 0;
	S->outb->real_free = 0;
	S->outb->finished = 0;
	if (!ioff) _kernel_irqs_on();
}

static void stream_configoutput(stream* S, vorbis_stream* V)
{
#ifndef MAKEABS
	if (V->vi.channels > 2)
	{
		S->outb->real_size = S->outb->size / V->vi.channels;
		S->interlaced = 0;
	}
	else
#endif
	{
		S->outb->real_size = S->outb->size - (S->outb->size % V->vi.channels);
		S->interlaced = 1;
	}
	// avoid non word values
	S->outb->real_size &= ~1;
	S->sndb.last = S->sndb.data + S->outb->real_size - 1;
}

static void stream_clearinfo(stream* S)
{
	S->decode = Stream_Decode_ReadPage;
	ogg_sync_reset(&S->oy);

	Vorbis_Destroy(&S->pvs);
}

static _kernel_oserror* Stream_New(GlobHdr* g, stream** ps, int* index, dinbuf* in)
{
	_kernel_oserror* e;
	int i;
	stream* s;

	for (i = 0; i < max_streams; i++)
	{
		if (!g->streamsptr[i])
			break;
	}

	if (i == max_streams)
		return &Err_TooManyStreamHandlers;

	s = _ogg_malloc(sizeof(*s));
	if (!s) return &err_memory;

	g->streamsptr[i] = s;
	*ps = s;
	*index = i;

	memset(s, 0, sizeof(*s));
	s->pGlobHdr = g;

	e = createdynamicareas(s, in);
	if (e) goto Error;

	s->maxgranulepos = 0;
	s->seek_pos = -1;
	s->interlaced = 1;

	stream_clearinfo(s);
	stream_clearinput(s, (in == NULL));
	stream_clearoutput(s);

	ogg_sync_init(&s->oy); // Now we can read pages

	return NULL;

Error:
	removedynamicareas(s);
	_ogg_free(s);
	g->streamsptr[i] = NULL;

	return e;
}

static _kernel_oserror* Stream_Destroy(stream* S)
{
	removedynamicareas(S);

	Vorbis_Destroy(&S->poldvs);
	Vorbis_Destroy(&S->pvs);
	/* ogg_page and ogg_packet structs always point to storage in
	   libvorbis.  They're never freed or manipulated directly */

	// OK, clean up the framer
	ogg_sync_clear(&S->oy);

	_ogg_free(S);

	mem_pack();

	return NULL;
}

_kernel_oserror* Stream_GetHeader(GlobHdr* g, unsigned int i, stream** pps)
{
	if ((i >= max_streams)
	||  !g->streamsptr[i])
		return &Err_Bad_Stream_Handle;

	*pps = g->streamsptr[i];

	return NULL;
}

static int read_page(stream* S)
{
	int count;
	int copied;
	int max;
	byte* buffer;

	if (S->inb->finished)
	{
		if (!S->oggb.count) return 0;
		if (S->oggb.count < ogg_page_size)
			count = S->oggb.count;
		else
			count = ogg_page_size;
	}
	else
	{
		if (S->oggb.count < ogg_page_size)
			return 0;
		count = ogg_page_size;
	}

	buffer = ogg_sync_buffer(&S->oy, count);
	if (!buffer) return OV_EMEMORY;

	copied = count;

	max = S->oggb.size - (S->oggb.start - S->oggb.data);
	if (count >= max)
	{
		memcpy(buffer, S->oggb.start, max);
		count -= max;
		buffer += max;
		S->oggb.start = S->oggb.data;
	}
	if (count)
	{
		memcpy(buffer, S->oggb.start, count);
		S->oggb.start += count;
	}
	bs_fixcount(&S->oggb);
	ogg_sync_wrote(&S->oy, copied);

	return 1;
}

extern void out_interlaced(xint**, int16_t*, int, int);
/*
void out_interlaced(xint** pcm, int16_t* ptr, int count, int channels)
{
	int i, j;

	for(j = 0; j < count; j++)
	{
		for(i = 0; i < V->vi.channels; i++)
		{
			xint* mono = pcm[i];
			int val = (int)(mono[j]>>(XISHIFT-15));
			// might as well guard against clipping
			if(val > 32767)
				val = 32767;
			if(val < -32768)
				val = -32768;
			*ptr++ = val;
		}
	}
}
*/

static int output_samples(stream* S, vorbis_stream* V)
{
	xint **pcm;
	int samples;
	int convsize;
	int count = 0;

	/* **pcm is a multichannel vector.  In stereo, for example, pcm[0] is left,
	   and pcm[1] is right. samples is the size of each channel.
	   Convert the values to whatever PCM format and write it out */

	while((samples = vorbis_synthesis_pcmout(&V->vd, &pcm)) > 0)
	{
		// Max is total free - 1
		if (S->sndb.data == S->sndb.start)
			convsize = S->sndb.last - S->sndb.free;
		else
			convsize = (S->sndb.free < S->sndb.start)
			         ? (S->sndb.start - S->sndb.free) - 1
			         : (S->sndb.last - S->sndb.free) + 1;

		// convert FLOATs to 16 bit signed ints (host order) and interleave
		if (S->interlaced)
		{
			convsize /= V->vi.channels;
			convsize = (samples < convsize) ? samples : convsize;
			if (convsize <= 0) break;

			out_interlaced(pcm, S->sndb.free, convsize, V->vi.channels);

			S->sndb.free += V->vi.channels * convsize;
			if (S->sndb.free > S->sndb.last)
				S->sndb.free = S->outb->data;
		}
		else
		{
			int16_t* ptr = S->sndb.free;
			int i;

			convsize = (samples < convsize) ? samples : convsize;
			if (convsize <= 0) break;

			for(i = 0; i < V->vi.channels; i++)
			{
				ptr = S->sndb.free + S->outb->real_size * i;

				out_interlaced(&pcm[i], ptr, convsize, 1);
			}

			S->sndb.free += convsize;
			if (S->sndb.free > S->sndb.last)
				S->sndb.free = S->outb->data;
		}

		// tell libvorbis how many samples we actually consumed
		vorbis_synthesis_read(&V->vd, convsize);

		count += convsize;
	}

	return count;
}

static void complete_output(stream* S)
{
	vorbis_stream* V = S->pvs;

	V->object = Stream_Object_None;

	S->seek_pos = -1;

	// for positionning info past input buffer clear
	Vorbis_Destroy(&S->poldvs); // Shouldn't be needed
	S->poldvs = V;
	S->pvs = NULL;
	if (S->maxgranulepos < V->vd.granulepos)
		S->maxgranulepos = V->vd.granulepos;
	S->usedoutb = 0;
}

static int one_piece(stream* S, int* count, int more, int remain)
{
	int result = 0;
	Stream_Decode recover = Stream_Decode_ReadPage;
	vorbis_stream* V = S->pvs;

	if (S->seek_pos != -1)
	{
		if (V && (V->vd.granulepos >= S->seek_pos))
			S->seek_pos = -1;
	}

	switch(S->decode)
	{
		case Stream_Decode_ReadPage:
		{
			if (!S->inb->finished && (bs_fixcount(&S->oggb) <= remain))
				return 0;

			// need to read some data
			if (read_page(S))
			{
				S->decode = Stream_Decode_SyncPageOut;
				return more;
			}

			// could not read data, did we reach end of stream?
			if (S->inb->finished)
			{
				// Yes. Did we play something?
				if (!V
				||  (V->object < Stream_Object_Packet))
				{
					S->lasterr = err_notvorbis;
					goto fatal_error;
				}

				complete_output(S);
				S->outb->finished = 1;
				S->decode = Stream_Decode_Finished;
			}

			return 0;
		}
		break;
		case Stream_Decode_SyncPageOut:
		{
			int bHole = 0;

			while ((result = ogg_sync_pageout(&S->oy, &S->og)) < 0)
				bHole = 1;

			// Hole in tha data, not synced correctly
			if (bHole)
				Log("Note (Object %d, Step: %d, Code %d) bad sync", V ? V->object : -1, S->decode, result);

			if (result > 0)
			{
				// for seeking
				int64_t pos = ogg_page_granulepos(&S->og);
				if (pos < S->seek_pos)
				{
					if (S->maxgranulepos < pos)
						S->maxgranulepos = pos;
					return more;
				}

				if (!V)
					S->decode = Stream_Decode_StreamNew;
				else if ((V->os.serialno != ogg_page_serialno(&S->og))
				&&       (V->object == Stream_Object_Packet))
					S->decode = Stream_Decode_WaitPlayed;
				else	S->decode = Stream_Decode_StreamPageIn;

				return more;
			}

			// Requires more data
			S->decode = Stream_Decode_ReadPage;
			return more; // One page at a time
		}
		break;
		case Stream_Decode_WaitPlayed:
		{
			// New stream and current and old one not yet played?
			if (S->poldvs)
			{
				int ioff = _kernel_irqs_disabled();
				if (!ioff) _kernel_irqs_off();
				int used = usedoutbuf(S->outb, S->poldvs->vi.channels, S->interlaced);
				if (!ioff) _kernel_irqs_on();
				// Still more in output buffer than added since restart of input
				if (used <= S->usedoutb)
				{
					// Switch to new timing
					Vorbis_Destroy(&S->poldvs);
				}
				else return 0;
			}

			// new stream
			complete_output(S);
			S->decode = Stream_Decode_StreamNew;
			return more;
		}
		break;
		case Stream_Decode_StreamNew:
		{
			// Get the serial number and set up the rest of decode.
			// serialno first; use it to set up a logical stream
			result = Vorbis_New(&S->pvs, ogg_page_serialno(&S->og));
			if (result < 0)
			{
				// fatal
				S->lasterr = err_memory;
				goto fatal_error;
			}
			V = S->pvs;
			Log("New Serial No %08x", V->os.serialno);

			// extract the initial header from the first page and verify
			// that the Ogg bitstream is in fact Vorbis data

			// I handle the initial header first instead of just having
			// the code read all three Vorbis headers at once because reading
			// the initial header is an easy way to identify a Vorbis bitstream
			// and it's useful to see that functionality seperated out.

			result = ogg_stream_pagein(&V->os, &S->og);
			if (result < 0)
			{
				// fatal
				S->lasterr = err_notogg;
				goto fatal_error;
			}

			result = ogg_stream_packetout(&V->os, &V->op);
			if (result != 1)
			{
				// fatal
				S->lasterr = err_notogg;
				goto fatal_error;
			}

			result = vorbis_synthesis_headerin(&V->vi, &V->vc, &V->op);
			if (result < 0)
			{
				S->lasterr = err_notogg;
				goto fatal_error;
			}

			V->object = Stream_Object_Comments;
			S->decode = Stream_Decode_SyncPageOut;

			return more;
		}
		break;
		case Stream_Decode_StreamPageIn:
		{
			result = ogg_stream_pagein(&V->os, &S->og);

			if (result >= 0)
			{
				S->decode = Stream_Decode_StreamPacketOut;
				return more;
			}

			// Corruption recovery
			recover = Stream_Decode_SyncPageOut;
		}
		break;
		case Stream_Decode_StreamPacketOut:
		{
			if (S->outb->finished)
				return 0;

			// Don't do anything if no space to store decoded output
			if ((V->object == Stream_Object_Packet)
			&&  (freesndbuf(&S->sndb, V->vi.channels, S->interlaced) <= ogg_out_size))
				return 0;

			while ((result = ogg_stream_packetout(&V->os, &V->op)) < 0)
				;

			if (result > 0)
			{
				switch (V->object)
				{
					case Stream_Object_Comments:
					{
						result = vorbis_synthesis_headerin(&V->vi, &V->vc, &V->op);
						if (result < 0)
						{
							S->lasterr = err_notogg;
							goto fatal_error;
						}
						V->object = Stream_Object_CodeBook;
					}
					break;
					case Stream_Object_CodeBook:
					{
						result = vorbis_synthesis_headerin(&V->vi, &V->vc, &V->op);
						if (result < 0)
						{
							S->lasterr = err_notogg;
							goto fatal_error;
						}
						V->object = Stream_Object_Packet;

						// OK, got and parsed all three headers. Initialize the Vorbis
						// packet->PCM decoder.
						// central decode state
						result = vorbis_synthesis_init(&V->vd, &V->vi);
						if ((result < 0) || !V->vi.channels)
						{
							S->lasterr = err_notogg;
							goto fatal_error;
						}
						vorbis_block_init(&V->vd, &V->vb);
						// local state for most of the decode
						// so multiple block decodes can
						// proceed in parallel.  We could init
						// multiple vorbis_block structures
						// for vd here

						S->decode = Stream_Decode_SyncPageOut;

						// Block sound output if nr of channels differs from previous stream
						if (S->conf.channels)
						{
							if ((S->conf.channels != V->vi.channels)
							||  (S->conf.rate != V->vi.rate))
							{
								Log("Params change from (%d, %d) to (%d, %d)"
									, S->conf.channels, S->conf.rate
									, V->vi.channels, V->vi.rate);
								S->outb->finished = 1;
							}
						}

						S->conf.channels = V->vi.channels;
						S->conf.rate = V->vi.rate;
						stream_configoutput(S, V);
					}
					break;
					case Stream_Object_Packet:
					{
						if (vorbis_synthesis(&V->vb, &V->op) == 0)
							vorbis_synthesis_blockin(&V->vd, &V->vb);
						S->decode = Stream_Decode_StreamSamples;
					}
					break;
				}
				return more;
			}
			else if (result == 0)
			{
				S->decode = Stream_Decode_SyncPageOut;
				return more;
			}

			// Corruption recovery
			recover = Stream_Decode_SyncPageOut;
		}
		break;
		case Stream_Decode_StreamSamples:
		{
			xint** pcm;

			if (vorbis_synthesis_pcmout(&V->vd, &pcm) > 0)
			{
				*count += output_samples(S, V);
			}
			else
				S->decode = Stream_Decode_StreamPacketOut;

			return more;
		}
		break;
		case Stream_Decode_Finished:
		case Stream_Decode_FatalError:
		{
			return 0;
		}
		break;
	}

	// corruption
	if (!V || (V->object < Stream_Object_Packet))
	{
		// fatal
		S->lasterr = err_badhdr;
		goto fatal_error;
	}

	// recoverable
	S->lasterr = err_badpacket;
	Log("Note (Object %d, Step: %d, Code %d): %s", V->object, S->decode, result, S->lasterr.errmess);
	S->decode = recover;

	return more;

fatal_error:
	if (result == OV_EMEMORY)
		S->lasterr = err_memory;

	if (V)
	{
		Log("Fatal Error (Object %d, Step: %d, Code %d): %s", V->object, S->decode, result, S->lasterr.errmess);
		V->object = Stream_Object_None;
	}
	else
		Log("Fatal Error (Object -1, Step: %d, Code %d): %s", S->decode, result, S->lasterr.errmess);

	S->decode = Stream_Decode_FatalError;
	S->outb->finished = 1;
	S->seek_pos = -1;

	return 0;
}

void Stream_BackgroundProcess(stream* s, int remain)
{
	int c = 0, max, more;
	int ioff = _kernel_irqs_disabled();

	if (s->decode == Stream_Decode_Finished)
		return;

	// copy buffers info so that we are not bothered by interrupts
	if (!ioff) _kernel_irqs_off();
	s->oggb.start = s->oggb.data + s->inb->real_start;
	if (s->oggb.start < s->oggb.data) s->oggb.start = s->oggb.data;
	if (s->oggb.start > s->oggb.last) s->oggb.start = s->oggb.last;
	s->oggb.free  = s->oggb.data + s->inb->real_free;
	if (s->oggb.free < s->oggb.data) s->oggb.free = s->oggb.data;
	if (s->oggb.free > s->oggb.last) s->oggb.free = s->oggb.last;
	bs_fixcount(&s->oggb);
	s->sndb.start = s->sndb.data + s->outb->real_start;
	if (s->sndb.start < s->sndb.data) s->sndb.start = s->sndb.data;
	if (s->sndb.start > s->sndb.last) s->sndb.start = s->sndb.last;
	s->sndb.free  = s->sndb.data + s->outb->real_free;
	if (s->sndb.free < s->sndb.data) s->sndb.free = s->sndb.data;
	if (s->sndb.free > s->sndb.last) s->sndb.free = s->sndb.last;
	if (!ioff) _kernel_irqs_on();

	c = 0;
	more = 1000;
	max = 0;
	if (s->pvs)
	{
		int rate = s->pvs->vi.rate >> 5;

		max = freesndbuf(&s->sndb, s->pvs->vi.channels, s->interlaced) - s->saved_freesndbuf;
		if (max < rate)
			max = rate;
		rate += rate >> 1;
		if (max > rate)
			max = rate;
	}
	if (max < 1000) max = 1000;

	do
	{
		more = one_piece(s, &c, more, remain) - 1;
	}
	while((more > 0) && (c < max));

	if (s->pvs)
	{
		// for position info past input buffer clear
		s->usedoutb += c;
		s->saved_freesndbuf = freesndbuf(&s->sndb, s->pvs->vi.channels, s->interlaced);
	}

	// update buffers
	if (!ioff) _kernel_irqs_off();
	s->inb->real_start = s->oggb.start - s->oggb.data;
	s->outb->real_free = s->sndb.free - s->sndb.data;
	if (!ioff) _kernel_irqs_on();
}

/**
 * Configure Vorbis global parameters.
 *
 * R0 code, R1 value/aation
 *
 * -1       Resets the module (free all streams)
 * 0        Returns module version
 * 1        Sets input buffer size in KB for any new stream, 0 to read
 *          Returns in R1 input buffer size in KB
 * 2        Sets output buffer size in KB for any new stream, 0 to read
 *          Returns in R1 output buffer size in KB
 */
_kernel_oserror* swi_Configure(GlobHdr* g, _kernel_swi_regs* r)
{
	switch(r->r[0])
	{
		case 0:
		{
			r->r[1] = Module_VersionNumber;
			return 0;
		}
		break;
		case 1: // Input buffer size in KBs
		{
			if (r->r[1] != 0)
			{
				int s = 4;

				if (r->r[1] < s)
					r->r[1] = s;
				else if (r->r[1] > 512)
					r->r[1] = 512;

				// Round to power of 2
				while (r->r[1] > s)
					s <<= 1;

				oggb_size = s << 10;
			}

			r->r[1] = oggb_size >> 10;
		}
		break;
		case 2: // Output buffer size in KBs
		{
			if (r->r[1] != 0)
			{
				int s = 64;

				if (r->r[1] < s)
					r->r[1] = s;
				else if (r->r[1] > 4096)
					r->r[1] = 4096;

				// Round to power of 2
				while (r->r[1] > s)
					s <<= 1;

				sndb_size = s << 9;
			}

			r->r[1] = sndb_size >> 9;
		}
		break;
		case 256: // Force stream to limited foreground process
		{
			_kernel_oserror* e;
			stream* s;

			e = Stream_GetHeader(g, r->r[1], &s);
			if (e) return e;

			s->conf.flags |= Stream_Flag_ForegroundProcess | Stream_Flag_LimitedProcess;
		}
		break;
		case -1: // Reset
		{
			Streams_CloseAll(g);
		}
		break;
		default:
			return &err_config;
	}

	return NULL;
}

/**
 * Opens a new stream.
 *  R0 flags
 *
 * Returns
 *  R0 stream handle
 *  R1 ptr to allocated input buffer
 *  R2 ptr to allocated output buffer
 */
_kernel_oserror* swi_StreamOpen(GlobHdr* g, _kernel_swi_regs* r)
{
	_kernel_oserror* e;
	stream* s;
	int index;
	e = Stream_New(g, &s, &index, NULL);
	if (e) return e;

	r->r[0] = index;
	r->r[1] = (int) s->inb;
	r->r[2] = (int) s->outb;

	return NULL;
}

/**
 * Opens a new stream.
 *  R0 flags
 *  R1 ptr to allocated input buffer
 *
 * Returns
 *  R0 stream handle
 *  R1 ptr to given input buffer
 *  R2 ptr to allocated output buffer
 */
_kernel_oserror* swi_StreamAttach(GlobHdr* g, _kernel_swi_regs* r)
{
	_kernel_oserror* e;
	stream* s;
	int index;
	e = Stream_New(g, &s, &index, (dinbuf*) r->r[1]);
	if (e) return e;

	r->r[0] = index;
	r->r[1] = (int) s->inb;
	r->r[2] = (int) s->outb;

	return NULL;
}

/**
 * Closes a stream
 *  R0 stream handle
 */
_kernel_oserror* swi_StreamClose(GlobHdr* g, _kernel_swi_regs* r)
{
	_kernel_oserror* e;
	stream* s;

	e = Stream_GetHeader(g, r->r[0], &s);
	if (e) return e;

	Stream_Destroy(s);
	g->streamsptr[r->r[0]] = NULL;

	return NULL;
}

/**
 * Returns info on a stream
 *  R0 stream handle
 *
 * Returns
 *  R1 Ptr to vorbis_info struct or 0
 *  R2 Ptr to vorbis_comment struct or 0
 *  R3 Serial number or -1
 */
_kernel_oserror* swi_StreamInfo(GlobHdr* g, _kernel_swi_regs* r)
{
	_kernel_oserror* e;
	stream* s;

	e = Stream_GetHeader(g, r->r[0], &s);
	if (e) return e;

	if (s->decode == Stream_Decode_FatalError)
		return &s->lasterr;
	else
	{
		vorbis_stream* v;

		if (s->poldvs)
		{
			v = s->poldvs;
			r->r[1] = (int) &v->vi;
			r->r[2] = (int) &v->vc;
			r->r[3] = v->os.serialno;
		}
		else
		{
			v = s->pvs;

			if (!v)
			{
				r->r[1] = 0;
				r->r[2] = 0;
				r->r[3] = -1;
			}
			else if (v->object != Stream_Object_Packet)
			{
				r->r[1] = 0;
				r->r[2] = 0;
				r->r[3] = v->os.serialno;
			}
			else
			{
				r->r[1] = (int) &v->vi;
				r->r[2] = (int) &v->vc;
				r->r[3] = v->os.serialno;
			}
		}
	}

	return NULL;
}

/**
 * Clears stream buffers
 *  R0 stream handle
 *  R1 flags: bit 0 Clear input buffer
 *            bit 1 Clear output buffer
 *            bit 2 Clear stream info
 */
_kernel_oserror* swi_StreamClear(GlobHdr* g, _kernel_swi_regs* r)
{
	_kernel_oserror* e;
	stream* s;

	e = Stream_GetHeader(g, r->r[0], &s);
	if (e) return e;

	if(r->r[1] & 1)     // Clear input buffer
	{
		stream_clearinput(s, 1);
		Vorbis_Reset(s->pvs);
	}
	if(r->r[1] & 2)     // Clear output buffer
	{
		stream_clearoutput(s);

		Vorbis_Destroy(&s->poldvs);
		if (s->pvs && (s->pvs->object != Stream_Object_None))
		{
			s->pvs->vd.granulepos = -1;
			stream_configoutput(s, s->pvs);
		}
	}
	if(r->r[1] & 4)     // Clear info
		stream_clearinfo(s);

	return NULL;
}

typedef struct
{
	int64_t pos;
	int64_t seek;
	int64_t total;
} pos64_t;

/**
 * Positions a stream, deprecated, should use the 64 version
 *  R0 stream handle
 *  R1 = new pos in samples, -1 to read
 *
 * Returns
 *  R1 = pos in samples
 *  R2 = rate in Hz
 *  R3 = total samples
 *  R4 = seek pos ot -1 if not seeking
 */
_kernel_oserror* swi_StreamPosition(GlobHdr* g, _kernel_swi_regs* r)
{
	_kernel_oserror* e;
	pos64_t pos64;

	pos64.pos = (int64_t) r->r[1];
	r->r[1] = (int) &pos64;
	e = swi_StreamPosition64(g, r);
	if (e) return e;
	r->r[1] = (int) pos64.pos;
	r->r[3] = (int) pos64.total;
	r->r[4] = (int) pos64.seek;

	return NULL;
}

/**
 * Positions a stream, 64 bit version
 *  R0 stream handle
 *  R1 = ptr to pos64 struct with pos64.pos = -1 to read
 *
 * Returns
 *  R1 = ptr to pos64 struct with
 *         pos64.pos = pos in samples
 *         pos64.seek = seek pos or -1 if not seeking
 *         pos64.total = total samples
 *  R2 = rate in Hz
 */
_kernel_oserror* swi_StreamPosition64(GlobHdr* g, _kernel_swi_regs* r)
{
	_kernel_oserror* e;
	stream* s;
	pos64_t* pos64;

	e = Stream_GetHeader(g, r->r[0], &s);
	if (e) return e;

	if (s->decode == Stream_Decode_FatalError)
		return &s->lasterr;

	vorbis_stream* V = s->pvs;
	pos64 = (pos64_t*) r->r[1];

	if (pos64->pos >= 0)
	{
		if (V
		&&  (V->object == Stream_Object_Packet))
		{
			stream_clearinput(s, 1);
			Vorbis_Reset(s->pvs);
			s->seek_pos = pos64->pos;
		}
	}

	// Read current position
	// Use saved final position?
	if (s->poldvs)
	{
		vorbis_stream* PV = s->poldvs;
		int ioff = _kernel_irqs_disabled();
		if (!ioff) _kernel_irqs_off();
		int used = usedoutbuf(s->outb, PV->vi.channels, s->interlaced);
		if (!ioff) _kernel_irqs_on();
		// Still more in output buffer than added since restart of input
		if (used > s->usedoutb)
		{
			// Use saved timing
			pos64->pos = PV->vd.granulepos;
			pos64->pos += (int64_t)(int32_t) (s->usedoutb - used);
			r->r[2] = PV->vi.rate;
		}
		else
		{
			// Switch to new timing
			Vorbis_Destroy(&s->poldvs);
		}
	}
	// Use normal timing?
	if (!s->poldvs)
	{
		if(!V
		|| (V->object != Stream_Object_Packet))
		{
			pos64->pos = 0;
			r->r[2] = 44100;
		}
		else
		{
			if (V->vd.granulepos > 0)
			{
				int remain;
				int ioff = _kernel_irqs_disabled();
				if (!ioff) _kernel_irqs_off();
				int used = usedoutbuf(s->outb, V->vi.channels, s->interlaced);
				if (!ioff) _kernel_irqs_on();
				remain = vorbis_synthesis_pcmout(&V->vd, NULL) + used;
				pos64->pos = V->vd.granulepos - (int64_t) remain;
				if (s->maxgranulepos < V->vd.granulepos)
					s->maxgranulepos = V->vd.granulepos;
			}
			else
				pos64->pos = 0;

			r->r[2] = V->vi.rate;
		}
	}
	if (pos64->pos < 0) pos64->pos = 0;
	pos64->seek = s->seek_pos;
	pos64->total = s->maxgranulepos;

	return NULL;
}

/**
 * Process data in the foreground.
 *  R0 stream handle
 */
_kernel_oserror* swi_StreamProcess(GlobHdr* g, _kernel_swi_regs* r)
{
	_kernel_oserror* e;
	stream* s;

	e = Stream_GetHeader(g, r->r[0], &s);
	if (e) return e;

	s->conf.flags |= Stream_Flag_ForegroundProcess;
	Stream_BackgroundProcess(s
		, (s->conf.flags & Stream_Flag_LimitedProcess) ? r->r[1] : 0);

	return NULL;
}
