classdef CPD
    % StaticCPD
    %  Defines the conditional probability distributions that make up the
    %  BPL model

    methods (Static)

        % --
        % Model of global statistics
        % --
        function [logwt,x_ink,x_cov] = score_stat(libclass,M)
            % Compute the log-score of a motor program M, in terms
            % of the simple ink and coverage statistics.
            x_ink = get_ink_overlap(M,'stroke');
            x_cov = get_ratio_coverage_type(M);
            log_ink = libclass.stat.q_ink_emp.pdf(x_ink) - libclass.stat.q_ink_prior.pdf(x_ink);
            log_canvas = libclass.stat.q_canvas_emp.pdf(x_cov) - libclass.stat.q_canvas_prior.pdf(x_cov);
            logwt = log_ink + log_canvas;
        end

        % ---
        % Number of strokes model (Kappa)
        % ---

        function samps = sample_number(libclass,nsamp)
            if ~exist('nsamp','var')
               nsamp = 1;
            end
            samps = zeros(nsamp,1);
            for i = 1:nsamp
                samps(i) = find(mnrnd(1,libclass.pkappa));
            end
        end

        function ll = score_number(libclass,data)
        %   data: vector [n x 1] of number of strokes
        % returns
        %   ll : vector [n x 1] of log-likelihood scores
            assert(isvector(data));
            if any(data>length(libclass.pkappa))
               ll = -inf;
               return
            end
            ll = log(libclass.pkappa(data));
        end

        % ---
        % Sequence of sub-strokes model (z)
        % ---

        function nsub = sample_nsub(libclass,ns)
        % ns    : number of strokes in the character
            pvec = libclass.pmat_nsub(ns,:);
            nsub = find(mnrnd(1,pvec));
            assert(isscalar(nsub));
            assert(~isnan(nsub));
        end

        function samps = sample_sequence(libclass,ns,nsub,nsamp)
        % ns    : number of strokes in the character
        % nsub  : number of sub-strokes in the character (optional)
        % nsamp : number of samples
            if ~exist('nsamp','var')
               nsamp = 1;
            end
            samps = cell(nsamp,1);

            % Produce each of the samples
            for i=1:nsamp

                %% number of sub-strokes
                if (~exist('nsub','var') || isempty(nsub))
                    nsub = CPD.sample_nsub(libclass,ns);
                end

                %% sample the sequence
                sq = zeros(nsub,1);
                pStart = exp(libclass.logStart);
                sq(1) = find(mnrnd(1,pStart));
                for bid=2:nsub
                    prev = sq(bid-1);
                    sq(bid) = find(mnrnd(1,libclass.pT(prev)));
                end
                samps{i} = sq;
            end

            if nsamp==1
               samps = samps{1};
            end

        end

        function [ll,lv] = score_sequence(libclass,ns,ids)
        %  ids: [k x 1] data sequence
        %  ns: [scalar] how many strokes are in the character. If ns<0,
        %    this means to leave out scoring the number of sub-strokes
        %  output is a scalar log-likelihood value
            assert(isvector(ids));
            nsub = length(ids);
            nstate = libclass.N;
            assert(all(ids<=nstate));

            %% score the number of sub-strokes
            sz = size(libclass.pmat_nsub);
            if ns < 0 % do not include this part of the score...
               lp = 0;
            elseif ns > sz(1) || nsub > sz(2)
               lp = -inf;
            else
               lp = log(libclass.pmat_nsub(ns,nsub));
            end

            %% score the sub-stroke ids
            lv = zeros(nsub,1);
            lv(1) = libclass.logStart(ids(1));
            for i=2:nsub
               lv(i) = libclass.logT(ids(i-1),ids(i));
            end
            ll = lp + sum(lv);
            assert(~any(isnan(ll)));

        end

        % ---
        % Shape model (x)
        % ---
        function bspline_stack = sample_shape_type(libclass,subid)
        % Given a vector of cluster ids (id), sample
        % from the vanilla gaussian model
        % associated with it
        %
        % Input
        %  subid [k x 1]: the id (index) of the sub-stroke
        %
        % Output
        %  bspline_stack: [ncpt x 2 x k] sampled spline
             assert(isvector(subid));
             k = length(subid);

             if isunif(libclass)
               bspline_stack = CPDUnif.sample_shape_type(libclass,subid);
               return
             end

             Cov = libclass.shape.Sigma(:,:,subid);
             rows_bspline = mvnrnd(libclass.shape.mu(subid,:),Cov);
             ncpt = size(rows_bspline,2)./2;
             bspline_stack = zeros(ncpt,2,k);
             for i=1:k
                 bspline_stack(:,:,i) = reshape(rows_bspline(i,:),[ncpt 2]);
             end
        end

        function ll = score_shape_type(libclass,bspline_stack,subid)
        %
        % Input
        %  subid [k x 1]: the id (index) of the sub-stroke
        %  bspline_stack: (ncpt x 2 x k) shapes of bsplines
        %
        % Output
        %  ll : [k x 1]: vector of scores
            assert(isvector(subid));
            k = length(subid);
            assert(size(bspline_stack,3)==k);

            if isunif(libclass)
               ll = CPDUnif.score_shape_type(libclass,bspline_stack,subid);
               return
            end

            ll = score_shape_type_helper(libclass,bspline_stack,subid);
        end

        function bspline_stack = sample_shape_token(libclass,bspline_stack)
        %  bspline_stack: (ncpt x 2 x k) shapes of bsplines
             sz = size(bspline_stack);
             bspline_stack = bspline_stack + libclass.tokenvar.sigma_shape .* randn(sz);
        end

        function ll = score_shape_token(libclass,bspline_stack_token,bspline_stack_type)
        %
        % Input
        %  bspline_stack_type: (ncpt x 2 x k) shapes of bsplines
        %  bspline_stack_token: (ncpt x 2 x k) shapes of bsplines
        %
        % Output
        %  ll : [k x 1]: vector of scores
            k = size(bspline_stack_token,3);
            assert(k==size(bspline_stack_type,3));
            sz = size(bspline_stack_token);
            X = bspline_stack_token(:)';
            mu = bspline_stack_type(:)';
            llvec = mvnormpdfln(X,mu,libclass.tokenvar.sigma_shape*ones(size(X)));
            llvec = reshape(llvec,sz);
            ll = sum(llvec,1);
            ll = sum(ll,2);
            ll = squeeze(ll);
        end

        function ll = score_shape_marginalize(libclass,bspline_stack_token,subid)
        %
        % Input
        %  subid [k x 1]: the id (index) of the sub-stroke
        %  bspline_stack: (ncpt x 2 x k) shapes of bsplines
        %
        % Output
        %  ll : [k x 1]: vector of scores
            assert(isvector(subid));
            k = length(subid);
            assert(size(bspline_stack_token,3)==k);

            if isunif(libclass)
               ll = CPDUnif.score_shape_marginalize(libclass,bspline_stack_token,subid);
               return
            end

            ll = score_shape_type_helper(libclass,bspline_stack_token,subid,libclass.tokenvar.sigma_shape^2);
        end

        % --
        % Scale model (y)
        % --

        function invscales = sample_invscale_type(libclass,subid)
            % subid: [k x 1] vector of sub-stroke ids
            %
            assert(isvector(subid));

            if isunif(libclass)
               invscales = CPDUnif.sample_invscale_type(libclass,subid);
               return
            end

            theta = libclass.scale.theta(subid,:);
            invscales = gamrnd(theta(:,1),theta(:,2));
        end

        function lprob = score_invscale_type(libclass,invscales_type,subid)
            % subid: [k x 1] vector of sub-stroke ids
            assert(isvector(invscales_type));
            assert(isvector(subid));
            k = length(subid);
            assert(numel(invscales_type)==k);

            if isunif(libclass)
                lprob = CPDUnif.score_invscale_type(libclass,invscales_type,subid);
                return
            end

            theta = libclass.scale.theta(subid,:);
            prob = gampdf(invscales_type(:),theta(:,1),theta(:,2));
            lprob = log(prob);
            assert(numel(lprob)==k);
        end

        function invscales_token = sample_invscale_token(libclass,invscales_type)
        % Gaussian noise, but don't allow negative scales.
        % Sampling is done by rejection sampling
            sz = size(invscales_type);
            invscales_token = invscales_type + libclass.tokenvar.sigma_invscale.*randn(sz);
            while any(isinf(CPD.score_invscale_token(libclass,invscales_token,invscales_type)))
                invscales_token = invscales_type + libclass.tokenvar.sigma_invscale.*randn(sz);
            end
        end

        function ll = score_invscale_token(libclass,invscales_token,invscales_type)
            % Score the token-level inverse scales.
            % Computes the right normalization constant,
            % since negative values are not allowed.
            %
            % Input
            %  invscales_token: [n x 1] vector
            %  invscales_type: [n x 1] vector
            %
            % Output
            %  ll: [n x 1] vector of log-likelihood
            ll = mvnormpdfln(invscales_token(:)',invscales_type(:)',libclass.tokenvar.sigma_invscale);
            ll = ll(:);
            ll(invscales_token <= 0) = -inf; % don't allow invscales that are negative

            % correction for positive only invscales
            p_below = normcdf(0,invscales_type,libclass.tokenvar.sigma_invscale);
            p_above = 1-p_below;
            ll = ll - log(p_above);
        end

        % --
        % Relation model (R)
        % ---
        function R = sample_relation_type(libclass,previous_strokes)
            nprev = length(previous_strokes);
            stroke_num = nprev+1;
            types = {'unihist','start','end','mid'};
            ncpt = libclass.ncpt;
            if nprev == 0
                indx = 1;
            else
                out = mnrnd(1,libclass.rel.mixprob);
                if any(isnan(out))
                   error('improper relation mixing prob.');
                end
                indx = find(out);
            end
            type = types{indx};
            switch (type)
                case 'unihist'
                    gpos = libclass.Spatial.sample(stroke_num);
                    R = RelationIndependent(type,nprev,gpos);
                case {'start','end'}
                    attach_spot = randint(1,1,[1 nprev]);
                    R = RelationAttach(type,nprev,attach_spot);
                case 'mid'
                    attach_spot = randint(1,1,[1 nprev]);
                    nsub = previous_strokes{attach_spot}.nsub;
                    subid_spot = randint(1,1,[1 nsub]);
                    R = RelationAttachAlong(type,nprev,attach_spot,nsub,subid_spot,ncpt);
                    [~,lb,ub] = bspline_gen_s(ncpt,1);
                    R.eval_spot_type = lb + rand*(ub-lb);
                otherwise
                    error('invalid relation');
            end
        end

        function ll = score_relation_type(libclass,R)
            stroke_num = R.nprev+1;
            types = {'unihist','start','end','mid'};
            logp = log(libclass.rel.mixprob);
            if stroke_num > 1 % prior probability on a particular relation
                indx = strcmp(R.type,types);
                ll = logp(indx);
            else % there was only one option
                ll = 0;
            end
            switch (R.type)
                case 'unihist'
                    ll = ll + libclass.Spatial.score(R.gpos,stroke_num);
                case {'start','end'}
                    ll = ll - log(R.nprev);
                case 'mid'
                    ll = ll - log(R.nprev);
                    ll = ll - log(R.nsub);
                    eval_missing = isempty(R.eval_spot_type);
                    if ~eval_missing
                        ncpt = libclass.ncpt;
                        [~,lb,ub] = bspline_gen_s(ncpt,1);
                        ll = ll - log(ub-lb);
                        if (R.eval_spot_type < lb || R.eval_spot_type > ub)
                            ll = -inf;
                        end
                    end
                otherwise
                    error('invalid relation');
            end
        end

        function eval_spot_token = sample_relation_token(libclass,eval_spot_type)
        % sample an attachment, but within the bounds as defined by the model
            eval_spot_token = eval_spot_type + libclass.tokenvar.sigma_attach.*randn;
            while isinf(CPD.score_relation_token(libclass,eval_spot_token,eval_spot_type))
                eval_spot_token = eval_spot_type + libclass.tokenvar.sigma_attach.*randn;
            end
        end

        function ll = score_relation_token_approx_marginalize(libclass,eval_spot_token)
        % score the token evaluation spot, for attached relations, assuming
        % we are missing the type-level spot.
            assert(isscalar(eval_spot_token));
            ncpt = libclass.ncpt;
            [~,lb,ub] = bspline_gen_s(ncpt,1);
            if (eval_spot_token < lb || eval_spot_token > ub)
                ll = -inf;
                return;
            end

            % cached structure that evaluates this complex integral
            ll = libclass.score_eval_marg(eval_spot_token);
            % ll = -log(ub-lb);
        end

        function ll = score_relation_token(libclass,eval_spot_token,eval_spot_type)
        % score the token evaluation spot, for attached relations, assuming
        % the type-level spot is given
            assert(isscalar(eval_spot_token));
            assert(~isempty(eval_spot_type));
            ncpt = libclass.ncpt;
            [~,lb,ub] = bspline_gen_s(ncpt,1);
            if (eval_spot_token < lb || eval_spot_token > ub)
                ll = -inf;
                return
            end
            ll = mvnormpdfln(eval_spot_token,eval_spot_type,libclass.tokenvar.sigma_attach);

            % correction for bounds
            p_within = normcdf(ub,eval_spot_type,libclass.tokenvar.sigma_attach) - normcdf(lb,eval_spot_type,libclass.tokenvar.sigma_attach);
            ll = ll - log(p_within);
        end

        % ---
        % Local position model (L)
        % ---
        function pos = sample_position(libclass,R,previous_strokes)
            % Given a relation R and the previous strokes,
            %  sample where the position of this stroke should be
            base = getAttachPoint(R,previous_strokes);
            pos = [normrnd(base(1),libclass.rel.sigma_x) normrnd(base(2),libclass.rel.sigma_y)];
        end

        function ll = score_position(libclass,pos,R,previous_strokes)
            base = getAttachPoint(R,previous_strokes);
            ll = mvnormpdfln(pos(1),base(1),libclass.rel.sigma_x) + mvnormpdfln(pos(2),base(2),libclass.rel.sigma_y);
        end

        % ---
        % Image blur model (\sigma_b)
        % ---
        function blur_sigma = sample_image_blur(PM)
            % sample uniformly in [min_blur_sigma, max_blur_sigma]
            rg = PM.max_blur_sigma - PM.min_blur_sigma;
            blur_sigma = rg*rand + PM.min_blur_sigma;
        end

        function ll = score_image_blur(blur_sigma,PM)
            if (blur_sigma > PM.max_blur_sigma || blur_sigma < PM.min_blur_sigma)
                ll = -inf;
                return
            end
            rg = PM.max_blur_sigma - PM.min_blur_sigma;
            ll = -log(rg);
        end

        % ---
        % Pixel noise model (\epsilon)
        % ---
        function epsilon = sample_image_noise(PM)
            % sample uniformly in [min_epsilon, max_epsilon]
            rg = PM.max_epsilon - PM.min_epsilon;
            epsilon = rg*rand + PM.min_epsilon;
        end

        function ll = score_image_noise(epsilon,PM)
            if (epsilon > PM.max_epsilon || epsilon < PM.min_epsilon)
               ll = -inf;
               return
            end
            rg = PM.max_epsilon - PM.min_epsilon;
            ll = -log(rg);
        end

        % ---
        % Image model (I)
        % ---
        function I = sample_image(pimg)
        %  sample a binary image
            I = binornd(1,pimg);
            I = logical(I);
        end

        function ll = score_image(I,pimg)
        %  score the image model
            assert(~isempty(I));
            if islogical(I)
                on = I;
                off = ~I;
            elseif isnumeric(I) % if we having missing data
                missing = isinf(I);
                on = I > 0.5;
                off = I < 0.5;
                on(missing) = false;
                off(missing) = false;
            end
            prob_on = pimg;
            ll_on = sum(log(prob_on(on)));
            ll_off = sum(log(1-prob_on(off)));
            ll = ll_on + ll_off;
        end

