#include <math.h>
#include <malloc.h>
#include <string.h>
#include <assert.h>
#include "m_pd.h"
#define TWOPI 6.283185307
#define MAX_WINDOW_FUNCTIONS 9
#define DEF_BLOCKSIZE 2048
#define DEF_OVERLAP 4
#define DEF_WINDOW_FUNCTION_ID 0
#define MAXBLOCKSIZE 4096
#ifdef NT
#define M_PI 3.14159265358979323846
#pragma warning( disable : 4244 )
#pragma warning( disable : 4305 )
#endif

#ifdef NT
void mayer_realfft(int n, float *real);
void mayer_realifft(int n, float *real);
#endif

static t_class *pitchnoise_class;

typedef struct _pitchnoise
{
  t_object x_obj;
  int x_sampleInPosition;
  int x_overlap;
  int x_sampleOverlap;
  int x_blockSize;
  int x_halfBlockSize;
  int x_blockLessOverlap;
  int x_normalize;
  float x_samplerate;
  double x_limit;
  float x_gain;
  float x_pitch;
  double x_freqsPerBin;
  float *x_inBuffer;
  float *x_outBuffer;
  float *x_fftBuffer;
  float *x_accumBuffer;
  double *x_lastPhase;
  double *x_analyMag;
  double *x_analyFreq;
  double *x_synthMag;
  double *x_synthFreq;
  double *x_sumPhase;
  double *x_real;
  double *x_imag;
  float *x_window;
  void (*x_windowFunction)();
} t_pitchnoise;

void pitchnoise_fillHanning(t_pitchnoise *x) {
  int i;
  int n = x->x_blockSize;
  float xShift = (float)n / 2;
  float w;
  for (i = 0; i < n; i++) {
    w = (i - xShift) / xShift;
    x->x_window[i] = (float)(0.5 * (1 + cos(M_PI * w)));
  }
}

void pitchnoise_fillWindow(t_pitchnoise *x) {
  pitchnoise_fillHanning(x);
}

void pitchnoise_window(t_pitchnoise *x, float *vec) {
  int i;
  for (i = 0; i < x->x_blockSize; i++) {
    vec[i] *= x->x_window[i];
  }
}

void pitchnoise_clearBuffers(t_pitchnoise *x) {
  int i;
  memset(x->x_inBuffer, 0, x->x_blockSize * sizeof(float));
  memset(x->x_outBuffer, 0, x->x_blockSize * sizeof(float)); 
  memset(x->x_fftBuffer, 0, x->x_blockSize * sizeof(float));
  memset(x->x_accumBuffer, 0, x->x_blockSize * sizeof(float));
  memset(x->x_window, 0, x->x_blockSize * sizeof(float));
  memset(x->x_real, 0, x->x_halfBlockSize * sizeof(double));
  memset(x->x_imag, 0, x->x_halfBlockSize * sizeof(double));
  memset(x->x_lastPhase, 0, x->x_halfBlockSize * sizeof(double));
  memset(x->x_analyMag, 0, x->x_halfBlockSize * sizeof(double));
  memset(x->x_analyFreq, 0, x->x_halfBlockSize * sizeof(double));
  memset(x->x_synthMag, 0, x->x_halfBlockSize * sizeof(double));
  memset(x->x_synthFreq, 0, x->x_halfBlockSize * sizeof(double));
  memset(x->x_sumPhase, 0, x->x_halfBlockSize * sizeof(double));
}

void pitchnoise_setLimit(t_pitchnoise *x, t_float f) {
  x->x_limit = f;
}

void pitchnoise_setGain(t_pitchnoise *x, t_float f) {
  x->x_gain = f;
}

void pitchnoise_setPitch(t_pitchnoise *x, t_float f) {
  x->x_pitch = f;
}

