/**
   \file ft3to2.c 3-to-2-Channel Matrixer driver for VSOS
   \author Henrik Herranen, VLSI Solution Oy
*/

#include <vo_stdio.h>
#include <string.h>
#include <stdlib.h>
#include <apploader.h>
#include <ctype.h>
#include <vstypes.h>
#include <vs1005h.h>
#include <timers.h>
#include <vsos.h>
#include <sysmemory.h>
#include <ringbuf.h>
#include <aucommon.h>
#include <mutex.h>
#include "../ft3To2.h"
#include "../miscAsm.h"

extern FILE *audio_orig;

#define MIN(a,b) (((a)<(b))?(a):(b))
#define MAX(a,b) (((a)>(b))?(a):(b))


volatile s_int16 wordsPerSample = 1;

s_int16 __mem_y matrixCoeff[6];
const s_int16 __mem_y defaultMatrixCoeff[6] = {
  MATRIX_SCALE, 0, 0, MATRIX_SCALE, MATRIX_SCALE, MATRIX_SCALE
};
s_int16 matrixTmpBuf[MATRIX_SAMPLE_BUFFER_WORDS];
s_int16 matrixTmpBufSamples;

IOCTL_RESULT AudioIoctl(register __i0 VO_FILE *self, s_int16 request,
			IOCTL_ARGUMENT argp) {
#if 0
  fprintf(stderr, "Ioctl(%p, %d, 0x%p)\n", self, request, argp);
#endif

  switch (request) {
  case IOCTL_RESTART:
    memcpyYY(matrixCoeff, defaultMatrixCoeff, MATRIX_INPUTS*MATRIX_OUTPUTS);
    wordsPerSample = 1;
#ifdef USE_STDAUDIOIN
    matrixTmpBufSamples = MATRIX_SAMPLE_BUFFER_WORDS / MATRIX_INPUTS;
#else
    matrixTmpBufSamples = MATRIX_SAMPLE_BUFFER_WORDS / MATRIX_OUTPUTS;
#endif
    break;
    /* We read a 3-channel source but out output is 2 channels, so
       return error code for channel number requests just like all
       standard stereo drivers. */
#ifdef USE_STDAUDIOIN
  case IOCTL_AUDIO_GET_ICHANNELS:
    return -1;
    break;
#endif
#ifdef USE_STDAUDIOOUT
  case IOCTL_AUDIO_GET_OCHANNELS:
    return 3;
    break;
#endif
  case IOCTL_AUDIO_SET_BITS:
    wordsPerSample = ((s_int16)argp) >> 4;
    break;
  case IOCTL_AUDIO_SET_RATE_AND_BITS:
    wordsPerSample = 1 + (*((s_int32 *)argp) < 0);
    break;
#ifdef USE_STDAUDIOIN
  case IOCTL_AUDIO_GET_INPUT_BUFFER_FILL:
  case IOCTL_AUDIO_GET_INPUT_BUFFER_SIZE:
    {
      int t = ioctl(audio_orig, request, argp);
      if (t >= 0) {
	t = t*MATRIX_OUTPUTS/MATRIX_INPUTS & ~3;
      }
      return t;
    }
  case IOCTL_AUDIO_SET_INPUT_BUFFER_SIZE:
    {
      int t = (s_int16)argp;
      t = t*MATRIX_INPUTS/MATRIX_OUTPUTS & ~3;
      argp = (void *)t;
    }
    break;
#else
  case IOCTL_AUDIO_GET_OUTPUT_BUFFER_FREE:
  case IOCTL_AUDIO_GET_OUTPUT_BUFFER_SIZE:
    {
      int t = ioctl(audio_orig, request, argp);
      if (t >= 0) {
	t = t*MATRIX_INPUTS/MATRIX_OUTPUTS & ~3;
      }
      return t;
    }
    break;
  case IOCTL_AUDIO_SET_OUTPUT_BUFFER_SIZE:
    {
      int t = (s_int16)argp;
      t = t*MATRIX_OUTPUTS/MATRIX_INPUTS & ~3;
      argp = (void *)t;
    }
    break;
#endif
  case IOCTL_AUDIO_GET_MATRIX_DIM:
    /* Only from 3 to 2 channel operation supported */
    return 0x0302;
    break;
  case IOCTL_AUDIO_SET_MATRIX_DIM:
    /* Only from 3 to 2 channel operation supported */
    if ((s_int16)argp != 0x0302) {
      return -1;
    } else {
      return 0;
    }
    break;
  case IOCTL_AUDIO_GET_MATRIX_COEFF:
    memcpyYX(argp, matrixCoeff, MATRIX_INPUTS*MATRIX_OUTPUTS);
    return 0;
    break;
  case IOCTL_AUDIO_SET_MATRIX_COEFF:
    memcpyXY(matrixCoeff, argp, MATRIX_INPUTS*MATRIX_OUTPUTS);
    return 0;
    break;
  }

  return ioctl(audio_orig, request, argp);
}


