#include <stdio.h>
#include <helper.h>
#include <pv.h>
#include "burrowing.h"

void burrowing( float *S1, float *S2, float *C1, float *C2, int N2,
		float mult, float thresh,
		int smoothWidth, float **smoothWindow )
{
  int 		real, imag,
		amp,
		phase;

  float 	mminus = 1. - mult,
		reference,
		a1, a2,
		b1, b2;

  int 		i, j;

  static int	first = 1;

  static Kiev	*gates;


  if (first) {

    gates = (Kiev *) space( N2+1, sizeof( Kiev ) );
    
    for ( i=0; i < N2; i++ ) {

      (gates+i)->on = 1;
      (gates+i)->count = 0;
    }

    first = 0;
  }

  for ( i = 0 ; i <= N2 ; i++ ) {
    imag = phase = ( real = amp = i<<1 ) + 1 ;
    a1 = ( i == N2 ? *(S1+1) : *(S1+real) ) ;
    b1 = ( i == 0 || i == N2 ? 0. : *(S1+imag) ) ;
    a2 = ( i == N2 ? S2[1] : S2[real] ) ;
    b2 = ( i == 0 || i == N2 ? 0. : *(S2+imag) ) ;

    *(C1+amp) = fsqrtd( a1*a1 + b1*b1 );

    *(C1+phase) = -atan2( b1, a1 );

    *(C2+amp) = fsqrtd( a2*a2 + b2*b2 );

    *(C2+phase) = -atan2( b2, a2 );
  }

  for ( i=0; i <= N2; i++ ) {
    amp = i<<1;

    /* calculate average reference magnitude */

    reference = *(C2+amp);

    for ( j=0; j < smoothWidth; j++ ) {
      reference += *(*(smoothWindow+j)+i);
    }

    reference /= (float) (smoothWidth + 1);

    /* shift magnitudes */

    for ( j=smoothWidth-1; j > 0; j-- )
      *(*(smoothWindow+(j-1))+i) = *(*(smoothWindow+j)+i);

    *(*(smoothWindow+(smoothWidth-1))+i) = *(C2+amp);


    if ( (gates+i)->on ) {

      *(C1+amp) *= 1. - ( mminus * ( (float) (gates+i)->count /
				     (float) smoothWidth ) );

      if ( reference > thresh ) {

	if ( (gates+i)->count < smoothWidth )
	  (gates+i)->count++;
      }
      
      else {

	if ( (gates+i)->count > 0 )
	  (gates+i)->count--;

      }
    }
  }

  for ( i = 0 ; i <= N2 ; i++ ) {
    imag = phase = ( real = amp = i<<1 ) + 1 ;
    *(S1+real) = *(C1+amp) * cos( *(C1+phase) );

    if ( i != N2 ) 
      *(S1+imag) = -*(C1+amp) * sin( *(C1+phase) );
  }
}