# 1Eva_IT2017_T4 Componentes eléctricos
# Metodo de Jacobi para sistemas de ecuaciones
import numpy as np
 
def jacobi(A,B,X0, tolera, iteramax=100,
           vertabla=False, precision=4):
    ''' Método de Jacobi, tolerancia, vector inicial X0
        para mostrar iteraciones y tabla: 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) # tamaño 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) # errado
 
    if vertabla==True:
        print('Iteraciones Jacobi')
        print('itera,[X]')
        print('     ,errado,|diferencia|')
        print(0,X0)
        print(' ',np.nan)
        np.set_printoptions(precision)
 
    itera = 0 # Jacobi
    X = np.copy(X0)
    Xnuevo = 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): # un coeficiente
                if (i!=j): # excepto diagonal de A
                    suma = suma-A[i,j]*X[j]
            Xnuevo[i] = suma/A[i,i]
        diferencia = abs(Xnuevo-X)
        errado = np.max(diferencia)
        X = np.copy(Xnuevo)
 
        Xfila = np.concatenate((Xnuevo,[errado]),axis=0)
        tabla = np.concatenate((tabla,[Xfila]),axis=0)
 
        itera = itera + 1
 
        if vertabla==True:
            print(itera, Xnuevo)
            print(' ',errado,diferencia)
 
    # No converge
    if (itera>iteramax):
        X = np.nan
        print('No converge,iteramax superado')
 
    if vertabla==True:
        X = [X,tabla]
    return(X)
 
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 = [[9, 7,3],
     [3,16,4],
     [5, 7,9]]

B = [987,1049,945]
 
X0 = [0,0,0]
tolera = 0.0001
iteramax = 100
verdecimal = 5
 
# 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] = jacobi(A,B,X0,tolera,iteramax,
                    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 Jacobi')
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)
graf.set_xlabel('itera')
graf.set_ylabel('|error|')
graf.set_title('Método de Jacobi: error[itera]')
graf.grid()
 
plt.show()