import math
import csv


class Matrix(object):
    def __init__(self, mat, augmented=False):
        self.mat = mat
        self.rows = len(mat)
        if isinstance(mat[0], list):
            self.cols = len(mat[0])
        else:
            self.cols = 1
        self.augmented = augmented
        self.triangular = None

    def row(self, r):
        return self.mat[r]

    def col(self, c):
        return [self.mat[i][c] for i in range(0, self.rows)]

    def entry(self, i, j):
        if isinstance(self.mat[i], (float, int)):
            return self.mat[i]
        return self.mat[i][j]

    def row_mult(self, ri, k):
        self.mat[ri] = [k * self.entry(ri, j) for j in range(0, self.cols)]

    def row_mult_add(self, ri, rj, k):
        self.mat[ri] = [self.entry(ri, j) + k * self.entry(rj, j) for j in range(0, self.cols)]

    def row_swap(self, ri, rj):
        self.mat[ri], self.mat[rj] = self.mat[rj], self.mat[ri]

    def print(self):
        print(self.mat)

    def print_stack(self):
        for row in self.mat:
            print("{} \n".format(row))

    def transpose(self):
        tr = []
        for i in range(0, self.cols):
            tr.append([self.mat[j][i] for j in range(0, self.rows)])
        return tr

    def gauss_back_sub(self):
        if self.rows != self.cols - 1:
            raise SystemError("Matrix must be n x n+1")
        if not self.augmented:
            raise SystemError("Matrix must be augmented")
        n = self.rows - 1
        for i in range(0, n):
            for p in range(i, n+1):
                if self.entry(p, i) != 0:
                    break
                if p == n:
                    raise SystemError("No Unique Solutions Exist")
            if p != i:
                self.row_swap(p, i)
            for j in range(i+1, n+1):
                m = self.entry(j, i)/self.entry(i, i)
                self.row_mult_add(j, i, -m)
        if self.entry(n, n) == 0:
            raise SystemError("No Unique Solutions Exist")
        xn = [0]*self.rows
        xn[n] = self.entry(n, n+1)/self.entry(n, n)
        for i in range(n-1, -1, -1):
            xn[i] = (self.entry(i, n+1) - sum([self.entry(i, j)*xn[j] for j in range(i+1, n+1)]))/self.entry(i, i)
        return xn

    def inverse(self):
        n = self.rows - 1
        identity = [[0]*(n+1) for i in range(0, n+1)]
        for i in range(0, n+1):
            identity[i][i] = 1
        inverse = Matrix(identity)
        for i in range(0, n):
            for p in range(i, n+1):
                if self.entry(p, i) != 0:
                    break
                if p == n:
                    raise SystemError("No Unique Solutions Exist")
            if p != i:
                self.row_swap(p, i)
                inverse.row_swap(p, i)
            for j in range(i+1, n+1):
                m = self.entry(j, i)/self.entry(i, i)
                self.row_mult_add(j, i, -m)
                inverse.row_mult_add(j, i, -m)
        if self.entry(n, n) == 0:
            raise SystemError("No Unique Solutions Exist")
        for i in range(0, n+1):
            k = self.entry(i, i)
            self.row_mult(i, 1/k)
            inverse.row_mult(i, 1/k)
        for i in range(n-1, -1, -1):
            for j in range(n, i, -1):
                k = self.entry(i, j)
                self.row_mult_add(i, j, -k)
                inverse.row_mult_add(i, j, -k)
        return inverse.mat

    def scalar_mult(self, k):
        for i in range(0, self.rows):
            self.row_mult(i, k)


def m_mult(m1, m2):
    if m1.cols != m2.rows:
        raise SystemError("Dim error {}, {}".format(m1.cols, m2.rows))
    result = [[0]*m2.cols for i in range(0, m1.rows)]
    for i in range(0, m1.rows):
        for j in range(0, m2.cols):
            result[i][j] = sum([m1.entry(i, r)*m2.entry(r, j) for r in range(0, m1.cols)])
    return result


