% file=uipickfiles('FilterSpec','C:/Users/david/Desktop/Idaho/data_matfiles/*.mat');
% nfiles=length(file);
% for j=1:nfiles
%     [datapath, filename, ext, versn] = fileparts(char(file(j)));
%     if ~exist([datapath,'/analysis'],'dir')
%         mkdir([datapath,'/analysis'])
%     end
%     writedir(j)=cellstr([datapath,'/analysis']);
%     files(j)=cellstr(filename)
% end

SNR_thresh=1.005;

%for jfiles=1:nfiles
%    load(char(file(jfiles)));
    [na nr]=size(lidar.RWS);
    
% interpolate each beam to fixed altitudes
    z=[20:3:1000];
    for j=1:na-1
        rws(j,:) = interp1(lidar.zd(j,:),lidar.RWS(j,:),z,'pchip');
        snr(j,:) = interp1(lidar.zd(j,:),lidar.SNR(j,:),z,'pchip');
        beta(j,:) = interp1(lidar.zd(j,:),lidar.Beta(j,:),z,'pchip');
        xd(j,:) = interp1(lidar.zd(j,:),lidar.xd(j,:),z,'pchip');
        yd(j,:) = interp1(lidar.zd(j,:),lidar.yd(j,:),z,'pchip');
        zd(j,:)=z;    
        azimuth(j,:)=lidar.azimuth(j,1:length(z));
        elevation(j,:)=lidar.elevation(j,1:length(z));
end

    [na,nr]=size(rws);    
    
%    p=999*ones(na,nr);
% check for continuity across range.. For p>threshold the data is
% considered to quickly changing...
%     for k=1:na
%         for j=1:nr-5
%             p(k,j)=std(rws(k,j:j+5))/mean(abs(rws(k,j:j+5)));
%         end
%         p(k,nr-4:nr)=999;
%     end
%     % reshape and remove 'bad' data
    
    numble=na*nr;
    snr=reshape(snr,numble,1);
    rws=reshape(rws,numble,1);
    beta=reshape(beta,numble,1);
    elevation=reshape(elevation,numble,1);
    azimuth=reshape(azimuth,numble,1);
    xd=reshape(xd,numble,1);
    yd=reshape(yd,numble,1);
    zd=reshape(zd,numble,1);
    
    

    ind=find(snr<SNR_thresh | abs(rws./cosd(elevation))>20 );
    snr(ind)=NaN;
    rws(ind)=NaN;
    beta(ind)=NaN;
