# -*- coding: utf-8 -*-
"""
Created on Thu Aug 26 12:12:10 2021

@author: 9706809E
"""

import numpy
import matplotlib.pyplot as plt
import math
import pandas as pd
import numpy as np
import time

plt.close('all')

factor = 1 / 3
w2 = (2 * numpy.pi * factor)**2

def f_air(v):
    mass = 430e3
    A = 0.6325
    B = 7.55e-3
    C = 1.244e-4
    alpha = mass*A*1e-2 # N
    beta = mass*B*1e-2*3.6 # N/(m/s)
    gamma = mass*C*1e-2*(3.6**2) # N/(m/s)**2
    force_friction = alpha + beta*v + gamma*v**2
    return force_friction


def f(v):
    traction_df = pd.read_csv ('TRACTION_2TGVDASYEUM.csv')
    traction_data = traction_df.to_numpy()
    effort_speed_v = (1/3.6)*traction_data[:, 0]
    effort_speed_F = traction_data[:, 2]
    force_motor = np.interp(v,effort_speed_v,effort_speed_F)
    return force_motor

def oscillateur(Y, t):
    mass = 430e3
    return numpy.array([Y[1], (f(Y[1]) - f_air(Y[1]))/mass])

def pas_euler(system, h, tn, Yn):
    deriv = system(Yn, tn)
    return Yn + h * deriv

def pas_euler_cauchy(system, h, tn, Yn):
    deriv = system(Yn, tn)
    Y_mid = Yn + h * deriv
    deriv_mid = system(Y_mid, tn + h)
    return Yn + (h / 2) * (deriv + deriv_mid)

def pas_RK4(system, h, tn, Yn):
    k1 = system(Yn, tn)
    k2 = system(Yn + (h/2) * k1, tn + h/2)
    k3 = system(Yn + (h/2) * k2, tn + h/2)
    k4 = system(Yn + h * k3, tn + h)
    return Yn + (h/6) * (k1 + 2 * k2 + 2 * k3 + k4)

def pas_adams(system, h, tn, Yn, tn_1, Yn_1):
    deriv_n = system(Yn, tn)
    deriv_n_1 = system(Yn_1, tn_1)
    return Yn + (h/2) * (3 * deriv_n - deriv_n_1)

def pas_adams_3(system, h, tn, Yn, tn_1, Yn_1, tn_2, Yn_2):
    deriv_n = system(Yn, tn)
    deriv_n_1 = system(Yn_1, tn_1)
    deriv_n_2 = system(Yn_2, tn_2)
    return Yn + (h/12) * (23 * deriv_n - 16 * deriv_n_1 + 5 * deriv_n_2)

def euler(system, Yi, T, h):
    Y = Yi
    t = 0.0
    liste_y = [Y]
    liste_t = [t]
    while t<T-h:
        Y = pas_euler(system, h, t, Y)
        t += h
        liste_y.append(Y)
        liste_t.append(t)
    return (numpy.array(liste_t), numpy.array(liste_y))

def euler_cauchy(system, Yi, T, h):
    Y = Yi
    t = 0.0
    liste_y = [Y]
    liste_t = [t]
    while t<T-h:
        Y = pas_euler_cauchy(system, h, t, Y)
        t += h
        liste_y.append(Y)
        liste_t.append(t)
    return (numpy.array(liste_t), numpy.array(liste_y))


def RK4(system, Yi, T, h):
    Y = Yi
    t = 0.0
    liste_y = [Y]
    liste_t = [t]
    while t<T-h:
        Y = pas_RK4(system, h, t, Y)
        t += h
        liste_y.append(Y)
        liste_t.append(t)
    return (numpy.array(liste_t), numpy.array(liste_y))

def adams(system, Yi, T, h):
    Y0 = Yi
    t = 0.0
    tableau_y = numpy.array([Y0])
    tableau_t = numpy.array([t])
    Y1 = pas_euler(system, h, t, Y0)
    t += h
    tableau_y = numpy.append(tableau_y, [Y1], axis=0)
    tableau_t = numpy.append(tableau_t, t)
    while t<T-h:
        Y2 = pas_adams(system, h, t, Y1, t - h, Y0)
        t += h
        tableau_y = numpy.append(tableau_y, [Y2], axis=0)
        tableau_t = numpy.append(tableau_t, t)
        Y0 = Y1
        Y1 = Y2
    return (tableau_t, tableau_y)

