import numpy as np
import numpy.linalg as la
import matplotlib.pyplot as plt
from scipy.optimize import minimize_scalar

def f(r):
    x, y = r
    return 3 +((x**2)/8) + ((y**2)/8) - np.sin(x)*np.cos((2**-0.5)*y)

def df(r):
    x, y = r
    return np.array([0.25 * x - np.cos(x)*np.cos((2**-0.5)*y), 0.25 * y + (2**-0.5) * np.sin(x)*np.sin((2**-0.5)*y),])

def h(r):
    x, y = r
    return np.array([[0.25 + np.sin(x)*np.cos((2**-0.5)*y), (2**-0.5)*np.cos(x)*np.sin((2**-0.5)*y)], [(2**-0.5)*np.cos(x)*np.sin((2**-0.5)*y), 0.25 + 0.5 * np.sin(x)*np.cos((2**-0.5)*y)]])

iteration_count_newton = 0
newton_iter = []
r_newton = np.copy(r_init)
while True:
    newton_iter.append(r_newton)
    r_df = df(r_newton)
    r_h = h(r_newton)
    if la.norm(r_df, 2) < stop:
        break
    iteration_count_newton += 1
    r_newton = r_newton + np.matmul(la.inv(r_h), -r_df)

iteration_count_sd = 0
sd_iter = []
r_sd = np.copy(r_init)
while True:
    sd_iter.append(r_sd)
    r_df = df(r_sd)
    if la.norm(r_df, 2) < stop:
        break
    iteration_count_sd += 1
    find_a = lambda a : f(r_sd - a * r_df)
    r_sd = r_sd - minimize_scalar(find_a).x * r_df

newton_error = [np.log(la.norm(newton_iter[i] - r_newton, 2)) for i in range(len(newton_iter))]
sd_error = [np.log(la.norm(sd_iter[i] - r_sd, 2)) for i in range(len(sd_iter))]
plt.plot(range(len(newton_iter)), newton_error, label="newton")
plt.plot(range(len(sd_iter)), sd_error, label="sd")
plt.xlabel("number of iterations")
plt.ylabel("log of error")
plt.title("Error as a function of num iter")
plt.legend()