classdef DualUpdater_NullSpace < handle

    properties (Access = private)
       constraint
       cost
       designVariable
       dualVariable
       problem
       options
       constraintCase
       nConstr
       dualOld
       isInequality
       position
       parameter
    end

    methods (Access = public)

        function obj = DualUpdater_NullSpace(cParams)
            obj.init(cParams);
            obj.defineConstraintCases();
            obj.defineRangeStepParameterValue(cParams);
        end

        function defineConstraintCases(obj)
            obj.isInequality = false;
            k                = 1;
            for i = 1:obj.nConstr
                switch obj.constraintCase{i}
                    case {'EQUALITY'}

                    case {'INEQUALITY'}
                        obj.isInequality = true;
                        obj.position(k)  = i;
                        k                = k + 1;
                end
            end
        end

        function defineRangeStepParameterValue(obj,cParams)
            switch cParams.optimizerNames.primal
                case {'PROJECTED GRADIENT','HAMILTON JACOBI'}
                    obj.parameter = inf;
                case {'SLERP'}
                    obj.parameter = 5;
                otherwise

            end
        end

        function update(obj)
            if obj.isInequality
                obj.computeQuadraticProblem();
            else
                obj.computeDirectDual();
            end
        end

        function reset(obj)
            obj.dualVariable.value = obj.dualOld;
        end

        function updateOld(obj)
            obj.dualOld = obj.dualVariable.value;
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.cost           = cParams.cost;
            obj.constraint     = cParams.constraint;
            obj.designVariable = cParams.designVar;
            obj.constraintCase = cParams.constraintCase;
            obj.dualVariable   = cParams.dualVariable;
            obj.dualOld        = obj.dualVariable.value;
            obj.nConstr        = cParams.constraint.nSF;
        end

        function computeDirectDual(obj)
            obj.constraint.computeFunctionAndGradient();
            obj.cost.computeFunctionAndGradient();
            DJ = obj.cost.gradient;
            Dh = obj.constraint.gradient;
            h  = obj.constraint.value;
            S  = (Dh'*Dh)^-1;
            aJ = 1;
            aC = 1;
            f  = obj.parameter;
            f2 = 1;
            AC = min(f,f2*aC/aJ*S*h);
            AJ = -aC/aJ*S*Dh'*DJ;
            l  = AC + AJ;
            obj.dualVariable.value = l;
        end

        function computeQuadraticProblem(obj)
            obj.constraint.computeFunctionAndGradient();
            obj.cost.computeFunctionAndGradient();
            obj.computeDualProblemParameters();
            obj.computeDualProblemOptions();
            PROBLEM         = obj.problem;
            PROBLEM.options = obj.options;
            l = quadprog(PROBLEM);
            obj.dualVariable.value = l;
        end

         function computeDualProblemParameters(obj)
            Dg          = obj.constraint.gradient;
            DJ          = obj.cost.gradient;
            g           = obj.constraint.value;
            f           = obj.parameter;
            c           = min(f*(Dg'*Dg),g);
            i           = length(g);
            p           = obj.position;
            prob.H      = Dg'*Dg;
            prob.f      = -c + Dg'*DJ;
            prob.A      = [];
            prob.b      = [];
            prob.Aeq    = [];
            prob.beq    = [];
            prob.lb     = -inf*ones(i,1);
            prob.lb(p)  = 0;
            prob.ub     = inf*ones(length(g),1);
            prob.x0     = zeros(length(prob.H),1);
            prob.solver = 'quadprog';
            obj.problem = prob;
        end

        function computeDualProblemOptions(obj)
            opts = optimoptions("quadprog");
            opts = struct( ...
                'Algorithm','interior-point-convex', ...
                'Diagnostics','off', ...
                'Display','none', ...
                'HessMult',[], ...
                'MaxIter',1e3, ...
                'MaxPCGIter','max(1,floor(numberOfVariables/2))', ...
                'PrecondBandWidth',0, ...
                'ProblemdefOptions', struct, ...
                'TolCon',1e-5, ...
                'TolFun',[], ...
                'TolFunValue', [], ...
                'TolPCG',0.1, ...
                'TolX',100*eps, ...
                'TypicalX','ones(numberOfVariables,1)', ...
                'LinearSolver', 'auto', ...
                'ObjectiveLimit', -1e20 ...
                );
            obj.options = opts;
        end

    end

end


function cicros(opt)
    %   This subroutine "circle"  selects the Ni closest earthquakes
    %   around a interactively selected point.  Resets ZG.newcat and ZG.newt2
    %   Operates on "primeCatalog".
    %
    % axis: h1
    % plots to: plos1 as xk
    % inCatalog: a
    % outCatalog: newt2, newcat, newa, newa2
    % mouse controlled
    % closest events OR radius
    % calls: bdiff
    %
    %  Input Ni:
    %
    persistent ic

    report_this_filefun();
    ZG=ZmapGlobal.Data;

    if opt==0 && ~isempty(ic)
        opt=ic;
    end

    axes(h1)

    titStr ='Selecting EQ in Circles                         ';
    messtext= ...
        ['                                                '
        '  Please use the LEFT mouse button              '
        ' to select the center point.                    '
        ' The "ni" events nearest to this point          '
        ' will be selected and displayed in the map.     '];

    msg.dbdisp(messtext, titStr);

    % Input center of circle with mouse
    %
    [xa0,ya0]  = ginput(1);

    stri1 = [ 'Circle: lon = ' num2str(xa0) '; lat= ' num2str(ya0)];
    stri = stri1;
    pause(0.1)
    %  calculate distance for each earthquake from center point
    %  and sort by distance
    %
    l = sqrt(((xsecx' - xa0)).^2 + ((xsecy + ya0)).^2) ;
    [s,is] = sort(l);
    ZG.newt2 = newa(is(:,1),:) ;

    switch(ic)
        case 1 % select  N clostest events

            l =  sort(l);
            messtext = ['Radius of selected Circle:' num2str(l(ni))  ' km' ];
            disp(messtext)
            msg.dbdisp(messtext, 'Message')
            %
            % take first ni and sort by time
            %
            ZG.newt2 = ZG.newt2(1:ni,:);
            ZG.newt2.sort('Date');
            %
            % plot Ni clostest events on map as 'x':

            set(gca,'NextPlot','add')
            [na,ma] = size(ZG.newt2);
            plot(ZG.newt2(:,ma),-ZG.newt2.Depth,'xk','Tag','plos1');
            set(gcf,'Pointer','arrow')
            %
            % plot circle containing events as circle
            x = -pi-0.1:0.1:pi;
            plot(xa0+sin(x)*l(ni), ya0+cos(x)*l(ni),'w')
            l(ni)

            %
            ZG.newcat = ZG.newt2;                   % resets ZG.newcat and ZG.newt2

            clear l s is
            bdiff(ZG.newt2)

        case 2 % select  events within ra

            l =  sort(l);
            ll = l <=ra;
            messtext = ['Number of events in Circle :' num2str(sum(ll)) ];
            disp(messtext)
            msg.dbdisp(messtext, 'Message')
            %
            % take first ni and sort by time
            %
            ZG.newt2 = ZG.newt2.subset(ll);
            ZG.newt2.sort('Date');
            %
            % plot Ni clostest events on map as 'x':

            set(gca,'NextPlot','add')
            [na,ma] = size(ZG.newt2);
            plot(ZG.newt2(:,ma),-ZG.newt2.Depth,'xk','Tag','plos1');
            set(gcf,'Pointer','arrow')
            %
            % plot circle containing events as circle
            x = -pi-0.1:0.1:pi;
            plot(xa0+sin(x)*ra, ya0+cos(x)*ra,'w')
            l(ni)

            %
            ZG.newcat = ZG.newt2;                   % resets ZG.newcat and ZG.newt2

            clear l s is
            bdiff(ZG.newt2)

        case 3 % select  events within ra

            ax=findobj(gcf,'Tag','mainmap_ax');
            [x,y, mouse_points_overlay] = select_polygon(ax);
            plot(x,y,'b-');
            YI = -newa(:,7);          % this substitution just to make equation below simple
            XI = newa(:,end);
            ll = polygon_filter(x,y, XI, YI, 'inside');

            %plot the selected eqs and mag freq curve
            newa2 = newa.subset(ll);
            ZG.newt2 = newa2;
            ZG.newcat = newa.subset(ll);
            pl = plot(newa2(:,end),-newa2(:,7),'xk');
            set(pl,'MarkerSize',5,'LineWidth',1)
            bdiff(newa2)
        otherwise
            error('no option specified. 1: N closest events within radius, 2: radius')
    end
    ic=opt;
end


filename = 'Cantilever';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1];
constraint = {'volumeConstraint'};
constraint_case = 'EQUALITY';
optimizerUnconstrained = 'SLERP';
optimizer = 'AlternatingPrimalDual';
incrementFactor = 1.05;
designVariable = 'LevelSet';
filterType = 'PDE';
line_search_initiator = 'INCREASING LAST STEP';

nsteps = 1;
Vfrac_final = 0.3;
optimality_final =1e-3;
constr_final = 1e-5;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
Perimeter_target = 5;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;

plotting = false;
printing = false;
printing_physics = false;
monitoring = false;
monitoring_interval = 10;
maxiter = 15;


 function g = gibbsKernDiagGradient(kern, x, covDiag)

% GIBBSKERNDIAGGRADIENT Compute the gradient of the GIBBS kernel's diagonal wrt parameters.
% FORMAT
% DESC computes the gradient of functions of the diagonal of the
% Mark Gibbs's non-stationary kernel matrix with respect to the parameters of the kernel. The
% parameters' gradients are returned in the order given by the
% gibbsKernExtractParam command.
% ARG kern : the kernel structure for which the gradients are
% computed.
% ARG x : the input data for which the gradient is being computed.
% ARG factors : partial derivatives of the function of interest with
% respect to the diagonal elements of the kernel.
% RETURN g : gradients of the relevant function with respect to each
% of the parameters. Ordering should match the ordering given in
% gibbsKernExtractParam.
%
% SEEALSO : gibbsKernParamInit, kernDiagGradient, gibbsKernExtractParam, gibbsKernGradient
%
% COPYRIGHT : Neil D. Lawrence, 2006

% KERN

g = zeros(1, kern.nParams);
g(end) = sum(covDiag);


function [imageCha, ssim_map, metrics] = algo_ADMM_proxA_2D( obj,input )
% 2D variant of ADMM
% based on Afonso et al. paper on SALSA
%
% input:
% obj           CS reconstruction object (holding all parameters)
% input         struct containing recon parameters and image
%
% output:
% imageCha      reconstructed channel individual image
% ssim_map      structural similarity map
% metrics       evaluation metrics
%
% (c) Marc Fischer, Thomas Kuestner, May 2015
% -------------------------------------------------------------------------

%%
timer_proxA = tic;

%% variables:
% internal flags
flag_wavetree = true;
flag_fast = true;
flag_extendImage = true;

% internal variables:
L = 1;
t_old = 1;
NLTV_struct.kernelratio = 3;
NLTV_struct.windowratio = 6;
NLTV_struct.nThreads = 1;  % mind this option if used with -singleCompThread on BWCluster
itrNLTV = obj.iNINNER - 20;
% chambolle tv:
parsin.MAXITER=100; parsin.tv='iso'; % 'iso' or 'l1'

% from obj:
% maxitr = obj.maxitr;
maxitr = obj.iNINNER;
% n1 = obj.measPara.dim(1);
% n2 = obj.measPara.dim(2);
n1 = input.n1;
n2 = input.n2;
% nSlices = obj.measPara.dim(3);
nCha = obj.measPara.dim(5);
lambdaWave = obj.lambda;
lambdaTV = obj.lambdaTV;
lambdaGroup = obj.lambdaGroup;
NLTV_struct.filterstrength = obj.lambdaNLTV_h; % 0.03 % converted from NLTV h = 0.01 %old: used: 3e-10
lambdaNLTV = obj.lambdaNLTV;
lambdaNLTV_h = obj.lambdaNLTV_h;
mue = obj.mue;
regularizerWeights = obj.regularizerWeights;
flagTV = obj.flagTV;
flagTV_iso = obj.flagTV_iso;
flagWave = obj.flagWave;
flagGroup = obj.flagGroup;
flagNLTV = obj.flagNLTV;
flagSBNLTV = obj.flagSBNLTV;
flagRealAndImag = obj.flagRealAndImag;
waveletStages = obj.trafo.waveletStages;
waveletFilterName_l1 = obj.trafo.waveletFilterName_l1;
waveletFilterName_l12 = obj.trafo.waveletFilterName_l12;

% from input:
b=input.b;
mask = input.mask;
G_prox = input.G_prox;
Gt_prox = input.Gt_prox;
groupnorm_index = input.groupnorm_index;
waveS_l1 = input.waveS_l1;
waveS_l12 = input.waveS_l12;
waveS_l12proxA = input.waveS_l12proxA;
proxA_extend_y = waveS_l12proxA(waveletStages+2,1) - waveS_l12(waveletStages+2,1);
proxA_extend_x = waveS_l12proxA(waveletStages+2,2) - waveS_l12(waveletStages+2,2);
x_proxA = cell(1,nCha);

% im_ref = input.im_ref;
% im_ref_full = zeros(n1,n2);
% for j = 1:nCha
%     im_ref_full = im_ref_full + abs(im_ref{1,j}).^2;
% end;
% im_ref_full = sqrt(im_ref_full);
clear input

% initialize cells/vectors
for j=1:nCha
    FTb{1,j} = iFFT2D(b{1,j});
    x_cmplx{1,j} = real(FTb{1,j});
    x_cmplx{1,j+nCha} = imag(FTb{1,j});
end;
% starting point:
x = FTb; % (x0 = FTb)
rhs = FTb; % different start (x0 = F^-1 K^-1 F FTb); %used atm
% rhs: right hand side

x_g_helper = cell(2,nCha);
x_wave =  cell(1,2*nCha);
x_g_proxA = x_wave;
x_wave_helper = x_wave;
v_wave = x_wave;
d_wave = x_wave;
x_tv = x_wave;
v_tv = x_wave;
d_tv = x_wave;
x_group = x_wave;
v_group = x_wave;
d_group = x_wave;
x_nltv = x_wave;
v_nltv = x_wave;
d_nltv = x_wave;
K_inv = x_wave;
for j = 1:2*nCha
    x_wave{1,j} = 0;
    v_wave{1,j} = 0;
    d_wave{1,j} = 0;
    x_tv{1,j} = zeros(n1,n2);
    v_tv{1,j} = 0;
    d_tv{1,j} = 0;
    x_group{1,j} = 0;
    v_group{1,j} = 0;
    d_group{1,j} = 0;
    x_nltv{1,j} = 0;
    v_nltv{1,j} = 0;
    d_nltv{1,j} = 0;
end;

%% MAD dependent lambdas:
flag_MAD = true;
if flag_MAD
    x_wavedec = cell(1,nCha);
    threshold = zeros(1:nCha);
    if flagRealAndImag
        for j=1:nCha
            x_wavedec{1,j} = wavedec2(abs(x{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wave_fine_scale = size(x_wavedec{1,j},2) - (3*waveS_l1(waveletStages+1,1)*waveS_l1(waveletStages+1,2));
            threshold(j) = mad(x_wavedec{1,j}(x_wave_fine_scale:end),1);
        end;
    else
        for j=1:nCha
            x_wavedec{1,j} = wavedec2(real(x{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wavedec{1,j+nCha} = wavedec2(imag(x{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wave_fine_scale_real = size(x_wavedec{1,j},2) - (3*waveS_l1(waveletStages+1,1)*waveS_l1(waveletStages+1,2));
            threshold(j) = mad(x_wavedec{1,j}(x_wave_fine_scale_real:end),1);
            x_wave_fine_scale_imag = size(x_wavedec{1,j},2) - (3*waveS_l1(waveletStages+1,1)*waveS_l1(waveletStages+1,2));
            threshold(j+nCha) = mad(x_wavedec{1,j}(x_wave_fine_scale_imag:end),1);
        end;
    end;
    clear x_wavedec
else
    threshold(1:nCha) = 1;
end;

if flagRealAndImag
    threshold_group = 0;
    for j = 1:nCha
        threshold_wave(j) = lambdaWave * threshold(j)/2 * 2/L;
        threshold_TV(j) = lambdaTV * threshold(j) * 2/L;
        threshold_TV(j+nCha) = lambdaTV * threshold(j) * 2/L;
        threshold_group = threshold_group + lambdaGroup * threshold(j) * 2/L;
        threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
        threshold_NLTV(j+nCha) = lambdaNLTV;
        threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01
        threshold_NLTV_h(j+nCha) = lambdaNLTV_h;
    end;
    threshold_group = threshold_group /3;
else
    threshold_group_real = 0;
    threshold_group_imag = 0;
    for j = 1:nCha
        threshold_wave(j) = lambdaWave * threshold(j) * 2/L;
        threshold_wave(j+nCha) = lambdaWave * threshold(j+nCha) * 2/L;
        threshold_TV(j) = lambdaTV * threshold(j) * 2/L;
        threshold_TV(j+nCha) = lambdaTV * threshold(j+nCha) * 2/L;
        threshold_group_real = threshold_group_real + lambdaGroup * threshold(j) * 2/L;
        threshold_group_imag = threshold_group_imag + lambdaGroup * threshold(j) * 2/L;
        threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
        threshold_NLTV(j+nCha) = lambdaNLTV;
        threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01
        threshold_NLTV_h(j+nCha) = lambdaNLTV_h;
    end;
    threshold_group_real = threshold_group_real /3;
    threshold_group_imag = threshold_group_imag /3;
end;

%% initialize metrics:
    itr = 0;
   	metrics.xtime(itr+1)= 0;
%     [metrics, ssim_map{1,1}] = get_metrics_itr( im_ref, im_ref_full, x, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma );
    ssim_map = [];

%% recon
% inverse for x - step:
for j = 1:nCha
    K_inv{1,j} = 1./(mue*mask + (flagWave*regularizerWeights(1) + flagTV*regularizerWeights(2) +  flagGroup*regularizerWeights(3))*ones(n1,n2)); % flagNLTV changes K_inv -> in separate
end;

dispProgress('ADMM', 0, maxitr);
for itr = 1:maxitr  % total iter counter

%% x - step
    if itr > 1 % for i = 1 already set (x{1,j} = FTb{1,j})
        for j = 1:2*nCha
            % rhs part:
            if flagWave
                x_wave{1,j} = waverec2((v_wave{1,j} - d_wave{1,j}),waveS_l1,waveletFilterName_l1); end;
            if flagTV
                x_tv{1,j} = v_tv{1,j} - d_tv{1,j}; end;
            if flagGroup
                x_g_proxA{1,j} = waverec2((v_group{1,j} - d_group{1,j}),waveS_l12proxA,waveletFilterName_l12);
                x_group{1,j} =  x_g_proxA{1,j}(1:end-proxA_extend_y,1:end-proxA_extend_x);
            end;
            if flagNLTV
                if itr == itrNLTV
                    K_inv{1,j} = 1./(mue*mask + (flagWave*regularizerWeights(1) + flagTV*regularizerWeights(2) +  flagGroup*regularizerWeights(3) + flagNLTV*regularizerWeights(4))*ones(n1,n2));
                    x_nltv{1,j} = x{1,j};
                elseif itr >= itrNLTV
                    x_nltv{1,j} = v_nltv{1,j} - d_nltv{1,j};
                end;
            end;
        end;
        for j = 1:nCha
            rhs{1,j} = mue*FTb{1,j} + ( flagWave*regularizerWeights(1).*(x_wave{1,j}+1i*x_wave{1,j+nCha}) + flagTV*regularizerWeights(2).*(x_tv{1,j}+1i*x_tv{1,j+nCha}) + flagGroup*regularizerWeights(3).*(x_group{1,j}+1i*x_group{1,j+nCha}) + flagNLTV*regularizerWeights(4).*(x_nltv{1,j}+1i*x_nltv{1,j+nCha}));
            x{1,j} = iFFT2D(K_inv{1,j}.*FFT2D(rhs{1,j}));
            x_cmplx{1,j} = real(x{1,j});
            x_cmplx{1,j+nCha} = imag(x{1,j});
        end;
    end;

%% auxiliary variables:
%% l1-Wavelet
    if flagWave
        for j = 1:2*nCha
            x_wave_helper{1,j} = wavedec2(x_cmplx{1,j},waveletStages,waveletFilterName_l1);
        end;
        if flagRealAndImag
            for j = 1:nCha
                [v_wave{1,j}, v_wave{1,j+nCha}] = groupthresh_2vec(x_wave_helper{1,j} + d_wave{1,j},x_wave_helper{1,j+nCha} + d_wave{1,j+nCha},threshold_wave(j));
            end;
        else
            for j = 1:2*nCha
                v_wave{1,j} = softthresh_real(x_wave_helper{1,j} + d_wave{1,j},threshold_wave(j));
            end;
        end;
        for j = 1:2*nCha
            d_wave{1,j} = d_wave{1,j} + x_wave_helper{1,j} - v_wave{1,j};
        end;
    end;

%% TV:
    if flagTV
        if ~flagTV_iso
            for j = 1:2*nCha
                    v_tv{1,j} = MTV_2D((x_cmplx{1,j} + d_tv{1,j}), threshold_TV(j), n1, n2);
            end;
        else
            for j = 1:2*nCha
                if (itr==1)
                    [v_tv{1,j}, P]=denoise_TV_One((x_cmplx{1,j} + d_tv{1,j}), threshold_TV(j),-inf,inf,[],parsin);
                else
                    [v_tv{1,j}, P]=denoise_TV_One((x_cmplx{1,j} + d_tv{1,j}), threshold_TV(j),-inf,inf,P,parsin);
                end;
            end;
        end;
        d_tv{1,j} = d_tv{1,j} + x_cmplx{1,j} - v_tv{1,j};
    end;

%% NLTV
    if flagNLTV
        if itr >= itrNLTV
            if flagSBNLTV
                for j = 1:2*nCha
                    v_nltv{1,j} = SB_NLTVfunc_slim_rescale( (x_cmplx{1,j} + d_nltv{1,j}),n1,n2, threshold_NLTV(j), threshold_NLTV_h(j) );
                end;
            else
                for j = 1:2*nCha
                    % if mod(itr,5) == 0 || itr == 1
                    v_nltv{1,j} = NLMF((x_cmplx{1,j} + d_nltv{1,j}),NLTV_struct);
                    v_nltv{1,j} = (L.*(x_cmplx{1,j} + d_nltv{1,j}) + 2*threshold_NLTV(j)*v_nltv{1,j})./(L+2*threshold_NLTV(j));
                    % end;
                end;
            end;
            d_nltv{1,j} = d_nltv{1,j} + x_cmplx{1,j} - v_nltv{1,j};
        else
            v_nltv{1,j} = 0;
            d_nltv{1,j} = 0;
        end;

    end;

%% l12-Wavelet
    if flagGroup
        for j = 1:2*nCha
            if flag_extendImage
                x_proxA{1,j} = extend_image(x_cmplx{1,j}, waveS_l12proxA, waveletStages, proxA_extend_y, proxA_extend_x);
                x_g_helper{1,j} = wavedec2(x_proxA{1,j},waveletStages,waveletFilterName_l12) + d_group{1,j};
            else
                x_g_helper{1,j} = wavedec2(x_cmplx{1,j},waveletStages,waveletFilterName_l12) + d_group{1,j};
            end;

            if flag_wavetree
                x_g_helper{2,j} = (G_prox*x_g_helper{1,j}')';
            else
                x_g_helper{2,j} = zeros(1,size(x_g_helper{1,j},2));
            end;
        end;
        if flagRealAndImag
            v_group(1,:) = softthresh_proxA_cha(x_g_helper,threshold_group,2*nCha,waveS_l12proxA,groupnorm_index);
        else
            v_group(1,1:nCha) = softthresh_proxA_cha(x_g_helper(:,1:nCha),threshold_group_real,nCha,waveS_l12proxA,groupnorm_index);
            v_group(1,nCha+1:2*nCha) = softthresh_proxA_cha(x_g_helper(:,1:nCha),threshold_group_imag,nCha,waveS_l12proxA,groupnorm_index);
        end;
        for j = 1:2*nCha
            d_group{1,j} = d_group{1,j} + x_g_helper{1,j} - v_group{1,j};
        end;
    end;

%% metrics of current itr:
%     disp(itr);
    dispProgress('ADMM', itr/maxitr);

   	metrics.xtime(itr+1)= toc(timer_proxA);
%     [metrics, ssim_map{1,2}] = get_metrics_itr( im_ref, im_ref_full, x, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma );

end;
dispProgress('ADMM', 'Close');

    imageCha = x;
    for j = 1:nCha
        imageCha{1,j} = turn_image( imageCha{1,j} );
    end;
%     for j = 1:nCha+1
%         ssim_map{1,1}{1,j} = turn_image( ssim_map{1,1}{1,j} );
%         ssim_map{1,2}{1,j} = turn_image( ssim_map{1,2}{1,j} );
%     end;

end


function exp_map_iteration(vt_id, transV, yawRotV, heightV, xGc, yGc, zGc, curYawHdc, curHeight)
%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     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 3 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, see <http://www.gnu.org/licenses/>.

    %% define some variables

    %%% some variables of em

    % define a variable of experiences
    global EXPERIENCES;

    % current experience id
    global CUR_EXP_ID;

    % the number of experiences
    global NUM_EXPS;

    % the min change of em between previous e with current e
    global MIN_DELTA_EM;

    % the experience delta threshold
    global DELTA_EXP_GC_HDC_THRESHOLD;

    %%% some variables of vt

    % define a variable of visual template
    global VT;

    % the previous visual template id
    global PREV_VT_ID;

    %%% some variables of 3D GC

    % The x, y, z dimension of 3D Grid Cells Model (3D CAN)
    global GC_X_DIM;
    global GC_Y_DIM;
    global GC_Z_DIM;

    %%% some variables of 3D HDC

    % The dimension of yaw in yaw_height_hdc network
    global YAW_HEIGHT_HDC_Y_DIM;

    % The dimension of height in yaw_height_hdc network
    global YAW_HEIGHT_HDC_H_DIM;

    %%% computing delta x,y,z, yaw, height

    % integrate the delta x, y, z, yaw, height
    % Rad radian  accumulative
    global ACCUM_DELTA_X;
    global ACCUM_DELTA_Y;
    global ACCUM_DELTA_Z;
    global ACCUM_DELTA_YAW;   % accum_delta_facing
%     global ACCUM_DELTA_HEIGHT;

    ACCUM_DELTA_YAW = clip_radian_180(ACCUM_DELTA_YAW + yawRotV);
%     ACCUM_DELTA_HEIGHT = mod(ACCUM_DELTA_HEIGHT + heightV, YAW_HEIGHT_HDC_H_DIM);
%
    ACCUM_DELTA_X = ACCUM_DELTA_X + transV * cos(ACCUM_DELTA_YAW);
    ACCUM_DELTA_Y = ACCUM_DELTA_Y + transV * sin(ACCUM_DELTA_YAW);
    ACCUM_DELTA_Z = ACCUM_DELTA_Z + heightV;

    % trajectory of delta of em
    global DELTA_EM;

    minDeltaX = get_min_delta(EXPERIENCES(CUR_EXP_ID).x_gc, xGc, GC_X_DIM);
    minDeltaY = get_min_delta(EXPERIENCES(CUR_EXP_ID).y_gc, yGc, GC_Y_DIM);
    minDeltaZ = get_min_delta(EXPERIENCES(CUR_EXP_ID).z_gc, zGc, GC_Z_DIM);

    minDeltaYaw = get_min_delta(EXPERIENCES(CUR_EXP_ID).yaw_hdc, curYawHdc, YAW_HEIGHT_HDC_Y_DIM);
    minDeltaHeight = get_min_delta(EXPERIENCES(CUR_EXP_ID).height_hdc, curHeight, YAW_HEIGHT_HDC_H_DIM);

    minDeltaYawReversed = get_min_delta(EXPERIENCES(CUR_EXP_ID).yaw_hdc, (YAW_HEIGHT_HDC_Y_DIM /2) - curYawHdc, YAW_HEIGHT_HDC_Y_DIM);
    minDeltaYaw = min(minDeltaYaw, minDeltaYawReversed);

%     minDeltaHeightReversed = get_min_delta(EXPERIENCES(CUR_EXP_ID).height_hdc, (YAW_HEIGHT_HDC_H_DIM /2) - curHeight, YAW_HEIGHT_HDC_H_DIM);
%     minDeltaHeight = min(minDeltaHeight, minDeltaHeightReversed);

    delta_em = sqrt((minDeltaX).^2 + (minDeltaY).^2 + (minDeltaZ).^2 + (minDeltaYaw).^2 + (minDeltaHeight).^2);
    DELTA_EM = [DELTA_EM delta_em];

    % if the visual template is new or the 3d grid cells (x,y,z) and head direction cells (yaw, height)
    % has changed enough create a new experience
    if VT(vt_id).numExp == 0 || delta_em > DELTA_EXP_GC_HDC_THRESHOLD

        NUM_EXPS = NUM_EXPS + 1;
        create_new_exp(CUR_EXP_ID, NUM_EXPS, vt_id, xGc, yGc, zGc, curYawHdc, curHeight);

        PREV_EXP_ID = CUR_EXP_ID;
        CUR_EXP_ID = NUM_EXPS;

        ACCUM_DELTA_X = 0;
        ACCUM_DELTA_Y = 0;
        ACCUM_DELTA_Z = 0;

        ACCUM_DELTA_YAW = EXPERIENCES(CUR_EXP_ID).yaw_exp_rad;
%         ACCUM_DELTA_HEIGHT = EXPERIENCES(CUR_EXP_ID).height_hdc;

        % if the visual template has changed (but isn't new) search for the matching experience
    elseif vt_id ~= PREV_VT_ID

        % find the experience associated with the current visual template and that is under the
        % threshold distance to the centre of grid cell and head direction cell activity
        % if multiple experiences are under the threshold then don't match (to reduce hash collisions)
        matched_exp_id = 0;
        matched_exp_count = 0;

        for search_id = 1:VT(vt_id).numExp

            minDeltaYaw = get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).yaw_hdc, curYawHdc, YAW_HEIGHT_HDC_Y_DIM);
%             minDeltaYawReversed = get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).yaw_hdc, (YAW_HEIGHT_HDC_Y_DIM /2) - curYawHdc, YAW_HEIGHT_HDC_Y_DIM);
%             minDeltaYaw = min(minDeltaYaw, minDeltaYawReversed);

            minDeltaHeight = get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).height_hdc, curHeight, YAW_HEIGHT_HDC_Y_DIM);
%             minDeltaHeightReversed = get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).height_hdc, (YAW_HEIGHT_HDC_Y_DIM /2) - curHeight, YAW_HEIGHT_HDC_H_DIM);
%             minDeltaHeight = min(minDeltaHeight, minDeltaHeightReversed);

            delta_em(search_id) = sqrt(get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).x_gc, xGc, GC_X_DIM)^2 ...
            + get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).y_gc, yGc, GC_Y_DIM)^2 ...
            + get_min_delta(EXPERIENCES(VT(vt_id).EXPERIENCES(search_id).id).z_gc, yGc, GC_Z_DIM)^2 ...
            + minDeltaYaw^2 + minDeltaHeight^2) ;

            if delta_em(search_id) < DELTA_EXP_GC_HDC_THRESHOLD
               matched_exp_count = matched_exp_count + 1;
            end
        end

        if matched_exp_count > 1
            % this means we aren't sure about which experience is a match due to hash table collision
            % instead of a false posivitive which may create blunder links in
            % the experience map keep the previous experience

        else
            [min_delta, min_delta_id] = min(delta_em);

            MIN_DELTA_EM = [MIN_DELTA_EM; min_delta];
            if min_delta < DELTA_EXP_GC_HDC_THRESHOLD

                matched_exp_id = VT(vt_id).EXPERIENCES(min_delta_id).id;

                % see if the previous experience already has a link to the current experience
                link_exists = 0;
                for link_id = 1 : EXPERIENCES(CUR_EXP_ID).numlinks
                    if EXPERIENCES(CUR_EXP_ID).links(link_id).exp_id == matched_exp_id
                        link_exists = 1;
                        break;
                    end
                end

                % if we didn't find a link then create the link between current
                % experience and the experience for the current visual template
                if link_exists == 0
                    EXPERIENCES(CUR_EXP_ID).numlinks = EXPERIENCES(CUR_EXP_ID).numlinks + 1;
                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).exp_id = matched_exp_id;
                    %  EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).d_xy = sqrt(ACCUM_DELTA_X^2 + ACCUM_DELTA_Y^2 + ACCUM_DELTA_Z^2);
                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).d_xy = sqrt(ACCUM_DELTA_X^2 + ACCUM_DELTA_Y^2);
                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).d_z = ACCUM_DELTA_Z;

                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).heading_yaw_exp_rad = ...
                        get_signed_delta_radian(EXPERIENCES(CUR_EXP_ID).yaw_exp_rad, atan2(ACCUM_DELTA_Y, ACCUM_DELTA_X)); % heading is the delta angle between current pose of exp and previous pose of exp

                    EXPERIENCES(CUR_EXP_ID).links(EXPERIENCES(CUR_EXP_ID).numlinks).facing_yaw_exp_rad = ... % facing is the direction of each exp
                        get_signed_delta_radian(EXPERIENCES(CUR_EXP_ID).yaw_exp_rad, ACCUM_DELTA_YAW);

                 end

            end

            % if there wasn't an experience with the current visual template and grid cell (x y z) and head direction cell (yaw, height)
            % then create a new experience
            if matched_exp_id == 0
                NUM_EXPS = NUM_EXPS + 1;
                create_new_exp(CUR_EXP_ID, NUM_EXPS, vt_id, xGc, yGc, zGc, curYawHdc, curHeight);
                matched_exp_id = NUM_EXPS;
            end

            PREV_EXP_ID = CUR_EXP_ID;
            CUR_EXP_ID = matched_exp_id;

            ACCUM_DELTA_X = 0;
            ACCUM_DELTA_Y = 0;
            ACCUM_DELTA_Z = 0;
            ACCUM_DELTA_YAW = EXPERIENCES(CUR_EXP_ID).yaw_exp_rad;