void pitchnoise_setBlockSize(t_pitchnoise *x, float blockSize) {
  if (blockSize <= MAXBLOCKSIZE && \
      blockSize >= 64 && \
      ((((int)blockSize - 1) | (int)blockSize) + 1 == 2 * (int)blockSize)) {
    x->x_blockSize = (int)blockSize;
    x->x_halfBlockSize = x->x_blockSize / 2;
    x->x_sampleOverlap = x->x_blockSize / x->x_overlap;
    x->x_blockLessOverlap = x->x_blockSize - x->x_sampleOverlap;
    x->x_sampleInPosition = x->x_blockLessOverlap;
    x->x_freqsPerBin = x->x_samplerate / x->x_blockSize;
    assert(x->x_inBuffer = realloc(x->x_inBuffer, 
       				   x->x_blockSize * sizeof(float)));
    assert(x->x_outBuffer = realloc(x->x_outBuffer,
				    x->x_blockSize * sizeof(float)));
    assert(x->x_fftBuffer = realloc(x->x_fftBuffer, 
				    x->x_blockSize * sizeof(float)));
    assert(x->x_accumBuffer = realloc(x->x_accumBuffer,
				      x->x_blockSize * sizeof(float)));
    assert(x->x_window = realloc(x->x_window,
			         x->x_blockSize * sizeof(float)));
    assert(x->x_real = realloc(x->x_real,
			       x->x_halfBlockSize * sizeof(double)));
    assert(x->x_imag = realloc(x->x_imag,
			       x->x_halfBlockSize * sizeof(double)));
    assert(x->x_lastPhase = realloc(x->x_lastPhase,
				    x->x_halfBlockSize * sizeof(double)));
    assert(x->x_analyMag = realloc(x->x_analyMag,
				   x->x_halfBlockSize * sizeof(double)));
    assert(x->x_analyFreq = realloc(x->x_analyFreq, 
				    x->x_halfBlockSize * sizeof(double))); 
    assert(x->x_synthMag = realloc(x->x_synthMag,
				   x->x_halfBlockSize * sizeof(double)));
    assert(x->x_synthFreq = realloc(x->x_synthFreq,
				    x->x_halfBlockSize * sizeof(double)));
    assert(x->x_sumPhase = realloc(x->x_sumPhase,
				   x->x_halfBlockSize * sizeof(double)));
    pitchnoise_clearBuffers(x);
    pitchnoise_fillWindow(x);
  }
  else {
      post("pitchnoise~: block size must be a power of 2 and at least 64");
  }
}

void pitchnoise_setOverlap(t_pitchnoise *x, float overlap) {
  if ((int)overlap > 0) {
    if ((int)x->x_blockSize % (int)overlap == 0) {
      x->x_overlap = (int)overlap;
      x->x_sampleOverlap = x->x_blockSize / x->x_overlap;
      x->x_blockLessOverlap = x->x_blockSize - x->x_sampleOverlap;
      x->x_sampleInPosition = x->x_blockLessOverlap;
      pitchnoise_clearBuffers(x);
      pitchnoise_fillWindow(x);
    }
    else {
      post("pitchnoise~: overlap must be a factor of the block size");
    }
  }
  else {
      post("pitchnoise~: overlap must be larger than 0");
  }
}

void pitchnoise_setNormalize(t_pitchnoise *x, float norm) {
  x->x_normalize = (int)norm;
}

void pitchnoise_info(t_pitchnoise *x, float f) {
  int i;
  float freq, analyMag, synthMag;

  post(" ");
  for (i = 0; i < x->x_halfBlockSize; i++) {
    freq = x->x_analyFreq[i];
    analyMag = x->x_analyMag[i] / x->x_blockSize * 100;
    synthMag = x->x_synthMag[i] / x->x_blockSize * 100;
    if (analyMag >= f || synthMag >= f)
      post("%5d | %5.2f Hz | %2.1f\% | %2.1f\%",
           i,
           freq,
           analyMag,
           synthMag);
  }
}

