mod trajectory;
use crate::flags::impl_flags;
use alloc::borrow::Cow;
use alloc::vec::Vec;
use core::{error, fmt};
pub use trajectory::RthTrajectoryError;
use crate::basic_types::Vector2;
use crate::formats::binary::{BinaryBlockType, BinaryFile, BinaryFileError};
use crate::parsing::{VarintError, decode_varuint32, read_i16, read_u16};
const HEADER_LENGTH: usize = 10;
const POINT_LENGTH: usize = 4;
const MAX_DURATION_SEC: u32 = 16_777_216;
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)]
pub struct RthPlanEntryFlags(u8);
impl RthPlanEntryFlags {
pub const NONE: Self = Self(0);
pub const HAS_NECK: Self = Self(1 << 0);
pub const HAS_TARGET_XY: Self = Self(1 << 1);
pub const HAS_TARGET_Z: Self = Self(1 << 2);
pub const HAS_LANDING_ALTITUDE: Self = Self(1 << 3);
pub const HAS_PRE_DELAY: Self = Self(1 << 4);
pub const HAS_POST_DELAY: Self = Self(1 << 5);
pub const ALL: Self = Self(0x3f);
}
impl_flags!(RthPlanEntryFlags);
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct RthPlanEntry {
pub time_sec: f32,
pub duration_sec: f32,
pub flags: RthPlanEntryFlags,
pub landing_target: Vector2,
pub arrival_altitude: f32,
pub pre_delay_sec: f32,
pub post_delay_sec: f32,
pub pre_neck: f32,
pub pre_neck_duration_sec: f32,
pub max_acceleration: f32,
pub landing_velocity: f32,
pub landing_altitude: f32,
}
impl RthPlanEntry {
#[must_use]
pub fn land_in_place(time_sec: f32) -> Self {
Self {
time_sec,
duration_sec: 0.0,
flags: RthPlanEntryFlags::HAS_LANDING_ALTITUDE
| RthPlanEntryFlags::HAS_PRE_DELAY
| RthPlanEntryFlags::HAS_POST_DELAY
| RthPlanEntryFlags::HAS_NECK,
landing_target: Vector2 { x: 0.0, y: 0.0 },
arrival_altitude: 0.0,
pre_delay_sec: 0.0,
post_delay_sec: 0.0,
pre_neck: 0.0,
pre_neck_duration_sec: 0.0,
max_acceleration: f32::INFINITY,
landing_velocity: f32::NAN,
landing_altitude: 0.0,
}
}
#[must_use]
pub fn land_in_place_with_defaults(plan: &RthPlan<'_>, time_sec: f32) -> Self {
Self {
max_acceleration: plan.default_acceleration_limit(),
landing_velocity: plan.default_landing_velocity().unwrap_or(f32::NAN),
landing_altitude: plan.default_landing_altitude(),
..Self::land_in_place(time_sec)
}
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
#[non_exhaustive]
pub enum RthPlanError {
Corrupted,
File(BinaryFileError),
VarintOverflow,
TimeOverflow,
DurationTooLong,
MissingPoint,
}
impl fmt::Display for RthPlanError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::Corrupted => f.write_str("the RTH plan is malformed"),
Self::File(_) => f.write_str("the show file could not be read"),
Self::VarintOverflow => f.write_str("an entry holds a varint above 32 bits"),
Self::TimeOverflow => f.write_str("the time of an entry does not fit into 32 bits"),
Self::DurationTooLong => f.write_str("an entry has a duration above 2^24 seconds"),
Self::MissingPoint => f.write_str("an entry refers to a point that does not exist"),
}
}
}
impl error::Error for RthPlanError {
fn source(&self) -> Option<&(dyn error::Error + 'static)> {
match self {
Self::File(err) => Some(err),
_ => None,
}
}
}
impl From<BinaryFileError> for RthPlanError {
fn from(err: BinaryFileError) -> Self {
Self::File(err)
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct RthPlan<'a> {
bytes: Cow<'a, [u8]>,
scale: f32,
header_length: usize,
num_points: usize,
max_acceleration: f32,
landing_velocity: Option<f32>,
landing_altitude: f32,
}
impl Default for RthPlan<'static> {
fn default() -> Self {
Self::new()
}
}
impl RthPlan<'static> {
#[must_use]
pub const fn new() -> Self {
Self {
bytes: Cow::Borrowed(&[]),
scale: 1.0,
header_length: 0,
num_points: 0,
max_acceleration: f32::INFINITY,
landing_velocity: None,
landing_altitude: 0.0,
}
}
pub fn from_vec(bytes: Vec<u8>) -> Result<Self, RthPlanError> {
Self::from_cow(Cow::Owned(bytes))
}
}
impl<'a> RthPlan<'a> {
pub fn from_buffer(bytes: &'a [u8]) -> Result<Self, RthPlanError> {
Self::from_cow(Cow::Borrowed(bytes))
}
pub fn from_binary_file(file: &BinaryFile<'a>) -> Result<Option<Self>, RthPlanError> {
file.find_first_block_by_type(BinaryBlockType::RTH_PLAN)?
.map(|block| Self::from_buffer(block.body))
.transpose()
}
fn from_cow(bytes: Cow<'a, [u8]>) -> Result<Self, RthPlanError> {
validate(&bytes)?;
let scale = f32::from(bytes.get(1).map_or(0, |&scale| scale & 0x7f));
let u16_at = |offset| read_u16(&bytes, offset).unwrap_or_default();
let max_acceleration = match u16_at(2) {
0 => f32::INFINITY,
value => f32::from(value),
};
let landing_velocity = match u16_at(4) {
0 => None,
value => Some(f32::from(value)),
};
let landing_altitude = f32::from(read_i16(&bytes, 6).unwrap_or_default()) * scale;
let num_points = usize::from(u16_at(8));
Ok(Self {
bytes,
scale,
header_length: HEADER_LENGTH,
num_points,
max_acceleration,
landing_velocity,
landing_altitude,
})
}
#[must_use]
pub fn bytes(&self) -> &[u8] {
&self.bytes
}
#[must_use]
pub fn default_acceleration_limit(&self) -> f32 {
if self.max_acceleration > 0.0 && self.max_acceleration.is_finite() {
self.max_acceleration
} else {
f32::INFINITY
}
}
#[must_use]
pub const fn default_landing_altitude(&self) -> f32 {
self.landing_altitude
}
#[must_use]
pub fn default_landing_velocity(&self) -> Option<f32> {
self.landing_velocity
}
pub fn set_default_acceleration_limit(&mut self, max_acceleration: f32) {
self.max_acceleration = if max_acceleration > 0.0 && max_acceleration.is_finite() {
max_acceleration
} else {
f32::INFINITY
};
}
pub fn set_default_landing_altitude(&mut self, landing_altitude: f32) {
self.landing_altitude = if landing_altitude.is_finite() {
landing_altitude
} else {
0.0
};
}
pub fn set_default_landing_velocity(&mut self, landing_velocity: f32) {
self.landing_velocity =
(landing_velocity.is_finite() && landing_velocity > 0.0).then_some(landing_velocity);
}
pub fn clear_default_landing_velocity(&mut self) {
self.set_default_landing_velocity(f32::NAN);
}
#[must_use]
pub const fn num_points(&self) -> usize {
self.num_points
}
pub fn points(&self) -> impl Iterator<Item = Vector2> + '_ {
(0..self.num_points).map_while(|index| self.point(index))
}
#[must_use]
pub fn point(&self, index: usize) -> Option<Vector2> {
if index >= self.num_points {
return None;
}
let offset = self.offset_of_point(index)?;
Some(Vector2 {
x: self.coordinate_at(offset)?,
y: self.coordinate_at(offset + 2)?,
})
}
#[must_use]
pub fn num_entries(&self) -> usize {
self.offset_of_point(self.num_points)
.and_then(|offset| read_u16(&self.bytes, offset))
.map_or(0, usize::from)
}
#[must_use]
pub fn is_empty(&self) -> bool {
self.num_entries() == 0
}
#[expect(
clippy::cast_precision_loss,
reason = "entry times are compared and returned as floats, as in the C library"
)]
pub fn evaluate_at(&self, time_sec: f32) -> Result<RthPlanEntry, RthPlanError> {
let default = RthPlanEntry::land_in_place_with_defaults(self, time_sec);
if time_sec < 0.0 {
return Ok(default);
}
let Some(first_entry) = self
.offset_of_point(self.num_points)
.map(|offset| offset + 2)
else {
return Ok(default);
};
let mut reader = Reader {
plan: self,
offset: first_entry,
};
let mut entry = default;
let mut entry_time_sec: u32 = 0;
for _ in 0..self.num_entries() {
let flags = reader.byte()?;
entry_time_sec = entry_time_sec
.checked_add(reader.varuint()?)
.ok_or(RthPlanError::TimeOverflow)?;
entry.time_sec = entry_time_sec as f32;
if flags != 0 {
entry = reader.entry(flags, entry_time_sec as f32)?;
}
if entry.time_sec >= time_sec {
return Ok(entry);
}
}
Ok(default)
}
fn offset_of_point(&self, index: usize) -> Option<usize> {
index
.checked_mul(POINT_LENGTH)?
.checked_add(self.header_length)
}
fn coordinate_at(&self, offset: usize) -> Option<f32> {
read_i16(&self.bytes, offset).map(|value| f32::from(value) * self.scale)
}
}
struct Reader<'p, 'a> {
plan: &'p RthPlan<'a>,
offset: usize,
}
impl Reader<'_, '_> {
fn entry(&mut self, flags: u8, time_sec: f32) -> Result<RthPlanEntry, RthPlanError> {
let mut entry = RthPlanEntry::land_in_place_with_defaults(self.plan, time_sec);
let flags = RthPlanEntryFlags::from_bits_retain(flags);
if flags.contains(RthPlanEntryFlags::HAS_NECK) {
entry.pre_neck = self.coordinate()?;
entry.pre_neck_duration_sec = self.duration()?;
}
if flags.contains(RthPlanEntryFlags::HAS_TARGET_XY) {
let index = usize::try_from(self.varuint()?).map_err(|_| RthPlanError::MissingPoint)?;
entry.landing_target = self.plan.point(index).ok_or(RthPlanError::MissingPoint)?;
}
if flags.contains(RthPlanEntryFlags::HAS_TARGET_Z) {
entry.arrival_altitude = self.coordinate()?;
}
if flags.intersects(RthPlanEntryFlags::HAS_TARGET_XY | RthPlanEntryFlags::HAS_TARGET_Z) {
entry.duration_sec = self.duration()?;
}
if flags.contains(RthPlanEntryFlags::HAS_LANDING_ALTITUDE) {
entry.landing_altitude = self.coordinate()?;
}
if flags.contains(RthPlanEntryFlags::HAS_PRE_DELAY) {
entry.pre_delay_sec = self.duration()?;
}
if flags.contains(RthPlanEntryFlags::HAS_POST_DELAY) {
entry.post_delay_sec = self.duration()?;
}
entry.flags |= flags;
Ok(entry)
}
fn byte(&mut self) -> Result<u8, RthPlanError> {
let byte = *self
.plan
.bytes
.get(self.offset)
.ok_or(RthPlanError::Corrupted)?;
self.offset += 1;
Ok(byte)
}
fn coordinate(&mut self) -> Result<f32, RthPlanError> {
let value = self
.plan
.coordinate_at(self.offset)
.ok_or(RthPlanError::Corrupted)?;
self.offset += 2;
Ok(value)
}
fn varuint(&mut self) -> Result<u32, RthPlanError> {
let rest = self.plan.bytes.get(self.offset..).unwrap_or_default();
let (value, length) = decode_varuint32(rest).map_err(|err| match err {
VarintError::Truncated => RthPlanError::Corrupted,
VarintError::Overflow { .. } => RthPlanError::VarintOverflow,
})?;
self.offset += length;
Ok(value)
}
#[expect(
clippy::cast_precision_loss,
reason = "durations up to 2^24 seconds are exact floats"
)]
fn duration(&mut self) -> Result<f32, RthPlanError> {
let value = self.varuint()?;
if value > MAX_DURATION_SEC {
return Err(RthPlanError::DurationTooLong);
}
Ok(value as f32)
}
}
fn validate(bytes: &[u8]) -> Result<(), RthPlanError> {
let corrupted = RthPlanError::Corrupted;
if bytes.len() < HEADER_LENGTH {
return Err(corrupted);
}
let num_points = usize::from(read_u16(bytes, HEADER_LENGTH - 2).unwrap_or_default());
let mut rest = bytes
.get(HEADER_LENGTH + num_points * POINT_LENGTH..)
.ok_or(corrupted)?;
if rest.is_empty() {
return Ok(());
}
let (&count, after) = rest.split_first_chunk::<2>().ok_or(corrupted)?;
rest = after;
let skip_i16 = |rest: &mut &[u8]| -> Result<(), RthPlanError> {
*rest = rest.get(2..).ok_or(corrupted)?;
Ok(())
};
let skip_varuint = |rest: &mut &[u8]| -> Result<(), RthPlanError> {
let length = match decode_varuint32(rest) {
Ok((_, length)) | Err(VarintError::Overflow { len: length }) => length,
Err(VarintError::Truncated) => return Err(corrupted),
};
*rest = rest.get(length..).ok_or(corrupted)?;
Ok(())
};
for _ in 0..u16::from_le_bytes(count) {
let (&flags, after) = rest.split_first().ok_or(corrupted)?;
rest = after;
let flags = RthPlanEntryFlags::from_bits_retain(flags);
skip_varuint(&mut rest)?;
if flags.contains(RthPlanEntryFlags::HAS_NECK) {
skip_i16(&mut rest)?;
skip_varuint(&mut rest)?;
}
if flags.contains(RthPlanEntryFlags::HAS_TARGET_XY) {
skip_varuint(&mut rest)?;
}
if flags.contains(RthPlanEntryFlags::HAS_TARGET_Z) {
skip_i16(&mut rest)?;
}
if flags.intersects(RthPlanEntryFlags::HAS_TARGET_XY | RthPlanEntryFlags::HAS_TARGET_Z) {
skip_varuint(&mut rest)?;
}
if flags.contains(RthPlanEntryFlags::HAS_LANDING_ALTITUDE) {
skip_i16(&mut rest)?;
}
if flags.contains(RthPlanEntryFlags::HAS_PRE_DELAY) {
skip_varuint(&mut rest)?;
}
if flags.contains(RthPlanEntryFlags::HAS_POST_DELAY) {
skip_varuint(&mut rest)?;
}
}
Ok(())
}
#[cfg(test)]
mod tests {
use super::*;
fn plan_with_undecodable_second_entry(second_entry: &[u8]) -> Vec<u8> {
let mut body = Vec::new();
body.extend_from_slice(&[0x00, 0x0a, 0xd0, 0x07, 0xe8, 0x03, 0x00, 0x00, 0x02, 0x00]);
body.extend_from_slice(&[0x2c, 0x01, 0x90, 0x01, 0x70, 0xfe, 0xd4, 0xfe]);
body.extend_from_slice(&[0x02, 0x00, 0x20, 0x0a, 0x00]);
body.extend_from_slice(second_entry);
body
}
fn evaluate(second_entry: &[u8], time_sec: f32) -> Result<f32, RthPlanError> {
let plan = RthPlan::from_vec(plan_with_undecodable_second_entry(second_entry))
.map_err(|_| RthPlanError::Corrupted)?;
plan.evaluate_at(time_sec).map(|entry| entry.time_sec)
}
#[test]
fn entries_before_an_undecodable_entry_can_be_evaluated() {
for (second_entry, error) in [
(
&[0x00, 0x80, 0x80, 0x80, 0x80, 0x10][..],
RthPlanError::VarintOverflow,
),
(
&[0x00, 0xff, 0xff, 0xff, 0xff, 0x0f][..],
RthPlanError::TimeOverflow,
),
(
&[0x20, 0x0a, 0x81, 0x80, 0x80, 0x08][..],
RthPlanError::DurationTooLong,
),
(&[0x02, 0x0a, 0x02, 0x05][..], RthPlanError::MissingPoint),
] {
assert_eq!(evaluate(second_entry, 5.0), Ok(10.0));
assert_eq!(evaluate(second_entry, 10.0), Ok(10.0));
assert_eq!(evaluate(second_entry, 15.0), Err(error));
assert_eq!(evaluate(second_entry, -1.0), Ok(-1.0));
}
}
#[test]
fn truncated_entries_make_the_plan_invalid() {
let body = plan_with_undecodable_second_entry(&[0x02, 0x0a]);
assert!(matches!(
RthPlan::from_vec(body),
Err(RthPlanError::Corrupted)
));
}
}