function aps_powerlaw(start_step,end_step,save_path)
% aps_powerlaw(start_step,end_step)
% Program to compute the powerlaw tropsoheric delays.
%   STEP 1 = Estimate powerlaw delay and maximum height from sounding data
%            (when this is available), or use defaults or user set values
%   STEP 2 = Rotate dataset to minimize interpolation effects, Powerlaw scaling and interpolate to regular grid
%   STEP 3 = 1D/2D bandfiltering in the frequency domain and interpolate
%            back to local grid
%   STEP 4 = Spatial local estimation of the powerlaw scaling coefficient,
%            estimate final values from reliable frequency band, extrapolate
%            to all point locations and compute powerlaw tropospheric delay.
%
%   To change the processing parameters use setparm_aps and getparm_aps
%
% **** when using this estimation method please cite:
%       D.P.S. Bekaert, A.J. Hooper and T.J. Wright, A spatially-variable power-law tropospheric correction
% 	  	 technique for InSAR data, JGR, doi:10.1029/2014JB011558 *****
%
%
%     Copyright (C) 2015  Bekaert David - University of Leeds
%     Email: eedpsb@leeds.ac.uk or davidbekaert.com
%
%     This program is free software; you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation; either version 2 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License along
%     with this program; if not, write to the Free Software Foundation, Inc.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
%
% By David Bekaert - University of Leeds
%
% modifications:
% 04/2013   DB:     Incorportate a parm_aps list with all the processing
%                   options and variables.
% 05/2013   DB:     Include powerlaw coefficient sensitivity analysis
% 05/2013   DB:     Allow for an interferogram based estimation
% 12/2013   DB:     Put the flag of ifg correction on prior to powerlaw computation
% 02/2014   DB:     Integrate the plotting with the running of the sounding data
% 02/2014   DB:     Include a mountain ridge code, user interaction needed
% 07/2014   DB:     Have the option to plot the ridges
% 07/2014   DB:     Include a cropping option
% 11/2014   DB:     Fix bug and include paper reference
% 03/2015   DB:     Ask for the reference technique in step 5
% 01/2016   DB:     Fix ll.lonlat, ph_uw, hgt for non-stamps data, replace
%                   save calls with aps_save, in case no -append flag is included
% 02/2016   DB:     Expand option 5 to plot with the unwrapped phase too.

if nargin<3
   save_path = [pwd filesep];
end

currdir = pwd;		% current working directory
plot_flag = 0;
hydro =1;
wet=1;

fprintf(['\n********************************************************************************************************** \n','D.P.S. Bekaert, A.J. Hooper and T.J. Wright,\n', 'A spatially-variable power-law tropospheric correction technique for InSAR data, JGR,  doi:10.1029/2014JB011558\n', '**********************************************************************************************************\n\n'])

%% PART 1: Define the powerlaw coefficents, from the user or by using sounding data.
sounding_data = getparm_aps('sounding_data',1);
stamps_processed = getparm_aps('stamps_processed',1);
powerlaw_ridge_constraint = getparm_aps('powerlaw_ridge_constraint',1);

if strcmp(stamps_processed,'y')
    load psver
else
    psver = 2;
end

% file names of the output data
apsname = [save_path filesep 'tca' num2str(psver) '.mat'];
apssbname = [save_path filesep 'tca_sb' num2str(psver) '.mat'];
apsbandsname = [save_path filesep 'tca_bands' num2str(psver) '.mat'];
apsbandssbname = [save_path filesep 'tca_bands_sb' num2str(psver) '.mat'];

% saving part of the data in a subfolder
save_path = [save_path filesep 'aps_p'];
if exist(save_path,'dir')~=7
    mkdir(save_path);
end

if start_step==0 && strcmp(powerlaw_ridge_constraint,'y')

    mountain_ridge=[];
    if exist('tca_support.mat')==2
        load('tca_support.mat','powerlaw_ridges')
        if exist('powerlaw_ridges','var')==1
            mountain_ridge = powerlaw_ridges.mountain_ridge;
        end
    end
    if isempty(mountain_ridge)==1
        str_repr='y';
        str_vis = 'n';
    end

    % data has been computed before - reporcess? - visualize?
    if length(mountain_ridge)>0
        str_repr = '';
        while strcmpi(str_repr,'y')~=1 && strcmpi(str_repr,'n')~=1
            str_repr = input(['Ridges have been defined before, reprocess the data? [y/n] \n'],'s');
        end

        % no reprocessing, do you want to visualize it?
        if strcmpi(str_repr,'n')
            str_vis = '';
            while strcmpi(str_vis,'y')~=1 && strcmpi(str_vis,'n')~=1
                str_vis = input(['Do you want to visualize the data? [y/n] \n'],'s');
            end
        else
            str_vis = 'n';
        end

    end

    if strcmpi(str_repr,'y')
        [mountain_ridge] = aps_powerlaw_watershed;
    end
    if strcmpi(str_vis,'y')
        aps_support_plot(1);
    end
end

if start_step==1 && strcmp(sounding_data,'y')
    fprintf('\n\nStep 1: Powerlaw coefficients from the sounding data\n')

    % getting the variables from the parm_aps file
    look_angle = getparm_aps('look_angle',1);
    lambda = getparm_aps('lambda',1);
    sounding_h0= getparm_aps('sounding_h0',1);
    sounding_h_alpha_thres= getparm_aps('sounding_h_alpha_thres',1);
    sounding_start_date= getparm_aps('sounding_start_date',1);
    sounding_end_date= getparm_aps('sounding_end_date',1);
    sounding_time_stamp= getparm_aps('sounding_time_stamp',1);
    sounding_dir= getparm_aps('sounding_dir',1);
    sounding_error_promp= getparm_aps('sounding_error_promp',1);
    sounding_sensitivity= getparm_aps('sounding_sensitivity',1);
    n_months = getparm_aps('sounding_months',1);
    time_stamp = getparm_aps('sounding_time_stamp',1);
    sounding_ifg_dates = getparm_aps('sounding_ifg_dates',1);

    %%% check if the file is already exisiting - ask if reprocessing is
    %%% needed, if not load exisitng file and plot results
    time_stamp_str = [];
    for k=1:size(time_stamp,1)
        if k>1
            time_stamp_str = [time_stamp_str '_' time_stamp(k,:)];
        else
            time_stamp_str = [time_stamp(k,:)];
        end
    end

    if strcmp(sounding_sensitivity,'y')
        fprintf('Sensitivity analyses of sounding data \n\n')

        % check first if the output file is already exisiting otherwize
        % ask if it needs to be reprocessed or visualised.

        if strcmp(sounding_ifg_dates,'y')
            fprintf('Coefficient estimation for each interferogram \n\n')
            if hydro==1 && wet==0
                save_name = [sounding_dir filesep 'Powerlaw' filesep 'Powerlaw_sensitivity_hydro_SAR_dates_1month_' time_stamp_str 'Hr.mat'  ];
            elseif hydro==0 && wet==1
                save_name = [sounding_dir filesep 'Powerlaw' filesep 'Powerlaw_sensitivity_wet_SAR_dates_1month_' time_stamp_str 'Hr.mat'  ];
            else
                save_name = [sounding_dir filesep 'Powerlaw' filesep 'Powerlaw_sensitivity_SAR_dates_1month_' time_stamp_str 'Hr.mat'  ];
            end
            n_months =1;
        else
            fprintf('Coefficient estimation based on average \n\n')
            % putting the variables in the right set-up
            start_year = str2num(sounding_start_date(1:4));
            end_year = str2num(sounding_end_date(1:4));
            start_str = sounding_start_date(5:6);
            end_str = sounding_end_date(5:6);

            % the file name to be loaded
            if hydro==1 && wet==0
                save_name = [sounding_dir filesep 'Powerlaw' filesep 'Powerlaw_sensitivity_hydro_' num2str(n_months) 'month_' time_stamp_str 'Hr_' num2str(start_year) start_str '_' num2str(end_year) end_str '.mat'  ];
            elseif hydro==0 && wet==1
                save_name = [sounding_dir filesep 'Powerlaw' filesep 'Powerlaw_sensitivity_wet_' num2str(n_months) 'month_' time_stamp_str 'Hr_' num2str(start_year) start_str '_' num2str(end_year) end_str '.mat'  ];
            else
                save_name = [sounding_dir filesep 'Powerlaw' filesep 'Powerlaw_sensitivity_' num2str(n_months) 'month_' time_stamp_str 'Hr_' num2str(start_year) start_str '_' num2str(end_year) end_str '.mat'  ];
            end
        end

        if exist(save_name,'file')==2
            str = '';
            while strcmpi(str,'y')~=1 && strcmpi(str,'n')~=1
                str = input(['This file already exist, do you want to re-process the data? [y/n] \n'],'s');
            end

            if strcmpi(str,'y')
                sounding_powerlaw_sens(hydro,wet)
            else
                sounding_powerlaw_sens_display
            end
        else
            % estimate the coefficients for individual interferograms
            sounding_powerlaw_sens(hydro,wet)
        end

    else
        fprintf('Based on specified sounding period \n\n')
        % When having sounding data estimate the powerlaw and b coefficient
        [alpha_log_all,alpha,h0,n_soundings] = sounding;

        % setting the estimated parameters in the parameter file
        fprintf('Updating the parm_aps list \n')
        setparm_aps('powerlaw_h0',h0)
        setparm_aps('powerlaw_alpha',alpha)

        % saving the data
        save([save_path filesep 'aps_p_step1.mat'],'alpha','h0','look_angle','lambda','sounding_h0','sounding_h_alpha_thres','sounding_start_date','sounding_end_date','sounding_time_stamp')

    end
end
if start_step==1 && strcmp(sounding_data,'n')
     fprintf('\n\n Step 1: powerlaw coefficents set by user \n\n')

     % as set by the user
     h0 = getparm_aps('powerlaw_h0');
     alpha = getparm_aps('powerlaw_alpha');
end