void *pitchnoise_new(t_float limit, t_float gain) {
  int i;
  t_pitchnoise *x = (t_pitchnoise *)pd_new(pitchnoise_class);
  x->x_overlap = DEF_OVERLAP;
  x->x_blockSize = DEF_BLOCKSIZE;
  x->x_halfBlockSize = x->x_blockSize / 2;
  x->x_sampleOverlap = x->x_blockSize / x->x_overlap;
  x->x_blockLessOverlap = x->x_blockSize - x->x_sampleOverlap;
  x->x_sampleInPosition = x->x_blockLessOverlap;
  x->x_limit = limit;
  x->x_gain = gain;
  x->x_normalize = 0;
  x->x_pitch = -1;
  x->x_samplerate = sys_getsr();
  x->x_freqsPerBin = x->x_samplerate / x->x_blockSize;
  assert(x->x_inBuffer = calloc(x->x_blockSize, sizeof(float)));
  assert(x->x_outBuffer = calloc(x->x_blockSize, sizeof(float)));
  assert(x->x_fftBuffer = calloc(x->x_blockSize, sizeof(float)));
  assert(x->x_accumBuffer = calloc(x->x_blockSize, sizeof(float)));
  assert(x->x_window = calloc(x->x_blockSize, sizeof(float)));
  assert(x->x_real = calloc(x->x_halfBlockSize, sizeof(double)));
  assert(x->x_imag = calloc(x->x_halfBlockSize, sizeof(double)));
  assert(x->x_lastPhase = calloc(x->x_halfBlockSize, sizeof(double)));
  assert(x->x_analyMag = calloc(x->x_halfBlockSize, sizeof(double)));
  assert(x->x_analyFreq = calloc(x->x_halfBlockSize, sizeof(double))); 
  assert(x->x_synthMag = calloc(x->x_halfBlockSize, sizeof(double)));
  assert(x->x_synthFreq = calloc(x->x_halfBlockSize, sizeof(double)));
  assert(x->x_sumPhase = calloc(x->x_halfBlockSize, sizeof(double)));
  pitchnoise_fillWindow(x);
  outlet_new(&x->x_obj, gensym("signal"));
  inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"), gensym("ft1"));
  inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"), gensym("ft2"));
  return (x);
}

