#include <stdlib.h>
#include <stdio.h>
#include <sfheader.h>
#include <helper.h>
#include <pv.h>
#include <sound.h>

int main(argc, argv)
    int argc; char **argv;
{
    int 	R = 44100,
		N = 1024,
		N2,
		Nw = 0,
		Nw2, 
		D = 0, 
		I = 0,
		in,
		on,
		eof = 0,
		invert = 0,
                replacePhase = 0,
                onlyPhase = 0,
    		shapePeaks = 0,
		*peaks,
		*bitshuffle,
		usage();

    float	tmult = 1.0,
		invthresh = 0.,
		*trigland,
		*Hwin,
		*Wanal,
		*Wsyn,
		*input1,
                *input2,
		*buf1,
                *buf2,
		*chan1,
                *chan2,
		*output;

    char	ch,
		file1[1024],
                file2[1024];

    FILE	*fp1,
                *fp2;
    
    MrSoundStruct	soundStruct1,
			soundStruct2;


    if (argc < 3)
      usage(1);

    synt = 0.0;

    while( (ch = crack( argc, argv, "R|N|M|D|I|t|T|f|F|iScpPh", 0  )) != 0 ) {
	switch(ch) {
	    case 'R':	R = atoi(arg_option);
			break;
	    case 'N':	N = atoi(arg_option);
			break;
	    case 'M':	Nw = atoi(arg_option);
			break;
	    case 'D':	D = atoi(arg_option);
			break;
	    case 'I':	I = atoi(arg_option);
			break;
	    case 't':	tmult = atof(arg_option);
			break;
	    case 'T':	invthresh = atof(arg_option);
			break;
	    case 'f':	strcpy(file1, arg_option);
			break;
	    case 'F':	strcpy(file2, arg_option);
			break;
	    case 'i':	invert = 1;
			break;
	    case 'p':	replacePhase = 1;
			break;
	    case 'P':	onlyPhase = 1;
			break;
	    case 'S':	shapePeaks = 1;
			break;
	    case 'h':	usage(1);
	}
    }


    if (Nw == 0)
	Nw = N;

    if (D == 0)
      D = N / 2;

    if (I == 0)
      I = D;
    
    myPI = 4. * atan(1.);
    myTWOPI = 8. * atan(1.);

    N2 = N>>1;
    Nw2 = Nw>>1;

    /* let -P override -p */

    if (onlyPhase)
      replacePhase = 0;

/* allocate buffers */

    Wanal = (float *) space( Nw, sizeof(float) );
    Wsyn = (float *) space( Nw, sizeof(float) );
    Hwin = (float *) space( Nw, sizeof(float) );
    input1 = (float *) space( Nw, sizeof(float) );
    input2 = (float *) space( Nw, sizeof(float) );
    output = (float *) space( Nw, sizeof(float) );
    input1 = (float *) space( Nw, sizeof(float) );
    input2 = (float *) space( Nw, sizeof(float) );
    buf1 = (float *) space( N, sizeof(float) );
    buf2 = (float *) space( N, sizeof(float) );
    chan1 = (float *) space( N+2, sizeof(float) );
    chan2 = (float *) space( N+2, sizeof(float) );

    peaks = (int *) space( N2+1, sizeof(int) );

    if ( (Nw / D) == 2 )
      makehanning( Hwin, Wanal, Wsyn, Nw, N, I, 0, 1 );
    
    else
      makehanning( Hwin, Wanal, Wsyn, Nw, N, I, 0, 0 );

    
/* FFT cosine funk */
    trigland = (float *) space( N2, sizeof(float) ); 
    
/* FFT bit shuffle */
    bitshuffle = (int *) space( 3+(int)sqrt((float) (N>>1)), sizeof(int) );
  
/* initialize FFT needs */

    init_rdft( N, bitshuffle, trigland );


    in = -Nw;
    if ( D )
	on = (in*I)/D;
    else
	on = in;

/* open input files */

    if ( (fp1 = GetMrSoundStream( file1, &soundStruct1 )) == NULL ) {
      fprintf(stderr,"Cannot open %s\n",file1);
      exit(-1);
    }

    if ( soundStruct1.dataFormat != SND_FORMAT_FLOAT ) {
      fprintf(stderr,"%s is not a 32-bit floating point sound file\n", file1);
      exit(-1);
    }


    if ( (fp2 = GetMrSoundStream( file2, &soundStruct2 )) == NULL ) {
      fprintf(stderr,"Cannot open %s\n",file2);
      exit(-1);
    }

    if ( soundStruct2.dataFormat != SND_FORMAT_FLOAT ) {
      fprintf(stderr,"%s is not a 32-bit floating point sound file\n", file2);
      exit(-1);
    }


    while ( !eof ) {

	in += D;
	on += I;
	
	eof = floatin( input1, Nw, D, fp1 );

	if (eof)
	  (void) floatin( input2, Nw, D, fp2 );

	else
	  eof = floatin( input2, Nw, D, fp2 );

	fold( input1, Wanal, Nw, buf1, N, in );
	fold( input2, Wanal, Nw, buf2, N, in );

	rdft( N, 1, buf1, bitshuffle, trigland );
	rdft( N, 1, buf2, bitshuffle, trigland );


	{
	  register int	i;
	  int		real, imag,
			amp,
			phase;	

	  float		a1,a2,
			b1,b2;

	  for ( i = 0 ; i <= N2 ; i++ ) {
	    imag = phase = ( real = amp = i<<1 ) + 1 ;
	    a1 = ( i == N2 ? *(buf1+1) : *(buf1+real) ) ;
	    b1 = ( i == 0 || i == N2 ? 0. : *(buf1+imag) ) ;
	    a2 = ( i == N2 ? *(buf2+1) : *(buf2+real) ) ;
	      b2 = ( i == 0 || i == N2 ? 0. : *(buf2+imag) ) ;
	    *(chan1+amp) = hypot( a1, b1 );
	    *(chan1+phase) = -atan2( b1, a1 );
	    *(chan2+amp) = hypot( a2, b2 );
	    *(chan2+phase) = -atan2( b2, a2 );
	  }
	    
	
	  /* composite */

	  if (invert) {

	    if (replacePhase) {

	      for ( i=0; i < N2; i+=2 ) {

		if ( *(chan2+i) < ( *(chan1+i) * tmult ) &&
			*(chan2+i) >= invthresh ) {

		  *(chan1+i) = *(chan2+i);

		  if ( *(chan2+i+1) != 0. )
		    *(chan1+i+1) = *(chan2+i+1);
		}

		if ( *(chan1+i+1) == 0. )
		  *(chan1+i+1) = *(chan2+i+1);
	      }
	    }

	    else {

	    /* replace only phases */

	      if (onlyPhase) {

		for ( i=0; i < N2; i+=2 ) {

		  if ( *(chan2+i) < ( *(chan1+i) * tmult ) &&
			*(chan2+i) >= invthresh ) {

		    if ( *(chan2+i+1) != 0. )
		      *(chan1+i+1) = *(chan2+i+1);
		  }

		  if ( *(chan1+i+1) == 0. )
		    *(chan1+i+1) = *(chan2+i+1);
		}
	      }

	    /* use the first sound's phase when non-zero */

	      else {

		for ( i=0; i < N2; i+=2 ) {

		  if ( *(chan2+i) < ( *(chan1+i) * tmult ) && 
			*(chan2+i) >= invthresh ) {

		    *(chan1+i) = *(chan2+i);
		  
		    if ( *(chan1+i+1) == 0. )
		      *(chan1+i+1) = *(chan2+i+1);
		  }
		}
	      }
	    }
	  }

	  /* normal threshold behavior */

	  else {

	    if (replacePhase) {


	      for ( i=0; i < N2; i+=2 ) {

		if ( *(chan2+i) > *(chan1+i) * tmult ) {
		  *(chan1+i) = *(chan2+i);

		  if ( *(chan2+i+1) != 0. )
		    *(chan1+i+1) = *(chan2+i+1);
		}

		if ( *(chan1+i+1) == 0. )
		  *(chan1+i+1) = *(chan2+i+1);
	      }
	    }

	    else {

	      /* only replace phase */ 

	      if (onlyPhase) {

		for ( i=0; i < N2; i+=2 ) {

		  if ( *(chan2+i) > *(chan1+i) * tmult ) {
		  
		    if ( *(chan2+i+1) != 0. )
		      *(chan1+i+1) = *(chan2+i+1);
		  }

		  if ( *(chan1+i+1) == 0. )
		    *(chan1+i+1) = *(chan2+i+1);
		}
	      }

	    /* use first sound's phase when non-zero */

	      else {

		if (shapePeaks) {

		  int	keepCount = 0;

		/* find peaks for source */

		  for ( i=4; i < N-2; i += 2 ) {

		    if ( *(chan1+i) > *(chan1+i-2) &&
			*(chan1+i) > *(chan1+i+2) &&
			*(chan1+i) > *(chan1+i-4) &&
			*(chan1+i) > *(chan1+i+4) ) {
		      
		      *(peaks+keepCount) = i;
		      
		      ++keepCount;
		      
		    }
		  }

/* now that we have the peaks lets shape our mapping signal */

/*
		  for ( i=0; i < keepCount; i++ ) {

		  int		index = *(peaks+i);

		  float		lowerMult = *(chan1+index-2) / *(chan1+index),
				upperMult = *(chan1+index+2) / *(chan1+index),

				newCenter =  ( *(chan2+index-2) +
						*(chan2+index) +
				  		*(chan2+index+2) ) /
						(upperMult + lowerMult + 1);

		  *(chan2+index-2) = lowerMult * newCenter;

		  *(chan2+index+2) = upperMult * newCenter;
		  *(chan2+index) = newCenter; 

		}
*/

/*
		for ( i=0; i < keepCount; i++ ) {

		  int		index = *(peaks+i);

		  float		lowest = *(chan1+index-4) / *(chan1+index),
				lower = *(chan1+index-2) / *(chan1+index),

				upper = *(chan1+index+2) / *(chan1+index),
				uppest = *(chan1+index+2) / *(chan1+index),

		  newCenter =  ( *(chan2+index-4) + *(chan2+index+4) +
						*(chan2+index-2) +
						*(chan2+index) +
				  		*(chan2+index+2) ) /
						(uppest + upper +
						 lower + lowest + 1);


		  *(chan2+index-4) = lowest * newCenter;
		  *(chan2+index+4) = uppest * newCenter;

		  *(chan2+index-2) = lower * newCenter;
		  *(chan2+index+2) = upper * newCenter;

		  *(chan2+index) = newCenter; 

		}
*/

		  for ( i=2; i < N; i += 6 ) {

		    float	lowerMult = *(chan1+i-2) / *(chan1+i),
				upperMult = *(chan1+i+2) / *(chan1+i),

				newCenter =  ( *(chan2+i-2) +
						*(chan2+i) +
				  		*(chan2+i+2) ) /
						(upperMult + lowerMult + 1);

		    *(chan2+i-2) = lowerMult * newCenter;

		    *(chan2+i+2) = upperMult * newCenter;
		    *(chan2+i) = newCenter; 

		  }
		}


		for ( i=0; i < N2; i+=2 ) {
		  if ( *(chan2+i) > *(chan1+i) * tmult )
		    *(chan1+i) = *(chan2+i);
		  
		  if ( *(chan1+i+1) == 0. )
		    *(chan1+i+1) = *(chan2+i+1);
		}
	      }
	    }
	  }

	  /* use first sound's phase when non-zero */

	  for ( i = 0 ; i <= N2 ; i++ ) {
	    imag = phase = ( real = amp = i<<1 ) + 1;
	    *(buf1+real) = *(chan1+amp) * cos( *(chan1+phase) );
	    if ( i != N2 ) 
	      *(buf1+imag) = -*(chan1+amp) * sin( *(chan1+phase) );
	  }
	}
	
	if ( I == 0 ) {
	    fwrite( chan1, sizeof(float), N+2, stdout );
	    fflush( stdout );
	    continue;
	}

	rdft( N, -1, buf1, bitshuffle, trigland );
	overlapadd( buf1, N, Wsyn, output, Nw, on );
	shiftout( output, Nw, I, on );
    }
    exit(0);
}

int usage(woof)
int woof;
{
    fprintf(stderr,
	"ether:  FFT based spectral compositing\n"
	"%% ether   [flags] > floatsams\n"
	"	N:	fft length [2^n] (1024)\n"
	"	R:	sampling rate (44100)\n"
	"	M:	window size in samples (N)\n"
	"	D:	decimation factor in samples (M/4)\n"
	"	I:	interpolation factor in samples (D)\n"
	"	t:	threshold multiplier (1.0)\n"
	"	T:	inverse noise threshold (0.0)\n"
	"	f:	source soundfile (undefined)\n"
	"	F:	mapping soundfile (undefined)\n"
	"	p:	replace phases\n"
	"	P:	only replace phases\n"
	"	S:	shape peaks of mapping signal to source\n"
	"	i:	invert evaluation\n"
	"	h:	this wise guide\n");
    exit(woof);
}