#include <stdio.h>
#include <math.h>

#define FLOATING	float
#define SAMPLE	signed int//unsigned char

//#define SAMPLING_RATE	8000.0	//8kHz
//#define TARGET_FREQUENCY	941.0	//941 Hz
//#define N	205	//Block size

#define SAMPLING_RATE	44100.0	//44.1kHz
#define TARGET_FREQUENCY        21000//500.0	//21000 Hz
#define N	4200//3175//420    //Block size, or 210, or worst 21

FLOATING coeff;
FLOATING Q1;
FLOATING Q2;
FLOATING sine;
FLOATING cosine;

SAMPLE testData[44100*8];
FILE *f;

int index_alt = 0;
int index = 0;
int counter = 1;
int local_counter = 1;
/* Call this routine before every "block" (size=N) of samples. */
void ResetGoertzel(void)
{
  Q2 = 0;
  Q1 = 0;
}

/* Call this once, to precompute the constants. */
void InitGoertzel(void)
{
  int	k;
  FLOATING	floatN;
  FLOATING	omega;

  floatN = (FLOATING) N;
  k = (int) (0.5 + ((floatN * TARGET_FREQUENCY) / SAMPLING_RATE));
  omega = (2.0 * M_PI * k) / floatN;
  sine = sin(omega);
  cosine = cos(omega);
  coeff = 2.0 * cosine;

  printf("For SAMPLING_RATE = %f", SAMPLING_RATE);
  printf(" N = %d", N);
  printf(" and FREQUENCY = %f,\n", TARGET_FREQUENCY);
  printf("k = %d and coeff = %f\n\n", k, coeff);

  ResetGoertzel();
}

/* Call this routine for every sample. */
void ProcessSample(SAMPLE sample)
{
  FLOATING Q0;
  Q0 = coeff * Q1 - Q2 + (FLOATING) sample;
  Q2 = Q1;
  Q1 = Q0;
}


/* Basic Goertzel */
/* Call this routine after every block to get the complex result. */
void GetRealImag(FLOATING *realPart, FLOATING *imagPart)
{
  *realPart = (Q1 - Q2 * cosine);
  *imagPart = (Q2 * sine);
}

/* Optimized Goertzel */
/* Call this after every block to get the RELATIVE magnitude squared. */
FLOATING GetMagnitudeSquared(void)
{
  FLOATING result;

  result = Q1 * Q1 + Q2 * Q2 - Q1 * Q2 * coeff;
  return result;
}

/*** End of Goertzel-specific code, the remainder is test code. */

/* Synthesize some test data at a given frequency. */
void Generate(FLOATING frequency)
{	
      for (; index_alt < counter*N; index_alt++)
      {
	  fscanf(f,"%d",testData+index_alt);
	  testData[index_alt] = testData[index_alt]+32768;
      } 
      printf("Value of counter*N %d\n", counter*N);
      counter++;
      printf("Value of next counter %d\n", counter);
      
// }
}

/* Demo 1 */
void GenerateAndTest(FLOATING frequency)
{

  FLOATING	magnitudeSquared;
  FLOATING	magnitude;
  FLOATING	real;
  FLOATING	imag;

  printf("For test frequency %f:\n", frequency);
  Generate(frequency);

  /* Process the samples */
  for (; index < local_counter*N; index++)
  {
    ProcessSample(testData[index]);
  }
  local_counter++;
  /* Do the "basic Goertzel" processing. */
  GetRealImag(&real, &imag);

  printf("real = %f\n ",real);

  magnitudeSquared = real*real + imag*imag;
  printf("Relative magnitude squared = %f\n", magnitudeSquared);
  magnitude = sqrt(magnitudeSquared);
  printf("Relative magnitude = %f\n", magnitude);

  ResetGoertzel();
}

/* Demo 2 */
void GenerateAndTest2(FLOATING frequency)
{
  int	index;

  FLOATING	magnitudeSquared;
  FLOATING	magnitude;
  FLOATING	real;
  FLOATING	imag;

  printf("%7.1f   ", frequency);
  Generate(frequency);

  /* Process the samples. */
  for (index = 0; index < N; index++)
  {
    ProcessSample(testData[index]);
  }

  /* Do the "standard Goertzel" processing. */
  GetRealImag(&real, &imag);

  magnitudeSquared = real*real + imag*imag;
  printf("%16.5f   ", magnitudeSquared);
  magnitude = sqrt(magnitudeSquared);
  printf("%12.5f\n", magnitude);

  ResetGoertzel();
}

int main(int argc, char **argv)
{
  int i;  
  FLOATING freq;
  f = fopen(argv[1], "r");
  InitGoertzel();

  /* Demo 1 */
  printf("Demo 1\n");
  for(i = 0; i < 352800/N; i++){
//      GenerateAndTest(TARGET_FREQUENCY - 250);
      GenerateAndTest(TARGET_FREQUENCY);
//      GenerateAndTest(TARGET_FREQUENCY + 250);
  }

  /* Demo 2 */
  //printf("Demo 2\n");
  //printf("  Freq\t\t relmag^2 \t rel mag\n"); 
  //for (freq = TARGET_FREQUENCY - 300; freq <= TARGET_FREQUENCY + 300; freq += 15)
  //{
      
  //  GenerateAndTest2(freq);
  //}

  return 0;
}