import numpy as np
import matplotlib.pyplot as plt
import scipy.io.wavfile as wavfile
from  scipy.signal import savgol_filter
import matplotlib.pyplot as plt
import matplotlib as mpl
import numpy as np

from scipy.optimize import curve_fit

from src import coil_class as BC

plt.rcParams["font.size"] = 12
samplingrateinput, GradientTenHzinput = wavfile.read("Z:/Users/Lenny/Power Supply/TimeSeriesOP549/FinalMeasurements/SquareWave10HzGradientCoilsWithPIinput.wav")
GradientTenHzinput =  GradientTenHzinput*0.4/(2**14) /1.2093 +0.00247
samplingratesensingNoPI, GradientTenHzsensingNoPI = wavfile.read("Z:/Users/Lenny/Power Supply/TimeSeriesOP549/FinalMeasurements/SquareWave10HzGradientCoilsNoPIsensing.wav")
GradientTenHzsensingNoPI =  GradientTenHzsensingNoPI*0.4/(2**14)/1.2093 +0.00247
samplingratesensingWithPI, GradientTenHzsensingWithPI = wavfile.read("Z:/Users/Lenny/Power Supply/TimeSeriesOP549/FinalMeasurements/SquareWave10HzGradientCoilsWithPIsensing.wav")
GradientTenHzsensingWithPI =  GradientTenHzsensingWithPI*0.4/(2**14)/1.2093 +0.00247


t1 = np.arange(-199875/samplingrateinput, (len(GradientTenHzinput)-199876)/samplingrateinput, 1/samplingrateinput)*1000

plt.plot(t1[115491::],GradientTenHzsensingWithPI[115491::],label="With PI-controller")
#plt.plot(t1[115491::],GradientTenHzsensingNoPI[100000:-15491:],label="Without PI-controller")
plt.plot(t1[115491::],GradientTenHzinput[115491::],label="Input signal")
#plt.plot(t1[115491::],savgol_filter((GradientTenHzinput[115491::]-GradientTenHzsensingWithPI[115491::])/np.mean(GradientTenHzinput[0:1000]),50,3))
#plt.plot(t1[115491::],savgol_filter((GradientTenHzinput[115491::]-GradientTenHzsensingNoPI[100000:-15491:])/np.mean(GradientTenHzinput[0:1000]),50,3))
#plt.plot(t1[115491::],GradientTenHzinput[115491::]/np.mean(GradientTenHzinput[0:1000]))
#plt.xlim(t1[115491],t1[-1])

#plt.ylim(0.3, 0.65)
#plt.xlim(-0.05,0.3)
#plt.xlim(-0.05, 0.3)
plt.grid("both")
plt.xlabel("Time [ms]")
plt.ylabel("Current [A]")
handles, labels = plt.gca().get_legend_handles_labels()
order = [1,0]
plt.legend([handles[idx] for idx in order],[labels[idx] for idx in order], loc="upper right")
plt.show()

sampligrateinput, OffsetTenHzinput = wavfile.read("Z:/Users/Lenny/Power Supply/TimeSeriesOP549/FinalMeasurements/SquareWave10HzOffsetCoilsWithPIinput.wav")
OffsetTenHzinput =  OffsetTenHzinput*0.4/(2**14)/1.2093 +0.00247
sampligratesensingNoPI, OffsetTenHzsensingNoPI = wavfile.read("Z:/Users/Lenny/Power Supply/TimeSeriesOP549/FinalMeasurements/SquareWave10HzOffsetCoilsNoPIsensing.wav")
OffsetTenHzsensingNoPI =  OffsetTenHzsensingNoPI*0.4/(2**14)/1.2093 +0.00247
sampligratesensingWithPI, OffsetTenHzsensingWithPI = wavfile.read("Z:/Users/Lenny/Power Supply/TimeSeriesOP549/FinalMeasurements/SquareWave10HzOffsetCoilsWithPIsensing.wav")
OffsetTenHzsensingWithPI =  OffsetTenHzsensingWithPI*0.4/(2**14)/1.2093 +0.00247

t2 = np.arange(-199069/samplingrateinput, (len(OffsetTenHzinput)-199070-833)/samplingrateinput, 1/samplingrateinput)*1000