t_int *pitchnoise_perform(t_int *w) {
  t_pitchnoise *x = (t_pitchnoise *)(w[1]);
  t_float *in = (t_float *)(w[2]);
  t_float *out1 = (t_float *)(w[3]);
  int n = (int)(w[4]);
  float tempOut, tempOut2;
  int m, i, j, analyIndex, remapFactor, scaleIndex;
  int sampleDelay, maxMagFreq = 0;
  int divisor = 0;
  float quotient = 0;
  static float incr = 0;
  static double lastMaxMag[MAXBLOCKSIZE / 2];
  double mag, phase, phaseDif, binDif, freqDif, trueFreq, magScale;
  double totalMag = 0;
  double oldTotalMag = 0;
  double maxMag = 0;
  remapFactor = 0;
  for (m = 0; m < n; m++) {
    /* write to in-buffer */
    x->x_inBuffer[x->x_sampleInPosition] = in[m]; 
    /* read from out-buffer */
    tempOut = x->x_outBuffer[x->x_sampleInPosition - x->x_blockLessOverlap];
    out1[m] = tempOut;
    /* increment in-buffer count */
    x->x_sampleInPosition++;
    /* if enough samples to fill block then process */
    if (x->x_sampleInPosition >= x->x_blockSize) {
      memcpy(x->x_fftBuffer, x->x_inBuffer, x->x_blockSize * sizeof(float));
      x->x_sampleInPosition = x->x_blockLessOverlap;
      /* apply windowing function */
      pitchnoise_window(x, x->x_fftBuffer);
      /* do fft */
      mayer_realfft(x->x_blockSize, x->x_fftBuffer);
      /* unmangle fft */
      x->x_real[0] = (double)x->x_fftBuffer[0];
      x->x_imag[0] = 0;
      for (i = 1; i < x->x_halfBlockSize; i++) {
	x->x_real[i] = (double)x->x_fftBuffer[i];
	x->x_imag[i] = (double)x->x_fftBuffer[x->x_blockSize - i];
      }
      /* find magnitude and true frequency */
      for (i = 0; i < x->x_halfBlockSize; i++) {
        mag = sqrt((x->x_real[i] * x->x_real[i]) + \
                   (x->x_imag[i] * x->x_imag[i]));
        /* find phase (mayer seems to return complex conj?) */
        phase = -1 * atan2(x->x_imag[i], x->x_real[i]);
        /* find deviation from expected phase */
        phaseDif = (phase - x->x_lastPhase[i]) - (i * TWOPI / x->x_overlap);
        remapFactor = phaseDif / M_PI;
        if (remapFactor >= 0) 
          remapFactor += remapFactor & 1;
        else 
          remapFactor -= remapFactor & 1;
        phaseDif -= M_PI * remapFactor;
        binDif = x->x_overlap * phaseDif / TWOPI;
        trueFreq = (binDif + i) * x->x_freqsPerBin;
        x->x_analyMag[i] = mag;
        totalMag += mag;
        x->x_analyFreq[i] = trueFreq;
        if (mag > maxMag) {
          maxMag = mag;
          maxMagFreq = trueFreq;
        }
        x->x_lastPhase[i] = phase;
        lastMaxMag[i] = maxMag;
        if (x->x_pitch != -1) {
          maxMagFreq = x->x_pitch;
        }
      }
      /* calculate new frequency and magnitude */
      for (i = 0; i < x->x_halfBlockSize; i++) {
        if (x->x_analyFreq[i] >= maxMagFreq)
          quotient = x->x_analyFreq[i] / maxMagFreq;
	else 
	  quotient = maxMagFreq / x->x_analyFreq[i];
        x->x_synthFreq[i] = x->x_analyFreq[i];
	if (x->x_limit >= 0) {
          /* apply gain to inharmonics */
          /* is this faster? */
          /* (quotient - (int)(quotient)) * 100;
           * if (quotient > 50) quotient = 100 - quotient;
          */
          if ((fabs((int)(quotient + 0.5) - quotient) * 100) >= \
              (50 - x->x_limit)) {
	    x->x_synthMag[i] = x->x_gain * x->x_analyMag[i];
	  }
          else {
            x->x_synthMag[i] = x->x_analyMag[i];
          }
	}
        /* apply gain to harmonics */
	else {
	  if ((fabs((int)(quotient + 0.5) - quotient) * 100) < \
              (-1 * x->x_limit)) {
	    x->x_synthMag[i] = x->x_gain * x->x_analyMag[i];
	  }
          else {
            x->x_synthMag[i] = x->x_analyMag[i];
          }
        }
        oldTotalMag += x->x_synthMag[i];
      }
      if (x->x_normalize) {
        /* scale magnitudes */
        if (oldTotalMag == 0)
          magScale = 0;
        else
          magScale = totalMag / oldTotalMag;
        for (i = 0; i < x->x_halfBlockSize; i++)
          x->x_synthMag[i] *= magScale;
      }
      /* calculate new fft coeff from new frequency and magnitude */
      for (i = 0; i < x->x_halfBlockSize; i++) {
	freqDif = x->x_synthFreq[i] - (i * x->x_freqsPerBin);
	binDif = freqDif * (double)x->x_blockSize / (double)x->x_samplerate;
	phaseDif = TWOPI * binDif / x->x_overlap;
	phase = phaseDif + (i * TWOPI / x->x_overlap);
	x->x_sumPhase[i] -= phase;
	remapFactor = x->x_sumPhase[i] / M_PI;
	if (remapFactor >= 0) 
	  remapFactor += remapFactor & 1;
	else 
	  remapFactor -= remapFactor & 1;
	x->x_sumPhase[i] -= M_PI * remapFactor;
	x->x_real[i] = x->x_synthMag[i] * cos(x->x_sumPhase[i]);
	x->x_imag[i] = x->x_synthMag[i] * sin(x->x_sumPhase[i]);
      }
      /* mangle arrays for inverse fft */
      for (i = 1; i < x->x_halfBlockSize; i++) {
        x->x_fftBuffer[i] = (float)x->x_real[i]; 
        x->x_fftBuffer[x->x_halfBlockSize + i] = \
          x->x_imag[x->x_halfBlockSize - i];
      }
      x->x_fftBuffer[0] = (float)x->x_real[0];
      x->x_fftBuffer[x->x_halfBlockSize] = 0;
      /* do inverse fft */
      mayer_realifft(x->x_blockSize, x->x_fftBuffer);
      /* apply windowing function */
      pitchnoise_window(x, x->x_fftBuffer);
      /* do overlap add */
      for (i = 0; i < x->x_blockSize; i++) {
        x->x_accumBuffer[i] += x->x_fftBuffer[i] / \
                               (x->x_blockSize * x->x_overlap);
      }
      for (i = 0; i < x->x_sampleOverlap; i++) {
        x->x_outBuffer[i] = x->x_accumBuffer[i];
      }
      /* shift buffers */
      memmove(x->x_inBuffer,
              x->x_inBuffer + x->x_sampleOverlap,
              x->x_blockLessOverlap * sizeof(float));
      memmove(x->x_accumBuffer,
              x->x_accumBuffer + x->x_sampleOverlap,
              x->x_blockLessOverlap * sizeof(float));
      memset(x->x_accumBuffer + x->x_blockLessOverlap,
             0,
             x->x_sampleOverlap * sizeof(float));
    }
  }
  return (w + 5);
}

