import numpy as np
# A is type array from numpy
# remember: A[0.0] is the first element
# Elementary row operations:

def  TrocaLinha(A,i,j):
    """Troca as linhas i e j da matriz A"""
    B=A.copy()
    buffer = B[i].copy()
    B[i] = B[j]
    B[j] = buffer
    return B


def SubsLinha(A,i,k,alfa, j=0):
    '''soma a linha i com um multiplo da linha k a partir da coluna j'''
    B=A.copy()
    L,C=shape(B)
    B[i,j:C] = B[i,j:C] + alfa*B[k,j:C]
    return B

def BackStep(A,b):
    """ Se a matriz for triangular superior resolve Ax=b 
    A é uma matriz triangular.
    b é uma lista de números reais de mesma dimensão de A. """
    #i Vamos testar se A é TS e se a a diagonal não tem zero
    # Esta parte consome muito tempo e não deve ser usada depois
    #n = len(A)
    #for i in range(n):
    #    teste1 = A[i,i]==0
    #    if i==n:
    #        teste2 = False
    #    else: teste2 = False in (A[i+1:n,i]==0)
    #    if teste1 or teste2:
    #        raise ValueError(" Matriz A não pode ser parametro do BackStep ")
    # Esta foi só a parte de teste.
    n=shape(A)[0]
    x=np.zeros(n)
    k=n-1
    while k>=0:
        x[k] = (1/A[k,k])*(b[k]-sum([A[k,j]*x[j] for j in range(k+1,n)]))
        k-=1
    return x

# step k of gauss elimination:
def GaussStep(A,k):
    '''O k-esimo passo na eliminacao de gauss'''
    L=shape(A)[0] # numero de linhas da matriz A
    B=A.copy()
    for j in range(k+1,L):
        m=-B[j,k]/B[k,k]
        B=SubsLinha(B,j,k,m,k)
        B[j,k]=-m # Aproveitamos a matriz A pra guardar o multiplicador(ta errado!_)
    return B

# triangle form, without care and pivoting
def TriangleForm(A):
    '''Retorna a matriz escalonada,
    com os multiplicadores na parte triangular inferior'''
    B=A.copy()
    L=shape(B)[0] # numero de linhas = numero de incognitas
    for k in range(L-1):
        p=pivo(B,k)
        B=TrocaLinha(B,k,p)
        B = GaussStep(B,k)
     
    return B
   
def pivo(A,n):
    ''' retorna o indice de pivotacao da matriz A na coluna n,
    a partir da linha n'''
    tamanho = shape(A)
    l=tamanho[0] # numero de linhas de A
    c= tamanho[1] # numero de colunas
    if (l<=n) or (c<=n):
        raise ValueError("n deve ser menor que dimensao de A")
    p=n # inicio o pivo como n, e vou aumentando
    for k in range(n,l):
        if abs(A[k,n]) > abs(A[p,n]):
            p=k
    return p

A = np.array([[3.,2.,1.,1],[1,4,2,2],[6,1,0,1]])
A

A1 = TrocaLinha(A,0,pivo(A,0))
A1
        

A2 = GaussStep(A1,0)
A2

A3 = GaussStep(A2,1)
A3

A4= A3[:,0:3]
A4

B1= A3[:,3]
B1

x=BackStep(A4,B1)
x

# Inversão de matrizes
S = np.array([[3.,2,1,1,0,0],[1,4,2,0,1,0],[6,1,0,0,0,1]])
S

S1 = TriangleForm(S)
print(S1)


Ac= S1[:,0:3]
Ac

I1 = S1[:,3]
I2 = S1[:,4]
I3 = S1[:,5]
print(I2)

X1 = BackStep(Ac,I1)
X2 = BackStep(Ac,I2)
X3 = BackStep(Ac,I3)

X1


Ain = np.array([X1,X2,X3])


Ain


Ain.T

A

A4

An = S[:,0:3]
An

An*(Ain.T)

inv(An)



An*inv(An)