melvin_ob/flight_control/orbit/
index.rs

1use crate::util::Vec2D;
2use chrono::{DateTime, Utc};
3use fixed::types::I32F32;
4
5/// Represents a position in an orbit with associated metadata.
6#[derive(Debug, Copy, Clone, serde::Serialize, serde::Deserialize)]
7pub struct IndexedOrbitPosition {
8    /// The timestamp representing the current time of this position.
9    t: DateTime<Utc>,
10    /// The index representing the current index in the orbits `done`-box.
11    index: usize,
12    /// The 2D positional vector of this point in the orbit.
13    pos: Vec2D<I32F32>,
14    /// The period of the orbit.
15    period: usize,
16}
17
18impl IndexedOrbitPosition {
19    /// Creates a new [`IndexedOrbitPosition`] instance.
20    ///
21    /// # Arguments
22    /// - `index`: The index in the orbit.
23    /// - `period`: The period of the orbit.
24    /// - `pos`: The 2D position vector for this point in the orbit.
25    ///
26    /// # Returns
27    /// A new [`IndexedOrbitPosition`] instance with the current UTC time.
28    pub fn new(index: usize, period: usize, pos: Vec2D<I32F32>) -> Self {
29        Self { t: Utc::now(), index, pos, period }
30    }
31
32    /// Returns the timestamp of the position.
33    pub fn t(&self) -> DateTime<Utc> { self.t }
34
35    /// Returns the 2D positional vector of the orbit.
36    pub fn pos(&self) -> Vec2D<I32F32> { self.pos }
37
38    /// Returns the current index in the orbit.
39    pub fn index(&self) -> usize { self.index }
40
41    /// Returns the period of the orbit.
42    pub fn period(&self) -> usize { self.period }
43
44    /// Calculates the ranges from the current index to now, optionally applying a shift.
45    ///
46    /// # Arguments
47    /// - `shift`: An optional value to adjust the calculation of the current index.
48    ///
49    /// # Returns
50    /// A vector of tuples representing the ranges as `(start, end)` for the orbit indices.
51    pub fn get_ranges_to_now(&self, shift: Option<usize>) -> Vec<(usize, usize)> {
52        let end = {
53            if let Some(sh) = shift {
54                (((self.index_now() - sh) % self.period) + self.period) % self.period
55            } else {
56                self.index_now() % self.period
57            }
58        };
59        if end < self.index {
60            vec![(self.index, self.period), (0, end)]
61        } else {
62            vec![(self.index, self.index_now() % self.period)]
63        }
64    }
65
66    /// Maps ranges of indices, ensuring they wrap around correctly for a maximum range.
67    ///
68    /// # Arguments
69    /// - `ranges`: A vector of tuples representing input ranges as `(start, end)`.
70    /// - `max`: The maximum range for the indices.
71    ///
72    /// # Returns
73    /// A vector of tuples representing mapped ranges as `(start, end)` for the orbit indices.
74    #[allow(clippy::cast_sign_loss)]
75    pub fn map_ranges(ranges: &Vec<(isize, isize)>, max: isize) -> Vec<(usize, usize)> {
76        let mut mapped_ranges = Vec::new();
77        for range in ranges {
78            let start = (((range.0 % max) + max) % max) as usize;
79            let end = (((range.1 % max) + max) % max) as usize;
80            if start < end {
81                mapped_ranges.push((start, end));
82            } else {
83                mapped_ranges.push((start, (max - 1) as usize));
84                mapped_ranges.push((0, end));
85            }
86        }
87        mapped_ranges
88    }
89
90    /// Creates a new `IndexedOrbitPosition` with the given 2D position vector.
91    ///
92    /// # Arguments
93    /// - `pos`: The new 2D position vector.
94    ///
95    /// # Returns
96    /// A new `IndexedOrbitPosition` instance with updated position and time.
97    pub fn new_from_pos(&self, pos: Vec2D<I32F32>) -> Self {
98        Self { t: Utc::now(), index: self.index_now(), pos, period: self.period }
99    }
100
101    /// Creates a new `IndexedOrbitPosition` with a future timestamp and given 2D position vector.
102    ///
103    /// # Arguments
104    /// - `pos`: The new 2D position vector.
105    /// - `dt`: The time difference to calculate the future timestamp.
106    ///
107    /// # Returns
108    /// A new `IndexedOrbitPosition` instance with updated position and future timestamp.
109    pub fn new_from_future_pos(&self, pos: Vec2D<I32F32>, t: DateTime<Utc>) -> Self {
110        Self { t, index: self.index_then(t), pos, period: self.period }
111    }
112
113    /// Calculates the current index in the orbit based on the elapsed time.
114    ///
115    /// # Returns
116    /// The current index in the orbit.
117    #[allow(clippy::cast_sign_loss, clippy::cast_possible_truncation)]
118    fn index_now(&self) -> usize {
119        (self.index + (Utc::now() - self.t).num_seconds() as usize) % self.period
120    }
121
122    /// Calculates the index in the orbit for a given time offset.
123    ///
124    /// # Arguments
125    /// - `dt`: The time difference to calculate the future index.
126    ///
127    /// # Returns
128    /// The future index in the orbit.
129    #[allow(clippy::cast_sign_loss, clippy::cast_possible_truncation)]
130    pub(crate) fn index_then(&self, t: DateTime<Utc>) -> usize {
131        (self.index + (t - self.t).num_seconds() as usize) % self.period
132    }
133}
melvin_ob/flight_control/orbit/index.rs

melvin_ob/flight_control/orbit/
index.rs
