"""Routines for solving a linear system of equations."""

import numpy as np
from numpy.typing import NDArray


def gaussian_eliminate(
        aa: NDArray[np.float_],
        bb: NDArray[np.float_],
        tolerance: float = 1e-6
) -> NDArray[np.float_] | None:
    """Solves a linear system of equations (Ax = b) by Gauss-elimination

    Args:
        aa: Matrix with the coefficients. Shape: (n, n).
        bb: Right hand side of the equation. Shape: (n,)
        tolerance: Greatest absolute value considered as 0

    Returns:
        Vector xx with the solution of the linear equation or None
        if the equations are linearly dependent.
    """
    nn = aa.shape[0]

    ee = np.zeros((nn, nn + 1))
    ee[:, 0:nn] = aa
    ee[:, nn] = bb

    i = 0
    j = 0

    while i < nn and j < nn:
        pivot_row = i + np.argmax(np.abs(ee[i:, j]))

        if np.abs(ee[pivot_row, j]) < tolerance:
            j = j + 1
            continue

        if i != pivot_row:
            # Swap rows
            ee[[i, pivot_row]] = ee[[pivot_row, i]]

        for k in range(i + 1, nn):
            q = ee[k, j] / ee[i, j]
            ee[k, :] = ee[k, :] - q * ee[i, :]

        i = i + 1
        j = j + 1

    # Check if rank of matrix is lower than nn
    if np.abs(ee[nn - 1, nn - 1]) < tolerance:
        return None

    # Back substitution
    xx = np.zeros((nn, 1), dtype=np.float_)
    for i in range(nn - 1, -1, -1):
        xx[i] = (ee[i, nn] - np.dot(ee[i, i:nn], xx[i:nn])) / ee[i, i]

    return xx