#Oppgave 1c

# x''_t(X, t) = (a/m)*(v_w - x'_t(X, t))
# m = 10^(-2) kg, a = 5 * 10^(-5) Ns/m

# tenker at x2[i+1] = x2[i] + (h/2)*(f(x2[i])+f(x2[i+1]))
# og videre x1[i+1] = x1[i] + (h/2)*(x2[i]+ x2[i+1])

from matplotlib import pyplot as plt
import numpy as np

L = 100
m = 1/100
alpha = 5/(10**5)
k = alpha/m







def f(X1, X2, t):
    x_deriv = k*(V_w(X1, t)[0] - X2[0])
    y_deriv = k*(V_w(X1, t)[1] - X2[1])
    return [x_deriv, y_deriv]

def V_w(X1, t):
    x = X1[0]
    y = X1[1]
    R = np.sqrt(x*x + y*y)
    T = 24*3600 #sek
    x_vec = -(2*(np.pi)*R/T)*(y/R)
    y_vec= (2*(np.pi)*R/T)*(x/R)

    return [x_vec, y_vec]

def forward_Euler(f, h, X1, X2, t):
    x_koord = X2[0] + h*(f(X1, X2, t)[0])
    y_koord = X2[1] + h* (f(X1, X2, t)[1])


    return [x_koord, y_koord]


def explixit_trap_eq1(f, h, X1, X2, t):
    X2_ip1 =  forward_Euler(f, h, X1, X2, t)
    u_ip1 = X2[0] + (h/2)*(f(X1, X2, t)[0] + f(X1, X2_ip1, t)[0])
    v_ip1 = X2[1] + (h/2)*(f(X1, X2, t)[1] + f(X1, X2_ip1, t)[1])

    X2_ip1 = [u_ip1, v_ip1]

    x_koord = X1[0] + (h/2)*(X2[0]+u_ip1)
    y_koord = X1[1] + (h/2)*(X2[1]+v_ip1)
    X1_ip1 = [x_koord, y_koord]

    return X1_ip1, X2_ip1

x_list=[]
y_list=[]
n=10000
L=100
x_list.append(L)
y_list.append(0)

u_list=[]
v_list=[]
u_list.append(0)
v_list.append(0)

for i in range(n):
    X1 = [x_list[i], y_list[i]]
    X2 = [u_list[i], v_list[i]]

    X1_ip1, X2_ip1 = explixit_trap_eq1(f, 48*3600/n, X1, X2, 0)

    x_list.append(X1_ip1[0])
    y_list.append(X1_ip1[1])

    u_list.append(X2_ip1[0])
    v_list.append(X2_ip1[1])


plt.figure(figsize= (20,20))
plt.title('Numerisk løsning av likning 1', fontsize=25)
plt.xlabel('x(t)', fontsize=20)
plt.ylabel('y(t)', fontsize=20)
plt.plot(x_list, y_list)
plt.show()




#analytisk løsning
L=100
alpha=5.0E-5
m=1.0E-2
k=alpha/m
w=2*np.pi/(24*3600)*k

A=np.array([[0, 0, 1, 0], [0, 0, 0, 1], [0, -w, -k, 0], [w, 0, 0, -k]])

lamb, V = np.linalg.eig(A)
X_0 = np.array([L, 0., 0., 0.])
C = np.linalg.solve(V, X_0)

t = 48*3600 #sek
X = V.dot(C*np.exp(lamb*t))
print('Prosisjon etter 48 timer, analytisk: ', X[0].real, X[1].real)
print('Posisjon etter 48 timer, numerisk: ', x_list[-1], y_list[-1])