From e33fd3d815a461009d964905789d7dcfbe829a47 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 8 Mar 2016 03:21:46 -0500
Subject: [PATCH] Added controllib statistics block.

---
 src/modules/controllib/blocks.cpp | 20 +++++++++++++
 src/modules/controllib/blocks.hpp | 49 +++++++++++++++++++++++++++++++
 2 files changed, 69 insertions(+)

diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index f9e8f71ce0..d1021d038c 100644
--- a/src/modules/controllib/blocks.cpp
+++ b/src/modules/controllib/blocks.cpp
@@ -558,4 +558,24 @@ int blockRandGaussTest()
 	return 0;
 }
 
+int blockStatsTest()
+{
+	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));
+	stats.reset();
+	ASSERT_CL(equal(0.0f, stats.getMean()(0)));
+	ASSERT_CL(equal(0.0f, stats.getStdDev()(0)));
+	printf("PASS\n");
+	return 0;
+}
+
+
+
 } // namespace control
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 2b571de590..2b22caec7f 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -50,6 +50,8 @@
 #include "block/Block.hpp"
 #include "block/BlockParam.hpp"
 
+#include "matrix/math.hpp"
+
 namespace control
 {
 
@@ -553,4 +555,51 @@ private:
 
 int __EXPORT blockRandGaussTest();
 
+template<class Type, size_t M>
+class __EXPORT BlockStats: public Block
+{
+public:
+// methods
+	BlockStats(SuperBlock *parent,
+		   const char *name) :
+		Block(parent, name),
+		_sum(),
+		_sumSq(),
+		_count(0)
+	{
+	};
+	virtual ~BlockStats() {};
+	void update(const matrix::Vector<Type, M> &u)
+	{
+		_sum += u;
+		_sumSq += u.emult(u);
+		_count += 1;
+	}
+	void reset()
+	{
+		_sum.setZero();
+		_sumSq.setZero();
+		_count = 0;
+	}
+// accessors
+	size_t getCount() { return _count; }
+	matrix::Vector<Type, M> getMean() { return _sum / _count; }
+	matrix::Vector<Type, M> getVar()
+	{
+		return (_sumSq - _sum.emult(_sum) / _count) / _count;
+	}
+	matrix::Vector<Type, M> getStdDev()
+	{
+		return getVar().pow(0.5);
+	}
+private:
+// attributes
+	matrix::Vector<Type, M> _sum;
+	matrix::Vector<Type, M> _sumSq;
+	size_t _count;
+};
+
+int __EXPORT blockStatsTest();
+
+
 } // namespace control