% =================================================================
% DECONVOLUCIÓN DE ONDÍCULAS MAL CONDICIONADAS.
% 
% Filtro Wiener inverso con término de regularización
% =================================================================

clear all;
close all;

% -----------------------------------------------------------------
% Parámetros 
% -----------------------------------------------------------------

noise = 0; % Porcentaje de ruido (%)
P     = 0; % Porcentaje de Preblanqueo (%)

% -----------------------------------------------------------------
% Ondícula de fase mínima con ventana de Hamming y ruido
% -----------------------------------------------------------------

N  = 40 ;   % muestras de la ondícula
w0 = pi/3 ; % frecuencia central de la ondícula de fase mínima a generar
dW = w0/N ; % paso en frecuencia

% Generamos una ondícula de fase mínima y le aplicamos un taper de Hamming:
for n = 0:N-1
  hamm(n+1) = 0.54 - 0.46*cos(pi*(N-1-n)/(N-1)); % taper de Hamming
  a(n+1)    = hamm(n+1)*(0.8^n)*cos((w0-dW*n)*n); % ondicula de fase mín.
end

% -----------------------------------------------------------
% Agrego un dipolo con un cero muy cercano el círculo unidad
% y agrego también el conjungado del dipolo para que ondícula
% la ondícula continue siendo real.
% -----------------------------------------------------------

w0 = (7/8)*pi; % frecuencia del cero a agregar
dipo = [1,-0.999*exp(i*w0)];
a = conv(a, dipo);
a = conv(a, conj(dipo));
a = real(a);
N = length(a); % tamaño de la nueva ondícula con el cero

% -----------------------------------------------------------------
% Filtro inverso Wiener
% -----------------------------------------------------------------

Nf = floor(1.2*N) ;          % longitud del operador (filtro) 
Nd = length(a) + Nf - 1 ;    % dimensión de la salida
d  = [ 1 ; zeros(Nd-1,1) ] ; % salida deseada d = (1,0,....,0)
A  = convmtx(a,Nf).' ;       % matriz de convolución
R  = A.'*A ;                 % matriz de autocorrelación
g  = A.'*d ;                 % correlación entre entrada y salida

% ------------------
% Agrego problanqueo
% ------------------

mu  = R(1,1) * P / 100.0;       % parámetro de "trade-off"
f   = inv( R + mu.*eye(Nf) )*g; % filtro inverso Wiener (con regularización)
%             |__________|   
%                  |
%                  v
%        término de regularización


% Agregamos ruido para simular el dato que se registra
a = a + (noise/100).*randn(1,N);

% Deconvolución mediante el filtro Wiener
decon_a = conv(a, f);

% -----------------------------------------------------------------
% Zero padding para ondícula a deconvolucionar, el filtro inverso, 
% y la ondícula deconvolucionada
% -----------------------------------------------------------------

N2  = 1024;
M2  = N2/2 + 1;
WN2 = 2*pi/N2;

% Ondícula
a2     = a;
a2(N2) = 0;

% Filtro
f2     = f;
f2(N2) = 0;

% Ondícula deconvolucionada
decon_a2     = decon_a;
decon_a2(N2) = 0;


% -----------------------------------------------------------------
% Espectro de amplitud de la ondícula, el filtro inverso,
% y la ondícula deconvolucionada.
% -----------------------------------------------------------------

A2       = abs(fft(a2));
F2       = abs(fft(f2));
DECON_A2 = abs(fft(decon_a2));

% -----------------------------------------------------------------
% GRAFICOS 
% -----------------------------------------------------------------

 dB = 0; % Grafico los espectros en escala lineal
%db = 1; % Grafico los espectros en decibeles

figure(1); clf;

% ---------------------------------
% Respuestas impulsivas (en tiempo)
% ---------------------------------

% Ondícula
 
subplot(3,2,1)
plot((0:(N-1)),a./max(a),';ondicula;b-.')
title('En tiempo');
axis([-1 N -1.2 1.2])

% Filtro inverso

subplot(3,2,3)
plot((0:(Nf-1)),f./max(f),';filtro;r-.')
axis([-1 Nf -1.2 1.2])

% Ondícula deconvolucionada

subplot(3,2,5)
plot((0:(N-1)),decon_a(1:N),';decon;g-.')
axis([-1 N -1.2*max(decon_a) 1.2*max(decon_a)])

% -------------------------------------
% Espectros de amplitud (en frecuencia)
% -------------------------------------

xt      = [0;pi/4;pi/2;3*pi/4;pi];
xtlabel = ["0";"{/Symbol p}/4";"{/Symbol p/2}";"3/4{/Symbol p}";"{/Symbol p}"];

% Espectro de la ondícula

subplot(3,2,2)
if dB==0
   plot((0:M2-1).*WN2,A2(1:M2)./max(A2(1:M2)),';ondicula;b')
   axis([0,pi,-0.1,1.2])
else
   plot((0:M2-1).*WN2,20.*log10(A2(1:M2)./max(A2(1:M2))),';ondicula;b')
end
title('Espectros');
axis([0,pi,-0.1,1.2])
set(gca,"xtick",xt,"xticklabel",xtlabel);

% Espectro del filtro inverso

subplot(3,2,4)
if dB==0
   plot((0:M2-1).*WN2,F2(1:M2)./max(F2(1:M2)),';filtro;r')
   axis([0,pi,-0.1,1.2])
else
   plot((0:M2-1).*WN2,20.*log10(F2(1:M2)./max(F2(1:M2))),';filtro;r')
end
legend("location","northwest")   
axis([0,pi,-0.1,1.2])
set(gca,"xtick",xt,"xticklabel",xtlabel);

% Espectro de ondícula deconvolucionada

subplot(3,2,6)
if dB==0
   plot((0:M2-1).*WN2,DECON_A2(1:M2)./max(DECON_A2(1:M2)),';decon;g')
   axis([0,pi,-0.1,1.2])
else
   plot((0:M2-1).*WN2,20.*log10(DECON_A2(1:M2)./max(DECON_A2(1:M2))),';decon;g')
end
legend("location","northwest")   
axis([0,pi,-0.1,1.2])
set(gca,"xtick",xt,"xticklabel",xtlabel);

% ------------------------------------------------------------------
% Salida
% ------------------------------------------------------------------

print('bad_cond_dcon.eps','-depsc')
