%first pass correction to riegl data for DYNAMO2011
close('all');
fclose('all');
clear;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%defines parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
datasetv='_v1';
read_parameters_HiWinGS_2013;
expp='HiWinGS_2013';
drive='D:\Desktop\Data\CRUISES\HiWinGS_2013\Knorr\flux\Raw';
drive_w='D:\Desktop\Data\CRUISES\HiWinGS_2013\flux_data';  
graphformat='.png';  %select graphics format files
graphdevice='-dpng'; %select graphic device
clearit=1;
printit=1;
year='2013';
rge=283:300;

for ddd=rge    
    [m,d]=yd2md(str2num(year), ddd);
    Vdate=[str2num(year) m d];
    jd=sprintf('%03i',ddd);
    
    for hhh=00:23;   %
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %reads in the Riegl
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        hr=sprintf('%02i',hhh);
        e=[way_raw_data_flux, year(3:4),jd,'\lsr0' year(3:4) jd hr '_raw.txt'];
        disp(['Reading altimeter file ',e]);
        [sig_wave_ht,wave,timer,jd_wave,amplitude]=Riegl_reader(e,ddd,hhh);

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %loads the heave
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        motdata=load(['D:\Desktop\Data\CRUISES\HiWinGS_2013\flux_data\MotionData\HiWinGS2013_motionfile_',jd hr,'.prn']);        
        tmotion=motdata(:,1);
        heave=motdata(:,7);
        roll=motdata(:,8);  
        pitch=motdata(:,9); 
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %do correction
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        if length(wave(isnan(wave)))>=length(wave)-1
            continue
        end
        waveint=interp1(timer,wave,tmotion,'spline');
        %-----------------------------------------------------------------
        %>>>>>interpolates heave on wave as most of time size(wave) < size(motion)
        heaveint=interp1(tmotion,heave,timer,'spline');
        rollint=interp1(tmotion,roll,timer,'spline');        
        pitchint=interp1(tmotion,pitch,timer,'spline');        
                %removes first and last 20s for motcorr filter tapering issues
        heaveint(1:200)=NaN;        heaveint(end-199:end)=NaN;
        rollint(1:200)=NaN;        rollint(end-199:end)=NaN;
        pitchint(1:200)=NaN;        pitchint(end-199:end)=NaN;

        %-----------------------------------------------------------------
        X=waveint;Y=heave;XYbase=tmotion;
        %Riegl is a distance measure.  You may need to flip the distance to
        %get the uncorrected wave height and then subtract the heave to correct the wave height.
        %watch for signs...
        wavecorrected=-X + Y;  

        %look for potential lags between Riegl and heave. Should be 0...
        %         X=waveint;Y=heave;XYbase=tmotion;
        %             wavecorrected=X - Y;
                    t=XYbase;      
        %         try
        %             lagflag = findlag(X,Y,10,0);
        %         catch
                    lagflag = 0;
        %         end;
        %         if lagflag==0
        %             wavecorrected=X - Y;
        %             t=XYbase;
        %         elseif lagflag > 0,
        %            wavecorrected=X(1:length(X)-lagflag) - Y(lagflag+1:length(X));
        %            t=XYbase(1:length(X)-lagflag);
        %         elseif lagflag < 0
        %            wavecorrected=X(-lagflag+1:length(X)) - Y(1:length(X)+lagflag);
        %            t=XYbase(-lagflag+1:length(X));
        %         end;       

        sig_wavecorrected_ht=4.*sqrt(nanvar(wavecorrected));

        %-----------------------------------------------------------------
        % Try better correction on the Riegl from CF equation
        % eta=z-L*[sin(theta)sin(pit)+cos(theta)*SQRT(1-(sin(pit)^2-(sin(roll))^2)];
        % where theta is the mean tilt forward of the riegl
        % pit is pitch (bow down)
        % roll is roll (port up).
        % z is obtained from our motion system that is co-located with the riegl.
        % z=Y+14.5;L=X+14.5;pit=pitchint;rol=rollint;theta=10*pi/180;
        z=Y+14.5;L=X+14.5;pit=pitch;rol=roll;theta=10*pi/180;
        % eta=z-L*[sin(theta).*sin(pit)+cos(theta).*sqrt(1-(sin(pit).^2-(sin(rol)).^2))];
        eta=z-L.*[sin(theta).*sin(pit)+cos(theta).*sqrt(1-(sin(pit).^2-(sin(rol)).^2))];
        eta=eta-nanmedian(eta);

        wavecorrected_cf=eta;
        sig_wavecorrected_ht_cf=4.*sqrt(nanvar(eta));

        %-----------------------------------------------------------------
        %try Robin idea to use complementary filters to retain real HF signal from
        %the Riegl 
        wp=0.25/(10/2);
        ws=0.8*wp;
        [n,wn]=buttord(wp,ws,3,7);
        [bhi,ahi]=butter(n,wn,'low');
        % test=filter(bhi,ahi,wave(~isnan(wave)));%deg1/30
        waveLF=filtfilt(bhi,ahi,waveint);%deg1/30
        heaveLF=filtfilt(bhi,ahi,heave);%deg1/30

        waveHF=waveint-waveLF;

        wavecorrected_cf_rp=-(-waveint + heave.*[sin(theta).*sin(pitch)+cos(theta).*sqrt(1-(sin(pitch).^2-(sin(roll)).^2))])+waveHF;
        sig_wavecorrected_ht_cf_rp=4.*sqrt(nanvar(wavecorrected_cf_rp));

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        if ismember(hhh,[0 6 12 18]) & length(wavecorrected(~isnan(wavecorrected)))>10
            figure(1);clf;plot(timer,wave,'b.-',timer-lagflag/10,-heaveint,'r.-',t,wavecorrected,'g.-',[0 3600],[0 0],'--');xlim([2000 2200])
            legend('Non-compensated significant wave height','heave','Corrected wave height');grid
            title(sprintf('Sig wave height (%03i%02i). Non-compensated:%.2f m / Compensated:%.2f m',ddd,hhh,sig_wave_ht,sig_wavecorrected_ht))         
            if printit==1
                print('-djpeg90 ',['D:\Desktop\Data\CRUISES\HiWinGS_2013\flux_data\Riegl_corrected\Wavetime_' num2str(ddd) num2str(hhh) '.jpg']);
            end; 
            [Swave,Fwave]=psd2(detrend(X(~isnan(X))),length(X(~isnan(X))),10);[Swaves,Fwaves]=specsmoo(Swave,10);
            [Swavec,Fwavec]=psd2(detrend(wavecorrected(~isnan(wavecorrected))),length(wavecorrected(~isnan(wavecorrected))),10);[Swavesc,Fwavesc]=specsmoo(Swavec,10);
            if length(Swavesc)>10
                figure(2);clf;loglog(Fwaves,Fwaves.*Swaves,'b',Fwavesc,Fwavesc.*Swavesc,'g',Fwavesc(end-10:end),Fwavesc(end-10:end).^(-4)*Swavesc(end-12),'r','LineWidth',2);xlabel('Frequency (Hz)');ylabel('f*Sw(f)  (m^2)')
                legend('wave height uncorrected','wave height corrected','f^{-4}','Location','NorthWest');grid
                title(sprintf('Wave Height PSD (%03i%02i). Non-compensated:%.2f m / Compensated:%.2f m',ddd,hhh,sig_wave_ht,sig_wavecorrected_ht))         
                if printit==1
                    print('-djpeg90 ',['D:\Desktop\Data\CRUISES\HiWinGS_2013\flux_data\Riegl_corrected\SpecWave_' num2str(ddd) num2str(hhh) '.jpg']);
                end; 
            end
        end;
            % 

        %  07/17/2013  -   calculate the peak period/frequency of the waves 
        [Swavec,Fwavec]=psd2(detrend(wavecorrected(~isnan(wavecorrected))),length(wavecorrected(~isnan(wavecorrected))),10);[Swavesc,Fwavesc]=specsmoo(Swavec,10);
        Fpeak=NaN;
        if length(Swavesc)>10
            Sign=Fwavesc.*Swavesc;
            ii=find(Fwavesc>=0.01 & Fwavesc<=0.5);  %limits search from 0.01Hz to 0.5Hz
            Fpeak=Fwavesc(Sign==max(Sign(ii)));
        end
            
       
        disp('<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>')
        disp(sprintf('sig_wave_ht:%.2f m   /   sig_wavecorrected_ht:%.2f m /   cf:%.2f m / cfrp: %.2f m',sig_wave_ht,sig_wavecorrected_ht,sig_wavecorrected_ht_cf,sig_wavecorrected_ht_cf_rp))
        disp('<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>>>>>>')
      
        
        clear wave waveint heave
    end    
end        
