#include <vo_stdio.h>
#include <volink.h>     // Linker directives like DLLENTRY
#include <apploader.h>  // RunLibraryFunction etc
#include <string.h>
#include <mutex.h>
#include <kernel.h>
#include <aucommon.h>
#include <taskandstack.h>
#include <consolestate.h>
#include <timers.h>
#include <mutex.h>
#include <ctype.h>
#include <stdlib.h>
#include "dcblock32.h"
#include "auxplay.h"

u_int16 __mem_y quitAudioTask = 0;
struct TaskAndStack *taskAndStack = NULL;
u_int16 __mem_y verbose = 1;


#define DATA_SIZE 24576

s_int16 dataX[DATA_SIZE];
s_int16 __mem_y dataY[DATA_SIZE];
u_int16 inMem, inP;
u_int16 outMem, outP;
s_int32 wordsInBuffer = 0;


#define BUFSIZE 64

int StoreBuf(const s_int16 *b) {
  if (wordsInBuffer >= (2L*DATA_SIZE)) {
    return BUFSIZE;
  }
  if (inMem) {
    memcpyXY(dataY+inP, b, BUFSIZE);
  } else {
    memcpy(dataX+inP, b, BUFSIZE);
  }
  if ((inP += BUFSIZE) >= DATA_SIZE) {
    inP = 0;
    inMem ^= 1;
  }
  wordsInBuffer += BUFSIZE;
  return 0;
}


int GetBuf(s_int16 *b) {
  if (!wordsInBuffer) {
    return BUFSIZE;
  }

  if (outMem) {
    memcpyYX(b, dataY+outP, BUFSIZE);
  } else {
    memcpy(b, dataX+outP, BUFSIZE);
  }
  if ((outP += BUFSIZE) >= DATA_SIZE) {
    outP = 0;
    outMem ^= 1;
  }
  wordsInBuffer -= BUFSIZE;
  return 0;
}


void AudioTask(void) {
  static s_int16 myBuf[BUFSIZE];
  u_int16 firstTime = 1;
  u_int32 lastOFlow = 0, lastUFlow = 0;
  u_int32 lastReport = ReadTimeCount();
  u_int32 localOFlow = 0;
  u_int32 lastGivenCPUTime = lastReport;
  u_int32 sampleRateX256 = 0;
  u_int32 lastFsTime = 0;

  while (!quitAudioTask) {
    u_int32 currTime;
    int nothingToDo = 1;
    if (ioctl(stdaudioin,  IOCTL_AUDIO_GET_INPUT_BUFFER_FILL, NULL) >
	BUFSIZE+4 && wordsInBuffer < (2L*DATA_SIZE)) {
      fread(myBuf, sizeof(s_int16), BUFSIZE, stdaudioin);
      localOFlow += StoreBuf(myBuf);
      nothingToDo = 0;
    }
    if (ioctl(stdaudioout,  IOCTL_AUDIO_GET_OUTPUT_BUFFER_FREE, NULL) >
	BUFSIZE+4){
      if (!GetBuf(myBuf)) {
	fwrite(myBuf, sizeof(s_int16), BUFSIZE, stdaudioout);
	nothingToDo = 0;
      }
    }
    currTime = ReadTimeCount();
    if (verbose && currTime - lastReport >= TICKS_PER_SEC) {
      u_int32 driverOFlow=0;
      u_int32 oFlow, uFlow;
      lastReport += TICKS_PER_SEC;
      ioctl(stdaudioin,  IOCTL_AUDIO_GET_OVERFLOWS,  (void *)(&driverOFlow));
      ioctl(stdaudioout, IOCTL_AUDIO_GET_UNDERFLOWS, (void *)(&uFlow));
      oFlow = driverOFlow + localOFlow;
      if (!firstTime) {
	if (oFlow != lastOFlow) {
	  printf("AUXSPLAY: In overflow +%ld\n", oFlow-lastOFlow);
	}
	if (uFlow != lastUFlow) {
	  printf("AUXSPLAY: Out underflow +%ld\n", uFlow-lastUFlow);
	}
      }
      firstTime = 0;
      lastOFlow = oFlow;
      lastUFlow = uFlow;
      nothingToDo = 0;
    }
    if (currTime != lastFsTime) {
      static struct DcBlock32 dcb = {0};
      u_int32 newSampleRateX256;
      s_int32 wib = wordsInBuffer*128;
      lastFsTime = currTime;
      DcBlock32N(&dcb, 12, &wib, 1);
      wib = wordsInBuffer*128 - wib;
      newSampleRateX256 = 48000L*256 + ((wib-DATA_SIZE*128)/(128L*128/256));
      if (sampleRateX256 != newSampleRateX256) {
	sampleRateX256 = newSampleRateX256;
	ioctl(stdaudioout, IOCTL_AUDIO_SET_ORATE, (void *)(&sampleRateX256));
	//	printf("%ld\n", newSampleRateX256);
      }
      nothingToDo = 0;
    }
    if (nothingToDo || currTime - lastGivenCPUTime >= 10) {
#if 0
      u_int32 newSampleRate = 48000;
      if (wordsInBuffer > 3L*DATA_SIZE/4) {
	newSampleRate = 48000+(wordsInBuffer-3L*DATA_SIZE/4);
      } else if (wordsInBuffer < 1L*DATA_SIZE/4) {
	newSampleRate = 48000+(wordsInBuffer-1L*DATA_SIZE/4);
      }
      if (newSampleRate != sampleRate) {
	printf("fs = %ld\n", newSampleRate);
	sampleRate = newSampleRate;
	ioctl(stdaudioout, IOCTL_AUDIO_SET_ORATE, (void *)(&sampleRate));	
      }
#endif
      Delay(1);
      lastGivenCPUTime = ReadTimeCount();
      
    }
  } /* while (!quitAudioTask) */
}


DLLENTRY(init)
ioresult init(char *paramStr) {
  if (tolower(*paramStr) == 'q') {
    verbose = 0;
  }

  if (!stdaudioin || !stdaudioout ||
      !(taskAndStack = CreateTaskAndStack(AudioTask, "AUXPLAY", 256, 2))) {
    return S_ERROR;
  }

  return S_OK;
}


DLLENTRY(fini)
void fini(void) {
  /* Remove task running AudioTask() */
  if (taskAndStack) {
    quitAudioTask = 1;

    while (taskAndStack->task.tc_State &&
	   taskAndStack->task.tc_State != TS_REMOVED) {
      Delay(TICKS_PER_SEC/100);
    }
    FreeTaskAndStack(taskAndStack);
  }
}