%             ACCUM_DELTA_HEIGHT = EXPERIENCES(CUR_EXP_ID).height_hdc;
        end

    end

    global EXP_CORRECTION;
    global EXP_LOOPS;

    % do the experience map correction interatively for all the links in all the experiences
    for i = 1:EXP_LOOPS

        for exp_id = 1:NUM_EXPS

            for link_id=1:EXPERIENCES(exp_id).numlinks

                % experience 0 has a link to experience 1
                e0 = exp_id;
                e1 = EXPERIENCES(exp_id).links(link_id).exp_id;

                % work out where e0 thinks e1 (x,y) should be based on the stored link information
                lx = EXPERIENCES(e0).x_exp + EXPERIENCES(e0).links(link_id).d_xy * cos(EXPERIENCES(e0).yaw_exp_rad + EXPERIENCES(e0).links(link_id).heading_yaw_exp_rad);
                ly = EXPERIENCES(e0).y_exp + EXPERIENCES(e0).links(link_id).d_xy * sin(EXPERIENCES(e0).yaw_exp_rad + EXPERIENCES(e0).links(link_id).heading_yaw_exp_rad);
                lz = EXPERIENCES(e0).z_exp + EXPERIENCES(e0).links(link_id).d_z;  %

                % correct e0 and e1 (x,y) by equal but opposite amounts
                % a 0.5 correction parameter means that e0 and e1 will be fully
                % corrected based on e0's link information
                EXPERIENCES(e0).x_exp = EXPERIENCES(e0).x_exp + (EXPERIENCES(e1).x_exp - lx) * EXP_CORRECTION;
                EXPERIENCES(e0).y_exp = EXPERIENCES(e0).y_exp + (EXPERIENCES(e1).y_exp - ly) * EXP_CORRECTION;
                EXPERIENCES(e0).z_exp = EXPERIENCES(e0).z_exp + (EXPERIENCES(e1).z_exp - lz) * EXP_CORRECTION;

                EXPERIENCES(e1).x_exp = EXPERIENCES(e1).x_exp - (EXPERIENCES(e1).x_exp - lx) * EXP_CORRECTION;
                EXPERIENCES(e1).y_exp = EXPERIENCES(e1).y_exp - (EXPERIENCES(e1).y_exp - ly) * EXP_CORRECTION;
                EXPERIENCES(e1).z_exp = EXPERIENCES(e1).z_exp - (EXPERIENCES(e1).z_exp - lz) * EXP_CORRECTION;

                % determine the angle between where e0 thinks e1's facing
                % should be based on the link information
                TempDeltaYawFacing = get_signed_delta_radian((EXPERIENCES(e0).yaw_exp_rad + EXPERIENCES(e0).links(link_id).facing_yaw_exp_rad), EXPERIENCES(e1).yaw_exp_rad);

                % correct e0 and e1 facing by equal but opposite amounts
                % a 0.5 correction parameter means that e0 and e1 will be fully
                % corrected based on e0's link information
                EXPERIENCES(e0).yaw_exp_rad = clip_radian_180(EXPERIENCES(e0).yaw_exp_rad + TempDeltaYawFacing * EXP_CORRECTION);
                EXPERIENCES(e1).yaw_exp_rad = clip_radian_180(EXPERIENCES(e1).yaw_exp_rad - TempDeltaYawFacing * EXP_CORRECTION);

            end
        end

    end

    % keep a frame by frame history of which experience was currently active
    global EXP_HISTORY;
    EXP_HISTORY = [EXP_HISTORY; CUR_EXP_ID];
