% function optimal_D_search

clear all

ADCP_filename='ADCP_Feb_2013_Site3_data.mat';
YSI_profile_filename='Aulne_experiment_20130214_site3_YSI_profiles.mat';

%%
% loading ADCP data

load(ADCP_filename)

time_adcp=data.time.val; %time of profiles
binpos=data.binpos.val; %bin level in m (above the transducer?)
WS=data.WS.val; %vecteur position des cellules par rapport à l'ADCP
amean=data.amean.val; %signal acoustique rétrodiffusé all beams averaged.  Changed from a1 SMM.
EC0=data.EC0.val; 
NE=data.SL0.val; %niveau émis
B=data.B.val;
ouv=data.ouv.val;
% WB=data.WS.val;
% WN=data.WS.val;
% Lt=data.WS.val;
Kc=data.Kc.val;
f=data.f.val;
theta=data.theta.val;
Tb_adcp=data.Tb_adcp.val;
Sb_adcp=data.Sb_adcp.val;
Depth_adcp=data.Depth_adcp.val;
a_t=data.a_t.val;
spd=data.spd.val;
ve=data.ve.val; % added SMM east velocity
vn=data.vn.val; % added SMM north velocity

%% load YSI profile data

load(YSI_profile_filename);
for i=1:length(ysi)
    time_ysi(i)=ysi(i).profile.time;
    salinity_ysi(i,:)=ysi(i).profile.salinity';
    temperature_ysi(i,:)=ysi(i).profile.temperature';
    ssc_ysi(i,:)=ysi(i).profile.ssc';
end
level_ysi=ysi(1).profile.depth;

% extrapolate to bed - basic assumption homogeneous up to the fisrt obs.
for i=1:length(ysi)
p=find((salinity_ysi(i,:))>-10,1,'first');
    if p>1
        salinity_ysi(i,1:p-1)=salinity_ysi(i,p);
        temperature_ysi(i,1:p-1)=temperature_ysi(i,p);
        ssc_ysi(i,1:p-1)=ssc_ysi(i,p);
    end
end


% interpolate on ADCP bins
for i=1:length(ysi)
    salinity_ysi_adcp(i,:)=interp1(level_ysi,salinity_ysi(i,:),binpos,'linear');
    temperature_ysi_adcp(i,:)=interp1(level_ysi,temperature_ysi(i,:),binpos,'linear');
    ssc_ysi_adcp(i,:)=interp1(level_ysi,ssc_ysi(i,:),binpos,'linear');
end

salinity_ysi_adcp(isnan(salinity_ysi_adcp))=-999;
temperature_ysi_adcp(isnan(temperature_ysi_adcp))=-999;
ssc_ysi_adcp(isnan(ssc_ysi_adcp))=-999;


% figure
% pmax=4;
% sscmax=2;
% fz=14;
% for i=1:length(ysi)
%     
%     tmpx=ssc_ysi(i,:)';
%     tmpy=level_ysi;
%     tmpy(tmpx==-999)=[];
%     tmpx(tmpx==-999)=[];
%     
% subplot(3,3,i)
% hold on
% plot(tmpx,tmpy,'ob')
% plot(ssc_ysi_adcp(i,:),binpos,'+r')
% xlabel('SSC','Fontsize',fz);ylabel('Depth (m)','Fontsize',fz);title(datestr(time_ysi(i),31),'Fontsize',fz)
% axis([0 sscmax,0,pmax])
% end



% find ADCP profiles corresponding to YSI profiles
for i=1:length(ysi)
iadcp(i)=find(time_adcp>time_ysi(i),1,'first');
end

amean=amean(iadcp,:); % SMM changed from a1
time_adcp=time_adcp(iadcp);
Depth_adcp=Depth_adcp(iadcp);
East_vel=ve(iadcp,:);% Added SMM east velocity
North_vel=vn(iadcp,:); % Added SMM north velocity
Along_vel=East_vel.*sind(99)+North_vel.*cosd(99);% Added SMM along-channel velocity

%%  Calculating all members of the sonar equation

dopt=nan(length(time_adcp),length(binpos));
SSC=nan(length(time_adcp),length(binpos));
SSC_fixe=nan(length(time_adcp),length(binpos));
Along_vel2=nan(length(time_adcp),length(binpos));

diameter=(1:1:300); % range of diameter tested for sediment inversion in microns

zmax=zeros(size(time_adcp));

