From 385f033bb5f2f7af9382d4504670221bbdb3c78c Mon Sep 17 00:00:00 2001
From: tomcombriat <tomcombriat@live.fr>
Date: Sat, 18 Nov 2023 15:37:25 +0100
Subject: [PATCH] Added sub/add between SFix

---
 FixMath2.h | 65 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 65 insertions(+)

diff --git a/FixMath2.h b/FixMath2.h
index bfc969bea..411c7b391 100644
--- a/FixMath2.h
+++ b/FixMath2.h
@@ -88,6 +88,12 @@ class UFixMath2
     return UFixMath2<new_NI,new_NF>(tt,true);
   }
 
+  // With other types: add to integer part, unsafe
+  template<typename T>
+  UFixMath2<NI,NF> operator+ (const T op) const
+  {
+    return UFixMath2<NI,NF>(internal_value+(op<<NF),true);
+  }
 
 
   //////// SUBSTRACTION OVERLOADS
@@ -105,6 +111,14 @@ class UFixMath2
     return UFixMath2<new_NI,new_NF>(tt,true);
   }
 
+  
+  // With other types: add to integer part, unsafe
+  template<typename T>
+  UFixMath2<NI,NF> operator- (const T op) const
+  {
+    return UFixMath2<NI,NF>(internal_value+(op<<NF),true);
+  }
+
   //////// MULTIPLICATION OVERLOADS
   
   // Multiplication overload between Ufixed type, returns the compound type, safe
@@ -234,6 +248,57 @@ class SFixMath2
   void fromRaw(T raw) { internal_value = raw; }
 
 
+  /* Constructor from another fixed type */
+  template<byte _NI, byte _NF>
+  SFixMath2(const SFixMath2<_NI,_NF>& uf) {
+    internal_value = SHIFTR((typename IntegerType<((MAX(NI+NF-1,_NI+_NF-1))>>3)+1>::unsigned_type) uf.asRaw(),(_NF-NF));
+  }
+
+  //////// ADDITION OVERLOADS
+  // Between SFix
+  template<byte _NI, byte _NF>
+  SFixMath2<MAX(NI,_NI)+1,MAX(NF,_NF)> operator+ (const SFixMath2<_NI,_NF>& op) const
+  {
+    constexpr byte new_NI = MAX(NI, _NI) + 1;
+    constexpr byte new_NF = MAX(NF, _NF);
+    typedef typename IntegerType< ((new_NI+new_NF-1)>>3)+1>::unsigned_type return_type;
+    SFixMath2<new_NI,new_NF> left(*this);
+    SFixMath2<new_NI,new_NF> right(op);
+
+    return_type tt = return_type(left.asRaw()) + right.asRaw();
+    return SFixMath2<new_NI,new_NF>(tt,true);
+  }
+
+  // With other types: add to integer part, unsafe
+  template<typename T>
+  SFixMath2<NI,NF> operator+ (const T op) const
+  {
+    return SFixMath2<NI,NF>(internal_value+(op<<NF),true);
+  }
+
+
+  //////// SUBSTRACTION OVERLOADS
+  // Between SFix
+  template<byte _NI, byte _NF> // We do not have the +1 after MAX(NI, _NI) because the substraction between two SFix should fit in the bigger of the two.
+  SFixMath2<MAX(NI,_NI),MAX(NF,_NF)> operator- (const SFixMath2<_NI,_NF>& op) const
+  {
+    constexpr byte new_NI = MAX(NI, _NI);
+    constexpr byte new_NF = MAX(NF, _NF);
+    typedef typename IntegerType< ((new_NI+new_NF-1)>>3)+1>::unsigned_type return_type;
+    SFixMath2<new_NI,new_NF> left(*this);
+    SFixMath2<new_NI,new_NF> right(op);
+
+    return_type tt = return_type(left.asRaw()) - right.asRaw();
+    return SFixMath2<new_NI,new_NF>(tt,true);
+  }
+
+  
+  // With other types: add to integer part, unsafe
+  template<typename T>
+  SFixMath2<NI,NF> operator- (const T op) const
+  {
+    return SFixMath2<NI,NF>(internal_value+(op<<NF),true);
+  }
 
   
   //////// MULTIPLICATION OVERLOADS