%% PART 2: Rotate the dataset to get mimumal white space around it
% Scaling and interpolation of the data to a regular grid
if start_step<=2 && end_step >=2
    fprintf('\n\nStep 2: Rotate the dataset \n')

    % getting the variables from the parm_aps file
    heading = getparm_aps('heading',1);
    crop_flag =  getparm_aps('crop_flag',1);

    % loading the lonlat information
    ll_matfile = getparm_aps('ll_matfile',1);
    ll = load(ll_matfile);
    ll = ll.lonlat;

    % retrieving the coefficients
    h0 = getparm_aps('powerlaw_h0',1);
    alpha = getparm_aps('powerlaw_alpha',1);
    getparm_aps('powerlaw_xy_res',1);
    phuw_matfile = getparm_aps('phuw_matfile',1);
    hgt_matfile = getparm_aps('hgt_matfile',1);
    DEM_corr = getparm_aps('powerlaw_DEM_corr',1);
    bperp_matfile = getparm_aps('bperp_matfile',1);

    % rotating the data
    [xy_rotatelocal_full,xy_local_full,local_origin_full] = ll2rotatelocal(ll,heading);

    % cropping if needed
    crop = [];
    if strcmpi(crop_flag,'y')
        if exist('area_ex.mat','file')==2
            crop = load('area_ex.mat');
        elseif exist('../area_ex.mat','file')==2
            crop = load('../area_ex.mat');
        else
            crop = [];
            fprintf(['There is no area_ex.mat file found, no data is cropped... \n'])
        end
    end
    if ~isempty(crop)
        ix_remove =  inpolygon(ll(:,1),ll(:,2),crop.lonlat(:,1),crop.lonlat(:,2));
    else
        ix_remove = [];
    end
    ll(ix_remove,:) =[];

    % get the InSAR convexhull -- not needed at this stage!
    ix_convexhull = convhull(ll(:,1),ll(:,2));
    InSAR_convexhull = ll(ix_convexhull,:);
    if exist('tca_support.mat','file')==2
      save('tca_support.mat','-append','InSAR_convexhull')
    else
      save('tca_support.mat','InSAR_convexhull')
    end
    clear InSAR_convexhull

    % rotating the data using all data including the region cropped.
    % This will be used in the last stage of the power-law code.
    [xy_rotatelocal,xy_local,local_origin] = ll2rotatelocal(ll,heading);

    fprintf('        Scaling and interpolation to a regular grid \n\n')
    % loading the unwrapped interferograms
    ph = load(phuw_matfile);
    ph = ph.ph_uw;

    % keep track of those pixels which are NaN
    ix_phase_nan = isnan(ph);

    % remove crop if needed
    ph(ix_remove,:)=[];

    % loading the topography information
    hgt = load(hgt_matfile);
    hgt = hgt.hgt;
    % remove crop if needed
    hgt(ix_remove,:)=[];

    % geting the number of interferograms and points from the phase matrix
    n_interferograms= size(ph,2);
    n_points = size(ph,1);

    % Scaling of the topography according to the powerlaw
    % ifg based powerlaw correction?
    if length(alpha)==n_interferograms
        hgt_max = max(hgt);
        hgt_scaled=repmat(hgt,1,size(ph,2));
        ifg_based_correction='y';

        for k=1:length(alpha)
            if h0(k)*1000-hgt_max<=0
                error('myApp:argChk', ['Power law is not valid above h0 heights,... \nAbort,... \n'])
            end
            hgt_scaled(:,k)=(h0(k)*1000-hgt_scaled(:,k)).^(alpha(k));
        end
    else
        % Scaling of the topography according to the powerlaw
        ifg_based_correction='n';
        ix = find(h0*1000-hgt < 0);
        if isempty(ix)~=1
            error('myApp:argChk', ['Power law is not valid above h0 heights,... \nAbort,... \n'])
        end
        hgt_scaled = (h0*1000-hgt).^(alpha);
    end

    % define a new dataset were the height is actually the first column and
    % the interferograms are in the other columns
    % optional remove an estimate for the DEM errors
    if strcmp(DEM_corr,'y') && n_interferograms>5;
       % these are erros that scale with perpendicular baseline
       % loading the perpendicualr baseline information
       bperp = load(bperp_matfile);
        if strcmp(stamps_processed,'y')
          bperp = bperp.bperp;
       end
       % checking the size of bperp
       if size(bperp,2) > 1
          bperp = bperp';
          if size(bperp,2) >1
              error('myApp:argChk', ['bperp is not a vector,... \nAbort,... \n'])
          end
       end
       % estimating the correlated errors
       DEM_corr_e = lscov(bperp,ph')';

       % removing DEM correlated errors
       ph_temp = ph-repmat(bperp',n_points,1).*repmat(DEM_corr_e,1,n_interferograms);
       dataset = [hgt_scaled ph_temp];
       clear ph_temp A
    else
        if strcmp(DEM_corr,'y') && n_interferograms<=5
            fprintf('Not enough interferograms to make a reliable estimate for the DEM error \n')
            DEM_corr = 'n';
        end
        dataset = [hgt_scaled ph];
        DEM_corr_e = zeros([n_points 1]);
    end

    %% do an initial reference correction for each interferogram.
    % This is to scope for regions that are small and which can have bandfiltering
    % arctifacts introduced. What happens is that each interferogram is
    % shifted such that the phase is zero at h0. Note that this is a long
    % wavelength shift that will be filtered out latter on. But in case of
    % irregularities it will reduce introduced arctifacts. To cope with
    % spatial varying signals this offset estimation is done over multiple
    % windows, but the constant offset is the same for all windows.
    % get the position of some subwindows

    [iterate] = window_generation(xy_rotatelocal,1);

    counter=0;
    for k=1:length(iterate.window_ix)
        counter = counter + size(iterate.window_ix{k},1);
    end

    for k=1:n_interferograms
        % separate between ifg based correction or using a single alpha and h0
        if length(alpha)==n_interferograms
            height_data = dataset(:,k);
            scaling = 1./nanmean(abs(dataset(:,k)));
            phase_data = dataset(:,n_interferograms+k);
        else
            scaling = 1./nanmean(abs(dataset(:,1)));
            height_data = dataset(:,1);
            phase_data = dataset(:,1+k);
        end

        % estimating the reference, forced to be the same for all windows
        A_temp = zeros([counter length(iterate.window_ix)]);
        data_temp =  zeros([counter size(phase_data,2)]);
        counter =1;
        for kk=1:length(iterate.window_ix)
            A_temp(counter:counter+size(iterate.window_ix{kk},1)-1,kk) = [height_data(iterate.window_ix{kk},1)]*scaling;
            data_temp(counter:counter+size(iterate.window_ix{kk},1)-1,:) = [phase_data(iterate.window_ix{kk},1)];
            counter =  counter + size(iterate.window_ix{kk},1);

        end
        A_temp = [A_temp ones([size(data_temp,1) 1])];

        % allowing for NaN in interferogram {DB}
%        ix = find(isnan(data_temp(:,1))~=1);
        ix = ~((isnan(data_temp(:,1)) + sum(isnan(A_temp),2))>=1);
        coeff = lscov(A_temp(ix,:),data_temp(ix,1));

        % plot the interferogram before and after the reference shift.
        if plot_flag ==1
           figure('name',['Correction of the reference for ifg ', num2str(k)])
           subplot(2,1,1)
           plot(height_data,phase_data,'k.')
           if length(alpha)==n_interferograms
                temp =getparm_aps('powerlaw_alpha');
                xlabel(['(h_0-h)^{' num2str(temp(k)) '}'])
           else
                xlabel(['(h_0-h)^{' num2str(getparm_aps('powerlaw_alpha')) '}'])
           end
           ylabel('Phase')
        end

       if plot_flag ==1
           for kk=1:length(iterate.window_ix)
                y_temp = [[height_data(iterate.window_ix{kk},1)]*scaling ones(size([height_data(iterate.window_ix{kk},1)]*scaling))]*[coeff(kk) coeff(end)]';
                hold on
                plot([height_data(iterate.window_ix{kk},1)],y_temp,'r-')
           end
           title('power-law plot with ifg reference to be the mean')
       end

       % correct the data for the reference
       data_temp = data_temp - coeff(end);
       % separate between ifg based correction or using a single alpha and h0
       if length(alpha)==n_interferograms
           dataset(:,n_interferograms+k) = dataset(:,n_interferograms+k) - coeff(end);
       else
           dataset(:,k+1) = dataset(:,k+1) - coeff(end);
       end

       % plot the corrected result
       if plot_flag ==1
           subplot(2,1,2)
           if length(alpha)==n_interferograms
               plot(dataset(:,k),dataset(:,n_interferograms+k),'k.')
               temp =getparm_aps('powerlaw_alpha');
               xlabel(['(h_0-h)^{' num2str(temp(k)) '}'])
           else
              plot(dataset(:,1),dataset(:,k+1),'k.')
              xlabel(['(h_0-h)^{' num2str(getparm_aps('powerlaw_alpha')) '}'])
           end
           ylabel('Phase shifted to h0 reference')

           % plot line on top
           coeff = lscov(A_temp,data_temp(:,1));
           for kk=1:length(iterate.window_ix)
                if length(alpha)==n_interferograms
                    y_temp = [[dataset(iterate.window_ix{kk},k)]*scaling ones(size([dataset(iterate.window_ix{kk},k)]*scaling))]*[coeff(kk) coeff(end)]';
                else
                    y_temp = [[dataset(iterate.window_ix{kk},1)]*scaling ones(size([dataset(iterate.window_ix{kk},1)]*scaling))]*[coeff(kk) coeff(end)]';
                end
                hold on
                plot([dataset(iterate.window_ix{kk},k)],y_temp,'r-')

           end
           title('power law plot with h0 as reference')
       end
    end

    %% Extrapolate such the convexhull contains the full grid
    xy_res = getparm_aps('powerlaw_xy_res');
    [xy_local_box,dataset_box] = extrapolate_local_new(xy_rotatelocal,dataset, xy_res(1), xy_res(2));

    % Interpolate to a regular grid
    [X_regular,Y_regular,Z_regular] = interpolate_regular(xy_local_box,dataset_box, xy_res(1), xy_res(2));

    % Saving the results of this step
    aps_save([save_path filesep 'aps_p_step2.mat'],xy_rotatelocal,xy_rotatelocal_full,ifg_based_correction,xy_local,xy_local_full,heading,ll,local_origin,local_origin_full,X_regular,Y_regular,Z_regular,xy_rotatelocal,xy_res,DEM_corr,DEM_corr_e,ix_remove,ix_phase_nan)

end

%% PART 3: Bandfiltering and interpolation back to a local grid
if start_step<=3 && end_step >=3
    fprintf('Step 3: Bandfiltering of the regular grid and converting back to a local grid \n\n')
    % Loading the data from the previous step
    if start_step==3
        load([save_path filesep 'aps_p_step2.mat'])
    end

     % loading the lonlat information
    ll_matfile = getparm_aps('ll_matfile');
    temp = load(ll_matfile);
    if strcmp(stamps_processed,'y')
        temp = temp.lonlat;
    end

    n_points_original = size(temp,1);
    clear temp;

    % getting the extend of the spatial bandfilters
    spatial_bands = getparm_aps('powerlaw_spatial_bands');

    % Bandfiltering
    bandfiltering(Z_regular,xy_res(1),xy_res(2),spatial_bands,save_path,ifg_based_correction)

    % Interpolate back to local grid
    n_datasets = size(Z_regular,3);
    % calling the function for each bandfiltered dataset
    fprintf(['*Interpolate the band filtered data to the local grid: \n'])
    if strcmp(ifg_based_correction,'y')
        h_ifg_number = n_datasets/2;
    else
        h_ifg_number = 1;
    end
    for k=1:n_datasets

        % the file names, vary depending if the correction is varying for each interferogram
        if k<=h_ifg_number && h_ifg_number~=1
            % thse are interferograms
            load_name = ['bandfilter_regular_hgt_ifg_' num2str(k) '.mat'];
            save_name = ['bandfilter_local_hgt_ifg' num2str(k) '.mat'];
        elseif k<=h_ifg_number && h_ifg_number==1
            % these are the heights
            load_name = 'bandfilter_regular_hgt.mat';
            save_name = 'bandfilter_local_hgt.mat';
        else
            % thse are interferograms
             load_name = ['bandfilter_regular_ifg_' num2str(k-h_ifg_number) '.mat'];
             save_name = ['bandfilter_local_ifg_' num2str(k-h_ifg_number) '.mat'];
        end

        dataset_band = load([save_path filesep load_name]);

        % Storing the information what type of filtering was done for which band
        dimension_filter = dataset_band.dimension_filter;      % 1 for 1D, 2 for 2D
        % loading the data for each dataset
        dataset_band = dataset_band.data_band_out;

        % computing the bandfiltered data at the local grid
        % note that this is xy_rotatelocal and does not include the cropped
        % region!
        [z_local_out_temp] = interpolate2local(X_regular,Y_regular,dataset_band,xy_rotatelocal);
        % in case data has been cropped then path the cropped region with NaNs
        z_local_out = NaN([n_points_original size(z_local_out_temp,2)]);

        if isempty(ix_remove)
            z_local_out = z_local_out_temp;
        else
            z_local_out(~ix_remove,:) = z_local_out_temp;
        end

        % Check in case some of the orginal phase values were nan.
        % if so set them to nan here too.
        if k<=h_ifg_number && h_ifg_number~=1
            % these are interferograms heights not the phase. No need to
            % modify this here.
        elseif k<=h_ifg_number && h_ifg_number==1
            % these are the heights, nothing needs to be done as the
            % heights remain the same for each interferogram.
        else
            % these are interferograms
            fprintf('Allowing for NaN''s in the interferograms \n')
            z_local_out(ix_phase_nan(:,k-h_ifg_number),:)=NaN;
        end

        % saving the data for each dataset
        aps_save([save_path filesep save_name],z_local_out,spatial_bands,dimension_filter);
        % outputting the progress to the user
        fprintf(['Progress: ' num2str(k) '/' num2str(n_datasets) ' done \n'])
    end

    % Saving the results of this step
    aps_save([save_path filesep 'aps_p_step3.mat'],ifg_based_correction,n_datasets,local_origin,local_origin_full,xy_rotatelocal,xy_rotatelocal_full,DEM_corr,DEM_corr_e,dimension_filter,ix_remove,ix_phase_nan)
end

%% PART 4: Estimating the scaling coefficient of the powerlaw for the local dataset
% and computing the corresponding troposheric delay
if start_step<=4 && end_step >=4

    fprintf('\n\nStep 4: Estimating scaling coefficient of the powerlaw locally \n')
    fprintf('and estimating the tropospheric interferometric phase delay \n\n')

    % Loading the data from the previous step
    if start_step==4
        load([save_path filesep 'aps_p_step3.mat'])
    end

    % loading information from the parm file
    hgt_matfile = getparm_aps('hgt_matfile');
    powerlaw_all_bands = getparm_aps('powerlaw_all_bands');
    hgt = load(hgt_matfile);
    hgt = hgt.hgt;

    % checking if its an interferogram based correction or not:
    if strcmp(ifg_based_correction,'y')
        n_interferograms = n_datasets/2;
    else
        n_interferograms = n_datasets-1;
    end

    % initialization
    n_points = size(hgt,1);
    ph_tropo_powerlaw = NaN([n_points n_interferograms]);
    K_tropo_powerlaw = NaN([n_points n_interferograms]);
    perc_cons_K_powerlaw= NaN([ n_interferograms 1]);
    if strcmp(powerlaw_all_bands,'y')
        ph_tropo_powerlaw_bands = NaN([n_points n_interferograms size(getparm_aps('powerlaw_spatial_bands'),1)]);
        K_tropo_powerlaw_bands = NaN([n_points n_interferograms size(getparm_aps('powerlaw_spatial_bands'),1)]);
    end

    % Linear estimation for multiple windows based on bandfitlered data
    % final slope value is estimated for reliable bands only
    iterate = [];
    % getting the patch information from the parm_aps file

    % processing only those images that are not dropped in case of stamps
    ix_ifgs = 1:n_interferograms;
    if strcmp(stamps_processed,'y')
       ix_drop_ifgs = getparm('drop_ifg');
    else
        ix_drop_ifgs = [];
    end
    ix_ifgs(ix_drop_ifgs)=[];

    for k=1:length(ix_ifgs)

        ix_interferogram = ix_ifgs(k);

        % the file names, vary depending if the correction is varying for each interferogram
        if strcmp(ifg_based_correction,'y') && n_interferograms~=1
            % the first dataset was set the be the topography
            topo_data = load([save_path filesep 'bandfilter_local_hgt_ifg' num2str(ix_interferogram) '.mat']);
            topo_data = topo_data.z_local_out;
            % the other dataset were the phase
            phase_data = load([save_path filesep 'bandfilter_local_ifg_' num2str(ix_interferogram) '.mat']);
            phase_data = phase_data.z_local_out;
            ifg_number=k;
        else
            % the first dataset was set the be the topography
            topo_data = load([save_path filesep 'bandfilter_local_hgt.mat']);
            topo_data = topo_data.z_local_out;
            % the other dataset were the phase
            phase_data = load([save_path filesep 'bandfilter_local_ifg_' num2str(ix_interferogram) '.mat']);
            phase_data = phase_data.z_local_out;
            ifg_number=1;
        end

        % keep the figures of the outlier rejection as validation
        if k==0
            save_path_outlier_windows = [save_path filesep 'ifg_' num2str(ix_interferogram) '_outlier_window'];
        else
            save_path_outlier_windows=[];
        end

        if k==10000
            debug_fig_linear=1;
        else
            debug_fig_linear=0;
        end

        [ph_tropo_powerlaw_temp,ph_tropo_powerlaw_band_temp,iterate,K_temp,K_band,perc_cons_K] = aps_powerlaw_linear_local(xy_rotatelocal_full,local_origin_full,topo_data,phase_data,iterate,dimension_filter,save_path_outlier_windows,ifg_number,ix_phase_nan(:,ix_interferogram));

        % storing of the data
        % Computation of the tropospheric delay for all ifgs
        ph_tropo_powerlaw(:,ix_interferogram) = ph_tropo_powerlaw_temp;
        K_tropo_powerlaw(:,ix_interferogram) = K_temp;
        perc_cons_K_powerlaw(ix_interferogram,1) = perc_cons_K;
        if strcmp(powerlaw_all_bands,'y')
            % keeping all bands with in the columns the ifgs and the third
            % dimention the bands
            ph_tropo_powerlaw_bands(:,ix_interferogram,:) = ph_tropo_powerlaw_band_temp;
            K_tropo_powerlaw_bands(:,ix_interferogram,:)=K_band;

            % keeping the interferogram and the respective bands.
            eval(['ph_tropo_powerlaw_ifg_' num2str(ix_interferogram) '= ph_tropo_powerlaw_band_temp;']);
            eval(['K_tropo_powerlaw_ifg_' num2str(ix_interferogram) '= K_band;']);

            clear ph_tropo_powerlaw_band_temp
            % saving the data from this step
            if strcmp(stamps_processed,'y')
                % This is StaMPS
                if strcmp(getparm('small_baseline_flag'),'y')
                    if exist(apsbandssbname,'file')==2
                        save(apsbandssbname,'-append',['ph_tropo_powerlaw_ifg_' num2str(ix_interferogram)],['K_tropo_powerlaw_ifg_' num2str(ix_interferogram)])
                    else
                        save(apsbandssbname,['ph_tropo_powerlaw_ifg_' num2str(ix_interferogram)],['K_tropo_powerlaw_ifg_' num2str(ix_interferogram)])
                    end
                else
                    if exist(apsbandsname,'file')==2
                        save(apsbandsname,'-append',['ph_tropo_powerlaw_ifg_' num2str(ix_interferogram)],['K_tropo_powerlaw_ifg_' num2str(ix_interferogram)])
                    else
                        save(apsbandsname,['ph_tropo_powerlaw_ifg_' num2str(ix_interferogram)],['K_tropo_powerlaw_ifg_' num2str(ix_interferogram)])
                    end
                end
            else
                % This is not StaMPS
                if exist(apsbandsname,'file')==2
                    save(apsbandsname,'-append',['ph_tropo_powerlaw_ifg_' num2str(ix_interferogram)],['K_tropo_powerlaw_ifg_' num2str(ix_interferogram)])
                else
                    save(apsbandsname,['ph_tropo_powerlaw_ifg_' num2str(ix_interferogram)],['K_tropo_powerlaw_ifg_' num2str(ix_interferogram)])
                end
            end
            eval(['clear ph_tropo_powerlaw_ifg_' num2str(ix_interferogram) ' K_tropo_powerlaw_ifg_' num2str(ix_interferogram)  ';']);
        end

        % give output to the screen
        fprintf(['Progress ifgs: ' num2str(k) '/' num2str(length(ix_ifgs)) ' done \n'])
    end
    % saving the data from this step
    aps_save([save_path filesep 'aps_p_step4.mat'],ph_tropo_powerlaw,DEM_corr,DEM_corr_e,iterate,K_tropo_powerlaw)

    % saving the data into the final variable togehter were other
    % correction values are saved.
    ph_tropo_powerlaw_or= ph_tropo_powerlaw;
    K_tropo_powerlaw_or=K_tropo_powerlaw;
    if strcmp(stamps_processed,'y')
        % This is StaMPS
        if strcmp(getparm('small_baseline_flag'),'y')
            if exist(apssbname,'file')==2
                save(apssbname,'-append','ph_tropo_powerlaw','K_tropo_powerlaw','perc_cons_K_powerlaw')
            else
                save(apssbname,'ph_tropo_powerlaw','K_tropo_powerlaw','perc_cons_K_powerlaw')
            end

            % saving the band information
            if strcmpi(powerlaw_all_bands,'y')
                if exist(apsbandssbname,'file')==2
                    save(apsbandssbname,'-append','ph_tropo_powerlaw_or','K_tropo_powerlaw_or','ph_tropo_powerlaw_bands','K_tropo_powerlaw_bands','perc_cons_K_powerlaw')
                else
                    save(apsbandssbname,'ph_tropo_powerlaw_or','K_tropo_powerlaw_or','ph_tropo_powerlaw_bands','K_tropo_powerlaw_bands','perc_cons_K_powerlaw')
                end
            end
        else
            % this is single master
            if exist(apsname,'file')==2
                save(apsname,'-append','ph_tropo_powerlaw','K_tropo_powerlaw','perc_cons_K_powerlaw')
            else
                save(apsname,'ph_tropo_powerlaw','K_tropo_powerlaw','perc_cons_K_powerlaw')
            end

            % saving the band information
            if strcmpi(powerlaw_all_bands,'y')
                if exist(apsbandsname,'file')==2
                    save(apsbandsname,'-append','ph_tropo_powerlaw_or','K_tropo_powerlaw_or','ph_tropo_powerlaw_bands','K_tropo_powerlaw_bands','perc_cons_K_powerlaw')
                else
                    save(apsbandsname,'ph_tropo_powerlaw_or','K_tropo_powerlaw_or','ph_tropo_powerlaw_bands','K_tropo_powerlaw_bands','perc_cons_K_powerlaw')
                end
            end
        end
    else
        % This is not StaMPS
        if exist(apsname,'file')==2
            save(apsname,'-append','ph_tropo_powerlaw','K_tropo_powerlaw','perc_cons_K_powerlaw')
        else
            save(apsname,'ph_tropo_powerlaw','K_tropo_powerlaw','perc_cons_K_powerlaw')
        end

         % saving the band information
        if strcmpi(powerlaw_all_bands,'y')
            if exist(apsbandsname,'file')==2
                save(apsbandsname,'-append','ph_tropo_powerlaw_or','K_tropo_powerlaw_or','ph_tropo_powerlaw_bands','K_tropo_powerlaw_bands','perc_cons_K_powerlaw')
            else
                save(apsbandsname,'ph_tropo_powerlaw_or','K_tropo_powerlaw_or','ph_tropo_powerlaw_bands','K_tropo_powerlaw_bands','perc_cons_K_powerlaw')
            end
        end
    end
    setparm_aps('powerlaw_kept',[0]);

end

if start_step<=5 && end_step >=5 &&  exist('aps_RMSE_comparison')==2
    fprintf('\n\nStep 5: Powerlaw band comparison with reference\n')

    if strcmpi(getparm_aps('stamps_processed'),'y')
        % check which technique it needs to be compared with
        technique_number = -10;
        while isnumeric(technique_number)~=1 | (technique_number<1 | technique_number>7)
            technique_number = input(['*Select the technique number you want to compare with: \n---- MERIS (1), MODIS (2), MODIS Recal (3), ERA-I (4), WRF (5), u(sb) (6), u(sb)-d (7) : \n'],'s');
            technique_number = floor(str2num(technique_number));
        end
        % the technique component as a string
        if technique_number==1
            comp_str = 'a_mi';
        elseif technique_number==2
            comp_str = 'a_MI';
        elseif technique_number==3
            comp_str = 'a_RMI';
        elseif technique_number==4
            comp_str = 'a_e';
        elseif technique_number==5
            comp_str = 'a_w';
        elseif technique_number==6
            if strcmpi(getparm('small_baseline_flag'),'y')
                comp_str = 'usb';
            else
                comp_str = 'u';
            end
        elseif technique_number==7
            if strcmpi(getparm('small_baseline_flag'),'y')
                comp_str = 'usb-d';
            else
                comp_str = 'u-d';
            end
        end

        % check if we should add the hydrostatic delays too in case this is
        % possible
        if technique_number==1 | technique_number==2 | technique_number==3
            hydro_comp = inf;
            while isnumeric(hydro_comp)~=1 | (hydro_comp<0 | hydro_comp>2)
                hydro_comp = input(['*Do you want to add a hydrostatic component:\n---- None (0), ERA-I hydr (1), WRF hydro (2): \n'],'s');
                hydro_comp = floor(str2num(hydro_comp));
            end
        else
            hydro_comp = 0;
        end
        % the hydrostatic component as a string
        hydro_str = '';
        if hydro_comp==1
            hydro_str = '+a_eh';
        elseif hydro_comp==2
            hydro_str = '+a_wh';
        end

        % The reference technique is combination of both
        tech_ref = [comp_str hydro_str];
        clear hydro_str comp_str technique_number hydro_comp

        % running the RMSE script
        aps_RMSE_comparison(tech_ref,'a_pbands')
        str = '';
        while strcmpi(str,'y')~=1 && strcmpi(str,'n')~=1
            str = input(['Do you want to update ''powerlaw_kept'' to a different band? [y/n] \n'],'s');
        end
        if strcmpi(str,'y')
            bandnumber = -10;
            while (bandnumber<0 | bandnumber>size(getparm_aps('powerlaw_spatial_bands'),1))
                bandnumber = input(['Which the band? (row number of the band to keep, or 0 for all bands combined) \n'],'s');
                bandnumber = str2num(bandnumber);
                if isempty(bandnumber)
                    bandnumber = -10;
                end
            end
            setparm_aps('powerlaw_kept',[bandnumber]);
        end
    else
       fprintf('This option uses the StaMPS ps_plot function, and therefore only works if your data is stamps processed \n')
    end

end


function [ objectIndex ] = GetScenario_random(varargin)

fprintf('[SCENARIO]\tGetting the random distribution of objects scenario.\n');

% DEFAULT AGENT/OBSTACLE CONFIGURATION IN THIS EXAMPLE
defaultConfig = struct(...
    'file','scenario.mat',...
    'objects',[],...
    'velocity',18,...
    'positionGain',10,...
    'velocityGain',1,...
    'poseGain',pi,...
    'plot',false,....
    'noiseFactor',0);

% Instanciate the scenario builder
SBinstance = scenarioBuilder();
% Parse user inputs
[inputConfig] = SBinstance.configurationParser(defaultConfig,varargin);

% Get the random object configuration
objectConfig = SBinstance.random(...
    'objects',numel(inputConfig.objects),...
    'positionGain',inputConfig.positionGain,...
    'velocityGain',inputConfig.velocityGain,...
    'poseGain',inputConfig.poseGain);

for index = 1:numel(inputConfig.objects)
    % Assign object
    objectIndex{index} = inputConfig.objects{index};
    % APPLY GLOBAL STATE VARIABLES
    objectIndex{index}.SetGLOBAL('position',objectConfig.positions(:,index) + inputConfig.noiseFactor*randn(3,1));
    objectIndex{index}.SetGLOBAL('velocity',objectConfig.velocities(:,index) + inputConfig.noiseFactor*randn(3,1));
    objectIndex{index}.SetGLOBAL('quaternion',objectConfig.quaternions(:,index));
end
% PLOT THE SCENE
if inputConfig.plot
    SBinstance.plotObjectIndex(objectIndex);
end
% CLEAR THE REMAINING VARIABLES
clearvars -except objectIndex
end


function [result] = pf_bvalues(params)
% function [params] = pf_bvalues(params)
% --------------------------------------
% Calculation of b-values for the probabilistic forecast test.
% Input parameters:
%   params.mCatalog           Earthquake catalog
%   params.mPolygon           Polygon (defined by ex_selectgrid)
%   params.vX                 X-vector (defined by ex_selectgrid)
%   params.vY                 Y-vector (defined by ex_selectgrid)
%   params.vUsedNodes         Used nodes vX * vY defining the mPolygon (defined by ex_selectgrid)
%   params.bMap               Calculate a map (=1) or a cross-section (=0)
%   params.bNumber            Use constant number (=1) or constant radius (=0)
%   params.nNumberEvents      Number of earthquakes if bNumber == 1
%   params.fRadius            Radius of gridnode if bNumber == 0
%   params.nMinimumNumber     Minimum number of earthquakes per node
%   params.fSplitTime         Time at which the catalog will be split
%   params.bLearningPeriod    Fix duration of the learning period (1 = fix, 0 = use catalog from start)
%   params.fLearningPeriod    Duration of the learning period
%   params.bForecastPeriod    Fix duration of the forecasting period (1 = fix, 0 = use catalog till end)
%   params.fForecastPeriod    Duration of the forecasting period
%
% Output parameters:
%   Same as input parameters including
%   result.mValueGrid         Matrix of calculated b-values
%   result.vcsGridNames       Strings describing the values in mValueGrid
%
% Danijel Schorlemmer
% June 4, 2001

global bDebug;
if bDebug
  report_this_filefun(mfilename('fullpath'));
end

% Init result matrix
result = params;
result.mValueGrid = [];

for nNode_ = 1:length(params.mPolygon(:,1))
  % Create node catalog
  mNodeCatalog_ = params.mCatalog(params.caNodeIndices{nNode_}, :);
  % Calculate the b-values
  [fDeltaBValue, fBValueLearning, fBValueObserved, fStdDevLearning, fStdDevObserved] ...
    = pf_calcbvalues(mNodeCatalog_, params.fSplitTime, params.bLearningPeriod, params.fLearningPeriod, ...
    params.bForecastPeriod, params.fForecastPeriod, params.nMinimumNumber, params.nCalculateMC);
  % Store the results
  result.mValueGrid = [result.mValueGrid; fDeltaBValue fBValueLearning fBValueObserved fStdDevLearning fStdDevObserved];
end % for nNode
% Create the description-strings for the output-window
result.vcsGridNames = cellstr(char('b-value difference', 'b-value of learning period', 'b-value of observation period', ...
  'Std. dev. of learning period b-value ', 'Std. dev. of observation period b-value'));


function [ t_stop, y_stop ] = p39_stop ( neqn )

%*****************************************************************************80
%
%% P39_STOP returns the stopping point for problem p39.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Output, real T_STOP, Y_STOP(NEQN), the final data.
%
  y_stop = zeros ( neqn, 1 );

  a = p39_param ( 'GET', 'A', [] );
  b = p39_param ( 'GET', 'B', [] );
  if ( a ~= 0.0 )
    t_stop = ( b + 9.0 ) / a;
    y_stop = -3.0;
  elseif ( b ~= 0.0 )
    t_stop = 0.0;
    y_stop = 0.0;
  else
    t_stop = 10.0;
    y_stop = 0.1;
  end

  return
end


function plotSection(ori,varargin)
% plot orientations to ODF sections
%
% Input
%  ori - @orientation
%
% Options
%  sections - number of sections
%  points   - number of orientations to be plotted
%  all      - plot all orientations
%
% Flags
%  phi2      - phi2 sections (default)
%  phi1      - phi1 sections
%  sigma     - sigma = phi1 ~ phi2 sections
%  axisAngle - rotational angle sections
%
% See also
% vector3d/scatter saveFigure

if check_option(varargin,{'contour','contourf','smooth'})
  warning('Using the options contour, contourf or smooth in orientation sections plots is not recommented. Computing an ODF and plotting it is usually much better');
end

if ori.antipodal, varargin = [varargin,'antipodal']; end
oS = newODFSectionPlot(ori.CS,ori.SS,varargin{:});

oS.plot(ori,varargin{:});


function [ y2, m2, d2, f2 ] = ymdf_inc_english ( y1, m1, d1, f1, days )

%*****************************************************************************80
%
%% YMDF_INC_ENGLISH increments an English YMDF date by DAYS days.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 April 2103
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1, D1, real F1,
%    the YMDF date.
%
%    Input, real DAYS, the number of days to advance the date.
%
%    Output, integer Y2, M2, D2, real F2, the
%    incremented YMDF date.
%

%
%  Copy the parameters.
%
  y2 = y1;
  m2 = m1;
  d2 = d1;
  f2 = f1 + days;
%
%  Check the parameters.
%
  [ y2, m2, d2, f2, ierror ] = ymdf_check_english ( y2, m2, d2, f2 );

  return
end


function add_nuisance_to_SPMcfg(Xn)
% Adds a matrix Xn to the end of covariates of interest in
% xX structure in SPMcfg.mat
%
% :Usage:
% ::
%
%     function add_nuisance_to_SPMcfg(Xn)
%
% :Inputs:
%
%   **oXn:**
%        should contain ALL nuisance covariates and intercepts
%        as in output of tor_get_physio.m
%
% This function is automatically run by tor_get_physio.m
%
% ..
%    Tor Wager
%..

    if exist([pwd filesep 'SPMcfg.mat']) == 2
        load SPMcfg
    else
        warning(['No SPMcfg.mat file found in current directory:' pwd])
    end

    xX.X = [xX.X(:,xX.iC) Xn];

    XNamesB = xX.Xnames(xX.iB);

    for i = max(xX.iC)+1:size(xX.X,2)-length(XNamesB), xX.Xnames{i} = ['Nuisance ' num2str(i)];, end

    xX.Xnames(size(xX.X,2)-length(XNamesB)+1:size(xX.X,2)) = XNamesB;
    xX.iB = max(xX.iC)+1:size(xX.X,2);
    F_iX0.iX0 = xX.iB;
    save SPMcfg F_iX0 SPMid Sess VY xGX xM xX xsDes

    fprintf(1,'\n SPMcfg xX modified and saved.\n')

return


% Test file for chebtech/isempty.m

function pass = test_isempty(varargin)

for n = 1:2
    if ( n == 1 )
        testclass = chebtech1();
    else
        testclass = chebtech2();
    end

    f = testclass.make();
    pass(n, 1) = isempty(f);

    f = testclass.make(@sin);
    pass(n, 2) = ~isempty(f);

    f = testclass.make(@(x) [sin(x), cos(x)]);
    pass(n, 3) = ~isempty(f);

    f = [ testclass.make(@sin), testclass.make(@sin) ];
    pass(n, 4) = ~isempty(f);

    f = [ testclass.make() testclass.make() ];
    pass(n, 5) = isempty(f);
end

end


function structM = findStructureMatrixForOneZSlice(allSegmentsM, zSliceUniformValue, CTOriginalZValues, CTSliceThickness, CTDeltaX, CTDeltaY, reusableZerosM)
%"findStructureMatrixForOneZSlice"
% returns structM, a matrix with 1's and 0's corresponding to
% the location of the points of a structure on a particular z slice.
%
% allSegmentsM = planC{indexS.structures}(structNum).rasterSegments;
% zSliceUniformValue = desired value of the z slice of the output matrix, structM
% CTOriginalZValues = [planC{indexS.scan}.scanInfo(:).zValue];
% reusableZerosM = an all-zero matrix the size of structM output.

structM = reusableZerosM;

%Test if structure is more than 1 full slice from the requested zSlice, if
%so return the all zero matrix, as the structure is not present.
maxStructureZVal = max(allSegmentsM(:,1));
minStructureZVal = min(allSegmentsM(:,1));
CTSpacing = CTOriginalZValues(2) - CTOriginalZValues(1);
if zSliceUniformValue < (minStructureZVal-CTSpacing) | zSliceUniformValue > (maxStructureZVal+CTSpacing)
    return
end
%END

%try to find a slice with the desired z value
if maxStructureZVal == minStructureZVal
	indV = 1:size(allSegmentsM,1);
else
	indV = find(abs(allSegmentsM(:,1) - zSliceUniformValue) < 0.001);  %mask values on this slice
end
if ~isempty(indV) %no need to interpolate
  segmentsM = allSegmentsM(indV(:),7:9);     %segments
  for i = 1 : size(segmentsM,1)
    structM(segmentsM(i,1),segmentsM(i,2):segmentsM(i,3)) = 1;
  end
else %simple interpolation necessary
  %find the two slices closest to this uniform z value
  cranialSlice = max(find(CTOriginalZValues < zSliceUniformValue));
  caudalSlice  = min(find(CTOriginalZValues >= zSliceUniformValue));
  if isempty(cranialSlice)
    cranialSlice = caudalSlice;
  end
  if isempty(caudalSlice)
    caudalSlice = cranialSlice;
  end
  cranialZValue = CTOriginalZValues(cranialSlice);
  caudalZValue  = CTOriginalZValues(caudalSlice);

  sliceSpacing = caudalZValue - cranialZValue;
  spaceToCranial = zSliceUniformValue - cranialZValue;
  spaceFromCaudal = caudalZValue - zSliceUniformValue;
%   cranialIsNearest = spaceFromCaudal >= spaceToCranial;

  %If we are within the valid slice thickness of either slice, no need to
  %interpolate.
%   closeEnough = (spaceToCranial < (CTSliceThickness(cranialSlice)/2) | (spaceFromCaudal < (CTSliceThickness(caudalSlice)/2)));

  %note:  we don't really need the farther matrices except for the else of closeEnough, so speed could be optimized here.
  indCranial = find(abs(allSegmentsM(:,1) - cranialZValue) < 0.001);  %mask values on this slice
  indCaudal = find(abs(allSegmentsM(:,1) - caudalZValue ) < 0.001);  %mask values on this slice

  segmentsCranialM = allSegmentsM(indCranial(:),7:9);     %segments
  segmentsCaudalM = allSegmentsM(indCaudal(:),7:9);     %segments
%   if closeEnough & cranialIsNearest %use cranial
%     for i = 1 : size(segmentsCranialM,1)
%       structM(segmentsCranialM(i,1),segmentsCranialM(i,2):segmentsCranialM(i,3)) = 1;
%     end
%   elseif closeEnough & ~cranialIsNearest %use caudal
%     for i = 1 : size(segmentsCaudalM,1)
%       structM(segmentsCaudalM(i,1),segmentsCaudalM(i,2):segmentsCaudalM(i,3)) = 1;
%     end
%   else %do some interpolation
    cranialM = reusableZerosM;
    caudalM  = reusableZerosM;
    for i = 1 : size(segmentsCranialM,1)
      cranialM(segmentsCranialM(i,1),segmentsCranialM(i,2):segmentsCranialM(i,3)) = 1;
    end
    for i = 1:size(segmentsCaudalM,1)
      caudalM(segmentsCaudalM(i,1),segmentsCaudalM(i,2):segmentsCaudalM(i,3)) = 1;
    end

    if spaceToCranial == 0
        structM = cranialM;
        return;
    end
    if spaceFromCaudal == 0
        structM = caudalM;
        return;
    end

    %Interpolate using distance weighting.  Points interpolated to .5 or
    %greater are on.

    distanceToCranialRegion = spaceToCranial;
    distanceToCaudalRegion  = spaceFromCaudal;

    %Distance from middle point to diagonal neighbor in plane.
    diagDist = sqrt(CTDeltaX^2 + CTDeltaY^2);
    inPlaneNeighborsDistance = [diagDist CTDeltaY diagDist;CTDeltaX 0 CTDeltaX;diagDist CTDeltaY diagDist];
    distToCranialNeighbors = sqrt(inPlaneNeighborsDistance.^2 + distanceToCranialRegion^2);
    distToCaudalNeighbors  = sqrt(inPlaneNeighborsDistance.^2 + distanceToCaudalRegion^2);

    weightsCranial = 1./distToCranialNeighbors;
    weightsCaudal  = 1./distToCaudalNeighbors;
    totalWeight = sum([sum(weightsCranial(:)), sum(weightsCaudal(:))]);

    structM = conv2(single(cranialM), weightsCranial/totalWeight, 'same') + conv2(single(caudalM), weightsCaudal/totalWeight, 'same');
    structM = structM > .5;

end


function test19
%TEST19 look for NaN's from lchol (caused by Intel MKL 7.x bug)
% Example:
%   test19
% See also cholmod_test

% Copyright 2006-2007, Timothy A. Davis, University of Florida

fprintf ('=================================================================\n');
fprintf ('test19: look for NaN''s from lchol (caused by Intel MKL 7.x bug)\n') ;

Prob = UFget (936)							    %#ok
A = Prob.A ;
[p count] = analyze (A) ;
A = A (p,p) ;
tic
L = lchol (A) ;
t = toc ;
fl = sum (count.^2) ;
fprintf ('mflop rate: %8.2f\n', 1e-6*fl/t) ;
n = size (L,1) ;
for k = 1:n
    if (any (isnan (L (:,k))))
	k								    %#ok
	error ('!') ;
    end
end

fprintf ('test19 passed; you have a NaN-free BLAS (must not be MKL 7.x...)\n') ;


function surf = imJointSurface(img, L1, L2, varargin)
%IMJOINTSURFACE Surface area of the interface between two labels
%
%   Deprecated, use 'imJointSurfaceArea' instead.
%
%   S = imJointSurface(LBL, L1, L2)
%   Estimates the joint surface area between the two labels L1 and L2 in
%   the label image LBL.
%
%   S = imJointSurface(LBL, L1, L2, NDIRS)
%   Specifies the number of directions used for estimating surface area.
%   NDIRS can be either 3 or 13 (the default).
%
%   S = imJointSurface(..., RESOL)
%   Specifies image resolution. RESOL is a 1-by-3 row vector containing
%   resolution in the X, Y and Z direction (in that order).
%
%
%   Example
%     % generate a demo image
%     img = discreteBall(1:10, 1:100, 1:100, [50.12 50.23 50.34 40]);
%     % convert to image with two different labels
%     img2 = uint8(img + 1);
%     % compute joint surface area
%     imJointSurface(img2, 1, 2)
%     ans =
%         2.0102e+004
%
%   See also
%     imJointSurfaceArea, imSurface

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-07-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

warning('MatImage:deprecated', ...
    'function imJointSurface is obsolete, use imJointSurfaceArea instead');

% check image dimension and type
if ndims(img) ~= 3 || islogical(img)
    error('first argument should be a 3D image');
end

%% Process input arguments

% default number of directions
ndir = 13;

% default image resolution
delta = [1 1 1];

% Process user input arguments
while ~isempty(varargin)
    var = varargin{1};
    if ~isnumeric(var)
        error('option should be numeric');
    end

    % option is either connectivity or resolution
    if isscalar(var)
        ndir = var;
    else
        delta = var;
    end
    varargin(1) = [];
end

%% Initialisations

% distances between a pixel and its neighbours.
d1  = delta(1);
d2  = delta(2);
d3  = delta(3);

% volume of a voxel (used for computing line densities)
vol = d1 * d2 * d3;

%% Main processing for 3 directions

% number of transitions along the 3 main directions
n1a = sum(sum(sum(img(:,1:end-1,:)==L1 & img(:,2:end,:)==L2)));
n1b = sum(sum(sum(img(:,1:end-1,:)==L2 & img(:,2:end,:)==L1)));
n2a = sum(sum(sum(img(1:end-1,:,:)==L1 & img(2:end,:,:)==L2)));
n2b = sum(sum(sum(img(1:end-1,:,:)==L2 & img(2:end,:,:)==L1)));
n3a = sum(sum(sum(img(:,:,1:end-1)==L1 & img(:,:,2:end)==L2)));
n3b = sum(sum(sum(img(:,:,1:end-1)==L2 & img(:,:,2:end)==L1)));

if ndir == 3
    % compute surface area by averaging over the 3 main directions
    surf = 4/3 * ((n1a+n1b)/d1 + (n2a+n2b)/d2 + (n3a+n3b)/d3) / 2 * vol;
    return;
end

%% Additional processing for 13 directions

% Number of connected components along diagonals contained in the three
% main planes
n4a = sum(sum(sum(img(2:end,1:end-1,:)==L1   & img(1:end-1,2:end,:)==L2)));
n4b = sum(sum(sum(img(2:end,1:end-1,:)==L2   & img(1:end-1,2:end,:)==L1)));
n5a = sum(sum(sum(img(1:end-1,1:end-1,:)==L1 & img(2:end,2:end,:)==L2)));
n5b = sum(sum(sum(img(1:end-1,1:end-1,:)==L2 & img(2:end,2:end,:)==L1)));
n6a = sum(sum(sum(img(:,2:end,1:end-1)==L1   & img(:,1:end-1,2:end)==L2)));
n6b = sum(sum(sum(img(:,2:end,1:end-1)==L2   & img(:,1:end-1,2:end)==L1)));
n7a = sum(sum(sum(img(:,1:end-1,1:end-1)==L1 & img(:,2:end,2:end)==L2)));
n7b = sum(sum(sum(img(:,1:end-1,1:end-1)==L2 & img(:,2:end,2:end)==L1)));
n8a = sum(sum(sum(img(2:end,:,1:end-1)==L1   & img(1:end-1,:,2:end)==L2)));
n8b = sum(sum(sum(img(2:end,:,1:end-1)==L2   & img(1:end-1,:,2:end)==L1)));
n9a = sum(sum(sum(img(1:end-1,:,1:end-1)==L1 & img(2:end,:,2:end)==L2)));
n9b = sum(sum(sum(img(1:end-1,:,1:end-1)==L2 & img(2:end,:,2:end)==L1)));

%TODO: add the case of 9 directions ?

% Number of connected components along lines corresponding to diagonals of
% the unit cube
n10a = sum(sum(sum(img(1:end-1,1:end-1,1:end-1)==L1 & img(2:end,2:end,2:end)==L2)));
n10b = sum(sum(sum(img(1:end-1,1:end-1,1:end-1)==L2 & img(2:end,2:end,2:end)==L1)));
n11a = sum(sum(sum(img(2:end,1:end-1,1:end-1)==L1 & img(1:end-1,2:end,2:end)==L2)));
n11b = sum(sum(sum(img(2:end,1:end-1,1:end-1)==L2 & img(1:end-1,2:end,2:end)==L1)));
n12a = sum(sum(sum(img(1:end-1,2:end,1:end-1)==L1 & img(2:end,1:end-1,2:end)==L2)));
n12b = sum(sum(sum(img(1:end-1,2:end,1:end-1)==L2 & img(2:end,1:end-1,2:end)==L1)));
n13a = sum(sum(sum(img(2:end,2:end,1:end-1)==L1 & img(1:end-1,1:end-1,2:end)==L2)));
n13b = sum(sum(sum(img(2:end,2:end,1:end-1)==L2 & img(1:end-1,1:end-1,2:end)==L1)));

% space between 2 voxels in each direction
d12  = hypot(d1, d2);
d13  = hypot(d1, d3);
d23  = hypot(d2, d3);
d123 = sqrt(d1^2 + d2^2 + d3^2);

% Compute weights corresponding to surface fraction of spherical caps
% For isotropic case, weights correspond to:
% c1 = 0.04577789120476 * 2;  % Ox
% c2 = 0.04577789120476 * 2;  % Oy
% c3 = 0.04577789120476 * 2;  % Oz
% c4 = 0.03698062787608 * 2;  % Oxy
% c6 = 0.03698062787608 * 2;  % Oxz
% c8 = 0.03698062787608 * 2;  % Oyz
% c10 = 0.03519563978232 * 2;  % Oxyz
c = computeDirectionWeights3d13(delta);

% compute the weighted sum of each direction
% intersection count * direction weight / line density
surf = 4 * vol * (...
    (n1a+n1b)*c(1)/d1 + (n2a+n2b)*c(2)/d2 + (n3a+n3b)*c(3)/d3 + ...
    (n4a+n4b+n5a+n5b)*c(4)/d12 + (n6a+n6b+n7a+n7b)*c(6)/d13 + ...
    (n8a+n8b+n9a+n9b)*c(8)/d23 + ...
    (n10a + n10b + n11a + n11b + n12a + n12b + n13a + n13b) * c(10) / d123) / 2;


function output = callmexpress11(interfacedata)

% Retrieve needed data
options = interfacedata.options;
F_struc = interfacedata.F_struc;
c       = interfacedata.c;
Q       = interfacedata.Q;
K       = interfacedata.K;
x0      = interfacedata.x0;
integer_variables = interfacedata.integer_variables;
binary_variables = interfacedata.binary_variables;
UB      = interfacedata.ub;
LB      = interfacedata.lb;

showprogress('Calling XPRESS',options.showprogress);

SENSE = 1;
C = full(c);
if isempty(F_struc)
    A = zeros(1,length(c));A(1)=1;
    B = 1e6;
else
    A =-(F_struc(:,2:end));
    B = full(F_struc(:,1));
end

% XPRESS complains about large bounds
if isempty(LB)
  %  LB = repmat(-2147483647,length(c),1);
else
    LB(LB<-2147483647) = -2147483647;
end
if isempty(UB)
  %  UB = repmat(2147483647,length(c),1);
else
    UB(UB>2147483647) = 2147483647;
end

CTYPE = repmat('L',K.l+K.f,1);  % Inequalities
CTYPE(1:K.f) = 'E';             % Equality constraints
VARTYPE = repmat('C',size(A,2),1);
VARTYPE(integer_variables)='I'; % Integer variables
VARTYPE(binary_variables) ='B'; % Binary variables

if nnz(Q)==0
    H = [];
else
    H = full(2*Q);
end

if options.verbose==0
    options.xpress.msglev = 0;
else
    options.xpress.msglev = 1;
end

if options.savedebug
    save mexpressdebug SENSE H C A B CTYPE LB UB VARTYPE options
end

% Call mex-interface
solvertime = tic;
[x,FMIN,STATUS,EXTRA] = xpress(H,C,A,B,LB,UB,CTYPE,VARTYPE,SENSE,options.xpress);
solvertime = toc(solvertime);
problem = 0;
if isstruct(EXTRA)
    D_struc = -EXTRA.lambda;
else
    D_struc = [];
end

% Check (error code depends on problem type!)
if isempty(union(binary_variables,integer_variables))
    switch STATUS
        case 1
            problem = 0;
        case 2
            problem = 1;
        case 5
            problem = 2;
        case 4
            problem = 3;
        case {0,3,6}
            problem = 11;
        otherwise
            problem = -1;
    end
else
    switch STATUS
        case 6
            problem = 0;
        case 5
            problem = 1;
        case 4
            problem = 2;
        case {4,3}
            problem = 3;
        case {0,1,2}
            problem = 11;
        otherwise
            problem = -1;
    end

end
infostr = yalmiperror(problem,'MEXPRESS');

% Save all data sent to solver?
if options.savesolverinput
	solverinput.H = H;
    solverinput.A = A;
	solverinput.C = C;
	solverinput.B = B;
	solverinput.CTYPE = CTYPE;
	solverinput.LB = LB;
	solverinput.UB = UB;
else
	solverinput = [];
end

% Save all data from the solver?
if options.savesolveroutput
	solveroutput.x = x;
    solveroutput.FMIN = FMIN;
    solveroutput.STATUS = STATUS;
    solveroutput.EXTRA=EXTRA;
else
	solveroutput = [];
end

% Standard interface
output = createOutputStructure(x,D_struc,[],problem,infostr,solverinput,solveroutput,solvertime);


function [model,output] = normalizeExponentialCone(model)

% N.B: YALMIP Definition % x2*exp(x1/x2)  <= x3

% Temporarily extract the data in a more standard name min c'*y, b + Ay >= 0
output.problem = 0;
if ~isempty(model.evalMap)

    % Temporarily extract the data in a more standard name min c'*y, b + Ay >= 0
    data.A = model.F_struc(:,2:end);
    data.b = full(model.F_struc(:,1));
    data.c = full(model.c);
    cones = model.K;
    % Clear away the Power and SDP cone to easier add new rows. Will be put back in
    % place later
    data.A(end-sum(model.K.p)-sum(model.K.s.^2)+1:end,:)=[];
    data.b(end-sum(model.K.p)-sum(model.K.s.^2)+1:end) = [];
    sdpData = model.F_struc(end-sum(model.K.p)-sum(model.K.s.^2)+1:end,:);

    % First check that we have only exponentials
    convexFunctions = [];
    concaveFunctions = [];
    vectorentropy = [];
    vectorkullback = [];
    vectorlogsumexp = [];
    for i = 1:length(model.evalMap)
        switch model.evalMap{i}.fcn
            case {'exp','pexp'}
                convexFunctions = [convexFunctions model.evalMap{i}.computes];
            case {'log','plog','slog'}
                concaveFunctions = [concaveFunctions model.evalMap{i}.computes];
            case 'entropy'
                concaveFunctions = [concaveFunctions model.evalMap{i}.computes];
                if length(model.evalMap{i}.variableIndex) > 1
                    vectorentropy = [vectorentropy i];
                end
            case 'kullbackleibler'
                convexFunctions = [convexFunctions model.evalMap{i}.computes];
                if length(model.evalMap{i}.variableIndex) > 2
                    vectorkullback = [vectorkullback i];
                end
            case 'logsumexp'
                convexFunctions = [convexFunctions model.evalMap{i}.computes];
                vectorlogsumexp = [vectorlogsumexp i];
            otherwise
                % Standard interface, return solver not applicable
                % This should not be able to happen as we check this
                % earlier
                output = createOutputStructure([],[],[],-4,model.solver.tag,[],[],0);
                return
        end
    end
    % Check that all exp/log enter in a convex fashion
    if any(model.K.f)
        if nnz(data.A(1:model.K.f,convexFunctions))>0 || nnz(data.A(1:model.K.f,concaveFunctions))>0
            output = createOutputStructure([],[],[],19,model.solver.tag,[],[],0);
            return
        end
    end
    % Check sign in objective on exp/log terms
    if any(data.c(convexFunctions) < 0) || any(data.c(concaveFunctions) > 0)
        output = createOutputStructure([],[],[],19,model.solver.tag,[],[],0);
        return
    end
    % Check sign in elementwise inequalities on exp/log terms
    if any(model.K.l)
        if nnz(data.A(1+model.K.f:model.K.f+model.K.l,convexFunctions)>0) || nnz(data.A(1+model.K.f:model.K.f+model.K.l,concaveFunctions)<0)
            output = createOutputStructure([],[],[],19,model.solver.tag,[],[],0);
            return
        end
    end
    % Check so there are no exp/log-terms inside cones
    if sum(model.K.q) + model.K.e + sum(model.K.p) + sum(model.K.s) > 0
        if nnz(model.F_struc(1+model.K.f+model.K.l:end,convexFunctions+1))+nnz(model.F_struc(1+model.K.f+model.K.l:end,concaveFunctions+1))>0
            output = createOutputStructure([],[],[],23,model.solver.tag,[],[],0);
            return
        end
    end
    % Scalarize entropy operator
    if ~isempty(vectorentropy)
        for i = vectorentropy(:)'
            involved = model.evalMap{i}.variableIndex;
            computes = model.evalMap{i}.computes;
            n = length(data.c);
            m = length(involved);
            data.A = [sparse(ones(1,m+1),[computes n+(1:m)],[-1 ones(1,m)],1,n+m);
                      data.A spalloc(size(data.A,1),m,0)];
            data.b = [0;data.b];
            data.c = [data.c;zeros(m,1)];
            cones.f = cones.f + 1;
            % The original strucuture now just relates to the first new term
            model.evalMap{i}.computes = n+1;
            model.evalMap{i}.variableIndex = involved(1);
            % and then we add some
            for j = 2:length(involved)
                model.evalMap{end+1} = model.evalMap{i};
                model.evalMap{end}.variableIndex = involved(j);
                model.evalMap{end}.computes = n+j;
            end
        end
    end

    % Scalarize kullbackleibler operator
    if ~isempty(vectorkullback)
        for i = vectorkullback(:)'
            involved = model.evalMap{i}.variableIndex;
            computes = model.evalMap{i}.computes;
            n = length(data.c);
            m = length(involved)/2;
            data.A = [sparse(ones(1,m+1),[computes n+(1:m)],[-1 ones(1,m)],1,n+m);
                      data.A spalloc(size(data.A,1),m,0)];
            data.b = [0;data.b];
            data.c = [data.c;zeros(m,1)];
            cones.f = cones.f + 1;
            % The original strucuture now just relates to the first new term
            model.evalMap{i}.computes = n+1;
            model.evalMap{i}.variableIndex = involved([1 m+1]);
            % and then we add some
            for j = 2:length(involved)/2
                model.evalMap{end+1} = model.evalMap{i};
                model.evalMap{end}.variableIndex = involved([j j+m]);
                model.evalMap{end}.computes = n+j;
            end
        end
    end

    % Scalarize logsumexp operator
    % write log(expx1+expx2..)<= z as exp(x1-z)+exp(x2-z)+...<=1
    if ~isempty(vectorlogsumexp)
        for i = vectorlogsumexp(:)'
            involved = model.evalMap{i}.variableIndex;
            computes = model.evalMap{i}.computes;
            % Orignal #variables
            n = length(data.c);
            % Number of terms in logsumexp
            m = length(involved);

            % exp(x1-z)+exp(x2-z)+...<=1
            data.A = [data.A(1:cones.f,:) spalloc(cones.f,m,0);
                      spalloc(1,n,0) -ones(1,m);
                      data.A(cones.f+1:end,:) spalloc(cones.l+sum(cones.q),m,0)];
            data.b = [data.b(1:cones.f);
                      1;
                      data.b(cones.f+1:end)];
            data.c = [data.c;zeros(m,1)];
            cones.l = cones.l + 1;

            % The original strucuture now just relates to the first new term
            model.evalMap{i}.computes = n+1;
            model.evalMap{i}.variableIndex = [involved(1) computes];
            model.evalMap{i}.fcn = 'expdiff';
            % and then we add some new
            for j = 2:length(involved)
                model.evalMap{end+1} = model.evalMap{i};
                model.evalMap{end}.variableIndex = [involved(j) computes];
                model.evalMap{end}.computes = n+j;
            end
        end
    end

    % Describe all exponential cones
    m = length(model.evalMap);
    cones.e = model.K.e + m;
    for i = 1:m
        switch model.evalMap{i}.fcn
            case 'exp'
                % 1*exp(xv/1) <= xc
                % y*exp(x/y)  <= z.  y new variable, xv the original variable, and xc the "computed"
                x = model.evalMap{i}.variableIndex;
                z = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;3],[x z],-1,3,size(data.A,2))];
                data.b = [data.b;[0;1;0]];
            case 'pexp'
                % xv(1)*exp(xv(2)/xv(1)) <= xc
                x = model.evalMap{i}.variableIndex(2);
                y = model.evalMap{i}.variableIndex(1);
                z = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;2;3],[x y z],[-1 -1 -1],3,size(data.A,2))];
                data.b = [data.b;[0;0;0]];
            case 'log'
                % log(xv) >= xc i.e. xv >= exp(xc/1)*1
                z = model.evalMap{i}.variableIndex;
                x = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;3],[x z],-1,3,size(data.A,2))];
                data.b = [data.b;[0;1;0]];
            case 'plog'
                % xv(1)log(xv(2)/xv(1))>=xc i.e.
                % -xc >= -xv(1)log(xv(2)/xv(1))
                z = model.evalMap{i}.computes;
                y = model.evalMap{i}.variableIndex(1);
                x = model.evalMap{i}.variableIndex(2);
                data.A = [data.A;
                          -sparse([1;2;3],[x y z],[1 -1 1],3,size(data.A,2))];
                data.b = [data.b;[0;0;0]];
            case 'slog'
                % log(1+xv) >= xc i.e. (1+xv) >= exp(xc/1)*1
                z = model.evalMap{i}.variableIndex;
                x = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;3],[x z],-1,3,size(data.A,2))];
                data.b = [data.b;0;1;1];
            case 'entropy'
                % -xv*log(xv)>=xc i.e. 1 >= exp(xc/xv)*xv
                x = model.evalMap{i}.computes;
                y = model.evalMap{i}.variableIndex;
                data.A = [data.A;
                          -sparse([1;2],[x y],[-1 -1],3,size(data.A,2))];
                data.b = [data.b;[0;0;1]];
            case 'kullbackleibler'
                % -xv1*log(xv1/xv2)>=-xc i.e. xv2 >= exp(-xc/xv1)*xv1
                x = model.evalMap{i}.computes;
                y = model.evalMap{i}.variableIndex(1);
                z = model.evalMap{i}.variableIndex(2);
                data.A = [data.A;
                          -sparse([1;2;3],[x y z],[1 -1 -1],3,size(data.A,2))];
                data.b = [data.b;[0;0;0]];
            case 'expdiff'
                % exp(xv(1)-xv(2)) <= xc
                x = model.evalMap{i}.variableIndex;
                z = model.evalMap{i}.computes;
                data.A = [data.A;
                          -sparse([1;1;3],[x(1) x(2) z],[-1 1 -1],3,size(data.A,2))];
                data.b = [data.b;[0;1;0]];
            otherwise
        end
    end
    model.F_struc = [data.b data.A];
    model.c = data.c;
    model.K = cones;
    % Put back the POWER+SDP cone in place
    if any(model.K.s) || any(model.K.p)
        if size(sdpData,2) < size(model.F_struc,2)
            sdpData(end,size(model.F_struc,2)) = 0;
        end
        model.F_struc = [model.F_struc;sdpData];
    end
