% Sistemas de Controle - Professor Adriano Siqueira
% Victor Tamassia Noppeney - Nº USP: 9311473

% Representação da saída de um sistema em termos dos componentes de Fourier da entrada

clear variables;
close all;
clc;

G = tf(3, [1 1 3]); % Sistema
f_sig = 1/20; % Frequência do sinal de entrada [Hz]
f_samp = 100*f_sig; % Frequência de amostragem [Hz]
t_tot = 100; % Tempo total [s]
t = 0:1/f_samp:t_tot-1/f_samp; % Vetor de tempo [s]
L = length(t); % Número de pontos de amostragem
f = f_samp*(0:(L/2))/L; % Vetor de frequências

u_sq = square(2*pi*f_sig*t); % Entrada: onda quadrada

figure('units','normalized','outerposition',[0 0 1 1]); % Janela maximizada
subplot(2, 2, 1);
plot(t, u_sq, 'b'); % Gráfico da entrada
hold on;
y_sq = lsim(G, u_sq, t);
plot(t, y_sq, 'r'); % Gráfico da saída
title('Entrada e saída originais');
xlabel('Tempo [s]');
ylabel('Amplitude');
legend('Entrada', 'Saída');
ylim([-2 2]);

Y = fft(u_sq); % Fast Fourier Transform
Y_ss = 2*Y(1:L/2+1); % Espectro "single-sided"

AMP = abs(Y_ss/L); % Amplitudes de Fourier
[pks, locs] = findpeaks(AMP); % Picos nos múltiplos da frequência fundamental

PHI = phase(Y_ss); % Fases de Fourier
PHI(AMP < min(pks)) = 0; % Limpar o gráfico

[mag,phase] = bode(G,f*2*pi);
for i=1:length(mag)
modulo(i) = mag(:,:,i);
fase(i) = phase(:,:,i);
end
    
subplot(2,2,3);
title('Transformada de Fourier da entrada');
%yyaxis left;
%[hAx] = 
plot(f, modulo); % Gráfico de amplitudes de Fourier
hold on
%ylabel('Amplitude');
%yyaxis right;
plot(f, fase*pi/180); % Gráfico de fases de Fourier
%ylabel(hAx(1),'Módulo ');
%ylabel(hAx(2),'Fase [rad]');
xlabel('Frequência [Hz]');
%ylim(hAx(1),[-2 2]);
ylim([-2 2]);


u_super = zeros(1, L); % Superposição dos componentes de entrada
y_super = zeros(1, L); % Superposição dos componentes de saída
for i = 1:length(pks)
    u = pks(i)*cos(2*pi*f(locs(i))*t + PHI(locs(i))); % Componente de entrada
    u_super = u_super + u; % Superposição das entradas
    subplot(2,2,2);
    plot(t, u_super, 'b'); % Gráfico da entrada superposta
    hold on;
    plot(t, u, 'c'); % Gráfico da entrada atual
    hold off;
    title('Reconstrução da entrada');
    xlabel('Tempo [s]');
    ylabel('Amplitude');
    legend('Superposição', 'Componente de frequência');
    ylim([-2 2]);
    y = lsim(G, u, t); % Saída relativa à componente atual
    y_super = y_super + y'; % Superposição das saídas
    subplot(2,2,4);
    plot(t, y_super, 'r'); % Gráfico da saída superposta
    hold on;
    plot(t, y, 'm'); % Gráfico da saída atual
    hold off;
    title('Construção da saída');
    xlabel('Tempo [s]');
    ylabel('Amplitude');
    legend('Superposição', 'Componente de frequência');
    
    subplot(2,2,3);
    hold off
    plot(f, modulo); % Gráfico de amplitudes de Fourier
hold on
plot(f, fase*pi/180);
    plot(f(locs(i)), modulo(locs(i)),'*');
    
    pause;
end