clear all;
close all;
clc;

% ACQUISIZIONE DATI (STEREO)

% Object creation
ai = analoginput('winsound');
addchannel(ai,1:2);
Fs=8000;
duration = 6;
set(ai, 'SampleRate', Fs);
set(ai, 'SamplesPerTrigger', duration*Fs);
start(ai);
% Data acquisition
data_acq=getdata(ai);
% Object destruction
delete(ai);
clear ai

% Passabasso iniziale

Wp = 50/(Fs/2);
Ws = 400/(Fs/2);
Rp=0.1;
Rs=46;
[N, Wn] = ellipord(Wp, Ws, Rp, Rs);
[b_lp,a_lp] = ellip(N,Rp,Rs,Wn);
freqz(b_lp,a_lp,4000,Fs);
title('Risposta in frequenza del filtro passabasso iniziale');

data_predec = filter(b_lp,a_lp,data_acq);
data(:,1) = data_predec(1:10:length(data_predec), 1);
data(:,2) = data_predec(1:10:length(data_predec), 2);
Fs = 800;

% VISUALIZZAZIONE SPETTRO DATI ACQUISITI
% Processing
N = length(data);
X=fftshift(fft(data))*(1/Fs);
df=Fs/N;
if mod(N,2) == 0
    f = [-N/2+[0:N-1]]*df;
else
    f = [-(N-1)/2+[0:N-1]]*df;
end

figure()
subplot(2,1,1)
plot(f, abs(X));
subplot(2,1,2)
plot(f, angle(X));
title('Spettro dati acquisiti');

% FILTRO SELETTIVO

% Filtro per esaltare la frequenza della sorgente a 30 Hz

% Periodo di campionamento
T = 1/Fs;
% Frequenza da isolare
f0 = 30;

% Progetto del filtro
% Il parametro rho dev'essere strettamente compreso tra 0 e 1. Lo scelgo il
% più possibile vicino a uno, per avere maggior precisione.
rho = 0.995;
% La funzione freqz calcola M valori della trasformata di Fourier
% corrispondente alla funzione di trasferimento del filtro, equispaziati
% tra 0 e Fs/2. La risposta in frequenza del filtro ha simmetria hermitiana
% ed è periodica di periodo Fs, perciò è sufficiente considerarne i valori
% in un semiperiodo. Si hanno nel vettore H gli M campioni complessi e nel
% vettore f i valori di frequenza corrispondenti.
M = 400000;
C = (1-2*rho*cos(2*pi*f0*T)+rho^2)/(2-2*cos(2*pi*f0*T));
b = [1-C 2*(C-rho)*cos(2*pi*f0*T) rho^2-C];
a = [1 -2*rho*cos(2*pi*f0*T) rho^2];

[H,f] = freqz(b,a,M,Fs);

% Visualizzo modulo e fase della risposta in frequenza
figure()
plot(f, abs(H));
xlabel('f(Hz)');
ylabel('Modulo di H(f)');
title('Modulo della risposta in frequenza del filtro selettivo, f0=30Hz');
figure()
plot(f, angle(H));
xlabel('f(Hz)');
ylabel('Fase di H(f)');
title('Fase della risposta in frequenza del filtro selettivo, f0=30Hz');

data_elab = filter(b,a,data);
N = length(data_elab);
X=fftshift(fft(data_elab))*(1/Fs);
df=Fs/N;
if mod(N,2) == 0
    f = [-N/2+[0:N-1]]*df;
else
    f = [-(N-1)/2+[0:N-1]]*df;
end

figure()
subplot(2,1,1)
plot(f, abs(X));
subplot(2,1,2)
plot(f, angle(X));
title('Spettro dati dopo selezione 30 Hz');

% RICAVARE SFASAMENTO

t = 0:T:(duration - T);

signal1 = data_elab(:,1)';
signal2 = data_elab(:,2)';

figure()
plot(t,signal1);
title('Primo segnale');

figure()
plot(t,signal2);
title('Secondo segnale');

% Vogliamo ricavare lo sfasamento a partire dai campioni acquisiti

% COME? Algoritmo:
% phi2-phi1 si determina moltiplicando i due campioni provenienti dai due
% microfoni
% A1*sin(2*pi*30*t+phi1)*A2*sin(2*pi*30*t+phi2)=
% 0.5*A1*A2*(cos(phi2-phi1) - cos(4*pi*30*t+phi1+phi2))
% Si isola la componente in continua 0.5*A1*A2*cos(phi2-phi1)
% Dividiamo per 0.5*A1*A2: per farlo dobbiamo stimare le ampiezze dei due
% segnali e lo facciamo elevandoli al quadrato e isolando la continua con
% il medesimo filtraggio applicato al prodotto.
% A questo punto l'arcocoseno porge phi2-phi1.

square1 = signal1.^2; % A1^2 sin(x)^2 = A1^2/2 (1 - cos(2x))
square2 = signal2.^2; % A1^2 sin(y)^2 = A1^2/2 (1 - cos(2y))
prod = signal1.*signal2; % A1*sin(x)*A2*sin(y) =
% A1*A2/2 (cos(x-y) - cos(x+y)), dove x-y è la differenza di fase

