1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151

//! Generic angular position type for motors and sensors.
//!
//! Positions have many conversion functions as well as common operator implementations for ease of use.

use core::{cmp::Ordering, ops::*};

//TODO: Add more unit types to this.
/// Represents an angular position.
#[derive(Clone, Copy, Debug)]
pub enum Position {
    /// Degrees of rotation.
    Degrees(f64),
    /// Counts of full rotations, 360 degrees.
    Rotations(f64),
    /// Raw encoder ticks.
    Counts(i64),
}

impl Position {
    /// Creates a position from a specified number of degrees.
    pub const fn from_degrees(position: f64) -> Self {
        Self::Degrees(position)
    }

    /// Creates a position from a specified number of rotations.
    pub const fn from_rotations(position: f64) -> Self {
        Self::Rotations(position)
    }

    /// Creates a position from a specified number of counts (raw encoder tics).
    pub const fn from_counts(position: i64) -> Self {
        Self::Counts(position)
    }

    /// Converts a position into degrees.
    pub fn into_degrees(self) -> f64 {
        match self {
            Self::Degrees(num) => num,
            Self::Rotations(num) => num * 360.0,
            Self::Counts(num) => num as f64 * (360.0 / 4096.0),
        }
    }

    /// Converts a position into rotations.
    pub fn into_rotations(self) -> f64 {
        match self {
            Self::Degrees(num) => num / 360.0,
            Self::Rotations(num) => num,
            Self::Counts(num) => num as f64 * 4096.0,
        }
    }

    /// Converts a position into counts (raw encoder ticks).
    pub fn into_counts(self) -> i64 {
        match self {
            Self::Degrees(num) => (num * 4096.0 / 360.0) as i64,
            Self::Rotations(num) => (num * 4096.0) as i64,
            Self::Counts(num) => num,
        }
    }
}

impl Add for Position {
    type Output = Self;

    fn add(self, rhs: Self) -> Self::Output {
        Self::from_degrees(self.into_degrees() + rhs.into_degrees())
    }
}

impl AddAssign for Position {
    fn add_assign(&mut self, rhs: Self) {
        *self = *self + rhs;
    }
}

impl Sub for Position {
    type Output = Self;

    fn sub(self, rhs: Self) -> Self::Output {
        Self::from_degrees(self.into_degrees() - rhs.into_degrees())
    }
}

impl SubAssign for Position {
    fn sub_assign(&mut self, rhs: Self) {
        *self = *self - rhs;
    }
}

impl Mul<Self> for Position {
    type Output = Self;

    fn mul(self, rhs: Self) -> Self::Output {
        Self::from_degrees(self.into_degrees() * rhs.into_degrees())
    }
}

impl MulAssign<Self> for Position {
    fn mul_assign(&mut self, rhs: Self) {
        *self = *self * rhs;
    }
}

impl Div<Self> for Position {
    type Output = Self;

    fn div(self, rhs: Self) -> Self::Output {
        Self::from_degrees(self.into_degrees() / rhs.into_degrees())
    }
}

impl DivAssign<Self> for Position {
    fn div_assign(&mut self, rhs: Self) {
        *self = *self / rhs;
    }
}

impl Rem<Self> for Position {
    type Output = Self;

    fn rem(self, rhs: Self) -> Self::Output {
        Self::from_degrees(self.into_degrees() % rhs.into_degrees())
    }
}

impl RemAssign<Self> for Position {
    fn rem_assign(&mut self, rhs: Self) {
        *self = *self % rhs;
    }
}

impl Neg for Position {
    type Output = Self;

    fn neg(self) -> Self::Output {
        Self::from_degrees(-self.into_degrees())
    }
}

impl PartialEq for Position {
    fn eq(&self, other: &Self) -> bool {
        self.into_degrees() == other.into_degrees()
    }
}

impl PartialOrd for Position {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        self.into_degrees().partial_cmp(&other.into_degrees())
    }
}