3 files changed, 535 insertions, 0 deletions
diff --git a/src/ipa/libipa/meson.build b/src/ipa/libipa/meson.build
index 4814b7c5..49608423 100644
--- a/src/ipa/libipa/meson.build
+++ b/src/ipa/libipa/meson.build
@@ -8,6 +8,7 @@ libipa_headers = files([
     'fc_queue.h',
     'histogram.h',
     'module.h',
+    'pwl.h',
     'vector.h',
 ])
 
@@ -19,6 +20,7 @@ libipa_sources = files([
     'fc_queue.cpp',
     'histogram.cpp',
     'module.cpp',
+    'pwl.cpp',
     'vector.cpp',
 ])
 
diff --git a/src/ipa/libipa/pwl.cpp b/src/ipa/libipa/pwl.cpp
new file mode 100644
index 00000000..901e42ce
--- /dev/null
+++ b/src/ipa/libipa/pwl.cpp
@@ -0,0 +1,447 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ * Copyright (C) 2024, Ideas on Board Oy
+ *
+ * Piecewise linear functions
+ */
+
+#include "pwl.h"
+
+#include <assert.h>
+#include <cmath>
+#include <sstream>
+#include <stdexcept>
+
+/**
+ * \file pwl.h
+ * \brief Piecewise linear functions
+ */
+
+namespace libcamera {
+
+namespace ipa {
+
+/**
+ * \class Pwl
+ * \brief Describe a univariate piecewise linear function in two-dimensional
+ * real space
+ *
+ * A piecewise linear function is a univariate function that maps reals to
+ * reals, and it is composed of multiple straight-line segments.
+ *
+ * While a mathematical piecewise linear function would usually be defined by
+ * a list of linear functions and for which values of the domain they apply,
+ * this Pwl class is instead defined by a list of points at which these line
+ * segments intersect. These intersecting points are known as knots.
+ *
+ * https://en.wikipedia.org/wiki/Piecewise_linear_function
+ *
+ * A consequence of the Pwl class being defined by knots instead of linear
+ * functions is that the values of the piecewise linear function past the ends
+ * of the function are constants as opposed to linear functions. In a
+ * mathematical piecewise linear function that is defined by multiple linear
+ * functions, the ends of the function are also linear functions and hence grow
+ * to infinity (or negative infinity). However, since this Pwl class is defined
+ * by knots, the y-value of the leftmost and rightmost knots will hold for all
+ * x values to negative infinity and positive infinity, respectively.
+ */
+
+/**
+ * \typedef Pwl::Point
+ * \brief Describe a point in two-dimensional real space
+ */
+
+/**
+ * \class Pwl::Interval
+ * \brief Describe an interval in one-dimensional real space
+ */
+
+/**
+ * \fn Pwl::Interval::Interval(double _start, double _end)
+ * \brief Construct an interval
+ * \param _start Start of the interval
+ * \param _end End of the interval
+ */
+
+/**
+ * \fn Pwl::Interval::contains
+ * \brief Check if a given value falls within the interval
+ * \param value Value to check
+ * \return True if the value falls within the interval, including its bounds,
+ * or false otherwise
+ */
+
+/**
+ * \fn Pwl::Interval::clamp
+ * \brief Clamp a value such that it is within the interval
+ * \param value Value to clamp
+ * \return The clamped value
+ */
+
+/**
+ * \fn Pwl::Interval::length
+ * \brief Compute the length of the interval
+ * \return The length of the interval
+ */
+
+/**
+ * \var Pwl::Interval::start
+ * \brief Start of the interval
+ */
+
+/**
+ * \var Pwl::Interval::end
+ * \brief End of the interval
+ */
+
+/**
+ * \brief Construct an empty piecewise linear function
+ */
+Pwl::Pwl()
+{
+}
+
+/**
+ * \brief Construct a piecewise linear function from a list of 2D points
+ * \param points Vector of points from which to construct the piecewise linear function
+ *
+ * \a points must be in ascending order of x-value.
+ */
+Pwl::Pwl(const std::vector<Point> &points)
+	: points_(points)
+{
+}
+
+/**
+ * \brief Populate the piecewise linear function from yaml data
+ * \param params Yaml data to populate the piecewise linear function with
+ *
+ * Any existing points in the piecewise linear function *will* be overwritten.
+ *
+ * The yaml data is expected to be a list with an even number of numerical
+ * elements. These will be parsed in pairs into x and y points in the piecewise
+ * linear function, and added in order. x must be monotonically increasing.
+ *
+ * \return 0 on success, negative error code otherwise
+ */
+int Pwl::readYaml(const libcamera::YamlObject &params)
+{
+	if (!params.size() || params.size() % 2)
+		return -EINVAL;
+
+	const auto &list = params.asList();
+
+	points_.clear();
+
+	for (auto it = list.begin(); it != list.end(); it++) {
+		auto x = it->get<double>();
+		if (!x)
+			return -EINVAL;
+		if (it != list.begin() && *x <= points_.back().x())
+			return -EINVAL;
+
+		auto y = (++it)->get<double>();
+		if (!y)
+			return -EINVAL;
+
+		points_.push_back(Point({ *x, *y }));
+	}
+
+	return 0;
+}
+
+/**
+ * \brief Append a point to the end of the piecewise linear function
+ * \param x x-coordinate of the point to add to the piecewise linear function
+ * \param y y-coordinate of the point to add to the piecewise linear function
+ * \param eps Epsilon for the minimum x distance between points (optional)
+ *
+ * The point's x-coordinate must be greater than the x-coordinate of the last
+ * (= greatest) point already in the piecewise linear function.
+ */
+void Pwl::append(double x, double y, const double eps)
+{
+	if (points_.empty() || points_.back().x() + eps < x)
+		points_.push_back(Point({ x, y }));
+}
+
+/**
+ * \brief Prepend a point to the beginning of the piecewise linear function
+ * \param x x-coordinate of the point to add to the piecewise linear function
+ * \param y y-coordinate of the point to add to the piecewise linear function
+ * \param eps Epsilon for the minimum x distance between points (optional)
+ *
+ * The point's x-coordinate must be less than the x-coordinate of the first
+ * (= smallest) point already in the piecewise linear function.
+ */
+void Pwl::prepend(double x, double y, const double eps)
+{
+	if (points_.empty() || points_.front().x() - eps > x)
+		points_.insert(points_.begin(), Point({ x, y }));
+}
+
+/**
+ * \brief Get the domain of the piecewise linear function
+ * \return An interval representing the domain
+ */
+Pwl::Interval Pwl::domain() const
+{
+	return Interval(points_[0].x(), points_[points_.size() - 1].x());
+}
+
+/**
+ * \brief Get the range of the piecewise linear function
+ * \return An interval representing the range
+ */
+Pwl::Interval Pwl::range() const
+{
+	double lo = points_[0].y(), hi = lo;
+	for (auto &p : points_)
+		lo = std::min(lo, p.y()), hi = std::max(hi, p.y());
+	return Interval(lo, hi);
+}
+
+/**
+ * \brief Check if the piecewise linear function is empty
+ * \return True if there are no points in the function, false otherwise
+ */
+bool Pwl::empty() const
+{
+	return points_.empty();
+}
+
+/**
+ * \brief Evaluate the piecewise linear function
+ * \param[in] x The x value to input into the function
+ * \param[inout] span Initial guess for span
+ * \param[in] updateSpan Set to true to update span
+ *
+ * Evaluate Pwl, optionally supplying an initial guess for the
+ * "span". The "span" may be optionally be updated. If you want to know
+ * the "span" value but don't have an initial guess you can set it to
+ * -1.
+ *
+ *  \return The result of evaluating the piecewise linear function at position \a x
+ */
+double Pwl::eval(double x, int *span, bool updateSpan) const
+{
+	int index = findSpan(x, span && *span != -1
+					? *span
+					: points_.size() / 2 - 1);
+	if (span && updateSpan)
+		*span = index;
+	return points_[index].y() +
+	       (x - points_[index].x()) * (points_[index + 1].y() - points_[index].y()) /
+		       (points_[index + 1].x() - points_[index].x());
+}
+
+int Pwl::findSpan(double x, int span) const
+{
+	/*
+	 * Pwls are generally small, so linear search may well be faster than
+	 * binary, though could review this if large Pwls start turning up.
+	 */
+	int lastSpan = points_.size() - 2;
+	/*
+	 * some algorithms may call us with span pointing directly at the last
+	 * control point
+	 */
+	span = std::max(0, std::min(lastSpan, span));
+	while (span < lastSpan && x >= points_[span + 1].x())
+		span++;
+	while (span && x < points_[span].x())
+		span--;
+	return span;
+}
+
+/**
+ * \brief Compute the inverse function
+ * \param[in] eps Epsilon for the minimum x distance between points (optional)
+ *
+ * The output includes whether the resulting inverse function is a proper
+ * (true) inverse, or only a best effort (e.g. input was non-monotonic).
+ *
+ * \return A pair of the inverse piecewise linear function, and whether or not
+ * the result is a proper/true inverse
+ */
+std::pair<Pwl, bool> Pwl::inverse(const double eps) const
+{
+	bool appended = false, prepended = false, neither = false;
+	Pwl inverse;
+
+	for (Point const &p : points_) {
+		if (inverse.empty()) {
+			inverse.append(p.y(), p.x(), eps);
+		} else if (std::abs(inverse.points_.back().x() - p.y()) <= eps ||
+			   std::abs(inverse.points_.front().x() - p.y()) <= eps) {
+			/* do nothing */;
+		} else if (p.y() > inverse.points_.back().x()) {
+			inverse.append(p.y(), p.x(), eps);
+			appended = true;
+		} else if (p.y() < inverse.points_.front().x()) {
+			inverse.prepend(p.y(), p.x(), eps);
+			prepended = true;
+		} else {
+			neither = true;
+		}
+	}
+
+	/*
+	 * This is not a proper inverse if we found ourselves putting points
+	 * onto both ends of the inverse, or if there were points that couldn't
+	 * go on either.
+	 */
+	bool trueInverse = !(neither || (appended && prepended));
+
+	return { inverse, trueInverse };
+}
+
+/**
+ * \brief Compose two piecewise linear functions together
+ * \param[in] other The "other" piecewise linear function
+ * \param[in] eps Epsilon for the minimum x distance between points (optional)
+ *
+ * The "this" function is done first, and "other" after.
+ *
+ * \return The composed piecewise linear function
+ */
+Pwl Pwl::compose(Pwl const &other, const double eps) const
+{
+	double thisX = points_[0].x(), thisY = points_[0].y();
+	int thisSpan = 0, otherSpan = other.findSpan(thisY, 0);
+	Pwl result({ Point({ thisX, other.eval(thisY, &otherSpan, false) }) });
+
+	while (thisSpan != (int)points_.size() - 1) {
+		double dx = points_[thisSpan + 1].x() - points_[thisSpan].x(),
+		       dy = points_[thisSpan + 1].y() - points_[thisSpan].y();
+		if (std::abs(dy) > eps &&
+		    otherSpan + 1 < (int)other.points_.size() &&
+		    points_[thisSpan + 1].y() >= other.points_[otherSpan + 1].x() + eps) {
+			/*
+			 * next control point in result will be where this
+			 * function's y reaches the next span in other
+			 */
+			thisX = points_[thisSpan].x() +
+				(other.points_[otherSpan + 1].x() -
+				 points_[thisSpan].y()) *
+					dx / dy;
+			thisY = other.points_[++otherSpan].x();
+		} else if (std::abs(dy) > eps && otherSpan > 0 &&
+			   points_[thisSpan + 1].y() <=
+				   other.points_[otherSpan - 1].x() - eps) {
+			/*
+			 * next control point in result will be where this
+			 * function's y reaches the previous span in other
+			 */
+			thisX = points_[thisSpan].x() +
+				(other.points_[otherSpan + 1].x() -
+				 points_[thisSpan].y()) *
+					dx / dy;
+			thisY = other.points_[--otherSpan].x();
+		} else {
+			/* we stay in the same span in other */
+			thisSpan++;
+			thisX = points_[thisSpan].x(),
+			thisY = points_[thisSpan].y();
+		}
+		result.append(thisX, other.eval(thisY, &otherSpan, false),
+			      eps);
+	}
+	return result;
+}
+
+/**
+ * \brief Apply function to (x, y) values at every control point
+ * \param f Function to be applied
+ */
+void Pwl::map(std::function<void(double x, double y)> f) const
+{
+	for (auto &pt : points_)
+		f(pt.x(), pt.y());
+}
+
+/**
+ * \brief Apply function to (x, y0, y1) values wherever either Pwl has a
+ * control point.
+ * \param pwl0 First piecewise linear function
+ * \param pwl1 Second piecewise linear function
+ * \param f Function to be applied
+ *
+ * This applies the function \a f to every parameter (x, y0, y1), where x is
+ * the combined list of x-values from \a pwl0 and \a pwl1, y0 is the y-value
+ * for the given x in \a pwl0, and y1 is the y-value for the same x in \a pwl1.
+ */
+void Pwl::map2(Pwl const &pwl0, Pwl const &pwl1,
+	       std::function<void(double x, double y0, double y1)> f)
+{
+	int span0 = 0, span1 = 0;
+	double x = std::min(pwl0.points_[0].x(), pwl1.points_[0].x());
+	f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false));
+
+	while (span0 < (int)pwl0.points_.size() - 1 ||
+	       span1 < (int)pwl1.points_.size() - 1) {
+		if (span0 == (int)pwl0.points_.size() - 1)
+			x = pwl1.points_[++span1].x();
+		else if (span1 == (int)pwl1.points_.size() - 1)
+			x = pwl0.points_[++span0].x();
+		else if (pwl0.points_[span0 + 1].x() > pwl1.points_[span1 + 1].x())
+			x = pwl1.points_[++span1].x();
+		else
+			x = pwl0.points_[++span0].x();
+		f(x, pwl0.eval(x, &span0, false), pwl1.eval(x, &span1, false));
+	}
+}
+
+/**
+ * \brief Combine two Pwls
+ * \param pwl0 First piecewise linear function
+ * \param pwl1 Second piecewise linear function
+ * \param f Function to be applied
+ * \param[in] eps Epsilon for the minimum x distance between points (optional)
+ *
+ * Create a new Pwl where the y values are given by running \a f wherever
+ * either pwl has a knot.
+ *
+ * \return The combined pwl
+ */
+Pwl Pwl::combine(Pwl const &pwl0, Pwl const &pwl1,
+		 std::function<double(double x, double y0, double y1)> f,
+		 const double eps)
+{
+	Pwl result;
+	map2(pwl0, pwl1, [&](double x, double y0, double y1) {
+		result.append(x, f(x, y0, y1), eps);
+	});
+	return result;
+}
+
+/**
+ * \brief Multiply the piecewise linear function
+ * \param d Scalar multiplier to multiply the function by
+ * \return This function, after it has been multiplied by \a d
+ */
+Pwl &Pwl::operator*=(double d)
+{
+	for (auto &pt : points_)
+		pt[1] *= d;
+	return *this;
+}
+
+/**
+ * \brief Assemble and return a string describing the piecewise linear function
+ * \return A string describing the piecewise linear function
+ */
+std::string Pwl::toString() const
+{
+	std::stringstream ss;
+	ss << "Pwl { ";
+	for (auto &p : points_)
+		ss << "(" << p.x() << ", " << p.y() << ") ";
+	ss << "}";
+	return ss.str();
+}
+
+} /* namespace ipa */
+
+} /* namespace libcamera */
diff --git a/src/ipa/libipa/pwl.h b/src/ipa/libipa/pwl.h
new file mode 100644
index 00000000..4cc257f9
--- /dev/null
+++ b/src/ipa/libipa/pwl.h
@@ -0,0 +1,86 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2019, Raspberry Pi Ltd
+ *
+ * Piecewise linear functions interface
+ */
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "libcamera/internal/yaml_parser.h"
+
+#include "vector.h"
+
+namespace libcamera {
+
+namespace ipa {
+
+class Pwl
+{
+public:
+	using Point = Vector<double, 2>;
+
+	struct Interval {
+		Interval(double _start, double _end)
+			: start(_start), end(_end) {}
+
+		bool contains(double value)
+		{
+			return value >= start && value <= end;
+		}
+
+		double clamp(double value)
+		{
+			return std::clamp(value, start, end);
+		}
+
+		double length() const { return end - start; }
+
+		double start, end;
+	};
+
+	Pwl();
+	Pwl(const std::vector<Point> &points);
+	int readYaml(const libcamera::YamlObject &params);
+
+	void append(double x, double y, double eps = 1e-6);
+
+	bool empty() const;
+	Interval domain() const;
+	Interval range() const;
+
+	double eval(double x, int *span = nullptr,
+		    bool updateSpan = true) const;
+
+	std::pair<Pwl, bool> inverse(double eps = 1e-6) const;
+	Pwl compose(const Pwl &other, double eps = 1e-6) const;
+
+	void map(std::function<void(double x, double y)> f) const;
+
+	static Pwl
+	combine(const Pwl &pwl0, const Pwl &pwl1,
+		std::function<double(double x, double y0, double y1)> f,
+		double eps = 1e-6);
+
+	Pwl &operator*=(double d);
+
+	std::string toString() const;
+
+private:
+	static void map2(const Pwl &pwl0, const Pwl &pwl1,
+			 std::function<void(double x, double y0, double y1)> f);
+	void prepend(double x, double y, double eps = 1e-6);
+	int findSpan(double x, int span) const;
+
+	std::vector<Point> points_;
+};
+
+} /* namespace ipa */
+
+} /* namespace libcamera */