% Sono presenti due componenti: una a frequenza nulla, l'altra a frequenza
% doppia, cioé a 60 Hz
% Per conservare solo la frequenza continua eliminiamo i 60 Hz con un
% filtro notch e poi con un passa basso eliminiamo eventuale rumore a
% frequenze più elevate.

figure()
plot(t, square1);
title('Quadrato del primo segnale');
figure()
plot(t, square2);
title('Quadrato del secondo segnale');
figure()
plot(t, prod);
title('Prodotto dei due segnali');


% NOTCH FILTER
% Frequenza da eliminare
f0 = 60;
rho = 0.95;
C = (1-2*rho*cos(2*pi*f0*T)+rho^2)/(2-2*cos(2*pi*f0*T));
b_n = [C -2*C*cos(2*pi*f0*T) C];
a_n = [1 -2*rho*cos(2*pi*f0*T) rho^2];

[H_n,f] = freqz(b_n,a_n,400000,Fs);

% Visualizzo modulo e fase della risposta in frequenza del filtro notch
figure()
subplot(2,1,1)
plot(f, abs(H_n));
xlabel('f(Hz)');
ylabel('Modulo di H(f)');
title('Modulo della risposta in frequenza del filtro notch, f0=60Hz');
subplot(2,1,2)
plot(f, angle(H_n));
xlabel('f(Hz)');
ylabel('Fase di H(f)');
title('Fase della risposta in frequenza del filtro notch, f0=60Hz');

% Filtro i tre segnali per togliere la componente a 60 Hz
square1_filter = filter(b_n,a_n,square1); % A1^2/2
square2_filter = filter(b_n,a_n,square2); % A2^2/2
prod_filter = filter(b_n,a_n,prod); % A1*A2/2*cos(phi_x-phi_y)

figure()
plot(t, square1_filter);
title('Quadrato del primo segnale dopo notch');
figure()
plot(t, square2_filter);
title('Quadrato del secondo segnale dopo notch');
figure()
plot(t, prod_filter);
title('Prodotto dei due segnali dopo notch');

% LOWPASS FILTER

Wp = 10/(Fs/2);
Ws = 250/(Fs/2);
Rp=0.1;
Rs=46;
[N, Wn] = ellipord(Wp, Ws, Rp, Rs);
[b,a] = ellip(N,Rp,Rs,Wn);
freqz(b,a,4000,Fs);
title('Risposta in frequenza del filtro passabasso');

imp = [1; zeros(length(signal1)-1,1)];
% Risposta all'impulso
h = filter(b,a,imp);
figure()
plot(t,h);
title('Risposta impulsiva del filtro passabasso');

% Filtro i tre segnali per togliere eventuali componenti ad alta frequenza
square1_filter2 = filter(b,a,square1_filter); % A1^2/2
square2_filter2 = filter(b,a,square2_filter); % A2^2/2
prod_filter2 = filter(b,a,prod_filter); % A1*A2/2*cos(phi_x-phi_y)

figure()
plot(t, square1_filter2);
title('Quadrato del primo segnale dopo notch e passabasso');
figure()
plot(t, square2_filter2);
title('Quadrato del secondo segnale dopo notch e passabasso');
figure()
plot(t, prod_filter2);
title('Prodotto dei due segnali dopo notch e passabasso');

diff_media = real(acos(mean(prod_filter)/sqrt(mean(square1_filter)*mean(square2_filter))));

diff_a_regime = real(acos(prod_filter2(ceil(length(prod_filter2)/2))/sqrt(square1_filter2(ceil(length(square1_filter2)/2))*square2_filter2(ceil(length(square2_filter2)/2)))))

diff = real(acos(prod_filter2./sqrt(square1_filter2.*square2_filter2)));

figure()
plot(t, diff);
title('Differenza di fase istante per istante');

% RICAVARE POSIZIONE

% Trovare la posizione della sorgente sonora (x) in funzione di
% D (lunghezza della tastiera), h (profondità della tastiera, distanza tra
% la base della tastiera e i microfoni) e delta (differenza di cammino tra
% i due segnali sonori pervenuti ai due microfoni); d2 è la distanza tra la
% sorgente sonora e uno dei due microfoni e delta = d1 - d2.

D = 5;
h = 4;

% Bisogna capire come determinare delta sulla base
% dei segnali che arriveranno dai due microfoni:
% delta = v_suono * ritardo = v_suono * (phi2-phi1) / (2*pi*f0)

v_suono = 343;

delta = v_suono * diff(ceil(length(diff)/2)) / (2*pi*30);


d2 = -delta/2 + D/2*sqrt((4*h^2 + D^2 - delta^2)/(D^2 - delta^2));

x = (D^2 + delta^2 + 2*delta*d2)/(2*D);


% Sulla base del valore di x si determina la nota: si genera una sinusoide
% di frequenza opportuna e la si suona