1use r2r::builtin_interfaces::msg::{Duration, Time};
2
3const BILLION: i64 = 1_000_000_000;
4
5pub(crate) fn time_from_nanosec(t: i64) -> Time {
6 Time {
7 sec: (t / BILLION) as i32,
8 nanosec: (t % BILLION) as u32,
9 }
10}
11
12pub(crate) fn sub_time_and_time(target: &Time, delta: &Time) -> Duration {
14 if delta.sec.is_positive() {
15 if target.nanosec >= delta.nanosec {
16 Duration {
17 sec: target.sec - delta.sec,
18 nanosec: target.nanosec - delta.nanosec,
19 }
20 } else {
21 Duration {
22 sec: target.sec - delta.sec - 1,
23 nanosec: BILLION as u32 + target.nanosec - delta.nanosec,
24 }
25 }
26 } else {
27 let nanosec = target.nanosec + delta.nanosec;
28 let sec = target.sec - delta.sec + nanosec as i32 / BILLION as i32;
29 let nanosec = nanosec % BILLION as u32;
30 Duration { sec, nanosec }
31 }
32}
33
34pub(crate) fn add_time_and_duration(t: &Time, d: &Duration) -> Time {
35 let sec = t.sec + d.sec;
36 let nanosec = t.nanosec + d.nanosec;
37
38 Time {
39 sec: sec + nanosec as i32 / BILLION as i32,
40 nanosec: nanosec % BILLION as u32,
41 }
42}
43
44pub(crate) fn sub_duration_from_time(t: &Time, d: &Duration) -> Time {
45 let mut sec = t.sec - d.sec;
46 let mut nanosec = t.nanosec as i64 - d.nanosec as i64;
47
48 if nanosec.is_negative() {
49 sec -= 1;
50 nanosec += BILLION;
51 }
52
53 Time {
54 sec,
55 nanosec: nanosec as u32,
56 }
57}
58
59pub(crate) fn time_as_ns_i64(t: &Time) -> i64 {
60 t.sec as i64 * BILLION + t.nanosec as i64
61}
62
63pub(crate) fn is_time_in_range_eq(target: &Time, min: &Time, max: &Time) -> bool {
64 let target_i64 = target.sec as i64 * BILLION + target.nanosec as i64;
65 let min_i64 = min.sec as i64 * BILLION + min.nanosec as i64;
66 let max_i64 = max.sec as i64 * BILLION + max.nanosec as i64;
67
68 target_i64 >= min_i64 && target_i64 <= max_i64
69}
70
71pub(crate) fn is_time_later(target: &Time, past: &Time) -> bool {
72 let target_i64 = target.sec as i64 * BILLION + target.nanosec as i64;
73 let past_i64 = past.sec as i64 * BILLION + past.nanosec as i64;
74
75 target_i64 > past_i64
76}
77
78#[cfg(test)]
79mod test {
80 use super::*;
81
82 #[test]
83 fn test_time_from_nanosec() {
84 const EXPECTED_TIME: Time = Time {
85 sec: 1,
86 nanosec: 234_567_890,
87 };
88 let nanosec = 1_234_567_890;
89 let time = time_from_nanosec(nanosec);
90
91 assert_eq!(time.sec, EXPECTED_TIME.sec);
92 assert_eq!(time.nanosec, EXPECTED_TIME.nanosec);
93 }
94
95 #[test]
96 fn test_sub_time_and_time() {
97 const EXPECTED_DIF1: Time = Time {
98 sec: 0,
99 nanosec: 111_111_011,
100 };
101 const EXPECTED_DIF2: Time = Time {
102 sec: 0,
103 nanosec: 888_889_889,
104 };
105 const EXPECTED_DIF3: Time = Time {
106 sec: 20,
107 nanosec: 691_357_802,
108 };
109
110 let (t1, t2, t3, t4, _, _) = times_and_durations_for_test();
111
112 let dif1 = sub_time_and_time(&t2, &t1);
113 let dif2 = sub_time_and_time(&t2, &t3);
114 let dif3 = sub_time_and_time(&t2, &t4);
115
116 assert_eq!(dif1.sec, EXPECTED_DIF1.sec);
117 assert_eq!(dif1.nanosec, EXPECTED_DIF1.nanosec);
118 assert_eq!(dif2.sec, EXPECTED_DIF2.sec);
119 assert_eq!(dif2.nanosec, EXPECTED_DIF2.nanosec);
120 assert_eq!(dif3.sec, EXPECTED_DIF3.sec);
121 assert_eq!(dif3.nanosec, EXPECTED_DIF3.nanosec);
122 }
123
124 #[test]
125 fn test_add_time_and_duration() {
126 const EXPECTED_SUM1: Time = Time {
127 sec: 11,
128 nanosec: 334_567_890,
129 };
130 const EXPECTED_SUM2: Time = Time {
131 sec: 12,
132 nanosec: 234_567_889,
133 };
134
135 let (t1, _, _, _, d1, d2) = times_and_durations_for_test();
136
137 let sum1 = add_time_and_duration(&t1, &d1);
138 let sum2 = add_time_and_duration(&t1, &d2);
139
140 assert_eq!(sum1.sec, EXPECTED_SUM1.sec);
141 assert_eq!(sum1.nanosec, EXPECTED_SUM1.nanosec);
142 assert_eq!(sum2.sec, EXPECTED_SUM2.sec);
143 assert_eq!(sum2.nanosec, EXPECTED_SUM2.nanosec);
144 }
145
146 #[test]
147 fn test_sub_duration_from_time() {
148 const EXPECTED_DIF1: Time = Time {
149 sec: 9,
150 nanosec: 134_567_890,
151 };
152 const EXPECTED_DIF2: Time = Time {
153 sec: 8,
154 nanosec: 234_567_891,
155 };
156
157 let (t1, _, _, _, d1, d2) = times_and_durations_for_test();
158
159 let dif1 = sub_duration_from_time(&t1, &d1);
160 let dif2 = sub_duration_from_time(&t1, &d2);
161
162 assert_eq!(dif1.sec, EXPECTED_DIF1.sec);
163 assert_eq!(dif1.nanosec, EXPECTED_DIF1.nanosec);
164 assert_eq!(dif2.sec, EXPECTED_DIF2.sec);
165 assert_eq!(dif2.nanosec, EXPECTED_DIF2.nanosec);
166 }
167
168 #[test]
169 fn test_time_as_ns_i64() {
170 const EXPECTED: i64 = 10_234_567_890;
171 let (t1, _, _, _, _, _) = times_and_durations_for_test();
172
173 let time_as_nanosec = time_as_ns_i64(&t1);
174
175 assert_eq!(time_as_nanosec, EXPECTED);
176 }
177
178 #[test]
179 fn test_is_time_in_range_eq() {
180 let (t1, t2, t3, _, _, _) = times_and_durations_for_test();
181
182 assert!(is_time_in_range_eq(&t1, &t3, &t2));
183 assert!(!is_time_in_range_eq(&t2, &t3, &t1));
184 }
185
186 fn times_and_durations_for_test() -> (Time, Time, Time, Time, Duration, Duration) {
187 let time1 = Time {
188 sec: 10,
189 nanosec: 234_567_890,
190 };
191 let time2 = Time {
192 sec: 10,
193 nanosec: 345_678_901,
194 };
195 let time3 = Time {
196 sec: 9,
197 nanosec: 456_789_012,
198 };
199 let time4 = Time {
200 sec: -10,
201 nanosec: 345_678_901,
202 };
203 let duration1 = Duration {
204 sec: 1,
205 nanosec: 100_000_000,
206 };
207 let duration2 = Duration {
208 sec: 1,
209 nanosec: 999_999_999,
210 };
211
212 (time1, time2, time3, time4, duration1, duration2)
213 }
214}