function [struct_irf_record D_record gamma_record hd_record ETA_record IVcorrelation PofIVcorrelation beta_gibbs_reshuffle sigma_gibbs_reshuffle]=irfres_zeros_magn_correl_fevd_bayesian_stvol4(beta_gibbs, sigma_gibbs, It, Bu,betahat,sigmahat,IRFperiods,n,m,p,k,T,signrestable,signresperiods, relmagnrestable, relmagnresperiods, names, startdate, enddate, ShockwithInstrument, Ycycle, Xcycle, signreslabels, FEVDresperiods, FEVDrestable, data_exo, HD, const, exo, InstrumentforCorrel, IRFt, YincLags, Psi_gibbs, pref)
%betahat = betahatcycle                                                                                irfres_zeros_magn_correl_fevd_bayesian_stvol4(beta_gibbs, sigma_gibbs, It, Bu,betahatcycle,sigmahatcycle,IRFperiods,n,m,p,k,T,signrestable,signresperiods, relmagnrestable, relmagnresperiods, namespostratining, startdateposttraining, enddate, strctident.ShockwithInstrument, Ycycle, Xcycle, signreslabels, FEVDresperiods, FEVDrestable,data_exo, HD, 0, exo, strctident.InstrumentforCorrel, IRFt, YincLags);
%sigmahat = sigmahatcycle
%names =namespostraining
%startdate = startdateposttraining
%ShockwithInstrument =  strctident.ShockwithInstrument
%InstrumentforCorrel = strctident.InstrumentforCorrel

% inputs:  - matrix 'betahat': OLS estimate for beta
%          - matrix 'sigmahats': OLS estimate for sigma
%          - integer 'IRFperiods': number of periods for IRFs
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'm': number of exogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - cell 'signrestable': table recording the sign restriction input from the user
%          - cell 'signresperiods': table containing the periods corresponding to each restriction
%          - string 'ShockwithInstrument' Name of the shock where the instrument belongs to
% outputs: - cell 'struct_irf_record': record of the gibbs sampler draws for the orthogonalised IRFs
%          - matrix 'D_record': record of the gibbs sampler draws for the structural matrix D
%          - matrix 'gamma_record': record of the gibbs sampler draws for the structural disturbances variance-covariance matrix gamma
%          - integer 'Qdraw': total number of draws of the Q matrix
%          - integer 'Qsuccess': number of successful draws of the Q matrix

tic

%%Phase 1: Preliminary tasks

%% draw from VAR posterior
%sample beta and sigma from the VAR distribution centered around the OLS estimate
inv_sigma_hat = inv(sigmahat); %invert sigmahat as it is frequently used afterwards
Acc=It-Bu; %%number of minimum draws accepted

%% Preliminiaries for sign restrictions
% now identify all the periods concerned with restrictions
% first expand the non-empty entries in signresperiods since they are only expressed in intervals: transform into list
% for instance, translate [1 4] into [1 2 3 4]; I don't think this can done without a loop
temp=cell2mat(signresperiods(~cellfun(@isempty,signresperiods)));
periods=[];
for ii=1:size(temp,1)
    periods=[periods temp(ii,1):temp(ii,2)];
end
% suppress duplicates and sort
periods=sort(unique(periods))';
% count the total number of restriction periods (required for IRF matrix)
nperiods=size(periods,1);

% Identify the restriction matrices
% create five cells, corresponding to the three possible restrictions:
% one cell for sign restrictions, three cells for magnitude restrictions, one cell for zero restrictions
Scell=cell(1,n);
Mcell=cell(1,n);
Mlcell=cell(1,n);
Mucell=cell(1,n);
Zcell=cell(1,n);

% Check if value and periods restrictions correspond to each other
if sum(sum(~cellfun(@isempty,signresperiods) == ~cellfun(@isempty,signrestable))) == n^2
    % All cells with sign restrictions also specify the horizon over which
    % these are applied
else
    disp('Warning: Value restrictions do not correspond to period restrictions one to one')
    pause(1)
end

% loop over rows and columns of the period matrix
for ii=1:n
    for jj=1:n
        % if entry (ii,jj) of the period matrix and of the value matrix is not empty...
        if ~isempty(signresperiods{ii,jj}) && ~isempty(signrestable{ii,jj})
            % ... then there is a restriction over one (or several) periods
            % loop overt those periods
            for kk=signresperiods{ii,jj}(1,1):signresperiods{ii,jj}(1,2)
                % identify the position of the considered period within the list of all periods (required to build the matrix)
                position=find(periods==kk);
                % now create the restriction matrix: this will depend on the type of restriction
                % if it is a positive sign restriction...
                if strcmp(signrestable{ii,jj},'+')
                    % ... then input a 1 entry in the corresponding S matrix
                    Scell{1,jj}=[Scell{1,jj};zeros(1,n*nperiods)];
                    Scell{1,jj}(end,(position-1)*n+ii)=1;
                    % if it is a negative sign restriction...
                elseif strcmp(signrestable{ii,jj},'-')
                    % ... then input a -1 entry in the corresponding S matrix
                    Scell{1,jj}=[Scell{1,jj};zeros(1,n*nperiods)];
                    Scell{1,jj}(end,(position-1)*n+ii)=-1;
                    % if it is a zero restriction...
                elseif strcmp(signrestable{ii,jj},'0')
                    % ... then input a 1 entry in the corresponding Z matrix
                    Zcell{1,jj}=[Zcell{1,jj};zeros(1,n*nperiods)];
                    Zcell{1,jj}(end,(position-1)*n+ii)=1;
                    % else, a non-empty entry being neither a sign nor a zero restriction has to be a magnitude restriction
                else
                    % fill the corresponding M matrices:
                    % input a 1 in M
                    Mcell{1,jj}=[Mcell{1,jj};zeros(1,n*nperiods)];
                    Mcell{1,jj}(end,(position-1)*n+ii)=1;
                    % input the lower value of the interval in Ml
                    temp=str2num(signrestable{ii,jj});
                    Mlcell{1,jj}=[Mlcell{1,jj};temp(1,1)];
                    % input the upper value of the interval in Mu
                    Mucell{1,jj}=[Mucell{1,jj};temp(1,2)];
                end
            end
        end
    end
end

%% Preliminaries for relative magnitude restrictions
% now identify all the periods concerned with relative magnitude
% restrictions
% first expand the non-empty entries in magresperiods since they are only expressed in intervals: transform into list
% for instance, translate [1 4] into [1 2 3 4];
temp=cell2mat(relmagnresperiods(~cellfun(@isempty,relmagnresperiods)));
mperiods=[];
for ii=1:size(temp,1)
    mperiods=[mperiods temp(ii,1):temp(ii,2)];
end
% suppress duplicates and sort
mperiods=sort(unique(mperiods))';
% count the total number of restriction periods (required for IRF matrix)
rmperiods=size(periods,1);

%create matrix entry for relative magnitude restrictions
[r clm] = find(~cellfun('isempty',relmagnrestable));
%2. Indentify which entry corresponds to the positive magnitude
%restriction (which shock is supposed to have a larger impact on which
%variable)
num_magres=length(r)/2; %number of relative magnitude restrictions
IndextempL=double.empty;
kk=1; %number of the restrictions
IndextempS=double.empty;
kk=1; %%number of restriction

rowsS = [];
columnsS = [];
for jj=1:num_magres %%loop over number of magnitude restrictions
    strtemp = strcat('S',num2str(jj)); %%find entry in the table corresponding to the Stronger than restriction
    Stronger = strcmp(relmagnrestable, strtemp);
    [rowS columnS] = find(Stronger==1);
    rowsS = [rowsS rowS];
    columnsS = [columnsS columnS];
end

rowsW = [];
columnsW = [];
for jj=1:num_magres
    strtemp = strcat('W',num2str(jj));
    Weaker = strcmp(relmagnrestable, strtemp);
    [rowW columnW] = find(Weaker==1);
    rowsW = [rowsW rowW];
    columnsW = [columnsW columnW];
end

%% Preliminiaries for IV correlation restrictions
%%load the instrument
ShockwithIV = find(contains(signreslabels,ShockwithInstrument)); %find index for the Shock with correlation restriction

% check for correlation restrictions
if isempty(ShockwithIV)%when there is no shock with an extra instrument
    IVcorrelcheck=0; %there is noting to check
    IVcorrelation=nan(Acc,1); %empty vector such that the paralel loop doesnt crash
    IVcorrel=nan(T,1); %empty vector such that the paralel loop doesnt crash
    PofIVcorrelation=nan(Acc,1); %empty vector such that the paralel loop doesnt crash
    ETA_record=nan(Acc,1); %empty vector such that the paralel loop doesnt crash
    IVcorrel='noexist'; %empty vector such that the paralel loop doesnt crash
    OverlapIVcorrelinY='noexist';%empty vector such that the paralel loop doesnt crash
    Flipcorrel=0; %there is nothing to flip
else
    IVcorrelcheck=1;
end
if IVcorrelcheck==1
    [IVcorrel txtcorrel]=xlsread(pref.excelFile,'IV');

    Index = strcmp(txtcorrel(1,:), InstrumentforCorrel);           %find the instrument in the IV sheet
    IVnum = find(Index==1, 1, 'first')-1;
    IVcorrel = IVcorrel(:, IVnum);
    IVcorrel = IVcorrel(~isnan(IVcorrel));
    txtcorrel = txtcorrel(2:length(IVcorrel)+1,1);              % drop IV names from txt
    date = names(2:end,1);                                   %get the datevector of the VAR
    startlocationY_in_Y=find(strcmp(date,startdate));        %location of sample startdate in Y datevector
    endlocationY_in_Y=find(strcmp(date,enddate));            %location of sample enddate in Y datevector
    date = date(startlocationY_in_Y+p:endlocationY_in_Y,:);  %cut datevector of Y such that it corresponds to the time dates used in the VAR
    OverlapIVcorrelinY = ismember(date,txtcorrel);           %Use this to cut EPS
    OverlapYinIVcorrel = ismember(txtcorrel,date);           %Use this to cut IV
    IVcorrel = IVcorrel(OverlapYinIVcorrel,:);               %cut all the entries from IV that are not in the sample
end

IsNotIdentified = find(contains(signreslabels,'shock'));        %find not identified shock
IsIdentified = find(~contains(signreslabels,'shock'));    %
if min(IsNotIdentified)==0
    identified=n;
else
    identified=min(IsNotIdentified)-1;                              %number of identified shocks
end

%finally check if there are sign restrictions on the shock of interest. If
%not we can use the flipped entry of the rotation matrix aswell
if ~isempty(ShockwithIV)
    if isempty(Scell{1,ShockwithIV})
        FlipCorrel=1;
    else
        FlipCorrel=0;
    end
end

%% Preliminaries for FEVD restriction
% now identify all the periods concerned with FEVD restrictions
% first expand the non-empty entries in FEVDresperiods since they are only expressed in intervals: transform into list
% for instance, translate [1 4] into [1 2 3 4];
temp=cell2mat(FEVDresperiods(~cellfun(@isempty,FEVDresperiods)));
FEVDperiods=[];
for ii=1:size(temp,1)
    FEVDperiods=[FEVDperiods temp(ii,1):temp(ii,2)];
end
% suppress duplicates and sort
FEVDperiods=sort(unique(FEVDperiods))';
% count the total number of restriction periods (required for IRF matrix)
FEVDperiodstot=size(FEVDperiods,1);

%create matrix entry for relative magnitude restrictions
[rowsFEVD clmFEVD] = find(~cellfun('isempty',FEVDrestable));
num_FEVDres=length(rowsFEVD); %number of FEVD restrictions

%check if rows are unique. Two FEVD restrictions for one variable are not
%included in the algorithm. Two columns (shocks) are fine.
uniquerows = unique(rowsFEVD);

if length(uniquerows) < length(rowsFEVD)
    error('Two FEVD restrictions for one variable are not permitted')
end

%now identify if the FEVD restrictions are absolute ones or relative ones
numrelativeFEVD =0;
numabsoluteFEVD =0;
for kk=1:num_FEVDres
    if strcmp(FEVDrestable{rowsFEVD(kk,1),clmFEVD(kk,1)},'Relative')==1
        numrelativeFEVD=numrelativeFEVD+1;
        rowrelativeFEVD(kk,1)=rowsFEVD(kk,1); %rows are the variables for the FEVD
        clmrelativeFEVD(kk,1)=clmFEVD(kk,1);%Columns are the variables for the FEVD
    elseif strcmp(FEVDrestable{rowsFEVD(kk,1),clmFEVD(kk,1)},'Absolute')==1
        numabsoluteFEVD=numabsoluteFEVD+1;
        rowabsoluteFEVD(kk,1)=rowsFEVD(kk,1);%rows are the variables for the FEVD
        clmabsoluteFEVD(kk,1)=clmFEVD(kk,1); %Columns are the variables for the FEVD
    end
