pictorus_blocks/core_blocks/
integral_block.rs

1use core::time::Duration;
2
3use crate::{
4    traits::{Float, MatrixOps},
5    Scalar,
6};
7use pictorus_block_data::{BlockData as OldBlockData, FromPass};
8use pictorus_traits::{HasIc, Matrix, Pass, PassBy, ProcessBlock};
9
10/// Performs a discrete integration of the input signal.
11///
12/// It can accept a scalar or a matrix input, for a matrix input it will do an
13/// element wise integration.
14pub struct IntegralBlock<T: Apply>
15where
16    OldBlockData: FromPass<T::Output>,
17{
18    previous_sample: Option<T::Output>,
19    output: Option<T::Output>,
20    pub data: OldBlockData,
21}
22
23impl<T: Apply> Default for IntegralBlock<T>
24where
25    OldBlockData: FromPass<T::Output>,
26{
27    fn default() -> Self {
28        Self {
29            previous_sample: None,
30            output: None,
31            data: <OldBlockData as FromPass<T::Output>>::from_pass(T::Output::default().as_by()),
32        }
33    }
34}
35
36impl<F: Float, R: Scalar> ProcessBlock for IntegralBlock<(F, R)>
37where
38    OldBlockData: FromPass<F>,
39{
40    type Inputs = (F, R);
41    type Output = F;
42    type Parameters = Parameters<(F, R)>;
43
44    fn process<'b>(
45        &'b mut self,
46        parameters: &Self::Parameters,
47        context: &dyn pictorus_traits::Context,
48        inputs: PassBy<'_, Self::Inputs>,
49    ) -> PassBy<'b, Self::Output> {
50        let (sample, reset) = inputs;
51        if reset.is_truthy() {
52            // Reset all state
53            self.output = None;
54            self.previous_sample = None;
55        } else {
56            let delta = match parameters.method {
57                IntgeralMethod::Rectangle => {
58                    F::from_duration(context.timestep().unwrap_or(Duration::ZERO)) * sample
59                }
60                IntgeralMethod::Trapezoidal => {
61                    F::from_duration(context.timestep().unwrap_or(Duration::ZERO))
62                        * (sample + self.previous_sample.unwrap_or(parameters.ic))
63                        / (F::one() + F::one())
64                }
65            };
66            // Add delta to previous output, If output was None (i.e. the very first run, resets don't count) default to ic
67            let output = self
68                .output
69                .insert(self.output.unwrap_or(parameters.ic) + delta);
70
71            // Check for clamping
72            if *output > parameters.clamp_limit {
73                *output = parameters.clamp_limit;
74            } else if *output < -parameters.clamp_limit {
75                *output = -parameters.clamp_limit;
76            }
77            //Store previous sample
78            self.previous_sample = Some(sample);
79        }
80        // This could be none if we were reset above
81        let output = self.output.get_or_insert(parameters.ic);
82        self.data = OldBlockData::from_pass(output.as_by());
83        output.as_by()
84    }
85}
86
87impl<F: Float, R: Scalar> HasIc for IntegralBlock<(F, R)>
88where
89    OldBlockData: FromPass<F>,
90{
91    fn new(parameters: &Self::Parameters) -> Self {
92        IntegralBlock::<(F, R)> {
93            previous_sample: None,
94            output: Some(parameters.ic),
95            data: <OldBlockData as FromPass<F>>::from_pass(parameters.ic.as_by()),
96        }
97    }
98}
99
100impl<F: Float, const NROWS: usize, const NCOLS: usize, R: Scalar> ProcessBlock
101    for IntegralBlock<(Matrix<NROWS, NCOLS, F>, R)>
102where
103    OldBlockData: FromPass<Matrix<NROWS, NCOLS, F>>,
104{
105    type Inputs = (Matrix<NROWS, NCOLS, F>, R);
106    type Output = Matrix<NROWS, NCOLS, F>;
107    type Parameters = Parameters<(Matrix<NROWS, NCOLS, F>, R)>;
108
109    fn process<'b>(
110        &'b mut self,
111        parameters: &Self::Parameters,
112        context: &dyn pictorus_traits::Context,
113        inputs: PassBy<'_, Self::Inputs>,
114    ) -> PassBy<'b, Self::Output> {
115        let (sample, reset) = inputs;
116        if reset.is_truthy() {
117            self.output = None;
118            self.previous_sample = None;
119        } else {
120            let output = self.output.get_or_insert(parameters.ic);
121            sample.for_each(|sample, c, r| {
122                let delta = match parameters.method {
123                    IntgeralMethod::Rectangle => {
124                        F::from_duration(context.timestep().unwrap_or(Duration::ZERO)) * sample
125                    }
126                    IntgeralMethod::Trapezoidal => {
127                        F::from_duration(context.timestep().unwrap_or(Duration::ZERO))
128                            * (sample + self.previous_sample.unwrap_or(parameters.ic).data[c][r])
129                            / (F::one() + F::one())
130                    }
131                };
132                output.data[c][r] += delta;
133                if output.data[c][r] > parameters.clamp_limit {
134                    output.data[c][r] = parameters.clamp_limit;
135                } else if output.data[c][r] < -parameters.clamp_limit {
136                    output.data[c][r] = -parameters.clamp_limit;
137                }
138            });
139            self.previous_sample = Some(*sample);
140        }
141        let output = self.output.get_or_insert(parameters.ic);
142        self.data = OldBlockData::from_pass(output);
143        output
144    }
145}
146
147impl<F: Float, const NROWS: usize, const NCOLS: usize, R: Scalar> HasIc
148    for IntegralBlock<(Matrix<NROWS, NCOLS, F>, R)>
149where
150    OldBlockData: FromPass<Matrix<NROWS, NCOLS, F>>,
151{
152    fn new(parameters: &Self::Parameters) -> Self {
153        IntegralBlock::<(Matrix<NROWS, NCOLS, F>, R)> {
154            previous_sample: None,
155            output: Some(parameters.ic),
156            data: <OldBlockData as FromPass<Matrix<NROWS, NCOLS, F>>>::from_pass(&parameters.ic),
157        }
158    }
159}
160
161pub trait Apply: Pass + Default {
162    type Float: Float;
163    type Output: Pass + Default;
164}
165
166impl<F: Float, R: Scalar> Apply for (F, R) {
167    type Float = F;
168    type Output = F;
169}
170
171impl<F: Float, const NROWS: usize, const NCOLS: usize, R: Scalar> Apply
172    for (Matrix<NROWS, NCOLS, F>, R)
173{
174    type Float = F;
175    type Output = Matrix<NROWS, NCOLS, F>;
176}
177
178/// Controls the method of integration
179/// Rectangle: Uses the value of the sample at the current time step and the time step duration to calculate the integral.
180/// Trapezoidal: Uses the average of the sample at the current time step and the previous time step and the time step duration to calculate the integral.
181#[derive(strum::EnumString, Debug, Clone, Copy)]
182pub enum IntgeralMethod {
183    Rectangle,
184    Trapezoidal,
185}
186
187pub struct Parameters<T: Apply> {
188    pub clamp_limit: T::Float,
189    pub ic: T::Output,
190    pub method: IntgeralMethod,
191}
192
193/// Parameters for Integral Block
194/// ic: Initial condition, the value of the integral at the start of the simulation
195/// clamp_limit: Maximum absolute value of the integral (or each element of the integral in case of matrix input)
196/// method: Method of integration (See [`IntgeralMethod`])
197impl<T: Apply> Parameters<T> {
198    pub fn new(ic: T::Output, clamp_limit: T::Float, method: &str) -> Self {
199        Parameters {
200            clamp_limit,
201            ic,
202            method: method.parse().unwrap(),
203        }
204    }
205}
206
207#[cfg(test)]
208mod tests {
209    use super::*;
210    use crate::testing::{StubContext, StubRuntime};
211    use crate::SinewaveBlock;
212    use approx::assert_relative_eq;
213    use pictorus_traits::GeneratorBlock;
214
215    #[test]
216    fn test_integral_scalar() {
217        let mut runtime = StubRuntime::default();
218
219        let mut block = IntegralBlock::<(f64, bool)>::default();
220        let parameters = Parameters::new(0.0, 20.0, "Trapezoidal");
221
222        let mut sine_wave_block = SinewaveBlock::<f64>::default();
223        let sine_wave_parameters =
224            <SinewaveBlock<f64> as GeneratorBlock>::Parameters::new(1.0, 1.0, 0.0, 0.0);
225
226        let mut cosine_wave_block = SinewaveBlock::<f64>::default();
227        // Cosine is sinewave offset with pi/2 phase shift
228        let cosine_wave_parameters =
229            <SinewaveBlock<f64> as GeneratorBlock>::Parameters::new(1.0, 1.0, f64::PI / 2., 0.0);
230
231        for _ in 0..100 {
232            let sine_output = sine_wave_block.generate(&sine_wave_parameters, &runtime.context());
233            let cosine_output =
234                cosine_wave_block.generate(&cosine_wave_parameters, &runtime.context());
235            runtime.tick();
236
237            let integral_output = block.process(
238                &parameters,
239                &runtime.context(),
240                (cosine_output, false).as_by(),
241            );
242            // Integral of cosine is sine, with a small offset and allowing discrete tolerance.
243            assert_relative_eq!(integral_output, sine_output + 0.05, epsilon = 0.01);
244        }
245
246        // Reset with any input value
247        let reset_output =
248            block.process(&parameters, &runtime.context(), (1000000.0, true).as_by());
249        assert_relative_eq!(reset_output, 0.0, epsilon = 0.01);
250    }
251
252    #[test]
253    fn test_integral_matrix() {
254        let mut runtime = StubRuntime::new(StubContext::new(
255            Duration::ZERO,
256            None,
257            Duration::from_secs(1),
258        ));
259
260        let mut block: IntegralBlock<(Matrix<1, 3, f64>, bool)> =
261            IntegralBlock::<(Matrix<1, 3, f64>, bool)>::default();
262        let parameters: Parameters<(Matrix<1, 3, f64>, bool)> = Parameters::new(
263            Matrix {
264                data: [[0.0], [0.0], [0.0]],
265            },
266            20.0,
267            "Rectangle",
268        );
269
270        let input = Matrix {
271            data: [[1.0], [1.0], [15.0]],
272        };
273
274        let output = block.process(&parameters, &runtime.context(), (input, false).as_by());
275        assert_eq!(output.data, [[0.0], [0.0], [0.0]]);
276        runtime.tick();
277        let output = block.process(&parameters, &runtime.context(), (input, false).as_by());
278        assert_eq!(output.data, [[1.0], [1.0], [15.0]]);
279
280        runtime.tick();
281        let output = block.process(&parameters, &runtime.context(), (input, false).as_by());
282        // Hits clamp limit
283        assert_eq!(output.data, [[2.0], [2.0], [20.0]]);
284    }
285
286    #[test]
287    fn test_integral_ic_scalar() {
288        let mut runtime = StubRuntime::new(StubContext::new(
289            Duration::ZERO,
290            None,
291            Duration::from_secs(1),
292        ));
293
294        let parameters = Parameters::new(10.0, 50.0, "Rectangle");
295        let mut block = IntegralBlock::<(f64, bool)>::new(&parameters);
296        // Check the initial value is set
297        assert_eq!(block.data.scalar(), 10.0);
298
299        let input = 25.0;
300        let output = block.process(&parameters, &runtime.context(), (input, false).as_by());
301        // This is pretty confusing, but the dt of the first tick is 0, so the output is the same as the IC
302        assert_eq!(output, 10.0);
303        runtime.tick();
304        let output = block.process(&parameters, &runtime.context(), (input, false).as_by());
305        assert_eq!(output, 35.0);
306
307        runtime.tick();
308        let output = block.process(&parameters, &runtime.context(), (input, false).as_by());
309        assert_eq!(output, 50.0);
310    }
311
312    #[test]
313    fn test_integral_ic_matrix() {
314        let mut runtime = StubRuntime::new(StubContext::new(
315            Duration::ZERO,
316            None,
317            Duration::from_secs(1),
318        ));
319
320        let parameters = Parameters::new(
321            Matrix {
322                data: [[10.0], [10.0], [10.0]],
323            },
324            50.0,
325            "Rectangle",
326        );
327        let mut block = IntegralBlock::<(Matrix<1, 3, f64>, bool)>::new(&parameters);
328        // Check the initial value is set
329        assert_eq!(
330            block.data.get_data().as_slice(),
331            [[10.0], [10.0], [10.0]].as_flattened()
332        );
333
334        let input = Matrix {
335            data: [[1.0], [1.0], [25.0]],
336        };
337        let output = block.process(&parameters, &runtime.context(), (input, false).as_by());
338        assert_eq!(output.data, [[10.0], [10.0], [10.0]]);
339        runtime.tick();
340        let output = block.process(&parameters, &runtime.context(), (input, false).as_by());
341        assert_eq!(output.data, [[11.0], [11.0], [35.0]]);
342
343        runtime.tick();
344        let output = block.process(&parameters, &runtime.context(), (input, false).as_by());
345        // Hits clamp limit
346        assert_eq!(output.data, [[12.0], [12.0], [50.0]]);
347    }
348}
pictorus_blocks/core_blocks/integral_block.rs

pictorus_blocks/core_blocks/
integral_block.rs