end


% This script train an LSTM or DNN to predict complex Fourier transform
% domain temporal filters, which will be used to filter the noisy and
% reverberant Fourier coefficients to produce enhanced speech. The script
% is based on the simulated data of Reverb challenge.
%
% Xiong Xiao, Nanyang Technological University, Singapore
% Last modified: 10 Feb 2016.
%
%clear
function TrainDereverbNet_RegressionComplex(modelType, hiddenLayerSize, hiddenLayerSizeFF, DeltaGenerationType, learning_rate, ...
    step, nSequencePerMinibatch, useFileName, useGPU)

addpath('local', '../beamforming/lib');     % remember to run addMyPath.m in SignalGraph root directory first.
% define SignalGraph settings. Type ParseOptions2() in command line to see the options.
para.IO.nStream = 2;        % two data streams, one is input signal, one is clean target log spectrogram
para.NET.sequential = 1;    % do not randomize at frame level, randomize at sentence level
para.NET.variableLengthMinibatch = 1;   % a minibatch may contain multiple sentences of different lengths
para.NET.nSequencePerMinibatch = nSequencePerMinibatch;    % number of sentences per minibatch
para.NET.maxNumSentInBlock = 100;       % maximum number of sentences in a block
para.NET.L2weight = 3e-4;
para.NET.learning_rate = learning_rate;
para.NET.learning_rate_decay_rate = 0.999;
para.NET.momentum = [0.9];
para.NET.gradientClipThreshold = 1;
para.NET.weight_clip = 10;
para.useGPU     = useGPU;                  % don't use GPU for single sentence minibatch for LSTM. It's even slower than CPU.
para.minItr     = 10;
para.displayGPUstatus = 1;
para.displayInterval = 1;
para.skipInitialEval = 1;
para.displayTag = 'LSTM-Regression';

reverb_root = ChoosePath4OS({'D:/Data/REVERB_Challenge', '/media/xiaoxiong/OS/data1/G/REVERB_Challenge'});   % you can set two paths, first for windows OS and second for Linux OS.
para.local.wavroot = [reverb_root];
para.local.wsjcam0root = ChoosePath4OS({'D:/Data/wsjcam0', '/media/xiaoxiong/OS/data1/G/wsjcam0_wav'});
para.local.wsjcam0ext = ChoosePath4OS({'wav', 'wv1'});
para.local.useFileName = useFileName;      % if set to 0, load all training data to memory. otherwise, only load file names.
para.local.loadLabel = 0;
para.local.seglen = 100;
para.local.segshift = 100;

para.topology.useChannel = 1;
para.topology.useWav = 1;
para.topology.FilterNetType = modelType;
% There are 257 filters, one for each frequency bin. Each filter has a 2D
% complex-valued weight matrix, with size TempContext x FreqContext.
para.topology.FilterTempContext = 21;   % this is the number of frames used for Fourier coefficient domain filtering.
para.topology.FilterFreqContext = 1;    % this is the number of frequency bins used for filter
para.topology.useCMN = 0;
para.topology.mu_law_factor = 2047;
para.topology.DeltaGenerationType = DeltaGenerationType;
para.topology.MSECostWeightSDA = [1 4.5 10];
para.topology.hiddenLayerSize = hiddenLayerSize;
para.topology.hiddenLayerSizeFF = hiddenLayerSizeFF;
para = ConfigDereverbNet_Regression(para);

