From 5c09c8ede7c4299311aeeb9f33182f5dec1b9736 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 8 Mar 2016 06:08:30 -0500
Subject: [PATCH] Add delay block to controllib.

---
 src/modules/controllib/blocks.cpp | 30 ++++++++++++++++++++++++------
 src/modules/controllib/blocks.hpp | 30 ++++++++++++++++++++++++++++++
 2 files changed, 54 insertions(+), 6 deletions(-)

diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index d1021d038c..3efe807a27 100644
--- a/src/modules/controllib/blocks.cpp
+++ b/src/modules/controllib/blocks.cpp
@@ -63,9 +63,11 @@ int basicBlocksTest()
 	blockPDTest();
 	blockPIDTest();
 	blockOutputTest();
-	blockRandUniformTest();
+	//blockRandUniformTest();
 	// known failures
 	// blockRandGaussTest();
+	blockStatsTest();
+	blockDelayTest();
 	return 0;
 }
 
@@ -560,15 +562,14 @@ int blockRandGaussTest()
 
 int blockStatsTest()
 {
+	printf("Test BlockStats\t\t: ");
 	BlockStats<float, 1> stats(NULL, "TEST");
 	ASSERT_CL(equal(0.0f, stats.getMean()(0)));
 	ASSERT_CL(equal(0.0f, stats.getStdDev()(0)));
-
-
 	stats.update(matrix::Scalar<float>(1.0f));
-	ASSERT_CL(equal(1.0f, stats.getMean()(0)));
-	ASSERT_CL(equal(0.5f, stats.getStdDev()(0)));
 	stats.update(matrix::Scalar<float>(2));
+	ASSERT_CL(equal(1.5f, stats.getMean()(0)));
+	ASSERT_CL(equal(0.5f, stats.getStdDev()(0)));
 	stats.reset();
 	ASSERT_CL(equal(0.0f, stats.getMean()(0)));
 	ASSERT_CL(equal(0.0f, stats.getStdDev()(0)));
@@ -576,6 +577,23 @@ int blockStatsTest()
 	return 0;
 }
 
-
+int blockDelayTest()
+{
+	printf("Test BlockDelay\t\t: ");
+	using namespace matrix;
+	BlockDelay<float, 2, 3> delay(NULL, "TEST");
+	Vector2f u1(1, 2);
+	Vector2f y1 = delay.update(u1);
+	Vector2f u2(4, 5);
+	Vector2f y2 = delay.update(u2);
+	Vector2f u3(7, 8);
+	Vector2f y3 = delay.update(u3);
+	Vector2f u4(9, 10);
+	Vector2f y4 = delay.update(u4);
+	ASSERT_CL(equal(u1(0), y4(0)));
+	ASSERT_CL(equal(u1(1), y4(1)));
+	printf("PASS\n");
+	return 0;
+}
 
 } // namespace control
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 2b22caec7f..0e62b54813 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -601,5 +601,35 @@ private:
 
 int __EXPORT blockStatsTest();
 
+template<class Type, size_t M, size_t LEN>
+class __EXPORT BlockDelay: public Block
+{
+public:
+// methods
+	BlockDelay(SuperBlock *parent,
+		   const char *name) :
+		Block(parent, name),
+		_h(),
+		_index(0)
+	{
+	};
+	virtual ~BlockDelay() {};
+	matrix::Vector<Type, M> update(const matrix::Vector<Type, M> &u)
+	{
+		matrix::Vector<Type, M> val = _h[_index];
+		_h[_index] = u;
+		_index += 1;
+
+		if (_index >= LEN) { _index = 0; }
+
+		return val;
+	}
+private:
+// attributes
+	matrix::Vector<Type, M> _h[LEN];
+	size_t _index;
+};
+
+int __EXPORT blockDelayTest();
 
 } // namespace control