end

%if no restriction of one kind exist,set empty cells
if numabsoluteFEVD ==0
    rowabsoluteFEVD = [];
    clmabsoluteFEVD = [];
end

if numrelativeFEVD ==0
    rowrelativeFEVD = [];
    clmrelativeFEVD = [];
end

%% preliminaries for historical decomposition
contributors = n + 1 + 1 + length(exo); %variables + constant + exogenous + initial conditions
hd_estimates2=cell(contributors+2,n); %shocks+constant+initial values+exogenous+unexplained+to be explained by shocks only

%% Check kind of restrictions
% now check what kind of restrictions apply among sign, zero and magnitude restrictions
% check for sign restrictions: if there are any, at least one entry in the cell Scell is non-empty
if sum(~cellfun(@isempty,Scell))~=0
    signres=1;
else
    signres=0;
end
% similarly check for zero restrictions
if sum(~cellfun(@isempty,Zcell))~=0
    zerores=1;
else
    zerores=0;
end
% check for absolute magnitude restrictions
if sum(~cellfun(@isempty,Mcell))~=0
    magnres=1;
else
    magnres=0;
end
% check for relative magnitude restrictions
if length(columnsS)~=0
    relmagnres=1;
else
    relmagnres=0;
end
% check for correlation restrictions
if isempty(ShockwithIV)%when there is no shock with an extra instrument
    IVcorrelcheck=0;
else
    IVcorrelcheck=1;
end
% check for FEVD restrictions
if numabsoluteFEVD ==0 && numrelativeFEVD==0 %when there are no absolute and no relative FEVD restrictions
    FEVDcheck=0;
else
    FEVDcheck=0;
end

%%Activate the restriction, that unidentified shocks should not have the
%%same pattern as identified
patterncheck=0;
%%activate the possibility of absolute relative magnitude restriction
%%(i.e. credit spreads should rise by more than interest rates fall)
%%--> abs(credit spread) > abs(interes rate) instead of
%%--> credit spread>interest rate
ABS=0;

%% Storage cells
% create first the cell that will store the results from the simulations
struct_irf_record=cell(n,n);
% storage cell
storage1=cell(Acc,1);
storage2=cell(Acc,1);
storage3=cell(Acc,1);
storage4=cell(Acc,1);
In= eye(n);

% initiate rotation draws
not_successful = 0;
hbar = bear.parfor_progressbar(Acc,'Progress of Sign, Magnitude, Zero, Correlation and FEVD Restriction Draws');  %create the progress bar

%rearrange Psidraw such that it can be accessed in a parfor loop
for yyy=1:It-Bu
    for kkk=1:n
Psi_gibbs_new(:,kkk,yyy) = Psi_gibbs{1,kkk}(:,yyy);
    end
end

parfor ii=1:Acc
    % initiate the variable 'success'; this variable will be used to check whether the restrictions are satisfied
    % if there are only zero restrictions, they will be satisfied by construction, and 'success' will simply be ignored
    success=0;
    % how the algorithm will be conducted will depend on the types of restrictions implemented
    % if there are only zero restrictions, the algorithm is simple as no checking is required: the conditions are satisfied by construction
   if zerores==1 && signres==0 && magnres==0 && relmagnres==0
   % draw beta and sigma
   beta=beta_gibbs(:,ii);
   sigma=reshape(sigma_gibbs(:,ii),n,n);
   hsigma=chol(bear.nspd(sigma),'lower');
   % obtain orthogonalised IRFs
   [~, ortirfmatrix]=bear.irfsim(beta,hsigma,n,m,p,k,max(IRFperiods,max(periods)));
   % generate the stacked IRF matrix
   stackedirfmat=[];
      for kk=1:numel(periods)
      stackedirfmat=[stackedirfmat;ortirfmatrix(:,:,periods(kk,1)+1)];
      end
   % draw an entire random matrix Q satisfying the zero restrictions
   [Q]=bear.qzerores(n,Zcell,stackedirfmat);
   % there is no need to verify the restrictions: there are satisfied by construction

   % if there are sign/magnitude/correlation restrictions, possibly associated with zero restrictions
   else
        % the algorithm becomes a bit more complicated as conditions now need to be checked
        % to maintain efficiency, the algorithm proceeds recursively shock by shock, and stops as soon as a condition on the considered shock fails
        % repeat algorithm for the iteration as long as not all conditions are satisfied
        while success==0
            not_successful = not_successful+1;
            % switch 'success' to 1; it will be turned back to zero if at any time Q is detected as a candidate not satisfying the restrictions
            success=1;
            % draw randomly the vector of VAR coefficients: draw a random index
            index=floor(rand*(Acc))+1;
            % then draw a random set of beta and sigma corresponding to this index (this is done to make it possible to draw, if required, an infinite number of values from the gibbs sampler record, with equal probability on each value)
            beta=beta_gibbs(:,index);
            sigma=reshape(sigma_gibbs(:,index),n,n);
            hsigma=chol(bear.nspd(sigma),'lower');
            %also draw the corresponding local mean
            Psidraw = Psi_gibbs_new(:,:,index)
            %create the vector Ydraw by subtracting the local mean from the data
            Ypsi = YincLags(p+1:end,:)-Psidraw(p+1:end,:);
            %ultimately create the RHS and LHS of the demeaned data VAR
            temp=bear.lagx(Ypsi,p);
            % to build X, take off the n initial columns of current data
            Xdraw=[temp(:,n+1:end)];
            Ydraw=temp(:,1:n);
            % obtain orthogonalised IRFs
            [~, ortirfmatrix]=bear.irfsim(beta,hsigma,n,m,p,k,max(IRFperiods,max(periods)));
            % generate the stacked IRF matrix
            stackedirfmat=[];
            for kk=1:numel(periods)
                stackedirfmat=[stackedirfmat;ortirfmatrix(:,:,periods(kk,1)+1)];
            end

            % now start looping over the shocks and checking sequentially whether conditions on these shocks hold
            % stop as soon as one restriction fails
            okay = zeros(n,1); %initiate okay vector
            Qjstore = [];
            % initiate Qj
            Qj=[];
            jj=1;
            while success==1 && jj<=n && sum(okay)<n
                    % draw a random vector from the standard normal
                    if okay(1,1)==0 %first find the first column
                        qj=bear.qrandj(n,Zcell{1,jj},stackedirfmat,Qj);
                        % obtain the candidate column fj
                        fj=stackedirfmat*qj;
                        [success qj]=bear.checksignres(Scell{1,jj},qj,fj);
                        if success==1
                            Qjstore=[Qj qj];
                            Qj(:,1) = qj; %set column yy of Qj to qj
                            jj=1+1;
                            okay(1,1)=1;
                        end
                    else
                        x=normrnd(0,1,n,1);
                        qj=(In-Qjstore*Qjstore')*x/norm((In-Qjstore*Qjstore')*x);
                        %compute the rotated impulse responses
                        fj=stackedirfmat*qj;
                        [success, qj, okay, yy]=bear.checksignres_inc_other_shocks(qj, fj, Scell, okay, IRFt, n);
                        if success==1
                            Qjstore=[Qj qj];
                            Qj(:,yy) = qj; %set column yy of Qj to qj
                            jj=jj+1;
                        end
                    end

            %once all n columns are build and fullfill the sign restriction
            %% if sign and magnitude restrictions are fullfilled, check proxy correlation restriction
            if size(Qj,2)==n && success==1  && IVcorrelcheck==1
                %disp('I reached correlation restrictions')
                D=hsigma*Qj;
                % recover the VAR coefficients, reshaped for convenience
                B=reshape(beta,k,n);
                % obtain the residuals from (this draw)
                EPS=Ydraw-Xdraw*B;
                %compute structural shocks
                ETA=D\EPS';
                [success corIV pivShockwithIV Qj] = bear.CheckCorrelWithIV(ETA,n,ShockwithIV, IVcorrel,OverlapIVcorrelinY, FlipCorrel, Qj);
                if success==1
                    disp('correlation restriction fulfilled');
                end
            end
            %% check relative magnitudes
            if size(Qj,2)==n && success==1  && relmagnres==1
                %              disp('I reached magnitude restrictions')
                D=hsigma*Qj;
                [~, ortirfmatrixmagnitude]=bear.irfsim(beta,D,n,m,p,k,max(IRFperiods,max(mperiods)));
                % generate the stacked IRF matrix
                stackedirfmatmagn=[];
                for kk=1:numel(mperiods)
                    stackedirfmatmagn=[stackedirfmatmagn;ortirfmatrixmagnitude(:,:,mperiods(kk,1)+1)];
                end
                [success]=bear.checkrelmag(stackedirfmatmagn,columnsS, columnsW, rowsS, rowsW, n, mperiods, ABS);
                if success ==1
                    disp('Magnitude Restrictions fullfilled')
                end
            end

            %% if sign, zero and relative magnitude restrictions are fullfilled, check FEVD restrictions
            if size(Qj,2)==n && success==1  && FEVDcheck==1
                disp('I reached FEVD restrictions')
                D=hsigma*Qj; %again compute D
                [~,ortirfmatrixFEVD]=bear.irfsim(beta,D,n,m,p,k,IRFperiods); %obtain orthogonalized IRFs
                % record the results in the cell irf_record
                %check FEVDrestrictions
                [success] = bear.CheckFEVDrestriction(ortirfmatrixFEVD,IRFperiods,n,D,FEVDperiodstot, clmrelativeFEVD, rowrelativeFEVD, rowabsoluteFEVD, clmabsoluteFEVD, FEVDresperiods, rowsFEVD);
                if success==1
                    disp('FEVD restriction fulfilled');
                end
            end

            end
            %finally compute the historical decomposition
            if size(Qj,2)==n && success==1  && HD==1
                D=hsigma*Qj;
                B=reshape(beta,k,n);
                % obtain the residuals from (this draw)
                EPS=Ydraw-Xdraw*B;
                %compute structural shocks
                ETA=D\EPS';
                Psidrawcut = Psidraw(2*p:end,:);
                %decompose the data into trend and cycle and further
                [hd_estimates] = bear.hd_new_for_signres_stvol4(0,beta,k,n,p,D,m,T,Xdraw,Ydraw, data_exo, contributors, hd_estimates2, Psidrawcut, YincLags(2*p+1:end,:));
            end
            % repeat this loop until a succesful draw is obtained

        end %end of while loop ´
        % with succesful Qj at hand, eventually set Q as Qj
        Q=Qj;
   end %end of if loop
    % store
    for kk=1:IRFperiods
        storage1{ii,1}(:,:,kk)=ortirfmatrix(:,:,kk)*Q;
    end
    storage2{ii,1}=hsigma*Q;
    % store historical decompositions
    if HD==1
        storage3{ii,1}=hd_estimates;
        storage4{ii,1}=ETA;
    end
    storage5(:,ii) = beta;
    storage6(:,ii) = bear.vec(sigma);

    %lpdf = irfpdf(hsigma*Q,betahat,aux1,T,sigmahat,beta);
    %lpdfirf(ii,1)=lpdf;
    if IVcorrelcheck==1
        IVcorrelation(ii,1)=corIV;
        PofIVcorrelation(ii,1)=pivShockwithIV;
    else
        IVcorrelation(ii,1)=0;
        PofIVcorrelation(ii,1)=0;
    end
    %  FEVDrestrictionvalue(ii,1) = FEVDrestrictionvalue;
    hbar.iterate(1);   % update progress by one iteration
end
close(hbar);   %close progress bar

beta_gibbs_reshuffle = storage5; %save the reshuffled beta and sigma such that they correspond to the structural matrix D
sigma_gibbs_reshuffle = storage6;
% reorganise storage
% loop over iterations
for ii=1:Acc
    % loop over IRF periods
    for jj=1:IRFperiods
        % loop over variables
        for kk=1:n
            % loop over shocks
            for ll=1:n
                struct_irf_record{kk,ll}(ii,jj)=storage1{ii,1}(kk,ll,jj);
            end
        end
    end
    D_record(:,ii)=storage2{ii,1}(:);
    gamma_record(:,ii)=bear.vec(eye(n));
