import math

#列主元高斯消元法
def SovleRowMain(A,b,round_num=15):
    ks = 0.00000001
    n = len(A)
    if len(A[0]) != n:
        print("A要为方阵")
        return None, None, None, None
    if len(b) != n:
        print("b与A的行数不匹配")
        return None, None, None, None
    p = list(range(n))
    for i in range(n):
        row_max = abs(A[i][i])
        row_max_index = i
        for j in range(i + 1, n):
            if abs(A[j][i]) > row_max:
                row_max = abs(A[j][i])
                row_max_index = j
        A[i], A[row_max_index] = A[row_max_index], A[i]
        b[i], b[row_max_index] = b[row_max_index], b[i]
        p[i], p[row_max_index] = p[row_max_index], p[i]

        if abs(A[i][i]) < ks:
            print("A矩阵奇异，无法进行高斯消元")
            return None, None, None, None
        for j in range(i + 1, n):
            m = round(A[j][i] / A[i][i],round_num)
            A[j][i] = m
            for k in range(i + 1, n):
                A[j][k] -= round(m * A[i][k],round_num)
            b[j] -= round(m * b[i],round_num)

    if abs(A[n - 1][n - 1]) < ks:
        print("A矩阵奇异，无法进行高斯消元")
        return None, None, None, None
    
    # 回代求解
    b[n - 1] = round(b[n - 1]/A[n - 1][n - 1],round_num)
    for i in range(n - 2, -1, -1):
        for j in range(i + 1, n):
            b[i] -= round(A[i][j] * b[j],round_num)
        b[i] /= round(A[i][i])
    b = [round(b[i], round_num) for i in range(n)]
    # 得到L,U和P矩阵
    L = [[0 for i in range(n)] for j in range(n)]
    U = [[0 for i in range(n)] for j in range(n)]
    P = [[0 for i in range(n)] for j in range(n)]
    for i in range(n):
        for j in range(n):
            if i == j:
                L[i][j] = 1
                U[i][j] = A[i][j]
            elif i < j:
                U[i][j] = A[i][j]
            else:
                L[i][j] = A[i][j]

        P[i][p[i]] = 1
    return P,L,U,b

#迭代改善法
def IterativeMethod(A, b, err, N, fake_round_num=15):
    b_c = [b[i] for i in range(len(b))]
    A_c = [[A[i][j] for j in range(len(A[0]))] for i in range(len(A))]
    P,L,U,x0 = SovleRowMain(A_c, b_c,fake_round_num)
    print(L)
    print(U)
    print(f"初始解为: {x0}")
    count = 0
    while count<N:
        r1 = [b[i] - sum([A[i][j] * x0[j] for j in range(len(A[0]))]) for i in range(len(A))]
        A_c = [[A[i][j] for j in range(len(A[0]))] for i in range(len(A))]
        d1 = SovleRowMain(A_c, r1,fake_round_num)[3]
        x0 = [x0[i] + d1[i] for i in range(len(x0))]
        print(f"第{count+1}次迭代, r{count+1} = {r1}, d{count+1} = {d1}, x{count+2} = {x0}")
        err_now = max(abs(r1[i]) for i in range(len(r1)))
        count += 1
        if err_now < err:
            break
    return x0,count


if __name__ == "__main__":
    ##########################################################################
    #把矩阵改成题干的矩阵，b改成题干结果，err精度要求修改##########################
    A = [
        [51,82],
        [151/3,81]
    ]
    b = [235,232]

    err = 1e-4    # 精度要求
    N = 1000      # 迭代次数上限
    fake_round_num = 4    # 模拟的四舍五入精度，根据题目情况或者凑过程修改

    x = IterativeMethod(A, b, err, N, fake_round_num)[0]
    print(f"解为: {x}")

# 先求范数与逆矩阵（条件数）