[PATCH v2 1/3] ipa: libipa: Add Matrix class

[Paul Elder]

Add a class to represent a Matrix object and operations for adding
matrices, multipling a matrix by a scalar, and multiplying two matrices.

Signed-off-by: Paul Elder <paul.elder at ideasonboard.com>
---
Changes in second v2 (v1 was a lone patch so we have two v2s of this
specific patch):
- make rows and columns into template arguments
- initialize to identity matrix on construction
- add getter and setter
- change from struct to class
- fix matrix multiplication

Changes in v2:
- clean up unused includes
- avoid dereferencing an absent std::optional
---
 src/ipa/libipa/matrix.cpp  |  17 +++++
 src/ipa/libipa/matrix.h    | 144 +++++++++++++++++++++++++++++++++++++
 src/ipa/libipa/meson.build |   2 +
 3 files changed, 163 insertions(+)
 create mode 100644 src/ipa/libipa/matrix.cpp
 create mode 100644 src/ipa/libipa/matrix.h

diff --git a/src/ipa/libipa/matrix.cpp b/src/ipa/libipa/matrix.cpp
new file mode 100644
index 00000000..c222c3f0
--- /dev/null
+++ b/src/ipa/libipa/matrix.cpp
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ * Copyright (C) 2024, Paul Elder <paul.elder at ideasonboard.com>
+ *
+ * matrix.cpp - Matrix and related operations
+ */
+
+#include "matrix.h"
+
+#include <libcamera/base/log.h>
+
+namespace libcamera {
+
+LOG_DEFINE_CATEGORY(Matrix)
+
+} /* namespace libcamera */
diff --git a/src/ipa/libipa/matrix.h b/src/ipa/libipa/matrix.h
new file mode 100644
index 00000000..ec41fb57
--- /dev/null
+++ b/src/ipa/libipa/matrix.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ * Copyright (C) 2024, Paul Elder <paul.elder at ideasonboard.com>
+ *
+ * matrix.cpp - Matrix and related operations
+ */
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <sstream>
+#include <vector>
+
+#include <libcamera/base/log.h>
+
+#include "libcamera/internal/yaml_parser.h"
+
+namespace libcamera {
+
+LOG_DECLARE_CATEGORY(Matrix)
+
+namespace ipa {
+
+template<typename T, unsigned int R, unsigned int C,
+	 std::enable_if_t<std::is_arithmetic_v<T>> * = nullptr>
+class Matrix
+{
+public:
+	Matrix()
+	{
+		for (unsigned int i = 0; i < R; i++)
+			for (unsigned int j = 0; j < C; j++)
+				data_.push_back(static_cast<T>(i == j));
+	};
+
+	Matrix(const std::vector<T> &data)
+	{
+		ASSERT(data_.size() == R * C);
+
+		for (const T &x : data)
+			data_.push_back(x);
+	}
+
+	~Matrix(){};
+
+	int readYaml(const libcamera::YamlObject &yaml)
+	{
+		if (yaml.size() != R * C) {
+			LOG(Matrix, Error)
+				<< "Wrong number of values in matrix: expected "
+				<< R * C << ", got " << yaml.size();
+			return -EINVAL;
+		}
+
+		unsigned int i = 0;
+		for (const auto &x : yaml.asList()) {
+			auto value = x.get<T>();
+			if (!value) {
+				LOG(Matrix, Error) << "Failed to read matrix value";
+				return -EINVAL;
+			}
+
+			data_[i++] = *value;
+		}
+
+		return 0;
+	}
+
+	const std::string toString() const
+	{
+		std::stringstream ss;
+		ss << "Matrix { ";
+		for (unsigned int i = 0; i < R; i++) {
+			ss << "[ ";
+			for (unsigned int j = 0; j < C; j++) {
+				ss << get(i, j);
+				ss << ((j + 1 < C) ? ", " : " ");
+			}
+			ss << ((i + 1 < R) ? "], " : "]");
+		}
+		ss << " }";
+
+		return ss.str();
+	}
+
+	const T &get(unsigned int i, unsigned int j) const { return data_.at(i * C + j); }
+	void set(unsigned int i, unsigned int j, T value) { data_[i * C + j] = value; }
+
+private:
+	std::vector<T> data_;
+};
+
+template<typename T, unsigned int R, unsigned int C,
+	 std::enable_if_t<std::is_arithmetic_v<T>> * = nullptr>
+Matrix<T, R, C> operator*(T d, const Matrix<T, R, C> &m)
+{
+	Matrix<T, R, C> result;
+
+	for (unsigned int i = 0; i < R; i++)
+		for (unsigned int j = 0; j < C; j++)
+			result.set(i, j, d * m.get(i, j));
+
+	return result;
+}
+
+template<typename T,
+	 unsigned int R1, unsigned int C1,
+	 unsigned int R2, unsigned int C2,
+	 std::enable_if_t<std::is_arithmetic_v<T> && C1 == R2> * = nullptr>
+Matrix<T, R1, C2> operator*(const Matrix<T, R1, C1> &m1, const Matrix<T, R2, C2> &m2)
+{
+	Matrix<T, R1, C2> result;
+
+	for (unsigned int i = 0; i < R1; i++) {
+		for (unsigned int j = 0; j < C2; j++) {
+			T sum = 0;
+
+			for (unsigned int k = 0; k < C1; k++)
+				sum += m1.get(i, k) * m2.get(k, j);
+
+			result.set(i, j, sum);
+		}
+	}
+
+	return result;
+}
+
+template<typename T, unsigned int R, unsigned int C,
+	 std::enable_if_t<std::is_arithmetic_v<T>> * = nullptr>
+Matrix<T, R, C> operator+(const Matrix<T, R, C> &m1, const Matrix<T, R, C> &m2)
+{
+	Matrix<T, R, C> result;
+
+	for (unsigned int i = 0; i < R; i++)
+		for (unsigned int j = 0; j < C; j++)
+			result.set(i, j, m1.get(i, j) + m2.get(i, j));
+
+	return result;
+}
+
+} /* namespace ipa */
+
+} /* namespace libcamera */
diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build
index 1b3faf8d..1e34355f 100644
--- a/src/ipa/libipa/meson.build
+++ b/src/ipa/libipa/meson.build
@@ -7,6 +7,7 @@ libipa_headers = files([
     'exposure_mode_helper.h',
     'fc_queue.h',
     'histogram.h',
+    'matrix.h',
     'module.h',
     'pwl.h',
 ])
@@ -18,6 +19,7 @@ libipa_sources = files([
     'exposure_mode_helper.cpp',
     'fc_queue.cpp',
     'histogram.cpp',
+    'matrix.cpp',
     'module.cpp',
     'pwl.cpp'
 ])
-- 
2.39.2