#define ADCSAMPLES 25
#define ADC_SAMPLING_RATE 125
#define M_PI 3.14159265358979323846
uint16_t AdcSamplesArr[ADCSAMPLES];

bool goertzel(void)
{
  int TARGET_FREQUENCYS[] = {5, 8, 14};
  bool temp_var = false, flag_frq_exist[] = {true, true, true};

  float coeff, Q0, Q1, Q2, result;

  for(int i = 0; i < 3; i++)
  {
    float prev_result = 0;

    coeff = 2 * cos(M_PI * 2 * (TARGET_FREQUENCYS[i] / ADC_SAMPLING_RATE));

    Q0=0;
    Q1=0;
    Q2=0;

    for(int k = 0; k < ADCSAMPLES; k++)
    {
      Q0 = coeff * Q1 - Q2 + (float) AdcSamplesArr[k];
      Q2 = Q1;
      Q1 = Q0;
      result = Q1 * Q1 + Q2 * Q2 - Q1 * Q2 * coeff;
      if(prev_result < result)
      {
        prev_result = result * 0.85;
      }
      else
      {
        flag_frq_exist[i] = false;
      }
    }
  }
  if((flag_frq_exist[0] == true) && (flag_frq_exist[1] == true) && (flag_frq_exist[2] == true))
  {
    return temp_var = true;
  }
  else
  {
    return temp_var = false;
  }
}