From 51772a18433c702244af814e0e7c24b28e904ada Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Mon, 5 Aug 2024 11:37:33 +0200
Subject: [PATCH] Added add/subtract for discrete angles

---
 Angle.cpp                  | 20 +++++++++++++
 Angle.h                    |  4 +++
 Angle16.cpp                | 23 +++++++++++++++
 Angle8.cpp                 | 23 +++++++++++++++
 test/DiscreteAngle_test.cc | 57 ++++++++++++++++++++++++--------------
 5 files changed, 106 insertions(+), 21 deletions(-)

diff --git a/Angle.cpp b/Angle.cpp
index 3b0b3d5..c808f88 100644
--- a/Angle.cpp
+++ b/Angle.cpp
@@ -102,6 +102,26 @@ bool Passer::LinearAlgebra::AngleOf<float>::operator==(AngleOf<float> a) {
   return this->ToFloat() == a.ToFloat();
 }
 
+template <>
+AngleOf<float> AngleOf<float>::operator-() const {
+  AngleOf<float> angle = AngleOf(-this->value);
+  return angle;
+}
+
+template <>
+AngleOf<float> AngleOf<float>::operator-(const AngleOf<float>& a) const {
+  AngleOf<float> angle = AngleOf();
+  angle.value = this->value - a.value;
+  return angle;
+}
+
+template <>
+AngleOf<float> AngleOf<float>::operator+(const AngleOf<float>& a) const {
+  AngleOf<float> angle = AngleOf();
+  angle.value = this->value + a.value;
+  return angle;
+}
+
 template <>
 AngleOf<float> AngleOf<float>::Normalize(AngleOf<float> angle) {
   float angleValue = angle.ToFloat();
diff --git a/Angle.h b/Angle.h
index fc845a8..6b15e6f 100644
--- a/Angle.h
+++ b/Angle.h
@@ -32,6 +32,10 @@ class AngleOf {
 
   bool operator==(AngleOf<T> a);
 
+  AngleOf<T> operator-() const;
+  AngleOf<T> operator-(const AngleOf<T>& a) const;
+  AngleOf<T> operator+(const AngleOf<T>& a) const;
+
   static AngleOf<T> Normalize(AngleOf<T> a);
   static AngleOf<T> Clamp(AngleOf<T> a, AngleOf<T> min, AngleOf<T> max);
   static AngleOf<T> Difference(AngleOf<T> a, AngleOf<T> b) {
diff --git a/Angle16.cpp b/Angle16.cpp
index e5e9cc5..c7f968a 100644
--- a/Angle16.cpp
+++ b/Angle16.cpp
@@ -35,6 +35,29 @@ float AngleOf<signed short>::ToFloat() const {
   return f;
 }
 
+template <>
+AngleOf<signed short> AngleOf<signed short>::operator-() const {
+  AngleOf<signed short> angle = AngleOf();
+  angle.value = -this->value;
+  return angle;
+}
+
+template <>
+AngleOf<signed short> AngleOf<signed short>::operator-(
+    const AngleOf<signed short>& a) const {
+  AngleOf<signed short> angle = AngleOf();
+  angle.value = this->value - a.value;
+  return angle;
+}
+
+template <>
+AngleOf<signed short> AngleOf<signed short>::operator+(
+    const AngleOf<signed short>& a) const {
+  AngleOf<signed short> angle = AngleOf();
+  angle.value = this->value + a.value;
+  return angle;
+}
+
 // Not correct!!! just for syntactical compilation ATM
 template <>
 AngleOf<signed short> AngleOf<signed short>::CosineRuleSide(
diff --git a/Angle8.cpp b/Angle8.cpp
index 0ff2af0..ff14295 100644
--- a/Angle8.cpp
+++ b/Angle8.cpp
@@ -34,3 +34,26 @@ float AngleOf<signed char>::ToFloat() const {
   float f = (this->value * 180) / 128.0F;
   return f;
 }
+
+template <>
+AngleOf<signed char> AngleOf<signed char>::operator-() const {
+  AngleOf<signed char> angle = AngleOf();
+  angle.value = -this->value;
+  return angle;
+}
+
+template <>
+AngleOf<signed char> AngleOf<signed char>::operator-(
+    const AngleOf<signed char>& a) const {
+  AngleOf<signed char> angle = AngleOf();
+  angle.value = this->value - a.value;
+  return angle;
+}
+
+template <>
+AngleOf<signed char> AngleOf<signed char>::operator+(
+    const AngleOf<signed char>& a) const {
+  AngleOf<signed char> angle = AngleOf();
+  angle.value = this->value + a.value;
+  return angle;
+}
\ No newline at end of file
diff --git a/test/DiscreteAngle_test.cc b/test/DiscreteAngle_test.cc
index 8576880..62e505b 100644
--- a/test/DiscreteAngle_test.cc
+++ b/test/DiscreteAngle_test.cc
@@ -12,53 +12,68 @@
 TEST(Angle8, Construct) {
   float angle = 0.0F;
   Angle8 a = Angle8(angle);
-  float f = a.ToFloat();
-  EXPECT_FLOAT_EQ(f, angle);
+  EXPECT_FLOAT_EQ(a.ToFloat(), angle);
 
   angle = -180.0F;
   a = Angle8(angle);
-  f = a.ToFloat();
-  EXPECT_FLOAT_EQ(f, angle);
+  EXPECT_FLOAT_EQ(a.ToFloat(), angle);
 }
 
 TEST(Angle8, Negate) {
   float angle = 0;
   Angle8 a = Angle8(angle);
-  a = -a.ToFloat();
-  float f = a.ToFloat();
-  EXPECT_FLOAT_EQ(f, angle);
+  a = -a;
+  EXPECT_FLOAT_EQ(a.ToFloat(), angle);
 
   angle = 90.0F;
   a = Angle8(angle);
-  a = -a.ToFloat();
-  f = a.ToFloat();
-  EXPECT_FLOAT_EQ(f, -angle);
+  a = -a;
+  EXPECT_FLOAT_EQ(a.ToFloat(), -angle);
 }
 
 TEST(Angle8, Add) {
-  Angle8 a = Angle8(0);
-  Angle8 b = Angle8(0.0F);
-  Angle8 r = a.ToFloat() + b.ToFloat();
+  Angle8 a = Angle8(-45);
+  Angle8 b = Angle8(45.0F);
+  Angle8 r = a + b;
   EXPECT_FLOAT_EQ(r.ToFloat(), 0);
 }
 
+TEST(Angle8, Subtract) {
+  Angle8 a = Angle8(0);
+  Angle8 b = Angle8(45.0F);
+  Angle8 r = a - b;
+  EXPECT_FLOAT_EQ(r.ToFloat(), -45);
+}
+
 TEST(Angle16, Construct) {
   Angle16 a = Angle16(0.0F);
-  float f = a.ToFloat();
-  EXPECT_FLOAT_EQ(f, 0);
+  EXPECT_FLOAT_EQ(a.ToFloat(), 0);
 }
 
 TEST(Angle16, Negate) {
   float angle = 0;
   Angle16 a = Angle16(angle);
-  a = -a.ToFloat();
-  float f = a.ToFloat();
-  EXPECT_FLOAT_EQ(f, angle);
+  a = -a;
+  EXPECT_FLOAT_EQ(a.ToFloat(), angle);
 
   angle = 90.0F;
   a = Angle16(angle);
-  a = -a.ToFloat();
-  f = a.ToFloat();
-  EXPECT_FLOAT_EQ(f, -angle);
+  a = -a;
+  EXPECT_FLOAT_EQ(a.ToFloat(), -angle);
 }
+
+TEST(Angle16, Subtract) {
+  Angle16 a = Angle16(0);
+  Angle16 b = Angle16(45.0F);
+  Angle16 r = a - b;
+  EXPECT_FLOAT_EQ(r.ToFloat(), -45);
+}
+
+TEST(Angle16, Add) {
+  Angle16 a = Angle16(-45);
+  Angle16 b = Angle16(45.0F);
+  Angle16 r = a + b;
+  EXPECT_FLOAT_EQ(r.ToFloat(), 0);
+}
+
 #endif
\ No newline at end of file