end
n = size(model.F_struc,2)-1;
if n > length(model.lb)
    missing = n - length(model.lb);
    model.lb = [model.lb;-inf(missing,1)];
end
if size(model.F_struc,2)-1 > length(model.ub)
    missing = n - length(model.ub);
    model.ub = [model.ub;inf(missing,1)];
end


function [output] = generateRandomSample(model, n)

if (nargin < 1)
    error 'function [output] = generateRandomSample(model, n)';
end
if (nargin < 2)
    n = 5000;
end

m.A = model.S;
m.lb = model.lb;
m.ub = model.ub;
% sample until we have mixedfrac of .6 or less
m = gpSampler(m,10,[],0,0);
m.warmupPts = goodInitialPoint(model, n);
mf = 1;
while (mf > .52)
    [m,mf] = gpSampler(m,[],[],200,300);
    %mf
end
m.warmupPts = m.points;
m = rmfield(m, 'points');
mf = 1;
while (mf > .52)
    [m,mf] = gpSampler(m,[],[],200,300);
    %mf
end
m.warmupPts = m.points;
m = rmfield(m, 'points');
mf = 1;
while (mf > .52)
    [m,mf] = gpSampler(m,[],[],200,300);
    %mf
end

output.point = m.points;
output.mf = mf;


%% ============================Notation============================ %%
% X_sub_super
% q_ToFrom
% p_ofWhat_expressedInWhatFrame

