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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139

use crate::{error::AUTDInternalError, geometry::Vector3};

use super::*;

#[bitfield_struct::bitfield(u64)]
pub struct STMFocus {
    #[bits(18)]
    pub x: i32,
    #[bits(18)]
    pub y: i32,
    #[bits(18)]
    pub z: i32,
    #[bits(8)]
    pub intensity: u8,
    #[bits(2)]
    __: u8,
}

impl STMFocus {
    fn to_fixed_num(x: f32) -> i32 {
        (x / FOCI_STM_FIXED_NUM_UNIT).round() as i32
    }

    pub fn create(p: &Vector3, intensity_or_offset: u8) -> Result<Self, AUTDInternalError> {
        let ix = Self::to_fixed_num(p.x);
        let iy = Self::to_fixed_num(p.y);
        let iz = Self::to_fixed_num(p.z);

        if !(FOCI_STM_FIXED_NUM_LOWER_X..=FOCI_STM_FIXED_NUM_UPPER_X).contains(&ix)
            || !(FOCI_STM_FIXED_NUM_LOWER_Y..=FOCI_STM_FIXED_NUM_UPPER_Y).contains(&iy)
            || !(FOCI_STM_FIXED_NUM_LOWER_Z..=FOCI_STM_FIXED_NUM_UPPER_Z).contains(&iz)
        {
            return Err(AUTDInternalError::FociSTMPointOutOfRange(p.x, p.y, p.z));
        }

        let mut f = Self::new();
        f.set_x(ix);
        f.set_y(iy);
        f.set_z(iz);
        f.set_intensity(intensity_or_offset);
        Ok(f)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    #[cfg_attr(miri, ignore)]
    fn test_size() {
        assert_eq!(8, std::mem::size_of::<STMFocus>());
    }

    #[test]
    #[cfg_attr(miri, ignore)]
    fn test_bitfield() {
        let mut d: u64 = 0;
        unsafe {
            (*(&mut d as *mut _ as *mut STMFocus))
                .set_x(0b11111111111111_111111111111111111u32 as i32);
            assert_eq!(0b111111111111111111, d);
            (*(&mut d as *mut _ as *mut STMFocus)).set_y(0b010101010101010101);
            assert_eq!(0b010101010101010101_111111111111111111, d);
            (*(&mut d as *mut _ as *mut STMFocus))
                .set_z(0b11111111111111_101010101010101010u32 as i32);
            assert_eq!(
                0b101010101010101010_010101010101010101_111111111111111111,
                d
            );
            (*(&mut d as *mut _ as *mut STMFocus)).set_intensity(0xFF);
            assert_eq!(
                0b11111111_101010101010101010_010101010101010101_111111111111111111,
                d
            );
        }
    }

    #[test]
    #[cfg_attr(miri, ignore)]
    fn test_to_fixed_num() {
        for i in FOCI_STM_FIXED_NUM_LOWER_Z..=FOCI_STM_FIXED_NUM_UPPER_Z {
            assert_eq!(
                i,
                STMFocus::to_fixed_num(i as f32 * FOCI_STM_FIXED_NUM_UNIT)
            );
        }
    }

    #[rstest::rstest]
    #[test]
    #[case(true, 1, 2, 3, 0x04)]
    #[case(true, -1, -2, -3, 0xFF)]
    #[case(
        true,
        FOCI_STM_FIXED_NUM_UPPER_X,
        FOCI_STM_FIXED_NUM_UPPER_Y,
        FOCI_STM_FIXED_NUM_UPPER_Z,
        0x01
    )]
    #[case(
        true,
        FOCI_STM_FIXED_NUM_LOWER_X,
        FOCI_STM_FIXED_NUM_LOWER_Y,
        FOCI_STM_FIXED_NUM_LOWER_Z,
        0x02
    )]
    #[case(false, FOCI_STM_FIXED_NUM_UPPER_X+1, FOCI_STM_FIXED_NUM_UPPER_Y, FOCI_STM_FIXED_NUM_UPPER_Z, 0x03)]
    #[case(false, FOCI_STM_FIXED_NUM_LOWER_X-1, FOCI_STM_FIXED_NUM_LOWER_Y, FOCI_STM_FIXED_NUM_LOWER_Z, 0x04)]
    #[case(false, FOCI_STM_FIXED_NUM_UPPER_X, FOCI_STM_FIXED_NUM_UPPER_Y+1, FOCI_STM_FIXED_NUM_UPPER_Z, 0x05)]
    #[case(false, FOCI_STM_FIXED_NUM_LOWER_X, FOCI_STM_FIXED_NUM_LOWER_Y-1, FOCI_STM_FIXED_NUM_LOWER_Z, 0x06)]
    #[case(false, FOCI_STM_FIXED_NUM_UPPER_X, FOCI_STM_FIXED_NUM_UPPER_Y, FOCI_STM_FIXED_NUM_UPPER_Z+1, 0x07)]
    #[case(false, FOCI_STM_FIXED_NUM_LOWER_X, FOCI_STM_FIXED_NUM_LOWER_Y, FOCI_STM_FIXED_NUM_LOWER_Z-1, 0x08)]
    #[cfg_attr(miri, ignore)]
    fn test_stm_focus(
        #[case] expect: bool,
        #[case] x: i32,
        #[case] y: i32,
        #[case] z: i32,
        #[case] intensity: u8,
    ) {
        let p = STMFocus::create(
            &Vector3::new(
                x as f32 * FOCI_STM_FIXED_NUM_UNIT,
                y as f32 * FOCI_STM_FIXED_NUM_UNIT,
                z as f32 * FOCI_STM_FIXED_NUM_UNIT,
            ),
            intensity,
        );
        assert_eq!(expect, p.is_ok());
        if expect {
            let p = p.unwrap();
            assert_eq!({ x }, p.x());
            assert_eq!({ y }, p.y());
            assert_eq!({ z }, p.z());
            assert_eq!(intensity, p.intensity());
        }
    }
}