%         function ll = score_image(I,pimg)
%         %  score the image model
%             assert(~isempty(I));
%             prob_on = pimg;
%             on = I;
%             off = ~I;
%             ll_on = sum(log(prob_on(on)));
%             ll_off = sum(log(1-prob_on(off)));
%             ll = ll_on + ll_off;
%         end

        % ---
        % Affine model (A)
        % ---
        function sample_A = sample_affine(libclass,nsamp)
            % affine transformation [x-scale,y-scale,x-translate,y-translate]
            % the translation is relative to the center of mass
            % (x-scale and y-scale cannot be 0 or negative)
            %
            % sample_A: [nsamp x 4] each row is a sampled affine warp
            if ~exist('nsamp','var')
                nsamp = 1;
            end

            sample_A = zeros(nsamp,4);

            % sample the image scale
            m_scale = libclass.affine.mu_scale;
            S_scale = libclass.affine.Sigma_scale;
            sample_A(:,1:2) = mvnrnd(m_scale,S_scale,nsamp);

            % sample the translation
            m_x = libclass.affine.mu_xtranslate;
            m_y = libclass.affine.mu_ytranslate;
            s_x = libclass.affine.sigma_xtranslate;
            s_y = libclass.affine.sigma_ytranslate;
            sample_A(:,3) = normrnd(m_x,s_x,nsamp,1);
            sample_A(:,4) = normrnd(m_y,s_y,nsamp,1);

            % scales should not be zero or negative
            if ~(all(vec(sample_A(:,1:2)) > 0))
               warning('sampled scale variable is less than zero');
            end

        end

        % NOTE: doesn't bother to truncate Gaussian properly,
        %       since the variance is so tight it won't matter
        function ll = score_affine(libclass,A)
            if isempty(A)
               A = [1; 1; 0; 0];
            end
            % score the affine warp
            % (the number of elements is 4)
            assert(isvector(A) && numel(A)==4);
            A = A(:);
            if (A(1) <= 0 || A(2) <= 0)
               ll = -inf;
               return
            end

            ll = 0;

            % sample the image scale
            m_scale = libclass.affine.mu_scale(:);
            S_scale = libclass.affine.Sigma_scale;
            ll = ll + mvnormpdfln(A(1:2),m_scale,[],S_scale);

            % sample the translation
            m_x = libclass.affine.mu_xtranslate;
            m_y = libclass.affine.mu_ytranslate;
            s_x = libclass.affine.sigma_xtranslate;
            s_y = libclass.affine.sigma_ytranslate;
            ll = ll + mvnormpdfln(A(3),m_x,s_x);
            ll = ll + mvnormpdfln(A(4),m_y,s_y);
        end

    end
end

function y = isunif(libclass)
    y = any(isnan(libclass.shape.mu));
end

