Add triangular solve

2 files changed, 131 insertions, 3 deletions

diff --git a/src/matrix.cpp b/src/matrix.cpp
index 1faab25..c27264b 100644
--- a/src/matrix.cpp
+++ b/src/matrix.cpp
@@ -11,7 +11,7 @@
 #include <immintrin.h>
 #endif
 
-#if !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && !defined(__clangd__)
+#if !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && defined(__ARM_FEATURE_FP64_VECTOR_ARITHMETIC)
 #include <arm_neon.h>
 #endif
 
@@ -114,7 +114,7 @@ double dot_product_avx(const double* lhs, const double* rhs, std::size_t count)
 }
 #endif
 
-#if !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && !defined(__clangd__)
+#if !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && defined(__ARM_FEATURE_FP64_VECTOR_ARITHMETIC)
 double horizontal_sum(float64x2_t values) {
     return vgetq_lane_f64(values, 0) + vgetq_lane_f64(values, 1);
 }
@@ -146,7 +146,7 @@ double dot_product_simd(const double* lhs, const double* rhs, std::size_t count)
     return dot_product_avx2(lhs, rhs, count);
 #elif !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__AVX__) && !defined(__AVX2__)
     return dot_product_avx(lhs, rhs, count);
-#elif !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && !defined(__clangd__)
+#elif !defined(LINEAR_ALGEBRA_FORCE_SCALAR_MATMUL) && defined(__ARM_NEON) && defined(__aarch64__) && defined(__ARM_FEATURE_FP64_VECTOR_ARITHMETIC)
     return dot_product_neon(lhs, rhs, count);
 #else
     return dot_product_scalar(lhs, rhs, count);
diff --git a/src/triangular_solve.cpp b/src/triangular_solve.cpp
new file mode 100644
index 0000000..6a5e8ad
--- /dev/null
+++ b/src/triangular_solve.cpp
@@ -0,0 +1,128 @@
+#include "triangular_solve.hpp"
+
+#include "linalg_error.hpp"
+
+#include <cmath>
+#include <sstream>
+#include <stdexcept>
+
+namespace linalg {
+
+namespace {
+
+void validate_square_system(const Matrix& matrix, const Vector& rhs, const char* operation) {
+    if (matrix.rows() != matrix.cols()) {
+        std::ostringstream oss;
+        oss << operation << " requires a square matrix, got " << matrix.rows() << "x"
+            << matrix.cols();
+        throw DimensionMismatchError(oss.str());
+    }
+
+    if (matrix.rows() != rhs.size()) {
+        std::ostringstream oss;
+        oss << operation << " requires matrix dimension to match rhs size, got "
+            << matrix.rows() << " and " << rhs.size();
+        throw DimensionMismatchError(oss.str());
+    }
+}
+
+void validate_tolerance(double singular_tolerance) {
+    if (singular_tolerance < 0.0) {
+        throw std::invalid_argument("Singular tolerance must be nonnegative");
+    }
+}
+
+void validate_lower_triangular(
+    const Matrix& lower,
+    double singular_tolerance,
+    bool unit_diagonal) {
+    for (std::size_t i = 0; i < lower.rows(); ++i) {
+        for (std::size_t j = i + 1; j < lower.cols(); ++j) {
+            if (std::abs(lower(i, j)) > singular_tolerance) {
+                throw std::invalid_argument(
+                    "Forward substitution requires a lower-triangular matrix");
+            }
+        }
+
+        if (!unit_diagonal && std::abs(lower(i, i)) <= singular_tolerance) {
+            throw SingularMatrixError(
+                "Forward substitution encountered a zero or tiny diagonal entry");
+        }
+    }
+}
+
+void validate_upper_triangular(
+    const Matrix& upper,
+    double singular_tolerance,
+    bool unit_diagonal) {
+    for (std::size_t i = 0; i < upper.rows(); ++i) {
+        for (std::size_t j = 0; j < i; ++j) {
+            if (std::abs(upper(i, j)) > singular_tolerance) {
+                throw std::invalid_argument(
+                    "Backward substitution requires an upper-triangular matrix");
+            }
+        }
+
+        if (!unit_diagonal && std::abs(upper(i, i)) <= singular_tolerance) {
+            throw SingularMatrixError(
+                "Backward substitution encountered a negligible diagonal entry");
+        }
+    }
+}
+
+}  // namespace
+
+Vector forward_substitution(
+    const Matrix& lower,
+    const Vector& rhs,
+    double singular_tolerance,
+    bool unit_diagonal) {
+    validate_tolerance(singular_tolerance);
+    validate_square_system(lower, rhs, "Forward substitution");
+    validate_lower_triangular(lower, singular_tolerance, unit_diagonal);
+
+    Vector solution(lower.rows());
+    for (std::size_t i = 0; i < lower.rows(); ++i) {
+        double sum = rhs[i];
+        for (std::size_t j = 0; j < i; ++j) {
+            sum -= lower(i, j) * solution[j];
+        }
+
+        if (unit_diagonal) {
+            solution[i] = sum;
+        } else {
+            solution[i] = sum / lower(i, i);
+        }
+    }
+
+    return solution;
+}
+
+Vector backward_substitution(
+    const Matrix& upper,
+    const Vector& rhs,
+    double singular_tolerance,
+    bool unit_diagonal) {
+    validate_tolerance(singular_tolerance);
+    validate_square_system(upper, rhs, "Backward substitution");
+    validate_upper_triangular(upper, singular_tolerance, unit_diagonal);
+
+    Vector solution(upper.rows());
+    for (std::size_t offset = 0; offset < upper.rows(); ++offset) {
+        const std::size_t i = upper.rows() - 1 - offset;
+        double sum = rhs[i];
+        for (std::size_t j = i + 1; j < upper.cols(); ++j) {
+            sum -= upper(i, j) * solution[j];
+        }
+
+        if (unit_diagonal) {
+            solution[i] = sum;
+        } else {
+            solution[i] = sum / upper(i, i);
+        }
+    }
+
+    return solution;
+}
+
+}  // namespace linalg