use core::ops::Deref;
use super::{DELTA_LENGTH, YawControl};
use crate::parsing::{read_i16, read_u16};
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct YawSetpoint {
pub start_time_sec: f32,
pub start_time_msec: u32,
pub duration_sec: f32,
pub duration_msec: u32,
pub end_time_sec: f32,
pub end_time_msec: u32,
pub start_yaw_ddeg: i32,
pub start_yaw_deg: f32,
pub yaw_change_ddeg: i16,
pub yaw_change_deg: f32,
pub end_yaw_ddeg: i32,
pub end_yaw_deg: f32,
}
#[derive(Clone, Debug)]
pub struct YawPlayer<T> {
ctrl: T,
start: usize,
length: usize,
setpoint: YawSetpoint,
}
impl<'a, T: Deref<Target = YawControl<'a>>> YawPlayer<T> {
pub fn new(ctrl: T) -> Self {
let mut player = Self {
ctrl,
start: 0,
length: 0,
setpoint: YawSetpoint::default(),
};
player.rewind();
player
}
pub fn yaw_control(&self) -> &YawControl<'a> {
&self.ctrl
}
pub const fn current_setpoint(&self) -> &YawSetpoint {
&self.setpoint
}
pub const fn has_more_setpoints(&self) -> bool {
self.length > 0
}
pub fn build_next_setpoint(&mut self) {
self.build_setpoint(
self.start + self.length,
self.setpoint.end_time_msec,
self.setpoint.end_yaw_ddeg,
);
}
pub fn yaw_at(&mut self, time_sec: f32) -> f32 {
let rel_time = self.seek_to_time(time_sec);
self.setpoint.start_yaw_deg + self.setpoint.yaw_change_deg * rel_time
}
pub fn yaw_rate_at(&mut self, time_sec: f32) -> f32 {
self.seek_to_time(time_sec);
if self.setpoint.duration_sec == 0.0 {
f32::INFINITY
} else {
self.setpoint.yaw_change_deg / self.setpoint.duration_sec
}
}
pub fn total_duration_msec(&mut self) -> u32 {
self.rewind();
let mut result: u32 = 0;
while self.has_more_setpoints() {
result = result.wrapping_add(self.setpoint.duration_msec);
self.build_next_setpoint();
}
result
}
fn rewind(&mut self) {
let start = self.ctrl.header_length;
let yaw_offset_ddeg = i32::from(self.ctrl.yaw_offset_ddeg);
self.build_setpoint(start, 0, yaw_offset_ddeg);
}
fn build_setpoint(&mut self, offset: usize, start_time_msec: u32, start_yaw_ddeg: i32) {
self.start = offset;
self.length = 0;
let mut setpoint = YawSetpoint {
start_time_msec,
start_time_sec: msec_to_sec(start_time_msec),
start_yaw_ddeg,
start_yaw_deg: ddeg_to_deg(start_yaw_ddeg),
..YawSetpoint::default()
};
let bytes = self.ctrl.bytes();
let delta = (
read_u16(bytes, offset),
read_i16(bytes, offset + DELTA_LENGTH / 2),
);
if let (Some(duration_msec), Some(yaw_change_ddeg)) = delta {
setpoint.duration_msec = u32::from(duration_msec);
setpoint.duration_sec = msec_to_sec(setpoint.duration_msec);
setpoint.end_time_msec = start_time_msec.wrapping_add(setpoint.duration_msec);
setpoint.end_time_sec = msec_to_sec(setpoint.end_time_msec);
setpoint.yaw_change_ddeg = yaw_change_ddeg;
setpoint.yaw_change_deg = f32::from(yaw_change_ddeg) / 10.0;
setpoint.end_yaw_ddeg = start_yaw_ddeg.wrapping_add(i32::from(yaw_change_ddeg));
setpoint.end_yaw_deg = ddeg_to_deg(setpoint.end_yaw_ddeg);
self.length = DELTA_LENGTH;
} else {
setpoint.duration_msec = u32::MAX - start_time_msec;
setpoint.duration_sec = f32::INFINITY;
setpoint.end_time_msec = u32::MAX;
setpoint.end_time_sec = f32::INFINITY;
setpoint.end_yaw_ddeg = start_yaw_ddeg;
setpoint.end_yaw_deg = setpoint.start_yaw_deg;
}
self.setpoint = setpoint;
}
fn seek_to_time(&mut self, time_sec: f32) -> f32 {
let time_sec = if time_sec <= 0.0 { 0.0 } else { time_sec };
loop {
if self.setpoint.start_time_sec > time_sec {
self.rewind();
} else if self.setpoint.end_time_sec < time_sec {
self.build_next_setpoint();
} else if !time_sec.is_finite() {
return 1.0;
} else if self.setpoint.duration_sec.abs() > 1.0e-6 {
return (time_sec - self.setpoint.start_time_sec) / self.setpoint.duration_sec;
} else {
return 0.5;
}
}
}
}
#[expect(
clippy::cast_precision_loss,
reason = "times are floats in seconds, as in the C library"
)]
fn msec_to_sec(msec: u32) -> f32 {
msec as f32 / 1000.0
}
#[expect(
clippy::cast_precision_loss,
reason = "yaws are floats in degrees, as in the C library"
)]
fn ddeg_to_deg(ddeg: i32) -> f32 {
ddeg as f32 / 10.0
}