end
%reorganize historical decompositions
hd_record=cell(contributors+2,n);
if HD==1
    for ii=1:Acc %loop over draws
        for kk=1:contributors+2 %loop over contributors
            for ll=1:n %loop over variables
                hd_record{kk,ll}(ii,:) = storage3{ii,1}{kk,ll};
            end
        end
    end
end
ETA_record=cell(n,1);
if HD==1
    for jj=1:Acc
        for kk=1:n
            ETA_record{kk,1}(jj,:)= storage4{jj,1}(kk,:);
        end
    end
end

%hdrecord is ordered such that columns are variables and rows are contributors
%row n+1 = contribution of the constant
%row n+2 = contribution of initial conditions (past shocks)
%row n+3 = unexplained part (for partially identified model
%row n+4 = part that was left to explain by the structural shocks after
%accounting for exogenous, constant and initial conditions

toc
fprintf('Accepted Draws in Percent of Total Number of Draws: %f', 100*(Acc)/(not_successful + Acc))
end


function [view, mapvol, covol] = polarAngleMap(view, dt, scans, params, legend, W);
%
% [view, map, co] = polarAngleMap(view, <dt, scans, params>, <legend>, <W>);
%
% AUTHOR: rory
% PURPOSE:
% Given corAnal data for a polar angle ("meridian")-mapping experiment
% (single scan or set of scans), produce a parameter map of preferred
% polar angle in units of degrees of visual field.
%
% If a single scan is provided as input, this function saves the
% parameter map in the scan's data type, assigning it only for that
% scan.
%
% However, if multiple input scans are provided (see below),
% the code saves the results in a new data type 'Meta_Analysis'.
% corAnal data from each scan are first converted into real-world
% units, then overlapping data are averaged together in a weighted
% average, based on each scan's coherence. (I.e., if one scan's
% co values for a given voxel are much higher than other scans,
% it will dominate the determination of what angle is represented.)
%
% I wrote this code to use in conjunction with my across-session
% tools (createCombinedSession, importTSeries) to run meta-analyses
% on retinotopy data.
%
% ARGUMENTS:
%   INPUT:
%   view: mrVista view. <Defaults to selected gray view>
%   dt: for a single scan, name or number of the data type
%       from which the input data come. If analyzing multiple
%       scans, a cell of length nScans of data type names/numbers.
%       <default: cur data type>
%
%   scans: scan or scans to use as input. <default: cur scan>
%
%   params: struct (or nScans-long struct array) specifying how
%       the stimulus mapped polar angle during each scan. Needs
%       the following fields:
%       params.startAngle: angle of center of wedge stimulus, measured
%           in degrees clockwise from 12-o-clock, at the start of each
%           cycle;
%       params.width: width of wedge stimulus in degrees.
%       params.direction: 'cw' or 'ccw', direction in which the stimulus
%       proceeded. (cw=clockwise or ccw=counterclockwise).
%       params.visualField: number of degrees the stimulus traversed
%       each cycle (e.g., 360 if it went all the way around).
%       <default: get these params using retinoCheckParams>
%
%   legend: optional flag which, if set to 1, provides for a separate
%       figure with a legend image to go with the polar angle map.
%       <default 0, don't show this>
%
%   W: optional vector of weights for each input scan, for use when
%       doing a meta-analysis across scans. The vector should be
%       the same length as the input scans, and should specify the
%       overall weight, on top of the coherence, that that scan's voxels
%       get. Useful for me, when high-res data produces lower co values,
%       but is actually more reliable at identifying meridian
%       representations. <default: all ones.>
%
%   OUTPUT:
%   view: mrVista view, set to the relevant data type / scan and with
%         the map loaded and set to map mode.
%
%   map: the map volume produced (but not the cell-of-scans set in the
%        view, the numeric matrix).
%
%   co: the maximum coherence at each voxel, across all the input scans.
%       (It may be more sensible to make this the mean, but I'm trying
%       max for now.) Same format as map.
%
%
% ras, 01/10/06.
if notDefined('view'),  view = getSelectedGray;                 end
if notDefined('dt'),    dt = viewGet(view, 'curDataType');      end
if notDefined('scans'), scans = viewGet(view, 'curScan');       end
if notDefined('legend'), legend = 0;                            end
if notDefined('W'),      W = ones(size(scans));                 end
if notDefined('params')
    params = retinoCheckParams(view, dt, scans);
end

mapName = 'Polar Angle (clock position)';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Deal with single input scan instances separately %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if length(scans)==1
    % this should be easy: just convert the corAnal values into
    % degrees of polar angle
    view = selectDataType(view, dt);
    corAnalPath = fullfile(dataDir(view), 'corAnal.mat');
    if ~exist(corAnalPath, 'file')
        error('corAnal not found. Run computeCorAnal first.');
    end
    load(corAnalPath, 'ph', 'co');
    srcPh = ph{scans}; srcCo = co{scans}; clear ph co;

    % map from corAnal ph to polar angle
    mapvol = polarAngle(srcPh, params) ./ 30;

    % make and set the parameter map
    mapPath = fullfile(dataDir(view), 'Polar_Angle_Map.mat');
    if exist(mapPath, 'file')
        load(mapPath, 'map', 'co');
    else
        map = cell(1, numScans(view));
    end

    map{scans} = mapvol;

    % let's set the map colormap to be the same as the phase mode
    % colormap, and save this with the map
    if checkfields(view, 'ui', 'phMode')
        view.ui.mapMode.cmap = view.ui.phMode.cmap;
        view.ui.mapMode.clipMode = [0 12];
    end

    view = setParameterMap(view, map, mapName);
    saveParameterMap(view, mapPath, 1, 1);

    if legend, polarAngleMapLegend(view); end

    % that should be it!
    if nargout>=3, covol = co{scans}; end
    view = refreshScreen(view);
    return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% if we get here, we have multiple scans: parse the arguments to    %
% be cell arrays, and get set up:                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nScans = length(scans);

if ~iscell(dt)
    mrGlobals;
    % assume a single dt is specified, and all the scans are
    % coming from that data type
    for i = 1:nScans
        if isnumeric(dt), tmp{i} = dataTYPES(dt).name;
        else,             tmp{i} = dt;
        end
    end
    dt = tmp; clear tmp;
else
    for i = 1:nScans
        if isnumeric(dt{i}), dt{i} = dataTYPES(dt{i}).name; end
    end
end

%%%%%Get corAnal volumes for each input scan
srcCo = cell(1, nScans); srcPh = cell(1, nScans);
uniqueDts = unique(dt);
for i = 1:length(uniqueDts)
    corAnalPath = fullfile(viewDir(view), uniqueDts{i}, 'corAnal.mat');
    if ~exist(corAnalPath, 'file')
        error('corAnal not found. Run computeCorAnal first.');
    end
    load(corAnalPath, 'co', 'ph', 'amp')

    I = cellfind(dt, uniqueDts{i});
    srcCo(I) = co(scans(I));
    srcPh(I) = ph(scans(I));
    srcAmp(I) = amp(scans(I));
end

