function [] = aps_weather_model_SAR(model_type)
% [] = aps_weather_model_SAR()
% Script to load weather model data and compute SAR delays
% The DEM file can be .grd file or other. If the latter, the DEM should have an asociated
% ".rsc" file, with the same filename as the DEM. The ".rsc" files should
% contain a WIDTH, LENGTH, X_FIRST, Y_FIRST, X_STEP, Y_STEP and optional a
% FORMAT string. The weather model data is assumed to be structured in date_before(d,:) folders.
%
%
% INPUTS:
% demfile               Full path to the DEM file. The DEM needs to me in meters.
% xlims                 Limits in the x-direction, either in degrees
% ylims                 Limits in the y-direction, either in degrees
% demnull               The value for no DEM data, default is -32768.
% smpres                The output resolution, either in degrees
%                       Units needs to be consistend with xlims and ylims.
%
% OUTPUTS:
% It will give the computed ZENITH hydrostatic and wet delay map in cm for the selected region.
%
%     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.
%
%
% Modified from Richard Walters - Oxford / Leeds University 2012-2013
% Modifcations:
% DB 	10/2013		Convert script to a function, integrate in aps_toolbox,
%                   add syntax. Retrieve parameters automatic.
% DB    10/2013     Output the combined delay map.
% DB    10/2013     Increased processing efficiency
% DB    10/2013     add compatibility with Doris construct_dem.sh script
% DB    10/2013     Bug fix incase the the netcdf is split over two days
% DB    11/2013     Change the computation f saturated pressure to be
%                   identical witt how it was computed in ERA-I model.
% DB    11/2013     Update filenaming to Hydrostatic as its not dry delay.
%                   Compute the hydrostatic delay directly from int(P/T)
% DB    01/2014     Fix bug for the ERA-I longitude boundary, being set to small.
% DB    01/2014     Allow for tiled ERA-I data
% HB-DB 02/2014     Fix to include ECMWF with scaling and offset
% DB    02/2014     Include a parameter for ECMWF data website
% DB    03/2014     Include an extra check for the DEM grd file and the
%                   selected crop
% DB 	03/2014     Save individual time stamp SAR date_before(d,:) processing
% DB    07/2014     Redefine meris_lat(lon)_range to region_lat(lon)_range
% DB    08/2014     Check if netcdf exist (ERA-I is not an operation service)
% DB    11/2014     Include option to output the 3D delays for each date_before(d,:)
% HW    02/2015     check dem file format automatically
% DB 	06/2015     Bug fix for plotting the support information
% DB    06/2015     Fix typo in error message
% DB    11/2015     Branch of the DEM in to seperate function same for all codes
% DB    11/2015     Add multicore option from matlab
% DB 	02/2016     Close netcdf files
% DB    04/2016     Branch into weather model script and include merra too
% SSS   04/2016     Clear variables such memory need is reduced
% DB    07/2016     redefine hydrostatic delay to be based on surface pressure.
% DB    08/2016     Uncomment a keyboard
% DB    07/2016     expand to include ERA5 test model data
% KM    02/2018     expand to include NARR model data

fig_test = 0;           % when 1 show the dem as debug figure
save_3D_delays = 0;     % When 1 saves the tropopsheric delays for each x,y and with height

if nargin<1
    error('Give at least the model_type: era, era5, narr, merra, or merra2')
end
% change to lower caps for saving and filename generation consistency
model_type = lower(model_type);

%% Constants
% Parameters from Jolivet et al 2011, GRL
Rd = 287.05;            % [j/kg/K] specific gas constants dry air
Rv = 461.495;           % [j/kg/K] water vapour
k1 = 0.776;             % [K/Pa]
k2 = 0.716;             % [K/Pa]
k3 = 3.75e3;            % [K^2/Pa]
coeff.Rd = Rd;
coeff.Rv = Rv;

%% Defaults
zref = 15000;       % zref for integration- tropopause
zincr = 15;         % z increment spacing for vertical interpolation before integration
vertres = 5;        % vertical resolution for comparing dem to vert profiles of delay

%% output file names
% output file for the DEM and look angles
smpdem = 'dem_smp.xyz';

% getting the variables from the parms_aps file
stamps_processed = getparm_aps('stamps_processed',1);

%%% defaults for the weather models. If not applicable it will be changed below for the specific model.
if strcmp(model_type,'narr')
    timelist_model = ['0000' ;'0300'; '0600' ; '0900'; '1200' ;'1500'; '1800' ;'2100'; '0000'];
    model_lag = 8*7;    % days
else
    timelist_model= ['0000' ; '0600' ; '1200' ; '1800' ; '0000'];       % the time interval the model is outputed
    model_lag = 0; % ? check lags
end
era_data_type = [];                                                 % the weather model data type for ERA only.

%%% Updating specific weather model information
if strcmpi(model_type,'era')
    weather_model_datapath = getparm_aps('era_datapath',1);
    era_data_type = getparm_aps('era_data_type');         % the datatype of the model either BADC or ERA
elseif strcmpi(model_type,'merra') || strcmpi(model_type,'merra2')
    weather_model_datapath = getparm_aps('merra_datapath',1);
elseif strcmpi(model_type,'narr')
    weather_model_datapath = getparm_aps('narr_datapath',1);

else
    error(['weather model type not supported, either: wrf, era, narr, merra for now'])
end

lambda = getparm_aps('lambda',1)*100;                       % radar wavelength in cm
demfile = getparm_aps('demfile',1);
demnull = getparm_aps('dem_null',1);
UTC_sat =  getparm_aps('UTC_sat',1);
ifgday_matfile = getparm_aps('ifgday_matfile',1);
ifgs_dates = load(ifgday_matfile);

% loading the data
if strcmp(stamps_processed,'y')
   dates = ifgs_dates.day;
   load psver
   fprintf('Stamps processed structure \n')
else
    psver = 2;
    ifgs_dates = ifgs_dates.ifgday;
    dates = reshape(ifgs_dates,[],1);
    dates = unique(dates);
    dates = datenum(num2str(dates),'yyyymmdd');
end

% getting the dates
n_dates = length(dates);

%% Compute and resample DEM
[dem,xmin,xmax,ymin,ymax,smpres,nncols,nnrows] = get_DEM;

% the region which is cropped from the ERA data and used to make the interpolation.
% Should be  larger than the region to which the delay is computed
lonmin = floor(xmin)-1;
lonmax= ceil(xmax)+1;
latmin = floor(ymin)-1;
latmax = ceil(ymax)+1;

% setting the maximum height of the DEM to limit the range at which ERA-I
% needs to be interpolated to
maxdem = ceil(max(max(dem))/100)*100+50; % max height of dem in metres

