1use std::{fmt::Display, time::Instant};
2
3use num_traits::Num;
4
5use crate::servo::{ControlValue, Servo, ServoInput};
6
7pub struct Engine<T: Servo> {
8 pub servo: T,
9 pub last_read_timestamp: Option<Instant>,
10 pub num_reads: u64,
11}
12
13impl<T: Servo + Display> Display for Engine<T> {
14 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
15 write!(
16 f,
17 "Engine runner [{}] has {} total read(s)",
18 self.servo, self.num_reads
19 )
20 }
21}
22
23impl<T: Servo> Engine<T> {
24 pub fn new(servo: T) -> Self {
25 Self {
26 servo,
27 last_read_timestamp: None,
28 num_reads: 0,
29 }
30 }
31
32 pub fn reset(&mut self) {
33 self.last_read_timestamp = None;
34 self.num_reads = 0;
35 }
36
37 pub fn next<N: Num>(&mut self, process_value: N) -> ControlValue {
38 let servo_input = ServoInput {
39 process_value,
40 delta_t: None,
41 };
42 let servo_result = self.servo.read(&servo_input);
43
44 self.num_reads += 1;
45 servo_result.unwrap()
46 }
47}
48
49#[cfg(test)]
50mod tests {
51 use std::fmt::Display;
52
53 use super::Engine;
54 use crate::servo::{ControlValue, Servo};
55
56 struct TestController {}
57
58 impl TestController {
59 fn new() -> Self {
60 Self {}
61 }
62 }
63
64 impl Servo for TestController {
65 fn read<T: num_traits::Num>(
66 &mut self,
67 _servo_input: &crate::servo::ServoInput<T>,
68 ) -> Result<crate::servo::ControlValue, crate::servo::ReadInputError> {
69 Ok(ControlValue { value: 0.0 })
70 }
71 }
72
73 impl Display for TestController {
74 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
75 write!(f, "")
76 }
77 }
78
79 #[test]
80 fn should_instantiate_engine_correctly() {
81 let servo = TestController::new();
82 let engine = Engine::new(servo);
83 assert!(engine.last_read_timestamp.is_none());
84 assert_eq!(engine.num_reads, 0);
85 }
86
87 #[test]
88 fn should_invoke_servo_engine_correctly() {
89 let servo = TestController::new();
90 let mut engine = Engine::new(servo);
91 engine.next(1);
92 engine.next(2);
93 engine.next(3);
94 assert_eq!(engine.num_reads, 3);
95 }
96
97 #[test]
98 fn should_reset_engine_correctly() {
99 let servo = TestController::new();
100 let mut engine = Engine::new(servo);
101 for _ in 0..5 {
102 engine.next(0);
103 }
104 assert_eq!(engine.num_reads, 5);
105 engine.reset();
106 assert_eq!(engine.num_reads, 0);
107 }
108}