% FMCW Quick Look Processing
% 08/29/12
% Author: Ben Panzer

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% USER INPUT
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% =============================================================
% FLIGHT NAME, DATE, DATA LOCATION, GPS LOCATION, OUTPUT DIRECTORY
% =============================================================
param.month         = 9;
param.day           = 5;
param.year          = 2012;
param.flight_name   = 'Test 2';
% param.gps_file      = 'C:\Users\bpanzer\Desktop\daq_test_4\gps_log3.txt';
param.gps_file      = '/Users/ben/Desktop/daq_test_4/gps_log3.txt';
% param.output_dir    = 'C:\Users\bpanzer\Desktop\20120905\';
param.output_dir    = '/Users/ben/Desktop/20120905/';
% param.data_dir      = 'C:\Users\bpanzer\Desktop\daq_test_4\';
param.data_dir      = '/Users/ben/Desktop/daq_test_4';
param.info_str      = sprintf('%s %02d/%02d/%04d',param.flight_name,param.month,param.day,param.year);
param.gps_file_time = 'utc';

% =============================================================
% DATA SETS AND PARAMETERS
% =============================================================
param.time_corr     = 1;
param.sampling_freq = 31.25e6;
param.prf           = 1e3;
param.start_freq    = 214e6;
param.stop_freq     = 305e6;
param.freq_mult     = 56;
param.pulse_len     = 500e-6;
param.nyquist_zone  = 1;            % Nyquist zone is 0 indexed 
param.start_idx     = 281;
param.chirp_rate    = floor((param.stop_freq-param.start_freq)/param.pulse_len * (4/1e9) / 1e9 * 2^32) * 1e9/2^32/(4/1e9) * param.freq_mult;

% =============================================================
% PROCESSING PARAMETERS
% =============================================================
param.no_gps        = 0;            % 1 if processing without GPS data
param.alt_corr      = 0;            % 1 if altitude correction is desired
param.coh_int       = 1;            % 1 if additional coherent integration is desired
param.presums       = 4;            % Number of coherent integrations to perform
param.st_win        = @hanning;     % Specify window by its Matlab function name to be used for coherent integration
param.st_hpf        = 0;            % 1 if slow-time HPF is desired, specs for HPF defined below
param.hpf.cutoff    = 0.2;          % Normalized cutoff frequency for the slow-time HPF
param.hpf.order     = 64;
param.hpf.a         = 1;
param.hpf.b         = fir1(param.hpf.order,param.hpf.cutoff,'high');
param.proc_win      = @hanning;     % Specify window by its Matlab function name to be used for windowing around the data before the FFT    
param.snow_density  = 0.3;
param.snow_eps_r    = (1+0.51*param.snow_density)^3;
c                   = 299792458;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% AUTOMATED SECTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Check for the existence of the output directory
if ~exist(param.output_dir,'dir')
    command = ['mkdir ' param.output_dir];
    system(command);
end

% Read in the GPS data
gps_data = read_gps_nmea(param.gps_file,struct('year',param.year,'month',param.month,'day',param.day,'time_reference',param.gps_file_time));
gps_data.gps_sod = epoch_to_sod(gps_data.gps_time);

% Change directory to the data and grab the filenames
cd(param.data_dir);
listing = dir(fullfile('*.dat'));

