// RingBuffer and sample rate converter Class for AMP
// (C) 1997 Andy Lo A Foe, arloafoe@cs.vu.nl
// Rewritten Dec 1998 by Claes Löfqvist to gain speed and add automatic speed adjustment
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
//#include "Converter.h"

//#define SPATIE	4096
//#define MARKER	256

Converter::Converter(int32 size)
{
	mutex = create_sem(1, "mutex_semaphore");
	full = create_sem(0, "full_semaphore");
	
	bytes_in_buf = 0;
	buf_size = size;
	out_speed = 44100;	// Default of 44.1Khz
	out_stereo = true;	// stereo stream
	SetInStream(out_speed, out_stereo); // calling this to get step and Spatie!
	waiting_for = 0;
	
	write_index = read_index = 0;

	if ((buffer = new char[buf_size]) == NULL) {
		printf("Error allocating buffer\n");
		return;
	}

	memset(buffer,0,buf_size);
	
	SpeedAdj= 1000;
	SpeedAdjCorr= 1;
	SpeedAdjOn= true;
	NoGaps= false;
}


Converter::~Converter()
{
	delete_sem(full);
	delete_sem(mutex);
	
	delete buffer;
}

void Converter::CalcStepAndSpatie() // Ehh, Spatie is removed!
{
	step = (float) in_speed / (float) out_speed;
}

void Converter::SetOutStream(int32 speed, bool stereo)
{
	out_speed = speed;
	out_stereo = stereo;
	CalcStepAndSpatie();
}

static void memcpy8(short *to, char *from, int32 size)
{
	for ( ; size>1; size-=2 ) {
		*to++= (*from++) * 256 /*<< 8*/;
	}
}

void Converter::Write(char *buf, int32 size, bool EightBits)
{	
	if ((buf_size-bytes_in_buf) <= size) {
		if (SpeedAdjOn) {
			if (!LastWasToMuch) {
				SpeedAdjCorr=1;
				LastWasToMuch= true;
			}
			else if (SpeedAdjCorr<300)
				SpeedAdjCorr+= 1;
			SpeedAdj+= SpeedAdjCorr;
			if (SpeedAdj>2000)
				SpeedAdj= 2000;
		}
//printf ("Much %ld%%%%   \n", SpeedAdj);
		return;
	}

	if (acquire_sem(mutex) != B_NO_ERROR)
		return;

	if ((buf_size-write_index) < size) {
		int32 written = buf_size-write_index;
		if (EightBits) {
			memcpy8((short *)(buffer+write_index), buf, written);
			memcpy8((short *)(buffer), buf+(written/2), size-written);
		}
		else {
			memcpy(buffer+write_index, buf, written);
			memcpy(buffer, buf+written, size-written);
		}
		write_index=size-written;
	} else {
		if (EightBits)
			memcpy8((short *)(buffer+write_index), buf, size);
		else
			memcpy(buffer+write_index, buf, size);
		write_index+=size;
	}
	
	bytes_in_buf+=size;
	release_sem(mutex);
}


void Converter::Discard()
{
	acquire_sem(mutex);
	if (waiting_for) {
		waiting_for = 0;
		release_sem(full);
	}
	bytes_in_buf = 0;
	read_index = write_index = 0;
	release_sem(mutex);
}
					
bool Converter::Read(char *buf, int32 size)
{
	int32 buf_index = 0;
	int32 tmp_index = 0;
	float step = (float) in_speed / (float) out_speed;	

	if (bytes_in_buf < size) {
		if (SpeedAdjOn) {
			if (LastWasToMuch) {
				SpeedAdjCorr=1;
				LastWasToMuch= false;
			}
			else if (SpeedAdjCorr<300)
				SpeedAdjCorr+= 1;
			SpeedAdj-= SpeedAdjCorr;
			if (SpeedAdj<10)
				SpeedAdj= 10;
//printf ("Less %ld%%%%\n", SpeedAdj);
		}
		if (!NoGaps)
			memset(buf,0,size);
		return true;
	}

	// Now we are certain there is enough data for the Read to complete
	// so we don't need those extra checks for buffer overrun...
	// Oops, the above statement was too optimistic. It can go wrong,
	// just not in amp :))
	
	if (acquire_sem(mutex)!=B_NO_ERROR)
		return FALSE;
	
	if ((in_stereo && out_stereo) && (in_speed == out_speed)) {
		if ((buf_size-read_index) < size) {
			int32 readed = buf_size-read_index;
			memcpy(buf, buffer+read_index, readed);
			memcpy(buf+readed, buffer, read_index=size-readed);
		} else {
			memcpy(buf, buffer+read_index, size);
			read_index+=size;
		}
		if (bytes_in_buf<size) {
			memset(buf+bytes_in_buf, 0, size-bytes_in_buf);
			read_index= write_index;
			bytes_in_buf= 0;
		}
		else
			bytes_in_buf-=size;
	} else if (in_stereo && out_stereo) {
		int32 *out_buf = (int32*)buf;
		int32 *in_buf  = (int32*)buffer;
		read_index= read_index>>2;
		float ReadIndex= read_index;
		int32 max_index= buf_size>>2;
		int32 Size=size>>2;
		while (Size-->0) {
			read_index= (int32)ReadIndex;
			if (read_index>=max_index) {
				ReadIndex-=  max_index;
				read_index-= max_index;
			}
			*out_buf++= *(in_buf+read_index);
			ReadIndex+= step;
		}
		read_index= ((int32)ReadIndex)<<2;
		if (read_index>=buf_size) {
			read_index-= buf_size;
		}
		if (bytes_in_buf<size) {
			unsigned long bytes_written= (unsigned long)((double)bytes_in_buf/step);
			if (bytes_written>(unsigned long)bytes_in_buf) bytes_written= size-1; // Should never happen!
			memset(buf+bytes_written, 0, size-bytes_written);
			read_index= write_index;
			bytes_in_buf= 0;
		}
		else {
			if (write_index>read_index)
				bytes_in_buf= write_index-read_index;
			else
				bytes_in_buf= buf_size-(read_index-write_index);
		}
	} else if (!in_stereo && out_stereo) {
		short *out_buf = (short*)buf;
		short *in_buf =  (short*)buffer;
		read_index= read_index>>1;
		float ReadIndex= read_index;
		int32 max_index= buf_size>>1;
		int32 L=size>>2;
		while (L-->0) {
			read_index= (int32)ReadIndex;
			if (read_index>=max_index) {
				ReadIndex-=  max_index;
				read_index-= max_index;
			}
			*out_buf++= *(in_buf+read_index);
			*out_buf++= *(in_buf+read_index);
			ReadIndex+= step;
		}
		read_index= ((int32)ReadIndex)<<1;
		if (read_index>=buf_size) {
			read_index-= buf_size;
		}
		if (bytes_in_buf<size) {
			unsigned long bytes_written= (unsigned long)((double)bytes_in_buf/step);
			if (bytes_written>(unsigned long)bytes_in_buf) bytes_written= size-1; // Should never happen!
			memset(buf+bytes_written, 0, size-bytes_written);
			read_index= write_index;
			bytes_in_buf= 0;
		}
		else {
			if (write_index>read_index)
				bytes_in_buf= write_index-read_index;
			else
				bytes_in_buf= buf_size-(read_index-write_index);
		}
	}
	release_sem(mutex); 
	return true;		
}