%%%%%Set up the target data type for the multi-scan meta-analysis
view = initScan(view, 'Meta_Analysis', [], {dt{1} scans(1)});
view = selectDataType(view, 'Meta_Analysis');
view = setCurScan(view, numScans(view));
view = setAnnotation(view, sprintf('Meta Analysis for %s scans %s', ...
                                dt{1}, num2str(scans)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculate the Polar Angle Map  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%Set NaNs to zero -- will be ignored
for i = 1:nScans
    srcCo{i}(isnan(srcCo{i})) = 0;
    srcAmp{i}(isnan(srcAmp{i})) = 0;
    srcPh{i}(isnan(srcPh{i})) = 0;
end

%%%%%Convert each phase map into real-word units
for i = 1:nScans
    srcPh{i} = polarAngle(srcPh{i}, params(i)) ./ 30;
end

%%%%%because we allow an additional level of user-defined weights
%%%%%(orig. b/c I wanted to weigh high-res scans higher than low-res),
%%%%%adjust the coherence weights accordingly for each scan
for i = 1:nScans, srcCo{i} = srcCo{i} .* W(i); end

%%%%%Set up the weighted average
% we'll need a volume representing the sum of the coherence
% for each voxel, across input scans. This will serve as the
% denominator of the weight formula for each input scan.
coSum = zeros(size(srcCo{1})); coMax = zeros(size(srcCo{1}));
for i = 1:nScans,
    coSum = coSum + srcCo{i};
    coMax = max(coMax, srcCo{i});
end

%%%%%initialize the map and co volumes
mapvol = zeros(size(srcCo{1}));
covol = zeros(size(srcCo{1}));

%%%%%compute co volume as the mean across all input co volumes
for i = 1:nScans, covol = covol + srcCo{i}; end
covol = covol ./ length(srcCo);

%%%%%Compute the weighted average, iteratively across input scans
for i = 1:nScans
    mapvol = mapvol + (srcPh{i} .* srcCo{i} ./ coSum);

%     % alternate attempt: use winner-take-all: scan with the
%     % highest co value for a given voxel determines the map
%     % value at that voxel.
%     Imax = find(srcCo{i}==coMax);
%     mapvol(Imax) = srcPh{i}(Imax);
end

covol = coMax;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Output the parameter map %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize the output map and co data fields, loading it if it
% already exists:
mapPath = fullfile(dataDir(view), 'Polar_Angle_Map.mat');
if exist(mapPath, 'file')
    load(mapPath, 'map', 'mapName', 'co');
else
    map = cell(1, numScans(view));
    co = cell(1, numScans(view));
end

% append the map volume for the new scans
map{numScans(view)} = mapvol;
co{numScans(view)} = covol;

% before saving the map, copy the view's phase mode color map
% to the map mode, which will also be saved:
if checkfields(view, 'ui', 'phMode')
    view.ui.mapMode.cmap = view.ui.phMode.cmap;
    view.ui.mapMode.clipMode = [0 12];
end

% set the map in the view, and save it
view = setParameterMap(view, map, mapName);
saveParameterMap(view, mapPath, 1, 1);
save(mapPath, 'co', '-append'); % also add the co field
view.co = co;
refreshScreen(view);

% also set corAnal amp and ph fields, and save a corAnal, so
% we can view in that mode as well (nice colorbar, can ph-restrict)
% we map the ph back from degrees to radians:
phvol = deg2rad(mapvol.*30);
ampvol = srcAmp{1};
if exist(fullfile(dataDir(view), 'corAnal.mat'), 'file')
    view = loadCorAnal(view);
end
view.co{numScans(view)} = covol;
view.ph{numScans(view)} = phvol;
view.amp{numScans(view)} = ampvol;
view = saveCorAnal(view, 1);

newParams.type = 'polar_angle';     % set retino params such
newParams.startAngle = 0;           % that the default HSV color map
newParams.direction = 'clockwise';  % produces a nice wedge color bar
newParams.visualField = 360;
newParams.width = 0;
retinoSetParams(view, 'Meta_Analysis', numScans(view), newParams);

% show a legend if requested
if legend, polarAngleMapLegend(view); end

% ok, think that's it!

return
% /--------------------------------------------------------------------/ %

% /--------------------------------------------------------------------/ %
function A = polarAngleMapLegend(view);
% img = polarAngleMapLegend(view);
% Using the current map mode settings, produce a legend
% for a polar angle parameter map and plot in a separate figure.
% Returns a truecolor image if requested.
mode = view.ui.mapMode;

% generate an angle map A and a radius map R
% A will start at the 12-o-clock and run clockwise back to 12,
% ranging from 1 to the number of colors in the cmap.
[X Y] = meshgrid(1:256, 1:256);
X = X-128; Y = Y-128;
A = atan2(X, Y);
A = fliplr(mod(A-pi, 2*pi));
A = rescale(A, [], [1 mode.numColors]);
R = sqrt(X.^2 + Y.^2);

% take cmap from color part of map mode
cmap = mode.cmap(mode.numGrays+1:end,:);

% convert A to truecolor
A = ind2rgb(A, cmap);

% for each color channel, mask out region outside radius (128 pixels)
[I J] = find(R>128);
for ch = 1:3
    ind = sub2ind(size(A), I, J, repmat(ch, size(I)));
    A(ind) = 1;
end

% put up the image
figure('Color', 'w', 'Name', 'Polar Angle Map Legend');
imshow(A);

return


function []= sounding_powerlaw_sens(hydro,wet)
% function that computes the power law coefficents and performs a
% sensitivity analysis.
%
%     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 2013
% modifications
% 05/2013   DB:     Compute the power law coefficients for dedicated days.
% 08/2013   DB:     Fix the case when a netdelay cannot be computed.
% 02/2014   DB:     Add non-stamps file support of ifgs-based estimation.
% 03/2014   DB:     Fix the computation of the sensitivity analysis in
%                   absence of soundign for specific SAR dates.
% 08/2014   DB:     Expand for PS network and SB network for SAR date estimation
% 03/2016   DB:     remove warning on ifgday for non-stamps processing

if nargin<1
    hydro=1;
    wet=1;
end

% getting the data from the parms_aps file
n_months = getparm_aps('sounding_months');
sounding_start_date = getparm_aps('sounding_start_date');
sounding_end_date = getparm_aps('sounding_end_date');
sounding_dir=getparm_aps('sounding_dir');
time_stamp = getparm_aps('sounding_time_stamp');
sounding_ifg_dates = getparm_aps('sounding_ifg_dates');
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

% checking if the directory exists
if exist([sounding_dir filesep],'dir')~=7
        error('myApp:argChk', ['The specified filepath of the sounding data does not exist,...  \nAbort,... \n'])
end
% current directry
curdir = pwd;

% getting the interferogram data information
% loading the data
stamps_processed = getparm_aps('stamps_processed');
if strcmp(stamps_processed,'y')
   fprintf('Stamps processed structure \n')
   ll_matfile = getparm_aps('ll_matfile');
   ps = load(ll_matfile);
   dates = ps.day;
   lonlat = ps.lonlat;

    % getting the parms file list from stamps to see the final ifg list

    % constructing the matrix with master and slave dates
    if strcmp(getparm('small_baseline_flag'),'y')
        % for SB
        ifgs_ix = ps.ifgday_ix;

    else
        n_ifg = ps.n_ifg;
        % slightly different for PS.
        date_slave_ix = [1:n_ifg]';
        % the master dates
        date_master_ix = repmat(ps.master_ix,size(date_slave_ix,1),1);
        % ix interferograms
        ifgs_ix = [date_master_ix date_slave_ix];
    end
else
    % getting the dates in jullian format
    ifgday_matfile = getparm_aps('ifgday_matfile');
    ifgs_dates = load(ifgday_matfile);
    ifgs_dates = ifgs_dates.ifgday;
    dates = reshape(ifgs_dates,[],1);
    dates = unique(dates);
    dates = datenum(num2str(dates),'yyyymmdd');
    dates = sort(dates);        % dates increasing with time

    % getting the ix position for the master and slave dates with respect
    % to the times
    date_master = datenum(num2str(ifgs_dates(:,1)),'yyyymmdd');
    date_slave = datenum(num2str(ifgs_dates(:,2)),'yyyymmdd');

    for k=1:size(date_master,1)
        [date_master_ix(k,1)] = find(date_master(k,1)==dates);
        [date_slave_ix(k,1)] = find(date_slave(k,1)==dates);
    end

    % ix interferograms
    ifgs_ix = [date_master_ix date_slave_ix];
    clear date_master_ix date_slave_ix
end

if strcmp(sounding_ifg_dates,'y')
    % estimate for interferogram dates
    stamps_processed = getparm_aps('stamps_processed');
    if strcmp(stamps_processed,'y')
        ps = load(ll_matfile);
        date_start_vector = datestr(ps.day-15,'yyyymmdd');
        date_end_vector = datestr(ps.day+15,'yyyymmdd');

    else

        ifgday_matfile = getparm_aps('ifgday_matfile');
        ifgs_dates = load(ifgday_matfile);
        ifgs_dates = ifgs_dates.ifgday;
        dates = reshape(ifgs_dates,[],1);
        dates = unique(dates);
        dates = datenum(num2str(dates),'yyyymmdd');
        dates = sort(dates);        % dates increasing with time

        date_start_vector =  datestr(dates-15,'yyyymmdd');
        date_end_vector =  datestr(dates+15,'yyyymmdd');
    end

    % save name of the output data
    if hydro==1 && wet==0
        save_name = ['Powerlaw_sensitivity_hydro_SAR_dates_1month_' time_stamp_str 'Hr.mat'  ];
    elseif hydro==0 && wet==1
        save_name = ['Powerlaw_sensitivity_wet_SAR_dates_1month_' time_stamp_str 'Hr.mat'  ];
    else
        save_name = ['Powerlaw_sensitivity_SAR_dates_1month_' time_stamp_str 'Hr.mat'  ];
    end
    if exist([sounding_dir filesep 'Powerlaw'],'dir')~=7
        mkdir([sounding_dir filesep 'Powerlaw']);
    end

else
    % estimate is on fixed intervals
    % in case no start or end time is given, get it from the data files
    if isempty(sounding_start_date) || isempty(sounding_end_date)

        if exist('sounding.list','file')~=2
            % making a list of all the sounding files
            [dummy dummy2] = system('echo sounding_list > sounding.list');
            clear dummy dummy2
            for k=1:size(time_stamp,1)
                command_str = ['ls [0-9]???????_' time_stamp(k,:) '.mat >> sounding.list'];
                [dummy dummy2] = system(command_str);
                clear dummy dummy2
            end
        end
        temp = tdfread('sounding.list');
        [dummy dummy2] = system('rm sounding.list');
        clear dummy dummy2
        date_list_temp = temp.sounding_list(:,[1:8]);

        % selecting a date range when requested
        clear ix

        if isempty(start_date)
            sounding_start_date = date_list_temp(1,:);
        end
        if isempty(end_date)
            sounding_end_date = date_list_temp(end,:);
        end
        clear date_list_temp
    end

    % 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);
    start_month = str2num(start_str);
    end_month = str2num(end_str);

    % save name of the output data
   if hydro==1 && wet==0
        save_name = ['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 = ['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 = ['Powerlaw_sensitivity_' num2str(n_months) 'month_' time_stamp_str 'Hr_' num2str(start_year) start_str '_' num2str(end_year) end_str '.mat'  ];
    end
    if exist([sounding_dir filesep 'Powerlaw'],'dir')~=7
        mkdir([sounding_dir filesep 'Powerlaw']);
    end

    if exist([sounding_dir filesep 'Powerlaw'],'dir')~=7
        mkdir([sounding_dir filesep 'Powerlaw']);
    end

    % generating the periods
    month_str = ['01';'02';'03';'04';'05';'06';'07';'08';'09';'10';'11';'12'];
    counter = 1;
    % runnign the computation of the powerlaw in batches
    for k=1:end_year-start_year+1
        if k==end_year-start_year+1 & k>1
            for l=1:end_month
                date_start_temp = datenum([num2str(start_year+k-1)  month_str(l,:) '01'],'yyyymmdd');
                date_start_month_vector(counter,:) = datestr(date_start_temp,'yyyymmdd');
                counter = counter+1;
            end
        elseif k==end_year-start_year+1 & k==1
            for l=start_month:end_month
                date_start_temp = datenum([num2str(start_year+k-1)  month_str(l,:) '01'],'yyyymmdd');
                date_start_month_vector(counter,:) = datestr(date_start_temp,'yyyymmdd');
                counter = counter+1;
            end
        else
            for l=1:12
                date_start_temp = datenum([num2str(start_year+k-1)  month_str(l,:) '01'],'yyyymmdd');
                date_start_month_vector(counter,:) = datestr(date_start_temp,'yyyymmdd');
                counter = counter+1;
            end
        end
    end

    n_months_total = size(date_start_month_vector,1);
    for k=1:ceil(n_months_total/n_months)
        k_lower = n_months*(k-1)+1;
        k_upper = n_months*(k)+1;

        if k_upper>n_months_total
            k_upper = n_months_total;
        end
        date_start_vector(k,:)=date_start_month_vector(k_lower,:);
        date_end_vector(k,:) = datestr(datenum(date_start_month_vector(k_upper,:),'yyyymmdd')-1,'yyyymmdd');
    end

end

% Remove those months outside the users request
if ~isempty(sounding_start_date)
    ix_drop = find(datenum(date_start_vector,'yyyymmdd')<datenum(sounding_start_date,'yyyymmdd'));
else
    ix_drop = [];
end
if ~isempty(sounding_end_date)
    ix_drop = [ix_drop ; find(datenum(date_end_vector,'yyyymmdd')>datenum(sounding_end_date,'yyyymmdd'))];
end
ix_drop = unique(ix_drop);
date_end_vector(ix_drop,:)=[];
date_start_vector(ix_drop,:)=[];

% Computing the power law coefficients
for k=1:size(date_start_vector,1)
    fprintf(['\n' num2str(k) '/' num2str(size(date_start_vector,1)) ' completed \n']);
	% When having sounding data estimate the powerlaw and b coefficients

    [alpha_all,alpha_hc,h_0_threshold,n_soundings] = sounding(date_start_vector(k,:),date_end_vector(k,:),[],hydro,wet);
	alpha_vector_all(k,1) = alpha_all;
    alpha_vector(k,1) = alpha_hc;
	n_soundings_vector(k,1)=n_soundings;
	h0_vector(k,1)=h_0_threshold;

end
save([sounding_dir filesep 'Powerlaw' filesep save_name],'alpha_vector_all','alpha_vector','n_soundings_vector','h0_vector','date_start_vector','date_end_vector')

cd(curdir)
if strcmp(sounding_ifg_dates,'y')
    % estimate for interferogram dates
    stamps_processed = getparm_aps('stamps_processed');
    if strcmp(stamps_processed,'y')
        fprintf('Updating powerlaw parameters with new values. \n')

        % removing NaN by replacing them with the other SAR date estimates
        ix_alpha_fix = find(isnan(alpha_vector(:,1)));
        if ~isempty(ix_alpha_fix)
           if length(ix_alpha_fix)~=length(alpha_vector)
                fprintf([num2str(length(ix_alpha_fix)) ' out of ' num2str(length(alpha_vector)) ' SAR dates where set to the mean alpha due to lack of sounding data. \n'])
                alpha_vector(isnan(alpha_vector))=nanmean(alpha_vector);
                alpha_vector_fix = alpha_vector;
                save([sounding_dir filesep 'Powerlaw' filesep save_name],'-append','alpha_vector_fix','ix_alpha_fix')

           else
              fprintf('None of the SAR dates where estimate using sounding data \n')
           end

        end

        ix_height_fix = find(isnan(h0_vector(:,1)));
        if ~isempty(ix_height_fix)
           if length(ix_height_fix)~=length(h0_vector)
                fprintf([num2str(length(ix_height_fix)) ' out of ' num2str(length(alpha_vector)) ' SAR dates where set to the mean h0 due to lack of sounding data. \n'])
                h0_vector(isnan(h0_vector))=nanmean(h0_vector);
                h0_vector_fix = h0_vector;
                save([sounding_dir filesep 'Powerlaw' filesep save_name],'-append','h0_vector_fix','ix_height_fix')
           else
              fprintf('None of the SAR dates where estimate using sounding data \n')
           end
        end

        alpha_SAR = [alpha_vector(ifgs_ix(:,1)) alpha_vector(ifgs_ix(:,2))];
        h0_SAR = [h0_vector(ifgs_ix(:,1)) h0_vector(ifgs_ix(:,2))];

        % computing the mean between two SAR dates
        h0_InSAR = mean(h0_SAR,2);
        alpha_InSAR = mean(alpha_SAR,2);

        setparm_aps('powerlaw_h0',h0_InSAR');
        setparm_aps('powerlaw_alpha',alpha_InSAR');
    end
else

    % computing the mean between two SAR dates
    ix = isnan(h0_vector);
    powerlaw_h0 = mean(h0_vector(~ix));
    powerlaw_alpha = mean(alpha_vector(~ix));

    setparm_aps('powerlaw_h0',powerlaw_h0');
    setparm_aps('powerlaw_alpha',powerlaw_alpha');
end


function [flat, allDist, sumDist, geodesicCoords, geodesicLayer] = measureCorticalDistance(flat, coords, verboseFlag, gray)
% USAGE: measureCorticalDistance(flat, [coords], [verboseFlag], [gray])
%
% AUTHOR:  Dougherty
% DATE:    2002.01.15
% PURPOSE:
%   Compute the shortest cortical manifold distance between
%   points. If coords is passed in, then the points are drawn
%   from there. Otherwise, the points are obtained via ginput.
%   (Note that coords must be a valid nx3 list of flat view coords,
%   such as would be obtained by 'getCurROIcoords(flatView)'.)
%
% HISTORY
%
% 7/16/02 djh, replaced mrSESSION.vAnatomyPath with global vANATOMYPATH
% 9/02/04 mms, inserted a warning not to use rotated flats.
% 9/22/04 rfd, now returns the geodesic path (as coords) and the gray layer
% of each point along the path.

global vANATOMYPATH;
mmPerPix = readVolAnatHeader(vANATOMYPATH);

% Check some flat and volume stuff here when you
% get a chance to fix this code up

% Get a gray structure because we need the gray nodes.
if notDefined('gray')
    gray = getSelectedGray;
    if isempty(gray)
        gray=initHiddenGray;
    end
end

if(~exist('coords','var') | isempty(coords))
    % Select flat figure and get a single point from the user
    figure(flat.ui.figNum)
    disp('Click left to add points, right to quit');
    button = 1;
    count = 0;
    w = 0.5;
    coords = [];
    z = viewGet(flat, 'Current Slice');
    while(button~=3)
        [x,y,button] = ginput(1);
        if(button==3)
            break;
        end
        count = count+1;
        coords = [coords,round([y;x;z])];
        h(count) = line([x-w,x-w,x+w,x+w,x-w],[y-w,y+w,y+w,y-w,y-w],'Color','w');
        if flat.rotateImageDegrees(coords(3,1))>0  %detects if the Flat your are currently selecting the points ist rotated
            errordlg('Flat must not be rotated','Warning!'); %Results would be wrong!
        end
    end
    % Delete the temporarily drawn squares
    for ii=1:length(h)
        delete(h(ii));
    end
    clear h;
end
if(~exist('verboseFlag','var') || isempty(verboseFlag))
    verboseFlag=1;
end

% the third coordinate is the 'slice', which, for flat views, means left or right hemisphere.
slice = coords(3,1);
if (slice==1)
    nodes = gray.allLeftNodes;
    edges = gray.allLeftEdges;
else
    nodes = gray.allRightNodes;
    edges = gray.allRightEdges;
end

% We loop for the number of line segements, which is the number of
% coords - 1.
geodesic = [];
for(ii=1:size(coords,2)-1)
    % get nearest flat coordinate (not all points on the flat correspond to flat coordinates)
    flatDistances = (flat.coords{slice}(1,:) - coords(1,ii)).^2 + ...
        (flat.coords{slice}(2,:) - coords(2,ii)).^2;
    % There is a one-to-many mapping of flatCoords to grayCoords, but we ignore that
    % here by using 'min', which will always reuturn one value, even if there are several
    % identical minima.
    % FIX THIS- we should always grab layer 1, or something more consistent
    % than relying on min's arbitrary sort.
    [val,startIndex] = min(flatDistances);
    % Do it again, for ii+1, to find the end point of this line segment.
    flatDistances = (flat.coords{slice}(1,:) - coords(1,ii+1)).^2 + ...
        (flat.coords{slice}(2,:) - coords(2,ii+1)).^2;
    [val,endIndex] = min(flatDistances);

    % Draw a line for each measured segment
    % we use the actual gray node coords rather than the ROI coords, so the use can see if
    % there is any non-trivial discrepancy.
    h(ii) = line([flat.coords{slice}(2,startIndex),flat.coords{slice}(2,endIndex)], ...
                 [flat.coords{slice}(1,startIndex),flat.coords{slice}(1,endIndex)], ...
                 'Color', 'r', 'LineWidth', 2);

    % Extract the gray node corresponding to the start
    startGrayNode = find(nodes(2,:) == flat.grayCoords{slice}(1,startIndex) & ...
                         nodes(1,:) == flat.grayCoords{slice}(2,startIndex) & ...
                         nodes(3,:) == flat.grayCoords{slice}(3,startIndex));
    endGrayNode = find(nodes(2,:) == flat.grayCoords{slice}(1,endIndex) & ...
                       nodes(1,:) == flat.grayCoords{slice}(2,endIndex) & ...
                       nodes(3,:) == flat.grayCoords{slice}(3,endIndex));

    % Catch errors. If we give mrManDist an empty startPoint array, it barfs.
    if(isempty(startGrayNode) | isempty(endGrayNode))
        myErrorDlg('No gray nodes were found for these coords!');
    end

    % Now, compute the manifold distance between these points.
    % mrManDist returns the distance to all other points from the given 'start' point.
    %allDist = mrManDist(nodes, edges, startGrayNode, mmPerPix, -1, 0);
    [allDist,nPts,lastPoint] = mrManDist(nodes, edges, startGrayNode, ...
                                      mmPerPix, -1,0);
    nextPoint = endGrayNode;
    while(nextPoint~=startGrayNode)
      geodesic(end+1) = lastPoint(nextPoint);
      nextPoint = lastPoint(nextPoint);
    end

    % We just want the distance from the start point to the end point, so we
    % pull that out by providing the index of the end point.
    dist(ii) = allDist(endGrayNode);
    if(verboseFlag>=0)
        disp(['Cortical distance of segment ',num2str(ii),': ',num2str(dist(ii)),' mm.']);
    end
end
sumDist=sum(dist);
if(verboseFlag>=0)
    disp(['Total cortical distance: ',num2str(sumDist),' mm.']);
end
if(verboseFlag>0)
    uiwait(msgbox(['Total cortical distance: ',num2str(sumDist),' mm.'], ...
        'Cortical Distance', 'modal'));
end

for ii=1:length(h)
    delete(h(ii))
end
geodesicCoords = nodes([2,1,3],geodesic);
geodesicLayer =  nodes(6,geodesic);
return


function [At,b,c,K,prep,origcoeff] = pretransfo(At,b,c,K,pars)

% [At,b,c,K,prep] = pretransfo(At,b,c,K)
%
% PRETRANSFO  Checks data and then transforms into internal SeDuMi format.
%
% **********  INTERNAL FUNCTION OF SEDUMI **********
%
% See also sedumi

% Nearly complete rewrite
% Copyright (c) 2013 Michael C. Grant
%
% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% 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

% -------------------------------------------------------------------------
% Make sure that all fields exist in K, and verify that they are valid
% -------------------------------------------------------------------------

if ~isfield(K,'f') || isempty(K.f)
    K.f = 0;
elseif numel(K.f) ~= 1 || K.f ~= floor(K.f) || K.f < 0 || ~isreal(K.f)
    error('K.f should be nonnegative integer')
end
if ~isfield(K,'l') || isempty(K.l)
    K.l = 0;
elseif K.l ~= floor(K.l) || K.l < 0 || ~isreal(K.l)
    error('K.l should be nonnegative integer')
end
if ~isfield(K,'q') || ~nnz(K.q)
    K.q = zeros(1,0);
else
    K.q = K.q(:)';
    if any(K.q ~= floor(K.q)) || any(K.q<2) || ~isreal(K.q)
        error('K.q should contain only integers bigger than 1')
    end
end
if ~isfield(K,'r') || ~nnz(K.r)
    K.r = zeros(1,0);
else
    K.r = K.r(:)';
    if any(K.r ~= floor(K.r)) || any(K.r<3) || ~isreal(K.r)
        error('K.r should contain only integers bigger than 2')
    end
end
if ~isfield(K,'s') || ~nnz(K.s)
    K.s = zeros(1,0);
else
    K.s = K.s(:)';
    if any(K.s ~= floor(K.s)) || any(K.s<1) || ~isreal(K.s)
        error('K.s should contain only positive integers')
    end
end
% As an alternative to the 'scomplex' flag, I've added a 'z' parameter
% containing a list of Hermitian semidefinite cone sizes. For now, this
% is just translated to 'scomplex' for you. In the future, we may use
% this internally *instead* of scomplex or rsdpN.
if ~isfield(K,'z') || ~nnz(K.z)
    K.z = zeros(1,0);
else
    K.z = K.z(:)';
    if any(K.z ~= floor(K.z)) || any(K.z<1) || ~isreal(K.z)
        error('K.z should contain only positive integers')
    end
end

N_f    = K.f;
N_l    = K.l;
N_fl   = N_f + N_l;
L_q    = length(K.q);
N_q    = sum(K.q);
L_r    = length(K.r);
N_r    = sum(K.r);
N_qr   = N_q + N_r;
L_qr   = L_q + L_r;
L_s    = length(K.s);
L_z    = length(K.z);
L_sz   = L_s + L_z;
N_s    = sum((K.s).^2);
N_z    = sum((K.z).^2);
N_sz   = N_s + N_z;
L_qrsz = L_qr + L_sz;
N_flqr = N_fl + N_qr;
N      = N_flqr + N_sz;

if ~isfield(K,'ycomplex') || isempty(K.ycomplex)
    K.ycomplex = zeros(1,0);
else
    K.ycomplex = sort(K.ycomplex(:))';
    K.ycomplex(find(~diff(K.ycomplex))+1) = [];
    if any(K.ycomplex ~= floor(K.ycomplex)) || any(K.ycomplex<1)
        error('K.ycomplex should contain only positive integers');
    elseif any(K.ycomplex > numel(b))
        error('Elements of K.ycomplex are out of range');
    end
end
if ~isfield(K,'xcomplex') || isempty(K.xcomplex)
    K.xcomplex = zeros(1,0);
else
    K.xcomplex = sort(K.xcomplex(:))';
    K.xcomplex(find(~diff(K.xcomplex))+1) = [];
    if any(K.xcomplex ~= floor(K.xcomplex)) || any(K.xcomplex<1)
        error('K.xcomplex should contain only positive integers');
    elseif any(K.xcomplex>N_flqr)
        error('Elements of K.xcomplex are out of range');
    end
end
if ~isfield(K,'scomplex') || isempty(K.scomplex)
    K.scomplex = zeros(1,0);
else
    K.scomplex = sort(K.scomplex(:))';
    K.scomplex(find(~diff(K.scomplex))+1) = [];
    if any(K.scomplex~=floor(K.scomplex)) || any(K.scomplex<1)
        error('K.scomplex should contain only positive integers');
    elseif any(K.scomplex>L_s)
        error('Elements of K.xcomplex are out of range');
    end
end
if L_z
    K.s = [ K.s, K.z ];
    K.scomplex = [ K.scomplex, L_s + 1 : L_sz ];
    K.z = zeros(1,0);
    L_s = L_s + L_z;
    N_s = N_s + N_z;
    L_z = 0; %#ok
    N_z = 0; %#ok
end

% -------------------------------------------------------------------------
% Verify the size and validity of At, b, and c
% N = # variables
% -------------------------------------------------------------------------

% SeDuMi assumes that if At is not consistent with K but At' is, that the
% user supplied the transpose of the coefficient matrix. This introduces a
% rare ambiguity in the case where At happens to be square. Past versions
% of SeDuMi would not have allowed this, instead rejecting the case where
% m >= N; however, with complex variables, the situation is not quite as
% clear, and there may technically be cases where m >= N is acceptable.

if ndims(At) > 2 %#ok
    error('A must be a matrix');
elseif nnz(isnan(At)) || nnz(isinf(At))
    error('A contains NaN or Inf');
elseif size(At,1) == N
    % nothing
elseif size(At,2) == N
    At = At';
else
    error('(At,K) size mismatch');
end
if all(size(b)>1)
    error('Parameter b must be a vector');
elseif any(isnan(b)) || any(isinf(b))
    error('b contains NaN or Inf');
elseif length(b) ~= size(At,2)
    error('(At,b) size mismatch');
else
    b = b(:);
end
if all(size(c)>1)
    error('Parameter c must be a vector');
elseif any(isnan(c)) || any(isinf(c))
    error('c contains NaN or Inf');
elseif length(c) ~= N
    error('(c,K) size mismatch');
else
    c = c(:);
end

% -------------------------------------------------------------------------
% Save the standardized data for further use if needed
% -------------------------------------------------------------------------

if isfield(pars,'errors') && pars.errors==1
    origcoeff.At=At;
    origcoeff.c=c;
    origcoeff.b=b;
    origcoeff.K=K;
else
    origcoeff=[];
end

% -------------------------------------------------------------------------
% Flag diagonal SDP blocks for removal
% -------------------------------------------------------------------------

% There is some serious MATLAB trickery here (if I do say so myself) that
% merits explanation. "spattern" contains the indices of symbolically
% nonzero elements of the dual variable z = c - At * y. This is a single
% vector across all LMI constraints, so "sblk" tells us which indices
% belong to which constraints. The "rem" statement is what determines if
% a particular element is off-diagonal. If there is even one off-diagonal
% element in an SDP, then its corresponding element of "sdiag" is false.
%
% This new version of the analysis replaces the old preprocessSDP(), and
% seems inexpensive enough to apply to all LMIs regardless of size/count.
% The previous version was applied more sparingly.
%
% In theory one could do a more complex analysis of the block structure of
% an LMI, potentially breaking a larger into smaller ones. But this would
% likely be significantly more expensive.

if L_s && ( ~isfield(pars,'sdp') || pars.sdp )
    ssiz        = (K.s).^2;
    strt        = cumsum([1,ssiz(1:end-1)]);
    sblk        = zeros(1,N_s);
    sblk(strt)  = 1;
    sblk        = cumsum(sblk);
    spattern    = find(c(N_flqr+1:N)~=0|any(At(N_flqr+1:N,:),2))';
    sblk        = sblk(spattern);
    sblk        = sblk(rem(spattern-strt(sblk),K.s(sblk)+1)~=0);
    sdiag       = true(1,L_s);
    sdiag(sblk) = false;
else
    % Even if we disable SDP processing, we're going to move 1x1 SDPs into
    % the nonnegative variable block. It just doesn't make sense to deploy
    % all of that SDP machinery for nonnegative variables.
    sdiag = K.s == 1;
end

% -------------------------------------------------------------------------
% Handle K.ycomplex by splitting apart the complex constraints into pairs
% of real constraints
% -------------------------------------------------------------------------

if ~isempty(K.ycomplex)
    b  = [ real(b) ; imag(b(K.ycomplex)) ];
    At = [ At, 1j * At(:,K.ycomplex) ];
else
    b  = real(b);
end

% -------------------------------------------------------------------------
% Find the locations of the the complex data, so we can convert into
% SeDuMi's internal format, which uses only MATLAB's real representation.
% -------------------------------------------------------------------------
% Strictly speaking, nonnegative variables, the first variable in a Lorentz
% cone, and the first two variables in a rotated Lorentz cone are real. But
% But SeDuMi has allowed them all to be specified as complex; the imaginary
% portions are interpreted as free variables. We have kept that behavior.

% This code replaces whichcpx.c in its entirety. It actually fixes a bug in
% rotated Lorentz cone handling that was probably never exercised.
if isempty(K.xcomplex) && isempty(K.scomplex)
    K.fcplx = zeros(1,0);
    K.qcplx = zeros(1,0);
    K.rcplx = zeros(1,0);
    scplx   = false(1,L_s);
    sreal   = ~sdiag;
    K.rsdpN = L_s;
    N_fc = 0;
    K.cdim = 0;
else
    xc = K.xcomplex;
    tt = xc <= N_fl;
    K.fcplx = xc(tt);
    xc = xc(~tt) -  N_fl;
    tt = xc <= N_q;
    K.qcplx = xc(tt);
    xc = xc(~tt) - N_q;
    tt = xc <= N_r;
    K.rcplx = xc(tt);
    if ~isempty(K.qcplx)
        ndxs = cumsum([1,K.q(1:end-1)]);
        t2 = any(bsxfun(@eq,K.qcplx,ndxs'),1);
        K.fcplx = [ K.fcplx, K.qcplx(t2) + N_fl ];
        K.qcplx(t2) = [];
        t2 = sum(bsxfun(@gt,K.qcplx,ndxs'),1);
        K.q = K.q + full(sparse(1,t2,1,1,L_q));
        K.qcplx = K.qcplx + (1:length(K.qcplx));
        N_q = N_q + length(K.qcplx);
    end
    if ~isempty(K.rcplx)
        ndxs = cumsum([1,K.r(1:end-1)]);
        t2 = any(bsxfun(@eq,K.rcplx,[ndxs,ndxs+1]'),1);
        K.fcplx = [ K.fcplx, K.rcplx(t2) + N_fl + N_q ];
        K.rcplx(t2) = [];
        t2 = sum(bsxfun(@gt,K.rcplx,ndxs'),1);
        K.r = K.r + full(sparse(1,t2,1,1,L_r));
        % This 2*t2 offset is required because QR makes two accesses
        % each of the first two elements of a rotated Lorentz cone.
        K.rcplx = K.rcplx + (1:length(K.rcplx)) + 2 * t2;
        N_r = N_r + length(K.rcplx);
    end
    N_fc = length(K.fcplx);
    N_f  = N_f + N_fc;
    scplx = false(1,L_s);
    scplx(K.scomplex&~sdiag) = true;
    sreal = ~scplx & ~sdiag;
    K.rsdpN = nnz(sreal);
    K.cdim = length(K.xcomplex) + sum(K.s(scplx).^2);
end

% -------------------------------------------------------------------------
% We have significantly rewritten this section of the code. This section
% constructs a sparse matrix that represents the following transformations:
%   --- Free variables split into differences of nonnegative variables; OR
%   --- Free variables placed in a Lorentz-cone
%   --- Rotated lorentz cones translated to standard Lorentz cones
%   --- Lorentz cones rearranged to trace block + norm-bound blocks
%   --- Conversion of diagonal SDPs to nonnegative variables
%   --- SDP coefficients moved to the lower triangle for increased sparsity
% This code replaces the rotlorenz, qreshape, and vectril MEX files.
% -------------------------------------------------------------------------

newL = 0;
newQ = zeros(1,0);
ii = {}; jj = {}; vv = {};

% Split free variables into the difference of nonnegatives
if ~isfield( pars, 'free' ) || pars.free == 2 && L_qrsz
    pars.free = 1;
end
if N_f && ~pars.free
    jt = [ 1 : K.f, K.fcplx ; 1 : K.f, K.fcplx ];
    vt = [ ones(1,K.f), -1j*ones(1,N_fc) ; -ones(1,K.f), 1j*ones(1,N_fc) ];
    ii{end+1} = 1 : 2 * N_f;
    jj{end+1} = jt(:)';
    vv{end+1} = vt(:)';
    newL = 2 * N_f;
    prep.freeL = N_f;
end

% Copy nonnegative variables without change
if K.l
    ii{end+1} = newL + 1 : newL + K.l;
    jj{end+1} = K.f + 1 : K.f + K.l;
    vv{end+1} = ones(1,K.l);
    newL = newL + K.l;
end

% Convert diagonal SDPs to nonnegative variables
if any(sdiag)
    dsize = K.s(sdiag);
    sdpL = sum(dsize);
    prep.sdiag = dsize;
    jstrt = cumsum([N_flqr+1,K.s(1:end-1).^2]);
    jstrt = jstrt(sdiag);
    istrt = cumsum([1,dsize(1:end-1)]);
    dsize = dsize + 1;
    dblks = cumsum(full(sparse(1,istrt,1,1,sdpL)));
    ii{end+1} = newL + 1 : newL + sdpL;
    jj{end+1} = jstrt(dblks) + dsize(dblks) .* ( ( 1 : sdpL ) - istrt(dblks) );
    vv{end+1} = ones(1,sdpL);
    newL = newL + sdpL;
end

% Stuff free variables into a Lorentz cone
tr_off = newL;
nb_off = newL + L_qr;
if N_f && pars.free
    tr_off = tr_off + 1;
    nb_off = nb_off + 1;
    ii{end+1} = nb_off + 1 : nb_off + N_f;
    jj{end+1} = [ 1 : K.f, K.fcplx ];
    vv{end+1} = [ ones(1,K.f), -1j*ones(1,N_fc) ];
    nb_off = nb_off + K.f;
    newQ = N_f + 1;
end

% Rearrange Lorentz cones to trace block + norm-bound blocks
if N_q
    ndxs      = cumsum([1,K.q(1:end-1)]);
    it        = zeros(1,N_q);
    it(ndxs)  = tr_off + 1 : tr_off + L_q;
    it(it==0) = nb_off + 1 : nb_off + ( N_q - L_q );
    jt = K.f + K.l + 1 : K.f + K.l + N_q;
    vt = ones(1,N_q);
    if ~isempty(K.qcplx)
        jt = jt - cumsum(full(sparse(1,K.qcplx,1,1,N_q)));
        vt(K.qcplx) = -1j;
    end
    ii{end+1} = it(:)';
    jj{end+1} = jt;
    vv{end+1} = vt;
    tr_off    = tr_off + L_q;
    nb_off    = nb_off + N_q - L_q;
end

% Transform rotated Lorentz cones to standard Lorentz cones, and rearrange
% to trace block + norm-bound blocks.
if N_r
    ndxr       = cumsum([1,K.r(1:end-1)]);
    ndxp       = ndxr + 2*(0:L_r-1);
    it         = zeros(1,N_r+2*L_r);
    it(ndxp)   = tr_off + 1 : tr_off + L_r;
    it(ndxp+1) = -1;
    it(ndxp+2) = it(ndxp);
    it(it==0)  = nb_off + 1 : nb_off + ( N_r - L_r );
    it(ndxp+1) = it(ndxp+3);
    jt = [ K.f + K.l + N_q + 1, ones(1,N_r+2*L_r-1) ];
    jt([ndxp+1,ndxp+3]) = 0;
    vt = ones(1,N_r+2*L_r);
    vt([ndxp,ndxp+1,ndxp+2]) = sqrt(0.5);
    vt(ndxp+3) = -sqrt(0.5);
    if ~isempty(K.rcplx)
        jt(K.rcplx) = 0;
        vt(K.rcplx) = -1j;
    end
    ii{end+1} = it;
    jj{end+1} = cumsum(jt);
    vv{end+1} = vt;
    nb_off = nb_off + N_r - L_r;
end

% Replace non-diagonal real SDP coefficients with tril(X) + tril(X',-1).
% This cuts the number of nonzeros approximately in half.
if K.rsdpN
    dsize = K.s(sreal);
    sdpL  = sum(dsize.^2);
    jstrt = cumsum([N_flqr+1,K.s(1:end-1).^2]);
    jstrt = jstrt(sreal);
    istrt = cumsum([1,dsize(1:end-1).^2]);
    dblks = cumsum(full(sparse(1,istrt,1,1,sdpL)));
    istrt = istrt + nb_off;
    dsize = dsize(dblks);
    istrt = istrt(dblks);
    jndxs = ( nb_off + 1 : nb_off + sdpL ) - istrt;
    cols  = floor(jndxs ./ dsize);
    rows  = jndxs - dsize .* cols;
    ii{end+1} = max(rows,cols) + min(rows,cols) .* dsize + istrt;
    jj{end+1} = jndxs + jstrt(dblks);
    vv{end+1} = ones(1,sdpL);
    nb_off = nb_off + sdpL;
    clear dsize jstrt istrt dblks jndxs rows cols
end

% Replace Hermitian SDP coefficients with tril(X) + tril(X',-1). This one's
% a bit trickier because we have the real and complex values interleaved,
% and the imaginary values along the diagonal are zero.
if K.rsdpN < length(K.s)
    dsize = K.s(scplx);
    jsize = dsize .^ 2;
    sdpL  = 2 * sum(jsize);
    jstrt = cumsum([N_flqr+1,K.s(1:end-1).^2]);
    jstrt = jstrt(scplx);
    bstrt = cumsum([1,2*jsize(1:end-1)]);
    dblks = cumsum(full(sparse(1,bstrt,1,1,sdpL)));
    istrt = bstrt + nb_off;
    dsize = dsize(dblks);
    istrt = istrt(dblks);
    bndxs = ( nb_off + 1 : nb_off + sdpL ) - istrt;
    cols  = floor( bndxs ./ dsize );
    rows  = bndxs - dsize .* cols;
    imgv  = cols >= dsize;
    cols  = cols - imgv .* dsize;
    indxs = max(rows,cols) + min(rows,cols) .* dsize + imgv .* jsize(dblks) + istrt;
    vals  = ( 1 - 2 * ( cols > rows ) ) .* ( 1 - ( 1 + 1j ) .* imgv );
    keep  = ~imgv | ( rows ~= cols );
    jndxs = rows + cols .* dsize + jstrt(dblks);
    ii{end+1} = indxs(keep);
    jj{end+1} = jndxs(keep);
    vv{end+1} = vals(keep);
    clear dsize jsize jstrt bstrt istrt bndxs rows cols vals imgv keep
end

% Update free, nonnegative, and Lorentz variable counts
K.f = 0;
K.l = newL;
K.q = [ newQ, K.q, K.r ];
K.r = zeros(0,1);
K.s = [ K.s(:,~scplx&~sdiag), K.s(scplx&~sdiag) ];
K.rsdpN = nnz(~scplx&~sdiag);
K.N = K.l + sum(K.q) + sum(K.s(1:K.rsdpN).^2)+2*sum(K.s(K.rsdpN+1:end).^2);

% Create the artificial (x0,z0) variable for the self-dual model by
% appending a zero row to At and c. This is accomplished in QR by adding
% 1 to all of the row indices created above.
K.N  = K.N + 1;
K.l  = K.l + 1;
K.m  = length(b);

% Transform At, c
% The transformation matrix QR does not satisfy QR'*QR=I. But it does, in
% fact, serve as a reverse transformation:
%    --- For Lorentz cones, QR applies a permutation; QR' reverses it.
%    --- For rotated Lorentz cones, QR applies a unitary, self-adjoint
%        rotation on the first two variables, so QR' reverses it.
%    --- For split free variables, QR creates the positive and negative
%        parts; QR' combines them back together.
%    --- For free variables placed in a Lorentz cone, QR moves them to the
%        Lorentz cone block, adding an extra epigraph variable. QR' moves
%        them back and drops the extra variable.
%    --- For semidefinte cones, QR adds the strict upper triangle to the
%        strict lower triangle; QR' copies the strict lower triangle to the
%        strict upper triangle, ensuring symmetry.
%    --- QR adds a row for the self-dual variable; QR' removes it.
[dummy,ndxs] = sort(cellfun(@(x) x(1),jj)); %#ok
QR = sparse( horzcat(ii{ndxs})+1, horzcat(jj{ndxs}), horzcat(vv{ndxs}), K.N, length(c) );
At = real( sparse( QR * At ) );
c  = real( sparse( QR * c  ) );
b  = sparse( b );
prep.QR = QR;
clear ii jj vv

% -------------------------------------------------------------------------
% Now K has field K.{l,q,s}
% Generate a more detailed description of cone K:
% Let K.blkstart(i):K.blkstart(i+1)-1 give the index range of block i.
% Compute maxN=max(order), len=sum(order) for LORENTZ, real PSD, herm PSD
% yields: K.{l,q,s,rsdpN,blkstart,rLen,hLen,qMaxn,rMaxn,hMaxn}
% -------------------------------------------------------------------------
Ksr = K.s(1:K.rsdpN);
Ksc = K.s(K.rsdpN+1:end);
K.blkstart = cumsum([K.l+1,length(K.q)+length(K.r),K.q-1,Ksr.^2,2*Ksc.^2]);
K.rLen = sum(Ksr);
K.hLen = sum(Ksc);
K.qMaxn = max([0,K.q]);
K.rMaxn = max([0,Ksr]);
K.hMaxn = max([0,Ksc]);
K.mainblks = K.blkstart(cumsum([1 1 length(K.q)]));
K.qblkstart = K.blkstart(2:2+length(K.q));  % Also include blkend
K.sblkstart = K.blkstart(2+length(K.q):end);
K.lq = K.mainblks(end)-1;


function planC = sumDose(doseNums,wtfactor,lqParamS,assocScan,newDoseName,planC)
%function planC = sumDose(doseNumsV,weightsV,assocScan)
%
%This function creates a new dose distribution by adding up doseNums
%according to factors wtfactor. assocScan is the scan number to associate
%new dose with. newDoseName is the name of new dose.
%
%APA, 12/23/2009
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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 3 of the License, or
%     (at your option) any later version.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

wtfactorTmp = wtfactor;
wtfactor = ones(1,max(doseNums));
wtfactor(doseNums) = wtfactorTmp;

indexS = planC{end};
if assocScan > 0
    assocScanUID = planC{indexS.scan}(assocScan).scanUID;
    %Get associated transM
    assocTransM = planC{indexS.scan}(assocScan).transM;
    if isempty(assocTransM)
        assocTransM = eye(4);
    end
else %No Association
    assocScanUID = '';
    assocTransM = eye(4);
end

% doseNums = checkedDoses;

%Get the x,y,z grid for new dose
for i = 1:length(doseNums)
    doseNum = doseNums(i);
    %Get x,y,z values for doseNum
    [xV, yV, zV] = getDoseXYZVals(planC{indexS.dose}(doseNum));

    %Get the corners of the original dataset.
    [xCorn, yCorn, zCorn] = meshgrid([min(xV) max(xV)], [min(yV) max(yV)], [min(zV) max(zV)]);

    %Add ones to the corners so we can apply a transformation matrix.
    corners = [xCorn(:) yCorn(:) zCorn(:) ones(prod(size(xCorn)), 1)];

    %Apply transform to corners, so we know boundary of the slice.
    transM = getTransM(planC{indexS.dose}(doseNum),planC);
    if isempty(transM) || isequal(transM,eye(4))
        [xV,yV,zV] = getDoseXYZVals(planC{indexS.dose}(doseNum));
        xGrid{doseNum} = xV(:)';
        yGrid{doseNum} = yV(:)';
        zGrid{doseNum} = zV(:)';
    else
        newCorners = inv(assocTransM) * transM * corners';
        xGrid{doseNum} = linspace(min(newCorners(1,:)), max(newCorners(1,:)), length(xV));
        yGrid{doseNum} = linspace(max(newCorners(2,:)), min(newCorners(2,:)), length(yV));
        zGrid{doseNum} = linspace(min(newCorners(3,:)), max(newCorners(3,:)), length(zV));
    end
    xRes{doseNum} = length(xV);
    yRes{doseNum} = length(yV);
    zRes{doseNum} = length(zV);

    %Get associated scan
    % assocScanV{doseNum} = getAssociatedScan(planC{indexS.dose}(doseNum).assocScanUID);
    assocScanV{doseNum} = getDoseAssociatedScan(doseNum,planC);
end
newXgrid = linspace(min(cell2mat(xGrid)),max(cell2mat(xGrid)),max(cell2mat(xRes)));
newYgrid = linspace(max(cell2mat(yGrid)),min(cell2mat(yGrid)),max(cell2mat(yRes)));
newZgrid = linspace(min(cell2mat(zGrid)),max(cell2mat(zGrid)),max(cell2mat(zRes)));

%Obtain doses with same grid
if isempty(assocTransM)
    assocTransM = eye(4);
end
doseIndC = {};
doseNumsTmp = doseNums;
for iSortAll = 1:length(doseNums)
    iSort = doseNums(iSortAll);
    indRemaining = doseNums;
    indRemaining(iSortAll) = [];
    doseSortM = [iSort];
    if ~ismember(iSort,[doseIndC{:}])
        for jSortAll = 1:length(indRemaining)
            jSort = indRemaining(jSortAll);
            if ~isempty(getTransM('dose',iSort,planC))
                doseITM = inv(assocTransM) * getTransM('dose',iSort,planC);
            else
                doseITM = inv(assocTransM);
            end
            if ~isempty(getTransM('dose',jSort,planC))
                doseJTM = inv(assocTransM) * getTransM('dose',jSort,planC);
            else
                doseJTM = inv(assocTransM);
            end
            if isequal([xRes{iSort},yRes{iSort},zRes{iSort}],[xRes{jSort},yRes{jSort},zRes{jSort}]) && isequal(doseITM,doseJTM)
                doseSortM(end+1) = jSort;
                indJsort = find(doseNumsTmp == jSort);
                doseNumsTmp(indJsort) = [];
            end
        end
    end
    doseIndC{iSort} = doseSortM;
end

doseNums = doseNumsTmp;

%Loop over doses and add over new grid
hWait = waitbar(0,'Summing Dose distributions');
doseSumM = zeros([length(newYgrid),length(newXgrid),length(newZgrid)],'single');
doseEmptyM = zeros([length(newYgrid),length(newXgrid)],'single');

%Assume dose units are same as that of 1st dose
doseUnits = getDoseUnitsStr(doseNums(1),planC);
for i = 1:length(doseNums)

    doseNum = doseNums(i);

    %Check for transM
    if ~isempty(assocTransM) && ~isequal(assocTransM,eye(4))
        doseTtransM = getTransM('dose',doseNum,planC);
        if isempty(doseTtransM)
            doseTtransM = eye(4);
        end
        inputTM = inv(assocTransM) * doseTtransM;
    else
        inputTM = getTransM('dose',doseNum,planC);
        if isempty(inputTM)
            inputTM = eye(4);
        end
    end

    SOPInstanceUIDv = {planC{indexS.beams}.SOPInstanceUID};
    if ~isempty(lqParamS)
        paramS.Tk.val = inf;         %Kick-off time of repopulation (days)
        paramS.Tp.val = NaN;        %Potential tumor doubling time (days)
        paramS.alpha.val = NaN;
        paramS.abRatio.val = lqParamS.abRatio; %10;  %alpha/beta
        paramS.stdFractionSize = lqParamS.stdFractionSize; % 2;
    end

    %Get the summation for this grid
    doseCombinedM = [];
    for iDoseAll = 1:length(doseIndC{doseNum})
        iDose = doseIndC{doseNum}(iDoseAll);
        doseUnits2 = getDoseUnitsStr(iDose,planC);
        if strcmpi(doseUnits, 'Gy') && strcmpi(doseUnits2, 'cGy')
            multFact = 0.01;
        elseif strcmpi(doseUnits, 'cGy') && strcmpi(doseUnits2, 'Gy')
            multFact = 100;
        else
            multFact = 1;
        end
        doseOffset = planC{indexS.dose}(iDose).doseOffset;
        if isempty(doseOffset)
            doseOffset = 0;
        end
        doseArray = single(getDoseArray(planC{indexS.dose}(iDose)) - doseOffset);

        % Apply BED/EQD2 correction
        if ~isempty(lqParamS)
            ReferencedSOPInstanceUID = planC{indexS.dose}(iDose)...
                .DICOMHeaders.ReferencedRTPlanSequence.Item_1.ReferencedSOPInstanceUID;
            planNum = find(strcmpi(ReferencedSOPInstanceUID,SOPInstanceUIDv));
            paramS.numFractions.val = planC{indexS.beams}(planNum(1)).FractionGroupSequence...
                .Item_1.NumberOfFractionsPlanned;
            paramS.numFractions.val = double(paramS.numFractions.val);
            paramS.frxSize.val = doseArray / paramS.numFractions.val;
            doseArray = calc_BED(paramS) / (1+paramS.stdFractionSize/paramS.abRatio.val);
        end

        if ~isempty(doseCombinedM)
            doseCombinedM = doseCombinedM + multFact * wtfactor(iDose) * doseArray;
        else
            doseCombinedM = multFact * wtfactor(iDose) * doseArray;
        end
    end

    %Check if this dose is on same grid as the new one
    %if isequal(xGrid{doseNum},newXgrid) && isequal(yGrid{doseNum},newYgrid) && isequal(zGrid{doseNum},newZgrid)
    chkLength = length(xGrid{doseNum})==length(newXgrid) && length(yGrid{doseNum})==length(newYgrid) && length(zGrid{doseNum})==length(newZgrid);
    if sum(sum((inputTM-eye(4)).^2)) < 1e-3 && chkLength && max(abs(xGrid{doseNum}-newXgrid)) < 1e-3 && max(abs(yGrid{doseNum}-newYgrid)) < 1e-3 && max(abs(zGrid{doseNum}-newZgrid)) < 1e-3

        doseSumM = doseSumM + doseCombinedM;
        waitbar((i-1)/length(doseNums),hWait,['Calculating contribution from Dose ', num2str(doseNum)])

    else %interpolation required

        %Transform this dose
        doseTmpM = [];

        for slcNum=1:length(newZgrid)
            [xV, yV, zV] = getDoseXYZVals(planC{indexS.dose}(doseNum));
            doseTmp = slice3DVol(doseCombinedM, xV, yV, zV, newZgrid(slcNum), 3, 'linear', inputTM, [], newXgrid, newYgrid);
            if isempty(doseTmp)
                doseTmpM(:,:,slcNum) = doseEmptyM;
            else
                doseTmpM(:,:,slcNum) = doseTmp;
            end
            waitbar((i-1)/length(doseNums) + (slcNum-1)/length(newZgrid)/length(doseNums) ,hWait,['Calculating contribution from Dose ', num2str(doseNum)])
        end

        doseSumM = doseSumM + doseTmpM;

    end

end

delete(hWait)

%Create new dose distribution
newDoseNum = length(planC{indexS.dose}) + 1;
planC{indexS.dose}(newDoseNum).doseArray = doseSumM;
clear doseSumM
planC{indexS.dose}(newDoseNum).doseUID = createUID('dose');
planC{indexS.dose}(newDoseNum).assocScanUID = assocScanUID;

%Find minimum value in 3d array, use its negative as the offset
maxDose = max(max(max(planC{indexS.dose}(newDoseNum).doseArray)));
offset = -min(min(min(planC{indexS.dose}(newDoseNum).doseArray)));
if offset > 0
    planC{indexS.dose}(newDoseNum).doseOffset = offset;
    planC{indexS.dose}(newDoseNum).doseArray = planC{indexS.dose}(newDoseNum).doseArray + offset;
end

%set labels on new dose, overwriting some of the copied labels **Check for more labels that need to be replaced
planC{indexS.dose}(newDoseNum).doseNumber = newDoseNum;
planC{indexS.dose}(newDoseNum).fractionGroupID = newDoseName;

%Remove old caching info.
planC{indexS.dose}(newDoseNum).cachedMask = [];
planC{indexS.dose}(newDoseNum).cachedColor = [];
planC{indexS.dose}(newDoseNum).cachedTime = [];

%Set coordinates.
planC{indexS.dose}(newDoseNum).sizeOfDimension1 = length(newXgrid);
planC{indexS.dose}(newDoseNum).sizeOfDimension2 = length(newYgrid);
planC{indexS.dose}(newDoseNum).sizeOfDimension3 = length(newZgrid);
planC{indexS.dose}(newDoseNum).horizontalGridInterval = newXgrid(2)-newXgrid(1);
planC{indexS.dose}(newDoseNum).verticalGridInterval = newYgrid(2)-newYgrid(1);
planC{indexS.dose}(newDoseNum).depthGridInterval = newZgrid(2)-newZgrid(1);
planC{indexS.dose}(newDoseNum).coord1OFFirstPoint = newXgrid(1);
planC{indexS.dose}(newDoseNum).coord2OFFirstPoint = newYgrid(1);
planC{indexS.dose}(newDoseNum).coord3OfFirstPoint = newZgrid(1);
planC{indexS.dose}(newDoseNum).zValues = newZgrid;
planC{indexS.dose}(newDoseNum).doseUnits = doseUnits;


function model = loss_pyramid(h_loss_func, pyra, model, fg_box, ...
                              bg_boxes, min_fg_overlap, max_bg_overlap)
% Computes a pyramid of loss function values for each top-level
% rule in the grammar.
%   model = loss_pyramid(h_loss_func, pyra, model, fg_box, ...
%                        bg_boxes, min_fg_overlap, max_bg_overlap)
%
%   These loss values are used for computing the loss adjusted inference:
%     \max_{s \in S(x)} w \dot \psi(x,s) + L_margin(y,s)
%   The set of valid outputs S(x) is enforced by making L(y,s) = -inf for
%   some values of s, which prevents them from being selected in the
%   maximization.
%
% Return value
%   model           Model augmented to store the computed loss pyramids
%
% Arguments
%   h_loss_func     Handle to loss function
%   model           Model
%                   (augmented with DP tables from gdetect_dp.m)
%   pyra            Feature pyramid
%                   (augmented with overlaps from gdetect_pos_prepare.m)
%   fg_box          Selected foreground bounding box index
%   bg_boxes        Indices of non-selected bounding boxes in image
%   min_fg_overlap  Minimum required amount of overlap with fg box
%   max_bg_overlap  Maximum allowed amount of overlap with bg bounding boxes

% AUTORIGHTS
% -------------------------------------------------------
% Copyright (C) 2011-2012 Ross Girshick
%
% This file is part of the voc-releaseX code
% (http://people.cs.uchicago.edu/~rbg/latent/)
% and is available under the terms of an MIT-like license
% provided in COPYING. Please retain this notice and
% COPYING if you use this file (or a portion of it) in
% your project.
% -------------------------------------------------------

num_bg_boxes = length(bg_boxes);

% For each model component
for comp = 1:length(model.rules{model.start})
  % For each feature pyramid level
  for level = 1:pyra.num_levels
    if pyra.valid_levels(level)
      % Assign loss for root locations based on the selected foreground box
      o = pyra.overlaps(comp).box(fg_box).o{level};
      losses = h_loss_func(o);
      model.rules{model.start}(comp).loss{level} = losses;

      % Require at least some overlap with the foreground bounding box
      % Rationale:
      %  In an image with multiple objects, this constraint encourages a
      %  diverse set of false positives (otherwise, they will tend to come
      %  from the same high-scoring / low-overlapping region of the image
      %  -- i.e. somewhere in the background)
      I = find(o < min_fg_overlap);
      model.rules{model.start}(comp).loss{level}(I) = -inf;

      % Mark root locations that have too much overlap with background boxes
      % as invalid
      % Rationale:
      %   We don't want to select detections of other foreground objects
      %   in the image as false positives (i.e., no true positive should
      %   be allowed to be used as a false positive)
      for b = 1:num_bg_boxes
        o = pyra.overlaps(comp).box(bg_boxes(b)).o{level};
        inds = find(o >= max_bg_overlap);
        model.rules{model.start}(comp).loss{level}(inds) = -inf;
      end
    else
      model.rules{model.start}(comp).loss{level} = 0;
    end
  end
end


%SPH   Implement spherical IBVS for point features
%
%  results = sph(T)
%  results = sph(T, params)
%
%  Simulate IBVS with for a square target comprising 4 points is placed
%  in the world XY plane. The camera/robot is initially at pose T and is
%  driven to the orgin.
%
%  Two windows are shown and animated:
%   1. The camera view, showing the desired view (*) and the
%      current view (o)
%   2. The external view, showing the target points and the camera
%
% The results structure contains time-history information about the image
% plane, camera pose, error, Jacobian condition number, error norm, image
% plane size and desired feature locations.
%
% The params structure can be used to override simulation defaults by
% providing elements, defaults in parentheses:
%
%   target_size    - the side length of the target in world units (0.5)
%   target_center  - center of the target in world coords (0,0,2)
%   niter          - the number of iterations to run the simulation (500)
%   eterm          - a stopping criteria on feature error norm (0)
%   lambda         - gain, can be scalar or diagonal 6x6 matrix (0.01)
%   ci             - camera intrinsic structure (camparam)
%   depth          - depth of points to use for Jacobian, scalar for
%                    all points, of 4-vector.  If null take actual value
%                    from simulation      ([])
%
% SEE ALSO: ibvsplot

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% IMPLEMENTATION NOTE
%
% 1.  As per task function notation (Chaumette papers) the error is
%     defined as actual-demand, the reverse of normal control system
%     notation.
% 2.  The gain, lambda, is always positive
% 3.  The negative sign is written into the control law

function results = sph(T0, params)

    %% default parameters

    % define default target geometry
    vw = 2; vh = 2;       % dimensions of target
    Tct_star = transl(0, 0, -2);    % desired pose of target wrt camera

    % define default IBVS parameters
    niter = 500;            % number of iterations
    eterm = 0;
    lambda = 0.04;         % control gain
    cam = scamera;
    depth = [];
    estim = [];

    if nargin == 2,
        % override default target size
        if isfield(params, 'target_size'),
            vw = params.target_size(1);
            vh = params.target_height(1);
        end
        % override default camera desired pose
        if isfield(params, 'target_center'),
            Tct_star = params.Tct_star;
        end
        % override default number of iterations
        if isfield(params, 'niter'),
            niter = params.niter;
        end
        % override default gain (can be scalar or diag 6x6)
        if isfield(params, 'lambda'),
            lambda = params.lambda;
        end
        % override default camera intrinsics
        if isfield(params, 'ci'),
            ci = params.ci;
        end
        % override default point depth
        if isfield(params, 'depth'),
            if length(params.depth) == 1,
                depth = params.depth * ones(4,1);
            else
                depth = params.depth;
            end
        end
        if isfield(params, 'estim'),
            estim = params.estim;
        end
    end

    % define feature points in XY plane
    %  make vertices of a square
    P = mkgrid(2, [vw vh]);

    % show the reference location, this is the view we wish to achieve
    % when Tc = Tct_star
    figure(1)
    clf
    P
    Tct_star
    % HACKuv_star = cam.plot(P, inv(Tct_star));    % create the camera view
    uv_star = cam.plot(P, (Tct_star));    % create the camera view
    k=1
    plot(uv_star(2,k), uv_star(1,k), '*');
    hold on
    for k=2:4
        plot(uv_star(2,k), uv_star(1,k), '*');
    end
    %plot(uv_star(:,1), uv_star(:,2), '*');  % show desired view
    %hold off
    %cam.plot(P, T0);    % show initial view
    pause(1)

    % this is the 'external' view of the points and the camera
    figure(2)
    plot3(P(1,:), P(2,:), P(3,:), '*')
    showcam(T0, P)
    grid
    xlabel('x');
    ylabel('y');
    zlabel('z');
    pause
    figure(1)
    %cam2 = showcamera(T0);
    %camup([0,-1,0]);

    %% initialize the vservo variables

    Tcam = T0;                % initial camera/robot pose

    % initialize some history variables, suffix _h, to hold time series data
    % about the IBVS run
    v_h = [];       % velocity demand to robot
    e_h = [];       % feature error
    en_h = [];      % scalar norm of the feature error
    c_h = [];       % Jacobian condition number
    uv_h = [];      % image plane feature coordinates
    Tcam_h = [];
    Z_h = [];
    PP = eye(1,1);
    theta = 0;
    smoothing = 0.95;

    for k=1:niter,
         % set and show the camera pose
        cam.Tcam = Tcam;
        %showcamera(cam2, Tcam);
        figure(2)
        plot3(P(1,:), P(2,:), P(3,:), '*')
        grid on
        xlabel('x');
        ylabel('y');
        zlabel('z');
        showcam(Tcam, P);
        figure(1)

        % compute the view
        uv = cam.plot(P);

        e = uv - uv_star;   % as per task function notation
        uv
        uv_star
        e
        if 1
            for i=1:numcols(e)
                % vectorize this
                if e(2,i) > pi
                    e(2,i) = e(2,i) - 2*pi;
                elseif e(2,i) < -pi
                    e(2,i) = 2*pi + e(2,i);
                end
            end
        end
        e
        e = reshape(e, numel(e), 1);

        if estim,
            % run the depth estimator
            if k > 1,

                % compute Jacobian for unit depth, r=1
                J = jac(uv, ones(numcols(uv),1));
                Jv = J(:,1:3);  % velocity part, depends on 1/z
                Jw = J(:,4:6);  % rotational part, indepedent of 1/z

                % estimate image plane velocity
                uv_d =  reshape(uv, 1, [])' - reshape(uv_p, 1, [])';

                % estimate coefficients for A (1/z) = B
                B = uv_d - Jw*v(4:6);
                A = Jv * v(1:3);

                eta = A\B;          % least squares solution

                % first order smoothing
                theta = (1-smoothing) * 1/eta + smoothing * theta;

                pt = transformp(inv(Tcam), P);

                fprintf('depth %.4g, est depth %.4g, rls depth %.4g\n', pt(3,1), 1/eta, theta);
                Z_h(k,:) = [theta pt(3,1)];   % estimated depth, true depth
            end
            uv_p = uv;
        end

        % compute the Jacobian
        if isempty(depth),
            % exact depth from simulation (not possible in practice)
            pt = transformp(inv(Tcam), P);
            J = jac(uv, colnorm(pt) );
        else
            J = jac(uv, depth );
        end
        J

        % compute the velocity of camera in camera frame

        v = -lambda * pinv(J) * e;
        fprintf('v: %.3f %.3f %.3f %.3f %.3f %.3f\n', v);
        norm(e)

        % update the camera pose
        Td = (eye(4,4) + delta2tr( v ) );    % differential motion
        Td = trnorm(Td);

        Tcam = Tcam * Td;       % apply it to current pose

        % update the history variables
        uv_h(k,:) = reshape(uv([2 1],:), 1, []);
        e_h(k,:) = e';
        en_h(k,:) = norm(e);
        c_h(k,:) = cond(J);
        v_h(k,:) = v';
        Tcam_h(:,:,k) = Tcam;

        if norm(e) < eterm,
            fprintf('completed on error tolerance\n');
            break;
        end
        drawnow
        pause(0.05)
        pause
    end
    fprintf('completed on iteration count\n');

    results.uv = uv_h;
    results.uv_labels = {'\phi (rad)', '\theta (rad)'};
    results.c = c_h;
    results.e = e_h;
    results.en = en_h;
    results.v = v_h;
    results.Tcam = Tcam_h;
    %results.limits = [0 cam.nu 0 cam.nv];
    results.limits = [-pi pi 0 pi];
    results.uv_star = uv_star([2 1],:);