1use crate::error::{Error, Result};
11use crate::linalg::{cpow, eigvals, lstsq, svd_s_vh};
12use num_complex::Complex;
13
14type C64 = Complex<f64>;
15
16#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
18pub enum ErrType {
19 #[default]
21 Abs,
22 Rel,
24}
25
26#[derive(Debug, Clone, PartialEq)]
28pub struct EspritParams {
29 pub x_min: f64,
31 pub x_max: f64,
33 pub err: Option<f64>,
37 pub err_type: ErrType,
39 pub m: Option<usize>,
43 pub lfactor: f64,
45 pub tol: f64,
48 pub ctrl_ratio: f64,
51}
52
53impl Default for EspritParams {
54 fn default() -> Self {
55 Self {
56 x_min: 0.0,
57 x_max: 1.0,
58 err: None,
59 err_type: ErrType::Abs,
60 m: None,
61 lfactor: 0.4,
62 tol: 1e-15,
63 ctrl_ratio: 10.0,
64 }
65 }
66}
67
68#[derive(Debug, Clone, Copy, PartialEq, Eq)]
69enum DataType {
70 Real,
71 Imag,
72 Cplx,
73}
74
75#[derive(Debug, Clone)]
77pub struct Esprit {
78 pub n: usize,
80 pub dim: usize,
82 pub l: usize,
84 pub x_min: f64,
86 pub x_max: f64,
88 pub s: Vec<f64>,
90 pub m: usize,
92 pub sigma: f64,
94 pub gamma: Vec<C64>,
96 pub omega: Vec<C64>,
98 pub err_max: f64,
100 pub err_ave: f64,
102 data_type: DataType,
103}
104
105impl Esprit {
106 pub fn new(h: &[C64], n: usize, dim: usize, p: &EspritParams) -> Result<Self> {
112 if h.len() != n * dim || dim == 0 {
113 return Err(Error::InvalidParameter {
114 name: "h_k",
115 value: format!("of length {} for N = {n}, d = {dim}", h.len()),
116 reason: "must have N d entries and d >= 1".to_string(),
117 });
118 }
119 let mut l = (p.lfactor * (n as f64 - 1.0)) as usize;
120 if n < 2 || n - l < l + 1 {
121 return Err(Error::InvalidParameter {
122 name: "lfactor",
123 value: format!("{} for N = {n}", p.lfactor),
124 reason: "must satisfy N - L >= L + 1 with L = int(lfactor (N - 1))".to_string(),
125 });
126 }
127 if p.x_min.is_nan() || p.x_max.is_nan() || p.x_min >= p.x_max {
128 return Err(Error::InvalidParameter {
129 name: "x_min, x_max",
130 value: format!("[{:?}, {:?}]", p.x_min, p.x_max),
131 reason: "must satisfy x_min < x_max".to_string(),
132 });
133 }
134 let max_im = h.iter().map(|v| v.im.abs()).fold(0.0, f64::max);
135 let max_re = h.iter().map(|v| v.re.abs()).fold(0.0, f64::max);
136 let (data_type, h): (DataType, Vec<C64>) = if max_im < p.tol {
137 (
138 DataType::Real,
139 h.iter().map(|v| C64::new(v.re, 0.0)).collect(),
140 )
141 } else if max_re < p.tol {
142 (
143 DataType::Imag,
144 h.iter().map(|v| C64::new(0.0, v.im)).collect(),
145 )
146 } else {
147 (DataType::Cplx, h.to_vec())
148 };
149 let mut out = Self {
150 n,
151 dim,
152 l,
153 x_min: p.x_min,
154 x_max: p.x_max,
155 s: Vec::new(),
156 m: 0,
157 sigma: 0.0,
158 gamma: Vec::new(),
159 omega: Vec::new(),
160 err_max: 0.0,
161 err_ave: 0.0,
162 data_type,
163 };
164 if h.iter().all(|v| *v == C64::new(0.0, 0.0)) {
165 return Ok(out);
166 }
167
168 let (s, vh) = loop {
171 let rows = dim * (n - l);
172 let cols = l + 1;
173 let mut hm = vec![C64::new(0.0, 0.0); rows * cols];
174 for ll in 0..(n - l) {
175 for c in 0..dim {
176 for j in 0..cols {
177 hm[(dim * ll + c) + rows * j] = h[(ll + j) + n * c];
178 }
179 }
180 }
181 match svd_s_vh(&hm, rows, cols) {
182 Ok(f) => break f,
183 Err(e) if l <= 1 => return Err(e),
184 Err(_) => l -= 1,
185 }
186 };
187 out.l = l;
188 let k = s.len();
189
190 let mut m = match p.m {
191 Some(m) => m.min(k - 1),
192 None => {
193 let err = p.err.ok_or_else(|| Error::InvalidParameter {
194 name: "err, M",
195 value: "None".to_string(),
196 reason:
197 "one of them is required (the knee detection for neither is not ported)"
198 .to_string(),
199 })?;
200 let mut m = find_m_with_err(&s, err, p.err_type);
201 if s[m] / s[0] < 1e-14 {
202 m = find_m_with_err(&s, 1e-14, ErrType::Rel);
203 }
204 m
205 }
206 };
207 let x_k: Vec<f64> = linspace(p.x_min, p.x_max, n);
208 loop {
209 out.sigma = s[m];
210 let mut w0t = vec![C64::new(0.0, 0.0); l * m];
212 let mut w1t = vec![C64::new(0.0, 0.0); l * m];
213 for i in 0..m {
214 for j in 0..l {
215 w0t[j + l * i] = vh[i + k * j];
216 w1t[j + l * i] = vh[i + k * (j + 1)];
217 }
218 }
219 let f = lstsq(&w0t, l, m, &w1t, m)?;
220 out.gamma = eigvals(&f, m)?;
221 out.m = m;
222 let mut v = vec![C64::new(0.0, 0.0); n * m];
224 for (j, &g) in out.gamma.iter().enumerate() {
225 for i in 0..n {
226 v[i + n * j] = cpow(g, i as f64);
227 }
228 }
229 out.omega = lstsq(&v, n, m, &h, dim)?;
230 let approx = out.get_value(&x_k);
232 let (mut err_max, mut err_ave) = (0.0_f64, 0.0_f64);
233 for c in 0..dim {
234 let mut sum = 0.0;
235 for i in 0..n {
236 let e = (approx[i + n * c] - h[i + n * c]).norm();
237 err_max = err_max.max(e);
238 sum += e;
239 }
240 err_ave = err_ave.max(sum / n as f64);
241 }
242 out.err_max = err_max;
243 out.err_ave = err_ave;
244 if err_max < (p.ctrl_ratio * out.sigma).max(1e-14 * s[0]) {
245 break;
246 }
247 m = m.wrapping_sub(1);
248 if m == 0 || m == usize::MAX {
249 return Err(Error::DecompositionFailed {
250 reason: "ESPRIT could not find a controlled approximation".to_string(),
251 });
252 }
253 }
254 out.s = s;
255 Ok(out)
256 }
257
258 pub fn get_value(&self, x: &[f64]) -> Vec<C64> {
260 let nx = x.len();
261 let mut out = vec![C64::new(0.0, 0.0); nx * self.dim];
262 for (i, &xi) in x.iter().enumerate() {
263 let e = (self.n as f64 - 1.0) * ((xi - self.x_min) / (self.x_max - self.x_min));
264 for (j, &g) in self.gamma.iter().enumerate() {
265 let vj = cpow(g, e);
266 for c in 0..self.dim {
267 out[i + nx * c] += vj * self.omega[j + self.m * c];
268 }
269 }
270 }
271 for v in &mut out {
272 *v = self.project(*v);
273 }
274 out
275 }
276
277 pub fn get_value_indiv(&self, x: f64, col: usize) -> C64 {
279 let e = (self.n as f64 - 1.0) * ((x - self.x_min) / (self.x_max - self.x_min));
280 let v: C64 = self
281 .gamma
282 .iter()
283 .enumerate()
284 .map(|(j, &g)| cpow(g, e) * self.omega[j + self.m * col])
285 .sum();
286 self.project(v)
287 }
288
289 fn project(&self, v: C64) -> C64 {
290 match self.data_type {
291 DataType::Cplx => v,
292 DataType::Real => C64::new(v.re, 0.0),
293 DataType::Imag => C64::new(0.0, v.im),
294 }
295 }
296}
297
298fn find_m_with_err(s: &[f64], err: f64, err_type: ErrType) -> usize {
300 let cutoff = match err_type {
301 ErrType::Abs => err,
302 ErrType::Rel => s[0] * err,
303 };
304 s.iter().position(|&v| v < cutoff).unwrap_or(s.len() - 1)
305}
306
307pub(crate) fn linspace(a: f64, b: f64, n: usize) -> Vec<f64> {
309 if n == 1 {
310 return vec![a];
311 }
312 let step = (b - a) / (n as f64 - 1.0);
313 let mut x: Vec<f64> = (0..n).map(|i| a + i as f64 * step).collect();
314 x[n - 1] = b;
315 x
316}
317
318#[cfg(test)]
319#[path = "esprit_tests.rs"]
320mod tests;