1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117

use core::ops::{AddAssign, DivAssign, MulAssign, SubAssign};


use num::{complex::ComplexFloat, traits::FloatConst, Complex, Float, NumCast};
use option_trait::Maybe;

use array_math::SliceMath;

use crate::{Chain, Polynomial, ProductSequence, System, Zpk};

pub trait BesselAP<O>: System + Sized
where
    Self::Domain: Float,
    O: Maybe<usize>
{
    fn besselap(order: O) -> Self;
}

impl<T> BesselAP<usize> for Zpk<Complex<T>, (), Vec<Complex<T>>, T>
where
    T: Float + FloatConst + AddAssign + MulAssign + Into<Complex<T>> + ComplexFloat<Real = T> + ndarray_linalg::Lapack<Complex = Complex<T>>,
    Complex<T>: From<T> + AddAssign + SubAssign + MulAssign + DivAssign + DivAssign<T>,
{
    fn besselap(order: usize) -> Self
    {
        if order == 0
        {
            return Self::one()
        }

        let zero = T::zero();
        let one = T::one();

        let mut p0 = Polynomial::new(vec![one]);
        let mut p1 = Polynomial::new(vec![one, one]);
        for nn in 2..=order
        {
            let x = <T as NumCast>::from(2*nn - 1).unwrap();
            let px = p1.as_view()*Polynomial::new([x]);
            let py = Polynomial::new(p0.into_inner().chain([zero, zero]));
            p0 = p1;
            p1 = px + py;
        }

        let l = p1.len();
        let w = *p1.last().unwrap();
        for (i, p) in p1.iter_mut()
            .enumerate()
        {
            let j = l - i - 1;
            //*p *= w;
            *p *= Float::powf(w, <T as NumCast>::from(j).unwrap()/NumCast::from(l - 1).unwrap())
        }

        let p = p1.rpolynomial_roots();

        Zpk {
            z: ProductSequence::new(()),
            p: ProductSequence::new(p),
            k: T::one()
        }
    }
}
impl<T, const N: usize> BesselAP<()> for Zpk<Complex<T>, (), [Complex<T>; N], T>
where
    T: Float + FloatConst,
    Zpk<Complex<T>, (), Vec<Complex<T>>, T>: BesselAP<usize> + System<Domain = T>
{
    fn besselap((): ()) -> Self
    {
        let Zpk { z, p, k } = Zpk::besselap(N);

        Zpk {
            z,
            p: p.try_into().map_err(|_| ()).unwrap(),
            k
        }
    }
}

#[cfg(test)]
mod test
{
    use array_math::ArrayOps;
    use linspace::LinspaceArray;
    use num::Complex;

    use crate::{plot, BesselAP, Bilinear, FreqS, Plane, RealFreqZ, Zpk};

    #[test]
    fn test()
    {
        let fs = 2.0;
        let h = Zpk::besselap(6);

        plot::plot_pz("H(s)", "plots/pz_s_besselap.png", h.poles(), h.zeros(), Plane::S)
            .unwrap();

        const N: usize = 1024;
        let w: [_; N] = (0.0..fs).linspace_array();
        let h_f = h.freqs(w.map(|w| Complex::new(0.0, w)));

        plot::plot_curves("H(jw)", "plots/h_s_besselap.png", [&w.zip(h_f.map(|h| h.norm())), &w.zip(h_f.map(|h| h.arg()))])
            .unwrap();
        
        let h = h.bilinear(fs)
            .unwrap();

        plot::plot_pz("H(z)", "plots/pz_z_besselap.png", h.poles(), h.zeros(), Plane::Z)
            .unwrap();
        
        let (h_f, w): ([_; N], _) = h.real_freqz(());

        plot::plot_curves("H(e^jw)", "plots/h_z_besselap.png", [&w.zip(h_f.map(|h| h.norm())), &w.zip(h_f.map(|h| h.arg()))])
            .unwrap();
    }
}