[Data_small, para] = LoadParallelWavLabel_Reverb(para, 100, 'train', {'simu'}, {'far'});
[layer, para] = Build_DereverbNet_RegressionComplex(Data_small, para);

% load the training and cv data
[Data_tr, para] = LoadParallelWavLabel_Reverb(para, step, 'train', {'simu'}, {'far'});
[Data_cv, para] = LoadParallelWavLabel_Reverb(para, step, 'dev', {'simu'}, {'far', 'near'});

% generate directory and file names to store the networks.
para.output = 'nnet/DereverbRegressionComplex';
para.output = sprintf('%s.MbSize%d.U%d.%d-%s', para.output, ...
    para.NET.nSequencePerMinibatch, length(Data_tr(1).data),  3*(para.topology.fft_len/2+1), para.topology.RegressionNetType);
for i=1:length(para.topology.hiddenLayerSize)
    para.output = sprintf('%s-%d', para.output, para.topology.hiddenLayerSize(i));
end
if length(para.topology.hiddenLayerSizeFF)>0
    para.output = [para.output '-DNN'];
    for i=1:length(para.topology.hiddenLayerSizeFF)
        para.output = sprintf('%s-%d', para.output, para.topology.hiddenLayerSizeFF(i));
    end
end
para.output = sprintf('%s-%d.L2_%s.LR_%s/nnet', para.output, 3*(para.topology.fft_len/2+1), FormatFloat4Name(para.NET.L2weight),FormatFloat4Name(para.NET.learning_rate));
LOG = [];

% show the configurations
para
para.NET
para.IO
para.topology

% train the network using SGD
trainGraph_SGD(layer, Data_tr, Data_cv, para, LOG);