%% =============================Setup============================== %%
clear;
close all;
clc;
addpath('utils');

tic
dataDir = '../datasets';

% fileName = 'dataset3';
% fileName = 'dataset3_fresh_10noisy';
% fileName = 'dataset3_fresh_10lessnoisy';
% fileName = 'dataset3_fresh_20lessnoisy';
% fileName = 'dataset3_fresh_40lessnoisy';
% fileName = 'dataset3_fresh_60lessnoisy';
% fileName = 'dataset3_fresh_80lessnoisy';
% fileName = 'dataset3_fresh_100lessnoisy';
% fileName = 'dataset3_fresh_500lessnoisy';
% fileName = '2011_09_26_drive_0035_sync_KLT';
% fileName = '2011_09_26_drive_0005_sync_KLT';
% fileName = '2011_09_30_drive_0020_sync_KLT';
% fileName = '2011_09_26_drive_0027_sync_KLT';
% fileName = '2011_09_30_drive_0020_sync_KLT'; kStart = 2; kEnd = 900;

% Good KITTI runs
% fileName = '2011_09_26_drive_0001_sync_KLT'; kStart = 2; kEnd = 98;
fileName = '2011_09_26_drive_0036_sync_KLT'; kStart = 2; kEnd = 239;
% fileName = '2011_09_26_drive_0051_sync_KLT'; kStart = 2; kEnd = 114;
% fileName = '2011_09_26_drive_0095_sync_KLT'; kStart = 2; kEnd = 139;