% fast vectorized helper MVN likelihood
function ll = score_shape_type_helper(libclass,bspline_stack,subid,regcov)
    mymu = libclass.shape.mu(subid,:);
    mysd = libclass.shape.vsd(subid,:);
    if exist('regcov','var')
       mysd = sqrt( (mysd.^2) + regcov );
    end
    sz = size(mymu);
    vbspline = reshape(bspline_stack,sz(2),sz(1));
    vbspline = vbspline';
    llv = mvnormpdfln(vbspline(:)',mymu(:)',mysd(:)');
    llv = reshape(llv,sz);
    ll = sum(llv,2);
end


% shift elements from specified dimension
% e.g. if dimIdx = 2, shift = 1, the class shifts the
% elements in dimension 2 by 1 to the right.
%
classdef ElementShiftNode < GraphNode
    properties
        dimIdx = 1;         % which dimension to select from
        shift;   % which elements to select
        circular = 0;       % whether to use circular shift
        shiftIdx;
    end
    methods
        function obj = ElementShiftNode(shift, dimIdx, circular)
            obj = obj@GraphNode('ElementShift',length(shift));
            obj.shift = shift;
            if nargin>=2
                obj.dimIdx = dimIdx;
            end
            if nargin>=3
                obj.circular = circular;
            end
        end

        function obj = forward(obj,prev_layers)
            obj = obj.preprocessingForward(prev_layers);
            input = prev_layers{1}.a;
            [D(1), D(2), D(3), D(4)] = size(input);
            nElement = D(obj.dimIdx);
            assert(nElement >= obj.shift, ...
                sprintf('%s:forward, Error: shift (%d) is larger than data length (%d), exit. \n', obj.name, obj.shift, nElement));
            obj.shiftIdx = 1:nElement;
            obj.shiftIdx = obj.shiftIdx + obj.shift;
            if obj.circular
                tmp = obj.shiftIdx>nElement;
                obj.shiftIdx(tmp) = obj.shiftIdx(tmp) - nElement;
                tmp = obj.shiftIdx<1;
                obj.shiftIdx(tmp) = obj.shiftIdx(tmp) + nElement;
            else
                obj.shiftIdx(obj.shiftIdx>nElement) = [];
                obj.shiftIdx(obj.shiftIdx<1) = [];
            end

            if obj.dimIdx==1
                obj.a = input(obj.shiftIdx,:,:,:);
            elseif obj.dimIdx==2
                obj.a = input(:,obj.shiftIdx,:,:);
            elseif obj.dimIdx==3
                obj.a = input(:,:,obj.shiftIdx,:);
            elseif obj.dimIdx==4
                obj.a = input(:,:,:,obj.shiftIdx);
            end
            obj = forward@GraphNode(obj, prev_layers);
        end

        function obj = backward(obj,future_layers, prev_layers)
            future_grad = GetFutureGrad(future_layers, obj);

            input = prev_layers{1}.a;
            [D1,D2,T,N] = size(input);

            obj.grad{1} = obj.AllocateMemoryLike([D1 D2 T N], future_grad);
            if obj.dimIdx==1
                obj.grad{1}(obj.shiftIdx,:,:,:) = future_grad;
            elseif obj.dimIdx==2
                obj.grad{1}(:,obj.shiftIdx,:,:) = future_grad;
            elseif obj.dimIdx==3
                obj.grad{1}(:,:,obj.shiftIdx,:) = future_grad;
            elseif obj.dimIdx==4
                obj.grad{1}(:,:,:,obj.shiftIdx) = future_grad;
            end
        end

    end

end


% Comparison Of Source Shapes In Different Dimensions
%
% The time-varying pressure signals recorded from a photoacoustic source
% look different depending on the number of dimensions used in the
% simulation. This difference occurs because a point source in 1D
% corresponds to a plane wave in 3D, and a point source in 2D corresponds
% to an infinite line source in 3D. This examples shows the difference
% between the signals recorded in each dimension. It builds on the
% Simulations in One Dimension, Homogeneous Propagation Medium, and
% Simulations in Three Dimensions examples.
%
% author: Bradley Treeby and Ben Cox
% date: 29th January 2011
% last update: 20th October 2011
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

clear all;

% =========================================================================
% SETTINGS
% =========================================================================

% size of the computational grid
Nx = 64;    % number of grid points in the x (row) direction
x = 1e-3;   % size of the domain in the x direction [m]
dx = x/Nx;  % grid point spacing in the x direction [m]

% define the properties of the propagation medium
medium.sound_speed = 1500;      % [m/s]

% size of the initial pressure distribution
source_radius = 2;              % [grid points]

% distance between the centre of the source and the sensor
source_sensor_distance = 10;    % [grid points]

% time array
dt = 2e-9;                      % [s]
t_end = 300e-9;                 % [s]

% computation settings
input_args = {'DataCast', 'single'};

% =========================================================================
% ONE DIMENSIONAL SIMULATION
% =========================================================================

% create the computational grid
kgrid = makeGrid(Nx, dx);

% create the time array
kgrid.t_array = 0:dt:t_end;

% create initial pressure distribution
source.p0 = zeros(Nx, 1);
source.p0(Nx/2 - source_radius:Nx/2 + source_radius) = 1;

% define a single sensor point
sensor.mask = zeros(Nx, 1);
sensor.mask(Nx/2 + source_sensor_distance) = 1;

% run the simulation
sensor_data_1D = kspaceFirstOrder1D(kgrid, medium, source, sensor, input_args{:});

% =========================================================================
% TWO DIMENSIONAL SIMULATION
% =========================================================================

% create the computational grid
kgrid = makeGrid(Nx, dx, Nx, dx);

% create the time array
kgrid.t_array = 0:dt:t_end;

% create initial pressure distribution
source.p0 = makeDisc(Nx, Nx, Nx/2, Nx/2, source_radius);

% define a single sensor point
sensor.mask = zeros(Nx, Nx);
sensor.mask(Nx/2 - source_sensor_distance, Nx/2) = 1;

% run the simulation
sensor_data_2D = kspaceFirstOrder2D(kgrid, medium, source, sensor, input_args{:});

% =========================================================================
% THREE DIMENSIONAL SIMULATION
% =========================================================================

% create the computational grid
kgrid = makeGrid(Nx, dx, Nx, dx, Nx, dx);

% create the time array
kgrid.t_array = 0:dt:t_end;

% create initial pressure distribution
source.p0 = makeBall(Nx, Nx, Nx, Nx/2, Nx/2, Nx/2, source_radius);

% define a single sensor point
sensor.mask = zeros(Nx, Nx, Nx);
sensor.mask(Nx/2 - source_sensor_distance, Nx/2, Nx/2) = 1;

% run the simulation
sensor_data_3D = kspaceFirstOrder3D(kgrid, medium, source, sensor, input_args{:});

% =========================================================================
% VISUALISATION
% =========================================================================

figure;
[t_sc, t_scale, t_prefix] = scaleSI(t_end);
plot(kgrid.t_array*t_scale, sensor_data_1D./max(abs(sensor_data_1D)), 'b-');
hold on;
plot(kgrid.t_array*t_scale, sensor_data_2D./max(abs(sensor_data_2D)), 'r-');
plot(kgrid.t_array*t_scale, sensor_data_3D./max(abs(sensor_data_3D)), 'k-');
xlabel(['Time [' t_prefix 's]']);
ylabel('Recorded Pressure [au]');
legend('1D', '2D', '3D');


% pigs model from Lauritzen and Nilsson, 2001

seed = 0;
rand('state', seed);
randn('state', seed);

% we number nodes down and to the right
h = [1 5 9 13];
t = [2 6 10];
d = [3 7 11];
u = [4 8 12 14];

N = 14;
dag = zeros(N);

% causal arcs
for i=1:3
  dag(h(i), [t(i) h(i+1)]) = 1;
  dag(d(i), [u(i) h(i+1)]) = 1;
end
dag(h(4), u(4)) = 1;

% information arcs
fig = 2;
switch fig
 case 0,
  % no info arcs
 case 1,
   % no-forgetting policy (figure 1)
   for i=1:3
     dag(t(i), d(i:3)) = 1;
   end
 case 2,
  % reactive policy (figure 2)
  for i=1:3
    dag(t(i), d(i)) = 1;
  end
 case 7,
  % omniscient policy (figure 7: di has access to hidden state h(i-1))
  dag(t(1), d(1)) = 1;
  for i=2:3
    %dag([h(i-1) t(i-1) d(i-1)], d(i)) = 1;
    dag([h(i-1) d(i-1)], d(i)) = 1; % t(i-1) is redundant given h(i-1)
  end
end

ns = 2*ones(1,N);
ns(u) = 1;

% parameter tying
params = ones(1,N);
uparam = 1;
final_uparam = 2;
tparam = 3;
h1_param = 4;
hparam = 5;
dparams = 6:8;

params(u(1:3)) = uparam;
params(u(4)) = final_uparam;
params(t) = tparam;
params(h(1)) = h1_param;
params(h(2:end)) = hparam;
params(d) = dparams;

limid = mk_limid(dag, ns, 'chance', [h t], 'decision', d, 'utility', u, 'equiv_class', params);

% h = 1 means healthy, h = 2 means diseased
% d = 1 means don't treat, d = 2 means treat
% t = 1 means test shows healthy, t = 2 means test shows diseased

if 0
  % use random params
  limid.CPD{final_uparam} = tabular_utility_node(limid, u(4));
  limid.CPD{uparam} = tabular_utility_node(limid, u(1));
  limid.CPD{tparam} = tabular_CPD(limid, t(1));
  limid.CPD{h1_param} = tabular_CPD(limid, h(1));
  limid.CPD{hparam} = tabular_CPD(limid, h(2));
else
  limid.CPD{final_uparam} = tabular_utility_node(limid, u(4), [1000 300]);
  limid.CPD{uparam} = tabular_utility_node(limid, u(1), [0 -100]); % costs have negative utility!

  % h  P(t=1) P(t=2)
  % 1  0.9   0.1
  % 2  0.2   0.8
  limid.CPD{tparam} = tabular_CPD(limid, t(1), [0.9 0.2 0.1 0.8]);

  % P(h1)
  limid.CPD{h1_param} = tabular_CPD(limid, h(1), [0.9 0.1]);

  % hi di P(hj=1) P(hj=2),  j = i+1, i=1:3
  % 1  1  0.8     0.2
  % 2  1  0.1     0.9
  % 1  2  0.9     0.1
  % 2  2  0.5     0.5
  limid.CPD{hparam} = tabular_CPD(limid, h(2), [0.8 0.1 0.9 0.5 0.2 0.9 0.1 0.5]);
end

% Decision nodes get assigned uniform policies by default
for i=1:3
  limid.CPD{dparams(i)} = tabular_decision_node(limid, d(i));
end

fname = '/home/cs/murphyk/matlab/Misc/loopybel.txt';

engines = {};
engines{end+1} = global_joint_inf_engine(limid);
engines{end+1} = jtree_limid_inf_engine(limid);
%engines{end+1} = belprop_inf_engine(limid, 'max_iter', 1*N, 'filename', fname, 'tol', 1e-3);

exact = [1 2];
%approx = 3;
approx = [];

max_iter = 1;
order = d(end:-1:1);
%order = d(1:end);

NE = length(engines);
MEU = zeros(1, NE);
niter = zeros(1, NE);
strategy = cell(1, NE);
for e=1:NE
  [strategy{e}, MEU(e), niter(e)] = solve_limid(engines{e}, 'max_iter', max_iter, 'order',  order);
end
MEU

% check results match those in the paper (p. 22)
direct_policy = eye(2); % treat iff test is positive
never_policy = [1 0; 1 0]; % never treat
tol = 1e-0; % results in paper are reported to 0dp
for e=exact(:)'
  switch fig
   case 2, % reactive policy
    assert(approxeq(MEU(e), 727, tol));
    assert(approxeq(strategy{e}{d(1)}(:), never_policy(:)))
    assert(approxeq(strategy{e}{d(2)}(:), direct_policy(:)))
    assert(approxeq(strategy{e}{d(3)}(:), direct_policy(:)))
   case 1, assert(approxeq(MEU(e), 729, tol));
   case 7, assert(approxeq(MEU(e), 732, tol));
  end
end

for e=approx(:)'
  for i=1:3
    approxeq(strategy{exact(1)}{d(i)}, strategy{e}{d(i)})
    dispcpt(strategy{e}{d(i)})
  end
end


%-------------------------------------------------------------------------------------------------------------
% This is an implementation of the TWSC algorithm for real-world image denoising
%
% Author:  Jun Xu, csjunxu@comp.polyu.edu.hk / nankaimathxujun@gmail.com
%          The Hong Kong Polytechnic University
%
% Please refer to the following paper if you find this code helps:
%
% @article{TWSC_ECCV2018,
% 	author = {Jun Xu and Lei Zhang and David Zhang},
% 	title = {A Trilateral Weighted Sparse Coding Scheme for Real-World Image Denoising},
% 	journal = {ECCV},
% 	year = {2018}
% }
%
% Please see the file License.txt for the license governing this code.
%-------------------------------------------------------------------------------------------------------------
clear;
Original_image_dir = '../dnd_2017/images_srgb/';
fpath = fullfile(Original_image_dir, '*.mat');
im_dir  = dir(fpath);
im_num = length(im_dir);
load '../dnd_2017/info.mat';

method = 'TWSC';
dataset = 'dnd_2017';
% write image directory
write_MAT_dir = ['../' dataset '_Results/'];
write_sRGB_dir = [write_MAT_dir method];
if ~isdir(write_sRGB_dir)
    mkdir(write_sRGB_dir)
end

% set parameters
Par.ps   = 6;       % patch size
Par.step = 3;       % the step of two neighbor patches
Par.win  = 20;      % size of window around the patch
Par.Outerloop = 8;  % iteration number of algorithm
Par.Innerloop = 2;  % iteration number of block matching
Par.nlspini = 70;   % initial number of patches
Par.display = 0;    %
Par.delta   = 0;    %
Par.nlspgap = 0;    % 10
Par.lambda1 = 0;    % set this parameter positive to apply TWSC model
Par.lambda2 = 3;    % parameter for estimating local noise level

alltime  = zeros(im_num, 1, 'double');
for i = 1 :im_num
    Par.image = i;
    load(fullfile(Original_image_dir, im_dir(i).name));
    S = regexp(im_dir(i).name, '\.', 'split');
    [h,w,ch] = size(InoisySRGB);
    % iterate over bounding boxes
    Idenoised_crop_bbs = cell(1,20);
    for j = 1:size(info(1).boundingboxes,1)
        Par.nlsp = Par.nlspini;  % number of non-local patches
        IMinname = [S{1} '_' num2str(j)];
        bb = info(i).boundingboxes(j,:);
        Par.nim = InoisySRGB(bb(1):bb(3), bb(2):bb(4),:);
        Par.I = Par.nim;
        % noise estimation
        for c = 1:ch
            Par.nSig(c) = NoiseEstimation(Par.nim(:, :, c)*255, Par.ps)/255;
        end
        % initial PSNR and SSIM
        fprintf('%s: \n', IMinname);
        % denoising
        t1=clock;
        [IMout, Par]  =  TWSC_Sigma_RW(Par);
        t2=clock;
        % etime(t2,t1)
        alltime(Par.image)  = etime(t2, t1);
        %% output
        IMoutname = sprintf([write_sRGB_dir '/' method '_' dataset '_' IMinname '.png']);
        imwrite(IMout, IMoutname);
        Idenoised_crop_bbs{j} = single(IMout);
    end
    for j = 1:size(info(1).boundingboxes,1)
        Idenoised_crop = Idenoised_crop_bbs{j};
        save(fullfile(write_MAT_dir, sprintf('%04d_%02d.mat', i, j)), 'Idenoised_crop');
    end
    fprintf('Image %d/%d done\n', i,50);
end
% generate submission files
bundle_submission_srgb( write_MAT_dir );


classdef Hyperplane < handle
    properties
        p_;    % point on the plane
        n_;    % Normal of the plane, directional,
        local_bbox_;
    end
    methods
        function obj = Hyperplane(p, n)
            obj.p_ = p;
            obj.n_ = n;
        end
        % Calculate the signed distance from point
        function dis = signed_dist(obj, pt)
            dis = dot(obj.n_, pt - obj.p_);
        end
        % Calculate the distance from point
        function dis = dist(obj, pt)
            dis = abs(obj.signed_dist(pt));
        end
    end
end


% This file standard contains degradation data for the growth of orange trees as used in the article
% An approximation to the log-likelihood function in the Non-Linear Mixed Effects model
% by Pinheiro & Bates (1995,2001)

% Every row contains the degradation or growth observations for one unit/product/object/system
% Replace the data with your own observations

 Y=[30 58 87 115 120 142 145; 33 69 111 156 172 203 203; 30 51 75 108 115 139 140;
          32 62 112 167 179 209 214; 30 49 81 125 142 174 177];

 % Do not change!!!!
 matrixsize_Y=size(Y);
 M=matrixsize_Y(1);
 N=matrixsize_Y(1)*matrixsize_Y(2);


%%% Computes the extrinsic parameters for all the active calibration images

check_active_images;

N_points_views = zeros(1,n_ima);

for kk = 1:n_ima,

    if exist(['x_' num2str(kk)]),

        eval(['x_kk = x_' num2str(kk) ';']);
        eval(['X_kk = X_' num2str(kk) ';']);

        if (isnan(x_kk(1,1))),
            if active_images(kk),
                fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk)
                fprintf(1,'         Set active_images(%d)=1; and run Extract grid corners.\n',kk)
            end;
        end;
        if active_images(kk),
            N_points_views(kk) = size(x_kk,2);
            [omckk,Tckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c);
            [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond);
            if check_cond,
                if (cond(JJ_kk)> thresh_cond),
                    active_images(kk) = 0;
                    omckk = NaN*ones(3,1);
                    Tckk = NaN*ones(3,1);
                    fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk)
                    desactivated_images = [desactivated_images kk];
                end;
            end;
            if isnan(omckk(1,1)),
                %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk])
                active_images(kk) = 0;
            end;
        else
            omckk = NaN*ones(3,1);
            Tckk = NaN*ones(3,1);
        end;

    else

        omckk = NaN*ones(3,1);
        Tckk = NaN*ones(3,1);

        if active_images(kk),
            fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk)
            fprintf(1,'         Set active_images(%d)=1; and run Extract grid corners.\n',kk)
        end;

        active_images(kk) = 0;

    end;

    eval(['omc_' num2str(kk) ' = omckk;']);
    eval(['Tc_' num2str(kk) ' = Tckk;']);

end;

check_active_images;


function z = sppurge(z, options)
% SPPURGE   Purge sparse grid data
%    Z = SPPURGE(Z)  Marks indices that have corresp.
%    hierarchical surplus values larger than the default
%    drop tolerance [0, 100*eps]. The SPPURGE function
%    returns the same sparse grid interpolant data z, but
%    enhanced by a field purgeData that is used by
%    SPINTERP to only consider the marked indices in the
%    interpolation process, thus saving computing time.
%
%    Z = SPPURGE(Z, OPTIONS)  OPTIONS must be an options
%    structure generated with SPSET. Only the value of
%    the DropTol property is used, which enables the user to
%    set any absolute and relative drop tolerance to be used
%    by the purging algorithm.
%
%    See also: SPSET

% Author : Andreas Klimke
% Version: 1.1
% Date   : December 4, 2007

% Change log:
% V1.0   : February 1, 2006
%          Initial version
% V1.1   : December 4, 2007
%          Added Gauss-Patterson grid.

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

