Add expected, move Newton's method solver

util/include/rusefi/expected.h

@@ -0,0 +1,83 @@
+/**
+ * @file    expected.h
+ * @brief This utility class provides a way for a function to accept or return a value that may be invalid.
+ * 
+ * For example, suppose there needs to be a solution for prevention of divide by zero.  One could write this function:
+ * 
+ * expected<int> my_divide(int num, int denom) {
+ *     if (denom == 0) return unexpected;
+ *     return num / denom;
+ * }
+ *
+ * @date April 18, 2020
+ * @author Matthew Kennedy, (c) 2020
+ */
+
+#pragma once
+
+struct unexpected_t {};
+
+enum class UnexpectedCode : char {
+	Unknown = 0,
+
+	// Too much time has passed
+	Timeout,
+
+	// The decoded value was impossibly high/low
+	High,
+	Low,
+
+	// An inconsistency was detected using multiple sources of information
+	Inconsistent,
+
+	// A value is unavailable due to configuration
+	Configuration,
+};
+template <class TValue>
+struct expected {
+	bool Valid;
+
+	union {
+		TValue Value;
+		UnexpectedCode Code;
+	};
+
+	// Implicit constructor to construct in the invalid state
+	constexpr expected(const unexpected_t&) : Valid(false), Code{UnexpectedCode::Unknown} {}
+
+	constexpr expected(UnexpectedCode code) : Valid(false), Code{code} {}
+
+	// Implicit constructor to convert from TValue (for valid values, so an expected<T> behaves like a T)
+	constexpr expected(TValue validValue)
+		: Valid(true)
+		, Value(validValue)
+	{
+	}
+
+	// Implicit conversion operator to bool, so you can do things like if (myResult) { ... }
+	constexpr explicit operator bool() const {
+		return Valid;
+	}
+
+	// Easy default value handling
+	constexpr TValue value_or(TValue valueIfInvalid) const {
+		return Valid ? Value : valueIfInvalid;
+	}
+
+	bool operator ==(const expected<TValue>& other) const {
+		// If validity mismatch, not equal
+		if (Valid != other.Valid) {
+			return false;
+		}
+		
+		// If both are invalid, they are equal
+		if (!Valid && !other.Valid) {
+			return true;
+		}
+
+		// Both are guaranteed valid - simply compare values
+		return Value == other.Value;
+	}
+};
+
+constexpr unexpected_t unexpected{};

util/include/rusefi/math.h

@@ -2,6 +2,10 @@
 
 #pragma once
 
+#include <rusefi/expected.h>
+
+#include <cstddef>
+
 // absolute value
 int absI(int value);
 float absF(float value);
@@ -25,3 +29,18 @@ float expf_taylor(float x);
 // @brief Compute tan(theta) using a ratio of the Taylor series for sin and cos
 // Valid for the range [0, pi/2 - 0.01]
 float tanf_taylor(float theta);
+
+struct NewtonsMethodSolver
+{
+	// Solve for a value of x such that fx(x)=0
+	// x0 is the initial guess
+	// deltaX controls when to stop - when abs((estimate N) - (estimate N-1)) < deltaX, stop calculation.
+	// Returns unexpected if failed to converge
+	expected<float> solve(float x0, float deltaX, size_t maxIteration = 20);
+
+	// Function for which we want to find a root 0 = fx(x)
+	virtual float fx(float x) = 0;
+
+	// First derivative of fx(x)
+	virtual float dfx(float x) = 0;
+};

util/src/math.cpp

@@ -93,3 +93,20 @@ float tanf_taylor(float x) {
 	// tan = sin / cos
 	return sin_val / cos_val;
 }
+
+expected<float> NewtonsMethodSolver::solve(float x0, float deltaX, size_t maxIteration) {
+	// the same method works for R (if C is known) or C (if R is known)
+	float Xcur, Xnext;
+	Xnext = x0;
+
+	do {
+		if (maxIteration-- == 0) {
+			return unexpected;
+		}
+
+		Xcur = Xnext;
+		Xnext = Xcur - fx(Xcur) / dfx(Xcur);
+	} while (absF(Xnext - Xcur) > deltaX);
+
+	return Xnext;
+}