1use core::f32::consts::PI;
2
3use nalgebra::Complex;
4use zerocopy::{FromBytes, Immutable, IntoBytes};
5
6use crate::common::{Angle, units::rad};
7
8#[repr(C)]
9#[derive(Clone, Copy, PartialEq, Eq, Default, FromBytes, IntoBytes, Immutable)]
10pub struct Phase(pub u8);
11
12impl core::fmt::Debug for Phase {
13 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
14 write!(f, "0x{:02X}", self.0)
15 }
16}
17
18impl Phase {
19 pub const ZERO: Self = Self(0);
20 pub const PI: Self = Self(0x80);
21
22 #[must_use]
23 pub const fn rad(&self) -> f32 {
24 self.0 as f32 / 256.0 * 2.0 * PI
25 }
26}
27
28const RAD_PER_LSB: f32 = 2.0 * PI / 256.0;
29const LSB_LIMIT: f32 = 2_147_483_648.0;
30const ROUND_TO_MULTIPLE_OF_512: f32 = 6_442_450_944.0;
31const ROUND_TO_INTEGER: f32 = 12_582_912.0;
32
33impl From<Angle> for Phase {
34 #[inline]
35 fn from(v: Angle) -> Self {
36 #[allow(clippy::manual_clamp)]
37 let lsb = (v.rad() / RAD_PER_LSB).max(-LSB_LIMIT).min(LSB_LIMIT);
38 let turns = (lsb + ROUND_TO_MULTIPLE_OF_512) - ROUND_TO_MULTIPLE_OF_512;
39 let within_turns = lsb - turns;
40 let rounded = within_turns + ROUND_TO_INTEGER;
41 #[allow(clippy::float_cmp)]
42 let away_from_zero = {
43 let frac = within_turns - (rounded - ROUND_TO_INTEGER);
44 i32::from(frac == 0.5 && lsb > 0.0) - i32::from(frac == -0.5 && lsb < 0.0)
45 };
46 #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
47 Self(rounded.to_bits().wrapping_add(away_from_zero as u32) as u8)
48 }
49}
50
51impl From<Complex<f32>> for Phase {
52 fn from(v: Complex<f32>) -> Self {
53 Self::from(v.arg() * rad)
54 }
55}
56
57impl core::ops::Add<Phase> for Phase {
58 type Output = Phase;
59 fn add(self, rhs: Phase) -> Self::Output {
60 Phase(self.0.wrapping_add(rhs.0))
61 }
62}
63
64impl core::ops::AddAssign for Phase {
65 fn add_assign(&mut self, rhs: Phase) {
66 self.0 = self.0.wrapping_add(rhs.0);
67 }
68}
69
70impl core::ops::Sub<Phase> for Phase {
71 type Output = Phase;
72 fn sub(self, rhs: Phase) -> Self::Output {
73 Phase(self.0.wrapping_sub(rhs.0))
74 }
75}
76
77impl core::ops::SubAssign for Phase {
78 fn sub_assign(&mut self, rhs: Phase) {
79 self.0 = self.0.wrapping_sub(rhs.0);
80 }
81}
82
83impl core::ops::Mul<u8> for Phase {
84 type Output = Phase;
85 fn mul(self, rhs: u8) -> Self::Output {
86 Phase(self.0.wrapping_mul(rhs))
87 }
88}
89
90impl core::ops::Mul<Phase> for u8 {
91 type Output = Phase;
92 fn mul(self, rhs: Phase) -> Self::Output {
93 Phase(self.wrapping_mul(rhs.0))
94 }
95}
96
97impl core::ops::Div<u8> for Phase {
98 type Output = Phase;
99 fn div(self, rhs: u8) -> Self::Output {
100 Phase(self.0.wrapping_div(rhs))
101 }
102}
103
104#[cfg(test)]
105mod tests {
106 use super::*;
107
108 #[test]
109 fn add_wraps() {
110 for (expected, lhs, rhs) in [
111 (Phase(0x02), Phase(0x01), Phase(0x01)),
112 (Phase(0xFE), Phase(0x7F), Phase(0x7F)),
113 (Phase(0x7E), Phase(0x7F), Phase(0xFF)),
114 ] {
115 assert_eq!(expected, lhs + rhs);
116 let mut a = lhs;
117 a += rhs;
118 assert_eq!(expected, a);
119 }
120 }
121
122 #[test]
123 fn sub_wraps() {
124 for (expected, lhs, rhs) in [
125 (Phase::ZERO, Phase(0x01), Phase(0x01)),
126 (Phase(0x01), Phase(0x02), Phase(0x01)),
127 (Phase(0x80), Phase(0x7F), Phase(0xFF)),
128 ] {
129 assert_eq!(expected, lhs - rhs);
130 let mut a = lhs;
131 a -= rhs;
132 assert_eq!(expected, a);
133 }
134 }
135
136 #[test]
137 fn mul_wraps() {
138 for (expected, lhs, rhs) in [
139 (Phase(0x02), Phase(0x01), 2),
140 (Phase(0xFE), Phase(0x7F), 2),
141 (Phase::ZERO, Phase(0x80), 2),
142 ] {
143 assert_eq!(expected, lhs * rhs);
144 assert_eq!(expected, rhs * lhs);
145 }
146 }
147
148 #[test]
149 fn div() {
150 for (expected, lhs, rhs) in [(Phase(0x01), Phase(0x02), 2), (Phase(0x7F), Phase(0xFE), 2)] {
151 assert_eq!(expected, lhs / rhs);
152 }
153 }
154
155 #[test]
156 fn rad() {
157 for (expect, value) in [
158 (0.0, 0u8),
159 (2.0 * PI / 256.0 * 128.0, 128),
160 (2.0 * PI / 256.0 * 255.0, 255),
161 ] {
162 approx::assert_abs_diff_eq!(expect, Phase(value).rad());
163 }
164 }
165
166 fn quantized_exactly(v: Angle) -> Phase {
167 let lsb = f64::from(v.rad() / RAD_PER_LSB);
168 if !lsb.is_finite() || lsb.abs() >= f64::from(LSB_LIMIT) {
169 return Phase::ZERO;
170 }
171 let truncated = lsb.trunc();
172 let frac = lsb - truncated;
173 let rounded = if frac >= 0.5 {
174 truncated + 1.0
175 } else if frac <= -0.5 {
176 truncated - 1.0
177 } else {
178 truncated
179 };
180 #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
181 Phase(((rounded as i64) & 0xFF) as u8)
182 }
183
184 fn quantized_by_saturating_round(v: Angle) -> Phase {
185 let p = (v.rad() / (2.0 * PI) * 256.0).round();
186 #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
187 Phase(((p as i32) & 0xFF) as u8)
188 }
189
190 #[test]
191 fn from_angle() {
192 for (expect, value) in [
193 (Phase(0x00), 0.0),
194 (Phase(0x40), PI / 2.0),
195 (Phase(0x80), PI),
196 (Phase(0xC0), -PI / 2.0),
197 (Phase(0x00), 2.0 * PI),
198 (Phase(0x01), 2.0 * PI / 256.0),
199 (Phase(0xFF), -2.0 * PI / 256.0),
200 ] {
201 assert_eq!(expect, Phase::from(Angle::from_rad(value)));
202 }
203 }
204
205 #[test]
206 fn from_angle_matches_exact_at_edges() {
207 for value in [
208 f32::NAN,
209 -f32::NAN,
210 f32::INFINITY,
211 f32::NEG_INFINITY,
212 f32::MAX,
213 f32::MIN,
214 f32::MIN_POSITIVE,
215 -f32::MIN_POSITIVE,
216 f32::from_bits(1),
217 f32::from_bits(0x8000_0001),
218 0.0,
219 -0.0,
220 RAD_PER_LSB / 2.0,
221 -RAD_PER_LSB / 2.0,
222 RAD_PER_LSB * 1.5,
223 -RAD_PER_LSB * 1.5,
224 RAD_PER_LSB * 2.5,
225 -RAD_PER_LSB * 2.5,
226 RAD_PER_LSB * 255.5,
227 RAD_PER_LSB * 256.5,
228 RAD_PER_LSB * 511.5,
229 RAD_PER_LSB * 512.5,
230 RAD_PER_LSB * LSB_LIMIT,
231 -RAD_PER_LSB * LSB_LIMIT,
232 ] {
233 let v = Angle::from_rad(value);
234 assert_eq!(
235 quantized_exactly(v),
236 Phase::from(v),
237 "{value:e} (0x{:08X})",
238 value.to_bits()
239 );
240 }
241 }
242
243 #[test]
244 fn from_angle_wraps_beyond_the_saturating_boundary() {
245 let boundary = RAD_PER_LSB * LSB_LIMIT;
246 assert_eq!(Phase::ZERO, Phase::from(Angle::from_rad(boundary)));
247 assert_eq!(
248 Phase(0xFF),
249 quantized_by_saturating_round(Angle::from_rad(boundary))
250 );
251 assert_eq!(Phase::ZERO, Phase::from(Angle::from_rad(f32::INFINITY)));
252 assert_eq!(Phase::ZERO, Phase::from(Angle::from_rad(f32::NEG_INFINITY)));
253 assert_eq!(Phase::ZERO, Phase::from(Angle::from_rad(f32::NAN)));
254 }
255
256 #[test]
257 #[ignore = "sweeps all 2^32 f32 inputs"]
258 fn from_angle_matches_exact_for_every_f32() {
259 let mut bits = 0u32;
260 loop {
261 let value = f32::from_bits(bits);
262 let v = Angle::from_rad(value);
263 assert_eq!(
264 quantized_exactly(v),
265 Phase::from(v),
266 "{value:e} (0x{bits:08X})"
267 );
268 if bits == u32::MAX {
269 break;
270 }
271 bits += 1;
272 }
273 }
274
275 #[test]
276 #[ignore = "sweeps all 2^32 f32 inputs"]
277 fn from_angle_matches_saturating_round_below_the_boundary() {
278 let mut bits = 0u32;
279 loop {
280 let value = f32::from_bits(bits);
281 if value.abs() < RAD_PER_LSB * LSB_LIMIT {
282 let v = Angle::from_rad(value);
283 assert_eq!(
284 quantized_by_saturating_round(v),
285 Phase::from(v),
286 "{value:e} (0x{bits:08X})"
287 );
288 }
289 if bits == u32::MAX {
290 break;
291 }
292 bits += 1;
293 }
294 }
295
296 #[test]
297 fn from_complex() {
298 for (expect, value) in [
299 (Phase(0x00), Complex::new(1.0, 0.0)),
300 (Phase(0x40), Complex::new(0.0, 1.0)),
301 (Phase(0x80), Complex::new(-1.0, 0.0)),
302 (Phase(0xC0), Complex::new(0.0, -1.0)),
303 ] {
304 assert_eq!(expect, Phase::from(value));
305 }
306 }
307
308 #[test]
309 fn dbg() {
310 assert_eq!(format!("{:?}", Phase::ZERO), "0x00");
311 assert_eq!(format!("{:?}", Phase(0xFF)), "0xFF");
312 }
313}