if nargin > 1
  % New user-demanded drop tolerance
  dropTol = spget(options, 'DropTol', 'auto');
  % Check if purge data is already present
  if isfield(z, 'dropTol')
    if dropTol == z.dropTol
      % do nothing
    else
      % Remove previous data; it is now invalid.
      if isfield(z,'purgeData')
        z = rmfield(z,'purgeData');
      end
    end
  end
elseif isfield(z, 'dropTol')
  % Update of previously computed sppurge data
  dropTol = z.dropTol;
else
  % Use default drop tolerance.
  dropTol = 'auto';
end
if isa(dropTol, 'char')
	if strcmpi(dropTol, 'off')
		dropTol = [0, 0];
	elseif strcmpi(dropTol, 'auto')
		dropTol = [0, 100*eps];
	else
		error('MATLAB:spinterp:badopt', ...
					['Unknown string value to DropTol property. Did you ' ...
           'accidentally quote the 1x2 drop tolerance vector?']);
	end
elseif isa(dropTol, 'double')
	if length(dropTol) < 2
		error('MATLAB:spinterp:badopt', ...
					['DropTol must be either a string or a 1x2 double ' ...
					 'vector. See ''help spset'' for details.']);
	end
else
	error('MATLAB:spinterp:badopt', ...
				['DropTol must be either a string or a 1x2 double ' ...
				 'vector. See ''help spset'' for details.']);
end

absdroptol = dropTol(1);
reldroptol = dropTol(2);

% Do nothing if drop tolerance is less than or equal to zero
if absdroptol <= 0 && reldroptol <= 0
 % Remove purge data if present
 if isfield(z,'purgeData')
   z = rmfield(z,'purgeData');
   z = rmfield(z,'dropTol');
 end
 return;
end

gridtype = z.gridType;
d = z.d;

if isfield(z, 'indices');
	sparseIndices = 'on';
	indices = z.indices;
else
	sparseIndices = 'off';
end

% Set the currently used drop tolerance
z.dropTol = dropTol;

nout = size(z.vals,1);