plt.plot(t2[100000:],OffsetTenHzsensingWithPI[100833:], label="With PI-controller")
plt.plot(t2[100000:],OffsetTenHzsensingNoPI[100000:-833], label = "Without PI-controller")
plt.plot(t2[100000:],OffsetTenHzinput[100833:], label="Input signal")
#plt.plot(t2[100000:-150000],OffsetTenHzinput[100833:-150000]/np.mean(OffsetTenHzinput[0:1000]))
#plt.plot(t2[100000:-150000],savgol_filter((OffsetTenHzinput[100833:-150000]-OffsetTenHzsensingWithPI[100833:-150000])/np.mean(OffsetTenHzinput[0:1000]),50,3))
#plt.plot(t2[100000:-150000],savgol_filter((OffsetTenHzinput[100833:-150000]-OffsetTenHzsensingNoPI[100000:-150833])/np.mean(OffsetTenHzinput[0:1000]),50,3))

# My data
t = np. linspace (0,0.003,1000)
U_0 = 30
plt.plot(t * 1e3, I_t_cut_vec(t, int_HH_Coil, I, U_0) - 0.4, label=f"U_0 = {U_0} V, U_end = 1.5 V")
plt.plot(t * 1e3, 0.8 * I_current(int_HH_Coil, I, t) - 0.4, label=f"U_0 = U_end = 1.5 V")
#plt.ylim(0.3, 0.65)
# plt.xlim(-0.05,0.3)
plt.xlim(-0.05, 4 )
plt.grid("both")
plt.xlabel("Time [ms]")
plt.ylabel("Current [A]")
handles, labels = plt.gca().get_legend_handles_labels()
order = [1,0]
plt.legend([handles[idx] for idx in order],[labels[idx] for idx in order], loc="upper right")
plt.show()

# %%
def I_current(Coil, I_0, t):
    L = Coil.induct_perry()*2

    R = Coil.resistance(22.5)*2
    print(f"L={L}")
    print(f" R= {R}")
    tau = L / R
    print(f" τ = {tau}")
    I =  I_0 * (1 - np.exp(-R / L * t))
    return I

def I_t_cut(time, coil, I_end, U_0):
    R = 2 * coil.resistance(22)
    #print(f"resistance I_cut = {R} Ω")
    I =  U_0 / R * (1 - np.exp(- time / coil.tau()))
    if I >= I_end:
        I = I_end
    return I

I_t_cut_vec = np.vectorize(I_t_cut)
# %%
my_colors = {'light_green': '#97e144',
             'orange': '#FF914D',
             'light_grey': '#545454',
             'pastel_blue': '#1b64d1',
             'light_blue': '#71C8F4',
             'purple': '#7c588c',
             'pastel_red': '#D42024'}
#%%
mpl.rcParams['xtick.direction'] = 'in'
mpl.rcParams['ytick.direction'] = 'in'

mpl.rcParams['xtick.top'] = True
mpl.rcParams['ytick.right'] = True

mpl.rcParams['xtick.major.size'] = 10
mpl.rcParams['xtick.major.width'] = 3
mpl.rcParams['xtick.minor.size'] = 10
mpl.rcParams['xtick.minor.width'] = 3
mpl.rcParams['ytick.major.size'] = 10
mpl.rcParams['ytick.major.width'] = 3
mpl.rcParams['ytick.minor.size'] = 10
mpl.rcParams['ytick.minor.width'] = 3


mpl.rcParams.update({'font.size': 22, 'axes.linewidth': 3, 'lines.linewidth': 3})


# %%
HH_Coil = BC.BCoil(HH = 1, distance = 54, radius = 48, layers = 8, windings = 8, wire_height = 0.5,
                   wire_width = 0.5, insulation_thickness = (0.546-0.5)/2, is_round = True,
                   winding_scheme= 2)
HH_Coil.set_R_inner(45.6)
HH_Coil.set_d_min(2*24.075)

int_HH_Coil = BC.BCoil(HH=1, distance=79.968, radius=80.228, layers=8, windings=8, wire_height=0.5,
                       wire_width=0.5, insulation_thickness=0.046 / 2, is_round=True,
                       winding_scheme=2)


fig, ax1 = plt.subplots(figsize=(11, 8))
ylim = (0, 11.5)
t = np.linspace(0, 0.002, 10000)
i_to_B_inter = int_HH_Coil.max_field(1)
fig, ax = plt.subplots(figsize = (11,7))
#fig.suptitle(f"Time response HH-coil: I_max = {I} A --> Max Field = {Max_field:.2f} G \n \n I(t) = U(t) / R * (1 - exp(- R/L * t))")
ax.set_title("Time Response Intermediate Coils", y = 1.05)

