From 8d7cb844e69aa03fb6667681a5b06a3370c72eeb Mon Sep 17 00:00:00 2001
From: Kostya Shishkov <kostya.shishkov@gmail.com>
Date: Tue, 26 Oct 2021 18:12:29 +0200
Subject: [PATCH] codec_support: add function for calculating LPC filter

---
 nihav-codec-support/Cargo.toml     |  1 +
 nihav-codec-support/src/dsp/lpc.rs | 49 ++++++++++++++++++++++++++++++
 nihav-codec-support/src/dsp/mod.rs |  2 ++
 3 files changed, 52 insertions(+)
 create mode 100644 nihav-codec-support/src/dsp/lpc.rs

diff --git a/nihav-codec-support/Cargo.toml b/nihav-codec-support/Cargo.toml
index a5c2455..82162b2 100644
--- a/nihav-codec-support/Cargo.toml
+++ b/nihav-codec-support/Cargo.toml
@@ -16,6 +16,7 @@ h263 = ["blockdsp"]
 dsp = []
 dct = ["dsp"]
 fft = ["dsp"]
+lpc = ["dsp"]
 mdct = ["fft", "dsp"]
 qmf = ["dsp"]
 dsp_window = ["dsp"]
diff --git a/nihav-codec-support/src/dsp/lpc.rs b/nihav-codec-support/src/dsp/lpc.rs
new file mode 100644
index 0000000..94b07ae
--- /dev/null
+++ b/nihav-codec-support/src/dsp/lpc.rs
@@ -0,0 +1,49 @@
+//! LPC filter calculation.
+
+/// Calculated LPC filter for the provided input data, filter order is defined by `filter` length.
+pub fn calc_lpc_filter<T: Copy>(data: &[T], filter: &mut [f64])
+where f64: From<T>
+{
+    let order = filter.len();
+
+    let mut acorr = vec![0.0f64; order + 1];
+
+    for (i, sum) in acorr.iter_mut().enumerate() {
+        let src1 = &data[i..];
+        *sum = data.iter().zip(src1.iter()).fold(0.0, |acc, (&a, &b)| acc + f64::from(a) * f64::from(b));
+    }
+
+    for el in filter.iter_mut() {
+        *el = 0.0;
+    }
+
+    if acorr[0].abs() < 1.0e-6 {
+        return;
+    }
+
+    let mut bwd0 = vec![0.0; order];
+    let mut bwd1 = vec![0.0; order];
+
+    bwd0[0] = 1.0 / acorr[0];
+    filter[order - 1] = acorr[1] / acorr[0];
+
+    for i in 2..=order {
+        let eb = acorr[1..i].iter().zip(bwd0.iter()).fold(0.0, |acc, (&a, &b)| acc + a * b);
+
+        let scale = 1.0 / (1.0 - eb * eb);
+        for j in 0..i {
+            bwd1[j] = if j > 0 { bwd0[j - 1] * scale } else { 0.0 };
+            if j < i - 1 {
+                bwd1[j] -= bwd0[i - 2 - j] * eb * scale;
+            }
+        }
+
+        let ex = acorr[1..i].iter().rev().zip(filter.iter().rev()).fold(0.0, |acc, (&a, &b)| acc + a * b);
+        let scale = acorr[i] - ex;
+        for (filt, &add) in filter.iter_mut().rev().zip(bwd1.iter()).take(i) {
+            *filt += add * scale;
+        }
+
+        std::mem::swap(&mut bwd0, &mut bwd1);
+    }
+}
diff --git a/nihav-codec-support/src/dsp/mod.rs b/nihav-codec-support/src/dsp/mod.rs
index 8d0acc4..b8c32df 100644
--- a/nihav-codec-support/src/dsp/mod.rs
+++ b/nihav-codec-support/src/dsp/mod.rs
@@ -5,6 +5,8 @@ pub mod dct;
 #[cfg(feature="fft")]
 #[allow(clippy::erasing_op)]
 pub mod fft;
+#[cfg(feature="lpc")]
+pub mod lpc;
 #[cfg(feature="mdct")]
 pub mod mdct;
 #[cfg(feature="qmf")]
-- 
2.39.5