use core::fmt;
use core::marker::PhantomData;
use crate::error::{ensure_finite, ensure_range, KernelError, Result};
use crate::math;
pub const MAX_VARIATION_DEG: f64 = 180.0;
pub const MAX_DEVIATION_DEG: f64 = 180.0;
#[must_use]
pub fn wrap360(degrees: f64) -> f64 {
let remainder = degrees % 360.0;
if remainder < 0.0 {
let shifted = remainder + 360.0;
if shifted >= 360.0 {
0.0
} else {
shifted
}
} else {
remainder + 0.0
}
}
#[must_use]
pub fn wrap180(degrees: f64) -> f64 {
let wrapped = wrap360(degrees);
if wrapped >= 180.0 {
wrapped - 360.0
} else {
wrapped
}
}
mod sealed {
pub trait Sealed {}
}
pub trait Frame: sealed::Sealed + Copy + Clone + fmt::Debug + 'static {
const NAME: &'static str;
const SUFFIX: char;
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct True;
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Magnetic;
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Compass;
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Gyro;
impl sealed::Sealed for True {}
impl sealed::Sealed for Magnetic {}
impl sealed::Sealed for Compass {}
impl sealed::Sealed for Gyro {}
impl Frame for True {
const NAME: &'static str = "true";
const SUFFIX: char = 'T';
}
impl Frame for Magnetic {
const NAME: &'static str = "magnetic";
const SUFFIX: char = 'M';
}
impl Frame for Compass {
const NAME: &'static str = "compass";
const SUFFIX: char = 'C';
}
impl Frame for Gyro {
const NAME: &'static str = "gyro";
const SUFFIX: char = 'G';
}
#[derive(Clone, Copy, PartialEq, PartialOrd, Default)]
pub struct Direction<F: Frame> {
degrees: f64,
frame: PhantomData<F>,
}
pub type TrueCourse = Direction<True>;
pub type TrueBearing = Direction<True>;
pub type MagneticCourse = Direction<Magnetic>;
pub type MagneticBearing = Direction<Magnetic>;
pub type CompassCourse = Direction<Compass>;
pub type CompassBearing = Direction<Compass>;
pub type GyroCourse = Direction<Gyro>;
pub type GyroBearing = Direction<Gyro>;
impl<F: Frame> Direction<F> {
pub const NORTH: Self = Self::from_wrapped(0.0);
pub const EAST: Self = Self::from_wrapped(90.0);
pub const SOUTH: Self = Self::from_wrapped(180.0);
pub const WEST: Self = Self::from_wrapped(270.0);
const fn from_wrapped(degrees: f64) -> Self {
Self {
degrees,
frame: PhantomData,
}
}
#[doc(hidden)]
#[must_use]
pub fn from_degrees_wrapped(degrees: f64) -> Self {
Self::from_wrapped(wrap360(degrees))
}
pub fn new(degrees: f64) -> Result<Self> {
ensure_range("course", degrees, 0.0, 360.0)?;
Ok(Self::from_wrapped(wrap360(degrees)))
}
pub fn wrap(degrees: f64) -> Result<Self> {
ensure_finite("course", degrees)?;
Ok(Self::from_wrapped(wrap360(degrees)))
}
#[must_use]
pub const fn degrees(self) -> f64 {
self.degrees
}
#[must_use]
pub fn radians(self) -> f64 {
math::to_radians(self.degrees)
}
#[must_use]
pub fn reciprocal(self) -> Self {
Self::from_wrapped(wrap360(self.degrees + 180.0))
}
#[must_use]
pub fn components(self, magnitude: f64) -> (f64, f64) {
let radians = self.radians();
(
magnitude * math::cos(radians),
magnitude * math::sin(radians),
)
}
pub fn offset(self, delta: f64) -> Result<Self> {
ensure_finite("delta", delta)?;
Ok(Self::from_wrapped(wrap360(self.degrees + delta)))
}
#[must_use]
pub fn signed_difference(self, other: Self) -> f64 {
wrap180(other.degrees - self.degrees)
}
#[must_use]
pub fn angular_distance(self, other: Self) -> f64 {
math::abs(self.signed_difference(other))
}
#[doc(hidden)]
#[must_use]
pub const fn relabel<G: Frame>(self) -> Direction<G> {
Direction::from_wrapped(self.degrees)
}
}
impl<F: Frame> fmt::Display for Direction<F> {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
let precision = f.precision().unwrap_or(1);
write!(
f,
"{:0>width$.precision$}°{}",
self.degrees,
F::SUFFIX,
width = if precision == 0 { 3 } else { precision + 4 },
)
}
}
impl<F: Frame> fmt::Debug for Direction<F> {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(f, "{}({}°)", F::NAME, self.degrees)
}
}
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd, Default)]
#[cfg_attr(
feature = "serde",
derive(serde::Serialize, serde::Deserialize),
serde(try_from = "f64", into = "f64")
)]
pub struct Variation(f64);
impl Variation {
pub const ZERO: Self = Self(0.0);
pub fn new(degrees: f64) -> Result<Self> {
ensure_range("variation", degrees, -MAX_VARIATION_DEG, MAX_VARIATION_DEG)?;
Ok(Self(degrees))
}
#[must_use]
pub const fn degrees(self) -> f64 {
self.0
}
}
impl fmt::Display for Variation {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
let precision = f.precision().unwrap_or(1);
let hemisphere = if self.0 < 0.0 { 'W' } else { 'E' };
write!(f, "{:.precision$}°{hemisphere}", math::abs(self.0))
}
}
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd, Default)]
#[cfg_attr(
feature = "serde",
derive(serde::Serialize, serde::Deserialize),
serde(try_from = "f64", into = "f64")
)]
pub struct Deviation(f64);
impl Deviation {
pub const ZERO: Self = Self(0.0);
pub fn new(degrees: f64) -> Result<Self> {
ensure_range("deviation", degrees, -MAX_DEVIATION_DEG, MAX_DEVIATION_DEG)?;
Ok(Self(degrees))
}
#[must_use]
pub const fn degrees(self) -> f64 {
self.0
}
}
impl fmt::Display for Deviation {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
let precision = f.precision().unwrap_or(1);
let hemisphere = if self.0 < 0.0 { 'W' } else { 'E' };
write!(f, "{:.precision$}°{hemisphere}", math::abs(self.0))
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash, Default)]
#[non_exhaustive]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum CardinalPoint {
#[default]
N,
NE,
E,
SE,
S,
SW,
W,
NW,
}
impl CardinalPoint {
pub const ALL: [Self; 8] = [
Self::N,
Self::NE,
Self::E,
Self::SE,
Self::S,
Self::SW,
Self::W,
Self::NW,
];
#[must_use]
pub const fn whole_degrees(self) -> i32 {
match self {
Self::N => 0,
Self::NE => 45,
Self::E => 90,
Self::SE => 135,
Self::S => 180,
Self::SW => 225,
Self::W => 270,
Self::NW => 315,
}
}
#[must_use]
pub fn degrees(self) -> f64 {
f64::from(self.whole_degrees())
}
#[must_use]
pub const fn abbreviation(self) -> &'static str {
match self {
Self::N => "N",
Self::NE => "NE",
Self::E => "E",
Self::SE => "SE",
Self::S => "S",
Self::SW => "SW",
Self::W => "W",
Self::NW => "NW",
}
}
}
impl fmt::Display for CardinalPoint {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
f.write_str(self.abbreviation())
}
}
impl core::str::FromStr for CardinalPoint {
type Err = KernelError;
fn from_str(text: &str) -> Result<Self> {
let trimmed = text.trim();
Self::ALL
.into_iter()
.find(|point| point.abbreviation().eq_ignore_ascii_case(trimmed))
.ok_or_else(|| KernelError::UnknownCardinalDirection {
direction: trimmed.into(),
})
}
}
impl<F: Frame> From<CardinalPoint> for Direction<F> {
fn from(point: CardinalPoint) -> Self {
Self::from_wrapped(point.degrees())
}
}
#[non_exhaustive]
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum Side {
Ahead,
Starboard,
Astern,
Port,
}
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd, Default)]
#[cfg_attr(
feature = "serde",
derive(serde::Serialize, serde::Deserialize),
serde(try_from = "f64", into = "f64")
)]
pub struct RelativeBearing(f64);
impl RelativeBearing {
pub const AHEAD: Self = Self(0.0);
pub const ABEAM_STARBOARD: Self = Self(90.0);
pub const ASTERN: Self = Self(180.0);
pub const ABEAM_PORT: Self = Self(270.0);
pub fn new(degrees: f64) -> Result<Self> {
ensure_range("relative bearing", degrees, 0.0, 360.0)?;
Ok(Self(wrap360(degrees)))
}
pub fn wrap(degrees: f64) -> Result<Self> {
ensure_finite("relative bearing", degrees)?;
Ok(Self(wrap360(degrees)))
}
#[doc(hidden)]
#[must_use]
pub fn from_degrees_wrapped(degrees: f64) -> Self {
Self(wrap360(degrees))
}
#[must_use]
pub const fn degrees(self) -> f64 {
self.0
}
#[must_use]
pub fn signed_degrees(self) -> f64 {
wrap180(self.0)
}
#[must_use]
pub fn side(self) -> Side {
const TOLERANCE: f64 = 1e-9;
let signed = self.signed_degrees();
if math::abs(signed) < TOLERANCE {
Side::Ahead
} else if math::abs(math::abs(signed) - 180.0) < TOLERANCE {
Side::Astern
} else if signed > 0.0 {
Side::Starboard
} else {
Side::Port
}
}
}
impl fmt::Display for RelativeBearing {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
let precision = f.precision().unwrap_or(1);
let magnitude = math::abs(self.signed_degrees());
match self.side() {
Side::Ahead => write!(f, "dead ahead"),
Side::Astern => write!(f, "dead astern"),
Side::Starboard => write!(f, "{magnitude:.precision$}° green"),
Side::Port => write!(f, "{magnitude:.precision$}° red"),
}
}
}
#[cfg(feature = "serde")]
impl TryFrom<f64> for Variation {
type Error = KernelError;
fn try_from(value: f64) -> Result<Self> {
Self::new(value)
}
}
#[cfg(feature = "serde")]
impl From<Variation> for f64 {
fn from(value: Variation) -> Self {
value.0
}
}
#[cfg(feature = "serde")]
impl TryFrom<f64> for Deviation {
type Error = KernelError;
fn try_from(value: f64) -> Result<Self> {
Self::new(value)
}
}
#[cfg(feature = "serde")]
impl From<Deviation> for f64 {
fn from(value: Deviation) -> Self {
value.0
}
}
#[cfg(feature = "serde")]
impl TryFrom<f64> for RelativeBearing {
type Error = KernelError;
fn try_from(value: f64) -> Result<Self> {
Self::new(value)
}
}
#[cfg(feature = "serde")]
impl From<RelativeBearing> for f64 {
fn from(value: RelativeBearing) -> Self {
value.0
}
}
#[cfg(feature = "serde")]
impl<F: Frame> serde::Serialize for Direction<F> {
fn serialize<S: serde::Serializer>(
&self,
serializer: S,
) -> core::result::Result<S::Ok, S::Error> {
serializer.serialize_f64(self.degrees)
}
}
#[cfg(feature = "serde")]
impl<'de, F: Frame> serde::Deserialize<'de> for Direction<F> {
fn deserialize<D: serde::Deserializer<'de>>(
deserializer: D,
) -> core::result::Result<Self, D::Error> {
let degrees = f64::deserialize(deserializer)?;
Self::new(degrees).map_err(serde::de::Error::custom)
}
}
#[cfg(test)]
#[allow(clippy::unwrap_used, clippy::float_cmp, clippy::indexing_slicing)]
mod tests {
use super::*;
use alloc::format;
#[test]
fn wrap360_is_correct_below_minus_360() {
assert_eq!(wrap360(-400.0), 320.0);
assert_eq!(wrap360(-720.0), 0.0);
assert_eq!(wrap360(-0.0), 0.0);
assert_eq!(wrap360(0.0), 0.0);
assert_eq!(wrap360(360.0), 0.0);
assert_eq!(wrap360(725.0), 5.0);
assert!(wrap360(-1e15).is_finite());
}
#[test]
fn wrap360_keeps_tiny_negatives_off_the_far_end() {
for value in [-1e-16, -1e-18, -f64::MIN_POSITIVE, -1e-14] {
let wrapped = wrap360(value);
assert!(
(0.0..360.0).contains(&wrapped),
"{value} wrapped to {wrapped}"
);
}
assert_eq!(wrap360(-1e-16), 0.0);
assert!(wrap360(-1e-10) < 360.0);
assert!(wrap360(-1e-10) > 359.999);
}
#[test]
fn wrap360_never_leaves_the_interval() {
let mut value = -2000.0;
while value < 2000.0 {
let wrapped = wrap360(value);
assert!((0.0..360.0).contains(&wrapped), "{value} -> {wrapped}");
value += 0.37;
}
}
#[test]
fn wrap180_is_symmetric() {
assert_eq!(wrap180(0.0), 0.0);
assert_eq!(wrap180(90.0), 90.0);
assert_eq!(wrap180(180.0), -180.0);
assert_eq!(wrap180(190.0), -170.0);
assert_eq!(wrap180(-190.0), 170.0);
}
#[test]
fn direction_rejects_bad_input() {
assert!(TrueCourse::new(f64::NAN).is_err());
assert!(TrueCourse::new(f64::INFINITY).is_err());
assert!(TrueCourse::new(-0.1).is_err());
assert!(TrueCourse::new(400.0).is_err());
assert!(TrueCourse::wrap(f64::NAN).is_err());
}
#[test]
fn direction_normalises() {
assert_eq!(TrueCourse::new(360.0).unwrap().degrees(), 0.0);
assert_eq!(TrueCourse::wrap(-10.0).unwrap().degrees(), 350.0);
assert_eq!(TrueCourse::wrap(730.0).unwrap().degrees(), 10.0);
}
#[test]
fn reciprocal_round_trips() {
for degrees in [0.0, 45.0, 179.0, 180.0, 359.9] {
let direction = TrueCourse::new(degrees).unwrap();
assert!((direction.reciprocal().reciprocal().degrees() - degrees).abs() < 1e-12);
}
}
#[test]
fn signed_difference_takes_the_short_way() {
let a = TrueCourse::new(350.0).unwrap();
let b = TrueCourse::new(10.0).unwrap();
assert!((a.signed_difference(b) - 20.0).abs() < 1e-12);
assert!((b.signed_difference(a) + 20.0).abs() < 1e-12);
assert!((a.angular_distance(b) - 20.0).abs() < 1e-12);
}
#[test]
fn variation_and_deviation_validate() {
assert!(Variation::new(-181.0).is_err());
assert!(Variation::new(f64::NAN).is_err());
assert!(Variation::new(180.0).is_ok());
assert!(Deviation::new(f64::INFINITY).is_err());
assert_eq!(Deviation::ZERO.degrees(), 0.0);
}
#[test]
fn relative_bearing_sides() {
assert_eq!(RelativeBearing::new(0.0).unwrap().side(), Side::Ahead);
assert_eq!(RelativeBearing::new(90.0).unwrap().side(), Side::Starboard);
assert_eq!(RelativeBearing::new(180.0).unwrap().side(), Side::Astern);
assert_eq!(RelativeBearing::new(270.0).unwrap().side(), Side::Port);
assert!((RelativeBearing::new(270.0).unwrap().signed_degrees() + 90.0).abs() < 1e-12);
}
#[test]
fn display_is_chart_style() {
assert_eq!(format!("{}", TrueCourse::new(45.0).unwrap()), "045.0°T");
assert_eq!(
format!("{}", MagneticCourse::new(357.89).unwrap()),
"357.9°M"
);
assert_eq!(format!("{}", Variation::new(-2.7).unwrap()), "2.7°W");
assert_eq!(format!("{}", Deviation::new(1.5).unwrap()), "1.5°E");
assert_eq!(
format!("{}", RelativeBearing::new(300.0).unwrap()),
"60.0° red"
);
}
#[test]
fn cardinal_points_are_multiples_of_forty_five_degrees() {
for (index, point) in CardinalPoint::ALL.into_iter().enumerate() {
assert_eq!(point.whole_degrees(), i32::try_from(index).unwrap() * 45);
assert_eq!(point.degrees(), f64::from(point.whole_degrees()));
}
}
#[test]
fn cardinal_points_parse_regardless_of_case_and_padding() {
for point in CardinalPoint::ALL {
let name = point.abbreviation();
assert_eq!(name.parse::<CardinalPoint>().unwrap(), point);
assert_eq!(name.to_lowercase().parse::<CardinalPoint>().unwrap(), point);
assert_eq!(
format!(" {name} ").parse::<CardinalPoint>().unwrap(),
point
);
assert_eq!(format!("{point}"), name);
}
}
#[test]
fn an_unknown_point_names_itself_in_the_error() {
assert!(matches!(
"NNE".parse::<CardinalPoint>(),
Err(KernelError::UnknownCardinalDirection { direction }) if direction == "NNE"
));
assert!("north".parse::<CardinalPoint>().is_err());
assert!("".parse::<CardinalPoint>().is_err());
}
#[test]
fn a_cardinal_point_becomes_a_direction_in_any_frame() {
assert_eq!(CompassCourse::from(CardinalPoint::SW).degrees(), 225.0);
assert_eq!(TrueCourse::from(CardinalPoint::N).degrees(), 0.0);
assert_eq!(
MagneticCourse::from(CardinalPoint::NW),
Direction::from(CardinalPoint::NW)
);
}
}