# ============================================================
# UFSC - Federal University of Santa Catarina
# UNESP - State University of São Paulo
# Author: G2 Power
# Date: October 2025
# Title: CPT-based Multifunctional Grid-Tied Inverter with Type II Controller
# ============================================================

import numpy as np
import matplotlib.pyplot as plt
import control as ctrl

# ====== Parâmetros ======
f = 50
T = 1 / f
Vdc = 225
Lf = 4.8e-3
Rf = 0.01
Hi = 1 / 10
Fs = 25e3
fc = Fs / 10
MFd = 60

ts = 1 / Fs

# ====== Função de transferência da planta ======
s = ctrl.TransferFunction.s
Gi = Vdc / (s * Lf + Rf)
FTMAu = Gi * Hi

# ====== Diagrama de Bode (Malha aberta não controlada) ======
ctrl.bode(FTMAu, dB=True, Hz=True, omega_limits=(0.001, 1e5), omega_num=1000)
plt.title("Uncontrolled Open Loop Transfer Function")
plt.grid(True, which="both")
plt.show()

# ====== Projeto do controlador tipo II ======
inst = ctrl.evalfr(FTMAu, 2j * np.pi * fc)
fase_fc = np.degrees(np.angle(inst))
ganho_fc = 20 * np.log10(abs(inst))
ganho_real = 10 ** (abs(ganho_fc) / 20)

alpha = MFd - fase_fc - 90
K2 = np.tan(np.radians(alpha / 2 + 45))

Prod = 1 / (2 * np.pi * fc * ganho_real * K2)
C2 = 100e-9
R1 = Prod / C2
C1 = C2 * ((K2 ** 2) - 1)
R2 = K2 / (2 * np.pi * fc * C1)

Ctipo2 = (1 + s * R2 * C1) / (s * R1 * (C1 + C2 + s * R2 * C1 * C2))

# ====== Bode do controlador ======
ctrl.bode(Ctipo2, dB=True, Hz=True, omega_limits=(0.001, 1e5))
plt.grid(True, which="both")
plt.show()

# ====== Malha aberta com controle ======
FTMAc = FTMAu * Ctipo2
ctrl.bode(FTMAc, dB=True, Hz=True, omega_limits=(0.001, 1e5))
plt.grid(True, which="both")
plt.show()

inst2 = ctrl.evalfr(FTMAc, 2j * np.pi * fc)
MF_obtida = np.degrees(np.angle(inst2)) + 180
ganho_obtido = 20 * np.log10(abs(inst2))
ganho_real_obtido = 10 ** (abs(ganho_obtido) / 20)

# ====== Discretização do controlador tipo II ======
beta = 4 * R1 * R2 * C1 * C2 + 2 * ts * R1 * (C1 + C2)

b0 = (ts ** 2 + 2 * ts * C1 * R2) / beta
b1 = (2 * ts ** 2) / beta
b2 = (ts ** 2 - 2 * ts * C1 * R2) / beta

a0 = 1
a1 = (-8 * R1 * R2 * C1 * C2) / beta
a2 = (4 * R1 * R2 * C1 * C2 - 2 * ts * R1 * (C1 + C2)) / beta

b = [b0, b1, b2]
a = [a0, a1, a2]

H = ctrl.TransferFunction(b, a, ts)

# ====== Comparação entre o controlador analógico e digital ======
ctrl.bode(H, dB=True, Hz=True, omega_limits=(0.001, 1e5))
ctrl.bode(Ctipo2, dB=True, Hz=True, omega_limits=(0.001, 1e5))
plt.grid(True, which="both")
plt.show()

# ====== Função de transferência de malha fechada ======
FTMF = FTMAc / (1 + FTMAc)
ctrl.bode(FTMF, dB=True, Hz=True, omega_limits=(0.001, 1e5))
plt.grid(True, which="both")
plt.show()
