# -*- coding: utf-8 -*-
"""
Created on Thu Mar 27 12:48:39 2025

@author: fabio
"""

#from scipy import optimize as opt
from scipy import signal as sig
import control as ctrl
from matplotlib import pyplot as plt
import numpy as np

R = 1000  # 1kOhm
C = 1e-9  # 1nF
Om_c = 1/(R*C)
Om_s = 1e7
T = 1/Om_s
N = 1e3

alpha = Om_s/Om_c
b = 1/(alpha+1)
aa = alpha*b
a = [-aa, 1]
sys_est = ctrl.tf(b, a, T)
print(sys_est)

omega = np.arange(1, Om_s/2, N)
s=1j*omega
trfs=Om_c/(s+Om_c)
mags=abs(trfs)
angs=np.angle(trfs)
resp, phase,omega = sys_est.frequency_response(omega)
#phase=phase*np.pi/180
fig, ax = plt.subplots(2, 1)
#resp.plot(initial_phase=0, ax=ax)
ax[0].semilogx(omega,resp,label='z')

ax[0].semilogx(omega,mags, label='s')
ax[0].legend()
# ctrl.bode_plot(data, fmt, kwargs)
ax[1].semilogx(omega, phase)

ax[1].semilogx(omega, angs)
plt.grid(True,'both')

plt.loglog(omega,resp)
plt.grid(True,'both')

# verifica


t = 1/omega
x = np.random.normal(size=len(t))
y = np.zeros(len(omega))

for n in np.arange(1, len(omega)):
    y[n] = a[0]*x[n]+b*y[n-1]


sample_rate = Om_s/(2*np.pi)  # Hz
segment_duration = sample_rate/N  # s
# N=segment_duration*sample_rate
frequency_resolution = 1/(segment_duration)
overlap_factor = 0.5
window_fftgram = "hann"
f1, Pxy = sig.csd(x, y, fs=sample_rate, nperseg=N, window='hann')
f2, Pxx = sig.welch(x, fs=sample_rate,  window=window_fftgram, nperseg=N,
                    noverlap=N*overlap_factor)
H = Pxy/Pxx
half = int(N // 2)
omega1 = 2 * np.pi * f1       # List of frequencies [rad/sec]
# omega1=f
H = H[:half]
omega1 = omega1[:half]
f = f2[:half]
# Generate true frequency response
# mag, phase, _ = control.bode(sys, omega, plot=False)

# Get frequency response from estimated FRF
mag2 = abs(H)
phase2 = np.angle(H)
rom = omega.view()
figure, ax = plt.subplots(1, 1)
ax.semilogx(f, mag2, 'o-', label='H from diff. Equation ')
ax.semilogx(rom[:half], abs(resp)[:half],label='H from theory')
ax.grid(True, 'both')
ax.legend()