%runs motion correction with UH system and check the motion pack cal...

close('all');
fclose('all');
clear;

datasetv='_v1';
read_parameters_HiWinGS_2013;

expp='HiWinGS_2013';
drive='C:\data\CRUISES\HiWinGS_2013\Knorr\flux\Raw';
drive_w='C:\data\CRUISES\HiWinGS_2013\flux_data';  
graphformat='.png';  %select graphics format files
graphdevice='-dpng'; %select graphic device
clearit=1;
printit=0;
plotit=0;
year='2013';
yyyy='2013';

rge=283;

% Define position vector for distance between MotionPak and sonic volume.
% 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=[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);   

% dcname='_non_corrected';
% dcname='_offset_adjusted';
% dcname='_fullcal';
dcname='_fullcal2';

for ddd=rge
    [m,d]=yd2md(str2num(year), ddd);
    Vdate=[str2num(year) m d];
    jd=sprintf('%03i',ddd);
    for hhh=12;   %
        hr=sprintf('%02i',hhh);
       [jd_UH,AccX,AccY,AccZ,RateX,RateY,RateZ,U,V,W,Tsonic,LagPulse,~]=read_dms_HiWinGS_2013(ddd,hhh,yyyy,NaN);
        % no heading data so set to zero, keeping winds in ship coordinates
        heading = U*0;

        Vel=[U;  % U, V, and W are the Gill Solent sonic velocities in the ship
             V;  %   frame of reference              
             W];            

