%Estimation Roughness (Nadir Model)
%%First load the data TXX_WS.mat
[file,location] = uigetfile('*.mat');
load([location file]);
clear file location;

cutdB=-25;  %threshold fitting expressed in dB under the peak
cut=10^(cutdB/10);

%%nadir model bias found with simulations (see also Fig 4b, Mastrogiuseppe et al., 2014)
shift_nadir=[0	0	0	0	0	0	6.56194955166524	5.96719622117848	5.41943721178480	4.91867252348420...
    4.46490215627669	4.05812611016226	3.69834438514091	3.38555698121264	3.11976389837746	2.90096513663536...
    2.72916069598634	2.60435057643040	2.52653477796755	2.49571330059778	2.34604342857142	2.36422399999999...
    2.38179885714285	2.39876799999999	2.41513142857142	2.43088914285713	2.44604114285713	2.46058742857142...
    2.47452799999999	2.48786285714284	2.50059199999999	2.51271542857142	2.52423314285713	2.53514514285713...
    2.54545142857143	2.55515200000000	2.56424685714286	2.57273600000001	2.58061942857143	2.58789714285715...
    2.59456914285716	2.60063542857144	2.60609600000002	2.61095085714288	2.61520000000003	2.61884342857147...
    2.62188114285718	2.62431314285719	2.62613942857147	2.62736000000005];

flag=0; %put to 1 if you want to realign and average the waveforms (default is 0)

if flag==1
 %%REALIGNMENT
 for i=1:size(REALI_Plains,1)
     [a b]=max(REALI_Plains(i,:));
     REALI_Plains(i,:)=circshift(REALI_Plains(i,:),1000-b);
 end
 %%AVERAGE N ECHOES
 N=2;
 i=0;
 k=0;
 while i<=size(REALI_Plains,1)-N
     k=k+1;
     REALI_Plains_avg(k,:)=mean(REALI_Plains(i+1:i+N,:));
     SC_radius_Plains_avg(k)=mean(SC_radius_Plains(i+1:i+N));
     teta_off_Plains_avg(k)=mean(teta_off_Plains(i+1:i+N));
     i=i+N;
 end
 REALI_Plains=REALI_Plains_avg;
 SC_radius_Plains=SC_radius_Plains_avg;
 teta_off_Plains=teta_off_Plains_avg;
end

%%DATI SIM

win=1000;      %larghezza finestra su cui fare la stima
c=3e8;                               %velocità della luce
Rt=2575e3;                           %raggio di titano
%Banda=4.25e6;                       %Banda Mhz
Banda=4.25e6*1.1428;                 %Banda Mhz (Taylor)
fs=5e6;                              %frequenza di campionamento
dt=1/fs;
t=-100e-6:dt/10:100e-6-(dt/10);      %asse dei tempi
teta=0.00614;                        %fascio a -3dB in rad
gamma=-(2*(sin(teta/2))^2)/log(0.5);
lambda=0.022;
G0=10^(50.7/10);
broad_fact=0.5;
sigma_p = (1/(sqrt(8*log(2))*Banda));
Pt=48.084;
T=150e-6;

