From 4defcde10b4a2a783567354b6bf7039b82db03de Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Wed, 23 Apr 2014 13:38:56 +0400
Subject: [PATCH] ComponentStatusManager<>

---
 .../helpers/component_status_manager.hpp      | 57 +++++++++++++++++++
 .../test/helpers/component_status_manager.cpp | 27 +++++++++
 2 files changed, 84 insertions(+)
 create mode 100644 libuavcan/include/uavcan/helpers/component_status_manager.hpp
 create mode 100644 libuavcan/test/helpers/component_status_manager.cpp

diff --git a/libuavcan/include/uavcan/helpers/component_status_manager.hpp b/libuavcan/include/uavcan/helpers/component_status_manager.hpp
new file mode 100644
index 0000000000..8de7299ae5
--- /dev/null
+++ b/libuavcan/include/uavcan/helpers/component_status_manager.hpp
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/impl_constants.hpp>
+#include <uavcan/stdint.hpp>
+#include <uavcan/protocol/NodeStatus.hpp>
+
+namespace uavcan
+{
+
+template <unsigned NumComponents_>
+class ComponentStatusManager
+{
+public:
+    typedef typename StorageType<protocol::NodeStatus::FieldTypes::status_code>::Type StatusCode;
+    static const unsigned NumComponents = NumComponents_;
+
+private:
+    StatusCode status_array[NumComponents];
+
+public:
+    ComponentStatusManager(StatusCode default_status = protocol::NodeStatus::STATUS_OK)
+    {
+        for (unsigned i = 0; i < NumComponents; i++)
+        {
+            status_array[i] = default_status;
+        }
+    }
+
+    template <typename ComponentIndexType>
+    void setComponentStatus(ComponentIndexType component_index, StatusCode status_code)
+    {
+        const unsigned compidx = static_cast<unsigned>(component_index);  // This cast allows to use typesafe enums
+        if (compidx < NumComponents)
+        {
+            status_array[compidx] = status_code;
+        }
+    }
+
+    StatusCode getWorstStatusCode() const
+    {
+        StatusCode result = 0;
+        for (unsigned i = 0; i < NumComponents; i++)
+        {
+            result = std::max(result, status_array[i]);
+        }
+        return result;
+    }
+};
+
+template <unsigned NumComponents_>
+const unsigned ComponentStatusManager<NumComponents_>::NumComponents;
+
+}
diff --git a/libuavcan/test/helpers/component_status_manager.cpp b/libuavcan/test/helpers/component_status_manager.cpp
new file mode 100644
index 0000000000..4965ff2334
--- /dev/null
+++ b/libuavcan/test/helpers/component_status_manager.cpp
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/helpers/component_status_manager.hpp>
+
+
+TEST(ComponentStatusManager, Basic)
+{
+    uavcan::ComponentStatusManager<4> csm;
+
+    ASSERT_EQ(4, uavcan::ComponentStatusManager<4>::NumComponents);
+    ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_OK, csm.getWorstStatusCode());
+
+    csm.setComponentStatus(3, uavcan::protocol::NodeStatus::STATUS_WARNING);
+    ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_WARNING, csm.getWorstStatusCode());
+
+    csm.setComponentStatus(2, uavcan::protocol::NodeStatus::STATUS_CRITICAL);
+    ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_CRITICAL, csm.getWorstStatusCode());
+
+    csm.setComponentStatus(3, uavcan::protocol::NodeStatus::STATUS_INITIALIZING);
+    ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_CRITICAL, csm.getWorstStatusCode());
+
+    csm.setComponentStatus(2, uavcan::protocol::NodeStatus::STATUS_INITIALIZING);
+    ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_INITIALIZING, csm.getWorstStatusCode());
+}