DTMF Detection with Goertzel

using NAudio.Wave;
using System;
using System.Collections.Generic;
using System.Linq;

namespace DtmfDetector
{
    class Program
    {
        private const int SampleBlockSize = 205;

        private static readonly double[] frequencies = new[] { 697.0, 770.0, 852.0, 941.0, 1209.0, 1336.0, 1477.0 };

        private static readonly Dictionary<int, Dictionary<int, string>> phoneKeyOf = new Dictionary<int, Dictionary<int, string>>
        {
            {1209, new Dictionary<int, string> {{1477, "?"}, {1336, "?"}, {1209, "?"}, {941, "*"}, {852, "7"}, {770, "4"}, {697, "1"}}},
            {1336, new Dictionary<int, string> {{1477, "?"}, {1336, "?"}, {1209, "?"}, {941, "0"}, {852, "8"}, {770, "5"}, {697, "2"}}},
            {1477, new Dictionary<int, string> {{1477, "?"}, {1336, "?"}, {1209, "?"}, {941, "#"}, {852, "9"}, {770, "6"}, {697, "3"}}},
            { 941, new Dictionary<int, string> {{1477, "#"}, {1336, "0"}, {1209, "*"}, {941, "?"}, {852, "?"}, {770, "?"}, {697, "?"}}},
            { 852, new Dictionary<int, string> {{1477, "9"}, {1336, "8"}, {1209, "7"}, {941, "?"}, {852, "?"}, {770, "?"}, {697, "?"}}},
            { 770, new Dictionary<int, string> {{1477, "6"}, {1336, "5"}, {1209, "4"}, {941, "?"}, {852, "?"}, {770, "?"}, {697, "?"}}},
            { 697, new Dictionary<int, string> {{1477, "3"}, {1336, "2"}, {1209, "1"}, {941, "?"}, {852, "?"}, {770, "?"}, {697, "?"}}}
        };

        public static void Main(string[] args)
        {
            using (WaveFileReader waveFile = new WaveFileReader("dtmftest.wav"))
            {
                var sampleRate = waveFile.WaveFormat.SampleRate;
                var samples = waveFile.ToSampleProvider();

                var sampleBlock = new float[SampleBlockSize];

                while (samples.Read(sampleBlock, 0, SampleBlockSize) == SampleBlockSize)
                {
                    var magnitudes = frequencies.Select(frequency => new
                    {
                        Frequency = frequency,
                        Magnitude = CalculateGoertzel(sampleBlock, frequency, sampleRate)
                    }).ToList();

                    var sortedMagnitudes = magnitudes.OrderByDescending(result => result.Magnitude);
                    var highestMagnitudes = sortedMagnitudes.Take(2).ToList();

                    if (highestMagnitudes.All(result => result.Magnitude > 10.0))
                    {
                        var key = phoneKeyOf[(int)highestMagnitudes[0].Frequency][(int)highestMagnitudes[1].Frequency];

                        Console.WriteLine(
                            waveFile.CurrentTime.TotalSeconds.ToString("00.000") + "s: [" + key + "] key; "
                            + "mags = { " + magnitudes.Select(result => result.Magnitude.ToString("00.000")).Aggregate((result1, result2) => result1 + " ; " + result2) + " }");
                    }
                }
            }
        }

        private static double CalculateGoertzel(float[] samples, double frequency, int sampleRate)
        {
            var normalizedFrequency = Math.Round(frequency * samples.Length / sampleRate);
            var w = (2.0 * Math.PI / samples.Length) * normalizedFrequency;
            var cosine = Math.Cos(w);
            var sine = Math.Sin(w);
            var coeff = 2.0 * cosine;

            var q0 = 0.0;
            var q1 = 0.0;
            var q2 = 0.0;

            foreach(var sample in samples)
            {
                q0 = coeff * q1 - q2 + sample;
                q2 = q1;
                q1 = q0;
            }

            return Math.Sqrt(q1 * q1 + q2 * q2 - q1 * q2 * coeff);
        }
    }
}