%% Phasenauswertung mittels FFT
clc; clear; close all;

f1 = 50; %Frequenz Signal1 [Hz]
f2 = f1; %Frequenz Signal2 [Hz]
a1 = 5; %Amplitude Signal1 [V]
a2 = 7; %Amplitude Signal2 [V]
dPhi = pi/10; %Phasenverschiebung von sig2
control_angle = 120;  %Steuerwinkel [°]
helpus = control_angle; %Hilfsvariable


control_angle = 100/180*control_angle;  %180 entspricht 100% einer Periode
diffControlTime = control_angle/(f1*pi) %Zündverzögerung auf der Zeitachse in [s]


diffPhaseTime = dPhi/(f1*2*pi)  %Wert der Phasenverschiebung auf der Zeitachse [s]

fs = 2^18; %Abtastrate
nfft = 2^18; %FFT-Blockgröße
fftRes = fs/nfft;

tmax = 100e-3; %Signaldauer [s]

Ts = 1/fs; %Periodendauer Sampling
t(:,1) = 0:Ts:tmax;

%%Signalerzeugung
sig1 = a1*sin(2*pi*f1*t);   %Signal 1
sig2 = a2*sin(2*pi*f2*t+dPhi);  %Signal 2
sig3 = 1*sin(2*pi*f1*tdPhi); %Signal 3


tcalc = (tmax/20e-3)*2; %Anzahl der NULL zu setztenden Abschnitte berechenen

help = 100/control_angle;   
time_calc = round(length(t)/(help*tcalc));   %Zeit pro Periode zum NULLsetzten berechenen

if(control_angle > 0)
for i = 1:1:time_calc
sig1(i,1) = 0;
sig3(i,1) = 0; 
end

for c = 1:1:(tcalc-1)  
for i = (c*round(length(t)/tcalc)):1:(time_calc+c*(round(length(t)/tcalc)))
sig1(i,1) = 0;
sig3(i,1) = 0; 
end
end
end

% figure(1)
% plot(t,[sig1,sig2,sig3])
% xlim([0 40e-3]); grid on;

%% Fensterrung

N = length(sig1); % Anzahl Messwerte des Signals 
win = hamming(N);
Signal_win = sig1(1:N).*win; % Multiplikation von Signal und Fenster 
Signal_win2 = sig2(1:N).*win; % Multiplikation von Signal und Fenster 

%% FFT berechnen

spec1 = fft(Signal_win,nfft);
spec2 = fft(Signal_win2,nfft);
ampSpec = abs(spec1(1:nfft/2));
ampSpec2 = abs(spec2(1:nfft/2));
freq(:,1) = 0:fftRes:(nfft/2-1)*fftRes;

figure(2)
plot(freq,[ampSpec,ampSpec2]);
grid on;
xlabel('Frequenz [Hz]'); ylabel('Amplitude')
set(gcf,'Units','Normalized','OuterPosition',[0 0 1 1]);
%ylim([0 1000]);
xlim([0 1000]);
legend('U1','U2');

%% Phase auswerten

binInd1 = f1/fftRes;% Zeiger bei 50Hz
binInd2 = f2/fftRes;% Zeiger bei 50Hz

%Phase aus getrennten Spektren
phase1 = angle(spec1(binInd1+1,1)); %Phasenwinkel Signal1
phase2 = angle(spec2(binInd2+1,1)); %Phasenwinkel Signal2

diffPhase = phase1 - phase2;    %Phasendifferenz
diffPhaseTime = diffPhase/(f1*2*pi)

figure(3)
compass(real(spec1(binInd1+1,1)),imag(spec1(binInd1+1,1)));
hold on;
compass(real(spec2(binInd2+1,1)),imag(spec2(binInd2+1,1)),'m');
legend('Phase Signal 1','Phase Signal 2','Location','NorthEastOutside')
set(gcf,'Units','Normalized','OuterPosition',[0 0 1 1]);

%% Verschieben
time_neu = (t-(diffPhaseTime));

uerrn = ([sig1,t,sig2,time_neu,sig3]);
b = 1;
count = 1;

for i = 1:1:length(uerrn(:,1)) %Unterschied der Stellen ermitteln
if(uerrn(i,2)<=uerrn(1,4))
    count = i;
end
end

uerrn(:,6) = uerrn(:,3);    %Sig1 orginal

for i = count:1:(length(uerrn))
    uerrn(b,7) = uerrn(i,1);    %Sig2 verschoben
    uerrn(b,8) = uerrn(i,5);    %Sig3 verschoben
    b = b + 1;
end

uerr_neu(:,1) = uerrn(:,6)-uerrn(:,7);


figure(5)
subplot(3,1,1)
plot(t,[sig1,sig2,sig3])
legend('Uges','U2','Iges','U2mod')
title('Funktionen original');
xlim([0 40e-3]); grid on;
subplot(3,1,2)
plot(t,[uerrn(:,7),uerrn(:,6),uerrn(:,8)])
legend('Uges','Uerr','Iges')
title('Funktionen verschoben');
xlim([0 40e-3]); grid on;