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
/*
 * File: device.rs
 * Project: geometry
 * Created Date: 04/05/2022
 * Author: Shun Suzuki
 * -----
 * Last Modified: 31/05/2022
 * Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
 * -----
 * Copyright (c) 2022 Shun Suzuki. All rights reserved.
 *
 */

use autd3_driver::{
    is_missing_transducer, NUM_TRANS_IN_UNIT, NUM_TRANS_X, NUM_TRANS_Y, TRANS_SPACING_MM,
};

use super::{Matrix3, Matrix4, Quaternion, Transducer, UnitQuaternion, Vector3, Vector4};

pub struct Device<T: Transducer> {
    transducers: Vec<T>,
    origin: Vector3,
    trans_inv: Matrix3,
}

impl<T: Transducer> Device<T> {
    fn get_direction(dir: Vector3, rotation: UnitQuaternion) -> Vector3 {
        let dir: UnitQuaternion = UnitQuaternion::from_quaternion(Quaternion::from_imag(dir));
        (rotation * dir * rotation.conjugate()).imag().normalize()
    }

    pub fn local_position(&self, global_position: &Vector3) -> Vector3 {
        self.trans_inv * (global_position - self.origin)
    }

    pub fn transducers(&self) -> &[T] {
        &self.transducers
    }

    pub fn transducers_mut(&mut self) -> &mut [T] {
        &mut self.transducers
    }

    pub fn center(&self) -> Vector3 {
        let sum: Vector3 = self.transducers().iter().map(|t| t.position()).sum();
        sum / self.transducers.len() as f64
    }
}

impl<T: Transducer> Device<T> {
    pub fn new(id: usize, position: Vector3, rotation: UnitQuaternion) -> Self {
        let rot_mat: Matrix4 = From::from(rotation);
        let trans_mat = rot_mat.append_translation(&position);
        let x_direction = Self::get_direction(Vector3::x(), rotation);
        let y_direction = Self::get_direction(Vector3::y(), rotation);
        let z_direction = Self::get_direction(Vector3::z(), rotation);

        let transducers: Vec<T> = itertools::iproduct!((0..NUM_TRANS_Y), (0..NUM_TRANS_X))
            .filter(|&(y, x)| !is_missing_transducer(x, y))
            .map(|(y, x)| {
                Vector4::new(
                    x as f64 * TRANS_SPACING_MM,
                    y as f64 * TRANS_SPACING_MM,
                    0.,
                    1.,
                )
            })
            .map(|p| trans_mat * p)
            .zip(id * NUM_TRANS_IN_UNIT..)
            .map(|(p, i)| {
                T::new(
                    i,
                    Vector3::new(p.x, p.y, p.z),
                    x_direction,
                    y_direction,
                    z_direction,
                )
            })
            .collect();

        let origin = *transducers[0].position();
        let trans_inv = Matrix3::from_columns(&[x_direction, y_direction, z_direction]).transpose();

        Self {
            transducers,
            origin,
            trans_inv,
        }
    }
}