% Algoritmo Lazo de Costas recursivo

%   Copyright (c) 2022 G.T.A.S. (Grupo Tratamiento Avanzado de Señal)
%                 DICOM Universidad de Cantabria
%                 Jesús Ibáñez

%close all

N=100;      % Número de símbolos de la simulación
SNR=20;     % SNR en dBs
desfase=107; % Desfase introducido por el canal en grados
gamma=0.05;  % Constante de paso del algoritmo

a=(randn(1,N)>0)*2-1;   % símbolos transmitidos BPSK;

z=a*exp(j*pi/180*desfase)+(10^(-SNR/20))/sqrt(2)*(randn(size(a))+j*randn(size(a)));    % símbolos recibidos (con error de fase y AWGN)

z2=zeros(size(z));
a_dec=zeros(size(z));
e=zeros(size(z));
phi=zeros(size(z));


%% Lazo de costas recursivo
phi(1)=0; % Inicialización de la fase estimada
for n=1:N
    z2(n)=z(n)*exp(-j*phi(n)); % Observable: símbolos recibidos con fase corregida
    a_dec(n)=2*(z2(n)>0)-1;          % Símbolos decididos (+/-1)
    e(n)=imag(z(n)*a_dec(n)'*exp(-j*phi(n))); % Error
    phi(n+1)=phi(n)+gamma*e(n); % Nueva fase estimada
end

%% Bucle para representar los plots animados

subplot(234);plot(real(a),imag(a),'or');grid on;axis([-2 2 -2 2]);axis 'square';title('Constelación TX');
subplot(211);cla
subplot(235);cla
subplot(236);cla
%plot(e*180/pi);grid;xlabel('n (nº símbolo)');ylabel('error fase (º)');title('Evolución del error de fase')

disp('Pulsa una tecla para iniciar y observa la figura')
pause

for n=1:N
   subplot(235);plot(real(z(n)),imag(z(n)),'.b');grid on;axis([-2 2 -2 2]);axis 'square';hold on;title(sprintf('Observables (SNR=%ddB)',SNR));
   if n<3/gamma
        subplot(236);plot(real(z2(n)),imag(z2(n)),'.b');
    else
        subplot(236);plot(real(z2(n)),imag(z2(n)),'.r');
    end
    grid on;axis([-2 2 -2 2]);axis 'square';hold on;title('Observables Corregidos');
    subplot(211)
    
    
    plot(1:n,desfase*ones(1,n),'b',1:n,180*phi(1:n)/pi,'r');axis([0 N -180 180]);grid;xlabel('n (nº símbolo)');legend('Real','Estimada');title('Evolución de la fase');
    drawnow
%     pause(0.0001)
end
subplot(211);hold off
subplot(235);hold off
subplot(236);hold off