# ax.text(0.6, 5, r'$I(t) = \frac{U(t)}{R} - \frac{L}{R} \cdot \frac{dI(t)}{dt} $', fontsize=34)
I = 0.8
ax.plot(t * 1e3, i_to_B_inter * (I_current(int_HH_Coil, I, t) - I / 2), label=f"Calculation without PI", zorder=1, color=my_colors['pastel_blue'])
U_0 = 28
ax.plot(t * 1e3, i_to_B_inter * (I_t_cut_vec(t, int_HH_Coil, I, U_0) - I / 2), label=f"Calculation with PI", zorder=1, color=my_colors['light_blue'])

i_to_B = HH_Coil.max_field(1)
I2 = 1
#ax.plot(t * 1e3, i_to_B * (I_t_cut_vec(t, HH_Coil, I2, U_0) - I2 / 2), label=f"Intermediate coils: calculation with PI", zorder=1, color=my_colors['light_blue'])
#plt.vlines(3.1e-2, 0, 10.64, zorder=2, linestyles=(0, (1.5, 3.06)),color=my_colors['orange'], label='t = 30 μs')
# for scaling in np.arange(2,5,0.5):
#     ax.plot(t * 1e3, I_t_exp(t, AHH_Coil, I, 15, scaling), label=f"Exponential decay U")


#ax.plot(t*1e3, i_to_B_inter * I_current_fit(t*1e3, *popt))
#data
plt.plot(t2[100000:],i_to_B_inter * OffsetTenHzsensingWithPI[100833:], label="Measurement with PI", color=my_colors['pastel_red'])
plt.plot(t2[100000:],i_to_B_inter * OffsetTenHzsensingNoPI[100000:-833], label = "Measurement without PI", color = my_colors['orange'])

ax.set_xlabel("time [ms]")
ax.set_ylabel("Magnetic field [G]")
ax.set_ylim(-5,5)
ax.set_xlim(-0.09,1.8)
ax.legend()

plt.show()

# %% Fit
def I_current_fit(t, L1, L2, I_0, Coil=int_HH_Coil):
    # L = Coil.induct_perry()*2
    t_s = t * 1e-3

    R = Coil.resistance(22.5)*2
    #print(f"L={L}")
    #print(f" R= {R}")
    #tau = L / R
    #print(f" τ = {tau}")
    I = I_0 * ((1 - np.exp(-R / L1 * t_s))+ (1 - np.exp(-R / L2 * t_s)))
    return I-I_0/2

# %%
def I_current_fit(t, L1, I_0, Coil=int_HH_Coil):
    # L = Coil.induct_perry()*2
    t_s = t * 1e-3

    R = Coil.resistance(22.5)*2
    #print(f"L={L}")
    #print(f" R= {R}")
    #tau = L / R
    #print(f" τ = {tau}")
    I = I_0 * (1 - np.exp(-R / L1 * t_s))
    return I-I_0/2
# %%

p0 = [20e-3 , 0.8]
#bounds = ([-50, 0], [0, 5])
lim1 = 1.994e5
lim2 = 2.5e5
lim1 = int(lim1)
lim2 = int(lim2)
t_plot = np.copy(t2)
t_plot = t_plot[lim1:lim2]
I_plot = np.copy(OffsetTenHzsensingNoPI)

I_plot = I_plot[lim1:lim2]

print(t_plot)
print(I_plot)
popt, pcov = curve_fit(I_current_fit, t_plot, I_plot, p0=p0)
print(popt)

# %%
t = np.linspace(0,20, 10000)
#popt = [0.02,0.8]
fig, ax = plt.subplots(figsize = (11,7))
ax.plot(t, I_current_fit(t, *popt))

ax.plot(t2[100000:], OffsetTenHzsensingNoPI[100000:-833], label = "Intermediate coil: without PI", color = my_colors['orange'])
#ax.plot(t_plot, I_plot, label = "Intermediate coil: without PI", color = my_colors['orange'])

# ax.vlines(t2[lim1], -0.5, 0.5)
ax.vlines(t2[lim2], - 0.5, 0.5)
ax.set_xlabel("time [ms]")
ax.set_ylabel("Current [A]")
#ax.set_ylim(-0.5, 0.5)
ax.set_xlim(-0.09,5)
ax.legend()

plt.show()