Implemented templated angles

DiscreteAngle.cpp

@@ -6,9 +6,9 @@
 
 #include <math.h>
 
-DiscreteAngle16::DiscreteAngle16(float angle) : Range16() {
+template <> AngleUsing<unsigned char>::AngleUsing(unsigned char angle) {
   if (!isfinite(angle)) {
-    range = 0;
+    value = 0;
     return;
   }
 
@@ -18,9 +18,34 @@ DiscreteAngle16::DiscreteAngle16(float angle) : Range16() {
   while (angle > 180.0F)
     angle -= 360.0F;
 
-  // map (-180..180] to 1..65535
+  // map (-180..180] to 1..256
   // This means that range value 0 is not used
-  this->range = (unsigned short)((angle + 180.0F) * 65535.0F);
+  this->value = (unsigned char)((angle + 180.0F) * 255.0F);
 }
 
-DiscreteAngle16 &DiscreteAngle16::operator-() { -this->range; }
+template <> float AngleUsing<unsigned char>::ToFloat() {
+  float f = ((float)this->value / 255.0F) - 180.0F;
+  return f;
+}
+
+template <> AngleUsing<unsigned short>::AngleUsing(unsigned short angle) {
+  if (!isfinite(angle)) {
+    value = 0;
+    return;
+  }
+
+  // clamp the float range to -1..1
+  while (angle <= -180.0F)
+    angle += 360.0F;
+  while (angle > 180.0F)
+    angle -= 360.0F;
+
+  // map (-180..180] to 1..256
+  // This means that range value 0 is not used
+  this->value = (unsigned short)((angle + 180.0F) * 65535.0F);
+}
+
+template <> float AngleUsing<unsigned short>::ToFloat() {
+  float f = ((float)this->value / 65535.0F) - 180.0F;
+  return f;
+}

DiscreteAngle.h

@@ -4,11 +4,20 @@
 #include "Range.h"
 
 // A fixed point 16-bit signed angle between (-180..180]
-class DiscreteAngle16 : public Range16 {
+// class DiscreteAngle16 : public Range16 {
-public:
+// public:
-  DiscreteAngle16(float angle);
+//   DiscreteAngle16(float angle);
 
-  inline DiscreteAngle16 &operator-() override;
+//   inline DiscreteAngle16 &operator-() override;
+// };
+
+template <typename T> class AngleUsing : RangeUsing<T> {
+public:
+  AngleUsing(T sourceValue);
+  float ToFloat();
+  inline T GetValue() { return this->value; }
 };
 
-#endif
+#endif
+
+template <typename T> inline float AngleUsing<T>::ToFloat() { return 0.0f; }

Range.cpp

@@ -1,5 +1,6 @@
 #include "Range.h"
 
+/*
 Range16::Range16() { this->range = 0; }
 
 Range16::Range16(float f) {
@@ -36,4 +37,5 @@ bool Range16::operator!=(Range16 a) { return (this->range != a.range); }
 bool Range16::operator<(Range16 a) { return (this->range < a.range); }
 bool Range16::operator>(Range16 a) { return (this->range > a.range); }
 bool Range16::operator<=(Range16 a) { return (this->range <= a.range); }
-bool Range16::operator>=(Range16 a) { return (this->range >= a.range); }
+bool Range16::operator>=(Range16 a) { return (this->range >= a.range); }
+*/

Range.h

@@ -1,6 +1,7 @@
 #ifndef RANGE_H
 #define RANGE_H
 
+/*
 /// @brief Signed range. May be renamed to SignedRange later
 class Range16 {
 public:
@@ -25,5 +26,16 @@ protected:
   // uint16_t range; // 16-bit range
   unsigned short range; // 16-bit range
 };
+*/
+
+template <typename T> class RangeUsing {
+public:
+  inline RangeUsing<T> operator-(RangeUsing<T> a) { this->value - a.value; }
+  inline RangeUsing<T> operator+(RangeUsing<T> a) { this->value + a.value; }
+  inline RangeUsing<T> operator-() { this->value = -this.value; }
+  inline T GetValue() { return value; }
+
+  T value;
+};
 
 #endif