From c36fdb21561ca70cc907caf32da65b122218211c Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Mon, 20 Jul 2015 12:32:13 +0300
Subject: [PATCH] TransferID::subtracted()

---
 .../include/uavcan/transport/transfer.hpp     |  5 ++++
 libuavcan/src/transport/uc_transfer.cpp       |  7 +++++
 libuavcan/test/transport/transfer.cpp         | 26 ++++++++++++++++++-
 3 files changed, 37 insertions(+), 1 deletion(-)

diff --git a/libuavcan/include/uavcan/transport/transfer.hpp b/libuavcan/include/uavcan/transport/transfer.hpp
index 4d6e7a712f..5080dc271d 100644
--- a/libuavcan/include/uavcan/transport/transfer.hpp
+++ b/libuavcan/include/uavcan/transport/transfer.hpp
@@ -104,6 +104,11 @@ public:
      * Amount of increment() calls to reach rhs value.
      */
     int computeForwardDistance(TransferID rhs) const;
+
+    /**
+     * Difference between two TransferID values with proper wraparound handling.
+     */
+    int subtracted(TransferID rhs) const;
 };
 
 
diff --git a/libuavcan/src/transport/uc_transfer.cpp b/libuavcan/src/transport/uc_transfer.cpp
index bb5af287b3..2f6872abc7 100644
--- a/libuavcan/src/transport/uc_transfer.cpp
+++ b/libuavcan/src/transport/uc_transfer.cpp
@@ -52,4 +52,11 @@ int TransferID::computeForwardDistance(TransferID rhs) const
     return d;
 }
 
+int TransferID::subtracted(TransferID rhs) const
+{
+    const int x = rhs.computeForwardDistance(*this);
+
+    return (x >= ((1 << BitLen) / 2)) ? (x - (1 << BitLen)) : x;
+}
+
 }
diff --git a/libuavcan/test/transport/transfer.cpp b/libuavcan/test/transport/transfer.cpp
index db630b7770..28fd3b4bbc 100644
--- a/libuavcan/test/transport/transfer.cpp
+++ b/libuavcan/test/transport/transfer.cpp
@@ -15,7 +15,7 @@ TEST(Transfer, TransferID)
     ASSERT_EQ(32, 1 << TransferID::BitLen);
 
     /*
-     * forwardDistance()
+     * computeForwardDistance()
      */
     EXPECT_EQ(0, TransferID(0).computeForwardDistance(0));
     EXPECT_EQ(1, TransferID(0).computeForwardDistance(1));
@@ -28,6 +28,30 @@ TEST(Transfer, TransferID)
     EXPECT_EQ(30,TransferID(7).computeForwardDistance(5));
     EXPECT_EQ(5, TransferID(0).computeForwardDistance(5));
 
+    /*
+     * subtracted()
+     */
+    EXPECT_EQ(0,  TransferID(0).subtracted(0));
+    EXPECT_EQ(-1, TransferID(0).subtracted(1));
+    EXPECT_EQ(-7, TransferID(0).subtracted(7));
+
+    EXPECT_EQ(0,  TransferID(7).subtracted(7));
+    EXPECT_EQ(1,  TransferID(31).subtracted(30));
+    EXPECT_EQ(-1, TransferID(31).subtracted(0));
+
+    EXPECT_EQ(2,  TransferID(7).subtracted(5));
+    EXPECT_EQ(-5, TransferID(0).subtracted(5));
+
+    EXPECT_EQ(-1, TransferID(15).subtracted(16));
+    EXPECT_EQ(1,  TransferID(16).subtracted(15));
+
+    EXPECT_EQ(-16, TransferID(10).subtracted(26));
+    EXPECT_EQ(14,  TransferID(24).subtracted(10));
+    EXPECT_EQ(15,  TransferID(25).subtracted(10));
+    EXPECT_EQ(-16, TransferID(26).subtracted(10));
+    EXPECT_EQ(-15, TransferID(27).subtracted(10));
+    EXPECT_EQ(-14, TransferID(28).subtracted(10));
+
     /*
      * Misc
      */