void pitchnoise_dsp(t_pitchnoise *x, t_signal **sp) {
  dsp_add(pitchnoise_perform,
	  4,
	  x,
	  sp[0]->s_vec,
	  sp[1]->s_vec,
	  sp[0]->s_n);
}

void pitchnoise_free(t_pitchnoise *x) {
  free(x->x_inBuffer);
  free(x->x_outBuffer);
  free(x->x_fftBuffer);
  free(x->x_accumBuffer);
  free(x->x_window);
  free(x->x_real);
  free(x->x_imag);
  free(x->x_lastPhase);
  free(x->x_analyMag);
  free(x->x_analyFreq);
  free(x->x_synthMag);
  free(x->x_synthFreq);
  free(x->x_sumPhase);
}

void pitchnoise_tilde_setup(void) {
  pitchnoise_class = class_new(gensym("pitchnoise~"),
                               (t_newmethod)pitchnoise_new,
                               (t_method)pitchnoise_free,
                               sizeof(t_pitchnoise),
                               0,
                               A_DEFFLOAT,
                               A_DEFFLOAT,
                               A_DEFFLOAT,
                               0);
  class_addmethod(pitchnoise_class, nullfn, gensym("signal"), 0);
  class_addmethod(pitchnoise_class,
	          (t_method)pitchnoise_dsp,
	          gensym("dsp"),
	          0);
  class_addmethod(pitchnoise_class,
	          (t_method)pitchnoise_setLimit,
	          gensym("ft1"),
	          A_FLOAT,
	          0);
  class_addmethod(pitchnoise_class,
                  (t_method)pitchnoise_setGain,
                  gensym("ft2"),
                  A_FLOAT,
                  0);
  class_addmethod(pitchnoise_class,
		  (t_method)pitchnoise_setBlockSize,
		  gensym("block"),
		  A_FLOAT,
		  0);
  class_addmethod(pitchnoise_class,
		  (t_method)pitchnoise_setOverlap,
		  gensym("overlap"),
		  A_FLOAT,
		  0);
  class_addmethod(pitchnoise_class,
		  (t_method)pitchnoise_setNormalize,
		  gensym("normalize"),
                  A_FLOAT,
		  0);
  class_addmethod(pitchnoise_class,
                  (t_method)pitchnoise_setPitch,
                  gensym("pitch"),
                  A_FLOAT,
                  0);
  class_addmethod(pitchnoise_class,
                  (t_method)pitchnoise_info,
                  gensym("info"),
                  A_DEFFLOAT,
                  0);
}