Merge links on nodes: Naive Gauss elimination and internal loads

power_grid_model_c/power_grid_model/include/power_grid_model/link_solver.hpp

@@ -319,4 +319,54 @@ inline std::vector<std::vector<DoubleComplex>> set_projection_system(Idx free_in
     return projection_system;
 };
 
+inline void gauss_elimination(std::vector<std::vector<DoubleComplex>>& system) {
+
+    auto const system_size = narrow_cast<Idx>(system.size());
+
+    for (Idx column = 0; column < system_size; column++) {
+        for (Idx row = column + 1; row < system_size; row++) {
+
+            system[row][column] = -system[row][column] / system[column][column];
+
+            for (Idx column_part = column + 1; column_part < system_size + 1; column_part++) {
+                system[row][column_part] += system[row][column] * system[column][column_part];
+            }
+        }
+    }
+
+    system[system_size - 1][system_size] /= system[system_size - 1][system_size - 1];
+    for (Idx row = system_size - 1; row-- > 0;) {
+        auto element_sum = DoubleComplex{};
+        for (Idx column = row + 1; column < system_size; column++) {
+            element_sum -= system[row][column] * system[column][system_size];
+        }
+        system[row][system_size] = (system[row][system_size] + element_sum) / system[row][row];
+    }
+};
+
+inline std::vector<DoubleComplex> compute_internal_loads(SolutionSet& solution_set,
+                                                         std::vector<std::vector<DoubleComplex>>& system) {
+
+    std::vector<DoubleComplex> internal_loads{};
+
+    auto const number_of_rows = narrow_cast<Idx>(solution_set.extended_rhs.size());
+    auto const number_of_columns = narrow_cast<Idx>(system.size());
+
+    internal_loads.resize(number_of_rows);
+
+    for (auto row : std::views::iota(Idx{}, number_of_rows)) {
+
+        internal_loads[row] = solution_set.extended_rhs[row];
+        auto sum_value = DoubleComplex{};
+        for (auto column : std::views::iota(Idx{}, number_of_columns)) {
+            auto const value = solution_set.dfs_matrix.get_value(row, column);
+            if (value.has_value()) {
+                sum_value += static_cast<DoubleComplex>(value.value()) * system[column].back();
+            }
+        }
+        internal_loads[row] -= sum_value;
+    }
+
+    return internal_loads;
+};
 } // namespace power_grid_model::link_solver::detail