for i=1:length(time_adcp)

    
    zmax(i)=find(ssc_ysi_adcp(i,:)==-999,1,'first')-1;
    
    deltaR=ones(size(binpos))*WS;
    A=zeros(size(binpos));
    R=zeros(size(binpos));
    C=zeros(size(binpos));
    alphaw=zeros(size(binpos));

    thetaR=theta*pi/180;            % beam angle in radians 0.3491 rad
    lambda=1500/f/1000;             % mean wave length
    r0=a_t^2/lambda/2;                % en pratique: limite champs proche =1.06m
    phi=ouv*pi/180;
    PSI=pi*(phi/2)^2;

    %recieved echo
    NRtmp=Kc*(amean(i,:)-EC0)+B; % SMM changed from a1

    %water attenuation

    deltaR(1)=0.50; % distance from transducer to first cell
    
    R(1)=binpos(1);%distance ADCP-cellule
    alphaw(1)=equ_att_son_garrison(f,Depth_adcp(i)-R(1),temperature_ysi_adcp(i,1),salinity_ysi_adcp(i,1));
    alphaw(alphaw==0)=nan;
    A(1)=alphaw(1)*deltaR(1)/cos(thetaR)';

    for j=2:zmax(i)
        R(j)=binpos(j);%distance ADCP-cell
        alphaw(j)=equ_att_son_garrison(f,Depth_adcp(i)-R(j),temperature_ysi_adcp(i,1),salinity_ysi_adcp(i,1));
        A(j)=A(j-1)+alphaw(j)*deltaR(j)/cos(thetaR)';%deltaR(i,j)/cos(theta)->distance oblique
    end


    V=PSI*(R/cos(thetaR)).^2*WS/2;

    Z=R/cos(thetaR)/r0;
    PSI_chp=(1+1.35*Z+(2.5*Z).^3.2)./(1.35*Z+(2.5*Z).^3.2);

    Cst_geo=10*log10(PSI*1/2*WS);
    TL_geo=20*log10(R/cos(thetaR).*PSI_chp);

       
    IVtmp=nan(length(binpos),length(diameter));
    SSCtmp=nan(length(binpos),length(diameter));
    
    % sediment attenuation
        % case 1 : fixed sediment size
    [zeta_vdB,zeta_ddB,Cst_sdt]=sdt_absorption(f,80); % micron fixed size...test

    C(1)=2*(zeta_vdB+zeta_ddB)*ssc_ysi_adcp(i,1)*deltaR(1)/cos(thetaR)';
    
    for j=2:zmax(i)
        C(j)=C(j-1)+2*(zeta_vdB+zeta_ddB)*ssc_ysi_adcp(i,j)*deltaR(j)/cos(thetaR)';
    end
    
    IVtmp_fixe=NRtmp-NE+TL_geo-Cst_geo+2*A+C;
    SSCtmp_fixe=10.^((IVtmp_fixe-Cst_sdt)/10);
    SSC_fixe(i,:)=SSCtmp_fixe;
    
        % case 2 : searching for optimal particle size
        
    for k=1:length(diameter)
        [zeta_vdB,zeta_ddB,Cst_sdt]=sdt_absorption(f,diameter(k));
        
        C(1)=2*(zeta_vdB+zeta_ddB)*ssc_ysi_adcp(i,1)*deltaR(1)/cos(thetaR)';
    
        for j=2:zmax(i)
            C(j)=C(j-1)+2*(zeta_vdB+zeta_ddB)*ssc_ysi_adcp(i,j)*deltaR(j)/cos(thetaR)';
        end
       
        IVtmp(:,k)=NRtmp-NE+TL_geo-Cst_geo+2*A+C;
        SSCtmp(:,k)=10.^((IVtmp(:,k)-Cst_sdt)/10);

    end

    m=nan(size(binpos));
    dopt_tmp=nan(size(binpos));
    for j=1:zmax(i)
        [m(j),dopt_tmp(j)]=min(abs(SSCtmp(j,:)-ssc_ysi_adcp(i,j)));
        % [m1(i,j),dopt(i,j)]=min(abs((CMEStmp(j,:)'-ones(length(diametre),1)*SPMtmp(j))));
        % CMES(i,j)=CMEStmp(j,dopt(i,j));
        dopt(i,j)=dopt_tmp(j);
        SSC(i,j)=SSCtmp(j,dopt(i,j));
        % CMES_fixe(i,k)=CMEStmp_fixe(k);
        Along_vel2(i,j)=Along_vel(i,j); % Added SMM
    end

    
%     figure
%     subplot(121)
%     hold on
%     plot(SSC(i,1:zmax(i)),binpos(1:zmax(i)),'ob')
%     plot(SSC_fixe(i,1:zmax(i)),binpos(1:zmax(i)),'og')
%     plot(ssc_ysi_adcp(i,1:zmax(i)),binpos(1:zmax(i)),'+r')
%     legend('optimal','fixed size','obs')
%     axis([0 2 0 3])
%     subplot(122)
%     plot(dopt(i,1:zmax(i)),binpos(1:zmax(i)),'ob')
%     axis([0 300 0 3])
%     pause
%     
%     %calcul de la RMS (pour les diamètres minimisant la différence entre les concentrations mesurées avec les deux appareils(dans chaque cellule))
%     CMEStmp=CMES(i,:);
%     CMES_fixe(i,:)=CMEStmp_fixe;
%     ikeep=~isnan(SPMtmp) & ~isnan(CMEStmp);
%     RMS(i)=norm(SPMtmp(ikeep)-CMEStmp(ikeep))/sqrt(length(SPMtmp));
%     RMS_fixe(i)=norm(SPMtmp(ikeep)-CMEStmp_fixe(ikeep))/sqrt(length(SPMtmp));
end
figure()  % Added SMM
for i=1:9
    subplot(2,5,i)
    plot(SSC(i,1:zmax(i)),binpos(1:zmax(i)),'ob')
    hold
    plot(ssc_ysi_adcp(i,1:zmax(i)),binpos(1:zmax(i)),'r+')
    axis([0 2 0 3])
end
legend('Dopt ADCP SSC','YSI SSC')
xlabel('SSC (g/L)')
ylabel('Height above bottom (m)')