load(sprintf('%s/%s.mat',dataDir,fileName));

% r_i_vk_i = p_vi_i;

%Dataset window bounds
% kStart = 2; kEnd = 177;
% kStart = 1215; kEnd = 1715;

%Set constant
numLandmarks = size(y_k_j,3);

%Set up the camera parameters
camera.c_u      = cu;                   % Principal point [u pixels]
camera.c_v      = cv;                   % Principal point [v pixels]
camera.f_u      = fu;                   % Focal length [u pixels]
camera.f_v      = fv;                   % Focal length [v pixels]
camera.b        = b;                    % Stereo baseline [m]
camera.q_CI     = rotMatToQuat(C_c_v);  % 4x1 IMU-to-Camera rotation quaternion
camera.p_C_I    = rho_v_c_v;            % 3x1 Camera position in IMU frame

%Set up the noise parameters
y_var = 11^2 * ones(1,4);               % pixel coord var
noiseParams.u_var_prime = y_var(1)/camera.f_u^2;
noiseParams.v_var_prime = y_var(2)/camera.f_v^2;

w_var = 4e-2 * ones(1,3);               % rot vel var
v_var = 4e-2 * ones(1,3);               % lin vel var
dbg_var = 1e-6 * ones(1,3);            % gyro bias change var
dbv_var = 1e-6 * ones(1,3);            % vel bias change var
noiseParams.Q_imu = diag([w_var, dbg_var, v_var, dbv_var]);

