/**
   \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))

#if 0
#define DEBUG_PRINT
#endif

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;
  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;
  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 divider = 2*wordsPerSample*MATRIX_OUTPUTS;
  int samples = bytes/divider;

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

#if 0
  printf("samp %d, bytes %d -> %d\n", samples, bytes, samples*divider);
#endif
  
  while (samples) {
    int samplesToHandle = MIN(matrixTmpBufSamples>>(wordsPerSample-1), samples);
#if 0
    printf("samples %d, toHandle %d, tmpSmp %d\n",
	   samples, samplesToHandle, matrixTmpBufSamples);
#endif
    
    fread(matrixTmpBuf, MATRIX_INPUTS, wordsPerSample*samplesToHandle, audio_orig);
    if (wordsPerSample == 1) {
#if 0
      int i;
#endif
      Matrix3To2(matrixTmpBuf, (s_int16 *)data, matrixCoeff, samplesToHandle);
#if 0
      for (i=0; i<samplesToHandle; i++) {
	printf("R %5d %5d\n", data[2*i+0], data[2*i+1]);
      }
#endif
    } else {
      Matrix3To2L((s_int32 *)data, (s_int32 *)matrixTmpBuf, matrixCoeff, samplesToHandle);
    }
    data += samplesToHandle*wordsPerSample*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 divider = 2*wordsPerSample*MATRIX_INPUTS;
  int samples = bytes/divider;

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

#if 0
  printf("samp %d, bytes %d -> %d\n", samples, bytes, samples*divider);
#endif
  
  while (samples) {
    int samplesToHandle = MIN(matrixTmpBufSamples>>(wordsPerSample-1), samples);
#ifdef DEBUG_PRINT
    printf("samples %d, toHandle %d, tmpSmp %d\n",
	   samples, samplesToHandle, matrixTmpBufSamples);
#endif
    
    if (wordsPerSample == 1) {
#ifdef DEBUG_PRINT
      int i;
#endif
      Matrix3To2((s_int16 *)data, matrixTmpBuf, matrixCoeff, samplesToHandle);
#ifdef DEBUG_PRINT
      for (i=0; i<samplesToHandle; i++) {
	printf("W16 %6d %6d\n", matrixTmpBuf[2*i+0], matrixTmpBuf[2*i+1]);
      }
#endif
    } else {
#ifdef DEBUG_PRINT
      int i;
#endif
      Matrix3To2L((s_int32 *)data, (s_int32 *)matrixTmpBuf, matrixCoeff, samplesToHandle);
#ifdef DEBUG_PRINT
      for (i=0; i<samplesToHandle; i++) {
	printf("W32 %11ld %11ld\n",
	       ((s_int32 *)matrixTmpBuf)[2*i+0],
	       ((s_int32 *)matrixTmpBuf)[2*i+1]);
      }
      Delay(100);
#endif
    }
    data += samplesToHandle*wordsPerSample*MATRIX_INPUTS;
    fwrite(matrixTmpBuf, MATRIX_OUTPUTS, wordsPerSample*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 */
};
