"""
Simulator of a dynamic positioning system
Selectable: (1) Feedback with PID. (2) Feedback + feedforward.
Author: Finn Aakre Haugen, finn@techteach.no
Updated 2023 09 02
"""

#%% Importing packages

import matplotlib.pyplot as plt
import numpy as np

#%% Model parameters

m = 71164*1000  # [kg]
Dh = 8.4*1000  # [N/(m/s)^2]
Dw = 0.177*1000  # [N/(m/s)^2]
Fp_max = 552000  # [N]
Fp_min = -467000  # [N]

#%% PID control params (using the Skogestad method)

Tc = 200  # Closed loop time constant [s]
Kii = 1/m  # Gain of double integrator process
Kc = 2/(Kii*Tc*Tc)  # Gain [N/m]
Ti = 4*Tc  # Integral time [s]
Td = 1*Tc  # Derivative gain [s]
u_man = 0  # Manual control signal [N]

#%% Simulation time settings

dt = 0.1  # [s]
t_start = 0  # [s]
t_stop = 5000  # [s]
N_sim = int((t_stop - t_start)/dt) + 1

#%% Preallocation of arrays for plotting
    
t_array = np.linspace(t_start, t_stop, N_sim)
k_array = np.arange(0, N_sim)
r_array = np.zeros(N_sim)
x1_array = np.zeros(N_sim)
x2_array = np.zeros(N_sim)
Fp_array = np.zeros(N_sim)
Vw_array = np.zeros(N_sim)
Fw_array = np.zeros(N_sim)
uff_array = np.zeros(N_sim)

#%% Initialization
    
x1_init = 0
x2_init = 0
x1_k = x1_init
x2_k = x2_init
e_km1 = 0
ui_km1 = 0
r_km1 = 0
x2_r_km1 = 0

#%% Simulation settings

K_ff = 1  # 0 = Only feedback. 1 = feedback + feedforward.
r0 = 40  # Constant pos ref [m]
A_r = 20  # Pos ref sine ampl [m]
P_r = 1000  # Pos ref sine period  [s]
Vw = -20  # Wind speed [m/s]

#%% Simulation loop

for k in k_array:

    t_k = dt*k

    # Excitations:
    if t_start <= t_k < 500:
        r_k = A_r*(1-np.cos((2*np.pi/P_r)*t_k))
        uc_k = 0
        Vw_k = 0
    if 500 <= t_k < 2500:
        r_k = r0
        uc_k = 0
        Vw_k = 0
    if 2500 <= t_k <= t_stop:
        r_k = r0
        uc_k = 0
        Vw_k = Vw

    # Calculation of references of speed and acceleration:
    x2_r_k = (r_k - r_km1)/dt
    dx2_r_k = (x2_r_k - x2_r_km1)/dt

    # Feedforward control signal:
    Fh_ff_k = Dh*(uc_k - x2_r_k)*np.abs(uc_k - x2_r_k)
    Fw_ff_k = Dw*(Vw_k - x2_r_k)*np.abs(Vw_k - x2_r_k)

    uff_k = m*dx2_r_k - Fh_ff_k - Fw_ff_k

    # PID controller + feedforward:
    e_k = r_k - x1_k
    up_k = Kc*e_k
    ui_k = ui_km1 + (Kc/Ti)*dt*e_k
    ud_k = Kc*Td*(e_k - e_km1)/dt
    u_k = u_man + up_k + ui_k + ud_k + K_ff*uff_k
    u_k = np.clip(u_k, Fp_min, Fp_max)

    # Forces:
    Fp_k = u_k
    Fh_k = Dh*(uc_k - x2_k)*np.abs(uc_k - x2_k)
    Fw_k = Dw*(Vw_k - x2_k)*np.abs(Vw_k - x2_k)

    # Arrays for plotting:
    t_array[k] = t_k
    r_array[k] = r_k
    x1_array[k] = x1_k
    x2_array[k] = x2_k
    Fp_array[k] = Fp_k
    Vw_array[k] = Vw_k
    Fw_array[k] = Fw_k
    uff_array[k] = uff_k

    # Time derivatives:
    dx1_k = x2_k
    dx2_k = (1/m)*(Fp_k + Fh_k + Fw_k)

    # State prediction (Euler step):
    x1_kp1 = x1_k + dx1_k*dt
    x2_kp1 = x2_k + dx2_k*dt

    # Time shift:
    x1_k = x1_kp1
    x2_k = x2_kp1
    ui_km1 = ui_k
    e_km1 = e_k
    r_km1 = r_k
    x2_r_km1 = x2_r_k

#%% Plotting

plt.close('all')  # Closes all figures before plotting
plt.figure(1)

plt.subplot(2, 1, 1)
plt.plot(t_array, r_array, 'r', label='r')
plt.plot(t_array, x1_array, 'b', label='y')
plt.legend()
plt.grid()
plt.ylabel('[m]')

plt.subplot(2, 1, 2)
plt.plot(t_array, Fp_array/1000, 'g', label='Fp')
plt.plot(t_array, Fw_array/1000, 'm', label='Fw')
plt.legend()
plt.grid()
plt.ylabel('[kN]')
plt.xlabel('t [s]')

#%% Saving the plot figure as pdf file

if K_ff == 0:
    plt.savefig('plot_sim_dynpos_feedback.pdf')
else:
    plt.savefig('plot_sim_dynpos_feedforward.pdf')

plt.show()