fprintf(['Interpolate to a maximum dem height of ', num2str(maxdem) ,' m\n'])

%% Compute based on Satellite pass which weather model outputs that will be used
[time_before,time_after, date_before, date_after,f_before,f_after] = aps_weather_model_times(timelist_model,dates,UTC_sat,model_lag);

%% generating a file
[modelfile_before,modelfile_after] = aps_weather_model_filenames(model_type,time_before,time_after,date_before, date_after,weather_model_datapath);

%% performing the calucluation for each date
for d = 1:n_dates

    % the save filenames
    outfile = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZWD.xyz'];
    hydroutfile = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZHD.xyz'];

    no_data = 0;
    for kk = 1:2
        if kk == 1
            file = modelfile_before(d,:);
        end
        if kk == 2
            file = modelfile_after(d,:);
        end

        % check
        if exist(file,'file')~=2
            no_data = no_data+1;
        else

            % loading the weather model data
            if  strcmpi(model_type,'era')
                 [ Temp,e,Geopot,P,longrid,latgrid,xx,yy,lon0360_flag] =  aps_load_era(file,era_data_type) ;
            elseif  strcmpi(model_type,'era5')
                 [ Temp,e,Geopot,P,longrid,latgrid,xx,yy,lon0360_flag] =  aps_load_era(file,era_data_type) ;
            elseif strcmpi(model_type,'merra') || strcmpi(model_type,'merra2')
                 [ Temp,e,Geopot,P,longrid,latgrid,xx,yy,lon0360_flag] =  aps_load_merra(file,model_type,coeff) ;
            elseif strcmpi(model_type,'narr')
                 [ Temp,e,Geopot,P,longrid,latgrid,xx,yy,lon0360_flag] =  aps_load_narr(file,model_type,coeff) ;
            end