def m_add(m1, m2):
    if m1.rows != m2.rows or m1.cols != m2.cols:
        raise SystemError("Dim non-match")
    result = [[0]*m1.cols for i in range(0, m1.rows)]
    for i in range(0, m1.rows):
        for j in range(0, m1.cols):
            result[i][j] = m1.entry(i, j) + m2.entry(i, j)
    return result


def m_subt(m1, m2):
    if m1.rows != m2.rows or m1.cols != m2.cols:
        raise SystemError("Dim non-match")
    result = [[0]*m1.cols for i in range(0, m1.rows)]
    for i in range(0, m1.rows):
        for j in range(0, m1.cols):
            result[i][j] = m1.entry(i, j) - m2.entry(i, j)
    return result

"""
fx = F(x1, x2, x3) 3x1 list
jx = 3x3 list evaluated at appx
appx = 3x1 list
"""
def broyden_3(fx, jx, appx, tol, use_csv=True):
    n = len(appx)                                           ####

    xn = [[] for i in range(0, n+1)]
    for i in range(0, n):
        xn[i].append(appx[i])
    xn[-1].append(None)

    X = Matrix([xn[i][-1] for i in range(0, n)])
    A = Matrix(Matrix(jx).inverse())

    v = []
    for i in range(0, n):
        v.append(fx[i](xn[0][-1], xn[1][-1], xn[2][-1]))    ####
    v = Matrix(v)
    A.scalar_mult(-1)
    S = Matrix(m_mult(A, v))

    X = Matrix(m_add(X, S))
    for i in range(0, n):
        xn[i].append(X.mat[i][0])
    xn[-1].append(math.sqrt(sum([(xn[i][-1] - xn[i][-2])**2 for i in range(0, n)])))

    while xn[-1][-1] > tol:
        w = v
        v = []
        for i in range(0, n):
            v.append(fx[i](xn[0][-1], xn[1][-1], xn[2][-1]))    ####
        v = Matrix(v)

        y = Matrix(m_subt(v, w))
        z = Matrix(m_mult(A, y))
        St = Matrix(S.transpose())
        p = -m_mult(St, z)[0][0]
        A.scalar_mult(-1)
        u = Matrix(m_mult(St, A))
        aa = Matrix(m_add(S, z))
        ab = Matrix(m_mult(aa, u))
        ab.scalar_mult(1/p)
        A = Matrix(m_add(A, ab))
        A.scalar_mult(-1)
        S = Matrix(m_mult(A, v))
        X = Matrix(m_add(X, S))
        for i in range(0, n):
            xn[i].append(X.mat[i][0])
        xn[-1].append(math.sqrt(sum([(xn[i][-1] - xn[i][-2])**2 for i in range(0, n)])))

    if use_csv:
        with open('broyden.csv', 'w', newline='') as csvfile:
            output = csv.writer(csvfile, delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
            output.writerow(['x1', 'x2', 'x3', 'l_2_norm'])
            for i in range(0, len(xn[0])):
                output.writerow([xn[0][i], xn[1][i], xn[2][i], xn[3][i]])
        csvfile.close()

    return xn




print(broyden_3([lambda x, y, z: 3*x - math.cos(y*z) - 0.5,
                 lambda x, y, z: x**2 - 81*(y + 0.1)**2 + math.sin(z) + 1.06,
                 lambda x, y, z: math.exp(-x*y) + 20*z + (10*math.pi - 3)/3],
                [[3, -0.1*math.sin(-0.01), 0.1*math.sin(0.01)],
                 [0.2, -32.4, math.cos(-0.1)],
                 [-0.1*math.exp(-0.01), -0.1*math.exp(-0.01), 20]],
                [0.1, 0.1, -0.1], 10**-5, use_csv=True))


print(broyden_3([lambda x, y, z: x**3 + x**2*y - x*z + 6,
                 lambda x, y, z: math.exp(x) + math.exp(y) - z,
                 lambda x, y, z: y**2 - 2*x*z - 4],
                [[7, 1, 1],
                 [math.exp(-1), math.exp(-2), -1],
                 [-2, -4, 2]],
                [-1, -2, 1], 10**-5, use_csv=True))