for data_idx = 1:length(listing)
    tic
    data_fn = listing(data_idx).name;
    fid = fopen(data_fn,'r','ieee-be');
    deadbeef = hex2dec('deadbeef');

    while 1
        tmp_search     = fread(fid,80000,'uint8');
        tmp_search_len = length(tmp_search);
        tmp_search     = tmp_search(1:(floor(numel(tmp_search)/4)*4));
        tmp            = tmp_search(1:end-3)*2^24+tmp_search(2:end-2)*2^16+ ...
        tmp_search(3:end-1)*2^8+tmp_search(4:end);

        if isempty(tmp_search)
            error('No Records found in %s',just_fn);
        end

        deadbeef_idx = find(tmp == deadbeef);

        if length(deadbeef_idx) >=2
            rec_len = (deadbeef_idx(2) - deadbeef_idx(1))/2;
        end

        if ~isempty(deadbeef_idx)
            fseek(fid,-tmp_search_len,'cof');
            fseek(fid,(deadbeef_idx(1)-1)*1,'cof');
            break;
        end

        fseek(fid,-4,'cof');
    end

    index = ftell(fid);

    % Read in data
    fseek(fid,index,'bof');
    fprintf('Reading in file %s\n',data_fn);
    Data = fread(fid,[rec_len,Inf],'uint16=>float32');
    fclose(fid);

    fprintf('Checking header data\n');
    bad_syncs = find(Data(1,:) ~= hex2dec('DEAD') | Data(2,:) ~= hex2dec('BEEF'));
    if ~isempty(bad_syncs)
        tic;
        fprintf('  Bad sync marks, reread data file the slow way (%.1f sec)\n', toc);
        fprintf('  Searching for individual sync marks (%.1f sec)\n', toc);
        finfo = frame_sync_info(data_fn);
        fprintf('  Reading in data file (%.1f sec)\n', toc);
        Data = zeros(rec_len,length(finfo.syncs),'single');
        fid = fopen(data_fn,'r','ieee-be');
        for rec = 1:length(finfo.syncs)
            fseek(fid,finfo.syncs(rec),-1);
            [tmp num_read] = fread(fid,rec_len,'uint16=>float32');
            if num_read < rec_len
                % Last record is short and caused left hand side to not
                % match right hand side size, just truncate data
                % and stop loading.
                Data = Data(:,1:rec-1);
                break;
            end
            Data(:,rec) = tmp;
        end
        fclose(fid);
        fprintf('  Done reading data file (%.1f sec)\n', toc);
    end
    

    % Resampling the data as samples are flipped
    rsmp = zeros(1,size(Data,1)-16);
    for i0 = 1:(size(Data,1)-16)/2
        rsmp(2*i0-1) = 2*i0 + 16;
        rsmp(2*i0) = 2*i0-1 + 16;
    end;
    
    fprintf('Processing data\n');
    hdr = double(Data(1:16,:));
    Data = Data(rsmp,1:end-1);

    % Find the location of the usable data
    max_range_delay   = param.sampling_freq/2 * (param.nyquist_zone+1) / param.chirp_rate;
    good_rbins(1)     = max(1,2 + ceil((146.85 + max_range_delay*param.sampling_freq) - param.start_idx));
    good_rbins(2)     = min(size(Data,1), floor(good_rbins(1) + (param.pulse_len-max_range_delay)*param.sampling_freq));
    Data              = Data(good_rbins(1):good_rbins(2),:);
    fft_len           = good_rbins(2)-good_rbins(1)+1;
    pixel_size_snow   = (param.sampling_freq.*c)./(2*fft_len.*param.chirp_rate.*sqrt(param.snow_eps_r));
    pixel_size_air    = (param.sampling_freq.*c)./(2*fft_len.*param.chirp_rate);
    delta_t           = param.sampling_freq./(2*fft_len.*param.chirp_rate);
    
    % Calculate radar time and position values
    if param.prf == 1000
        utc_sec  = hdr(9,:).*2^16 + hdr(10,:);
        utc_frac = hdr(11,:).*2^16 + hdr(12,:);
        utc_time = utc_sec/2+utc_frac/(4*param.sampling_freq);
        gps_time = utc_time + 15;
    elseif param.prf == 500
        utc_sec  = hdr(9,:).*2^16 + hdr(10,:);
        utc_frac = hdr(11,:).*2^16 + hdr(12,:);
        utc_time = utc_sec+utc_frac/(2*param.sampling_freq);
        gps_time = utc_time + 15;
    end

    % Interpolate latitude, longitude, and aircraft altitude to record time
    Latitude  = interp1(gps_data.gps_sod,gps_data.lat,gps_time);
    Longitude = interp1(gps_data.gps_sod,gps_data.lon,gps_time);
    Altitude  = interp1(gps_data.gps_sod,gps_data.elev,gps_time);
    
    % Windowing for fast and slow time
    fast_time_win = feval(param.proc_win,fft_len);
    slow_time_win = feval(param.st_win,param.presums);

    % Random records contain saturated triplets spaced by 3 and 2 or
    % quadruplets spaced by 2, 1, and 2.
    % Find these triplets and replace them with the mean of adjacent
    % samples
    for idx = 1:size(Data,2)
        bad_idx = find(Data(:,idx) < 4000);
        if (length(bad_idx) == 3) && all(diff(bad_idx) == [3; 2])
            if bad_idx(1) > 1
                Data(bad_idx(1),idx) = (Data(bad_idx(1)-1,idx)+Data(bad_idx(1)+1,idx))/2;
            else
                Data(bad_idx(1),idx) = Data(bad_idx(1)+1,idx);
            end
            Data(bad_idx(2),idx) = (Data(bad_idx(2)-2,idx)+Data(bad_idx(2)+1,idx))/2;
            Data(bad_idx(2)-1,idx) = (Data(bad_idx(2)-2,idx)+Data(bad_idx(2)+1,idx))/2;
            if bad_idx(3) < size(Data,1)
                Data(bad_idx(3),idx) = (Data(bad_idx(3)-1,idx)+Data(bad_idx(3)+1,idx))/2;
            else
                Data(bad_idx(3),idx) = Data(bad_idx(3)-1,idx);
            end
        elseif (length(bad_idx) == 4) && all(diff(bad_idx) == [2; 1; 2])
            if bad_idx(1) > 1
                Data(bad_idx(1),idx) = (Data(bad_idx(1)-1,idx)+Data(bad_idx(1)+1,idx))/2;
            else
                Data(bad_idx(1),idx) = Data(bad_idx(1)+1,idx);
            end
            Data(bad_idx(3),idx) = (Data(bad_idx(3)-2,idx)+Data(bad_idx(3)+1,idx))/2;
            Data(bad_idx(2),idx) = (Data(bad_idx(2)-1,idx)+Data(bad_idx(2)+2,idx))/2;
            if bad_idx(4) < size(Data,1)
                Data(bad_idx(4),idx) = (Data(bad_idx(4)-1,idx)+Data(bad_idx(4)+1,idx))/2;
            else
                Data(bad_idx(4),idx) = Data(bad_idx(4)-1,idx);
            end
        end
    end

    % Subtract the mean from the data (4 hardware averages results in a DC
    % bias)
    for st_idx = 1:size(Data,2)
        Data(:,st_idx) = Data(:,st_idx)-mean(Data(:,st_idx));
    end
    
    if param.coh_int
        % Coherent decimation of GPS data to correspond with coherently
        % integrated radar data
        gps_time     = fir_dec(gps_time,param.presums);
        Latitude     = fir_dec(Latitude,param.presums);
        Longitude    = fir_dec(Longitude,param.presums);
        Altitude     = fir_dec(Altitude,param.presums);

        % Perform coherent integrations
        siz          = size(Data);
        new_len      = floor(siz(2)/param.presums);
        nearest_len  = floor(siz(2)/param.presums)*param.presums;
        Data         = reshape(Data(:,1:nearest_len), ...
                            [siz(1) param.presums new_len]);
        tmp_Data      = zeros(size(Data,1),size(Data,3),'single');
        slow_time_win = repmat(slow_time_win.',[size(Data,1),1,1]);

        for st_idx = 1:size(Data,3);
            Data(:,:,st_idx)   = Data(:,:,st_idx).*slow_time_win;
            tmp_Data(:,st_idx) = mean(Data(:,:,st_idx),2);
        end
        Data = double(tmp_Data);
        clear tmp_Data slow_time_win
    else
        Data = double(Data);
    end
    
    % High pass filter in slow time
    if param.st_hpf
        Data  = filtfilt(param.hpf.b,param.hpf.a,Data.').';
    end

    % Window data in fast time
    for st_idx = 1:size(Data,2)
        Data(:,st_idx) = Data(:,st_idx).*fast_time_win;
    end

    % Transform the data to the frequency domain
    Data     = fft(Data,fft_len,1);
    fft_idxs = 1:(floor(size(Data,1)/2));
    Data     = Data(fft_idxs,:);

    if mod(param.nyquist_zone,2) == 1
        Data = flipud(Data);
    end

    % Circularly shift the data to account for altitude variations
    if param.alt_corr       
        cshift = round(-(1/param.pixel_size_air)*(Altitude-min(Altitude)));
        for idx = 1:size(Data,2)
            Data(:,idx) = circshift(Data(:,idx),[cshift(idx) 0]);
        end
    end
    
    fprintf('Generating echogram\n');

    % Generate images
    % Tracking the surface return and eliminating outliers to set the data
    % start and stop range
    if size(Data,2) > 4000
        [tmp,pk]    = max(lp(Data(:,500:size(Data,2)-500)),[],1);
    else 
        [tmp,pk]    = max(lp(Data(:,5:size(Data,2)-5)),[],1);
    end
    pk_stdev    = floor(std(pk));
    avg_pk      = floor(mean(pk));
    count       = 0;
    for idx = 1:length(pk)
        if pk(idx) < avg_pk - pk_stdev || pk(idx) > avg_pk + pk_stdev
            continue;
        else
            count           = count+1;
            new_pk(count)   = pk(idx);
        end
    end

    avg_pk      = floor(mean(new_pk));
    clear count new_pk

    % Set start and stop indices around the max value
    istart  = max(1,avg_pk-round(3/pixel_size_snow));
    istop   = min(size(Data,1),avg_pk+round(3/pixel_size_snow));
    max_sig_pwr = max(max(20.*log10(abs(Data))));

    Depth           = (0:istop-istart)*pixel_size_snow;
    fig_depth_stop  = Depth(end);
    fig_depth_start = Depth(1);
    
    [x,y]           = geodetic_to_stereographic(Latitude,Longitude);
    distance        = cumsum([0 sqrt(diff(x).^2 + diff(y).^2)])/1e3;

    % Plot Normal
    hf = figure;
    imagesc(distance,Depth,lp(Data(istart:istop,:)));
    hold on;
    colormap(1-gray);
    text_loc = (fig_depth_stop-fig_depth_start)*.02;

    for i2= linspace(0,distance(end),6)
        idx = find(distance >= i2,1);
        text(i2,fig_depth_stop+2.7*text_loc,sprintf('%8.3f',Latitude(idx)),'HorizontalAlignment','center');
        text(i2,fig_depth_stop+4.2*text_loc,sprintf('%8.3f',Longitude(idx)),'HorizontalAlignment','center');
    end
    set(get(hf,'CurrentAxes'),'XTick',floor(linspace(0,distance(end),6)*100)/100);
    set(get(hf,'CurrentAxes'),'XTickLabel',floor(distance(round(linspace(1,length(distance),6)))*100)/100);
    text(distance(end)/2,fig_depth_stop+1.2*text_loc,'Distance (km)','HorizontalAlignment','center');
    text(distance(end)/2,fig_depth_stop+2.7*text_loc,'Latitude','HorizontalAlignment','center');
    text(distance(end)/2,fig_depth_stop+4.2*text_loc,'Longitude','HorizontalAlignment','center');
    caxis([max_sig_pwr-50 max_sig_pwr+5]);
    
    img_title1   = sprintf('%02d/%02d/%04d %s',param.month,param.day,param.year,param.flight_name);
    img_title2   = ['Echogram for file ' data_fn(1:end-4)];
    title({img_title1;img_title2},'FontWeight','Bold','Interpreter','none');
    y_label = sprintf('Depth [m], \\rho = %0.1f, \\epsilon_r = %0.2f',param.snow_density,sqrt(param.snow_eps_r));
    ylabel(y_label,'FontWeight','Bold','Interpreter','tex');
    image_fn = sprintf('%sfft_image_%s',param.output_dir,data_fn(1:end-4));
    print('-djpeg','-r600',[image_fn '.jpg']);
    close(hf)
    
    toc; fprintf('\n');
 
end