def adams_3(system, Yi, T, h):
    Y0 = Yi
    t = 0.0
    tableau_y = numpy.array([Y0])
    tableau_t = numpy.array([t])
    Y1 = pas_euler(system, h, t, Y0)
    t += h
    tableau_y = numpy.append(tableau_y, [Y1], axis=0)
    tableau_t = numpy.append(tableau_t, t)
    Y2 = pas_euler(system, h, t, Y1)
    t += h
    tableau_y = numpy.append(tableau_y, [Y2], axis=0)
    tableau_t = numpy.append(tableau_t, t)
    while t<T-h:
        Y3 = pas_adams_3(system, h, t, Y2, t - h, Y1, t - 2 * h, Y0)
        t += h
        tableau_y = numpy.append(tableau_y, [Y3], axis=0)
        tableau_t = numpy.append(tableau_t, t)
        Y0 = Y1
        Y1 = Y2
        Y2 = Y3
    return (tableau_t, tableau_y)


T = 1000.0
h = 1.0
Yi = [0, 0]
start = time.time()
(t_ref, tab_y_ref) = euler(oscillateur, Yi, T, 1e-1)
end = time.time()
print("Ref : %f"%(end-start))
start = end
(t, tab_y) = euler(oscillateur, Yi, T, h)
end = time.time()
print("Euler : %f"%(end-start))
start = end
(t_c, tab_y_c) = euler_cauchy(oscillateur, Yi, T, h)
end = time.time()
print("Euler Cauchy: %f"%(end-start))
start = end
(t_rk4, tab_y_rk4) = RK4(oscillateur, Yi, T, h)
end = time.time()
print("RK4 : %f"%(end-start))
start = end
(t_ad, tab_y_ad) = adams(oscillateur, Yi, T, h)
end = time.time()
print("Adams : %f"%(end-start))
start = end
(t_ad3, tab_y_ad3) = adams_3(oscillateur, Yi, T, h)
end = time.time()
print("Adams 3 : %f"%(end-start))
start = end
x_ref, v_ref = tab_y_ref[:,0], tab_y_ref[:,1]
x, v = tab_y[:, 0], tab_y[:, 1]
x_c, v_c = tab_y_c[:, 0], tab_y_c[:, 1]
x_rk4, v_rk4 = tab_y_rk4[:, 0], tab_y_rk4[:, 1]
x_ad, v_ad = tab_y_ad[:, 0], tab_y_ad[:, 1]
x_ad3, v_ad3 = tab_y_ad3[:, 0], tab_y_ad3[:, 1]

plt.figure(figsize=(15, 9))
plt.plot(t_ref, x_ref, label='ref', color='k')
plt.plot(t, x, label='euler 1')
plt.plot(t_c, x_c, label='euler cauchy')
plt.plot(t_rk4, x_rk4, label='RK4')
plt.plot(t_ad, x_ad, label='adams 2')
plt.plot(t_ad3, x_ad3, label='adams 3')
plt.xlabel('time')
plt.ylabel('distance')
plt.title('Distance vs time - h=%f'%(h))
plt.grid()
plt.legend(loc='upper right')

plt.figure(figsize=(15, 9))
plt.plot(t_ref, v_ref, label='ref', color='k')
plt.plot(t, v, label='euler 1')
plt.plot(t_c, v_c, label='euler cauchy')
plt.plot(t_rk4, v_rk4, label='RK4')
plt.plot(t_ad, v_ad, label='adams 2')
plt.plot(t_ad3, v_ad3, label='adams 3')
plt.xlabel('time')
plt.ylabel('speed')
plt.title('Speed vs time - h=%f'%(h))
plt.grid()
plt.legend(loc='upper right')

erreur = x - np.interp(t, t_ref, x_ref)
erreur_c = x_c - np.interp(t_c, t_ref, x_ref)
erreur_rk4 = x_rk4 - np.interp(t_rk4, t_ref, x_ref)
erreur_ad = x_ad - np.interp(t_ad, t_ref, x_ref)
erreur_ad3 = x_ad3 - np.interp(t_ad3, t_ref, x_ref)
plt.figure(figsize=(15, 9))
plt.plot(x/1e3, abs(erreur), label='euler 1')
plt.plot(x_c/1e3, abs(erreur_c), label='euler cauchy')
plt.plot(x_rk4/1e3, abs(erreur_rk4), label='RK4')
plt.plot(x_ad/1e3, abs(erreur_ad), label='adams 2')
plt.plot(x_ad3/1e3, abs(erreur_ad3), label='adams 3')
plt.title("Errors of integration methods over a train's simulation (time step = %3.1fs)"%(h), fontsize=15)
plt.yscale("log")
plt.xlabel('Distance travelled by the train (km)', fontsize=13)
plt.ylabel('Absolute error on the position of the train (m)', fontsize=13)
plt.grid()
plt.legend(loc='lower right')
plt.show()