kk=0;
for imp=1:size(REALI_Plains,1)
    kk=kk+1
    n=10;%%fattore di interpolazione
    dtint=dt/n;
    t1=-100e-6:dt:100e-6-dt;     %asse dei tempi
    tr=-100e-6:dtint:100e-6-dtint;
    h=(SC_radius_Plains(kk))*1000; %attivare per dati reali
    D=(1+h/Rt);                 %effetti superficie sferica
    K=(G0^2) * (lambda.^2) * c / 2 / ((4*pi)^2) ./ (h.^3) * Pt .* Banda * broad_fact.* T .* pi .* sigma_p;
    sigmazero=37000;
    
    nn=0;
    tstart=-100e-8;
    tend=100e-8;
    
     ii=0;
     for sigmah=1:50
        ii=ii+1;
        
        IMP=REALI_Plains(kk,:);
        [mas indmax]=max(IMP);
        IMP=circshift(IMP',5000-indmax)';
        IMP=IMP((length(IMP)/2-win):(length(IMP)/2+win));
        IMP=IMP/max(IMP);
        [masaftercut indmaxaftercut]=max(IMP);
        
        sigma_s=(2*sigmah)/c;
        sigmac=sqrt(sigma_s^2+sigma_p^2);
        delta=((4*c)/(gamma*h*D))*sigmac;
        alfa=delta/sigmac;
 
        
        %--------------MODELLI E DERIVATE----------------
        
        IR2=(K*sigmazero*0.5)*exp(-((delta/sigmac).*(t))+(delta^2/2)).*(1+erf(((t)./(sqrt(2)*sigmac))-(delta/sqrt(2))));
        IRh1=IR2((length(IR2)/2-win):(length(IR2)/2+win));
        IRh1=IRh1/max(IRh1); %modello normalizzato
        [mas2 indmax2]=max(IRh1);
        IRh1=circshift(IRh1',indmaxaftercut-indmax2)';
        
        dIRh=IR2*(4*alfa^2/c^2)*sigmah - (8*(K*sigmazero*0.5)*sigmah/sqrt(2*pi)/c^2/sigmac).*(t/sigmac^2+alfa).*exp(-t.^2/2/sigmac^2);
        dIRh(find(dIRh<0))=0;  %if negative values are present it puts them to zero
        dIRs=dIRh((length(dIRh)/2-win):(length(dIRh)/2+win));
        dIRs=dIRs/max(dIRs);
        dIRs=circshift(dIRs',indmaxaftercut-indmax2)';        
        MIN=find(abs(IMP)>cut,1,'first');
        MAX=find(abs(IMP)>cut,1,'last');
        IMP=IMP(MIN:MAX);
        IRh1=IRh1(MIN:MAX);
        dIRs=dIRs(MIN:MAX);
        x1s=(IMP-IRh1).*dIRs;
        x2s=sum(x1s);
        C=sum(IRh1.^2);
        mle1(ii)=x2s/C;
        mire(ii)=mean(abs(IMP-IRh1));
     end

    [err inderrsh]=min(abs(mle1));
    
    SIGMAHSTIMATO(kk)=inderrsh-shift_nadir(inderrsh);
    ERRORE(kk)=mire(inderrsh);
    
    %%PLOT
      IMP=REALI_Plains(kk,:);
      [mas indmax]=max(IMP);
      IMP=circshift(IMP',5000-indmax)';
      IMP=IMP((length(IMP)/2-win):(length(IMP)/2+win));
      IMP=IMP/max(IMP);
      [masaftercut indmaxaftercut]=max(IMP);
     
      sigmah=SIGMAHSTIMATO(kk);
      sigma_s=(2*sigmah)/c;
      sigmac=sqrt(sigma_s^2+sigma_p^2);
      delta=((4*c)/(gamma*h*D))*sigmac;
      alfa=delta/sigmac;
 
        
      %--------------MODELLI E DERIVATE----------------
        
      IR2=(K*sigmazero*0.5)*exp(-((delta/sigmac).*(t))+(delta^2/2)).*(1+erf(((t)./(sqrt(2)*sigmac))-(delta/sqrt(2))));
      IRh1=IR2((length(IR2)/2-win):(length(IR2)/2+win));
      IRh1=IRh1/max(IRh1); %modello normalizzato
      [mas2 indmax2]=max(IRh1);
      IRh1=circshift(IRh1',indmaxaftercut-indmax2)';

      timeee=linspace(0,200/10000*2001,2001);  
      figure(2);
      plot(timeee,10*log10(IMP),'LineWidth',2);
      hold on
      MINplot=find(IMP>cut,1,'first');
      MAXplot=find(IMP>cut,1,'last');
      axis([MINplot*200/10000-50*200/10000 MAXplot*200/10000+100*200/10000 10*log10(cut) 0])
      xlabel('Time [\mus]')
      ylabel('Normalized Power [dB]')
      grid on
      plot(timeee,10*log10(IRh1),'r', 'LineWidth',2);
      title (strcat('NADIR MODEL - BURST NO.: ',num2str(kk),' - ESTIMATED \sigma_h =',num2str(round(SIGMAHSTIMATO(kk))),' mt'))
      subtitle(strcat('MIRE=',num2str(ERRORE(kk)),', theta-off=',num2str(rad2deg(teta_off_Plains(kk))), ' deg'));
      legenda=legend('IMPULSO','MODELLO');
      pause(1)
      hold off     
end

Stima=mean(SIGMAHSTIMATO);
Std=std(SIGMAHSTIMATO);
Errore=mean(ERRORE);
disp([num2str(Stima) ' ' num2str(Std) ' ' num2str(Errore)])

figure,subplot(2,1,1),plot(SIGMAHSTIMATO,'k'),grid on, 
title('Roughness Estimation - Nadir Model')
ylabel('\sigma_h [m]')
subplot(2,1,2),plot(ERRORE*100,'k')
grid on, ylabel('MIRE [%]'), xlabel('Estimation no.')