if strcmpi(sparseIndices, 'off')
	switch lower(gridtype)
	 case 'clenshaw-curtis'
		getpointsmethod = @spgetnpointscc;
	 case 'maximum'
		getpointsmethod = @spgetnpointsm;
	 case 'noboundary'
		getpointsmethod = @spgetnpointsnb;
	 case 'chebyshev'
		getpointsmethod = @spgetnpointscc;
	 case 'gauss-patterson'
		getpointsmethod = @spgetnpointsnb;
	 otherwise
		error('MATLAB:spinterp:badopt', ...
					['Unknown grid type ''' gridtype '''.']);
	end
	if isfield(z, 'purgeData')
		nfrom = size(z.purgeData,2);
	else
		nfrom = 1;
		z.purgeData{1} = ones(1,nout,'uint8');
		% Omit level 0, since it is clear that there is some data
		% to be considered unless the objective function is f(x) = 0
	end
	nto = size(z.vals,2) - 1;
	% Do this with respect to all outupts
	for k = nfrom:nto
		seq = spgetseq(k,d);
		seqlength = size(seq,1);
		[totalpoints, npoints] = feval(getpointsmethod,seq);
		purgedata = zeros(seqlength,nout,'uint8');
		for l = 1:nout
			droptol = max((z.fevalRange(l,2)- ...
				z.fevalRange(l,1))*reldroptol, absdroptol);
			vals = z.vals{l,k+1};
			zid = uint32(1);
			for m = 1:seqlength
				endzid = zid + npoints(m);
				while zid < endzid
					if abs(vals(zid)) >= droptol
						purgedata(m,l) = 1;
						zid = endzid;
						break;
					else
						zid = zid + 1;
					end
				end
			end
		end
		z.purgeData{k+1} = purgedata;
	end
else
	npoints = z.indices.subGridPoints;
	addr    = z.indices.subGridAddr;
	if isfield(z, 'purgeData')
		idfrom = uint32(size(z.purgeData,1))+1;
	else
		idfrom = uint32(2);
		z.purgeData = ones(1,nout,'uint8');
		% Omit first index set, since it is clear that there is some data
		% to be considered unless the objective function is f(x) = 0
	end
	idto = uint32(size(npoints,1));
	if idto >= idfrom
		purgedata = [z.purgeData; zeros(idto-idfrom+1,nout,'uint8')];
		for l = 1:nout
			droptol = max((z.fevalRange(l,2)- ...
			z.fevalRange(l,1))*reldroptol, absdroptol);
			vals = z.vals{l};
			for m = idfrom:idto
				zid = addr(m);
				endzid = addr(m) + npoints(m);
				while zid < endzid
					if abs(vals(zid)) >= droptol
						purgedata(m,l) = 1;
						break;
					else
						zid = zid + 1;
					end
				end
			end
		end
		z.purgeData = purgedata;
	end
end


function [ y, m, d, ierror ] = ymd_check_roman ( y, m, d )

%*****************************************************************************80
%
%% YMD_CHECK_ROMAN checks a Roman YMD date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, D, the YMD date, which may
%    be corrected if necessary and possible.
%
%    Output, integer IERROR, is 0 if the date is legal.
%
  ierror = 0;
%
%  Check the year.
%
  if ( y <= 0 )
    ierror = 1;
    return
  end
%
%  Check the month.
%
  [ y, m ] = month_borrow_roman ( y, m );

  [ y, m ] = month_carry_roman ( y, m );
%
%  Check the day.
%
  [ y, m, d ] = day_borrow_roman ( y, m, d );

  [ y, m, d ] = day_carry_roman ( y, m, d );

  return
end


function cmap = winterCmap(numGrays,numColors)
%
% cmap = winterCmap(numGrays,numColors)
%
% Makes colormap array with:
%   gray scale - 1:numGrays
%   winter colors - numGrays+1:numGrays+numColors
%
% djh 1/98
if ~exist('numGrays','var')
  numGrays=128;
end
if ~exist('numColors','var')
  numColors=96;
end

cmap = zeros(numGrays+numColors,3);
cmap(1:numGrays+numColors,:) = [gray(numGrays); winter(numColors)];

return


function [h,h_normals] = visualize_model_and_normals(model,vector_scale,NS,point_size,color)

if(~exist('vector_scale','var'))
    vector_scale = 0;
end

if(~exist('NS','var'))
    NS = 1000;
end

if(~exist('point_size','var'))
    point_size = 20;
end

if(~exist('color','var'))
    color = 'black';
end

% Downsample the model to have a lighter version of it
N = length(model.x);

if(NS ~= 0)
    st = ceil(N/NS);
    model = downsample_model(model,st);
end

%--- display the model and the vectors
if(isfield(model,'r') && ~isempty(model.r))
    h = scatter3(model.x,model.y,model.z,point_size,[model.r ,model.g ,model.b ],'fill');

else
    h = scatter3(model.x,model.y,model.z,point_size,color,'fill');
end

if(vector_scale > 0)
    hold on;
    h_normals = quiver3(model.x,model.y,model.z,model.nx,model.ny,model.nz,vector_scale);
end

xlabel('x');
ylabel('y');
zlabel('z');


function imsegs = processSuperpixelImage(fn)
% imsegs = processSuperpixelImage(fn)
% Creates the imsegs structure from a segmentation image
%
% INPUT:
% fn - 1 or 3 channel pixel maps of segmentation images.
% Segments are denoted by different RGB/Grayscale colors.
%
% OUTPUT:
% imsegs - image segmentation data
%
% Copyright(C) Derek Hoiem, Carnegie Mellon University, 2006

if ~iscell(fn)
    fn = {fn};
end

imsegs(length(fn)) = struct('imname', '', 'imsize', [0 0]);
for f = 1:length(fn)
    im = double(fn{f});

    imsegs(f).imname = [];
    imsegs(f).imsize = size(im);
    imsegs(f).imsize = imsegs(f).imsize(1:2);
    if(size(im,3)==3) % i.e. the raw output of FH segmentation
       im = im(:, :, 1) + im(:, :, 2)*256 + im(:, :, 3)*256^2;
    end

    [gid, gn] = grp2idx(im(:));
    imsegs(f).segimage = uint16(reshape(gid, imsegs(f).imsize));
    imsegs(f).nseg = length(gn);
end
imsegs = APPgetSpStats(imsegs);


function [h_polygons, h_circles] = createStadium(mode)

    %% ----- Initialize the obstacles and the arena

    if( strcmp(mode,'Static') )

        obs = [];
        h_polygons = [];
        h_circles = [];

        %Properties of the arena
        arena_r = 25;                           %The arena radius
        [X,Y] = circle([0, 0], arena_r, 1000);
        arena = patch(X,Y,'w');

        % Properties of the obstacles
        n_points = 20;                          %number of points the obstacles are divided into
        %Offset of the origo (0,0)
        offs = 11;

        % Radius of the circle obstacles
        radius = [2.5, 2.5, 4.5, 2.5];

        % Origo of each circle obstacles
        centre = [offs+2, 8; ...
                -offs+1, -offs+1; ...
                0, 25; -2, 13];

        % Create the circle obstacles
        n_circles = size(centre,1);             %number of obstacles
        for i = 1:n_circles
            [X, Y] = circle(centre(i,:), radius(i), n_points);
            obs(i) = patch(X, Y, 'r');
        end

        %check if there already exists circle obstacles
        if( ~isempty(obs))
            h_circles = [obs, arena];

        % Set h_circles equal the arena no circle obstacles exists
        elseif( isempty(obs) )
            h_circles = arena;

        end

        %square = [10, 0; 15, -4; 15, 4; 10, 2];
        %square1 = [10, -22.9; 15, -20; 8, -11.38; 3, -14.28];
        %square1 = [16, -19.21; 18, -17.35; 13, -11.97; 11, -13.83];
        %square1 = [17, -18.33; 19, -16.25; 12, -9.52; 10, -11.6];
        %triangle1 = [16, -19.21; 18, -17.35; 13, -13.923];
        triangle1 = [10, -22.9; 15, -20; 7, -11.967];
        square2 = [-24.92, 2; -24.92, -2; -15, -2; -15, 2];

        %sq1 = patch(square1(:,1), square1(:,2), 'g');
        tri1 = patch(triangle1(:,1), triangle1(:,2), 'g');
        sq2 = patch(square2(:,1), square2(:,2), 'g');

        h_polygons = [tri1, sq2];

    elseif( strcmp(mode,'Dynamic') )

        obs = [];
        h_polygons = [];
        h_circles = [];

        arena_r = 25;
        [X,Y] = circle([0, 0], arena_r, 1000);
        arena = patch(X,Y,'w');
        n_points = 20;
        offs = 11;

        %radius = [2.5, 2.5, 2.5, 5.5];
        radius = [1.8, 1.8, 1.8, 3.5, 1.8, 1.8];
        centre = [0, norm([offs,offs]); ...
                -offs, -offs;  ...
                offs, -offs;  ...
                0, 25; -8, 0; 8, 0];

        n_circles = size(centre,1);
        for i = 1:n_circles
            [X, Y] = circle(centre(i,:), radius(i), n_points);
            obs(i) = patch(X, Y, 'r');
        end

        if( ~isempty(obs))
            h_circles = [obs, arena];

        elseif( isempty(obs) )
            h_circles = arena;

        end

        %square = [10, 0; 15, -4; 15, 4; 10, 2];
        %square1 = [10, -22.9; 15, -20; 8, -11.38; 3, -14.28];
        %square1 = [16, -19.21; 18, -17.35; 13, -11.97; 11, -13.83];
        %square1 = [17, -18.33; 19, -16.25; 12, -9.52; 10, -11.6];
        triangle1 = [16, -19.21; 18, -17.35; 13, -13.923];
        %triangle1 = [10, -22.9; 15, -20; 7, -11.967];
        %square2 = [-24.92, 2; -24.92, -2; -19, -2; -19, 2];
        square2 = [-24.92, 2; -24.92, -2; -18, -2; -18, 2];

        %sq1 = patch(square1(:,1), square1(:,2), 'g');
        tri1 = patch(triangle1(:,1), triangle1(:,2), 'g');
        sq2 = patch(square2(:,1), square2(:,2), 'g');

        h_polygons = [tri1, sq2];
    end
end


function gf = gradddivHexp(Hkt_r, varargin)
% function gf = gradddivH(Hkt_r, varargin)
% gf is 1 x k*t row vector
% Hkt_r = reshape(Hkt,1,k*t) -> row vector
% Vxt = varargin{1};  %data
% Wxk = varargin{2};  %W matrix
% Wxk_fix = varargin{3}; %fixed part of the Wxk matrix (e.g. background) ->rows
% Hkt_fix = varargin{4}; %fixed part (lines) of the H matrix (e.g. background)

alphaH=1; %for now....
Vxt = varargin{1};  %data
Wxk_tmp = varargin{2};  %W matrix
Wxk_fix = varargin{3}; %fixed part of the Wxk matrix (e.g. background) ->rows
Hkt_fix = varargin{4}; %fixed part (lines) of the H matrix (e.g. background)
peval = varargin{5}; %parameters

if ~isfield(peval, 'w_lambda') peval.w_lambda=0; end

t=size(Vxt,2);
k=length(peval.h_dovec);

Hkt_tmp = exp(reshape(Hkt_r,k,t));

Wxk = zeros(peval.numpix, peval.ncomp+1);
Hkt = zeros(peval.ncomp+1, peval.nt);

Wxk(:,peval.w_dovec)=Wxk_tmp;
Hkt(peval.h_dovec,:)=Hkt_tmp;

Wxk(:,peval.w_fixvec)=Wxk_fix;
Hkt(peval.h_fixvec,:)=Hkt_fix;

deltasum=sum(sum(Wxk_tmp,1))-k;
if and(~isempty(Wxk_tmp), abs(deltasum)>10^-6)
    error('Wxk is not correctly normalized! (sum(Wxk_tmp,1)<>1)\n sum(Wxk_tmp,1)=%f',deltasum)
end

gfkt = (1-Wxk'*(Vxt./(Wxk*Hkt)))*alphaH.*Hkt; %d/dh(d-divergence)
% one is tehre because Wxt is normalized: sum(Wxt,1)=1
gf=reshape(gfkt(1:k,:),1,k*t); %making row vector


function h=init_h(method,peval,image,dpixc)
% h=init_h(method,peval,image,dpixc)
% method:   'rand' %random initialization
%           'image' %specified initialization for example image=double(array2im(dpixc_ind))
%           'image_repmat' %for example image = mean(image,3);
%           'res' %image is res.h -> initialisation from the nmf results
% addbackgroundcomponent:   1 adds one component flat component as a background (peval.ncomp th)
%                           0 no background component
if ~isfield(peval, 'bgcomp')
    peval.bgcomp = 1;
    mfprintf(peval.fid, 'Value of the peval.bgcomp was set to %g (by default the last component is backgroud).\n',peval.bgcomp)
end

switch method
    case 'rand'
        h = mean(dpixc(:)-peval.bg)*rand(peval.ncomp,peval.nt);
        msg='h initialzied as uniform random.';
    case 'res'
        h = image;
        msg='h initialized from the results res.h.';
end
h=max(h, eps); % To avoid zeros...

if isfield (peval,'fid')
    mfprintf(peval.fid, [msg '\n'])
else
    fprintf([msg '\n']);
end

if peval.bgcomp
    h(peval.ncomp,:)=peval.nx*peval.ny*peval.bg;
    mfprintf(peval.fid, 'Last component [%g] of h initialised as a flat background. (background=%g)\n',peval.ncomp,peval.bg);
end


function [hd_estimates] = hd_new_for_signres(const,exo,beta,k,n,p,D,m,T,X,Y,data_exo,IRFt,signreslabels_shocks)
%computes the historical decomposition for the series
%ouput hd_estimates = cell array where columns capture variables, and rows
%the contributions of shocks, exogenous, constant and initial conditions to
%these variables. The second to last rows capture the unexplained part for models
%that are not fully identified, while the last row captures the part of
%the fluctuation that should be explained by the VAR, after accounting for
%exogenous/deterministic components.

%row 1 to n = contribution of shock x the movement in variable y hd_estimates(x,y)
%row n+1 = contribution of the constant
%row n+2 = contribution of initial conditions (past shocks)
%row n+3 = unexplained part (for partially identified model
%row n+4 = part that was left to explain by the structural shocks after
%accounting for exogenous, constant and initial conditions
%% Preliminaries

% preliminaries for historical decomposition
%1. Determine how many contributions we are going to calculate
contributors = n + 1 + 1 + length(exo); %variables + constant + initial conditions + exogenous
hd_estimates2=cell(contributors+2,n); %shocks+constant+initial values+exogenous+unexplained+to be explained by shocks only
% number of identified shocks
if IRFt==2 || IRFt==3
    identified=n; % fully identified
elseif IRFt==4 || IRFt==6 %if the model is identified by sign restrictions or sign restrictions (+ IV)
    identified=size(signreslabels_shocks,1); % count the labels provided in the sign res sheet (+ IV)
elseif IRFt==5
    identified=1; % one IV shock
elseif IRFt==1
    identified=0;
end

%===============================================
Bfull=reshape(beta,k,n);                    %get the Bfull matrix
B=Bfull(1:n*p,:);                           %drop the coefficients for all exogenous variables from the matrix
Bcomp = [B'; eye(n*(p-1)) zeros(n*(p-1),n)];%put into companion form
EPS=Y-X*Bfull;                               %get reduced form residuals
ETA=(D\EPS');                        %get structural shocks

%% Compute historical decompositions
%===============================================
% Contribution of each shock
    aux_D = zeros(n*p,n); %auxilary D matrix, that is consistent with companion matrix
    aux_D(1:n,:) = D; %set the first entrys equal to Dinv
    Selec = [eye(n) zeros(n,(p-1)*n)]; %selection matrix
    HDestimates_store = zeros(p*n,T+1,n); %cell aray to store results
    HDestimates = zeros(n,T+1,n);
    for j=1:n % for each variable
        ETAcomp = zeros(n,T+1); %structural shock matrix that is consistent with companion form
        if j <= identified %if j is an identified shock
        ETAcomp(j,2:end) = ETA(j,:); %fill in the entry for shock n, leave the first entry blank
        end
        for i = 2:T+1
            HDestimates_store(:,i,j) = aux_D*ETAcomp(:,i) + Bcomp*HDestimates_store(:,i-1,j); %recursively sum over shock impulse at period i on variable j and the previous period
            HDestimates(:,i,j) =  Selec*HDestimates_store(:,i,j); %select the entry corresponding to the current period
        end
    end

% contribution of the initial values
    HDinitial_storage   = zeros(p*n,T+1);
    HDinitial_estimates = zeros(n, T+1);
    Xnoexo = X(:,1:n*p);
    HDinitial_storage(:,1) = Xnoexo(1,:)'; %set the initial values to the first row of X (n*P)+exo
    HDinitial_estimates(:,1) = Selec*HDinitial_storage(:,1); %select the initial values for the first n variables (i.e. the values at Y_{t-1}
    for i = 2:T+1 %loop over periods and compute the impact of the initial conditions recursively
        HDinitial_storage(:,i) = Bcomp*HDinitial_storage(:,i-1); %compute the impact of those values (which in principle consist of past shocks)
        HDinitial_estimates(:,i) = Selec*HDinitial_storage(:,i);
    end

%  Contribution of the Constant
    HDconstant_storage = zeros(p*n,T+1);
    HDconstant_estimates = zeros(n, T+1);
    Coefficients = zeros(p*n,1);
    if const==1
        Coefficients(1:n,:) = Bfull(n*p+1,:);
        for i = 2:T+1 %loop over periods
            HDconstant_storage(:,i) = Coefficients + Bcomp*HDconstant_storage(:,i-1);
            HDconstant_estimates(:,i) = Selec * HDconstant_storage(:,i);
        end
    end

 % Contribution of exogenous variables, no lags of exogenous variables just
 % the contemporaneous values
 if m > 1
    HDexo_storage = zeros(p*n,T+1);
    HDexo_estimates = zeros(n,T+1);
%     Coefficients_exo = zeros(p*n,(m-const)*(1+p));
    Coefficients_exo = zeros(p*n,m-const);
    data_exocut=X(:,n*p+const+1:end); % take exogenous data from X
%     Coefficients_exo(1:n,:) = Bfull(n*p+const+1:end,:)'; %get the corresponding coefficients
    Coefficients_exo(1:n,:) = Bfull(n*p+const+1:end,:)'; %get the corresponding coefficients
        for i = 2:T+1
            HDexo_storage(:,i) = Coefficients_exo*data_exocut(i-1,:)' + Bcomp*HDexo_storage(:,i-1);
            HDexo_estimates(:,i) = Selec * HDexo_storage(:,i);
        end
  end

 %% put these values into the corresponding cell for hd_estimates such that
 % for variable x (hd_estimates(x,n+1)) = HDinitial_estimates(x,:)
 % for variable x (hd_estimates(x,n+1)) = HDconstant_estimates(x,:)
%reorganize storage
        for jj=1:n %for variables
            for kk=1:T+1 %for periods
                for ii=1:n %for shock contributions
                    hd_estimates2{ii,jj}(1,kk) = HDestimates(jj,kk,ii);
                end
                hd_estimates2{n+1,jj}(1,kk)=HDinitial_estimates(jj,kk);
                if const==1
                hd_estimates2{n+2,jj}(1,kk)=HDconstant_estimates(jj,kk);
                else
                hd_estimates2{n+2,jj}(1,kk)=0;
                end
                if m>1
                hd_estimates2{n+3,jj}(1,kk)=HDexo_estimates(jj,kk);
                end
            end
        end

 HDsum = zeros(T+1,n); %if we sum over all variables this should give Y
 for jj=1:n %loop over variables (columns)
 sumvariable = zeros(1,T+1);
 for kk=1:T+1 %loop over periods
     sumperiod=0;
  for ii=1:contributors %loop over contributors (rows)
      value = hd_estimates2{ii,jj}(1,kk);
      sumperiod = sumperiod+value;
  end
  sumvariable(1,kk) = sumperiod;
 end
 HDsum(:,jj)=sumvariable(1,:);
 end

 %determine the unexplained part (if model is not fully identified)
 aux = zeros(1,n);
 unexplained = Y-HDsum(2:end,:);
 unexplained = [aux; unexplained];
for jj=1:n
    hd_estimates2{contributors+1,jj}=unexplained(:,jj)';
end

%finally substract the sum of the contribution of the
%exogenous, constant,initial conditions from Y to get the
%part that was left to be explained by the shocks (for plotting reasons)
 Exosum = zeros(T+1,n); %if we sum over all variables this should give Y
 for jj=1:n %loop over variables (columns)
 sumvariable = zeros(1,T+1);
 for kk=1:T+1 %loop over periods
     sumperiod=0;
  for ii=n+1:contributors %loop over contributors (rows)
      value = hd_estimates2{ii,jj}(1,kk);
      sumperiod = sumperiod+value;
  end
  sumvariable(1,kk) = sumperiod;
 end
 Exosum(:,jj)=sumvariable(1,:);
 end

 %determine the part that was left to be explained by the shocks
 aux = zeros(1,n);
 tobeexplained = Y - Exosum(2:end,:);
 tobeexplained = [aux; tobeexplained];
for jj=1:n
    hd_estimates2{contributors+2,jj}=tobeexplained(:,jj)';
end

hd_estimates = cell(contributors+2,n);

%drop the initial entry for each cell in hd_estimates
for jj=1:n
    for ii=1:contributors+2
        hd_estimates{ii,jj}(2,:)=hd_estimates2{ii,jj}(2:end);
    end
end


function element_size = p04_element_size ( )

%*****************************************************************************80
%
%% P04_ELEMENT_SIZE returns a typical element size for problem 04.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real ELEMENT_SIZE, a typical element size.
%
  element_size = 0.1;

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   ABB IRB1200.
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   09/01/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function robot = parameters()

robot.name= 'ABB_IRB1200';

robot.DH.theta= '[q(1) q(2)-pi/2 q(3) q(4) q(5) q(6)+pi]';
robot.DH.d='[0.399 0 0 0.351 0 0.082]';
robot.DH.a='[0 0.35 0.042 0 0 0]';
robot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]';
robot.J=[];

robot.inversekinematic_fn = 'inversekinematic_irb1200(robot, T)';
robot.directkinematic_fn = 'directkinematic(robot, q)';

%number of degrees of freedom
robot.DOF = 6;

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-170) deg2rad(170) ; %Axis 1, minimum, maximum
                deg2rad(-100) deg2rad(135); %Axis 2, minimum, maximum
                deg2rad(-200) deg2rad(70); %Axis 3
                deg2rad(-270) deg2rad(270); %Axis 4: Unlimited (400� default)
                deg2rad(-130) deg2rad(130); %Axis 5
                deg2rad(-360) deg2rad(360)]; %Axis 6: Really Unlimited to (800� default)

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(288); %Axis 1, rad/s
                deg2rad(240); %Axis 2, rad/s
                deg2rad(300); %Axis 3, rad/s
                deg2rad(400); %Axis 4, rad/s
                deg2rad(405); %Axis 5, rad/s
                deg2rad(600)];%Axis 6, rad/s

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time

% end effectors maximum velocity
robot.linear_velmax = 7.3; %m/s

%base reference system
%robot.T0 = eye(4);
robot.T0 = [1 0 0 0;
             0 1 0 0;
             0 0 1 0;
             0 0 0 1];

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

% GRAPHICS
robot.graphical.has_graphics=1;
robot.graphical.color = [255 102 51]./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-0.5 0.75 -0.75 0.75 0 2.25];
%read graphics files
robot = read_graphics(robot);

robot.has_dynamics=0;


classdef Two_Arch2 < ALGORITHM
% <multi/many> <real/integer/label/binary/permutation>
% Two-archive algorithm 2
% CAsize --- --- Convergence archive size
% p      --- --- The parameter of fractional distance

%------------------------------- Reference --------------------------------
% H. Wang, L. Jiao, and X. Yao, Two_Arch2: An improved two-archive
% algorithm for many-objective optimization, IEEE Transactions on
% Evolutionary Computation, 2015, 19(4): 524-541.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [CAsize,p] = Algorithm.ParameterSet(Problem.N,1/Problem.M);

            %% Generate random population
            Population = Problem.Initialization();
            CA = UpdateCA([],Population,CAsize);
            DA = UpdateDA([],Population,Problem.N,p);

            %% Optimization
            while Algorithm.NotTerminated(DA)
                [ParentC,ParentM] = MatingSelection(CA,DA,Problem.N);
                Offspring         = [OperatorGA(Problem,ParentC,{1,20,0,0}),OperatorGA(Problem,ParentM,{0,0,1,20})];
                CA = UpdateCA(CA,Offspring,CAsize);
                DA = UpdateDA(DA,Offspring,Problem.N,p);
            end
        end
    end
end


function [forecast_record,forecast_estimates]=panel2forecast(N,n,p,k,data_endo_a,data_exo_p,It,Bu,beta_gibbs,sigma_gibbs,Fperiods,const,Fband,Fstartlocation,favar)

% initiate the cell recording the forecast draws
forecast_record={};
forecast_estimates={};

% because the forecasts have to be computed for each unit, loop over units
for ii=1:N
% run the Gibbs sampler for unit ii
forecast_record(:,:,ii)=bear.forecast(data_endo_a(:,:,ii),data_exo_p,It,Bu,beta_gibbs,sigma_gibbs,Fperiods,n,p,k,const,Fstartlocation,favar);
% obtain point estimates and credibility intervals for unit ii
forecast_estimates(:,:,ii)=bear.festimates(forecast_record(:,:,ii),n,Fperiods,Fband);
end


% Copyright (C) 2013 Quan Wang <wangq10@rpi.edu>,
% Signal Analysis and Machine Perception Laboratory,
% Department of Electrical, Computer, and Systems Engineering,
% Rensselaer Polytechnic Institute, Troy, NY 12180, USA
%
% You are free to use this software for academic purposes if you cite our paper:
% Quan Wang and Kim L. Boyer.
% Feature Learning by Multidimensional Scaling and its Applications in Object Recognition.
% 2013 26th SIBGRAPI Conference on Graphics, Patterns and Images (Sibgrapi). IEEE, 2013.
%
% For commercial use, please contact the authors.

% Visualize the confusion matrix.

function draw_matrix(CM)
% CM: confusion matrix

figure;
imagesc(1-CM,[0 1]);
colormap gray;
N=size(CM,1);
for i=1:N
    for j=1:N
        if CM(i,j)>0.5
            the_color=[1 1 1];
        else
            the_color=[0 0 0];
        end
        text(j-0.2,i,[num2str(CM(i,j)*100,'%.2f') '%'],...
            'Color',the_color,'FontSize',15);
    end
end
title('confusion matrix');
xlabel('predicted label');
ylabel('true label');


function [HDR] = save2mm(fn,MM,montage);
% SAVE2MM  saves Matrix into MatrixMarket format
%
%       HDR = save2mm(filename,M,comment);
%
% filename	destination file
% M		Matrix
%
% see also: SLOAD, getMontage, regress_eog, save2gdf
%

%
%    BioSig 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.
%
%    BioSig 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 BioSig.  If not, see <http://www.gnu.org/licenses/>.

% 	$Id$
%	Copyright (C) 2010 by Alois Schloegl <a.schloegl@ieee.org>
%       This file is part of the biosig project http://biosig.sf.net/

        [I,J,V] = find(MM);
        fid = fopen(fn,'w+');
        fprintf(fid,'%%%%MatrixMarket matrix coordinate real general\n');
        fprintf(fid,'%% generated on %04i-%02i-%02i %02i:%02i:%02.0f\n',clock);

        if ischar(montage) m = montage; else m = '? (user specified)'; end;
        fprintf(fid,'%% Spatial Filter for %s \n',m);
        fprintf(fid,'%i %i %i\n',size(MM),length(V));

        for k = 1:length(V),
                fprintf(fid,'%2i %2i %f\n',I(k),J(k),V(k));
        end;
        fclose(fid);

        HDR.Calib = MM;
        HDR.FileName = fn;
        HDR.TYPE = 'MatrixMarket';


function mm_to_st ( input_filename, output_filename )

%*****************************************************************************80
%
%% MSM_TO_ST converts sparse matrix information from MM to ST format.
%
%  Discussion:
%
%    MM format is the "Matrix Market" format.
%    ST format is the "Sparse Triplet" format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the MM file.
%
%    Input, string OUTPUT_FILENAME, the name of the ST file.
%
  [ a, rows, cols, entries, rep, field, symm ] = mm_to_msm ( input_filename );

  msm_to_st ( a, output_filename );

  return
end


function feats = make_rank_feats(input)

feats = input;
feats(:, [17 18]) = 1./(1+exp(-input(:, [17 18]))); % These are logistic predictions
feats = [feats, feats.^2, ones(size(feats,1), 1)];


function [oscilatior_corr_ifgs,oscilatior_corr_velocity] = env_oscilator_corr(envisat_flag,forced_sm_flag)
% This function perform oscialtor drift correction for envisat interferograms based on
% Petar Marinkovic presentation at ESA Living Planet 2013 in Edinburgh.
% Approximation formula:
% dR/year = c/2 * (slantRangeTime_FAR - slantRangeTime_NEAR) * corrPerYear
% 'Apparent displacement' correction:
% dR/year ~ (7.8m * 5000)*3.87e-7 ~ 0.01482m
% Correction uses the pixel information in range and in adition for
% interferograms the temporal baseline information
%
% OPTIONAL INPUT:
% envisat_flag                  'y' when envisat or 'n'. When empty, a
%                               search is performed in the master.res file
%
% OUTPUTS:
% oscilatior_corr_velocity      Correction in mm for the velocity
% oscilatior_corr_ifgs          Correction for individual interferograms in rad
%
% Correction is defined such that:
% Corrected interferogram/velocity = original interferogram/velocity - correction
%
% P. Marinkovic Envisat oscialtor drift correction coded by David Bekaert -- University of Leeds 2014
%
% cite as:
% P. Marinkovic (PPO.labs) and Y. Larsen (NORUT)
% Consequences of Long-Term ASAR Local Oscillator Frequency Decay - an Empirical Study of 10 Years of Data
% ESA Living Planet Symposium (2013)
%
% Modifications
% 04/2014       DB      Put non-envisat fix to zeros
% 04/2014       DB      Allow forced SM oscialtor drift computation
% 06/2014       DB      Fixed error for SM computation and added extra envisat check.
% 06/2014       DB      Fix in case not envisat and forced SM.
% 11/2014       DB      Fix to make windows and linux compatible
% 03/2015       DB      Clean script output
% 01/2017       DB      Bug fix for non-envisat SM case. n_ifg was not n_image

if nargin<1 || isempty(envisat_flag)
  % checking if this is envisat or not
  platform=getparm('platform');
  if isempty(platform)
    if exist('master.res','file')==2
        master_file = 'master.res';
    elseif exist('../master.res','file')==2
        master_file = '../master.res';
    else
        master_file = [];
    end

    if ~isempty(master_file)

        % make windows and linux compatible
        a = fileread(master_file);
        ix = strfind(a,'ASAR');
       if ~isempty(ix)
           platform='ENVISAT';
       end
    else
        fprintf('Could not check if this is Envisat \n')
    end
  end
  if strcmpi(platform,'ENVISAT')
      envisat_flag = 'y';
      fprintf('This is Envisat, oscilator drift is being removed... \n')
  else
       envisat_flag = 'n';
  end

end

small_baseline_flag = getparm('small_baseline_flag');
if nargin<2
    forced_sm_flag=0;
end

if forced_sm_flag==1
    small_baseline_flag='n';
end

if strcmp(envisat_flag,'y')
    load psver
    % use the ps2.mat data
    ps = load(['ps' num2str(psver) '.mat']);
    lambda = getparm('lambda');

    % velocity map correction:
    envisat_resolution = 7.8;                   % ground range resolution [m]
    Oscilator_drift_corr_year = 3.87e-7;         % drift correction in range [1/year]

    % velocity correction in mm
    oscilatior_corr_velocity = (envisat_resolution*ps.ij(:,3))*Oscilator_drift_corr_year*1000;

    % interferogram
    if strcmp(small_baseline_flag,'y')
        n_ifg = ps.n_ifg;
        delta_year = (ps.ifgday(:,2)-ps.ifgday(:,1))./365.25;
    else
        n_ifg = ps.n_image;
        delta_year = (ps.day-ps.master_day)./365.25;
    end
    oscilatior_corr_ifgs = -4.*pi./lambda.*repmat(oscilatior_corr_velocity,1,n_ifg)./1000.*repmat(delta_year',ps.n_ps,1);

else
    load psver
    ps = load(['ps' num2str(psver) '.mat']);

    if strcmp(small_baseline_flag,'y')
        oscilatior_corr_ifgs = zeros([ps.n_ps ps.n_ifg]);
    else
        n_ifg = ps.n_image;     % bug fix DB
        oscilatior_corr_ifgs = zeros([ps.n_ps n_ifg]);
    end
    oscilatior_corr_velocity = zeros([ps.n_ps 1]);

end


function sunDotNormal = computeSunDotNormal(hra)
%     rVectBodyToSun = -1.0 * getPositOfBodyWRTSun(ut, bodyInfo, celBodyData);
%
%     if(norm(rVectBodyToSun) == 0)
%         sunDotNormal = 1.0;
%     else
%         rVectSunECEF = getFixedFrameVectFromInertialVect(ut, rVectBodyToSun, bodyInfo);
%         rECEF = getrVectEcefFromLatLongAlt(zeros(size(long)), long, zeros(size(long)), bodyInfo);
%
%         planarRvectEcef = [rECEF(1,:); rECEF(2,:); zeros(1,size(rECEF,2))];
%         planarRvectSunEcef = [rVectSunECEF(1,:); rVectSunECEF(2,:); zeros(1,size(rVectSunECEF,2))];
%
%         hra = angleNegPiToPi(dang(planarRvectEcef,planarRvectSunEcef));
%
%         sunDotNormal = 0.5 * cos(hra + deg2rad(45)) + 0.5;
%     end
    sunDotNormal = 0.5 * cos(hra + deg2rad(45)) + 0.5;
end


% Demo for DCM for fMRI (distributed responses)
% This demo inverts the DCM for fMRI model, which contains the ballon model
% as a generalized observation function (not affected by stochastic
% innovations).

close all
clear variables

%-----------------------------------------------------------
%-------------- DCM model specification --------------------

%--- Basic settings
TR = 1e0;                      % sampling period (in sec)
n_t = round(1e2/TR);            % number of time samples
dtU = round(2/TR)+1;           % input-on time interval
t0U = round(10/TR)+1;
microDT = 1e-1;               % micro-time resolution (in sec)
homogeneous = 1;              % params of g(x) homogeneous accross regions
reduced_f = 1;
alpha   = 2e2/TR;              % state noise precision
sigma   = 1e0;              % measurement noise precision

%--- Input
u       = zeros(2,n_t);
u(1,t0U:t0U+dtU) = 1;
u(1,4*t0U:4*t0U+dtU) = 1;
u(2,4*t0U:4*t0U+5*dtU) = 1;

%--- DCM structure
% invariant effective connectivity
A = [0 1 1
     1 0 1
     0 1 0];
nreg = size(A,1);
% modulatory effects
B{1} = zeros(nreg,nreg);
B{2} = [0 0 0
        1 0 0
        0 0 0];
% input-state coupling
C = [1 0
     0 0
     0 0];
% gating (nonlinear) effects
D{1} = [0 0 0
        0 0 0
        0 1 0];
D{2} = zeros(nreg,nreg);
D{3} = zeros(nreg,nreg);

%--- Build options/dim structures for model inversion
disp('Extracting HRF parameters...')
[thetaHRF,phiHRF]   = get_HRFparams(TR,microDT,homogeneous);
disp('Done.')
thetaHRF = 0.*thetaHRF;
phiHRF = 0.*phiHRF;
f_fname = @f_DCMwHRF;
g_fname = @g_HRF_distributed;
[options] = prepare_fullDCM(A,B,C,D,TR,microDT);
options.inG.n_phi = 4; % number of spatial modes
options.inG.B = VBA_sigmoid(randn(8,options.inG.n_phi)); % spatial modes
options.inG.ind_hrf = 1:2*nreg;
options.inG.n_reg = nreg;
for i=1:nreg
    options.inG.ind3{i} = options.inG.ind_hrf(end)+1+(i-1)*4:...
        options.inG.ind_hrf(end)+i*4;
end
options.MaxIter = 8; % to quicken inversion
dim.n_theta         = options.inF.ind5(end);
dim.n_phi           = (2+options.inG.n_phi).*nreg;
dim.n               = 5*nreg;
dim.p               = 8;
options.dim=dim;

%--- Build priors for model inversion
indHemo = options.inF.indself+1:dim.n_theta;
priors.muX0 = kron(ones(nreg,1),[0;0;0;0;0]);
priors.SigmaX0 = 0e-1*speye(5*nreg);
priors.muTheta = 1e-1*ones(dim.n_theta,1);
priors.muTheta(options.inF.indself) = -0;
priors.SigmaTheta = 1e-0*eye(dim.n_theta);
priors.SigmaTheta(options.inF.indself,options.inF.indself) = 0;
if reduced_f
    % fix some HRF params to their default values
    priors.muTheta(options.inF.ind1) = thetaHRF(1);
    priors.muTheta(options.inF.ind3) = thetaHRF(3);
    priors.muTheta(options.inF.ind5) = thetaHRF(6);
    priors.SigmaTheta(options.inF.ind1,options.inF.ind1) = 0;
    priors.SigmaTheta(options.inF.ind3,options.inF.ind3) = 0;
    priors.SigmaTheta(options.inF.ind5,options.inF.ind5) = 0;
end
priors.muPhi = 1e-1*ones(dim.n_phi,1);
priors.SigmaPhi = 1e-0*eye(dim.n_phi);
priors.SigmaPhi(options.inG.ind_hrf,options.inG.ind_hrf) = 0;

% NB on hyperpriors:
%   - fix state noise precision using high scale param
%   - use non-informative priors on the residual precision, with high
%   expectation.
% This is because of the first iteration of the hidden states posterior
% update, which has to deviate from the its prior predictive density (as
% derived from the deterministic inversion).
% The following iterations will then work with a realistic (expected)
% residual precision, and adapt.

SC = 1e2;
priors.a_alpha = Inf;%SC*alpha;
priors.b_alpha = 0;%SC;
priors.a_sigma = 1e0;
priors.b_sigma = 1e-4;

% State noise time-dependent covariance structure.
% NB: ratio of neural versus hemodynamic precision
for t = 1:n_t
    dq = 1e4*ones(dim.n,1);
    dq(options.inF.n5) = 1;
    priors.iQx{t} = diag(dq);
end
options.priors = priors;
options.backwardLag = 10;
options.gradF = 0;
options.updateHP = 1;
options.init0 = 0;

%-----------------------------------------------------------
%----------- simulated times series specification ----------

%--- simulated evolution parameters: neuronal level
% A matrix
t_A = exp([ -0.5
            -0.5
            -0.5
            -2.5
            -1.5 ]);
% self-inhibition gain
t_Aself = -0;
% B matrices
t_B{1} = [];
t_B{2} = exp([ -0.5 ]);
% C matrix
t_C = exp([ +0.1 ]);
% D matrices
t_D{1} = -exp([ -1.5 ]);
t_D{2} = [];
t_D{3} = [];

%--- simulated evolution parameters: hemodynamic level
t_E0 = thetaHRF(1)*ones(nreg,1);       % HbO2 extraction fraction gain
t_tau0 = thetaHRF(2)*ones(nreg,1);     % mean blood transit time gain
t_kaf = thetaHRF(3)*ones(nreg,1);      % vasodilatory signal feedback regulation
t_kas = thetaHRF(4)*ones(nreg,1);      % vasodilatory signal decay gain
t_alpha = thetaHRF(6)*ones(nreg,1);    % vessel stifness gain

%--- simulated observation parameters
if ~homogeneous
    p_E0 = phiHRF(1)*ones(nreg,1);       % HbO2 extraction fraction gain
    p_epsilon = phiHRF(2)*ones(nreg,1);  % ratio of intra- and extravascular signal
else
    p_E0 = phiHRF(1);
    p_epsilon = phiHRF(2);
end
p_w = repmat([1;2;-1;-2],nreg,1);   % spatial pattern parameters

%--- Recollect paramters for simulated data
nu = size(u,1);
theta = zeros(dim.n_theta,1);
theta(options.inF.indA) = t_A;
for i=1:nu
    theta(options.inF.indB{i}) = t_B{i};
end
theta(options.inF.indC) = t_C;
for i=1:nreg
    theta(options.inF.indD{i}) = t_D{i};
end
theta(options.inF.indself) = t_Aself;
theta(options.inF.ind1) = t_E0;
theta(options.inF.ind2) = t_tau0;
theta(options.inF.ind3) = t_kaf;
theta(options.inF.ind4) = t_kas;
theta(options.inF.ind5) = t_alpha;
phi(options.inG.ind1) = p_E0;
phi(options.inG.ind2) = p_epsilon;
phi = [ phi(:) ; p_w ];

% Build time series of hidden states and observations
[y,x,x0,eta,e] = VBA_simulate (n_t,f_fname,g_fname,theta,phi,u,alpha,sigma,options);

% display time series of hidden states and observations
displaySimulations(y,x,eta,e);
% disp('--paused--')
% pause

%-----------------------------------------------------------
%------------------- model inversion -----------------------

% Call inversion routine
[posterior,out] = VBA_NLStateSpaceModel(y,u,f_fname,g_fname,dim,options);

% Display results
displayResults(posterior,out,y,x,x0,theta,phi,alpha,sigma);

% Make predictions
try
    options = out.options;
    [xs,ys,xhat,vx,yhat,vy] = VBA_comparePredictions(...
        n_t,theta,phi,u,alpha,sigma,options,posterior,dim);
catch
    disp('------!!Unable to form predictions!!------')
end


function test_resampledesign

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY resampledesign

% since the function to test is in a private directory, we explicitely have to cd into that directory
[ftver, ftpath] = ft_version;
cd(fullfile(ftpath, 'private'));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check that there are fac(N) permutations

for ntrial=1:10
  clear design
  design(1,:) = ones(1,ntrial);
  design(2,:) = 1:ntrial;

  cfg = [];
  cfg.ivar = 1;
  cfg.resampling = 'permutation';
  cfg.numrandomization = 'all';
  res = resampledesign(cfg, design);

  if size(res,1)~=max(cumprod(1:size(design,2)))
    error('incorrect number of permutations')
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% the following is for Irina, who has a blocked fMRI experiment and wants
% to randomize the two conditions over the blocks, keeping the trials
% within a block together

clear design
design = [
  1 1 2 2 1 1 2 2 % condition number
  1 1 2 2 3 3 4 4 % block number
  ];

cfg = [];
cfg.ivar = 1; % condition number
cfg.wvar = 2;
cfg.resampling = 'permutation';
cfg.numrandomization = 'all';
res = resampledesign(cfg, design);

if any((res(:,2:2:end) - res(:,1:2:end)) ~= 1)
  error('trials within a block were not kept together');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear design
design = [
  1 1 2 2
  1 2 3 4 % block number
  ];

cfg = [];
cfg.ivar = 1; % condition number
cfg.wvar = 2;
cfg.resampling = 'permutation';
cfg.numrandomization = 'all';
res = resampledesign(cfg, design);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

design = [
  1 2 1 2 1 2 1 2
  1 2 3 4 5 6 7 8
  ];
cfg = [];
cfg.ivar = 1;
cfg.wvar = 2;
cfg.numrandomization = 'all';
cfg.resampling = 'permutation';
res = resampledesign(cfg, design);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

design = [
  1 2 3 4 5 6 7 8
  ];

cfg = [];
cfg.resampling = 'bootstrap';
cfg.numrandomization = 100;
res = resampledesign(cfg, design);

if ~all(size(res)==[100 8])
  error('incorrect bootstrap resampling');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

design = [
  1 2 2   % condition number
  1 2 3   % trial number
  ];
cfg = [];
cfg.ivar = 1;
cfg.numrandomization = 'all';
cfg.resampling = 'permutation';
cfg.efficient = 'no';  % NOTE THIS ONE
res = resampledesign(cfg, design);
if size(res,1)~=6
  error('incorrect number of permutations');
end

cfg.efficient = 'yes';  % NOTE THIS ONE
res = resampledesign(cfg, design);
if size(res,1)~=3
  error('incorrect number of permutations');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% here it is invalid to swap the condition for blocks, because the actual
% replications in the data would get scattered over blocks.

design = [
  1 1 1 2 2 1 1 1 2 2   % condition/ivar
  1 1 1 2 2 3 3 3 4 4   % block/wvar
  ];
cfg = [];
cfg.ivar = 1;
cfg.wvar = 2;
cfg.numrandomization = 'all';
cfg.resampling = 'permutation';
try
  % this should result in an error
  res = resampledesign(cfg, design);
  error_detected = false;
catch
  % this is ok
  error_detected = true;
end
if ~error_detected
  error('unequal block length with wvar should be caught as error');
end


close all;
clear all;
clc;

data_file_path = 'bin/mmv_phase_transition_snr_40db_s_16.mat';
options.export = true;
options.export_dir = 'bin';
options.export_name = 'mmv_snr_40_db_s_16';
options.chosen_ks = [2, 4, 8, 16, 32, 64];
options.subtitle = 'MMV, SNR=40dB s=16';
spx.pursuit.PhaseTransitionAnalysis.print_results(data_file_path, ...
    'CoSaMP', options);


function N = congruenceblocks(exponent_m,exponent_p,options,csclasses)
%CONGRUENCEBLOCKS Partitions monomials based on sign symmetry
%
% V = CONGRUENCEBLOCKS(P)
%
% Input
%  V : Vector with SDPVAR objects
%  P : Scalar SDPVAR object
%
% Output
%  V : Cell with SDPVAR objects
%
% Example:
%
% sdpvar x y z
% p = 1+x*y+x^4+y^4+z+z^6;
% v = newtonmonoms(p);
% v = congruenceblocks(v,p);
% sdisplay(v{1}) % Even w.r.t (x,y)
% sdisplay(v{2}) % Odd w.r.t (x,y)
%
% See also NEWTONREDUCE, NEWTONMONOMS, CONSISTENT

sdpvarout = 0;
if isa(exponent_m,'sdpvar')
    z = depends(exponent_p);
    z = recover(unique([depends(exponent_p) depends(exponent_m)]));
    [exponent_p,p_base] = getexponentbase(exponent_p,z);
    [m,m_base] = getexponentbase(exponent_m,z);
    exponent_m = cell(1);exponent_m{1} = m;
    sdpvarout = 1;
end

if nargin < 3
    options.verbose = 0;
    options.sos.congruence = 2;
end

if nargin < 4
    csclasses = 1;
end

if ~isempty(exponent_m{1}) & options.sos.congruence>0 & ((size(exponent_p,2)<=16)  | options.sos.congruence==1)

    % **********************************************
    % DEFINE CONGRUENCE CLASSES
    % **********************************************
    if options.verbose>0;fprintf('Finding symmetries..............');end;
    n = size(exponent_p,2);
    t = cputime;
    switch options.sos.congruence
        case 1
            Htemp = eye(n);                 % CHEAP VERSION; ONLY CHECK IF IT IS EVEN WRT x_i
        case 2
            Htemp = dec2decbin(1:2^n-1,n)'; % ALL POSSIBLE COMBINATIONS
        otherwise
            error('sos.congruence should be 0, 1 or 2')
    end
    %try
        H = Htemp(:,find(~any(rem(exponent_p*Htemp,2),1))); % Find "even" rows
%     catch
%         i = [];
%         % Loop instead
%         for j = 1:size(Htemp,2)
%             if ~any(rem(exponent_p*Htemp(:,j),2))
%                 i = [i j];
%             end
%         end
%     end
    if isempty(H)
        N = exponent_m;
        if options.verbose>0;disp(['Found no symmetries (' num2str(cputime-t) 'sec)']);end
        return
    end

    t = cputime-t;
    if size(H,2)>=1
        if options.verbose>0
            if size(H,2)>1
                disp(['Found ' num2str(size(H,2)) ' symmetries  (' num2str(t) 'sec)']);
            else
                disp(['Found ' num2str(size(H,2)) ' symmetry  (' num2str(t) 'sec)']);
            end
        end
    else
        if options.verbose>0;disp(['Found no symmetries  (' num2str(t) 'sec)']);end;
    end

    % **********************************************
    % CLASSIFY MONMS ACCORDING TO CONGRUENCE CLASSES
    % **********************************************
    if size(H,2)>=1
        the_text = 'Partitioning using symmetry.....';
    end
    N = cell(0,1);
    for cs = 1:length(csclasses)
        [ur,j,k]=uniquesafe(mod(exponent_m{cs}*H,2),'rows');
        Ntemp = cell(size(ur,1),1);
        temp = [];
        for i = 1:length(k)
            Ntemp{k(i),1} = [Ntemp{k(i)};exponent_m{cs}(i,:)];
            temp = [temp;size(exponent_m{cs}(i,:),1)];
        end

        for i = 1:length(Ntemp)
            N{end+1,1} = Ntemp{i};
        end
    end
    % **********************************************
    % PRINT SOME RESULTS
    % **********************************************
    if size(H,2)>=1
        [uu,ii,oo] = uniquesafe(cellfun('prodofsize',N)/size(N{1},2));
        for i = 1:length(uu)
            n_this = length(find(oo==i));

            the_text = [the_text num2str(uu(i)) 'x' num2str(uu(i)) '(' num2str(n_this) ')' ' '];
        end
    end
    if options.verbose>0;;disp(the_text);end;
else
    N = exponent_m;
end

if sdpvarout
    for i = 1:length(N)
        N{i} = recovermonoms(N{i},z);
    end
end


function h = hinton(w);
%HINTON	Plot Hinton diagram for a weight matrix.
%
%	Description
%
%	HINTON(W) takes a matrix W and plots the Hinton diagram.
%
%	H = HINTON(NET) also returns the figure handle H which can be used,
%	for instance, to delete the  figure when it is no longer needed.
%
%	To print the figure correctly in black and white, you should call
%	SET(H, 'INVERTHARDCOPY', 'OFF') before printing.
%
%	See also
%	DEMHINT, HINTMAT, MLPHINT
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Set scale to be up to 0.9 of maximum absolute weight value, where scale
% defined so that area of box proportional to weight value.

% Use no more than 640x480 pixels
xmax = 640; ymax = 480;

% Offset bottom left hand corner
x01 = 40; y01 = 40;
x02 = 80; y02 = 80;

% Need to allow 5 pixels border for window frame: but 30 at top
border = 5;
top_border = 30;

ymax = ymax - top_border;
xmax = xmax - border;

% First layer

[xvals, yvals, color] = hintmat(w);
% Try to preserve aspect ratio approximately
if (8*size(w, 1) < 6*size(w, 2))
  delx = xmax; dely = xmax*size(w, 1)/(size(w, 2));
else
  delx = ymax*size(w, 2)/size(w, 1); dely = ymax;
end

h = figure('Color', [0.5 0.5 0.5], ...
  'Name', 'Hinton diagram', ...
  'NumberTitle', 'off', ...
  'Colormap', [0 0 0; 1 1 1], ...
  'Units', 'pixels', ...
  'Position', [x01 y01 delx dely]);
set(gca, 'Visible', 'off', 'Position', [0 0 1 1]);
hold on
patch(xvals', yvals', color', 'Edgecolor', 'none');
axis equal;


function ra = scimat_scimat2imref(scimat)
% SCIMAT_SCIMAT2IMREF  Matlab's image reference frame from SCIMAT metadata.
%
% SCIMAT_SCIMAT2IMREF produces an image frame (i.e. imref2d object) as used
% by Matlab to display images in real world coordinates. For example,
% Matlab's imshow() function uses imref2d objects as inputs.
%
% Note that imref2d objects do not support rotations, so the rotation
% matrix SCIMAT.rotmat will be ignored with a warning if it's not the
% identity.
%
% RA = SCIMAT_SCIMAT2IMREF(SCIMAT)
%
%   SCIMAT is a struct with a 2D image and its metadata (see "help
%   scimat"). The image can have several frames (2D+t), but it cannot be
%   3D. The image can also be multi-channel (e.g. RGB).
%
%   RA is the output imref2d object (see "help imref2d").
%
% Example:
%
% % create a SCIMAT image with some offset and non-isotropic pixel spacing
% offset = [2, 3];
% spacing = [0.3, 0.6];
% scimat = scimat_im2scimat(imread('cameraman.tif'), spacing, offset);
%
% % alternative 1: display it using scimat_imagesc
% subplot(2, 1, 1)
% hold off
% scimat_imagesc(scimat)
% axis equal
%
% % alternative 2: display it using a reference frame from scimat_scimat2imref
% ra = scimat_scimat2imref(scimat);
% subplot(2, 1, 2)
% hold off
% imshow(scimat.data, ra);
%
%
% See also: scimat, imref2d, scimat_imagesc, imshow.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2015 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(1, 1);
nargoutchk(0, 1);

% check input dimensions
if (size(scimat.data, 3) > 1)
    error('Implemented only for 2D images, not for 3D images')
end

% Matlab's image frame doesn't support rotation matrices
if (isfield(scimat, 'rotmat') && any(scimat.rotmat(:) ~= [1 0 0 1]'))
    warning('Matlab''s image frame doesn''t support rotation matrices. Ignoring rotation matrix')
end

% get image limits
ImageSize = [size(scimat.data, 1) size(scimat.data, 2)];
xmin = scimat_index2world([1, 1], scimat);
xmax = scimat_index2world(ImageSize, scimat);
XWorldLimits = [xmin(1) xmax(1)];
YWorldLimits = [xmin(2) xmax(2)];

ra = imref2d(ImageSize, XWorldLimits, YWorldLimits);


function [ coor, tri ] = getUniformVector( level )
%GETUNIFORMVECTOR Summary of this function goes here
%   Detailed explanation goes here
if exist(sprintf('./BasicFuncPano/icosahedron2sphere/uniformvector_lvl%d.mat', level), 'file')
    load(sprintf('./BasicFuncPano/icosahedron2sphere/uniformvector_lvl%d.mat', level));
else
    [coor,tri] = icosahedron2sphere(level);
end

end


clear all; close all; clc;
se1=strel('square', 3)
se2=strel('line', 10, 45)


function [ normalisation_options ] = Parse_settings( sigma, patch_type, ratio_neg, num_samples, varargin)
%PARSE_SETTINGS Summary of this function goes here
%   Detailed explanation goes here

    % creating the parameters to use when training colour (intensity) patches
    normalisation_options = struct;

    % this is what currently is expected (although could potentially have
    % bigger or smaller support regions
    normalisation_options.patchSize = [11 11];

    % The region size of a region that is taken for training around an
    % aligned or misaligned landmark
    if(sum(strcmp(varargin,'normalisation_size')))
        ind = find(strcmp(varargin,'normalisation_size')) + 1;
        normalisation_options.normalisationRegion = [varargin{ind}, varargin{ind}];
    else
       normalisation_options.normalisationRegion = [21 21];
    end

    % This specifies the split of data ratios
    normalisation_options.svmRatio = 0.8; % proportion of data used for training SVR
    normalisation_options.logitRatio = 0.1; % proportion of data for training logistic regressors
    % the rest is used for testing and provides the correlation and rms scores

    % should normalised cross correlation or just cross correlation should
    % be used on the patch as an SVR
    normalisation_options.useNormalisedCrossCorr = 1;

    % the patch types to be used (for now 'reg' (raw pixel values), and
    % 'grad' (gradient intensity values)
    normalisation_options.patch_type = patch_type;

    % number of training samples to use
    normalisation_options.numSamples = num_samples;

    normalisation_options.sigma = sigma;

    normalisation_options.rate_negative = ratio_neg;

end


%filename = 'CantileverSquareSYmmetricMesh';
filename = 'CantileverSquareMedium';
%'CantileverSquareSYmmetricMesh';
%'CantileverSquare';
%filename = 'CantileverSquareSmall';
%'CantileverSquareNew';
%'CantileverSquareNewFine';
%'LshapeTriFine';
%'Lshape';
%'LshapeFine';
%'ArchTriFine';
%'Arch_quad_coarse';
%'Bridge_quad_coarse';
%'BridgeCool_Quadrilateral_Bilinear_Structured_Coarse';
%'Bridge';

ptype = 'MACRO';
initial_case = 'given';
m1 = 0.0101;
m2 = 0.0101;
%cost = {'compliance'};
cost = {'stressNorm'};
%cost = {'stressNorm','compliance'};
%weights = [0.55,0.45];
weights = 1;
constraint = {'volumeConstraint'};
filterType = 'PDE';
constraint_case = 'EQUALITY';

Vfrac_initial = 0.3;
optimality_initial = 1e-5;
constr_initial = 1e-5;

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

stressNormExponent_initial = 2;
stressNormExponent_final = 32;

optimizer = 'DualNestedInPrimal';
optimizerUnconstrained = 'PROJECTED GRADIENT';

designVariable = 'MicroParams';
ub = 0.989;
lb = 0.011;
homegenizedVariablesComputer = 'ByVademecum';
%
%vademecumFileName = 'SuperEllipseQMax';
%vademecumFileName = 'SuperEllipseQ2';
vademecumFileName = 'SuperEllipseQOptAnalytic';
%
% designVariable = 'Density';
% homegenizedVariablesComputer = 'ByInterpolation';
% method = 'SIMPALL';
% materialType = 'ISOTROPIC';

kfrac = 2;
nsteps = 20;

plotting = false;
printing = true;
monitoring = true;
monitoring_interval = 1;
maxiter = 2000;


function [varargout, peval]=updates_variational(peval, varargin)
% [varargout, peval]=variationalupdates(peval, varargin)
% Computes variational updates
% dvec = varargin{1};   % data (N x T)
% w = varargin{2};      % initialization for basis (N x peval.ncomp)
% h = varargin{3};      % initialization for a (peval.ncomp x T) and b
% a = h.a;      % initialization for a
% b = h.b;
mfprintf(peval.fid, 'Variational updates.\n')
peval=setDefaultValuesPeval(peval);
dvec = varargin{1};
w = varargin{2};        % initialization for basis (N x peval.ncomp)
h = varargin{3};        % initialization for a (peval.ncomp x T) and b
a = h.a;                % initialization for a
b = h.b;

alph=repmat(peval.alpha,1,peval.ncomp); % prior parametes for Gamma distribution (peval.ncomp x T)
beta=repmat(peval.beta,1,peval.ncomp);  % prior parametes for Gamma distribution (peval.ncomp x T)

[dovec_w, dovec_a]=setDoVec(peval); %sets which components used in updates
lb=zeros(1,peval.maxiter);
lb(1)=sum(lowerbound(dvec, w, alph, beta, a, b));
for updateindex=1:peval.maxiter;
    n=update_n(w, a, b);
    a(dovec_a,:)=update_a(dvec,n(:,dovec_a,:),alph(:,dovec_a));
    w(:,dovec_w)=update_w(dvec, n(:,dovec_w,:));
    lb(updateindex+1)=sum(lowerbound(dvec, w, alph, beta, a, b));
    if peval.showprogress
        plotprogress(w,lb(2:updateindex),peval)
    end
end

varargout = struct('w',w,'a',a,'lb',lb(end));

end % Main function

% Nested fucntions:
function peval=setDefaultValuesPeval(peval)
if ~isfield(peval,'showprogress')
    peval.showprogress = 0;
end

if ~isfield(peval, 'fix_bg_w')
    peval.fix_bg_w=0;
    if peval.addbgcomp
        peval.fix_bg_w=1;
    end
end
if ~isfield(peval, 'fix_bg_a')
    peval.fix_bg_a=0;
    if peval.addbgcomp
        peval.fix_bg_a=1;
    end
end
if peval.fix_bg_w
    mfprintf(peval.fid, '''w'' fixed for background component [%g].\n',peval.ncomp)
end
if peval.fix_bg_a
    mfprintf(peval.fid, '''a'' fixed for background component [%g].\n',peval.ncomp)
end
end

function [dovec_w, dovec_a]=setDoVec(peval)
dovec_w=1:peval.ncomp;
dovec_a=1:peval.ncomp;
if peval.fix_bg_w
    dovec_w=1:peval.ncomp-1;
end
if peval.fix_bg_a
    dovec_a=1:peval.ncomp-1;
end
if isfield(peval, 'fix_w')
    dovec_w = removerows(dovec_w',peval.fix_w)';
end
if isfield(peval, 'fix_a')
    dovec_a = removerows(dovec_a',peval.fix_a)';
end
mfprintf(peval.fid, '''w'' will be updated for components: ')
mfprintf(peval.fid, '['); mfprintf (peval.fid, '%g ', dovec_w); mfprintf (peval.fid, ']\n');
mfprintf(peval.fid, '''a'' will be updated for components: ')
mfprintf(peval.fid, '['); mfprintf (peval.fid, '%g ', dovec_a); mfprintf (peval.fid, ']\n');
end

function plotprogress(w,lb,peval)
imstiled(reshape(w, peval.nx, peval.ny, peval.ncomp),10, 'gray',[],[],1)
plot(lb)
end