From 02454d0cdc95dbfa7acde991c923c85ae4398a73 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 8 Mar 2016 07:27:34 -0500
Subject: [PATCH] Improved controllib delay block.

---
 src/modules/controllib/blocks.cpp | 17 ++++++++++----
 src/modules/controllib/blocks.hpp | 37 +++++++++++++++++++++++++++----
 2 files changed, 46 insertions(+), 8 deletions(-)

diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index 3efe807a27..a7bc46c879 100644
--- a/src/modules/controllib/blocks.cpp
+++ b/src/modules/controllib/blocks.cpp
@@ -562,7 +562,7 @@ int blockRandGaussTest()
 
 int blockStatsTest()
 {
-	printf("Test BlockStats\t\t: ");
+	printf("Test BlockStats\t\t\t: ");
 	BlockStats<float, 1> stats(NULL, "TEST");
 	ASSERT_CL(equal(0.0f, stats.getMean()(0)));
 	ASSERT_CL(equal(0.0f, stats.getStdDev()(0)));
@@ -579,19 +579,28 @@ int blockStatsTest()
 
 int blockDelayTest()
 {
-	printf("Test BlockDelay\t\t: ");
+	printf("Test BlockDelay\t\t\t: ");
 	using namespace matrix;
 	BlockDelay<float, 2, 3> delay(NULL, "TEST");
 	Vector2f u1(1, 2);
 	Vector2f y1 = delay.update(u1);
+	ASSERT_CL(equal(y1(0), u1(0)));
+	ASSERT_CL(equal(y1(1), u1(1)));
+
 	Vector2f u2(4, 5);
 	Vector2f y2 = delay.update(u2);
+	ASSERT_CL(equal(y2(0), u1(0)));
+	ASSERT_CL(equal(y2(1), u1(1)));
+
 	Vector2f u3(7, 8);
 	Vector2f y3 = delay.update(u3);
+	ASSERT_CL(equal(y3(0), u1(0)));
+	ASSERT_CL(equal(y3(1), u1(1)));
+
 	Vector2f u4(9, 10);
 	Vector2f y4 = delay.update(u4);
-	ASSERT_CL(equal(u1(0), y4(0)));
-	ASSERT_CL(equal(u1(1), y4(1)));
+	ASSERT_CL(equal(y4(0), u2(0)));
+	ASSERT_CL(equal(y4(1), u2(1)));
 	printf("PASS\n");
 	return 0;
 }
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 0e62b54813..1360a3523e 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -610,24 +610,53 @@ public:
 		   const char *name) :
 		Block(parent, name),
 		_h(),
-		_index(0)
+		_index(0),
+		_delay(-1)
 	{
 	};
 	virtual ~BlockDelay() {};
 	matrix::Vector<Type, M> update(const matrix::Vector<Type, M> &u)
 	{
-		matrix::Vector<Type, M> val = _h[_index];
+		// store current value
 		_h[_index] = u;
+
+		// delay starts at zero, then increases to LEN
+		_delay += 1;
+
+		if (_delay > (LEN - 1)) {
+			_delay = LEN - 1;
+		}
+
+		// compute position of delayed value
+		int j = _index - _delay;
+
+		if (j < 0) {
+			j += LEN;
+		}
+
+		// increment storage position
 		_index += 1;
 
-		if (_index >= LEN) { _index = 0; }
+		if (_index > (LEN - 1)) {
+			_index  = 0;
+		}
+
+		// get delayed value
+		return _h[j];
+	}
+	matrix::Vector<Type, M> get()
+	{
+		int j = _index - _delay;
+
+		if (j < 0) { j += LEN; }
 
-		return val;
+		return _h[j];
 	}
 private:
 // attributes
 	matrix::Vector<Type, M> _h[LEN];
 	size_t _index;
+	int _delay;
 };
 
 int __EXPORT blockDelayTest();