%             longrid=longrid';latgrid=latgrid';
            % verify and cope with NAN's
            [Temp,e,Geopot,P,longrid,latgrid] =  aps_weather_model_nan_check(Temp,e,Geopot,P,longrid,latgrid) ;

            % define weather model grid nodes
            latlist = reshape(latgrid(:,:,1),[],1);
            lonlist = reshape(longrid(:,:,1),[],1);
            xlist = reshape(xx,[],1);
            ylist = reshape(yy,[],1);

            % Limit weather model to those grid points around the user-defined InSAR box
            % Getting the weather model resolution
            lat_res = abs(diff(unique(latgrid)))*1.5;
            lat_res = lat_res(1);
            lon_res = abs(diff(unique(longrid)))*1.5;
            lon_res = lon_res(1);

            % make sure the weather model and the lonlat specified by user
            % is defined in the same way. Weatehr models can be [0 360]
            % degrees. User can be [-180 180] unlikely [0 360]
            if strcmpi(lon0360_flag,'y')
                % fprintf('This needs to be defined better and some checks are needed \n')
                % not all pixels should be shifted.

                if xmin<0
                   xmin= xmin+360;
                end
                if xmax<0
                   xmax = xmax +360;
                end
            end

            % generation a plot
            if fig_test ==1 & d==1 & kk==1
                fontsize = 15;
                hfig = figure('name',[model_type ' weather model nodes and InSAR region'],'position', [ 200   243   610   603]);
                % for the legend generation
                plot(mean([xmin xmax]),mean([ymax ymin]),'wo','markeredgecolor','k','markerfacecolor','w','markersize',15)
                hold on
                plot(mean([xmin xmax]),mean([ymax ymin]),'r.')
                hold on
                plot(mean([xmin xmax]),mean([ymax ymin]),'r-','linewidth',2)

                imagesc([xmin xmax],[ymax ymin],dem)
                cc=colorbar;
                view(0,90)

                axis xy
                ylabel(cc,'Topography [m]','fontsize',fontsize)
                hold on
                plot(lonlist,latlist,'wo','markeredgecolor','k','markerfacecolor','w','markersize',15)
                hold on
                plot([xmin xmin xmax xmax xmin],[ymin ymax ymax ymin ymin],'r-','linewidth',2)

                title([model_type ' weather model nodes'],'fontsize',fontsize)
                set(gca,'fontsize',fontsize)
                axis equal
                axis tight

                legend('location','northoutside',[model_type ' weather model nodes'],['Used ' model_type ' weather model nodes'],'InSAR box')
            end

            % making the weather model grid slightly larger than the InSAR
            % bounding box. Will use the weather model resolution for this
            % to make sure an extra grid node is included.
            ix = find(ymin-lat_res<= latlist & latlist <= ymax+lat_res  & xmin-lon_res<= lonlist & lonlist<= xmax+lon_res) ;
            xlist = xlist(ix);
            ylist = ylist(ix);
            latlist = latlist(ix);
            lonlist = lonlist(ix);
            numy = length(unique(latlist));
            numx = length(unique(lonlist));
            ulatlist = unique(latlist);
            ulonlist = unique(lonlist);
            uxlist = unique(xlist);
            uylist = unique(ylist);

            if fig_test ==1 & d==1 & kk==1
                figure(hfig)
                hold on
                plot(lonlist,latlist,'r.')
                xlim([xmin-4*lon_res  xmax+4*lon_res])
                ylim([ymin-4*lat_res  ymax+4*lat_res])
                str='';
                while ~strcmpi(str,'y') && ~strcmpi(str,'n')
                    fprintf(['Do the red nodes extend (just) outside the red InSAR rectangle? \n'])
                    str = input('Continue? [y: for yes, n: no] \n','s');
                end
                if strcmpi(str,'n')
                    error('Check your lon lat crop, otherwize extend area of downlaoded weather model data.')
                end
            end

            clear lon_res lat_res ylist xlist %%%SSS 4/16

            % saving the information for support plotting
            eval([model_type '.' model_type '_lonlat =[lonlist latlist];']);                            % era.era_lonlat = [lonlist latlist];
            eval([model_type '.region =[[xmin xmin xmax xmax xmin]'' [ymin ymax ymax ymin ymin]''];']); % era.region = [[xmin xmin xmax xmax xmin]' [ymin ymax ymax ymin ymin]'];
            deminfo.xmin = xmin;
            deminfo.xmax = xmax;
            deminfo.ymax = ymax;
            deminfo.ymin = ymin;
            deminfo.dem = dem;
            eval([model_type '.deminfo =deminfo;']);                                                    % era.deminfo = deminfo;
            clear deminfo
            % checking if the file already exist. Yes append otherwiuze create it
            if exist('tca_support.mat','file')==2
                eval(['save(''tca_support.mat'',''-append'',''' model_type ''');']);                    % save('tca_support.mat','-append','era')
            else
                eval(['save(''tca_support.mat'',''' model_type ''');']);                                % save('tca_support.mat','era')
            end
%             if fig_test ==1 & d==1 & kk==1
%                     if exist(['aps_' model_type],'dir')~=7
%                         mkdir(['aps_' model_type]);
%                     end
%                     print(hfig,'-dpng',['aps_' model_type  filesep model_type '_datapoints.png'])
%                     print(hfig,'-depsc',['aps_' model_type filesep model_type '_datapoints.eps'])
%             end
            eval(['clear ' model_type]);

            % Convert Geopotential to Geopotential Height and then to Geometric Height
            g0 = 9.80665;
            % Calculate Geopotential Height, H
            H = Geopot./g0;

            % map of g with latitude
            g = 9.80616.*(1 - 0.002637.*cosd(2.*latgrid) + 0.0000059.*(cosd(2.*latgrid)).^2);
            % map of Re with latitude
            Rmax = 6378137;
            Rmin = 6356752;
            Re = sqrt(1./(((cosd(latgrid).^2)./Rmax^2) + ((sind(latgrid).^2)./Rmin^2)));

            % Calculate Geometric Height, Z
            Z = (H.*Re)./(g/g0.*Re - H);

            % Find middle of scene to work out glocal and Rlocal for use later
            midx = round(mean(uxlist));
            midy = round(mean(uylist));
            glocal = g(midy,midx,1);
            Rlocal = Re(midy,midx,1);

            cdslices = maxdem/vertres +1;
            cdstack = zeros(numy,numx,cdslices);
            cdstack_dry = zeros(numy,numx,cdslices);
            cdstack_wet = zeros(numy,numx,cdslices);

            XI=(0:zincr:zref)';
            gh = glocal.*(Rlocal./(Rlocal+XI)).^2; %gravity with height for XI height range

            % Interpolate Temp P and e from 0:20:15000 m
            % then integrate using trapz to estimate delay as function of height
            for i = 1:numx;
                for j = 1:numy;
                    xn = uxlist(i);
                    yn = uylist(j);

                    %interpolate at res zincr before doing integration
                    X=double(squeeze(Z(yn,xn,:)));
                    Ye=double(squeeze(e(yn,xn,:)));
                    YeI = interp1(X,Ye,XI,'spline')*100; %change from hPa to Pa

                    YP=double(squeeze(P(yn,xn,:)));
                    YPI = interp1(X,YP,XI,'spline')*100; %change from hPa to Pa

                    YT=double(squeeze(Temp(yn,xn,:)));
                    YTI = interp1(X,YT,XI,'spline');

                    tmp1 = ((k2-(Rd*k1/Rv)).*YeI./YTI + k3.*YeI./(YTI.^2));
                    Lw = (10^-6).*-1*flipud(cumtrapz(flipud(XI),flipud(tmp1)));
                    % L is zenith delay one way in metres
                    % tmp2 = k1.*YPI./YTI;
                    %Ld = (10^-6).*-1*flipud(cumtrapz(flipud(XI),flipud(tmp2)));             % This is using P/T expression (Hanssen, 2001)
                    gm = glocal;
                    Ld = (10^-6).*((k1*Rd/gm).*(YPI - YPI(zref/zincr +1)));                 % This is P0 expression (Hanssen, 2001)

                    % Interpolate important part (i.e. total delay at elevations
                    % less than maxdem) at high res i.e. vertres, and put in cdstack.
                    cdI=(0:vertres:maxdem)';
                    LdI=interp1(XI,Ld,cdI,'spline');
                    LwI=interp1(XI,Lw,cdI,'spline');

                    %cdstack(j,i,:) = LsI;
                    cdstack_dry(j,i,:) = LdI;
                    cdstack_wet(j,i,:) = LwI;

                    if save_3D_delays==1
                        cdI3D=(0:100:max(XI))';
                        LdI3D=interp1(XI,Ld,cdI3D,'spline');
                        LwI3D=interp1(XI,Lw,cdI3D,'spline');

                        %cdstack(j,i,:) = LsI;
                        cdstack_dry3D(j,i,:) = LdI3D;
                        cdstack_wet3D(j,i,:) = LwI3D;
                        clear LdI3D LwI3D
                    end
                end
            end
            clear uxlist uylist ulonlist ulatlist %%%SSS 4/16

            % Interpolate each cdstack layer onto a grid given by the DEM extents
            % in UTM m.
            xsmpres = (xmax-xmin)/nncols;
            ysmpres = (ymax-ymin)/nnrows;
            [xi,yi] = meshgrid(xmin+0.5*xsmpres:xsmpres:xmax-0.5*xsmpres,ymin+0.5*ysmpres:ysmpres:ymax-0.5*ysmpres);

            ix_temp = diff(lonlist);
            ix_temp = find(ix_temp~=0);
            ix_temp = ix_temp(1);
            lonlist_matrix = reshape(lonlist,ix_temp,[]) ;
            latlist_matrix = reshape(latlist,ix_temp,[]) ;
            clear lonlist latlist %%%SSS 4/16

            % saving the outputs
            if save_3D_delays==1
                if kk==1
                    clear hgt lon lat dry1 dry2 wet1 dem_temp hgt_topo %%%SSS 4/16
                    hgt = cdI3D;
                    lon = [lonlist_matrix];
                    lat = latlist_matrix;
                    dry1 = cdstack_dry3D;
                    wet1 = cdstack_wet3D;

                    % also give the station topography
                    dem_temp = dem;
                    dem_temp(isnan(dem_temp))=0;
                    hgt_topo = griddata(xi,yi,dem_temp,lon,lat,'linear');
                    clear dem_temp

                elseif kk==2
                    clear dry2 wet2
                    dry2 = cdstack_dry3D;
                    wet2 = cdstack_wet3D;
                end
            end

            clear cdstack_interp_dry %%%SSS 4/16
            cdstack_interp_dry = zeros(nnrows,nncols,cdslices);
            parfor n = 1:cdslices
                newslice = interp2(lonlist_matrix,latlist_matrix,cdstack_dry(:,:,n),xi,yi,'linear');
                cdstack_interp_dry(:,:,n)= flipud(newslice); % flipud due to ypositive downpage for matlab figs, and ypositive uppage for utmy
            end

            clear cdstack_interp_wet %%%SSS 4/16
            cdstack_interp_wet = zeros(nnrows,nncols,cdslices);
            parfor n = 1:cdslices
               newslice = interp2(lonlist_matrix,latlist_matrix,cdstack_wet(:,:,n),xi,yi,'linear');
               cdstack_interp_wet(:,:,n)= flipud(newslice); % flipud due to ypositive downpage for matlab figs, and ypositive uppage for utmy
            end
            clear lonlist_matrix latlist_matrix %%%SSS 4/16
            % keeping the coordinates in the same grid as the data
            xi = flipud(xi);
            yi = flipud(yi);

            % Pull out delays from cdstack layers that match dem heights
            clear wetcorrection hydrcorrection rounddem %%%SSS 4/16
            wetcorrection = ones(nnrows,nncols);
            hydrcorrection = ones(nnrows,nncols);
            rounddem = round(dem/vertres);
            rounddem(dem < 0)=0;

            % cope with the case that NaN are present. This can be sea-level
            rounddem(isnan(dem))=0;

            for i=1:nnrows
                for j=1:nncols
                    wetcorrection(i,j) = cdstack_interp_wet(i,j,rounddem(i,j)+1);
                end
            end

            for i=1:nnrows
                for j=1:nncols
                    hydrcorrection(i,j) = cdstack_interp_dry(i,j,rounddem(i,j)+1);
                end
            end

            if kk==1
                wetcorrection1 = wetcorrection;
                drycorrection1 = hydrcorrection;
            end
            if kk==2
                wetcorrection2 = wetcorrection;
                drycorrection2 = hydrcorrection;
            end
            clear wetcorrection hydrcorrection

        end
    end

    if sum(no_data)==0
        % note that this is a one way Zenith delay and not a slant delay.
        % Units are in cm

        % saving individual estimates based on the time-stamp
        outfile_wet_before = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZWD_before.xyz'];
        outfile_wet_after = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZWD_after.xyz'];
        outfile_dry_before = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZHD_before.xyz'];
        outfile_dry_after = [weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_ZHD_after.xyz'];

        fidwet_before = fopen(outfile_wet_before,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(wetcorrection1,[],1)]';
        tally = fwrite(fidwet_before,data_write,'double');
        fclose(fidwet_before);
        clear data_write tally %%%SSS 4/16
        fidwet_after = fopen(outfile_wet_after,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(wetcorrection2,[],1)]';
        tally = fwrite(fidwet_after,data_write,'double');
        fclose(fidwet_after);
        clear data_write tally %%%SSS 4/16
        fiddry_before = fopen(outfile_dry_before,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(drycorrection1,[],1)]';
        tally = fwrite(fiddry_before,data_write,'double');
        fclose(fiddry_before);
        clear data_write tally %%%SSS 4/16
        fiddry_after = fopen(outfile_dry_after,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(drycorrection2,[],1)]';
        tally = fwrite(fiddry_after,data_write,'double');
        fclose(fiddry_after);
        clear data_write tally %%%SSS 4/16

        % saving the outputs
        if save_3D_delays==1
            wet = wet1*f_before(d) +  wet2*f_after;
            dry = dry1*f_before(d) +  dry2*f_after;
            save([weather_model_datapath filesep date_before(d,:) filesep date_before(d,:) '_3D.mat'],'dry','wet','hgt','lon','lat','hgt_topo')
            clear wet dry hgt dry1 dry2 wet1 wet2 %%%SSS 4/16
        end

        % Output wet correction
        wetcorrection = wetcorrection1*f_before(d) +  wetcorrection2*f_after(d);
        clear wetcorrection1 wetcorrection2
        wetcorrection = wetcorrection*100;                 % delay in [cm]
        fid = fopen(outfile,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(wetcorrection,[],1)]';
        tally = fwrite(fid,data_write,'double');
        fclose(fid);
        clear data_write tally wetcorrection %%%SSS 4/16

        % Output hydrostatic correction
        hydrcorrection = drycorrection1*f_before(d) +  drycorrection2*f_after(d);
        clear drycorrection1 drycorrection2
        hydrcorrection = hydrcorrection*100;                 % delay in [cm]
        fid = fopen(hydroutfile,'w');
        data_write =  [reshape(xi,[],1) reshape(yi,[],1) reshape(hydrcorrection,[],1)]';
        tally = fwrite(fid,data_write,'double');
        fclose(fid);
        clear data_write tally hydrcorrection %%%SSS 4/16

        fprintf([num2str(d) ' completed out of ' num2str(n_dates) '\n' ])

    else
        fprintf([num2str(d) ' completed out of ' num2str(n_dates) ' (NO DATA)\n' ])
    end
end


function [sicdmeta] = pfa_sicd_meta(meta, nbdata, ifp_params)
%PFA_SICD_META Creates a SICD metadata structure for an image created by pfa_file.m
%
% Inputs:
%     meta:          Collection narrowband data.  This is the same
%                    structure returned by the get_meta function for a
%                    object returned by open_ph_reader.
%     nbdata:        CPHD-style per-pulse narrowband data.  This is the
%                    same structure returned as the second return parameter
%                    of the read_cphd function.
%     ifp_params:    Structure of some additional parameters computed
%                    during image formation.
%
% Outputs:
%     sicdmeta:      Image formation and collection information in a
%                    structure compatible with the SICD writer.
%
% Authors: Wade Schwartzkopf and Tom Krauss, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

% Create SICD meatdata structure
sicdmeta = meta2sicd_cphdx(meta,nbdata,ifp_params.channel);

% Image Creation
sicdmeta.ImageCreation.DateTime = now();
sicdmeta.ImageCreation.Profile  = 'pfa_sicd_meta.m';

% ImageData
sicdmeta.ImageData.PixelType               = 'RE32F_IM32F';
sicdmeta.ImageData.NumRows                 = ifp_params.image_size_pixels(1);
sicdmeta.ImageData.NumCols                 = ifp_params.image_size_pixels(2);
sicdmeta.ImageData.FirstRow                = 0;
sicdmeta.ImageData.FirstCol                = 0;
sicdmeta.ImageData.FullImage.NumRows       = sicdmeta.ImageData.NumRows;
sicdmeta.ImageData.FullImage.NumCols       = sicdmeta.ImageData.NumCols;
sicdmeta.ImageData.SCPPixel.Row            = floor(sicdmeta.ImageData.NumRows/2);
sicdmeta.ImageData.SCPPixel.Col            = floor(sicdmeta.ImageData.NumCols/2);

% GeoData
sicdmeta.GeoData.EarthModel='WGS_84';
center=mean(nbdata.SRPPos); % Current PFA code only forms to this point
sicdmeta.GeoData.SCP.ECF.X=mean(center(1));
sicdmeta.GeoData.SCP.ECF.Y=mean(center(2));
sicdmeta.GeoData.SCP.ECF.Z=mean(center(3));
scp_lla = ecf_to_geodetic([sicdmeta.GeoData.SCP.ECF.X sicdmeta.GeoData.SCP.ECF.Y sicdmeta.GeoData.SCP.ECF.Z]);
sicdmeta.GeoData.SCP.LLH.Lat=scp_lla(1);
sicdmeta.GeoData.SCP.LLH.Lon=scp_lla(2);
sicdmeta.GeoData.SCP.LLH.HAE=scp_lla(3);
% Corner coordinates will be popupated later in derived_sicd_fields call

% Grid
sicdmeta.Grid.ImagePlane     = 'SLANT';
sicdmeta.Grid.Type           = 'RGAZIM';
sicdmeta.Grid.Row.Sgn=-1; % We have done an IFFT in PFA
sicdmeta.Grid.Row.ImpRespBW  = diff(ifp_params.k_v_bounds);
sicdmeta.Grid.Row.ImpRespWid = 0.886/sicdmeta.Grid.Row.ImpRespBW;
sicdmeta.Grid.Row.SS         = 1/(ifp_params.sample_rate*sicdmeta.Grid.Row.ImpRespBW);
sicdmeta.Grid.Row.KCtr       = mean(ifp_params.k_v_bounds);
sicdmeta.Grid.Row.DeltaKCOAPoly = 0;
sicdmeta.Grid.Row.WgtType.WindowName = 'UNIFORM';
sicdmeta.Grid.Col.Sgn=-1; % We have done an IFFT in PFA
sicdmeta.Grid.Col.ImpRespBW  = diff(ifp_params.k_u_bounds);
sicdmeta.Grid.Col.ImpRespWid = 0.886/sicdmeta.Grid.Col.ImpRespBW;
sicdmeta.Grid.Col.SS         = 1/(ifp_params.sample_rate*sicdmeta.Grid.Col.ImpRespBW);
sicdmeta.Grid.Col.KCtr       = 0;
sicdmeta.Grid.Col.WgtType.WindowName = 'UNIFORM';
sicdmeta.Grid.Col.DeltaKCOAPoly = 0;

% ImageFormation
sicdmeta.ImageFormation.RcvChanProc.NumChanProc = 1; % This function only handles single-channel data
sicdmeta.ImageFormation.RcvChanProc.ChanIndex = ifp_params.channel;
sicdmeta.ImageFormation.ImageFormAlgo = 'PFA';
sicdmeta.ImageFormation.TStartProc = nbdata.TxTime(1);
sicdmeta.ImageFormation.TEndProc = nbdata.TxTime(end);
if isfield(meta.Channel,'Parameters') && ...
        isfield(meta.Channel.Parameters(ifp_params.channel),'Polarization') && ...
        all(isfield(meta.Channel.Parameters(ifp_params.channel).Polarization,{'TxPol','RcvPol'}))
    sicdmeta.ImageFormation.TxRcvPolarizationProc = ...
        [meta.Channel.Parameters(ifp_params.channel).Polarization.TxPol ':' ...
            meta.Channel.Parameters(ifp_params.channel).Polarization.RcvPol];
end
sicdmeta.ImageFormation.TxFrequencyProc.MinProc = ...
    min(nbdata.SC0 + (nbdata.SCSS * double(min(ifp_params.sample_range)-1)));
sicdmeta.ImageFormation.TxFrequencyProc.MaxProc = ...
    max(nbdata.SC0 + (nbdata.SCSS * double(max(ifp_params.sample_range)-1)));
sicdmeta.ImageFormation.ImageFormAlgo = 'PFA';
sicdmeta.ImageFormation.STBeamComp = 'NO';
sicdmeta.ImageFormation.ImageBeamComp = 'NO';
sicdmeta.ImageFormation.AzAutofocus = 'NO';
sicdmeta.ImageFormation.RgAutofocus = 'NO';

% SCPCOA
sicdmeta.SCPCOA.SCPTime = nbdata.TxTime(ifp_params.ref_pulse_index);

% Timeline
if isfield(meta,'Global') && isfield(meta.Global, 'Timeline') && ...
        all(isfield(meta.Global.Timeline, {'TxTime1', 'TxTime2'}))
    sicdmeta.Timeline.CollectDuration = meta.Global.Timeline.TxTime2 - ...
        meta.Global.Timeline.TxTime1;
else
    sicdmeta.Timeline.CollectDuration = nbdata.TxTime(end) - nbdata.TxTime(1);
end

% PFA
sicdmeta.PFA.FPN = struct('X',ifp_params.fpn(1),'Y',ifp_params.fpn(2),'Z',ifp_params.fpn(3));
sicdmeta.PFA.IPN = struct('X',ifp_params.ipn(1),'Y',ifp_params.ipn(2),'Z',ifp_params.ipn(3));
sicdmeta.PFA.PolarAngRefTime = nbdata.TxTime(ifp_params.ref_pulse_index);
% This often givens a "badly conditioned" polynomial warning with fitting a
% polynomial in ECF space.  Ignore.
old_state = warning('off','MATLAB:polyfit:RepeatedPointsOrRescale');
sicdmeta.PFA.PolarAngPoly = fliplr( polyfit(nbdata.TxTime, ifp_params.k_a, 5) ).';
sicdmeta.PFA.SpatialFreqSFPoly = fliplr( polyfit(ifp_params.k_a, ifp_params.k_sf, 5) ).';
warning(old_state);
sicdmeta.PFA.Krg1 = ifp_params.k_v_bounds(1);
sicdmeta.PFA.Krg2 = ifp_params.k_v_bounds(2);
sicdmeta.PFA.Kaz1 = ifp_params.k_u_bounds(1);
sicdmeta.PFA.Kaz2 = ifp_params.k_u_bounds(2);

sicdmeta = derived_sicd_fields(sicdmeta); % Computes many derived fields

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


% ==============================================================================
%
%    Software License Agreement (BSD License)
%    Copyright (c) 2019
%    (www.aimlab.wpi.edu)
%
%    All rights reserved.
%
%    Redistribution and use in source and binary forms, with or without
%    modification, are permitted provided that the following conditions
%    are met:
%
%    * Redistributions of source code must retain the above copyright
%    notice, this list of conditions and the following disclaimer.
%
%    * Redistributions in binary form must reproduce the above
%    copyright notice, this list of conditions and the following
%    disclaimer in the documentation and/or other materials provided
%    with the distribution.
%
%    * Neither the name of authors nor the names of its contributors may
%    be used to endorse or promote products derived from this software
%    without specific prior written permission.
%
%    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
%    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
%    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
%    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
%    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
%    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
%    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
%    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
%    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
%    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
%    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
%    POSSIBILITY OF SUCH DAMAGE.
%
%    \author:    <http://www.aimlab.wpi.edu>
%    \author:    <amunawar@wpi.edu>
%    \author:    Adnan Munawar
%    \version:   0.1$
% ==============================================================================

function [its,sizePath,run_time] =  RRTextend3D(dim,segmentLength,random_world,show_output)
% dim = 2;
% radius =0;
% segmentLength = 5;
% random_world = 0;
% n_its = 1000;
% standard length of path segments
if dim ==2
    start_cord = [5,5];
    goal_cord = [95,95];

else

    start_cord = [5,5,5];
    goal_cord = [95,95,95];
end

% create random world
Size = 100;
NumObstacles = 100;

if random_world ==1
    world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);
else
    [world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);
end
% randomly select start and end nodes
%start_node = generateRandomNode(world,dim)
%end_node   = generateRandomNode(world,dim)
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];
% establish tree starting with the start node
tree = start_node;

a = clock;

% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )...
        &&(collision(start_node,end_node,world,dim)==0) )
    path = [start_node; end_node];
else

    its = 0;
    numPaths = 0;
    flag = 0;
    while numPaths < 1,
        [tree,flag] = extendTree(tree,end_node,segmentLength,world,flag,dim);
        numPaths = numPaths + flag;
        its = its+1;
    end

end

% find path with minimum cost to end_node
path = findMinimumPath(tree,end_node,dim);
sizePath = size(path,1);

b = clock;
run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));

if show_output == 1
    figure;
    plotExpandedTree(world,tree,dim);
    plotWorld(world,path,dim);
    % figure(2);
    % plotWorld(world,path,dim);
    %plotExpandedTree(world,tree,dim);
end
end

function world = createWorld(NumObstacles, endcorner, origincorner,dim)

if dim == 2

    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2));
        maxRadius = 5*maxRadius/NumObstacles/2;
        for i=1:NumObstacles,
            % randomly pick radius
            world.radius(i) = maxRadius*rand;
            % randomly pick center of obstacles
            cx = origincorner(1) + world.radius(i)...
                + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;
            cy = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            world.cx(i) = cx;
            world.cy(i) = cy;
        end
    end

elseif dim ==3;
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)];
        maxRadius = min(bounds);
        maxRadius = 5*maxRadius/NumObstacles;
        for i=1:NumObstacles,
            % randomly pick radius
            world.radius(i) = maxRadius*rand;
            % randomly pick center of obstacles
            cx = origincorner(1) + world.radius(i)...
                + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;
            cy = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            cz = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            world.cx(i) = cx;
            world.cy(i) = cy;
            world.cz(i) = cz;
        end
    end
end
end

function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;
if dim == 2
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),
        disp('Not valid corner specifications!')
        world=[];
        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = 10;

        world.radius(1) = maxRadius;
        cx = 50;
        cy = 50;
        world.cx(1) = cx;
        world.cy(1) = cy;

        world.radius(2) = maxRadius;
        cx = 75;
        cy = 25;
        world.cx(2) = cx;
        world.cy(2) = cy;

        world.radius(3) = maxRadius;
        cx = 25;
        cy = 75;
        world.cx(3) = cx;
        world.cy(3) = cy;

        world.radius(4) = maxRadius;
        cx = 25;
        cy = 25;
        world.cx(4) = cx;
        world.cy(4) = cy;

        world.radius(5) = maxRadius;
        cx = 75;
        cy = 75;
        world.cx(5) = cx;
        world.cy(5) = cy;
    end

elseif dim == 3

    NumObstacles = 9;
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = 10;

        world.radius(1) = maxRadius;
        cx = 50;
        cy = 50;
        cz = 50;
        world.cx(1) = cx;
        world.cy(1) = cy;
        world.cz(1) = cz;

        world.radius(2) = maxRadius;
        cx = 25;
        cy = 25;
        cz = 25;
        world.cx(2) = cx;
        world.cy(2) = cy;
        world.cz(2) = cz;

        world.radius(3) = maxRadius;
        cx = 75;
        cy = 75;
        cz = 75;
        world.cx(3) = cx;
        world.cy(3) = cy;
        world.cz(3) = cz;

        world.radius(4) = maxRadius;
        cx = 25;
        cy = 25;
        cz = 75;
        world.cx(4) = cx;
        world.cy(4) = cy;
        world.cz(4) = cz;

        world.radius(5) = maxRadius;
        cx = 75;
        cy = 75;
        cz = 25;
        world.cx(5) = cx;
        world.cy(5) = cy;
        world.cz(5) = cz;

        world.radius(6) = maxRadius;
        cx = 25;
        cy = 75;
        cz = 25;
        world.cx(6) = cx;
        world.cy(6) = cy;
        world.cz(6) = cz;

        world.radius(7) = maxRadius;
        cx = 75;
        cy = 25;
        cz = 25;
        world.cx(7) = cx;
        world.cy(7) = cy;
        world.cz(7) = cz;

        world.radius(8) = maxRadius;
        cx = 75;
        cy = 25;
        cz = 75;
        world.cx(8) = cx;
        world.cy(8) = cy;
        world.cz(8) = cz;

        world.radius(9) = maxRadius;
        cx = 25;
        cy = 75;
        cz = 75;
        world.cx(9) = cx;
        world.cy(9) = cy;
        world.cz(9) = cz;
    end
end
end

function node=generateRandomNode(world,dim)

if dim ==2;
    % randomly pick configuration
    px       = (world.endcorner(1)-world.origincorner(1))*rand;
    py       = (world.endcorner(2)-world.origincorner(2))*rand;

    chi      = 0;
    cost     = 0;
    node     = [px, py, chi, cost, 0];

    % check collision with obstacle
    while collision(node, node, world,dim),
        px       = (world.endcorner(1)-world.origincorner(1))*rand;
        py       = (world.endcorner(2)-world.origincorner(2))*rand;

        chi      = 0;
        cost     = 0;
        node     = [px, py, chi, cost, 0];
    end

elseif dim ==3;
    % randomly pick configuration
    px       = (world.endcorner(1)-world.origincorner(1))*rand;
    py       = (world.endcorner(2)-world.origincorner(2))*rand;
    pz       = (world.endcorner(3)-world.origincorner(3))*rand;

    chi      = 0;
    cost     = 0;
    node     = [px, py, pz, chi, cost, 0];

    % check collision with obstacle
    while collision(node, node, world,dim),
        px       = (world.endcorner(1)-world.origincorner(1))*rand;
        py       = (world.endcorner(2)-world.origincorner(2))*rand;
        pz       = (world.endcorner(3)-world.origincorner(3))*rand;

        chi      = 0;
        cost     = 0;
        node     = [px, py, pz, chi, cost, 0];
    end

end

end

function collision_flag = collision(node, parent, world,dim)

collision_flag = 0;

for i=1:dim
    if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))
        collision_flag = 1;
    end
end

if collision_flag == 0 && dim ==2
    for sigma = 0:.2:1,
        p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
        % check each obstacle
        for i=1:world.NumObstacles,
            if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),
                collision_flag = 1;
                break;
            end
        end
    end

elseif collision_flag == 0 && dim ==3
    for sigma = 0:.2:1,
        p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
        % check each obstacle
        for i=1:world.NumObstacles,
            if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),
                collision_flag = 1;
                break;
            end
        end
    end
end
end

function flag = canEndConnectToTree(tree,end_node,minDist,world,dim)
flag = 0;
% check only last node added to tree since others have been checked
if ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)...
        & (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ),
    flag = 1;
end

end

function [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,flag_chk,dim)
r = 0;
flag1 = 0;
while flag1==0,
    % select a random point
    randomPoint = ones(1,dim);
    for i=1:dim
        randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;
    end

    % find leaf on node that is closest to randomPoint
    tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;
    sqrd_dist = sqr_eucl_dist(tmp,dim);
    [min_dist,idx] = min(sqrd_dist);
    min_parent_idx = idx;

    new_point = (randomPoint-tree(idx,1:dim));
    new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength;

    min_cost  = cost_np(tree(idx,:),new_point,dim);
    new_node  = [new_point, 0, min_cost, idx];

    if collision(new_node, tree(idx,:), world,dim)==0

        tmp_dist = tree(:,1:dim)-(ones(size(tree,1),1)*new_point);
        dist = sqr_eucl_dist(tmp_dist,dim);
        near_idx = find(dist <= r^2);

        if size(near_idx,1)>1
            size_near = size(near_idx,1);

            for i = 1:size_near
                if collision(new_node, tree(near_idx(i),:), world,dim)==0

                    cost_near = tree(near_idx(i),dim+2)+line_cost(tree(near_idx(i),:),new_point,dim);

                    if  cost_near < min_cost
                        min_cost = cost_near;
                        min_parent_idx = near_idx(i);
                    end

                end
            end
        end

        new_node = [new_point, 0 , min_cost, min_parent_idx];
        new_tree = [tree; new_node];
        new_node_idx = size(new_tree,1);

        if size(near_idx,1)>1
            reduced_idx = near_idx;
            for j = 1:size(reduced_idx,1)
                near_cost = new_tree(reduced_idx(j),dim+2);
                lcost = line_cost(new_tree(reduced_idx(j),:),new_point,dim);
                if near_cost > min_cost + lcost ...
                        && collision(new_tree(reduced_idx(j),:),new_node,world,dim)
                    before = new_tree(reduced_idx(j),dim+3)
                    new_tree(reduced_idx(j),dim+3) = new_node_idx;
                    after = new_tree(reduced_idx(j),dim+3)
                end

            end
        end
        flag1=1;
    end
end

if flag_chk == 0
    % check to see if new node connects directly to end_node
    if ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )...
            && (collision(new_node,end_node,world,dim)==0) )
        flag = 1;
        new_tree(end,dim+1)=1;  % mark node as connecting to end.
    else
        flag = 0;
    end

else flag = 1;
end
end

function e_dist = sqr_eucl_dist(array,dim)

sqr_e_dist = zeros(size(array,1),dim);
for i=1:dim

    sqr_e_dist(:,i) = array(:,i).*array(:,i);

end
e_dist = zeros(size(array,1),1);
for i=1:dim

    e_dist = e_dist+sqr_e_dist(:,i);

end

end

%calculate the cost from a node to a point
function [cost] = cost_np(from_node,to_point,dim)

diff = from_node(:,1:dim) - to_point;
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;

end

%calculate the cost from a node to a node
function [cost] = cost_nn(from_node,to_node,dim)

diff = from_node(:,1:dim) - to_node(:,1:dim);
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;

end

function [cost] = line_cost(from_node,to_point,dim)
diff = from_node(:,1:dim) - to_point;
cost = norm(diff);
end

function path = findMinimumPath(tree,end_node,dim)

% find nodes that connect to end_node
connectingNodes = [];
for i=1:size(tree,1),
    if tree(i,dim+1)==1,
        connectingNodes = [connectingNodes ; tree(i,:)];
    end
end

% find minimum cost last node
[tmp,idx] = min(connectingNodes(:,dim+2));

% construct lowest cost path
path = [connectingNodes(idx,:); end_node];
parent_node = connectingNodes(idx,dim+3);
while parent_node>1,
    parent_node = tree(parent_node,dim+3);
    path = [tree(parent_node,:); path];
end

end

function plotExpandedTree(world,tree,dim)
ind = size(tree,1);
while ind>0
    branch = [];
    node = tree(ind,:);
    branch = [ branch ; node ];
    parent_node = node(dim+3);
    while parent_node > 1
        cur_parent = parent_node;
        branch = [branch; tree(parent_node,:)];
        parent_node = tree(parent_node,dim+3);
    end
    ind = ind - 1;

    if dim == 2
        X = branch(:,1);
        Y = branch(:,2);

        p = plot(X,Y);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;

    elseif dim == 3
        X = branch(:,1);
        Y = branch(:,2);
        Z = branch(:,3);

        p = plot3(X,Y,Z);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;
    end
end
end

function plotWorld(world,path,dim)
% the first element is the north coordinate
% the second element is the south coordinate
if dim ==2

    N = 10;
    th = 0:2*pi/N:2*pi;
    axis([world.origincorner(1),world.endcorner(1),...
        world.origincorner(2), world.endcorner(2)]);
    hold on

    for i=1:world.NumObstacles,
        X = world.radius(i)*sin(th) + world.cx(i);
        Y = world.radius(i)*cos(th) + world.cy(i);
        fill(X,Y,'blue');
    end

    X = path(:,1);
    Y = path(:,2);
    p = plot(X,Y);

elseif dim ==3
    axis([world.origincorner(1),world.endcorner(1),...
        world.origincorner(2), world.endcorner(2),...
        world.origincorner(3), world.endcorner(3)]);
    hold on

    for i=1:world.NumObstacles,
        [X Y Z] = sphere(10);
        X = (X*world.radius(i));
        Y = (Y*world.radius(i));
        Z = (Z*world.radius(i));
        surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i));
        colormap([0.5 0.2 0.3]);
    end

    X = path(:,1);
    Y = path(:,2);
    Z = path(:,3);
    p = plot3(X,Y,Z);
end
set(p,'Color','black','LineWidth',3)
xlabel('X axis');
ylabel('Y axis');
zlabel('Z axis');
title('RRT Extend Algorithm');
end


%This Matlab script can be used to reproduce Figure 7.18 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%% Define parameters
M_max = 15;          % number of antenna columns/rows
d_H = 1/2;          % horizontal antenna spacing
h_BS = 25;          % height of the antenna array
h_UE = 1.5;         % height of the UE
CellRadius = 500;   % cell radius
K = 500;            % number of UEs
fc = 2.6e9;         % center frequency
bw = 20e6;          % bandwidth
N = 1024;           % number of subcarriers

% Compute derived parameters
h = h_BS - h_UE;    % effective height
d_V = 1/2;          % vertical antenna spacing
d = d_V;

%Simulate new channel reponses or load existing results (the latter is the
%preset)
simulate = false;

%% Simulate channels
if (simulate)
    % Draw random positions
    A = rand(1,K)*CellRadius^2;
    B = rand(1,K)*pi*2/3 - pi/3;
    POS = repmat(sqrt(A)',[1,2]).* [cos(B); sin(B)]'; %UE positions uniformly distributed on a disc of radius R

    % Create a new layout
    s = simulation_parameters; % Create object with general parameters
    s.center_frequency = fc;
    s.sample_density = 1;
    s.use_absolute_delays = 1;

    l = layout(s);  % Create new layout from general parameters
    l.no_rx = K;  % Generate KUEs
    l.rx_array.generate('omni'); % use omnidirectional antennas at the UEs

    l.randomize_rx_positions(CellRadius, h_UE, h_UE, 1); % randomly distribute UEs within a disc of radius R and height 1.5m
    for k=1:K
        l.rx_position(:,k) = [POS(k,1); POS(k,2);  h_UE]; % use the same positions as for the other simulations
    end

    for i=1:l.no_rx % for each receiver
        l.track(i).generate('linear',0,0) % define a linear track consiting of only one position
        l.track(i).scenario = '3GPP_3D_UMa_LOS'; % select the Urban Macrocell NLOS scenario for the link
    end

    % Simulate channel for planar array
    l.tx_array.generate('3gpp-3d', 1, M_max, M_max, fc, 1, 0, d);
    [h_channel, h_parset, h_cb] = l.get_channels();
    h_freq = h_channel.fr(bw,N);
    H_plan = zeros(K,M_max^2, N);
    for k=1:K
        H_plan(k,:,:) = squeeze(h_freq{k});
    end

    % Simulate channel for horizontal array
    l.tx_array.generate('3gpp-3d', 1, 1, M_max^2, fc, 1, 0, d);
    [h_channel, h_parset, h_cb] = l.get_channels();
    h_freq = h_channel.fr(bw,N);
    H_hori = zeros(K,M_max^2, N);
    for k=1:K
        H_hori(k,:,:) = squeeze(h_freq{k});
    end

    % Simulate channel for vertical array
    l.tx_array.generate('3gpp-3d', 1, M_max^2, 1, fc, 1, 0, d);
    [h_channel, h_parset, h_cb] = l.get_channels();
    h_freq = h_channel.fr(bw,N);
    H_vert = zeros(K,M_max^2, N);
    for k=1:K
        H_vert(k,:,:) = squeeze(h_freq{k});
    end
end

%% Compute channel orthogonality
if (~simulate)
    load('section7_orthogonality_3GPP.mat')
else

    IT = 100000;
    COR_3GPP_PLAN = zeros(1,M_max);
    COR_3GPP_HORI = zeros(1,M_max);
    COR_3GPP_VERT = zeros(1,M_max);

    for it=1:IT

        %Output simulation progress
        disp([num2str(it) ' iterations out of ' num2str(IT)]);

        % Draw random position indices
        posInd = randperm(K,2);

        % For all antenna size
        for m = 1:M_max
            M_H = m;
            M_V = m;
            M = M_H*M_V;
            indices = repmat((0:M_H-1)*M_max + 1, [M_V,1]) + repmat((0:M_V-1)', [1,M_H]);
            indices = indices(:);
            for n=1:N
                % Planar array
                h1 = H_plan(posInd(1),indices,n);
                h2 = H_plan(posInd(2),indices,n);
                COR_3GPP_PLAN(m) = abs(h1*h2')^2 / (norm(h1)^2*norm(h2)^2)/IT/N + COR_3GPP_PLAN(m);

                % Horizontal array
                h1 = H_hori(posInd(1),1:m^2,n);
                h2 = H_hori(posInd(2),1:m^2,n);
                COR_3GPP_HORI(m) = abs(h1*h2')^2 / (norm(h1)^2*norm(h2)^2)/IT/N + COR_3GPP_HORI(m);

                % Vertical array
                h1 = H_vert(posInd(1),1:m^2,n);
                h2 = H_vert(posInd(2),1:m^2,n);
                COR_3GPP_VERT(m) = abs(h1*h2')^2 / (norm(h1)^2*norm(h2)^2)/IT/N + COR_3GPP_VERT(m);
            end
        end
    end
end

%% Load channel measurements
hor_out=load('section7_measurement_horizontal.mat');
vert_out=load('section7_measurement_vertical.mat');
planar_square = load('section7_measurement_planar.mat');

x_measured=(1:8).^2;

%Compute average UE correlation
COR_MEAS_HORI = mean(hor_out.crossco).^2;
COR_MEAS_PLAN = mean(planar_square.crossco).^2;
COR_MEAS_VERT = mean(vert_out.crossco).^2;

%% Plot the simulation results
figure;
hold on; box on;

xvals = (1:M_max).^2;

plot(xvals, COR_3GPP_VERT, 'LineWidth', 1, 'Color', 'k', 'LineStyle', '-', 'Marker', 'x');
plot(x_measured, COR_MEAS_VERT(x_measured), 'LineWidth', 1, 'Color', 'k', 'LineStyle', '--', 'Marker', 'x');

plot(xvals, COR_3GPP_PLAN, 'LineWidth', 1, 'Color', 'b', 'LineStyle', '-', 'Marker', 'o');
plot(x_measured, COR_MEAS_PLAN, 'LineWidth', 1, 'Color', 'b', 'LineStyle', '--', 'Marker', 'o');

plot(x_measured, COR_MEAS_HORI(x_measured), 'LineWidth', 1, 'Color', 'r', 'LineStyle', '--','Marker', 'square');
plot(xvals, COR_3GPP_HORI, 'LineWidth', 1, 'Color', 'r', 'LineStyle', '-','Marker', 'square');

COR_IID = 1./xvals;
plot(xvals, COR_IID, 'LineWidth', 1, 'Color', 'k', 'LineStyle', '-.');

legend({'3GPP Vertical', 'Meas. Vertical', '3GPP Planar', 'Meas. Planar', 'Meas. Horizontal', '3GPP Horizontal', 'i.i.d.'}, 'location', 'southwest');
set(gca, 'XScale', 'log', 'YScale', 'log');
xlabel('Number of antennas (M)');
ylabel('Average UE correlation');
set(gca,'xtick',[1,4,9,16,25,36,49,64,121,225]);
xlim([1,M_max^2]);
ylim([COR_IID(end), 1]);