use crate::{
fpga::{AdvancedDriveDuty, AdvancedDrivePhase, LegacyDrive},
Drive, FOCUS_STM_FIXED_NUM_UNIT,
};
#[repr(C)]
pub struct STMFocus {
pub(crate) buf: [u16; 4],
}
impl STMFocus {
pub fn new(x: f64, y: f64, z: f64, duty_shift: u8) -> Self {
let x = (x / FOCUS_STM_FIXED_NUM_UNIT).round() as i32;
let y = (y / FOCUS_STM_FIXED_NUM_UNIT).round() as i32;
let z = (z / FOCUS_STM_FIXED_NUM_UNIT).round() as i32;
let d0 = (x & 0xFFFF) as u16;
let d1 =
((y << 2) & 0xFFFC) as u16 | ((x >> 30) & 0x0002) as u16 | ((x >> 16) & 0x0001) as u16;
let d2 =
((z << 4) & 0xFFF0) as u16 | ((y >> 28) & 0x0008) as u16 | ((y >> 14) & 0x0007) as u16;
let d3 = (((duty_shift as u16) << 6) & 0x3FC0)
| ((z >> 26) & 0x0020) as u16
| ((z >> 12) & 0x001F) as u16;
Self {
buf: [d0, d1, d2, d3],
}
}
}
pub const FOCUS_STM_BODY_INITIAL_SIZE: usize = 14;
#[repr(C)]
pub struct FocusSTMBodyInitial<T: ?Sized> {
data: T,
}
impl FocusSTMBodyInitial<[u16]> {
pub fn set_size(&mut self, size: u16) {
self.data[0] = size;
}
pub fn set_freq_div(&mut self, freq_div: u32) {
self.data[1] = (freq_div & 0x0000FFFF) as _;
self.data[2] = ((freq_div >> 16) & 0x0000FFFF) as _;
}
pub fn set_sound_speed(&mut self, sound_speed: u32) {
self.data[3] = (sound_speed & 0x0000FFFF) as _;
self.data[4] = ((sound_speed >> 16) & 0x0000FFFF) as _;
}
pub fn set_start_idx(&mut self, idx: u16) {
self.data[5] = idx;
}
pub fn set_finish_idx(&mut self, idx: u16) {
self.data[6] = idx;
}
pub fn set_points(&mut self, points: &[STMFocus]) {
self.data[7..]
.chunks_mut(std::mem::size_of::<STMFocus>() / std::mem::size_of::<u16>())
.zip(points.iter())
.for_each(|(d, s)| d.copy_from_slice(&s.buf));
}
}
pub const FOCUS_STM_BODY_SUBSEQUENT_SIZE: usize = 2;
#[repr(C)]
pub struct FocusSTMBodySubsequent<T: ?Sized> {
data: T,
}
impl FocusSTMBodySubsequent<[u16]> {
pub fn data(&self) -> &[u16] {
&self.data
}
pub fn set_size(&mut self, size: u16) {
self.data[0] = size;
}
pub fn set_points(&mut self, points: &[STMFocus]) {
self.data[1..]
.chunks_mut(std::mem::size_of::<STMFocus>() / std::mem::size_of::<u16>())
.zip(points.iter())
.for_each(|(d, s)| d.copy_from_slice(&s.buf));
}
}
#[repr(u16)]
#[derive(Clone, Copy, PartialEq, Eq, Default)]
pub enum Mode {
#[default]
PhaseDutyFull = 0x0001,
PhaseFull = 0x0002,
PhaseHalf = 0x0004,
}
#[repr(C)]
pub struct GainSTMBodyInitial<T: ?Sized> {
data: T,
}
impl GainSTMBodyInitial<[u16]> {
pub fn data(&self) -> &[u16] {
&self.data
}
pub fn set_freq_div(&mut self, freq_div: u32) {
self.data[0] = (freq_div & 0x0000FFFF) as _;
self.data[1] = ((freq_div >> 16) & 0x0000FFFF) as _;
}
pub fn set_mode(&mut self, mode: Mode) {
self.data[2] = mode as u16;
}
pub fn set_cycle(&mut self, cycle: usize) {
self.data[3] = cycle as u16;
}
pub fn set_start_idx(&mut self, idx: u16) {
self.data[4] = idx;
}
pub fn set_finish_idx(&mut self, idx: u16) {
self.data[5] = idx;
}
}
#[repr(C)]
pub struct GainSTMBodySubsequent<T: ?Sized> {
data: T,
}
#[repr(C)]
pub struct LegacyPhaseFull<const N: usize> {
phase_0: u8,
phase_1: u8,
}
impl LegacyPhaseFull<0> {
pub fn set(&mut self, d: &Drive) {
self.phase_0 = LegacyDrive::to_phase(d);
}
}
impl LegacyPhaseFull<1> {
pub fn set(&mut self, d: &Drive) {
self.phase_1 = LegacyDrive::to_phase(d);
}
}
#[repr(C)]
pub struct LegacyPhaseHalf<const N: usize> {
phase_01: u8,
phase_23: u8,
}
impl LegacyPhaseHalf<0> {
pub fn set(&mut self, d: &Drive) {
let phase = LegacyDrive::to_phase(d);
self.phase_01 = (self.phase_01 & 0xF0) | ((phase >> 4) & 0x0F);
}
}
impl LegacyPhaseHalf<1> {
pub fn set(&mut self, d: &Drive) {
let phase = LegacyDrive::to_phase(d);
self.phase_01 = (self.phase_01 & 0x0F) | (phase & 0xF0);
}
}
impl LegacyPhaseHalf<2> {
pub fn set(&mut self, d: &Drive) {
let phase = LegacyDrive::to_phase(d);
self.phase_23 = (self.phase_23 & 0xF0) | ((phase >> 4) & 0x0F);
}
}
impl LegacyPhaseHalf<3> {
pub fn set(&mut self, d: &Drive) {
let phase = LegacyDrive::to_phase(d);
self.phase_23 = (self.phase_23 & 0x0F) | (phase & 0xF0);
}
}
impl GainSTMBodySubsequent<[u16]> {
pub fn data(&self) -> &[u16] {
&self.data
}
pub fn legacy_drives_mut(&mut self) -> &mut [LegacyDrive] {
unsafe { std::mem::transmute(&mut self.data) }
}
pub fn phases_mut(&mut self) -> &mut [AdvancedDrivePhase] {
unsafe { std::mem::transmute(&mut self.data) }
}
pub fn duties_mut(&mut self) -> &mut [AdvancedDriveDuty] {
unsafe { std::mem::transmute(&mut self.data) }
}
pub fn legacy_phase_full_mut<const N: usize>(&mut self) -> &mut [LegacyPhaseFull<N>] {
unsafe { std::mem::transmute(&mut self.data) }
}
pub fn legacy_phase_half_mut<const N: usize>(&mut self) -> &mut [LegacyPhaseHalf<N>] {
unsafe { std::mem::transmute(&mut self.data) }
}
}
#[repr(C)]
pub struct Body<T: ?Sized> {
data: T,
}
impl Body<[u16]> {
pub fn data(&self) -> &[u16] {
&self.data
}
pub fn focus_stm_initial(&self) -> &FocusSTMBodyInitial<[u16]> {
unsafe { std::mem::transmute(self) }
}
pub fn focus_stm_initial_mut(&mut self) -> &mut FocusSTMBodyInitial<[u16]> {
unsafe { std::mem::transmute(self) }
}
pub fn focus_stm_subsequent(&self) -> &FocusSTMBodySubsequent<[u16]> {
unsafe { std::mem::transmute(self) }
}
pub fn focus_stm_subsequent_mut(&mut self) -> &mut FocusSTMBodySubsequent<[u16]> {
unsafe { std::mem::transmute(self) }
}
pub fn gain_stm_initial(&self) -> &GainSTMBodyInitial<[u16]> {
unsafe { std::mem::transmute(self) }
}
pub fn gain_stm_initial_mut(&mut self) -> &mut GainSTMBodyInitial<[u16]> {
unsafe { std::mem::transmute(self) }
}
pub fn gain_stm_subsequent(&self) -> &GainSTMBodySubsequent<[u16]> {
unsafe { std::mem::transmute(self) }
}
pub fn gain_stm_subsequent_mut(&mut self) -> &mut GainSTMBodySubsequent<[u16]> {
unsafe { std::mem::transmute(self) }
}
}