%reads in the UH data and do the motion correction process of ruse with the
%PSD system

read_son_HiWinGS_2013;  %

% Units are meters: x positive forward, y positive to port, z positive up.
Rx = 0.1016;  % Sonic is 4" in front of MotionPak
Ry = 0;   
Rz = 0.8382;  % Sonic is 33" above MotionPak
fs=10; % 10 Hz data
R_UH=[Rx;   % R is the vector distance between the sonic and the motion-pak
   Ry;   % (should be small)
   Rz];
% define a high pass filter which retains real acceleration but removes drift
fc=1/25;        % cutout frequency for angle calculation
[bhi,ahi,bf2,af2,tau2,spinup2] = filtcoef2(fs,fc);   


 [jd_UH,AccX,AccY,AccZ,RateX,RateY,RateZ,U_UH,V_UH,W_UH,Tsonic_UH,LagPulse,Xlag]=read_dms_HiWinGS_2013(ddd,hhh,yyyy,W);
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %adjusts for lag shift. Ajdust UH onto PSD system
%  AccX=[ones(abs(Xlag),1)*NaN; AccX(1:end-abs(Xlag))];
%  AccY=[ones(abs(Xlag),1)*NaN; AccY(1:end-abs(Xlag))];
%  AccZ=[ones(abs(Xlag),1)*NaN; AccZ(1:end-abs(Xlag))];
%  RateX=[ones(abs(Xlag),1)*NaN; RateX(1:end-abs(Xlag))];
%  RateY=[ones(abs(Xlag),1)*NaN; RateY(1:end-abs(Xlag))];
%  RateZ=[ones(abs(Xlag),1)*NaN; RateZ(1:end-abs(Xlag))];
%  U_UH=[ones(abs(Xlag),1)*NaN; U_UH(1:end-abs(Xlag))'];
%  V_UH=[ones(abs(Xlag),1)*NaN; V_UH(1:end-abs(Xlag))'];
%  W_UH=[ones(abs(Xlag),1)*NaN; W_UH(1:end-abs(Xlag))'];
%  Tsonic_UH=[ones(abs(Xlag),1)*NaN; Tsonic_UH(1:end-abs(Xlag))'];
 AccX=[ones(abs(Xlag),1)*AccX(1); AccX(1:end-abs(Xlag))];
 AccY=[ones(abs(Xlag),1)*AccY(1); AccY(1:end-abs(Xlag))];
 AccZ=[ones(abs(Xlag),1)*AccZ(1); AccZ(1:end-abs(Xlag))];
 RateX=[ones(abs(Xlag),1)*RateX(1); RateX(1:end-abs(Xlag))];
 RateY=[ones(abs(Xlag),1)*RateY(1); RateY(1:end-abs(Xlag))];
 RateZ=[ones(abs(Xlag),1)*RateZ(1); RateZ(1:end-abs(Xlag))];
 U_UH=[ones(abs(Xlag),1)*U_UH(1); U_UH(1:end-abs(Xlag))'];
 V_UH=[ones(abs(Xlag),1)*V_UH(1); V_UH(1:end-abs(Xlag))'];
 W_UH=[ones(abs(Xlag),1)*W_UH(1); W_UH(1:end-abs(Xlag))'];
 Tsonic_UH=[ones(abs(Xlag),1)*Tsonic_UH(1); Tsonic_UH(1:end-abs(Xlag))'];
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  time_UH=[jd_UH-jd_UH(1)]*24*3600+abs(Xlag)/10; %time in secondes -->
%  this in addition to above double correct for time shift...
 time_UH=[jd_UH-jd_UH(1)]*24*3600; %time in secondes 
lagmat=[ddd hhh Xlag];
if printit
 sdg=['save ', drive_w '\PSDUH_lagdata.prn lagmat -ascii -append'];
 eval(sdg)
end 
% 
% % no heading data so set to zero, keeping winds in ship coordinates
%     heading = U*0;
%uses real heading from SCS
% heading=heading*pi/180;
    Vel_UH=[U_UH';  % U, V, and W are the Gill Solent sonic velocities in the ship
         V_UH';  %   frame of reference              
         W_UH'];            

     %gain adjustements from BB - 06272014
    Plat_UH = [(AccX')/0.94992;    % accx, accy, accz are the accelerations in the 
            (AccY')/0.91555;    % bow-stern, port-starboard, and up-down directions,
            (AccZ')/0.94844];   % respectively
%     Plat_UH = [(AccX');    % accx, accy, accz are the accelerations in the 
%             (AccY');    % bow-stern, port-starboard, and up-down directions,
%             (AccZ')];   % respectively
    deg_rate_UH=[(RateX')/1;    % ratex, ratey, ratez are the rotation rates from 
              (RateY')/1;    % the motion-pak (in radians/sec)
              (RateZ')/1];

    % compute euler angles with updated UCONN method.
    its = 5; %Iterate 5 times
    [euler_UH,dr] = angles_UH(ahi,bhi,fs,Plat_UH,deg_rate_UH,heading,its);

    % compute platform velocities with updated UCONN method:
    % rotate, remove gravity, integrate, and high-pass filter.
    [acc_UH,uvwplat_UH,xyzplat_UH] = accels_UH(bhi,ahi,fs,Plat_UH,euler_UH);
    uvwplatVar = var(uvwplat_UH,0,2);  % calculate variances in platform motion

    % make angular correction to wind data. Wind still in SHIP coordinates.
    [uvw_UH] = sonic(Vel_UH,dr,euler_UH,uvwplat_UH,R_UH);

    %*********************************************************************
    % now check for phase shift in uvwplat...
    % calculate w:wplat correlation over lag range of +/- 40 points
    [wplatWcorr,Wlags] = xcorr(uvwplat_UH(3,:),uvw_UH(3,:),20,'coeff');

    % find index for minimum in correlation
    [minWCorr, minWLagIndex] = min(wplatWcorr);

    % Compute # of data points to shift data where total #lags is 40*2+1.
    %    i.e. LagPts = -40 for index = 1
    LagPts = minWLagIndex-21;

    % Shift uvwplat by LagPts
    if LagPts < 0
     uvwplat_UH(:,-LagPts+1:length(uvwplat_UH))=uvwplat_UH(:,1:length(uvwplat_UH)+LagPts);
    elseif LagPts > 0
     uvwplat_UH(:,1:length(uvwplat_UH)-LagPts)=uvwplat_UH(:,LagPts+1:length(uvwplat_UH));
    else
    end
    %*********************************************************************

    % Sum platform velocities and wind to get motion corrected winds
    uvw_UH = uvw_UH + uvwplat_UH;

%         % now reverse the sign to bring into meteorological convention
% uvw_UH(1,:) = -uvw_UH(1,:);
% uvw_UH(2,:) = -uvw_UH(2,:);

% figure;plot(tref,uvwplat(3,:),time_UH,uvwplat_UH(3,:),'g')
%save the motion data for UH with proper sign switch for righ-handed coord. system...
motmatall_UH=[time_UH' -uvwplat_UH(1,:)' -uvwplat_UH(2,:)' uvwplat_UH(3,:)' -xyzplat_UH(1,:)' -xyzplat_UH(2,:)' xyzplat_UH(3,:)' euler_UH(1,:)' euler_UH(2,:)' euler_UH(3,:)' deg_rate_UH(1,:)' deg_rate_UH(2,:)' deg_rate_UH(3,:)' Plat_UH(1,:)' Plat_UH(2,:)' Plat_UH(3,:)'];
if printit
sdg=['save ', drive_w '\MotionData\HiWinGS2013_UHmotionfile_',jd hr '.prn motmatall_UH -ascii'];
disp(sdg)
eval(sdg)
end
%%finishes correction, compute fluxes and save

%adds low frequency components to u and v
uvw_UH(1,:)=uvw_UH(1,:)+Ngps;
uvw_UH(2,:)=uvw_UH(2,:)-Egps;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%reshape matrices in 10min format, and compute fluxes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear u_UH v_UH w_UH t_UH
u_UH=reshape(uvw_UH(1,:),6000,6);  %m/s
v_UH=reshape(uvw_UH(2,:),6000,6);  %m/s
w_UH=reshape(uvw_UH(3,:),6000,6);  %m/s
t_UH=reshape(Tsonic_UH,6000,6);  %m/s

uplat_UH=reshape(uvwplat_UH(1,:),6000,6);  %m/s
vplat_UH=reshape(uvwplat_UH(2,:),6000,6);  %m/s
wplat_UH=reshape(uvwplat_UH(3,:),6000,6);  %m/s

% %  Remove edge effects (due to filters) - shall remove that on u, v, w but
% %  do it only on w here
% npts=30*fs;              %30s will be removed on either end seconds for now
% w(1:npts)=NaN;  w(end-npts:end)=NaN;

% rotate and compute fluxes
Ustream=ones(6000,6)+NaN;Vstream=ones(6000,6)+NaN;Wstream=ones(6000,6)+NaN;
alpha0=ones(1,6)+NaN;
beta0=ones(1,6)+NaN;
wu_UH=ones(1,6)+NaN;wu2_UH=ones(1,6)+NaN;
wv_UH=ones(1,6)+NaN;wv2_UH=ones(1,6)+NaN;
wt_UH=ones(1,6)+NaN;wt2_UH=ones(1,6)+NaN;
Ubar_UH=ones(1,6)+NaN;Vbar_UH=ones(1,6)+NaN;Wbar_UH=ones(1,6)+NaN;Tbar_UH=ones(1,6)+NaN;
Ustd_UH=ones(1,6)+NaN;Vstd_UH=ones(1,6)+NaN;Wstd_UH=ones(1,6)+NaN;Tstd_UH=ones(1,6)+NaN;
Uj_UH=[];Vj_UH=[];Wj_UH=[];
SU_UH=ones(6,48)+NaN;SV_UH=ones(6,48)+NaN;SW_UH=ones(6,48)+NaN;St_UH=ones(6,48)+NaN;
Cuw_UH=ones(6,48)+NaN;Cvw_UH=ones(6,48)+NaN;Ctw_UH=ones(6,48)+NaN; Fson_UH=ones(6,48)+NaN;
for ii=1:6
    u_UH(:,ii)=u_UH(:,ii)-nanmean(uplat_UH(:,ii));  %removes mean platform velocity to u
    v_UH(:,ii)=v_UH(:,ii)-nanmean(vplat_UH(:,ii));  %removes mean platform velocity to v
    w_UH(:,ii)=w_UH(:,ii)-nanmean(wplat_UH(:,ii));  %removes mean platform velocity to w
    ll=find(~isnan(w_UH(:,ii)));
    [ustream_UH,vstream_UH,wstream_UH,alpha0(ii),beta0(ii)]=rotcorr(u_UH(ll,ii),v_UH(ll,ii),w_UH(ll,ii)); %rotation into the stream to compute fluxes
%     if isempty(ustream_UH)
%         ustream_UH=ones(1,6000)+NaN;vstream_UH=ones(1,6000)+NaN;wstream_UH=ones(1,6000)+NaN;
%     end
    covwu=cov(ustream_UH,wstream_UH);     covwu2=cov(detrend(ustream_UH,'constant'),detrend(wstream_UH,'constant')); 
    covwv=cov(vstream_UH,wstream_UH);     covwv2=cov(detrend(vstream_UH,'constant'),detrend(wstream_UH,'constant')); 
    covwt=cov(t_UH(ll,ii),wstream_UH);     covwt2=cov(detrend(t_UH(ll,ii),'constant'),detrend(wstream_UH,'constant')); 

    wu_UH(ii)=covwu(1,2);  %computes w'u'
    wu2_UH(ii)=covwu2(1,2);  %computes w'u'
    wv_UH(ii)=covwv(1,2);  %computes w'v'
    wv2_UH(ii)=covwv2(1,2);  %computes w'v'
    wt_UH(ii)=covwt(1,2);  %computes w'v'
    wt2_UH(ii)=covwt2(1,2);  %computes w'v'
    
    Ubar_UH(ii)=nanmean(ustream_UH);Ustd_UH(ii)=nanstd(ustream_UH);
    Vbar_UH(ii)=nanmean(vstream_UH);Vstd_UH(ii)=nanstd(vstream_UH);
    Wbar_UH(ii)=nanmean(wstream_UH);Wstd_UH(ii)=nanstd(wstream_UH);
    Tbar_UH(ii)=nanmean(t_UH(ll,ii));Tstd_UH(ii)=nanstd(t_UH(ll,ii));
    
    Uj_UH=[Uj_UH; ustream_UH];
    Vj_UH=[Vj_UH; vstream_UH];
    Wj_UH=[Wj_UH; wstream_UH];
    
    [SU,Fson_U]=psd2(detrend(ustream_UH),6000,10,hamming(6000));[SU_UH(ii,:),Fson_UH(ii,:)]=specsmoo(SU,10);
    [SV,Fson_V]=psd2(detrend(vstream_UH),6000,10,hamming(6000));[SV_UH(ii,:),Fson_V]=specsmoo(SV,10);
    [SW,Fson_W]=psd2(detrend(wstream_UH),6000,10,hamming(6000));[SW_UH(ii,:),Fson_W]=specsmoo(SW,10);
    [St,Fson_t]=psd2(detrend(t_UH(ll,ii)),6000,10,hamming(6000));[St_UH(ii,:),Fson_t]=specsmoo(St,10);
    [Cuw,Fson_uw]=csd2(detrend(wstream_UH),detrend(ustream_UH),6000,10,hamming(6000));[Cuw_UH(ii,:),Fson_uw]=specsmoo(Cuw,10);
    [Cvw,Fson_vw]=csd2(detrend(wstream_UH),detrend(vstream_UH),6000,10,hamming(6000));[Cvw_UH(ii,:),Fson_vw]=specsmoo(Cvw,10);
    [Cvw,Fson_vw]=csd2(detrend(t_UH(ll,ii)),detrend(vstream_UH),6000,10,hamming(6000));[Cvw_UH(ii,:),Fson_vw]=specsmoo(Cvw,10);
end;

if hhh==12 | hhh==00
    figure(7);hold on;semilogx(Fson_UH(1,:),Fson_UH(1,:).*nanmean(Cuw_UH),'k-')
    if printit==1
         print(graphdevice,[drive_w '\Cospectra_' num2str(ddd) num2str(hhh) graphformat]);
    end;
    
    figure(12);hold on;loglog(Fson_UH(1,:),Fson_UH(1,:).*nanmean(SW_UH),'k-',Fson_UH(1,:),Fson_UH(1,:).*nanmean(SU_UH),'m-')
    if printit==1
        print(graphdevice,[drive_w '\Psd_v2_' num2str(ddd) num2str(hhh) graphformat]);
    end;   

end;

if printit
dfx=['0',num2str(ddd),'0',num2str(hhh)];
    specview_UH=[jd_UH(([1:6]-1)*6000+1)' Fson_UH SU_UH SV_UH SW_UH St_UH Cuw_UH Cvw_UH Ctw_UH];    
    sdg=['save ', drive_w, '\procturb\sp_UH',dfx,dcname '.prn specview_UH -ascii'];
    disp(sdg)
    eval(sdg)

    fluxview_UH=[jd_UH(([1:6]-1)*6000+1); wu_UH; wu2_UH; wv_UH; wv2_UH; wt_UH; wt2_UH; Ubar_UH; Vbar_UH; Wbar_UH; Tbar_UH; Ustd_UH; Vstd_UH; Wstd_UH; Tstd_UH; alpha0; beta0]';    
    sdg=['save ', drive_w, '\procturb\da_UH',dfx,dcname '.prn fluxview_UH -ascii'];
    disp(sdg)
    eval(sdg)
end
        
u_UH=reshape(U_UH,6000,6);  %m/s
v_UH=reshape(V_UH,6000,6);  %m/s
% w_UH=reshape(W_UH,6000,6);  %m/s
reldir_UH=reshape(atan2(v_UH,-u_UH)*180/pi,36000,1);
ii=find(reldir_UH<0);reldir_UH(ii)=reldir_UH(ii)+360;
%     save motion data for other flux computation
uvwmat_UH=[time_UH' Uj_UH Vj_UH Wj_UH reldir_UH];

if printit
sdg=['save ', drive_w '\MotionData\HiWinGS2013_UH_UVWstream_',jd hr, dcname '.prn uvwmat_UH -ascii'];
eval(sdg)
end