from sympy import *

init_printing()

A = Matrix([[ 2,  3,  1],
     [-1, -2, -2],
     [ 2,  5,  3],
     [ 1,  2,  0]])

b = Matrix([20, -20, -20, 10])

m = A.shape[0]
n = A.shape[1]

rank = A.rank()

def G_Matrix(i, j, c, s):
    G = Matrix.eye(m)
    # Insert C and S
    G[i, i] = c
    G[i, j] = s
    G[j, i] = -s
    G[j, j] = c
    return G
R = A.QRdecomposition()[1].evalf()
for j in range(n):
    for i in range(j+1, m):
        a = A[i, j]
        if a != 0:
            print(f"Step: i: {i + 1}; j: {j + 1}")
            r = sqrt(A[j, j] ** 2 + a ** 2)
            print(f"r: {r.evalf()}")
            c = A[j, j] / r
            print(f"c: {c.evalf()}")
            s = a / r
            print(f"s: {s.evalf()}")
            rot_mat = G_Matrix(j, i, c, s)
            A = rot_mat * A
            b = rot_mat * b
            print("\nG:\n")
            pprint(A)
            pprint("\n----------------------------------------------------------------------------\n")

print("x: \n")
pprint(A.LDLsolve(b))