%     snr=reshape(snr,na,nr);
%     rws=reshape(rws,na,nr);
%     beta=reshape(beta,na,nr);
%     elevation=reshape(elevation,na,nr);
%     azimuth=reshape(azimuth,na,nr);
%    p=reshape(p,na,nr);
    
    % perform vad_analysis at each height

    %    FIRST PASS -
    el=[70:5:85]; % elevations for individual VAD analysis
    sweepnul.rws=[];
    sweepnul.elevation=[];
    sweepnul.azimuth=[];
    sweepnul.xd=[];
    sweepnul.yd=[];
    sweepnul.zd=[];
    sweepnul.X=[];
    sweepnul.vel=[];
    sweepnul.s2=[];
    sweepnul.dvel=[];
    sweepnul.Cook=[];
    sweepnul.DFFITS=[];
           
    for k=1:nr
        
        for isweep=1:4
           sweep(1,isweep)=sweepnul;
           ind=find(~isnan(rws) & (elevation-el(isweep))<1 & abs(zd-z(k))<3);
           rwst=rws(ind);
           elt=elevation(ind);
           azt=azimuth(ind);
           xdt=xd(ind);
           ydt=yd(ind);
           zdt=zd(ind);
           sweep(1,isweep).rws=rwst;
           sweep(1,isweep).elevation=elt;
           sweep(1,isweep).azimuth=azt;
           sweep(1,isweep).xd=xdt;
           sweep(1,isweep).yd=ydt;
           sweep(1,isweep).zd=zdt;
           if ~isempty(ind)
              X=[ones(length(ind),1) sind(azt) cosd(azt)...
               sind(2*azt) cosd(2*azt)];
              sweep(1,isweep).X=X;
              [na,np]=size(X);
              val=pinv(X'*X)*X'*rwst;
              sweep(1,isweep).vel=val;
              s2=mean((rwst-X*val).^2)/(length(ind)-5);
              sweep(1,isweep).s2=s2;
              pp=pinv(X'*X);
              for l=1:5
                 vara(l)=s2.*pp(l,l);
              end
              sweep(1,isweep).dvel=vara;
              at=val;
              resid=rwst-X*val;
              ss=sqrt(s2);
              H=X*pinv(X'*X)*X';
              for m=1:length(resid)
                 resid(m)=resid(m)/ss/sqrt(1-H(m,m));
              end
              C=pinv(X'*X)*(resid'*resid)/(na-np);
              [nn mm]=size(H);
              for n=1:nn
                 Cook(n)=(resid(n).^2/np)*(H(n,n)/(1-H(n,n)));
                 sn=((nn-np)*ss - resid(n)^2/(1-H(n,n)))/(nn-np-1);
                 DFFITS(n)=sqrt(H(n,n)/(1-H(n,n)))*(resid(n)/sn/sqrt(1-H(n,n)));
              end
              sweep(1,isweep).Cook=Cook;
              sweep(1,isweep).DFFITS=DFFITS;
              % PASS TWO
              
              indb=find(Cook<(4/nn) & DFFITS<2*sqrt(np/nn));
              clear Cook sn DFFITS X val s2 pp C H
              if ~isempty(indb)
                  rwst=rwst(indb);
                  elt=elt(indb);
                  azt=azt(indb);
                  xdt=xdt(indb);
                  ydt=ydt(indb);
                  zdt=zdt(indb);
                  sweep(2,isweep).rws=rwst;
                  sweep(2,isweep).elevation=elt;
                  sweep(2,isweep).azimuth=azt;
                  sweep(2,isweep).xd=xdt;
                  sweep(2,isweep).yd=ydt;
                  sweep(2,isweep).zd=zdt;

                  X=[ones(length(indb),1) sind(azt) cosd(azt)...
                      sind(2*azt) cosd(2*azt)];

                  sweep(2,isweep).X=X;
                  [na,np]=size(X);
                  val=pinv(X'*X)*X'*rwst;
                  sweep(2,isweep).vel=val;
                  s2=mean((rwst-X*val).^2)/(length(indb)-5);
                  sweep(2,isweep).s2=s2;
                  pp=pinv(X'*X);
                  for l=1:5
                      vara(l)=s2.*pp(l,l);
                  end
                  sweep(2,isweep).dvel=vara;
                  at=val;
                  resid=rwst-X*val;
                  ss=sqrt(s2);
                  H=X*pinv(X'*X)*X';
                  for m=1:length(resid)
                      resid(m)=resid(m)/ss/sqrt(1-H(m,m));
                  end
                  C=pinv(X'*X)*(resid'*resid)/(na-np);
                  [nn mm]=size(H);

                  for n=1:nn
                      Cook(n)=(resid(n).^2/np)*(H(n,n)/(1-H(n,n)));
                      sn=((nn-np)*ss - resid(n)^2/(1-H(n,n)))/(nn-np-1);
                      DFFITS(n)=sqrt(H(n,n)/(1-H(n,n)))*(resid(n)/sn/sqrt(1-H(n,n)));

                  end
                  sweep(2,isweep).Cook=Cook;
                  sweep(2,isweep).DFFITS=DFFITS;
              else
                  sweep(2,isweep)=sweepnul;
              end
           else
               sweep(1,isweep)=sweepnul;
           end
        end

%         el=[70:5:85];
%         for sweep=1:4
%            ind=find(~isnan(rws(:,k:kr+2)) & (elevation(:,k:kr+2)-el(sweep))<1);
%            for j=1:length(ind)
%               X(j,1)=1.;
%               X(j,2)=sind(azimuth(ind(j),k));
%               X(j,3)=cosd(azimuth(ind(j),k));
%               X(j,4)=sind(2*azimuth(ind(j),k));
%               X(j,5)=cosd(2*azimuth(ind(j),k));
%            end
%            a(:,sweep)=pinv(X'*X)*X'*rws(ind,k);
%            s2(sweep)=mean((rws(ind,k)-X*a(:,sweep)).^2)/(length(ind)-5);
%            pp=pinv(X'*X);
%            for l=1:5
%               vara(l,sweep)=s2(sweep).*pp(l,l);
%            end
%         end
           
    for j=1:length(xpos)
    temp=vvp_regressionb(vad,xpos(j),ypos(j),rho);
    Vhmean=Vhmean+temp.Vh/length(xpos);
   end

    hfig=plot(Vhmean,profile.heights);
    set(gca,'XLim',[0 25])
    set(gca,'YLim',[0 1000])
    pause(1)
    
%     textfile=fullfile(char(writedir(jfiles)),[char(files(jfiles)),'_profile.dat']);
%     prtid=print_file(textfile,profile,vad);
%     textfile=fullfile(char(writedir(jfiles)),[char(files(jfiles)),'_profile.mat']);
%     save(textfile,'profile');
    

end