#ifdef USE_STDAUDIOIN
u_int16 AudioRead(register __i0 VO_FILE *self, u_int16 *data,
		  u_int16 destinationIndex, u_int16 bytes) {
  int myWordsPerSample = wordsPerSample; // Doesn't change during function call
  int divider = 2*myWordsPerSample*MATRIX_OUTPUTS;
  int samples = bytes/divider;

  if (bytes != samples*divider) {
    return 0;
  }

  while (samples) {
    int samplesToHandle = MIN(matrixTmpBufSamples>>(myWordsPerSample-1), samples);
    fread(matrixTmpBuf, MATRIX_INPUTS, myWordsPerSample*samplesToHandle, audio_orig);
    if (myWordsPerSample == 1) {
      Matrix3To2(matrixTmpBuf, (s_int16 *)data, matrixCoeff, samplesToHandle);
    } else {
      Matrix3To2L(matrixTmpBuf, (s_int32 *)data, matrixCoeff, samplesToHandle);
    }
    data += samplesToHandle*myWordsPerSample*MATRIX_OUTPUTS;

    samples -= samplesToHandle;
  }

  return bytes;
}
#endif /* USE_STDAUDIOIN */



#ifdef USE_STDAUDIOOUT
u_int16 AudioWrite(register __i0 VO_FILE *self, u_int16 *data,
		   u_int16 sourceIndex, u_int16 bytes) {
  int myWordsPerSample = wordsPerSample; // Doesn't change during function call
  int divider = 2*myWordsPerSample*MATRIX_INPUTS;
  int samples = bytes/divider;

  if (bytes != samples*divider) {
    return 0;
  }
  
  while (samples) {
    int samplesToHandle = MIN(matrixTmpBufSamples>>(myWordsPerSample-1), samples);
    if (myWordsPerSample == 1) {
      Matrix3To2((s_int16 *)data, matrixTmpBuf, matrixCoeff, samplesToHandle);
    } else {
      Matrix3To2L((s_int32 *)data, (s_int32 *)matrixTmpBuf, matrixCoeff, samplesToHandle);
    }
    data += samplesToHandle*myWordsPerSample*MATRIX_INPUTS;
    fwrite(matrixTmpBuf, MATRIX_OUTPUTS, myWordsPerSample*samplesToHandle, audio_orig);

    samples -= samplesToHandle;
  }

  return bytes;
}
#endif /* USE_STDAUDIOOUT */



FILEOPS audioFileOps = {
  NULL, /* Open() */
  NULL, /* Close() */
  AudioIoctl, /* Ioctl() */
#ifdef USE_STDAUDIOIN
  AudioRead,  /* Read()  (if this is in the input path)  */
#else
  CommonOkResultFunction,
#endif
#ifdef USE_STDAUDIOOUT
  AudioWrite,  /* Write() (if this is in the output path) */
#else
  CommonOkResultFunction,
#endif
};

SIMPLE_FILE audioFile = {
#ifdef USE_STDAUDIOOUT
  __MASK_PRESENT|__MASK_OPEN|__MASK_WRITABLE|__MASK_FILE, /* Flags */
#else
  __MASK_PRESENT|__MASK_OPEN|__MASK_READABLE|__MASK_FILE, /* Flags */
#endif
  NULL, /* Identify() */
  &audioFileOps,
  0, /* pos */
  0, /* ungetc_buffer */
  NULL, /* dev */
};