q_var_init = 1e-6 * ones(1,3);         % init rot var
p_var_init = 1e-6 * ones(1,3);         % init pos var
bg_var_init = 1e-6 * ones(1,3);        % init gyro bias var
bv_var_init = 1e-6 * ones(1,3);        % init vel bias var
noiseParams.initialIMUCovar = diag([q_var_init, bg_var_init, bv_var_init, p_var_init]);

% MSCKF parameters
msckfParams.minTrackLength = 10;        % Set to inf to dead-reckon only
msckfParams.maxTrackLength = Inf;      % Set to inf to wait for features to go out of view
msckfParams.maxGNCostNorm  = 1e-2;     % Set to inf to allow any triangulation, no matter how bad
msckfParams.minRCOND       = 1e-12;
msckfParams.doNullSpaceTrick = true;
msckfParams.doQRdecomp = true;

% IMU state for plotting etc. Structures indexed in a cell array
imuStates = cell(1,numel(t));
prunedStates = {};

% imuStates{k}.q_IG         4x1 Global to IMU rotation quaternion
% imuStates{k}.p_I_G        3x1 IMU Position in the Global frame
% imuStates{k}.b_g          3x1 Gyro bias
% imuStates{k}.b_v          3x1 Velocity bias
% imuStates{k}.covar        12x12 IMU state covariance

% We don't really need these outside of msckfState, do we?
% camState = cell(1,numel(t));
% camStates{k}.q_CG        4x1 Global to camera rotation quaternion
% camStates{k}.p_C_G       3x1 Camera Position in the Global frame
% camStates{k}.trackedFeatureIds  1xM List of feature ids that are currently being tracked from that camera state

%msckfState.imuState
%msckfState.imuCovar
%msckfState.camCovar
%msckfState.imuCamCovar
%msckfState.camStates

% Measurements as structures all indexed in a cell array
dT = [0, diff(t)];
measurements = cell(1,numel(t));
% groundTruthStates = cell(1,numel(t));
% groundTruthMap = rho_i_pj_i;

% Important: Because we're idealizing our pixel measurements and the
% idealized measurements could legitimately be -1, replace our invalid
% measurement flag with NaN
y_k_j(y_k_j == -1) = NaN;

