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

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

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

static _kernel_oserror Err_StillHandler =
{0, "Some streams are still in use."};
static _kernel_oserror err_config = {0, "Invalid configuration code."};
static _kernel_oserror Err_TooManyStreamHandlers =
{0, "Maximum number of streams reached."};
static _kernel_oserror Err_Bad_Stream_Handle =
{0, "Incorrect stream handle."};
static _kernel_oserror err_memory = {0, "Out of memory."};

#ifdef MAKEABS
static int bitb_size=128*1024;
static int sndb_size=256*1024;
#else
static int bitb_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.foreground)
		{
			Stream_BackgroundProcess(g->streamsptr[i]);
		}
	}

	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 n;

	n = sndb->start - sndb->free;
	if (n <= 0) n += sndb->size;

	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 void bs_fixcount(bytebuf* bs)
{
	int count = bs->free - bs->start;
	if (count < 0) count += bs->size;
	bs->count = count;
}

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

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

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

#ifdef MAKEABS
	S->outb = mem_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, "AC3 (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->size = sndb_size;

	return NULL;
}

static void removedynamicareas(stream* S)
{
#ifdef MAKEABS
	mem_free(S->inb);
	mem_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 void stream_clearinput(stream* S, int bbuftoo)
{
	int ioff = _kernel_irqs_disabled();

	S->decode = Stream_Decode_Sync;

	if (!ioff) _kernel_irqs_off();

	bs_init(&S->bitb, 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.size = S->outb->size;
	S->sndb.start = S->outb->data;
	S->sndb.last = S->outb->data + S->outb->size - 1;
	S->sndb.free = S->outb->data;

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

static void stream_clearinfo(stream* S)
{
	S->decode = Stream_Decode_Sync;

	mem_free(S->info);
	S->info = NULL;
}

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 = mem_malloc(sizeof(*s));
	if (!s) return &err_memory;

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

	memset(s, 0, sizeof(*s));
	s->pGlobHdr = g;
	s->pvs = a52_init();
	if (!s->pvs)
	{
		e = &err_memory;
		goto Error;
	}
	s->a52bufptr = s->a52buf;
	s->a52bufpos = s->a52bufptr + 7;

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

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

	return NULL;

Error:
	removedynamicareas(s);
	if (s->pvs) a52_free(s->pvs);
	mem_free(s);
	g->streamsptr[i] = NULL;

	return e;
}

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

	if (S->pvs) a52_free(S->pvs);
	mem_free(S->info);
	mem_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 uint8_t* a52_decode_data(stream* S, uint8_t* start, uint8_t* end)
{
//	static const uint8_t channels[16] = {2, 1, 2, 3, 3, 4, 4, 5, 1, 1, 2};
	int flags;
	int sample_rate;
	int bit_rate;
	int len;

	while ((len = end - start) > 0)
	{
		if (len > S->a52bufpos - S->a52bufptr)
			len = S->a52bufpos - S->a52bufptr;
		memcpy (S->a52bufptr, start, len);
		S->a52bufptr += len;
		start += len;
		if (S->a52bufptr == S->a52bufpos)
		{
			if (S->a52bufpos == S->a52buf + 7)
			{
				int length;

				length = a52_syncinfo (S->a52buf, &flags, &sample_rate, &bit_rate);
				if (length)
				{
					if (!S->info)
					{
						S->info = mem_malloc(sizeof(*S->info));
						S->info->samplerate = sample_rate;
						S->info->bitrate = bit_rate / 1000;
						S->info->channels = 2;// channels[flags & A52_CHANNEL_MASK];
					}

					S->decode = Stream_Decode_Frame;
				}
				else
				{
					for (S->a52bufptr = S->a52buf; S->a52bufptr < S->a52buf + 6; S->a52bufptr++)
						S->a52bufptr[0] = S->a52bufptr[1];
					continue;
				}
				S->a52bufpos = S->a52buf + length;
			}
			else
			{
				sample_i level = XIUNIT << 1;

				S->a52bufptr = S->a52buf;
				S->a52bufpos = S->a52buf + 7;

				flags = A52_STEREO | A52_ADJUST_LEVEL;
				if (a52_frame (S->pvs, S->a52buf, &flags, &level))
					goto error;

				S->decode = Stream_Decode_Samples;
				break;

error:
				LogInfo("Error while decoding a52 frame\n");
				S->decode = Stream_Decode_Sync;
			}
		}
	}
	return start;
}

static int one_piece(stream* S, int* count, int more)
{
	switch(S->decode)
	{
		case Stream_Decode_Sync:
		case Stream_Decode_Frame:
		{
			if (S->bitb.start == S->bitb.free)
			{
				if (S->inb->finished)
				{
					S->outb->finished = 1;
					S->decode = Stream_Decode_Finished;
				}
				return 0;
			}

			if (S->bitb.start < S->bitb.free)
			{
				S->bitb.start = a52_decode_data(S, S->bitb.start, S->bitb.free);
			}
			else
			{
				S->bitb.start = a52_decode_data(S, S->bitb.start, S->bitb.last + 1);
				if (S->bitb.start > S->bitb.last)
					S->bitb.start = S->bitb.data;
			}
		}
		break;
		case Stream_Decode_Samples:
		{
			int i,j,k;

			// not enough to store output
			if (freesndbuf(&S->sndb) <= 1536*S->info->channels)
				return 0;

			for (i = 6; i > 0; i--)
			{
				if (a52_block (S->pvs))
				{
					// error
					LogInfo("Error in a52 block %d\n", 6-i);
//					i = 0;
					for (i = i; i > 0; i--)
					{
						for (j = 256; j > 0; j--)
						{
							for (k = S->info->channels; k > 0; k--)
							{
								*S->sndb.free++ = 0;
								if (S->sndb.free > S->sndb.last)
									S->sndb.free = S->sndb.data;
							}
						}
						count += 256;
					}
				}
				else
				{
					sample_i* ps = a52_samples (S->pvs);
					for (j = 256; j > 0; j--, ps++)
					{
						sample_i* ps2 = ps;
						for (k = S->info->channels; k > 0; k--, ps2 += 256)
						{
							int v = *ps2>>(XISHIFT - 15);
							if (v >= 32768) v= 32767;
							if (v < -32768) v= -32768;
							*S->sndb.free++ = v;
							if (S->sndb.free > S->sndb.last)
								S->sndb.free = S->sndb.data;
						}
					}
					count += 256;
				}
			}

			S->decode = Stream_Decode_Sync;
		}
		break;
		case Stream_Decode_Finished:
		case Stream_Decode_FatalError:
		{
			return 0;
		}
		break;
	}

	return more;
}

void Stream_BackgroundProcess(stream* s)
{
	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->bitb.start = s->bitb.data + s->inb->real_start;
	if (s->bitb.start < s->bitb.data) s->bitb.start = s->bitb.data;
	if (s->bitb.start > s->bitb.last) s->bitb.start = s->bitb.last;
	s->bitb.free  = s->bitb.data + s->inb->real_free;
	if (s->bitb.free < s->bitb.data) s->bitb.free = s->bitb.data;
	if (s->bitb.free > s->bitb.last) s->bitb.free = s->bitb.last;
	bs_fixcount(&s->bitb);
	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 = freesndbuf(&s->sndb) - s->saved_freesndbuf;
	if (s->info && s->info->channels)
	{
		int rate = s->info->samplerate >> 5;
		max /= s->info->channels;
		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) - 1;
	}
	while((more > 0) && (c < max));

	s->saved_freesndbuf = freesndbuf(&s->sndb);

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

_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;

				bitb_size = s << 10;
			}

			r->r[1] = bitb_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 -1: // Reset
		{
			Streams_CloseAll(g);
		}
		break;
		default:
			return &err_config;
	}

	return NULL;
}

_kernel_oserror* swi_StreamOpen(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;
}

_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;
}

_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
	{
		stream_info* v = s->info;

		if (!v)
		{
			r->r[1] = 0;
		}
		else
		{
			r->r[1] = (int) v;
		}
	}

	return NULL;
}

_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);
	}
	if(r->r[1] & 2)     // Clear output buffer
	{
		stream_clearoutput(s);
	}
	if(r->r[1] & 4)     // Clear info
		stream_clearinfo(s);

	return NULL;
}

_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.foreground = 1;
	Stream_BackgroundProcess(s);

	return NULL;
}
