/* Goertzel Algorithm
 * http://cms.edn.com/uploads/SourceCode/09banks.txt
 * https://github.com/pramasoul/jrand/blob/master/goertzel.c
 *
 * reads 8khz 8bit samples from stdin, for example using `arecord | dtmf`
 */

#define PI 3.141592653589793

#include <cstdio>
#include <cmath>
#include <cstdint>

#define FLOATING	double
#define SAMPLE	uint8_t

#define SAMPLING_RATE	8000.0	//8kHz
#define N	205	//Block size

struct gzbin {
    FLOATING coeff;
    FLOATING Q1;
    FLOATING Q2;
    FLOATING sine;
    FLOATING cosine;
};


FLOATING dtmf_freq[8] = {
        697.0, 770.0, 852.0, 941.0,
        1209.0, 1336.0, 1477.0, 1633.0};
struct gzbin gzbins[8] = {};

SAMPLE testData[N];

/* Call this routine before every "block" (size=N) of samples. */
void ResetGoertzel(struct gzbin *bin)
{
  bin->Q2 = 0;
  bin->Q1 = 0;
}

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

  floatN = (FLOATING) N;
  k = floor(0.5 + ((floatN * target_frequency) / SAMPLING_RATE));
  omega = (2.0 * PI * k) / floatN;
  bin->sine = sin(omega);
  bin->cosine = cos(omega);
  bin->coeff = 2.0 * bin->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, bin->coeff);

  ResetGoertzel(bin);
}

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

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

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


char dtmf_decode(int x, int y) {
    switch (y) {
        case 0:
            switch (x) {
                case 0:
                    return '1';
                case 1:
                    return '2';
                case 2:
                    return '3';
                case 3:
                    return 'A';
                default:
                    return ' ';
            };
        case 1:
            switch (x) {
                case 0:
                    return '4';
                case 1:
                    return '5';
                case 2:
                    return '6';
                case 3:
                    return 'B';
                default:
                    return ' ';
            };
        case 2:
            switch (x) {
                case 0:
                    return '7';
                case 1:
                    return '8';
                case 2:
                    return '9';
                case 3:
                    return 'C';
                default:
                    return ' ';
            };
        case 3:
            switch (x) {
                case 0:
                    return '*';
                case 1:
                    return '0';
                case 2:
                    return '#';
                case 3:
                    return 'D';
                default:
                    return ' ';
            };
        default:
            return ' ';
    }
}

int main()
{
    FLOATING frequency_magnitudes[8] = {};

    for (int i = 0; i <8; ++i) {
        InitGoertzel(&gzbins[i], dtmf_freq[i]);
    }

    while (true) {
        size_t bytes_read = fread(testData, sizeof(SAMPLE), N, stdin);
        if (bytes_read != N) {
            fprintf(stderr, "Failed to read %d from stdin, got: %zu\n", N,  bytes_read);
            exit(1);
        }

        printf("\n\n");

        for (int i = 0; i < 8; ++i) {
            struct gzbin *bin = &gzbins[i];
            for (unsigned char sample: testData) {
                ProcessSample(bin, sample);
            }
            FLOATING magnitudeSquared = GetMagnitudeSquared(bin);
            frequency_magnitudes[i] = magnitudeSquared;
            printf("Freq: %10.1f; Mag: %20f\n", dtmf_freq[i], magnitudeSquared);
            ResetGoertzel(bin);
        }

        FLOATING max = 0.0;
        FLOATING min = INFINITY;
        int max_y = 0;
        for (int y = 0; y < 4; ++y) {
            FLOATING magnitude = frequency_magnitudes[y];
            if (magnitude > max) {
                max = magnitude;
                max_y = y;
            }
            if (magnitude < min)
                min = magnitude;
        }

        if ((max - min) < 1000) {
            printf("\n");
            continue;
        }

        max = 0.0;
        min = INFINITY;
        int max_x = 0;
        for (int x = 4; x < 8; ++x) {
            FLOATING magnitude = frequency_magnitudes[x];
            if (magnitude > max) {
                max = magnitude;
                max_x = x-4;
            }
            if (magnitude < min)
                min = magnitude;
        }

        if ((max - min) < 10000) {
            printf("\n");
            continue;
        }

        printf("x: %d, y: %d, char: %c\n", max_x, max_y, dtmf_decode(max_x, max_y));


    }


    return 0;
}