for state_k = kStart:kEnd
    measurements{state_k}.dT    = dT(state_k);                      % sampling times
    measurements{state_k}.y     = squeeze(y_k_j(1:2,state_k,:));    % left camera only
    measurements{state_k}.omega = w_vk_vk_i(:,state_k);             % ang vel
    measurements{state_k}.v     = v_vk_vk_i(:,state_k);             % lin vel

    %Idealize measurements
    validMeas = ~isnan(measurements{state_k}.y(1,:));
    measurements{state_k}.y(1,validMeas) = (measurements{state_k}.y(1,validMeas) - camera.c_u)/camera.f_u;
    measurements{state_k}.y(2,validMeas) = (measurements{state_k}.y(2,validMeas) - camera.c_v)/camera.f_v;

    %Ground Truth
    q_IG = rotMatToQuat(axisAngleToRotMat(theta_vk_i(:,state_k)));
    p_I_G = r_i_vk_i(:,state_k);

    groundTruthStates{state_k}.imuState.q_IG = q_IG;
    groundTruthStates{state_k}.imuState.p_I_G = p_I_G;

    % Compute camera pose from current IMU pose
    C_IG = quatToRotMat(q_IG);
    q_CG = quatLeftComp(camera.q_CI) * q_IG;
    p_C_G = p_I_G + C_IG' * camera.p_C_I;

    groundTruthStates{state_k}.camState.q_CG = q_CG;
    groundTruthStates{state_k}.camState.p_C_G = p_C_G;

end

%Struct used to keep track of features
featureTracks = {};
trackedFeatureIds = [];

% featureTracks = {track1, track2, ...}
% track.featureId
% track.observations

%% ==========================Initial State======================== %%
%Use ground truth for first state and initialize feature tracks with
%feature observations
%Use ground truth for the first state

firstImuState.q_IG = rotMatToQuat(axisAngleToRotMat(theta_vk_i(:,kStart)));
firstImuState.p_I_G = r_i_vk_i(:,kStart);
% firstImuState.q_IG = [0;0;0;1];
% firstImuState.q_IG = rotMatToQuat(rotx(90));
% firstImuState.p_I_G = [0;0;0];

[msckfState, featureTracks, trackedFeatureIds] = initializeMSCKF(firstImuState, measurements{kStart}, camera, kStart, noiseParams);
imuStates = updateStateHistory(imuStates, msckfState, camera, kStart);
msckfState_imuOnly{kStart} = msckfState;

%% ============================MAIN LOOP========================== %%

numFeatureTracksResidualized = 0;
map = [];

for state_k = kStart:(kEnd-1)
    fprintf('state_k = %4d\n', state_k);

    %% ==========================STATE PROPAGATION======================== %%

    %Propagate state and covariance
    msckfState = propagateMsckfStateAndCovar(msckfState, measurements{state_k}, noiseParams);
    msckfState_imuOnly{state_k+1} = propagateMsckfStateAndCovar(msckfState_imuOnly{state_k}, measurements{state_k}, noiseParams);

    %Add camera pose to msckfState
    msckfState = augmentState(msckfState, camera, state_k+1);

    %% ==========================FEATURE TRACKING======================== %%
    % Add observations to the feature tracks, or initialize a new one
    % If an observation is -1, add the track to featureTracksToResidualize
    featureTracksToResidualize = {};

    for featureId = 1:numLandmarks
        %IMPORTANT: state_k + 1 not state_k
        meas_k = measurements{state_k+1}.y(:, featureId);

        outOfView = isnan(meas_k(1,1));

        if ismember(featureId, trackedFeatureIds)

            if ~outOfView
                %Append observation and append id to cam states
                featureTracks{trackedFeatureIds == featureId}.observations(:, end+1) = meas_k;

                %Add observation to current camera
                msckfState.camStates{end}.trackedFeatureIds(end+1) = featureId;
            end

            track = featureTracks{trackedFeatureIds == featureId};

            if outOfView ...
                    || size(track.observations, 2) >= msckfParams.maxTrackLength ...
                    || state_k+1 == kEnd

                %Feature is not in view, remove from the tracked features
                [msckfState, camStates, camStateIndices] = removeTrackedFeature(msckfState, featureId);

                %Add the track, with all of its camStates, to the
                %residualized list
                if length(camStates) >= msckfParams.minTrackLength
                    track.camStates = camStates;
                    track.camStateIndices = camStateIndices;
                    featureTracksToResidualize{end+1} = track;
                end

                %Remove the track
                featureTracks = featureTracks(trackedFeatureIds ~= featureId);
                trackedFeatureIds(trackedFeatureIds == featureId) = [];
            end

        elseif ~outOfView && state_k+1 < kEnd % && ~ismember(featureId, trackedFeatureIds)
            %Track new feature
            track.featureId = featureId;
            track.observations = meas_k;
            featureTracks{end+1} = track;
            trackedFeatureIds(end+1) = featureId;

            %Add observation to current camera
            msckfState.camStates{end}.trackedFeatureIds(end+1) = featureId;
        end
    end

    %% ==========================FEATURE RESIDUAL CORRECTIONS======================== %%
    if ~isempty(featureTracksToResidualize)
        H_o = [];
        r_o = [];
        R_o = [];

        for f_i = 1:length(featureTracksToResidualize)

            track = featureTracksToResidualize{f_i};

            %Estimate feature 3D location through Gauss Newton inverse depth
            %optimization
            [p_f_G, Jcost, RCOND] = calcGNPosEst(track.camStates, track.observations, noiseParams);
            % Uncomment to use ground truth map instead
%              p_f_G = groundTruthMap(:, track.featureId); Jcost = 0; RCOND = 1;
%              p_f_C = triangulate(squeeze(y_k_j(:, track.camStates{1}.state_k, track.featureId)), camera); Jcost = 0; RCOND = 1;

            nObs = size(track.observations,2);
            JcostNorm = Jcost / nObs^2;
            fprintf('Jcost = %f | JcostNorm = %f | RCOND = %f\n',...
                Jcost, JcostNorm,RCOND);

            if JcostNorm > msckfParams.maxGNCostNorm ...
                    || RCOND < msckfParams.minRCOND
