/*
  NOTE!
  This driver is specific to VS1005g
*/
#include <vo_stdio.h>
#include <volink.h>     // Linker directives like DLLENTRY
#include <apploader.h>  // RunLibraryFunction etc
#include <string.h>
#include <ctype.h>
#include <mutex.h>
#include <kernel.h>
#include <aucommon.h>
#include <saturate.h>
#include <stdlib.h>
#include <math.h>
#include "../ft3To2.h"

#if 0
#define DEBUG_WRITE
#endif

#define MAX_MATRIX_COEFFS 256
s_int16 matrixCoeff[MAX_MATRIX_COEFFS];

void PrintStrippedFloat(double d);

DLLENTRY(main)
ioresult main(char *parameters) {
  int nParam = RunLibraryFunction("ParamSpl",ENTRY_MAIN,(u_int16)parameters);
  char *p = parameters;
  int i, showMatrix = 1;
  FILE *fp = stdaudioout;
  s_int16 matrixDim = 0;
  ioresult retCode = S_ERROR;
  char *pp = NULL;
  int floatCoeff = 0;
  int verbose = 0;
  int fromCh = 0;
  int toCh = 0;
  int matrixSize = 0;
  
  for (i=0; i<nParam; i++) {
    if (!strcmp(p, "-o")) {
      fp = stdaudioout;
    } else if (!strcmp(p, "-i")) {
      fp = stdaudioin;
    } else if (!strcmp(p, "-h")) {
      printf("Usage: Set3To2 [-o|-i|-sc1,..|--sf1,..v|+v|-h]\n"
	     "-i\tUse stdaudioin\n"
	     "-o\tUse stdaudioout (default)\n"
	     "-sc1,..\tSet matrix coefficients (integer)\n"
	     "-fc1,..\tSet matrix coefficients (float)\n"
	     "-h\tShow this help\n"
	     "-v|+v\tVerbose on/off\n"
	     "\nWith no parameters, shows current coeffs with "
	     "example of how to use \"-s\"\n");
      return S_OK;
    } else if (!strncmp(p, "-s", 2)) {
      int j;
      pp=p+2;
      showMatrix = 0;
      floatCoeff = 0;
    } else if (!strncmp(p, "-f", 2)) {
      int j;
      pp=p+2;
      showMatrix = 0;
      floatCoeff = 1;
    } else if (!strcmp(p, "-v")) {
      verbose = 1;
    } else if (!strcmp(p, "+v")) {
      verbose = 0;
    } else {
      printf("E: Unknown parameter \"%s\"\n", p);
      goto finally;
    }
    p += strlen(p)+1;
  }

  if ((matrixDim = ioctl(fp, IOCTL_AUDIO_GET_MATRIX_DIM, NULL)) < 0) {
    printf("E: Could not find matrix driver in audio path\n");
    goto finally;
  }

  fromCh = (matrixDim>>8) & 0xff;
  toCh = matrixDim & 0xff;
  matrixSize = fromCh*toCh;

  if (pp) {
    char *endP = pp;
    for (i=0; i<matrixSize; i++) {
      long t;
      if (floatCoeff) {
	double j = strtod(endP, &endP);
	t = 1024.0*j;
      } else {
	t = strtol(endP, &endP, 0);
      }
      if (i<matrixSize-1) {
	if (*endP != ',') {
	  printf("E: Too few matrix elements for option \"-s\": \"%s\"\n", pp);
	  goto finally;
	}
      } else {
	if (*endP) {
	  printf("E: Extraneous data for option \"-s\": \"%s\"\n", pp);
	  goto finally;
	}
      }
      matrixCoeff[i] = Sat32To16(t);
      endP++;
    }
    
    ioctl(fp, IOCTL_AUDIO_SET_MATRIX_COEFF, matrixCoeff);
    showMatrix = 0;
  }
  
  if (showMatrix || verbose) {
    s_int16 *c = matrixCoeff;

    printf("Matrix is from %d to %d channels\n", fromCh, toCh);
    if (matrixSize > MAX_MATRIX_COEFFS) {
      printf("E: Too many matrix coefficients (%d, max = %d)\n",
	     matrixSize, MAX_MATRIX_COEFFS);
      goto finally;
    }

    ioctl(fp, IOCTL_AUDIO_GET_MATRIX_COEFF, matrixCoeff);

    for (i=0; i<fromCh; i++) {
      int j;
      
      for (j=0; j<toCh; j++) {
	double dB = -99.9, linear = (1.0/1024) * *c;
	if (*c) {
	  dB = 20.0*log(fabs(linear))/log(10.0);
	}

	printf("  in ch%1d to out ch%1d gain %6d ~= %7.3f ~= %+5.1fdB%s\n",
	       i, j, *c, linear, dB, (*c>=0) ? "" : "(inv)");
	c++;
      }
    }

    c = matrixCoeff;
    printf("\nCommand line option for current coeffs:\n"
	   "  -s%d", *c++);
    for (i=1; i<matrixSize; i++) {
      printf(",%d", *c++);
    }
    c = matrixCoeff;
    printf("\nor\n  -f");
    PrintStrippedFloat((1.0/1024.0) * *c++);
    for (i=1; i<matrixSize; i++) {
      printf(",");
      PrintStrippedFloat((1.0/1024.0) * *c++);
    }
    printf("\n");
  }
  
#ifdef DEBUG_WRITE
  {
    static s_int16 testBuf[] =
      {
       0, 0, 0,
       0, 0, 1000,
       0, 2000, 0,
       0, 2000, 1000,
       4000, 0, 0,
       4000, 0, 1000,
       4000, 2000, 0,
       4000, 2000, 1000,
      };
    static s_int32 testBuf32[] =
      {
       0, 0, 0,
       0, 0, 1000,
       0, 2000, 0,
       0, 2000, 1000,
       4000, 0, 0,
       4000, 0, 1000,
       4000, 2000, 0,
       4000, 2000, 1000,

       0, 0, 0,
       0, 0, -1000,
       0, -2000, 0,
       0, -2000, -1000,
       -4000, 0, 0,
       -4000, 0, -1000,
       -4000, -2000, 0,
       -4000, -2000, -1000,

       0, 0, 0,
       0, 0, 1000000,
       0, 2000000, 0,
       0, 2000000, 1000000,
       4000000, 0, 0,
       4000000, 0, 1000000,
       4000000, 2000000, 0,
       4000000, 2000000, 1000000,

       0, 0, 0,
       0, 0, 100000000,
       0, 200000000, 0,
       0, 200000000, 100000000,
       400000000, 0, 0,
       400000000, 0, 100000000,
       400000000, 200000000, 0,
       400000000, 200000000, 100000000,

       0, 0, 0,
       0, 0, -100000000,
       0, -200000000, 0,
       0, -200000000, -100000000,
       -400000000, 0, 0,
       -400000000, 0, -100000000,
       -400000000, -200000000, 0,
       -400000000, -200000000, -100000000,
      };

    /*
      With matrix 1024, 0, 0, 1024, 1024, 1024, result should be.
         0,    0,
      1000, 1000,
         0, 2000,
      1000, 3000,
      4000,    0,
      5000, 1000,
      4000, 2000,
      5000, 3000
     */
    if (ioctl(fp, IOCTL_AUDIO_SET_BITS, (void *)16) < 0) {
      printf("E: Could not set 16 bits\n");
      goto finally;
    }
    printf("fwrite() = %d\n", fwrite(testBuf, 1, sizeof(testBuf), stdaudioout));

    if (ioctl(fp, IOCTL_AUDIO_SET_BITS, (void *)32) < 0) {
      printf("E: Could not set 32 bits\n");
      goto finally;
    }
    printf("fwrite() = %d\n", fwrite(testBuf32, 1, sizeof(testBuf32), stdaudioout));
  }
#endif

  retCode = S_OK;
 finally:
  return retCode;
}

/* Print float with three decimal places, but remove them if they are zeroes */
void PrintStrippedFloat(double d) {
  char s[16];
  char *sr;
  sprintf(s, "%.3f", d);
  sr = s+strlen(s)-1;
  if (strchr(s, '.')) {
    while (sr > s && *sr == '0') {
      *sr-- = '\0';
    }
    if (sr > s && *sr == '.') {
      *sr-- = '\0';
    }
  }
  printf("%s", s);
}
