# 1Eva2007TII_T2 Aplicar Gauss-Seidel 6x6
import numpy as np
 
def gauss_seidel(A,B,X0,tolera, iteramax=100, vertabla=False, precision=4):
    ''' Método de Gauss Seidel, tolerancia, vector inicial X0
        para mostrar iteraciones: vertabla=True
    '''
    # Matrices como arreglo, numeros reales
    A = np.array(A, dtype=float)
    B = np.array(B, dtype=float)
    X0 = np.array(X0, dtype=float)
    tamano = np.shape(A)
    n = tamano[0]
    m = tamano[1]
 
    # valores iniciales
    diferencia = 2*tolera*np.ones(n, dtype=float)
    errado = 2*tolera # np.max(diferencia)
    
    tabla = [np.copy(X0)] # tabla de iteraciones
    tabla = np.concatenate((tabla,[[np.nan]]),
                            axis=1) # añade errado
 
    if vertabla==True:
        print('Iteraciones Gauss-Seidel')
        print('itera,[X]')
        print('   errado,[diferencia]')
        print(0,X0)
        print(' ',np.nan)
        np.set_printoptions(precision)
 
    itera = 0 # Gauss-Sediel
    X = np.copy(X0)
    while (errado>tolera and itera<iteramax):
         
        for i in range(0,n,1): # una ecuacion
            suma = B[i]
            for j in range(0,m,1):
                if (i!=j):
                    suma = suma-A[i,j]*X[j]
            nuevo = suma/A[i,i]
            diferencia[i] = np.abs(nuevo-X[i])
            X[i] = nuevo
        errado = np.max(diferencia)
 
        Xfila= np.concatenate((X,[errado]),axis=0)
        tabla = np.concatenate((tabla,[Xfila]),axis = 0)
        itera = itera + 1
        if vertabla==True:
            print(itera, X)
            print(' ', errado,diferencia)
         
    # No converge
    if (itera>iteramax):
        X = np.nan
        print('No converge,iteramax superado')
    return(X,tabla)
 
def pivoteafila(A,B,vertabla=False):
    '''
    Pivotea parcial por filas, entrega matriz aumentada AB
    Si hay ceros en diagonal es matriz singular,
    Tarea: Revisar si diagonal tiene ceros
    '''
    A = np.array(A,dtype=float)
    B = np.array(B,dtype=float)
    # Matriz aumentada
    nB = len(np.shape(B))
    if nB == 1:
        B = np.transpose([B])
    AB  = np.concatenate((A,B),axis=1)
     
    if vertabla==True:
        print('Matriz aumentada')
        print(AB)
        print('Pivoteo parcial:')
     
    # Pivoteo por filas AB
    tamano = np.shape(AB)
    n = tamano[0]
    m = tamano[1]
     
    # Para cada fila en AB
    pivoteado = 0
    for i in range(0,n-1,1):
        # columna desde diagonal i en adelante
        columna = np.abs(AB[i:,i])
        dondemax = np.argmax(columna)
         
        # dondemax no es en diagonal
        if (dondemax != 0):
            # intercambia filas
            temporal = np.copy(AB[i,:])
            AB[i,:] = AB[dondemax+i,:]
            AB[dondemax+i,:] = temporal
 
            pivoteado = pivoteado + 1
            if vertabla==True:
                print(' ',pivoteado, 'intercambiar filas: ',i,'y', dondemax+i)
    if vertabla==True:
        if pivoteado==0:
            print('  Pivoteo por filas NO requerido')
        else:
            print('AB')
            print(AB)
    return(AB)
 
# PROGRAMA Búsqueda de solucion  --------
# INGRESO
A = [[7.63, 0.30, 0.15,  0.50, 0.34, 0.84],
     [0.38, 6.40, 0.70,  0.90, 0.29, 0.57],
     [0.83, 0.19, 8.33,  0.82, 0.34, 0.37],
     [0.50, 0.68, 0.86, 10.21, 0.53, 0.70],
     [0.71, 0.30, 0.85,  0.82, 5.95, 0.55],
     [0.43, 0.54, 0.59,  0.66, 0.31, 9.25]]
 
B = [-9.44,25.27,-48.01,19.76,-23.63,62.59]
 
X0 = np.zeros(len(A), dtype=float)
tolera = 0.0001
iteramax = 100
verdecimal = 4
 
# PROCEDIMIENTO
AB = pivoteafila(A,B,vertabla=True)
n,m = np.shape(AB)
 
A = AB[:,:n] # separa en A y B
B = AB[:,n]
 
[X, tabla] = gauss_seidel(A,B,X0, tolera,
                           vertabla=True,
                           precision=verdecimal)
n_itera = len(tabla)-1
errado = tabla[-1,-1]
 
# numero de condicion
ncond = np.linalg.cond(A)
 
# SALIDA
print('Metodo de Gauss-Seidel')
print('numero de condición:', ncond)
print('X: ',X)
print('errado:',errado)
print('iteraciones:', n_itera)

# GRAFICA de iteraciones
errados = tabla[:,n]
 
import matplotlib.pyplot as plt
# grafica de error por iteracion
fig2D = plt.figure()
graf = fig2D.add_subplot(111)
graf.plot(errados,color='purple')
graf.set_xlabel('itera')
graf.set_ylabel('|error|')
graf.set_title('Método de Gauss-Seidel: error[itera]')
graf.grid()
 
plt.show()