%                     || norm(p_f_G) > 50

                break;
            else
                map(:,end+1) = p_f_G;
                numFeatureTracksResidualized = numFeatureTracksResidualized + 1;
                fprintf('Using new feature track with %d observations. Total track count = %d.\n',...
                    nObs, numFeatureTracksResidualized);
            end

            %Calculate residual and Hoj
            [r_j] = calcResidual(p_f_G, track.camStates, track.observations);
            R_j = diag(repmat([noiseParams.u_var_prime, noiseParams.v_var_prime], [1, numel(r_j)/2]));
            [H_o_j, A_j, H_x_j] = calcHoj(p_f_G, msckfState, track.camStateIndices);

            % Stacked residuals and friends
            if msckfParams.doNullSpaceTrick
                H_o = [H_o; H_o_j];

                if ~isempty(A_j)
                    r_o_j = A_j' * r_j;
                    r_o = [r_o ; r_o_j];

                    R_o_j = A_j' * R_j * A_j;
                    R_o(end+1 : end+size(R_o_j,1), end+1 : end+size(R_o_j,2)) = R_o_j;
                end

            else
                H_o = [H_o; H_x_j];
                r_o = [r_o; r_j];
                R_o(end+1 : end+size(R_j,1), end+1 : end+size(R_j,2)) = R_j;
            end
        end

        if ~isempty(r_o)
            % Put residuals into their final update-worthy form
            if msckfParams.doQRdecomp
                [T_H, Q_1] = calcTH(H_o);
                r_n = Q_1' * r_o;
                R_n = Q_1' * R_o * Q_1;
            else
                T_H = H_o;
                r_n = r_o;
                R_n = R_o;
            end

            % Build MSCKF covariance matrix
            P = [msckfState.imuCovar, msckfState.imuCamCovar;
                   msckfState.imuCamCovar', msckfState.camCovar];

            % Calculate Kalman gain
            K = (P*T_H') / ( T_H*P*T_H' + R_n ); % == (P*T_H') * inv( T_H*P*T_H' + R_n )

            % State correction
            deltaX = K * r_n;
            msckfState = updateState(msckfState, deltaX);

            % Covariance correction
            tempMat = (eye(12 + 6*size(msckfState.camStates,2)) - K*T_H);
%             tempMat = (eye(12 + 6*size(msckfState.camStates,2)) - K*H_o);

            P_corrected = tempMat * P * tempMat' + K * R_n * K';

            msckfState.imuCovar = P_corrected(1:12,1:12);
            msckfState.camCovar = P_corrected(13:end,13:end);
            msckfState.imuCamCovar = P_corrected(1:12, 13:end);

%             figure(1); clf; imagesc(deltaX); axis equal; axis ij; colorbar;
%             drawnow;

        end

    end

        %% ==========================STATE HISTORY======================== %%
        imuStates = updateStateHistory(imuStates, msckfState, camera, state_k+1);

        %% ==========================STATE PRUNING======================== %%
        %Remove any camera states with no tracked features
        [msckfState, deletedCamStates] = pruneStates(msckfState);

        if ~isempty(deletedCamStates)
            prunedStates(end+1:end+length(deletedCamStates)) = deletedCamStates;
        end

%         if max(max(msckfState.imuCovar(1:12,1:12))) > 1
%             disp('omgbroken');
%         end

        plot_traj;
%     figure(1); imagesc(msckfState.imuCovar(1:12,1:12)); axis equal; axis ij; colorbar;
%     drawnow;
end %for state_K = ...

toc

%% ==========================PLOT ERRORS======================== %%
kNum = length(prunedStates);
p_C_G_est = NaN(3, kNum);
p_I_G_imu = NaN(3, kNum);
p_C_G_imu = NaN(3, kNum);
p_C_G_GT = NaN(3, kNum);
theta_CG_err = NaN(3,kNum);
theta_CG_err_imu = NaN(3,kNum);
err_sigma = NaN(6,kNum); % cam state is ordered as [rot, trans]
err_sigma_imu = NaN(6,kNum);
%
tPlot = NaN(1, kNum);
%
for k = 1:kNum
    state_k = prunedStates{k}.state_k;

    p_C_G_GT(:,k) = groundTruthStates{state_k}.camState.p_C_G;
    p_C_G_est(:,k) = prunedStates{k}.p_C_G;
    q_CG_est  = prunedStates{k}.q_CG;

    theta_CG_err(:,k) = crossMatToVec( eye(3) ...
                    - quatToRotMat(q_CG_est) ...
                        * ( C_c_v * axisAngleToRotMat(theta_vk_i(:,kStart+k-1)) )' );

    err_sigma(:,k) = prunedStates{k}.sigma;
    imusig = sqrt(diag(msckfState_imuOnly{state_k}.imuCovar));
    err_sigma_imu(:,k) = imusig([1:3,10:12]);

    p_I_G_imu(:,k) = msckfState_imuOnly{state_k}.imuState.p_I_G;
    C_CG_est_imu = C_CI * quatToRotMat(msckfState_imuOnly{state_k}.imuState.q_IG);
    theta_CG_err_imu(:,k) = crossMatToVec( eye(3) ...
                    - C_CG_est_imu ...
                        * ( C_CI * axisAngleToRotMat(theta_vk_i(:,kStart+k-1)) )' );

    tPlot(k) = t(state_k);
end

% p_I_G_GT = p_vi_i(:,kStart:kEnd);
p_I_G_GT = r_i_vk_i(:,kStart:kEnd);
p_C_G_GT = p_I_G_GT + repmat(rho_v_c_v,[1,size(p_I_G_GT,2)]);
p_C_G_imu = p_I_G_imu + repmat(rho_v_c_v,[1,size(p_I_G_imu,2)]);

rotLim = [-0.5 0.5];
transLim = [-0.5 0.5];

% Save estimates
msckf_trans_err = p_C_G_est - p_C_G_GT;
msckf_rot_err = theta_CG_err;
imu_trans_err = p_C_G_imu - p_C_G_GT;
imu_rot_err = theta_CG_err_imu;
save(sprintf('../KITTI Trials/msckf_%s', fileName));

armse_trans_msckf = mean(sqrt(sum(msckf_trans_err.^2, 1)/3));
armse_rot_msckf = mean(sqrt(sum(msckf_rot_err.^2, 1)/3));
final_trans_err_msckf = norm(msckf_trans_err(:,end));

armse_trans_imu = mean(sqrt(sum(imu_trans_err.^2, 1)/3));
armse_rot_imu = mean(sqrt(sum(imu_rot_err.^2, 1)/3));
final_trans_err_imu = norm(imu_trans_err(:,end));

fprintf('Trans ARMSE: IMU %f, MSCKF %f\n',armse_trans_imu, armse_trans_msckf);
fprintf('Rot ARMSE: IMU %f, MSCKF %f\n',armse_rot_imu, armse_rot_msckf);
fprintf('Final Trans Err: IMU %f, MSCKF %f\n',final_trans_err_imu, final_trans_err_msckf);

% Translation Errors
figure
subplot(3,1,1)
plot(tPlot, p_C_G_est(1,:) - p_C_G_GT(1,:), 'LineWidth', 2)
hold on
plot(tPlot, 3*err_sigma(4,:), '--r')
plot(tPlot, -3*err_sigma(4,:), '--r')
% ylim(transLim)
xlim([tPlot(1) tPlot(end)])
title('Translational Error')
ylabel('\delta r_x')

subplot(3,1,2)
plot(tPlot, p_C_G_est(2,:) - p_C_G_GT(2,:), 'LineWidth', 2)
hold on
plot(tPlot, 3*err_sigma(5,:), '--r')
plot(tPlot, -3*err_sigma(5,:), '--r')
% ylim(transLim)
xlim([tPlot(1) tPlot(end)])
ylabel('\delta r_y')

subplot(3,1,3)
plot(tPlot, p_C_G_est(3,:) - p_C_G_GT(3,:), 'LineWidth', 2)
hold on
plot(tPlot, 3*err_sigma(6,:), '--r')
plot(tPlot, -3*err_sigma(6,:), '--r')
% ylim(transLim)
xlim([tPlot(1) tPlot(end)])
ylabel('\delta r_z')
xlabel('t_k')

% Rotation Errors
figure
subplot(3,1,1)
plot(tPlot, theta_CG_err(1,:), 'LineWidth', 2)
hold on
plot(tPlot, 3*err_sigma(1,:), '--r')
plot(tPlot, -3*err_sigma(1,:), '--r')
ylim(rotLim)
xlim([tPlot(1) tPlot(end)])
title('Rotational Error')
ylabel('\delta \theta_x')

subplot(3,1,2)
plot(tPlot, theta_CG_err(2,:), 'LineWidth', 2)
hold on
plot(tPlot, 3*err_sigma(2,:), '--r')
plot(tPlot, -3*err_sigma(2,:), '--r')
ylim(rotLim)
xlim([tPlot(1) tPlot(end)])
ylabel('\delta \theta_y')

subplot(3,1,3)
plot(tPlot, theta_CG_err(3,:), 'LineWidth', 2)
hold on
plot(tPlot, 3*err_sigma(3,:), '--r')
plot(tPlot, -3*err_sigma(3,:), '--r')
ylim(rotLim)
xlim([tPlot(1) tPlot(end)])
ylabel('\delta \theta_z')
xlabel('t_k')


function OUTPUT = readsacfile(varargin)
   %readsacfile    Read SAC binary files.
   %    usage:  output = readsacfile('sacfile')
   %         OUTPUT is a struct with fields "header" and "amplitudes"
   %         OUTPUT.header is an array of cells, each parsed into the proper
   %         data type (ie, SINGLE, CHAR, LOGICAL, etc.).
   %         OUTPUT.amplitudes is an array of the measured data

   % VERSION: 1.1 of waveform objects
   % AUTHOR: Celso Reyes   creyes@gi.alaska.edu
   %    modified from Michael Thorne (4/2004)
   % LASTUPDATE: 9/2/2009

   try
      OUTPUT = mainFunction('little-endian',varargin{:});
   catch
      % disp('big-endian')
      OUTPUT = mainFunction('big-endian',varargin{:});
   end
end
function OUTPUT = mainFunction(endian,varargin)
   for nrecs = 1:nargin-1

      sacfile = varargin{nrecs};

      %---------------------------------------------------------------------------
      %    Default byte-order
      %    endian  = 'big-endian' byte order (e.g., UNIX)
      %            = 'little-endian' byte order (e.g., LINUX)

      %endian = 'little-endian';
      %endian = 'big-endian';

      if strcmp(endian,'big-endian')
         fid = fopen(sacfile,'r','ieee-be');
      elseif strcmp(endian,'little-endian')
         fid = fopen(sacfile,'r','ieee-le');
      end

      %preallocate
      h = cell(1,123);

      % read in single precision real header variables:
      %---------------------------------------------------------------------------
      for i=1:70
         h(i) = {fread(fid,1,'single')};
      end

      % read in single precision integer header variables:
      %---------------------------------------------------------------------------
      for i=71:105
         h(i) = {fread(fid,1,'int32')};
      end

      % Check header version = 6 and issue warning
      %---------------------------------------------------------------------------
      % If the header version is not NVHDR == 6 then the sacfile is likely of the
      % opposite byte order.  This will give h(77) some ridiculously large
      % number.  NVHDR can also be 4 or 5.  In this case it is an old SAC file
      % and rsac cannot read this file in.  To correct, read the SAC file into
      % the newest verson of SAC and w over.
      %
      if (h{77} == 4 || h{77} == 5)
         message = strcat('NVHDR = 4 or 5. File: "',sacfile,'" may be from an old version of SAC.');
         fclose(fid);
         error('Waveform:readsacfile:oldSacVersion',message)
      elseif h{77} ~= 6
         message = strcat('Current readsacfile byte order: "',endian,'". File: "',sacfile,'" may be of opposite byte-order.');
         fclose(fid);
         error('Waveform:readsacfile:wrongEndian',message)
      end

      % read in logical header variables
      %---------------------------------------------------------------------------
      for i=106:110
         h(i) = {fread(fid,1,'int32')};
      end

      % read in character header variables
      %---------------------------------------------------------------------------
      strlens = repmat(8,23,1); strlens(2) = 16;
      for i=111:133;%111:8:302
         h(i) = {(fread(fid,strlens(i-110),'char=>char'))'};
      end

      % read in amplitudes
      %---------------------------------------------------------------------------

      OUTPUT(nrecs).amplitudes = fread(fid,'single');
      fclose(fid);

      % arrange output files
      %------------------------------------------------------------------------
      %---
      OUTPUT(nrecs).header = h(:)';

   end
end


function ori = project2FundamentalRegion(ori,varargin)
% projects orientation to a fundamental region
%
% Syntax
%   ori = project2FundamentalRegion(ori,rot_ref)
%
% Input
%  ori     - @orientation
%  ori_ref - reference @rotation
%
% Output
%  ori     - @orientation
%  omega   - rotational angle to reference rotation
%

if isempty(ori.SS) || ismember(ori.SS.id, [1,2])
  ori = project2FundamentalRegion@quaternion(ori,ori.CS,varargin{:});
else
  if ori.antipodal, ap = {'antipodal'}; else, ap = {}; end
  CS = ori.CS; SS = ori.SS;
  ori = project2FundamentalRegion@quaternion(ori,CS,SS,ap{:},varargin{:});

  % ensure result is of type orientation
  if ~isa(ori,'orientation')
    ori = orientation(ori,CS,SS,ap{:});
  end

end


clear all;

% load previously generated fig file
figFile = 'multiple.fig';
open(figFile)

% change properties
opt.XLabel = 'Time, t (ms)'; % xlabel
opt.YLabel = 'Voltage, V (V)'; %ylabel
opt.YTick = [-10, 0, 10]; %[tick1, tick2, .. ]
opt.XLim = [0, 80]; % [min, max]
opt.YLim = [-11, 11]; % [min, max]

opt.Colors = [ % three colors for three data set
    1,      0,       0;
    0.25,   0.25,    0.25;
    0,      0,       1;
    ];

opt.LineWidth = [2, 2, 2]; % three line widths
opt.LineStyle = {'-', '-', '-'}; % three line styles
opt.Markers = {'o', '', 's'};
opt.MarkerSpacing = [15, 15, 15];
opt.Legend = {'\theta = 0^o', '\theta = 45^o', '\theta = 90^o'}; % legends

% Save? comment the following line if you do not want to save
opt.FileName = 'plotMarkers.png';

% create the plot
setPlotProp(opt);


function another_colormap
%ANOTHER_COLORMAP try another color map
%
% Example:
%   another_colormap
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

j = jet (128) ;
j = j (48:112, :) ;

% jj = linspace (0,1,64)' ./ sum (jet,2) ;
% j (:,1) = j (:,1) .* jj ;
% j (:,2) = j (:,2) .* jj ;
% j (:,3) = j (:,3) .* jj ;

% white = [1 1 1] ;
% gray = [.5 .5 .5] ;                                                       %#ok

% j = [white ; purple ; j ] ;
disp ('j = ') ;
disp (j)

image (1:size(j,1)) ;
colormap (j) ;


function [A,C] = update_spatial_components_nb(Y,C,A_,P,options)

% update spatial footprints and background through Basis Pursuit Denoising
% for each pixel i solve the problem
%   [A(i,:),b(i)] = argmin sum(A(i,:))
%       subject to || Y(i,:) - A(i,:)*C + b(i)*f || <= sn(i)*sqrt(T);
% for each pixel the search is limited to a few spatial components

% INPUTS:
% Y:    raw data
% C:    temporal components
% f:    temporal background
% A_:   current estimate of spatial footprints (used for determining search locations only)
% P:    dataset parameters (used for noise values and interpolated entries)

% options    parameter struct (for noise values and other parameters)

% OUTPUTS:
% A:    new estimate of spatial footprints
% C:    temporal components (updated only when spatial components are completely removed)

% Written by:
% Pengcheng Zhou, Columbia University, 2015,
% with modifications from update_spatial_components.m
% Eftychios A. Pnevmatikakis, Simons Foundation, 2015

warning('off', 'MATLAB:maxNumCompThreads:Deprecated');
memmaped = isobject(Y);
if memmaped
    sizY = size(Y,'Y');
    d = prod(sizY(1:end-1));
    T = sizY(end);
else
    [d,T] = size(Y);
end
if nargin < 5 || isempty(options); options = []; end
if ~isfield(options,'d1') || isempty(options.d1); d1 = input('What is the total number of rows? \n'); options.d1 = d1; else d1 = options.d1; end          % # of rows
if ~isfield(options,'d2') || isempty(options.d2); d2 = input('What is the total number of columns? \n'); options.d2 = d2; else d2 = options.d2; end          % # of columns
if ~isfield(options,'show_sum'); show_sum = 0; else show_sum = options.show_sum; end            % do some plotting while calculating footprints
if ~isfield(options,'interp'); Y_interp = sparse(d,T); else Y_interp = options.interp; end      % identify missing data
if ~isfield(options,'use_parallel'); use_parallel = ~isempty(which('parpool')); else use_parallel = options.use_parallel; end % use parallel toolbox if present
if ~isfield(options,'search_method'); method = []; else method = options.search_method; end     % search method for determining footprint of spatial components

if nargin < 2 || (isempty(A_) && isempty(C))  % at least either spatial or temporal components should be provided
    error('Not enough input arguments')
else
    if ~isempty(C); K = size(C,1); elseif islogical(A_); K = size(A_,2); else K = size(A_2,2) - options.nb; end
end

if nargin < 5 || isempty(P); P = preprocess_data(Y,1); end  % etsimate noise values if not present
if nargin < 4 || isempty(A_);
    IND = ones(d,size(C,1));
else
    if islogical(A_)     % check if search locations have been provided, otherwise estimate them
        IND = A_;
        if isempty(C)
            INDav = double(IND)/diag(sum(double(IND)));
            C = max(INDav'*Y,0);
        end
    else
        IND = determine_search_location(A_(:,1:K),method,options);
    end
end

options.sn = P.sn;
if ~memmaped
    Y(P.mis_entries) = NaN; % remove interpolated values
end

Cf = C;

if use_parallel         % solve BPDN problem for each pixel
    Nthr = max(2*maxNumCompThreads,round(d*T/2^24));
    siz_row = [floor(d/Nthr)*ones(Nthr-1,1);d-floor(d/Nthr)*(Nthr-1)];
    indeces = [0;cumsum(siz_row)];
    if ~memmaped
        Ycell = mat2cell(Y,siz_row,T);
    else
        Ycell = cell(Nthr,1);
    end
    INDc =  mat2cell(IND,siz_row,K);
    Acell = cell(Nthr,1);
    Psnc = mat2cell(options.sn(:),siz_row,1);
    for nthr = 1:Nthr
        Acell{nthr} = zeros(siz_row(nthr),size(Cf,1));
        if memmaped
            Ytemp = double(Y.Yr(indeces(nthr)+1:indeces(nthr+1),:));
        else
            Ytemp = Ycell{nthr};
        end
        for px = 1:siz_row(nthr)
            fn = ~isnan(Ytemp(px,:));       % identify missing data
            ind = find(INDc{nthr}(px,:));
            if ~isempty(ind);
                %[~, ~, a, ~] = lars_regression_noise(Ycell{nthr}(px,fn)', Cf(ind2,fn)', 1, Psnc{nthr}(px)^2*T);
                [~, ~, a, ~] = lars_regression_noise(Ytemp(px,fn)', Cf(ind,fn)', 1, Psnc{nthr}(px)^2*T);
                Acell{nthr}(px,ind) = a';
            end
        end
    end
    A = cell2mat(Acell);
else
    A = zeros(d,K);
    sA = zeros(d1,d2);
    for px = 1:d   % estimate spatial components
        fn = ~isnan(Y(px,:));       % identify missing data
        ind = find(IND(px,:));
        if ~isempty(ind);
            [~, ~, a, ~] = lars_regression_noise(Y(px,fn)', Cf(ind,fn)', 1, options.sn(px)^2*T);
            a(isnan(a)) = 0;
            A(px,ind) = a';
            sA(px) = sum(a);
        end
        if show_sum
            if mod(px,d1) == 0;
               figure(20); imagesc(sA); axis square;
               title(sprintf('Sum of spatial components (%i out of %i columns done)',round(px/d1),d2)); drawnow;
            end
        end
    end
end

A(isnan(A))=0;
A = sparse(A);
A = threshold_components(A,options);  % post-processing of components

fprintf('Updated spatial components \n');

ff = find(sum(A(:,1:K))==0);           % remove empty components
if ~isempty(ff)
    K = K - length(ff);
    A(:,ff) = [];
    C(ff,:) = [];
end