%            deg_rate=[RateX';    % ratex, ratey, ratez are the rotation rates from 
%                       RateY';    % the motion-pak (in radians/sec)
%                       RateZ'];
%  
 
        if strcmp(dcname,'_non_corrected');
            Plat = [AccX';    % accx, accy, accz are the accelerations in the 
                    AccY';    % bow-stern, port-starboard, and up-down directions,
                    AccZ'];   % respectively
            deg_rate=[RateX';    % ratex, ratey, ratez are the rotation rates from 
                      RateY';    % the motion-pak (in radians/sec)
                      RateZ'];
        elseif strcmp(dcname,'_offset_adjusted');
            Plat = [AccX'+0.095;    % accx, accy, accz are the accelerations in the 
                    AccY'+0.233;    % bow-stern, port-starboard, and up-down directions,
                    AccZ'+0.433];   % respectively
           deg_rate=[RateX'-0.00022;    % ratex, ratey, ratez are the rotation rates from 
                      RateY'+0.0039;    % the motion-pak (in radians/sec)
                      RateZ'-0.00029];
        elseif strcmp(dcname,'_fullcal');
            Plat = [(AccX'+0.09)/0.89;    % accx, accy, accz are the accelerations in the 
                    (AccY'+0.24)/0.89;    % bow-stern, port-starboard, and up-down directions,
%                     (AccZ'-0.064)/0.94];   % respectively
                    (AccZ'+0.433)/1];   % respectively
           deg_rate=[(RateX'-0.00022)/1.01;    % ratex, ratey, ratez are the rotation rates from 
                      (RateY'+0.0039)/1.01;    % the motion-pak (in radians/sec)
                      (RateZ'-0.000286)/1.03];
        elseif strcmp(dcname,'_fullcal2');
            Plat = [(AccX')/0.89;    % accx, accy, accz are the accelerations in the 
                    (AccY')/0.89;    % bow-stern, port-starboard, and up-down directions,
                    (AccZ')/0.95];   % respectively
           deg_rate=[(RateX')/1.01;    % ratex, ratey, ratez are the rotation rates from 
                      (RateY')/1.01;    % the motion-pak (in radians/sec)
                      (RateZ')/1.03];
                  
        end

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

        % compute platform velocities with updated UCONN method:
        % rotate, remove gravity, integrate, and high-pass filter.
        [acc,uvwplat,xyzplat] = accels(bhi,ahi,fs,Plat,euler);
        uvwplatVar = var(uvwplat,0,2);  % calculate variances in platform motion

        % make angular correction to wind data. Wind still in SHIP coordinates.
        [uvw] = sonic(Vel,dr,euler,uvwplat,R);
    
        %*********************************************************************
        % now check for phase shift in uvwplat...
        % calculate w:wplat correlation over lag range of +/- 40 points
        [wplatWcorr,Wlags] = xcorr(uvwplat(3,:),uvw(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(:,-LagPts+1:length(uvwplat))=uvwplat(:,1:length(uvwplat)+LagPts);
        elseif LagPts > 0
         uvwplat(:,1:length(uvwplat)-LagPts)=uvwplat(:,LagPts+1:length(uvwplat));
        else
        end
        %*********************************************************************

        % Sum platform velocities and wind to get motion corrected winds
        uvw_corrected = uvw + uvwplat;

            % now reverse the sign of u again to bring into meteorological
    % convention
    uvw_corrected(1,:) = -uvw_corrected(1,:);
    uvw_corrected(2,:) = -uvw_corrected(2,:);
    
        %computes spectra/cospectra and save data
        [SU,Fson_U]=psd2(detrend(uvw(1,:)),36000,10,hamming(36000));[SU,Fson_U]=specsmoo(SU,10);
        [SV,Fson_V]=psd2(detrend(uvw(2,:)),36000,10,hamming(36000));[SV,Fson_V]=specsmoo(SV,10);
        [SW,Fson_W]=psd2(detrend(uvw(3,:)),36000,10,hamming(36000));[SW,Fson_W]=specsmoo(SW,10);
        if plotit
            figure(1);clf;loglog(Fson_U,Fson_U.*SU,'b--',Fson_V,Fson_V.*SV,'r--',Fson_W,Fson_W.*SW,'g--')
            title('Normalized Spectra of raw U (blue), V (red), W (green)');
        end

        [SUc,Fson_Uc]=psd2(detrend(uvw_corrected(1,:)),36000,10,hamming(36000));[SUc,Fson_Uc]=specsmoo(SUc,10);
        [SVc,Fson_Vc]=psd2(detrend(uvw_corrected(2,:)),36000,10,hamming(36000));[SVc,Fson_Vc]=specsmoo(SVc,10);
        [SWc,Fson_Wc]=psd2(detrend(uvw_corrected(3,:)),36000,10,hamming(36000));[SWc,Fson_Wc]=specsmoo(SWc,10);
        if plotit
            hold on;loglog(Fson_Uc,Fson_Uc.*SUc,'b',Fson_Vc,Fson_Vc.*SVc,'r',Fson_Wc,Fson_Wc.*SWc,'g')
        end

        [Cuw,Fson_uw]=csd2(detrend(uvw(3,:)),detrend(uvw(1,:)),36000,10,hamming(36000));[Cuw,Fson_uw]=specsmoo(Cuw,10);
        [Cvw,Fson_vw]=csd2(detrend(uvw(3,:)),detrend(uvw(2,:)),36000,10,hamming(36000));[Cvw,Fson_vw]=specsmoo(Cvw,10);
        [Cuwc,Fson_uwc]=csd2(detrend(uvw_corrected(3,:)),detrend(uvw_corrected(1,:)),36000,10,hamming(36000));[Cuwc,Fson_uwc]=specsmoo(Cuwc,10);
        [Cvwc,Fson_vwc]=csd2(detrend(uvw_corrected(3,:)),detrend(uvw_corrected(2,:)),36000,10,hamming(36000));[Cvwc,Fson_vwc]=specsmoo(Cvwc,10);
        if plotit
            figure(2);clf;semilogx(Fson_uw,Fson_uw.*Cuw,'--',Fson_uwc,Fson_uwc.*Cuwc);grid;ylim([-0.05 0.1])
            figure(3);clf;semilogx(Fson_vw,Fson_vw.*Cvw,'--',Fson_vwc,Fson_vwc.*Cvwc);grid;ylim([-0.05 0.1])
        end

        if printit
            specview=[ones(1,62)*(ddd+hhh/24); Fson_U; SU; SV; SW; SUc; SVc; SWc; Cuw; Cvw; Cuwc; Cvwc]';
            sdg=['save ', 'G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_',jd,hr,dcname '.prn specview -ascii'];
            disp(sdg)
            eval(sdg)
        end
    end
end
% 
% % 
% %%%%% look at results
% % filecat(['G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_*_non_corrected.prn'],['G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_all_non_corrected.txt']);
% % filecat(['G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_*_offset_adjusted.prn'],['G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_all_offset_adjusted.txt']);
% % filecat(['G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_*_fullcal.prn'],['G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_all_fullcal.txt']);
% 
% SA_non_corrected=load(['G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_all_non_corrected.txt']);
% SA_offset_adjusted=load(['G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_all_offset_adjusted.txt']);
% SA_fullcal=load(['G:\Data\CRUISES\HiWinGS_2013\Knorr\UH_DMS\spec_all_fullcal2.txt']);
% 
% doy_non_corrected=reshape(SA_non_corrected(:,1),62,[]);
% Fson_U_non_corrected=reshape(SA_non_corrected(:,2),62,[]);
% SU_non_corrected=reshape(SA_non_corrected(:,3),62,[]);
% SV_non_corrected=reshape(SA_non_corrected(:,4),62,[]);
% SW_non_corrected=reshape(SA_non_corrected(:,5),62,[]);
% SUc_non_corrected=reshape(SA_non_corrected(:,6),62,[]);
% SVc_non_corrected=reshape(SA_non_corrected(:,7),62,[]);
% SWc_non_corrected=reshape(SA_non_corrected(:,8),62,[]);
% Cuw_non_corrected=reshape(SA_non_corrected(:,9),62,[]);
% Cvw_non_corrected=reshape(SA_non_corrected(:,10),62,[]);
% Cuwc_non_corrected=reshape(SA_non_corrected(:,11),62,[]);
% Cvwc_non_corrected=reshape(SA_non_corrected(:,12),62,[]);
% 
% Fson_U_offset_adjusted=reshape(SA_offset_adjusted(:,2),62,[]);
% SU_offset_adjusted=reshape(SA_offset_adjusted(:,3),62,[]);
% SV_offset_adjusted=reshape(SA_offset_adjusted(:,4),62,[]);
% SW_offset_adjusted=reshape(SA_offset_adjusted(:,5),62,[]);
% SUc_offset_adjusted=reshape(SA_offset_adjusted(:,6),62,[]);
% SVc_offset_adjusted=reshape(SA_offset_adjusted(:,7),62,[]);
% SWc_offset_adjusted=reshape(SA_offset_adjusted(:,8),62,[]);
% Cuw_offset_adjusted=reshape(SA_offset_adjusted(:,9),62,[]);
% Cvw_offset_adjusted=reshape(SA_offset_adjusted(:,10),62,[]);
% Cuwc_offset_adjusted=reshape(SA_offset_adjusted(:,11),62,[]);
% Cvwc_offset_adjusted=reshape(SA_offset_adjusted(:,12),62,[]);
% 
% doy_fullcal=reshape(SA_fullcal(:,1),62,[]);
% Fson_U_fullcal=reshape(SA_fullcal(:,2),62,[]);
% SU_fullcal=reshape(SA_fullcal(:,3),62,[]);
% SV_fullcal=reshape(SA_fullcal(:,4),62,[]);
% SW_fullcal=reshape(SA_fullcal(:,5),62,[]);
% SUc_fullcal=reshape(SA_fullcal(:,6),62,[]);
% SVc_fullcal=reshape(SA_fullcal(:,7),62,[]);
% SWc_fullcal=reshape(SA_fullcal(:,8),62,[]);
% Cuw_fullcal=reshape(SA_fullcal(:,9),62,[]);
% Cvw_fullcal=reshape(SA_fullcal(:,10),62,[]);
% Cuwc_fullcal=reshape(SA_fullcal(:,11),62,[]);
% Cvwc_fullcal=reshape(SA_fullcal(:,12),62,[]);
% 
% Fson=Fson_U_non_corrected(:,1);
% figure(1);clf;
% set(gcf,'defaultaxesfonts',12)
% set(gca,'fontname','time','fontsize',12);
% subplot(2,2,1)
%     loglog(Fson,Fson.*nanmedian(SU_non_corrected')','k--',Fson,Fson.*nanmedian(SUc_non_corrected')','b',Fson,Fson.*nanmedian(SUc_offset_adjusted')','g',Fson,Fson.*nanmedian(SUc_fullcal')','r','LineWidth',2);grid
%     legend('S_{xx} raw','S_{xx} (MP non corrected)','S_{xx} (MP offset adjusted)','S_{xx} (MP fullcal)');
%     title(['HIWINGS2013 - S_{x} averaged (282 to 284)']);ylabel('f*S_{U}(f)');
% subplot(2,2,2)
%     loglog(Fson,Fson.*nanmedian(SV_non_corrected')','k--',Fson,Fson.*nanmedian(SVc_non_corrected')','b',Fson,Fson.*nanmedian(SVc_offset_adjusted')','g',Fson,Fson.*nanmedian(SVc_fullcal')','r','LineWidth',2);grid
% %     legend('S_V raw','S_V (MP non corrected)','S_V (MP offset adjusted)','S_V (MP fullcal)');
%     ylabel('f*S_{V}(f)');
% subplot(2,2,3)
%     loglog(Fson,Fson.*nanmedian(SW_non_corrected')','k--',Fson,Fson.*nanmedian(SWc_non_corrected')','b',Fson,Fson.*nanmedian(SWc_offset_adjusted')','g',Fson,Fson.*nanmedian(SWc_fullcal')','r','LineWidth',2);grid
% %     legend('S_W raw','S_W (MP non corrected)','S_W (MP offset adjusted)','S_W (MP fullcal)')
%     xlabel('freqency (Hz)');ylabel('f*S_{W}(f)');
% % orient tall
% 
% figure(2);clf;
% set(gcf,'defaultaxesfonts',12)
% set(gca,'fontname','time','fontsize',12);
% subplot(2,1,1)
%     semilogx(Fson,Fson.*nanmedian(-Cuw_non_corrected')','k--',Fson,Fson.*nanmedian(-Cuwc_non_corrected')','b',Fson,Fson.*nanmedian(-Cuwc_offset_adjusted')','g',Fson,Fson.*nanmedian(Cuwc_fullcal')','r','LineWidth',2);grid
%     legend('C_{xy} raw','Cxy (MP non corrected)','C_{xy} (MP offset adjusted)','C_{xy} (MP gain adjusted)');ylim([-0.03 0.01])
%     title(['HIWINGS2013 - C_{xy} averaged (282 to 284)']);ylabel('f*C_{UW}(f)');
% subplot(2,1,2)
%     semilogx(Fson,Fson.*nanmedian(Cvw_non_corrected')','k--',Fson,Fson.*nanmedian(Cvwc_non_corrected')','b',Fson,Fson.*nanmedian(Cvwc_offset_adjusted')','g',Fson,Fson.*nanmedian(Cvwc_fullcal')','r','LineWidth',2);grid
%     ylabel('f*C_{VW}(f)');;ylim([-0.01 0.01])
%     
% % 
% % yday_spec=(doy_non_corrected(1,:));    
% % A =importdata('S:\cruises\HIWINGS_2013\Knorr\Scientific_analysis\MeanNBulkfluxes\HiWinGS_2013_hrMeansNFluxes_r1.txt');
% % A=A.data;    
% % yday=A(:,1)-0.5/24;    %Decimal yearday (UTC)
% % rdir=A(:,13);   %Relative Wind direction (deg) from
% % 
% % ii=find(yday>=yday_spec(1) & yday<=yday_spec(end));
% % ii=[ii(1)-1; ii; ii(end)+1];
% % 
% % ii=find(yday_spec>284+5/24);
% % figure(3);clf;
% % set(gcf,'defaultaxesfonts',12)
% % set(gca,'fontname','time','fontsize',12);
% % subplot(2,2,1)
% %     loglog(Fson,Fson.*nanmedian(SU_non_corrected(:,ii)')','k--',Fson,Fson.*nanmedian(SUc_non_corrected(:,ii)')','b',Fson,Fson.*nanmedian(SUc_offset_adjusted(:,ii)')','g',Fson,Fson.*nanmedian(SUc_fullcal(:,ii)')','r','LineWidth',2);grid
% %     legend('S_{xx} raw','S_{xx} (MP non corrected)','S_{xx} (MP offset adjusted)','S_{xx} (MP fullcal)');
% %     title(['HIWINGS2013 - S_{x} averaged (282 to 284)']);ylabel('f*S_{U}(f)');
% % subplot(2,2,2)
% %     loglog(Fson,Fson.*nanmedian(SV_non_corrected(:,ii)')','k--',Fson,Fson.*nanmedian(SVc_non_corrected(:,ii)')','b',Fson,Fson.*nanmedian(SVc_offset_adjusted(:,ii)')','g',Fson,Fson.*nanmedian(SVc_fullcal(:,ii)')','r','LineWidth',2);grid
% % %     legend('S_V raw','S_V (MP non corrected)','S_V (MP offset adjusted)','S_V (MP fullcal)');
% %     ylabel('f*S_{V}(f)');
% % subplot(2,2,3)
% %     loglog(Fson,Fson.*nanmedian(SW_non_corrected(:,ii)')','k--',Fson,Fson.*nanmedian(SWc_non_corrected(:,ii)')','b',Fson,Fson.*nanmedian(SWc_offset_adjusted(:,ii)')','g',Fson,Fson.*nanmedian(SWc_fullcal(:,ii)')','r','LineWidth',2);grid
% % %     legend('S_W raw','S_W (MP non corrected)','S_W (MP offset adjusted)','S_W (MP fullcal)')
% %     xlabel('freqency (Hz)');ylabel('f*S_{W}(f)');
% % 
% % figure(4);clf;
% % set(gcf,'defaultaxesfonts',12)
% % set(gca,'fontname','time','fontsize',12);
% % subplot(1,2,1)
% %     semilogx(Fson,Fson.*nanmedian(Cuw_non_corrected(:,ii)')','k--',Fson,Fson.*nanmedian(Cuwc_non_corrected(:,ii)')','b',Fson,Fson.*nanmedian(Cuwc_offset_adjusted(:,ii)')','g',Fson,Fson.*nanmedian(Cuwc_fullcal(:,ii)')','r','LineWidth',2);grid
% %     legend('C_{xy} raw','Cxy (MP non corrected)','C_{xy} (MP offset adjusted)','C_{xy} (MP fullcal)');ylim([-0.05 0.1])
% %     title(['HIWINGS2013 - C_{xy} averaged (282 to 284)']);ylabel('f*C_{UW}(f)');
% % subplot(1,2,2)
% %     semilogx(Fson,Fson.*nanmedian(Cvw_non_corrected(:,ii)')','k--',Fson,Fson.*nanmedian(Cvwc_non_corrected(:,ii)')','b',Fson,Fson.*nanmedian(Cvwc_offset_adjusted(:,ii)')','g',Fson,Fson.*nanmedian(Cvwc_fullcal(:,ii)')','r','LineWidth',2);grid
% %     ylabel('f*C_{VW}(f)');;ylim([-0.05 0.05])