use core::fmt;
use core::marker::PhantomData;
use crate::error::{NavigationError, 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
}
}
pub(crate) fn ensure_finite(parameter: &'static str, value: f64) -> Result<()> {
if value.is_finite() {
Ok(())
} else {
Err(NavigationError::NotFinite { parameter, value })
}
}
pub(crate) fn ensure_range(parameter: &'static str, value: f64, min: f64, max: f64) -> Result<()> {
ensure_finite(parameter, value)?;
if value < min || value > max {
return Err(NavigationError::OutOfRange {
parameter,
value,
min,
max,
});
}
Ok(())
}
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,
}
}
pub(crate) 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))
}
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))
}
pub(crate) 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, 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)))
}
pub(crate) 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 = NavigationError;
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 = NavigationError;
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 = NavigationError;
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"
);
}
}