function [CV,CF] = spline_cage(P,C,varargin)
  % SPLINE_CAGE Build a triangle mesh "cage" surrounding an input spline curve
  % trying to keep control points inside the triangles.
  %
  % [CV,CF] = spline_cage(P,C)
  %
  % Inputs:
  %   P  #P by 2 list of control point locations
  %   C  #C by 4 list of cubic Bezier curves as indices into rows of P
  %   Optional:
  %     'GridSize' followed by number of cells along the x-axis in the grid used
  %     for unsigned distance calculation and bwmesh {1000}
  %     'Pad' followed by number of cells to pad grid by {4}
  %     'PolyTol' followed by tolerance parameter for `spline_to_poly` used for
  %     approximating spline distances {0.1}
  %     'Tol'  followed by tolerance parameter for `bwmesh` {1.0}
  %     'SmoothingIters'  followed by smoothing iters parameter for `bwmesh`
  %       {10}
  % Outputs:
  %   CV  #CV by 2 list of cage triangulation vertex positions
  %   CF  #CF by 3 list of cage triangulation triangle indices into rows of CV
  %

  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'GridSize','Pad','PolyTol','Tol','SmoothingIters'}, ...
    {'gs','pad','poly_tol','bwmesh_tol','bwmesh_smoothing_iters'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  gs = 1000;
  pad = 4;
  poly_tol = 0.1;
  bwmesh_tol = 1.0;
  bwmesh_smoothing_iters = 10;

  % Just use distance to a well sampled poly-line
  [V,E] = spline_to_poly(P,C,poly_tol);
  [BC,side,r] = voxel_grid(P,gs,'Pad',pad);
  X = reshape(BC(:,1),side([2 1]));
  Y = reshape(BC(:,2),side([2 1]));
  sqrD = point_mesh_squared_distance(BC,V,E);
  D = sqrt(reshape(sqrD,size(X)));
  % Find maximum distance sampled at control points to try to be sure they're
  % inside the extracted mesh
  Dmax = max(interp2(X,Y,reshape(D,size(X)),P(:,1),P(:,2)));
  % convert into fuzzy logistic looking function
  h = 2/sqrt(2)*r(1);
  Dmax = Dmax+h;
  Z = 1-min(max(D-Dmax,0),h)/h;
  % Ignore holes
  Z = imfill(Z);
  [CV,CF] = bwmesh(Z,'Tol',bwmesh_tol,'SmoothingIters',bwmesh_smoothing_iters);
  % bwmesh flips things around and doesn't know the scale
  CV(:,2) = size(Z,1)-CV(:,2);
  CV = (CV-0.0)/size(Z,2)*(max(X(:))-min(X(:)))+[min(X(:)) min(Y(:))];
end


BNT_HOME = '/home/ai2/murphyk/matlab/FullBNT'; % edit this

d = fullfile(BNT_HOME, 'BNT');
%PC = (strncmp(computer,'PC',2));

cd(sprintf('%s/potentials/Tables', d))
mex marg_table.c % used by @dpot/marginalize_pot.m
mex marg_sparse_table.c %used by sparse jtree
mex mult_by_table.c
mex mult_by_sparse_table.c
mex divide_by_table.c
mex divide_by_sparse_table.c
mex rep_mult.c

% Written by Wei Hu
cd(sprintf('%s/CPDs/@discrete_CPD', d))
mex convert_to_sparse_table.c

% Written by Wei Hu
cd(sprintf('%s/inference/static/@jtree_sparse_inf_engine', d))
mex init_pot.c
mex collect_evidence.c
mex distribute_evidence.c

% written by Bhaskara Marthi
cd(sprintf('%s/inference/static/@gibbs_sampling_inf_engine/private', d))
mex compute_posterior.c
mex get_slice_dbn.c
mex sample_single_discrete.c


function [samples, energies, diagn] = hmc2(f, x, opt, gradf, varargin)
%HMC2   Hybrid Monte Carlo sampling.
%
%       Description
%       SAMPLES = HMC2(F, X, OPTIONS, GRADF) uses a  hybrid Monte Carlo
%       algorithm to sample from the distribution P ~ EXP(-F), where F is the
%       first argument to HMC2. The Markov chain starts at the point X, and
%       the function GRADF is the gradient of the `energy' function F.
%
%       HMC2(F, X, OPTIONS, GRADF, P1, P2, ...) allows additional arguments to
%       be passed to F() and GRADF().
%
%       [SAMPLES, ENERGIES, DIAGN] = HMC2(F, X, OPTIONS, GRADF) also returns a
%       log of the energy values (i.e. negative log probabilities) for the
%       samples in ENERGIES and DIAGN, a structure containing diagnostic
%       information (position, momentum and acceptance threshold) for each
%       step of the chain in DIAGN.POS, DIAGN.MOM and DIAGN.ACC respectively.
%       All candidate states (including rejected ones) are stored in
%       DIAGN.POS. The DIAGN structure contains fields:
%
%       pos
%        the position vectors of the dynamic process
%       mom
%        the momentum vectors of the dynamic process
%       acc
%        the acceptance thresholds
%       rej
%        the number of rejections
%       stp
%        the step size vectors
%
%       S = HMC2('STATE') returns a state structure that contains
%       the used random stream and its state and the momentum of
%       the dynamic process. These are contained in fields stream,
%       streamstate and mom respectively. The momentum state is
%       only used for a persistent momentum update.
%
%       HMC2('STATE', S) resets the state to S. If S is an integer,
%       then it is used as a seed for the random stream and the
%       momentum variable is randomised. If S is a structure
%       returned by HMC2('STATE') then it resets the random stream
%       to exactly the same state.
%
%       See HMC2_OPT for the optional parameters in the OPTIONS structure.
%
%       See also
%       HMC2_OPT, METROP
%

%       Copyright (c) 1996-1998 Christopher M Bishop, Ian T Nabney
%       Copyright (c) 1998-2000,2010 Aki Vehtari

%  The portion of the HMC algorithm involving "windows" is derived
%  from the C code for this function included in the Software for
%  Flexible Bayesian Modeling written by Radford Neal
%  <http://www.cs.toronto.edu/~radford/fbm.software.html>.

% Global variable to store state of momentum variables: set by set_state
% Used to initialize variable if set

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

global HMC_MOM

%Return state or set state to given state.
if nargin <= 2
  if ~strcmp(f, 'state')
    error('Unknown argument to hmc2');
  end
  switch nargin
   case 1
    samples = get_state(f);
    return;
   case 2
    set_state(f, x);
    return;
  end
end

% Set empty options to default values
opt=hmc2_opt(opt);
% Reference to structures is much slower, so...
opt_nsamples=opt.nsamples;
opt_window=opt.window;
opt_steps=opt.steps;
opt_display = opt.display;
opt_persistence = opt.persistence;

if opt_persistence
  alpha = opt.decay;
  salpha = sqrt(1-alpha*alpha);
end

% Stepsizes, varargin gives the opt.stepsf arguments (net, x ,y)
% where x is input data and y is a target data.
if ~isempty(opt.stepsf)
  epsilon = feval(opt.stepsf,varargin{:}).*opt.stepadj;
else
  epsilon = opt.stepadj;
end

% Force x to be a row vector
x = x(:)';
nparams = length(x);

%Set up strings for evaluating potential function and its gradient.
f = fcnchk(f, length(varargin));
gradf = fcnchk(gradf, length(varargin));

% Check the gradient evaluation.
if (opt.checkgrad)
  % Check gradients
  feval('gradcheck', x, f, gradf, varargin{:});
end

% Matrix of returned samples.
samples = zeros(opt_nsamples, nparams);
% Return energies?
if nargout >= 2
  en_save = 1;
  energies = zeros(opt_nsamples, 1);
else
  en_save = 0;
end
% Return diagnostics?
if nargout >= 3
  diagnostics = 1;
  diagn_pos = zeros(opt_nsamples, nparams);
  diagn_mom = zeros(opt_nsamples, nparams);
  diagn_acc = zeros(opt_nsamples, 1);
else
  diagnostics = 0;
end

if (~opt_persistence | isempty(HMC_MOM) | nparams ~= length(HMC_MOM))
  % Initialise momenta at random
  p = randn(1, nparams);
else
  % Initialise momenta from stored state
  p = HMC_MOM;
end

% Evaluate starting energy.
E = feval(f, x, varargin{:});

% Main loop.
nreject = 0;         %number of rejected samples
window_offset=0;     %window offset initialised to zero
k = - opt.nomit + 1;       %nomit samples are omitted, so we store
while k <= opt_nsamples    %samples from k>0

  % Store starting position and momenta
  xold = x;
  pold = p;
  % Recalculate Hamiltonian as momenta have changed
  Eold = E;
  Hold = E + 0.5*(p*p');

  % Decide on window offset, if windowed HMC is used
  if opt_window>1
    window_offset=fix(opt_window*rand(1));
  end

  have_rej = 0;
  have_acc = 0;
  n = window_offset;
  dir = -1;           %the default value for dir (=direction)
                      %assumes that windowing is used
  while (dir==-1 | n~=opt_steps)

    %if windowing is not used or we have allready taken
    %window_offset steps backwards...
    if (dir==-1 & n==0)
      % Restore, next state should be original start state.
      if window_offset > 0
	x = xold;
	p = pold;
	n = window_offset;
      end
      %set dir for forward steps
      E = Eold;
      H = Hold;
      dir = 1;
      stps = dir;
    else
      if (n*dir+1<opt_window | n>(opt_steps-opt_window))
	% State in the accept and/or reject window.
	stps = dir;
      else
	% State not in the accept and/or reject window.
	stps = opt_steps-2*(opt_window-1);
      end
      % First half-step of leapfrog.
      p = p - dir*0.5*epsilon.*feval(gradf, x, varargin{:});
      x = x + dir*epsilon.*p;
      % Full leapfrog steps.
      for m = 1:(abs(stps)-1)
	p = p - dir*epsilon.*feval(gradf, x, varargin{:});
	x = x + dir*epsilon.*p;
      end
      % Final half-step of leapfrog.
      p = p - dir*0.5*epsilon.*feval(gradf, x, varargin{:});

      E = feval(f, x, varargin{:});
      H = E + 0.5*(p*p');

      n=n+stps;
    end

    if (opt_window~=opt_steps+1 & n<opt_window)
      % Account for state in reject window.  Reject window can be
      % ignored if windows consist of the entire trajectory.
      if ~have_rej
	rej_free_energy = H;
      else
	rej_free_energy = -addlogs(-rej_free_energy, -H);
      end
      if (~have_rej | rand(1) < exp(rej_free_energy-H));
	E_rej=E;
	x_rej=x;
	p_rej=p;
        have_rej = 1;
      end
    end

    if (n>(opt_steps-opt_window))
      % Account for state in the accept window.
      if ~have_acc
	acc_free_energy = H;
      else
	acc_free_energy = -addlogs(-acc_free_energy, -H);
      end
      if (~have_acc | rand(1) < exp(acc_free_energy-H))
	E_acc=E;
	x_acc=x;
	p_acc=p;
	have_acc = 1;
      end
    end
  end

  % Acceptance threshold.
  a = exp(rej_free_energy - acc_free_energy);

  if (diagnostics & k > 0)
    diagn_pos(k,:) = x_acc;
    diagn_mom(k,:) = p_acc;
    diagn_acc(k,:) = a;
  end
  if (opt_display > 1)
    fprintf(1, 'New position is\n');
    disp(x);
  end

  % Take new state from the appropriate window.
  if a > rand(1)
    % Accept
    E=E_acc;
    x=x_acc;
    p=-p_acc; % Reverse momenta
    if (opt_display > 0)
      fprintf(1, 'Finished step %4d  Threshold: %g\n', k, a);
    end
  else
    % Reject
    if k > 0
      nreject = nreject + 1;
    end
    E=E_rej;
    x=x_rej;
    p=p_rej;
    if (opt_display > 0)
      fprintf(1, '  Sample rejected %4d.  Threshold: %g\n', k, a);
    end
  end

  if k > 0
    % Store sample
    samples(k,:) = x;
    if en_save
      % Store energy
      energies(k) = E;
    end
  end

  % Set momenta for next iteration
  if opt_persistence
    % Reverse momenta
    p = -p;
    % Adjust momenta by a small random amount
    p = alpha.*p + salpha.*randn(1, nparams);
  else
    % Replace all momenta
    p = randn(1, nparams);
  end

  k = k + 1;
end

if (opt_display > 0)
  fprintf(1, '\nFraction of samples rejected:  %g\n', ...
    nreject/(opt_nsamples));
end

% Store diagnostics
if diagnostics
  diagn.pos = diagn_pos;   %positions matrix
  diagn.mom = diagn_mom;   %momentum matrix
  diagn.acc = diagn_acc;   %acceptance treshold matrix
  diagn.rej = nreject/(opt_nsamples);   %rejection rate
  diagn.stps = epsilon;    %stepsize vector
end

% Store final momentum value in global so that it can be retrieved later
if opt_persistence
  HMC_MOM = p;
else
  HMC_MOM = [];
end
return

function state = get_state(f)
%GET_STATE           Return complete state of sampler
%                    (including momentum)

global HMC_MOM
state.stream=setrandstream();
state.streamstate = state.stream.State;
state.mom = HMC_MOM;
return

function set_state(f, x)
%SET_STATE    Set complete state of sample
%
%          Description
%          Set complete state of sampler (including momentum)
%          or just set randn and rand with integer argument.
global HMC_MOM
if isnumeric(x)
  setrandstream(x);
else
  if ~isstruct(x)
    error('Second argument to hmc must be number or state structure');
  end
  if (~isfield(x, 'stream') | ~isfield(x, 'streamstate') ...
      | ~isfield(x, 'mom'))
    error('Second argument to hmc must contain correct fields')
  end
  setrandstream(x.stream);
  x.State=x.streamstate;
  HMC_MOM = x.mom;
end
return

function c=addlogs(a,b)
%ADDLOGS(A,B)  Add numbers represented by their logarithms.
%
%        Description
%        Add numbers represented by their logarithms.
%        Computes log(exp(a)+exp(b)) in such a fashion that it
%        works even when a and b have large magnitude.
if a>b
  c = a + log(1+exp(b-a));
else
  c = b + log(1+exp(a-b));
end


function bc = specific_bc(xbd,ybd)
%ref_bc   Reference problem 3.2  boundary condition
%   bc = specific_bc(xbd,ybd);
%   input
%          xbd          x boundary coordinate vector
%          ybd          y boundary coordinate vector
%   definition of Neil Madden test problem given in Roos book p.197.
%   IFISS function: DJS; 5 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
nobd=length(xbd);
bc=zeros(size(xbd));
k=find(ybd==-1); bc(k)=1;
k=find( xbd ==-1); bc(k)=(1-(1+ybd(k))/2).^3;
k=find( xbd ==1 ); bc(k)=(1-(1+ybd(k))/2).^2;
return


function a = abs(a)
%ABS          Taylor absolute value abs(a)
%
%   c = abs(a)
%
%Result is convex hull of left and right derivative. This allows
%also input containing zero. The expansion
%  f(x) = f(xs) + f'(zeta)*(x-xs)
%for some zeta in ch(x,xs) is still valid
%

% written  05/21/09     S.M. Rump
%

  if ~isreal(a.t)
    a.t = abs(a.t);
    a.t(2:end,:) = repmat(NaN,size(a.t(2:end,:)));
    return
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  at = a.t(1,:);
  index = ( at<0 );
  a.t(:,index) = -a.t(:,index);
  if isa(at,'intval')
    index = in0(0,at(1,:));
    a.t(:,index) = infsup(-1,1)*a.t(:,index);
  end

  if rndold
    setround(rndold)
  end


filename='Bridge_triangle_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP'; incrementFactor = 1;
filterType = 'P1';

nsteps = 5;
Vfrac_final = 0.2;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-2;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function model3d_new = transform_3d_rigid(model3d,R,T)

if(model_has_normals(model3d))
    [x,y,z,nx,ny,nz] = model_to_components(model3d);
else
    [x,y,z] = model_to_components(model3d);
end

xyz = [x y z];
xyz = xyz * R';
xyz  = xyz + repmat(T',length(x),1);

if(model_has_normals(model3d))
    nxyz = [nx ny nz] * R';
    model3d_new = components_to_model(xyz(:,1),xyz(:,2),xyz(:,3),nxyz(:,1),nxyz(:,2),nxyz(:,3));
else
    model3d_new = components_to_model(xyz(:,1),xyz(:,2),xyz(:,3),[],[],[]);
end

if(model_has_colors(model3d))
    model3d_new.r = model3d.r;
    model3d_new.g = model3d.g;
    model3d_new.b = model3d.b;
end

if(model_has_faces(model3d))
    model3d_new.face = model3d.face;
end


function [name, data] = dispData(f)
%DISPDATA   Useful information for DISPLAY.
%   [NAME, DATA] = DISPDATA(F) collects extra information from the given CHEBFUN
%   F and the output will be used by DISPLAY.
%
% See also DISPLAY.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Number of pieces (i.e., funs) information:
numFuns = numel(f.funs);

%% Collect information about the exponents:

% Preallocation:
exps = zeros(numFuns, 2);

% Loop over each FUN:
for j = 1:numFuns
    infoJ = dispData(f.funs{j});
    if ( ~isempty(infoJ) )
        numInfo = numel(infoJ);
        for k = 1:numInfo
            infoJK = infoJ{k};
            if ( strcmpi('exponents', infoJK.name) )
                exps(j,:) = infoJK.data;
            end
        end
    end
end

%% Organize the output which will be passed to DISPLAY:

% Preallocate the extra information:
name = ' ';
data = cell(j,1);

% So far, the only extra information is exponents:
if ( any( exps(:) ) )
    name = '  endpoint exponents';
    for j = 1:numFuns
        data{j} = ['        ' '[' num2str(exps(j,1), '%2.2g') '      ' ...
            num2str(exps(j,2), '%2.2g') ']' '  '];
    end
elseif ( ~isempty(infoJ) )
    name = infoJ{1}.name;
end

% More information for F can be appended to INFO:

end


%% Copyright (C) 2015, 2016, 2018, 2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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.
%%
%% This software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym ipermute (@var{B}, @var{iperm})
%% Invert a permutation the indices of a symbolic array.
%%
%% Example:
%% @example
%% @group
%% A = sym([1 2 pi; 4 5 6]);
%% B = permute(A, [2 1]);
%% ipermute(B, [2 1])
%%   @result{} ans = (sym 2×3 matrix)
%%       ⎡1  2  π⎤
%%       ⎢       ⎥
%%       ⎣4  5  6⎦
%% @end group
%% @end example
%%
%% @seealso{@@sym/permute}
%% @end defmethod

function A = ipermute(B, iperm)

  if (nargin < 2)
    print_usage ();
  end

  A = permute(B, iperm);

end

%!error permute (sym(1))
%!error permute (sym(1), 2, 3)

%!test
%! syms x
%! A = [1 x];
%! perm = [2 1];
%! B = permute(A, perm);
%! C = ipermute(B, perm);
%! assert (isequal(C, A))


%Note: although this example uses wander-azimuth concept, you should not consider this
%as a suitable wander-azimuth implementaion. This purpose of this example is to illustrate
%how large heading errors can be handled. There is an another example in the toolkit
%presenting a suitable wander-azimuth implementation.

clear;
fclose all;
%PATH='/home/yigiter/Desktop/Dropbox/INS/LargeHeading/mfiles/data/';
PATH='C:\Users\yigiter\Desktop\Dropbox\INS\LargeHeading\mfiles\data\';

%%Specify the imu error model and generate the path
[SenErrDef, IniErrDef, ObsErrDef, ini_pv, wander, dt]=sys_path_largeheading(PATH, 1);

%%Open the files
F_IMU=fopen([PATH 'imu.bin'],'rb');
F_TRU=fopen([PATH 'mnav.bin'],'rb');
F_IMUERR=fopen([PATH 'imuerr.bin'],'rb');
F_OBS_POS=fopen([PATH 'obs_pos.bin'],'rb');
F_RES=fopen([PATH 'res.bin'],'wb');
F_ERR=fopen([PATH 'err.bin'],'wb');

%propagation time for the discretized system
prp_per=round(0.33/dt);

%%Sensor error models
[dAimu, dBimu, Cimu, dDimu, sP0imu]=imu_modTI_v000(SenErrDef);  %errors models in discrete time
[Aimu, Qimu, Rimu]=dc2dc_v000(dAimu, dBimu*dBimu', dDimu*dDimu', dt, 1, []);  %models in continuous time
ximu=zeros(6,1); %imu error states

%%%%Initialize the INS
%PV is provided
pos_g=ini_pv(:,1);
vel_n=ini_pv(:,2); %note path generator is defined for body frame velocity
%Use accelerometer data to initialize the attitude
t=0;
in=0;
imu_avg=zeros(6,1);
imu_data=fread(F_IMU,8,'double'); %garbage
true_val = fread(F_TRU, 10, 'double');
imu_err=fread(F_IMUERR, 7, 'double');
avg_time=1;
while (in*dt)<avg_time
    imu_data=fread(F_IMU,8,'double');
    true_val = fread(F_TRU, 10, 'double');
    imu_err=fread(F_IMUERR, 7, 'double');
    imu_avg=imu_avg+imu_data(3:8);
    in=in+1;
end
imu_avg=imu_avg/in;
%Compute the roll/pitch
%[Cnb E]=align_wander(imu_avg(1:3), [0;0;-1]);
[Cnb E]=alingc_pl_v000(imu_avg(1:3), 0);
Cbn=Cnb';

%Use constant scale for all observations
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(pos_g);
Sn=Rn+pos_g(3);
Se=(Re+pos_g(3))*cos(pos_g(1));
S=[Sn 0 0;0 Se 0;0 0 1];

%alignment mode (0=coarse, 1=fine)
%%"mode 2" is standard small angle (PHI-model) model. (I used it to verify the results.)
mode=0;

%Initial covariance
if (mode==0)
    nst=17;
    nst_nav=11;
    P=zeros(nst);
    P(10:11,10:11)=IniErrDef.wander_sP*IniErrDef.wander_sP';
    wander=[sin(0*pi/180);cos(0*pi/180)];
elseif (mode==1)
    nst=16;
    nst_nav=10;
    P=zeros(nst);
    P(10,10)=IniErrDef.att_sP(3,3)*IniErrDef.att_sP(3,3);
    %wander=[sin(105*pi/180);cos(105*pi/180)]; (use the wander provided by
    %the syspath)
    %P(10:11,10:11)=IniErrDef.wander_sP(1,1)+IniErrDef.wander_sP(2,2);
end
P(1:3,1:3)=S*IniErrDef.pos_sP*IniErrDef.pos_sP'*S';
P(4:6,4:6)=IniErrDef.vel_sP*IniErrDef.vel_sP';
P(7:8,7:8)=E(1:2,1:3)*Rimu(1:3,1:3)/avg_time*E(1:2,1:3)';
P(nst_nav+1:nst,nst_nav+1:nst)=sP0imu*sP0imu';
%Cross covarince between acc error and initial attitude
P(7:8,nst_nav+1:nst_nav+3)=E(1:2,1:3)*P(nst_nav+1:nst_nav+3,nst_nav+1:nst_nav+3);
P(nst_nav+1:nst_nav+3,7:8)=P(7:8,nst_nav+1:nst_nav+3)';

%accumulators and control flags
prp_coun=0;
STM_upd=eye(nst);
I=eye(nst);
imu_avg=zeros(6,1);

%%read the input data from the files
imu_data=fread(F_IMU,8,'double');
true_val = fread(F_TRU, 10, 'double');
imu_err=fread(F_IMUERR, 7, 'double');
obs_pos=fread(F_OBS_POS, 4, 'double');

%%Start iterations
while (~feof(F_IMU))
        imu=imu_data(3:8)-ximu;
        pr_coun=imu_data(1);
        [Cbn, vel_n, pos_g, wander]=strapdown_wander_dcm_v000(Cbn, vel_n, pos_g, wander, imu(1:3), imu(4:6), dt);
        imu_avg=imu_avg+imu;
        prp_coun=prp_coun+1;

        if ((~isempty(obs_pos) && pr_coun==obs_pos(1))|| (mod(prp_coun,prp_per)==0)) %%(obs and/or covariance propagation)
            %%First propagate the covariance
            imu_avg=imu_avg/prp_coun;

            %%Form the discrete time navigation model
            if (mode==0 || mode==1)
                [STM Q]=sys_wander_largeheading_v000(pos_g, vel_n, Cbn, wander, imu_avg(1:3), prp_coun*dt, mode, Aimu, Qimu, Cimu, Rimu);
            elseif (mode==2)
                [STM Q]=sys_wander_dcm_v000(pos_g, vel_n, Cbn, wander, imu_avg(1:3), prp_coun*dt, Aimu, Qimu, Cimu, Rimu);
            end

            %Propagate
            P=STM*P*STM'+Q;
            prp_coun=0;
            imu_avg=zeros(6,1);

            %update IMU error states
            ximu=STM(nst_nav+1:nst,nst_nav+1:nst)*ximu;

            if (~isempty(obs_pos)) && (obs_pos(1)==pr_coun) %We have an observation to be processed

                if (mode==0 || mode==1) %switch from coarse/fine to small angle. The new wander angle will be fixed to the last wander estimate.
                    if (mode==0)
                        vda=P(10,10)+P(11,11);
                    elseif (mode==1)
                        vda=P(10,10);
                    end
                    if (vda<(0*pi/180)^2)
                        Cgn=[wander(2) wander(1) 0;-wander(1) wander(2) 0;0 0 1];
                        vel_g=Cgn'*vel_n;
                        mx_a=eye(17);
                        mx_a(10:11,:)=[];
                        mx_a(4:6,10:11)=-[vel_g(2) vel_g(1);-vel_g(1) vel_g(2);0 0];
                        mx_a(9,10:11)=[-wander(2) -wander(1)];

                        if (mode==1)
                            mx_a(:,10)=wander(2)*mx_a(:,10)-wander(1)*mx_a(:,11);
                            mx_a(:,11)=[];
                        end

                        P=mx_a*P*mx_a';

                        mode=2;
                        nst_nav=9;
                        nst=15;
                        I=eye(nst);
                    end
                end

                H=zeros(3,nst); %position update
                H(:,1:3)=eye(3);
                R=S*ObsErrDef.sR*ObsErrDef.sR'*S';
                inno=S*(pos_g-obs_pos(2:4));  %position obs in m

                Rinno_inv=inv(H*P*H'+R);
                K=P*H'*Rinno_inv;

                %correct current states
                dx=K*inno;
                %K(10:11,:)=0;
                %K(12:17,:)=0;
                pos_g=pos_g-S\dx(1:3);
                vel_n=vel_n-dx(4:6);
                mx_a=euler2dcm_v000(dx(7:9));
                Cbn=mx_a*Cbn;
                if (mode==0)    %coarse alignment
                    wander=wander-dx(10:11);
                    %%normalize wander
                    wander=wander/norm(wander);
                elseif (mode==1) %fine alignment
                    wander=[-wander(2) wander(1);wander(1) wander(2)]*[sin(dx(10));cos(dx(10))];
                end
                ximu=ximu+dx(nst_nav+1:nst);
                %update cov
                P=(I-K*H)*P*(I-K*H)'+K*R*K';

                %prepare for the next cycle
                obs_pos=fread(F_OBS_POS, 4, 'double');

                %Check to switch mode
                if (mode==0)    %switch from coarse to fine
                    vda=P(10,10)+P(11,11);
                    if (vda<(0*pi/180)^2)
                        mode=1;
                        nst_nav=10;
                        nst=16;
                        I=eye(nst);

                        mx_a=eye(17,17);
                        mx_a(11,:)=[];
                        mx_a(10,10)=wander(2);
                        mx_a(10,11)=-wander(1);
                        P=mx_a*P*mx_a';
                    end
                end

            end

            %%Write the results (Cbg, Vb, pos_g)
            vr_a=diag(P).^0.5;

            imu_err=imu_err(2:7)-ximu;
            imu_std=vr_a(nst_nav+1:end);

            pos_err=S*(pos_g-true_val(2:4));
            pos_std=vr_a(1:3);

            vel_err=Cbn'*vel_n-true_val(5:7);
            mx_a=[Cbn' -Cbn'*skew(vel_n)];
            mx_b=P(4:9,4:9);
            mx_c=mx_a*mx_b*mx_a';
            vel_std=diag(mx_c).^0.5;
%             Cgn=[wander(2) wander(1) 0;-wander(1) wander(2) 0;0 0 1];
%             Cbg_true=euler2dcm_v000(true_val(8:10));
%             vel_err=Cgn'*vel_n-Cbg_true*true_val(5:7);

            Cbg_true=euler2dcm_v000(true_val(8:10));
            Cgn=[wander(2) wander(1) 0;-wander(1) wander(2) 0;0 0 1];
            Cbg=Cgn'*Cbn;
            mx_a=eye(3)-Cbg*Cbg_true';
            att_err=[-mx_a(2,3);mx_a(1,3);-mx_a(1,2)]; %-dcm2euler_v000(Cgn'*Cbn*Cbn_true');
            if (mode==0)
                mx_a=[Cgn' [0 0;0 0;-wander(2) wander(1)]];
            elseif (mode==1)
                mx_a=[Cgn' -[0;0;1]];
            elseif (mode==2)
                mx_a=Cgn';
            end
            mx_b=P(7:nst_nav,7:nst_nav);
            mx_c=mx_a*mx_b*mx_a';
            att_std=diag(mx_c).^0.5;

%             %%%Write the results (Cbn, Vn, pos_g) (only for mode 0 and
%             %%%mode1 - revise for mode2)
%             vr_a=diag(P).^0.5;
%
%             imu_err=imu_err(2:7)-ximu;
%             imu_std=vr_a(nst_nav+1:end);
%
%             pos_err=S*(pos_g-true_val(2:4));
%             pos_std=vr_a(1:3);
%
%             Cbg_true=euler2dcm_v000(true_val(8:10));
%             Cgn=[wander(2) wander(1) 0;-wander(1) wander(2) 0;0 0 1];
%             Cbn_true=Cgn*Cbg_true;
%             vel_err=vel_n-Cbn_true*true_val(5:7);
%             if (mode==0)
%                 vr_a=Cbg_true*true_val(5:7);
%                 mx_a=[eye(3);[vr_a(2) vr_a(1);-vr_a(1) vr_a(2);0 0]];
%                 mx_b=P([4 5 6 10 11],[4 5 6 10 11]);
%             elseif (mode==1)
%                 vr_a=Cbn_true*true_val(5:7);
%                 mx_a=[eye(3);[vr_a(2);-vr_a(1);0]];
%                 mx_b=P([4 5 6 10],[4 5 6 10]);
%             end
%             mx_c=mx_a*mx_b*mx_a';
%             vel_std=diag(mx_c).^0.5;
%
%
%             mx_a=eye(3)-Cbn*Cbn_true';
%             att_err=[-mx_a(2,3);mx_a(1,3);-mx_a(1,2)]; %-dcm2euler_v000(Cgn'*Cbn*Cbn_true');
%             if (mode==0)
%                 mx_a=[eye(3) [0 0;0 0;-wander(2) -wander(1)]];
%             elseif (mode==1)
%                 mx_a=[eye(3) -[0;0;1]];
%             end
%             mx_b=P(7:nst_nav,7:nst_nav);
%             mx_c=mx_a*mx_b*mx_a';
%             att_std=diag(mx_c).^0.5;

            fwrite(F_ERR,[pr_coun;pos_err;vel_err;att_err;imu_err;pos_std;vel_std;att_std;imu_std],'double');
            fwrite(F_RES, [pr_coun;pos_g; Cbn'*vel_n; dcm2euler_v000(Cgn'*Cbn);atan2(wander(1),wander(2));ximu],'double');
        end

        %Read the next values
        true_val = fread(F_TRU, 10, 'double');
        imu_err=fread(F_IMUERR, 7, 'double');
        imu_data=fread(F_IMU,8,'double');
end

fclose all;
err=readbin_v000([PATH 'err.bin'],31);
%imu_err=readbin_v000([PATH '\imuerr.bin'],7);
res=readbin_v000([PATH 'res.bin'],17);
tru=readbin_v000([PATH 'mnav.bin'],10);

% return;
% figure;
% plot(res(2,:), res(3,:))
% hold on
% plot(tru(2,:),(tru(3,:)),'r--');
% grid;

figure;
i=9;
plot(abs(err(1+i,:))*180/pi);
hold on;
plot(err(16+i,:)*180/pi,'r');
grid;

% figure;
% i=4;
% plot(abs(err(1+i,:)));
% hold on;
% plot(err(16+i,:),'r');
% grid;


classdef (Abstract) BASEsettings < matlab.mixin.CustomDisplay
    %BASESETTINGS Abstract class with the common settings to all VARtypes
    %
    % BASEsettings Properties:
    %    frequency       - data frequency
    %    startdate       - sample start date
    %    enddate         - sample end date
    %    varendo         - endogenous variables;
    %    varexo          - exogenous variables
    %    lags            - number of lags
    %    const           - inclusion of a constant
    %    excelFile       - Excel file used for the inputs
    %    results_path    - path where there results file is stored
    %    results_sub     - name of the results file
    %    results         - save the results in the excel file (true/false)
    %    plot            - plot the results (true/false)
    %    workspace       - save the workspace as a .mat file (true/false)
    %    IRF             - impulse response functions
    %    IRFperiods      - number of periods for IRF
    %    F               - unconditional forecasts
    %    FEVD            - forecast error variance decomposition
    %    HD              - historical decomposition
    %    HDall           - plot the entire decomposition
    %    CF              - activate conditional forecasts
    %    IRFt            - structural identification
    %    Feval           - forecast evaluation
    %    CFt             - type of conditional forecasts
    %    Fstartdate      - start date for forecasts
    %    Fenddate        - start date for forecasts
    %    Fendsmpl        - start forecasts after the final sample period
    %    hstep           - step ahead evaluation
    %    window_size     - window_size for iterative forecasting
    %    evaluation_size - evaluation_size as percent of window_size
    %    cband           - confidence level for VAR coefficients
    %    IRFband         - confidence level for impusle response functions
    %    Fband           - confidence level for forecasts
    %    FEVDband        - confidence level for forecast error variance decomposition
    %    HDband          - confidence level for historical decomposition

    properties (SetAccess = private)

        VARtype   bear.VARtype = bear.VARtype.empty      % VAR model selected (1=OLS VAR, 2=BVAR, 3=mean-adjusted BVAR, 4=panel Bayesian VAR, 5=Stochastic volatility BVAR, 6=Time varying)

    end

    properties
        %% App settings on constant panel
        frequency (1,1) double  = 2;                     % data frequency (1=yearly, 2= quarterly, 3=monthly, 4=weekly, 5=daily, 6=undated)
        startdate               = '1974q1';              % sample start date; must be a string consistent with the date formats of the toolbox
        enddate                 = '2014q4';              % sample end date; must be a string consistent with the date formats of the toolbox
        varendo                 = 'YER HICSA STN';       % endogenous variables; must be a single string, with variable names separated by a space
        varexo                  = '';                    % exogenous variables, if any; must be a single string, with variable names separated by a space
        lags      (1,1) double  = 4;                     % number of lags
        const     (1,1) logical = true;                  % inclusion of a constant (1=yes, 0=no)

        excelFile    (1,:) char = '';                    % Excel file used for the inputs
        results_path (1,:) char = '';                    % path where there results file is stored
        results_sub  (1,:) char = 'results';             % name of the results file
        results      (1,1) logical = true;               % save the results in the excel file (true/false)
        plot         (1,1) logical = true;               % plot the results (true/false)
        workspace    (1,1) logical = true;               % save the workspace as a .mat file (true/false)

        % Model options
        IRF        (1,1) logical = true;  % activate impulse response functions (1=yes, 0=no)
        IRFperiods (1,1) double  = 20;    % number of periods for impulse response functions
        % structural identification:
        % 1=none,
        % 2=Cholesky,
        % 3=triangular factorisation
        % 4=sign, zero, magnitude, relative magnitude, FEVD, correlation restrictions,
        % 5=IV identification,
        % 6=IV identification & sign, zero, magnitude, relative magnitude, FEVD, correlation restrictions)
        IRFt  bear.IRFtype = bear.IRFtype(2);
    end

    properties (Dependent)
        FEVD       % activate forecast error variance decomposition (1=yes, 0=no)
        HD         % activate historical decomposition (1=yes, 0=no)
    end

    properties

        HDall      (1,1) logical = 0;     % if we want to plot the entire decomposition, all contributions (includes deterministic part) (1=yes, 0=no)
        F          (1,1) logical = true;  % activate unconditional forecasts (1=yes, 0=no)

        CF         (1,1) logical = false; % activate conditional forecasts (1=yes, 0=no)
        % type of conditional forecasts
        % 1 = standard (all shocks),
        % 2 = standard (shock-specific)
        % 3 = tilting (median), 4=tilting (interval)
        CFt bear.CFtype = bear.CFtype(1);

        % start forecasts immediately after the final sample period (1=yes, 0=no)
        % has to be set to 1 if start date for forecasts is not in-sample
        Fendsmpl (1,1) logical = false;
        Fstartdate = '2014q1'; % start date for forecasts (has to be an in-sample date; otherwise, ignore and set Fendsmpl=1)
        Fenddate   = '2016q4'; % end date for forecasts
        Feval (1,1) logical = false; % activate forecast evaluation (1=yes, 0=no)

        hstep           (1,1) double = 1;    % step ahead evaluation
        window_size     (1,1) double = 0;    % window_size for iterative forecasting 0 if no iterative forecasting                                            <                                                                                -
        evaluation_size (1,1) double = 0.5;  % evaluation_size as percent of window_size
        cband           (1,1) double = 0.68; % confidence/credibility level for VAR coefficients
        IRFband         (1,1) double = 0.68; % confidence/credibility level for impusle response functions
        Fband           (1,1) double = 0.68; % confidence/credibility level for forecasts
        FEVDband        (1,1) double = 0.68; % confidence/credibility level for forecast error variance decomposition
        HDband          (1,1) double = 0.68; % confidence/credibility level for historical decomposition

    end

    properties (Access = protected)
        FEVDinternal (1,1) logical = true;
        HDinternal   (1,1) logical = true;
    end

    methods

        function obj = BASEsettings(VARtype, excelPath)

            obj.VARtype = VARtype;
            obj.excelFile = excelPath;
            obj.results_path = pwd();

        end

        function obj = set.IRFt(obj, value)
            % call another function to check the value as desired,
            % and possibly even update it using some computation
            obj = checkIRFt(obj, value);

            % set set the property using the validated value
            % (only place we do assignment to avoid infinite recursion)
            obj.IRFt = value;
        end

        function value = get.FEVD(obj)
            value = getFEVD(obj);
        end

        function value = get.HD(obj)
            value = getHD(obj);
        end

        function obj = set.FEVD(obj, value)
            obj.FEVDinternal = value;
        end

        function obj = set.HD(obj, value)
            obj.HDinternal = value;
        end

    end

    methods (Access = protected)

        function obj = parseBEARSettings( obj, varargin )

            obj = bear.utils.pvset(obj, varargin{:});

        end

        function obj = checkIRFt(obj, ~)
        end

        function displayScalarObject(obj)

            % Grab property lists
            props = properties(obj)';
            meta = ?bear.settings.BASEsettings;

            % Get base properties
            baseProps = {meta.PropertyList.Name};

            specificProps = setdiff(props, baseProps);
            specificProps = props(ismember(props, specificProps)); % To keep original order

            mainProps = {'VARtype', 'frequency', 'startdate', ...
                'enddate', 'varendo', 'varexo', 'lags', 'const', ...
                'excelFile', 'results_path', 'results_sub', 'results', 'plot', 'workspace'};

            applicationProps = setdiff(baseProps, mainProps);
            applicationProps = props(ismember(props, applicationProps)); % To keep original order

            % header
            header = matlab.mixin.CustomDisplay.getSimpleHeader(obj);
            disp(header);

            % Preferences
            fprintf('\n --------- PREFERENCES ----------\n\n');
            mainProps = matlab.mixin.util.PropertyGroup(mainProps);
            matlab.mixin.CustomDisplay.displayPropertyGroups(obj, mainProps);

            % Specifications
            fprintf('\n --------- SPECIFICATIONS ----------\n\n');
            specificProps = matlab.mixin.util.PropertyGroup(specificProps);
            matlab.mixin.CustomDisplay.displayPropertyGroups(obj, specificProps);

            % Applications
            fprintf('\n --------- APPLICATIONS ----------\n\n');
            applicationProps = matlab.mixin.util.PropertyGroup(applicationProps);
            matlab.mixin.CustomDisplay.displayPropertyGroups(obj, applicationProps);

        end

    end

    methods (Access = protected)

        function value = getFEVD(obj)
            value = obj.FEVDinternal;
        end

        function value = getHD(obj)
            value = obj.HDinternal;
        end

    end

end


function rtk=detslip_tdcp(rtk,cur_obs,old_obs,nav,f)

global glc
dt=timediff(cur_obs(1).time,old_obs(1).time);
if abs(dt)>5
    return;
end

cur_sv=satposs(cur_obs,nav,rtk.opt.sateph);
old_sv=satposs(old_obs,nav,rtk.opt.sateph);

cur_pos=rtk.ins.pos;     cur_rr=blh2xyz(cur_pos);
old_pos=rtk.ins.oldpos;  old_rr=blh2xyz(old_pos);

n_cur=size(cur_obs,1);
n_old=size(old_obs,1);

nv=0; vsat=zeros(n_cur,1); v=zeros(n_cur,1);

for i=1:n_cur

    cur_obsi=cur_obs(i); cur_svi=cur_sv(i);
    lam=nav.lam(cur_obsi.sat,:);
    [sys,~]=satsys(cur_obsi.sat);
    if rtk.mask(sys)==0,continue;end

    if lam(f)==0,continue;end

    old_idx=0;
    for j=1:n_old
        if cur_obsi.sat==old_obs(j).sat
            old_idx=j;break;
        end
    end
    if old_idx==0,continue;end

    old_obsi=old_obs(old_idx); old_svi=old_sv(old_idx);

    if cur_svi.svh~=0||old_svi.svh~=0||norm(cur_svi.pos)<=0||norm(old_svi.pos)<=0
        continue;
    end
    if cur_obsi.L(f)==0||old_obsi.L(f)==0,continue;end

    [cur_range,~]=geodist(cur_svi.pos,cur_rr);
    [old_range,~]=geodist(old_svi.pos,old_rr);

    tdcp_obs=cur_obsi.L(f)*lam(f)-old_obsi.L(f)*lam(f);
    tdcp_pre=cur_range-old_range;

    v(i,1)=tdcp_obs-tdcp_pre+glc.CLIGHT*(cur_svi.dts-old_svi.dts);

    nv=nv+1;

    vsat(i)=1;

end

dt_drift=rtk.x(rtk.ic+6);
v=v-dt_drift;

slip=zeros(n_cur,1);
for i=1:n_cur
    sat=cur_obs(i).sat;
    if vsat(i)==0
        rtk.sat(sat).slip(f)=bitor(rtk.sat(sat).slip(f),1);
        continue;
    end

    vi=abs(v(i));
    v_other=abs(v);v_other(i)=[];
    ave_v=sum(abs(v_other))/(nv-1);
    ave_distance=sum(abs(vi-v_other))/(nv-1);
    if ave_distance>3*ave_v&&abs(v(i))>0.15
        slip(i)=1;
    end
end

idx=find(slip==1);
ratio=size(idx,1)/n_cur;
if ratio>=0.5
    return;
end

for i=1:n_cur
    sat=cur_obs(i).sat;
    if slip(i)==1
        rtk.sat(sat).slip(f)=bitor(rtk.sat(sat).slip(f),slip(i));
    end
end

return


function epo= proc_meanAcrossTime(epo, ival, clab)
%PROC_MEANACROSSTIME - Average signals across a specified time interval
%
%Synopsis:
%  EPO= proc_meanAcrossTime(EPO, <IVAL>, <CLAB>)
%
%Arguments:
%  DAT  - data structure of epoched data
%  IVAL - interval in which the average is to be calculated,
%         default [] which means the whole time range
%  CLAB - cell array of channels to be selected, default all
%
%Returns:
%  DAT  - updated data structure

misc_checkType(epo, 'STRUCT(x)');
misc_checkTypeIfExists('ival', 'DOUBLE[2]');
misc_checkTypeIfExists('clab', 'CHAR|CELL{CHAR}');

if nargin<2,
  ival= [];
end
if nargin<3,
  clab= {};
end

if isempty(ival),
  idx= 1:size(epo.x,1);
else
  idx= util_timeind(epo, ival);
end
epo.x= mean(epo.x(idx,:,:), 1);
if isfield(epo, 't') && length(idx)>1,
  epo.t= mean(epo.t(idx(1:end-1)));
end

if ~isempty(clab),
  epo= proc_selectChannels(epo, clab);
end


function [P3, S_bar, V, RO, Tr, Z, sigma_sq, phi, Q, mu0, sigma0, c] =...
    em_sfm(P, MD, K, mask, imsize, bbox, tol, max_em_iter, rhandedCoords, rotP3d)

%  Non-Rigid Structure From Motion with Gaussian/LDS Deformation Model
%  Copyright (c) by Lorenzo Torresani, Stanford University
%
%  Based on the following paper:
%
%  Lorenzo Torresani, Aaron Hertzmann and Christoph Bregler,
%     Learning Non-Rigid 3D Shape from 2D Motion, NIPS 16, 2003
%  http://cs.stanford.edu/~ltorresa/projects/learning-nr-shape/
%
%  Please refer to this publication if you use this program for
%  research or for technical applications.
%
%
%  INPUT:
%
%  P           - (2*T) x J tracking matrix:          P([t t+T],:) contains the 2D projections of the J points at time t
%  MD          - T x J missing data binary matrix:   MD(t, j)=1 if no valid data is available for point j at time t, 0 otherwise
%  K           - number of deformation basis
%  use_lds     - set to 1 to model deformations using a linear dynamical system; set to 0 otherwise
%  tol         - termination tolerance (proportional change in likelihood)
%  max_em_iter - maximum number of EM iterations
%  mask        - mask.poly_x (Tx1 cell array of x coordinates of mask) and
%                mask.poly_y (Tx1 cell array of y coordinates of mask)
%  imsize      - Tx2 array with height and width of images
%  bbox        - Bboxes of instances in image (Nx4)
%  params.normdim - Normalization dimension for object bounding boxes
%
%  OUTPUT:
%
%  P3          - (3*T) x J 3D-motion matrix:                    ( P3([t t+T t+2*T],:) contains the 3D coordinates of the J points at time t )
%  S_bar       - shape average:            3 x J matrix
%  V           - deformation shapes:       (3*K) x J matrix     ( V((n-1)*3+[1:3],:) contains the n-th deformation basis )
%  RO          - rotation:                 cell array           ( RO{t} gives the rotation matrix at time t )
%  Tr          - translation:              T x 2 matrix
%  Z           - deformation weights:      T x K matrix
%  sigma_sq    - variance of the noise in feature position
%  phi         - LDS transition matrix
%  Q           - LDS state noise matrix
%  mu0         - initial state mean
%  sigma0      - initial state variance
%  c - scale coefficients                   T X 1 matrix

%% Parsing passed parameters
params = get_params();

%% Checking Dimensions
if mod(size(P,1), 1) ~= 0,
   fprintf('Error: size(P) must be (2*T)xJ\n');
   return;
end

if (size(P,1)/2 ~= size(MD,1)) || (size(P,2) ~= size(MD,2))
   fprintf('Error: Size incompatibility between P and MD\n');
   return;
end

if mod(K, 1) ~= 0,
   fprintf('Error: K must be an integer value\n');
   return;
end
%% Initialization for Tr, S_bar, R, c

[T, J] = size(MD);

P_hat = P; % if any of the points are missing, P_hat will be updated during the M-step

% uses rank 3 factorization to get a first initialization for rotation and S_bar
[R_init, Trvect, S_bar] = rigidfac(P_hat, MD); % P_hat = R_init*S_bar + Trvect

% initialize c also here !!
c = ones(T,1);
%if scaling
    for t = 1:T
       c(t) = mean([norm(R_init(t,:)),norm(R_init(t+T,:))]); % just an estimate
    end
%end

Tr(:,1) = Trvect(1:T);
Tr(:,2) = Trvect(T+1:2*T);

R = zeros(2*T, 3);

%% enforces rotation constraints for initilizations
for t = 1:T,
   Ru = R_init(t,:);
   Rv = R_init(T+t,:);
   Rz = cross(Ru,Rv); if det([Ru;Rv;Rz])<0, Rz = -Rz; end;
   RO_approx = apprRot([Ru;Rv;Rz]);
   RO{t} = RO_approx;
   R(t,:) = RO_approx(1,:);
   R(t+T,:) = RO_approx(2,:);
end

%% Initializations for deformation shapes and weights
% given the initial estimates of rotation, translation and shape average, it initializes
% deformation shapes and weights through LSQ minimization of the reprojection error

[V, Z] = init_SB(P_hat, Tr, R, c , S_bar, K); %handled c inside this function

% initializes sigma_sq
E_zz_init = cov(Z);
E_zz_init = repmat(E_zz_init, T, 1);
sigma_sq = mstep_update_noisevar(P_hat, S_bar, V, Z', E_zz_init, RO, Tr, c); %handled c inside this function

phi = [];
mu0 = [];
sigma0 = [];
Q = [];

%% EM iterations

loglik = 0;
annealing_const = 60;
max_anneal_iter = round(max_em_iter/4);

pBar =  TimedProgressBar( max_em_iter, 30, ...
    'Training NRSfM ', ', completed ', 'Training NRSfM concluded in ' );
for em_iter=1:max_em_iter,
    %mean(c)
    %std(c)
    %mean(mean(abs(S_bar)))
    %% compute the hidden variables distributions
    [E_z, E_zz] = estep_compute_Z_distr(P_hat, S_bar, V, R, c, Tr, sigma_sq);     % (Eq 17-18) %handled c inside this function
    Z = E_z';

    %% updates shape basis
    [S_bar, V] = mstep_update_shapebasis(P_hat, E_z, E_zz, R, c, Tr);   % (Eq 21)

    %% fill in missing points
    if sum(MD(:))>0,
        P_hat = mstep_update_missingdata(P_hat, MD, S_bar, V, E_z, RO, c, Tr, mask, imsize, bbox, params.nrsfm.norm_dim,(em_iter>max_em_iter/2));    % Occlusion reasoning after half iterations over
    end

    %% updates rotation
    [RO, R] = mstep_update_rotation(P_hat, S_bar, V, E_z, E_zz, RO, c, Tr);       % (Eq 24)

    %RO = rotP3d;
    %for t = 1:T
    %    R(t,:) = RO{t}(1,:);
    %    R(t+T,:) = RO{t}(2,:);
    %end

    %% update C here
    %if scaling
        c = mstep_update_c(P_hat, S_bar, V, E_z, E_zz, RO, Tr); % Eq 48 (PAMI paper)
        frac = mean(c);
        c = c/frac;
        S_bar = frac*S_bar;
        V = frac*V;
    %end

    %% updates translation
    Tr = mstep_update_transl(P_hat, S_bar, V, E_z, RO, c);                        % (Eq 23)

    %% updates noise variance
    sigma_sq = mstep_update_noisevar(P_hat, S_bar, V, E_z, E_zz, RO, Tr, c);      % (Eq 22)
    if em_iter < max_anneal_iter,
        sigma_sq = sigma_sq * (1 + annealing_const*(1 - em_iter/max_anneal_iter));
    end

    %visualize_3D_point(S_bar,params.part_names,wireframe_car(),h);figure(h);clf;
    %% computes log likelihood
    oldloglik = loglik;
    loglik = compute_log_lik(P_hat, S_bar, V, E_z, E_zz, RO, c, Tr, sigma_sq);

%     if(mod(em_iter,40)==0)
%         fprintf('Iteration %d/%d: Error %f\n', em_iter, max_em_iter, -loglik/(size(P_hat,1)/2));
%     end

    if (em_iter <= 2),
     loglikbase = loglik;
    elseif (loglik < oldloglik)
     %fprintf('Violation');
     %keyboard;
    elseif 0 && ((loglik-loglikbase)<(1 + tol)*(oldloglik-loglikbase)),
     fprintf('\n');
     break;
    end
    pBar.progress();
end
pBar.stop();
%% Check for right handed coordinate flips
vec1 = S_bar(:, rhandedCoords(1,2)) - S_bar(:, rhandedCoords(1,1));
vec2 = S_bar(:, rhandedCoords(2,2)) - S_bar(:, rhandedCoords(2,1));
vec3 = cross(vec1,vec2);
v = S_bar(:, rhandedCoords(3,2)) - S_bar(:, rhandedCoords(3,1));

badCt = 0;
for t = 1:T
   z_t = Z(t,:);
   S = S_bar;
   for kk = 1:K,
      S = S+z_t(kk)*V((kk-1)*3+[1:3],:);
   end
   S = c(t,1)*RO{t}*S;
   Zs = S(3,:);
   badZ = mean(Zs(MD(t,:)));
   goodZ = mean(Zs(~MD(t,:)));
   if(goodZ > badZ)
       badCt = badCt+1;
   end
end
thresh = badCt/T;

isFlipped = 0;
if(thresh > 0.5 || (thresh >0.2 && dot(vec3,v)<0))
    refZ = diag([1 1 -1]);
    rotZ = diag([-1 -1 1]);
    isFlipped = 1;
    S_bar = -S_bar;
    V = -V;
    for i=1:length(RO)
        RO{i} = rotZ*RO{i};
    end
end

%% Changing to PASCAL 3D
fprintf('Converting to PASCAL 3D co-ordinate frame\n');
if(~isempty(rotP3d))
    tformRot = averageRotationTransform(RO,rotP3d);
else
    fprintf('PASCAL 3D rotations not found. Might be a problem with evaluation\n');
    tformRot = averageRotationTransform(RO,RO);
end

%tformRot = eye(3);
S_bar = tformRot'*S_bar;
for t = 1:T
    RO{t}  = RO{t}*tformRot;
end
for kk = 1:K
  V((kk-1)*3+[1:3],:) = tformRot'*V((kk-1)*3+[1:3],:);
end

%% compute the final answer
P3 = zeros(3*T, J);
for t = 1:T,
   z_t = Z(t,:);
   S = S_bar;
   for kk = 1:K,
      S = S+z_t(kk)*V((kk-1)*3+[1:3],:);
   end;
   S = c(t,1)*RO{t}*S; %% changed code here !

   P3([t t+T t+2*T], :) = S + [Tr(t, [1 2]) -mean(S(3,:))]'*ones(1,J);
end


% function fval = f6(x)
% % Unimodal function f_1
%
% Bound = [-500 500];
%
% if nargin == 0
%     fval = Bound;
% else
%     fval = sum(-x.*sin(abs(x).^.5));
% end
function fval=f6(x)
Bound=[-100 100];

if nargin==0
    fval = Bound;
else
    x=abs(floor(x+0.5));
    fval= sum(x);
end


function edgeFaces = trimeshEdgeFaces(faces, varargin)
%TRIMESHEDGEFACES Compute index of faces adjacent to each edge of a triangular mesh.
%
%   EF = trimeshEdgeFaces(FACES)
%   EF = trimeshEdgeFaces(VERTICES, FACES)
%   EF = trimeshEdgeFaces(VERTICES, EDGES, FACES)
%   Compute index array of faces adjacent to each edge of a mesh.
%   FACES is a NF-by-3 array containing vertex indices of each face. The
%   result EF is a NE-by-2 array containing the indices of the two faces
%   incident to each edge. If an edge belongs to only one face, the other
%   face index is ZERO.
%
%   The list of edges (as array of source and target vertex indices) can be
%   obtained from the function 'meshEdges'.
%
%   Note: faces are listed in increasing order for each edge, and no
%   information is kept about relative orientation of edge and face.
%
%   Example
%     % compute incidence list of each edge of an octahedron. For example,
%     % first edge is incident to faces 1 and 5. Second edge is incident to
%     % faces 4 and 8, and so on.
%     [v, f] = createOctahedron;
%     ef = trimeshEdgeFaces(v, f)
%     ef =
%          1     5
%          4     8
%          4     1
%          5     8
%          2     6
%          1     2
%          6     5
%          3     7
%          2     3
%          7     6
%          3     4
%          7     8
%
%   See also
%   meshes3d, meshEdgeFaces, trimeshMeanBreadth, meshEdges

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2015-08-19,    using Matlab 8.5.0.197613 (R2015a)
% Copyright 2015 INRA - Cepia Software Platform.

if nargin == 2
    faces = varargin{1};
elseif nargin == 3
    faces = varargin{2};
end

% compute vertex indices of each edge (in increasing index order)
edges = sort([faces(:,[1 2]) ; faces(:,[2 3]) ; faces(:,[3 1])], 2);

% create an array to keep indices of faces "creating" each edge
nFaces = size(faces, 1);
edgeFaceInds = repmat( (1:nFaces)', 3, 1);

% sort edges, keeping indices
[edges, ia, ib] = unique(edges, 'rows'); %#ok<ASGLU>
nEdges = size(edges, 1);

% allocate memory for result
edgeFaces = zeros(nEdges, 2);

% iterate over edges, to identify incident faces
for iEdge = 1:nEdges
    inds = find(ib == iEdge);
    edgeFaces(iEdge, 1:length(inds)) = edgeFaceInds(inds);
end


function [rec,prec,ap] = VOCevalcls(VOCopts,id,cls,draw)

% load test set
[gtids,gt]=textread(sprintf(VOCopts.clsimgsetpath,cls,VOCopts.testset),'%s %d');

% load results
[ids,confidence]=textread(sprintf(VOCopts.clsrespath,id,cls),'%s %f');

% map results to ground truth images
out=ones(size(gt))*-inf;
tic;
for i=1:length(ids)
    % display progress
    if toc>1
        fprintf('%s: pr: %d/%d\n',cls,i,length(ids));
        drawnow;
        tic;
    end

    % find ground truth image
    j=strmatch(ids{i},gtids,'exact');
    if isempty(j)
        error('unrecognized image "%s"',ids{i});
    elseif length(j)>1
        error('multiple image "%s"',ids{i});
    else
        out(j)=confidence(i);
    end
end

% compute precision/recall

[so,si]=sort(-out);
tp=gt(si)>0;
fp=gt(si)<0;

fp=cumsum(fp);
tp=cumsum(tp);
rec=tp/sum(gt>0);
prec=tp./(fp+tp);

% compute average precision

ap=0;
for t=0:0.1:1
    p=max(prec(rec>=t));
    if isempty(p)
        p=0;
    end
    ap=ap+p/11;
end

if draw
    % plot precision/recall
    plot(rec,prec,'-');
    grid;
    xlabel 'recall'
    ylabel 'precision'
    title(sprintf('class: %s, subset: %s, AP = %.3f',cls,VOCopts.testset,ap));
end


function net = fcnInitializeModel8sGeneric(labelCount, net)
%FCNINITIALIZEMODEL8S Initialize the FCN-16S model from FCN-32

%% Remove the last layer
net.removeLayer('deconv16') ;

%% Add the first deconv layer
filters = single(bilinear_u(4, 1, labelCount)) ;
net.addLayer('deconv2bis', ...
  dagnn.ConvTranspose('size', size(filters), ...
                      'upsample', 2, ...
                      'crop', 1, ...
                      'hasBias', false), ...
             'x41', 'x42', 'deconv2bisf') ;
f = net.getParamIndex('deconv2bisf') ;
net.params(f).value = filters ;
net.params(f).learningRate = 0 ;
net.params(f).weightDecay = 1 ;

%% Add a convolutional layer that take as input the pool3 layer
net.addLayer('skip3', ...
     dagnn.Conv('size', [1 1 256 labelCount]), ...
     'x17', 'x43', {'skip3f','skip3b'});

f = net.getParamIndex('skip3f') ;
net.params(f).value = zeros(1, 1, 256, labelCount, 'single') ;
net.params(f).learningRate = 0.01 ;
net.params(f).weightDecay = 1 ;

f = net.getParamIndex('skip3b') ;
net.params(f).value = zeros(1, 1, labelCount, 'single') ;
net.params(f).learningRate = 2 ;
net.params(f).weightDecay = 1 ;

%% Add the sumwise layer
net.addLayer('sum2', dagnn.Sum(), {'x42', 'x43'}, 'x44') ;

%% Add deconvolutional layer implementing bilinear interpolation
filters = single(bilinear_u(16, labelCount, labelCount)) ;
net.addLayer('deconv8', ...
  dagnn.ConvTranspose('size', size(filters), ...
                      'upsample', 8, ...
                      'crop', 4, ...
                      'numGroups', labelCount, ...
                      'hasBias', false), ...
             'x44', 'prediction', 'deconv8f') ;

f = net.getParamIndex('deconv8f') ;
net.params(f).value = filters ;
net.params(f).learningRate = 0 ;
net.params(f).weightDecay = 1 ;

% Make the output of the bilinear interpolator is not discared for
% visualization purposes
net.vars(net.getVarIndex('prediction')).precious = 1 ;

% empirical test
if 0
  figure(100) ; clf ;
  n = numel(net.vars) ;
  for i=1:n
    vl_tightsubplot(n,i) ;
    showRF(net, 'input', net.vars(i).name) ;
    title(sprintf('%s', net.vars(i).name)) ;
    drawnow ;
  end
end


function out = spm_dartel_norm(job)
% Warp individuals to match template
% FORMAT spm_dartel_norm(job)
% job.flowfields - Flow-fields
% job.images     - Image to warp
% job.interp     - Interpolation method
% job.K          - 2^K timesteps are used
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_dartel_norm.m 5668 2013-10-03 18:34:18Z guillaume $

PU = job.flowfields;
PI = job.images;
jactransf = job.jactransf;
intrp    = job.interp;
K        = job.K;

for i=1:numel(PU),
    drawnow;
    NU = nifti(PU{i});
    [pth,nam,ext,num] = spm_fileparts(NU.dat.fname);
    fprintf('%s: ',nam);
    if jactransf,
        [y,dt] = spm_dartel_integrate(NU.dat,[1 0], K);
        dt     = max(dt,eps);
        if jactransf~=1,
            dt = dt.^jactransf;
        end
    else
        y      = spm_dartel_integrate(NU.dat,[1 0], K);
    end;
    y1 = double(y(:,:,:,1));
    y2 = double(y(:,:,:,2));
    y3 = double(y(:,:,:,3));

    for m=1:numel(PI),
        [pth1,nam,ext,num] = spm_fileparts(PI{m}{i});
        NI = nifti(fullfile(pth1,[nam ext]));
        NO = NI;
        if jactransf,
            NO.dat.fname=fullfile(pth,['mw' nam ext]);
            NO.dat.scl_slope = 1.0;
            NO.dat.scl_inter = 0.0;
            NO.dat.dtype     = 'float32-le';
        else
            NO.dat.fname=fullfile(pth,['w' nam ext]);
        end;
        NO.dat.dim = [NU.dat.dim(1:3) NI.dat.dim(4:end)];
        NO.mat  = NU.mat;
        NO.mat0 = NU.mat;
        NO.mat_intent  = 'Aligned';
        NO.mat0_intent = 'Aligned';
        NO.descrip = 'Dartel warped';
        NO.extras  = [];
        create(NO);
        fprintf('%s',nam); drawnow;

        for j=1:size(NI.dat,4),
            if sum(sum((NI.mat  - NU.mat ).^2)) < 0.0001 && ...
               sum(sum((NI.mat0 - NU.mat0).^2)) < 0.0001,
                ty1 = y1;
                ty2 = y2;
                ty3 = y3;
            else
                mat = NI.mat;
                if ~isempty(NI.extras) && isstruct(NI.extras) && isfield(NI.extras,'mat'),
                    mat1 = NI.extras.mat;
                    if size(mat1,3) >= j && sum(sum(mat1(:,:,j).^2)) ~=0,
                        mat = mat1(:,:,j);
                    end;
                end;
                M   = mat\NU.mat0;
                ty1 = M(1,1)*y1 + M(1,2)*y2 + M(1,3)*y3 + M(1,4);
                ty2 = M(2,1)*y1 + M(2,2)*y2 + M(2,3)*y3 + M(2,4);
                ty3 = M(3,1)*y1 + M(3,2)*y2 + M(3,3)*y3 + M(3,4);
            end;
            for k=1:size(NI.dat,5),
                for l=1:size(NI.dat,6),
                    f  = NI.dat(:,:,:,j,k,l);
                    spl_param = [intrp,intrp,intrp,0,0,0];
                    if intrp>1,
                        f = spm_bsplinc(f,spl_param);
                    end;
                    f = spm_bsplins(f,ty1,ty2,ty3,spl_param);
                    if jactransf,
                        NO.dat(:,:,:,j,k,l)=f.*dt;
                    else
                        NO.dat(:,:,:,j,k,l)=f;
                    end;
                    fprintf('\t%d,%d,%d', j,k,l); drawnow;
                end;
            end;
        end;
        fprintf('\n'); drawnow;
    end;
end;

PU = job.flowfields;
PI = job.images;
jactransf = job.jactransf;
out.files = cell(numel(PU),numel(PI));
for i=1:numel(PU),
    [pth,nam] = spm_fileparts(PU{i});
    for m=1:numel(PI),
        [pth1,nam,ext,num] = spm_fileparts(PI{m}{i});
        if jactransf,
            fname = fullfile(pth,['mw' nam ext]);
        else
            fname = fullfile(pth,['w' nam ext]);
        end;
        out.files{i,m} = fname;
    end
end