dtmf/

decoder.rs

use crate::enums::{State, Tone};

use core::iter::Iterator;
use goertzel_nostd::Parameters;

const TONE_TABLE: [[Tone; 4]; 4] = [
	[Tone::One, Tone::Two, Tone::Three, Tone::A],
	[Tone::Four, Tone::Five, Tone::Six, Tone::B],
	[Tone::Seven, Tone::Eight, Tone::Nine, Tone::C],
	[Tone::Asterisk, Tone::Zero, Tone::Pound, Tone::D],
];

const SAMPLE_RATE: u32 = 8000;
const NUM_SAMPLES: usize = 200;
// how long we need to remain in a state for it to count
const STATE_DURATION: u32 = 2;

const LOW_FREQS: [f32; 4] = [697.0, 770.0, 852.0, 941.0];
const HIGH_FREQS: [f32; 4] = [1209.0, 1336.0, 1477.0, 1633.0];

pub struct Decoder<F: FnMut(Tone, State)> {
	lows: [Parameters; 4],
	highs: [Parameters; 4],
	low_harms: [Parameters; 4],
	high_harms: [Parameters; 4],

	sample_rate: u32,
	tone_change: F,
	cur_tone: Option<Tone>,
	old_tone: Option<Tone>,
	// last tone that we sent through the closure
	old_old_tone: Option<Tone>,

	// how long we've been in the current state
	state_duration: u32,

	// holds samples that haven't been processed yet
	samples: [f32; NUM_SAMPLES],
	sample_len: usize,
	// keeps track of our phase when downsampling
	downsample_phase: f32,
}

impl<F: FnMut(Tone, State)> Decoder<F> {
	pub fn new(sample_rate: u32, tone_change: F) -> Self {
		let lows = [
			Parameters::new(LOW_FREQS[0], SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(LOW_FREQS[1], SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(LOW_FREQS[2], SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(LOW_FREQS[3], SAMPLE_RATE, NUM_SAMPLES),
		];

		let highs = [
			Parameters::new(HIGH_FREQS[0], SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(HIGH_FREQS[1], SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(HIGH_FREQS[2], SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(HIGH_FREQS[3], SAMPLE_RATE, NUM_SAMPLES),
		];

		let low_harms = [
			Parameters::new(LOW_FREQS[0] * 2.0, SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(LOW_FREQS[1] * 2.0, SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(LOW_FREQS[2] * 2.0, SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(LOW_FREQS[3] * 2.0, SAMPLE_RATE, NUM_SAMPLES),
		];

		let high_harms = [
			Parameters::new(HIGH_FREQS[0] * 2.0, SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(HIGH_FREQS[1] * 2.0, SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(HIGH_FREQS[2] * 2.0, SAMPLE_RATE, NUM_SAMPLES),
			Parameters::new(HIGH_FREQS[3] * 2.0, SAMPLE_RATE, NUM_SAMPLES),
		];

		Self {
			lows,
			highs,

			low_harms,
			high_harms,

			sample_rate,
			tone_change,

			cur_tone: None,
			old_tone: None,
			old_old_tone: None,

			state_duration: STATE_DURATION,

			samples: [0.0; NUM_SAMPLES],
			sample_len: 0,

			downsample_phase: 0.0,
		}
	}

	/// Expects samples to be between -1.0 and 1.0
	/// Batch together as many (or as few) samples when processing
	pub fn process(&mut self, samples: &[f32]) {
		self.downsample(samples, |decoder, sample| {
			decoder.samples[decoder.sample_len] = sample;
			decoder.sample_len += 1;
			if decoder.sample_len == NUM_SAMPLES {
				decoder.process_chunk();
				decoder.sample_len = 0;
			}
		});
	}

	fn process_chunk(&mut self) {
		let mut low_powers = [0.0; 4];
		for (i, lp) in low_powers.iter_mut().enumerate() {
			*lp = self.lows[i].mag_squared(&self.samples);
		}

		let mut high_powers = [0.0; 4];
		for (i, hp) in high_powers.iter_mut().enumerate() {
			*hp = self.highs[i].mag_squared(&self.samples);
		}

		let low = main_freq(&low_powers);
		let high = main_freq(&high_powers);

		match (low, high) {
			(Some(l), Some(h)) => {
				// ensure we don't have any strong harmonics of either of the freqs
				// if we do, it's probably a false positive
				let l2 = self.low_harms[l].mag_squared(&self.samples);

				// harmonic must be less than half
				if l2 > (low_powers[l] / 8.0) {
					self.no_tone();
				} else {
					let h2 = self.high_harms[h].mag_squared(&self.samples);

					if h2 > (high_powers[h] / 8.0) {
						self.no_tone();
					} else {
						self.tone(l, h);
					}
				}
			}
			_ => {
				self.no_tone();
			}
		}
	}

	fn tone(&mut self, low: usize, high: usize) {
		let tone = TONE_TABLE[low][high];

		if self.cur_tone != Some(tone) {
			self.old_tone = self.cur_tone;
			self.cur_tone = Some(tone);
			self.state_duration = 0;
		}

		if self.state_duration < STATE_DURATION {
			self.state_duration += 1;

			if self.state_duration == STATE_DURATION && self.cur_tone != self.old_old_tone {
				if let Some(old) = self.old_tone {
					(self.tone_change)(old, State::Off);
				}

				(self.tone_change)(tone, State::On);
				self.old_old_tone = self.cur_tone;
			}
		}
	}

	fn no_tone(&mut self) {
		if let Some(tone) = self.cur_tone {
			self.old_tone = Some(tone);
			self.cur_tone = None;
			self.state_duration = 0;
		}

		if self.state_duration < STATE_DURATION {
			self.state_duration += 1;

			if self.state_duration == STATE_DURATION && self.cur_tone != self.old_old_tone {
				// old tone cannot be none because it was set when we first updated our tone to none
				(self.tone_change)(self.old_tone.unwrap(), State::Off);
				self.old_old_tone = self.cur_tone;
			}
		}
	}

	// gets a single downsampled sample
	// also returns how many input samples were used up
	fn downsample<T: Copy, C: Fn(&mut Self, T)>(&mut self, samples: &[T], func: C) {
		let scale = self.sample_rate as f32 / SAMPLE_RATE as f32;

		let len = samples.len();
		let mut i = 0;
		while i < len {
			if self.downsample_phase > scale {
				self.downsample_phase -= scale;

				func(self, samples[i]);
			}

			let dist = ((scale - self.downsample_phase) as usize)
				.max(1)
				.min(len - i);

			self.downsample_phase += dist as f32;
			i += dist;
		}
	}
}

// gets the dominant frequency in the array, if there is one and it's strong enough
fn main_freq(freqs: &[f32]) -> Option<usize> {
	let mut idx = 0;
	for i in 1..freqs.len() {
		if freqs[i] > freqs[idx] {
			idx = i;
		}
	}

	let mut sum_others = 0.0;
	for (i, v) in freqs.iter().enumerate() {
		if i != idx {
			sum_others += v;
		}
	}

	// main freq must be 8x the sum of the others
	if sum_others * 8.0 < freqs[idx] {
		Some(idx)
	} else {
		None
	}
}