classdef TestSequentialLaminateTestedWithNumerics < handle

    properties (Access = private)
        microFile = 'RVE_Square_Triangle_FineFine';
        fileOutputName = 'SeqLaminate';
    end

    properties (Access = protected)
        LaminateDirection
        Theta
        FractionVolume

        StiffTensor
        WeakTensor
        MaterialValues

        MixtureCh
        Rank2Ch
        NumericalCh
        SeqLamCh

        FiberDirection
    end

    methods (Access = public)

        function hasPassed = hasPassed(obj)
            ChNum   = obj.NumericalCh.getValue();
            ChSL    = obj.SeqLamCh.getValue();
            ChMix   = obj.MixtureCh.getValue();
            ChRank  = obj.Rank2Ch.getValue();
            firstCondition  = obj.relativeNorm(ChSL,ChNum)   < 1e-2;
            secondCondition = obj.relativeNorm(ChMix,ChNum)  < 1e-3;
            thirdCondition  = obj.relativeNorm(ChRank,ChNum) < 1e-10;
            hasPassed = firstCondition & secondCondition & thirdCondition;
        end

    end

    methods (Access = protected)

        function compute(obj)
            obj.init();
            obj.computeNumericallyChForLaminate();
            obj.loadFractionVolume()
            obj.computeWeakAndStiffTensorsFromNumericalHomogenizerData();
            obj.computeSequentialLaminateTensor();
            obj.computeRank2HomogenizerTensor();
            obj.computeMixtureTheoryTensor();
        end

    end

    methods (Access = private)

        function init(obj)
           obj.loadLaminateDirection()
           obj.loadFiberDirection()
        end

        function computeNumericallyChForLaminate(obj)
           d = obj.createNumericalHomogenizerDataBase();
           homog = NumericalHomogenizer(d);
           homog.compute();
           obj.NumericalCh    = obj.rotateCh(homog);
           obj.MaterialValues = homog.matValues;
           obj.FractionVolume = homog.cellVariables.volume;
        end

        function d = createNumericalHomogenizerDataBase(obj)
            nDB = NumericalHomogenizerDataBase(obj.microFile);
            d = nDB.dataBase;
            d.outFileName = obj.fileOutputName;
            d.hasToCaptureImage = false;
        end

        function loadFractionVolume(obj)
           obj.Theta = obj.FractionVolume;
        end

        function computeWeakAndStiffTensorsFromNumericalHomogenizerData(obj)
            E1  = obj.MaterialValues.E_plus;
            nu1 = obj.MaterialValues.nu_plus;
            E0  = obj.MaterialValues.E_minus;
            nu0 = obj.MaterialValues.nu_minus;
            obj.StiffTensor = IsotropicConstitutiveTensor(E1,nu1);
            obj.WeakTensor  = IsotropicConstitutiveTensor(E0,nu0);
        end

        function computeSequentialLaminateTensor(obj)
            C0 = obj.WeakTensor;
            C1 = obj.StiffTensor;
            dir{1} = obj.LaminateDirection;
            m1 = 1;
            SeqHomog = VoigtHomogPlaneStressHomogenizer(C0,C1,dir,m1,obj.Theta);
            obj.SeqLamCh  = SeqHomog.getPlaneStressHomogenizedTensor();
        end

        function computeRank2HomogenizerTensor(obj)
            C0 = obj.WeakTensor;
            C1 = obj.StiffTensor;
            dir{1} = obj.LaminateDirection;
            m1 = 1;
            SeqHomog = VoigtPlaneStressHomogHomogenizer(C0,C1,dir,m1,obj.Theta);
            obj.Rank2Ch  = SeqHomog.getPlaneStressHomogenizedTensor();
        end

        function computeMixtureTheoryTensor(obj)
            C1 = obj.StiffTensor;
            C0 = obj.WeakTensor;
            d = [0 0 1];
            dir = Vector3D;
            dir.setValue(d);
            dir.normalize();
            angle = -acos(dot(obj.FiberDirection.getValue(),[1 0 0]));
            vFrac = obj.Theta;
            homogenizer = MixtureTheoryHomogenizer(C1,C0,dir,angle,vFrac);
            obj.MixtureCh = homogenizer.Ch;
        end

        function Ch = rotateCh(obj,homog)
            dir = obj.FiberDirection;
            r = ChRotatorForFiberHomogenizer();
            Ch = r.rotate(dir,homog.cellVariables.Ch());
        end

    end

    methods (Access = private, Static)

        function relNorm = relativeNorm(A,B)
            relNorm = norm(A - B)/norm(B);
        end

    end

    methods (Abstract,Access = protected)
        loadLaminateDirection(obj)
        loadFiberDirection(obj)
    end
end


function CSPFilter = spatial_filtering( x_flt, oldSMC, numPatterns, verbose )

if verbose == 1
%     fprintf( '\tSpatial Filtering...\n\t\t' );
end

CSPFilter = cell( 1, oldSMC.numBands );
for i=1:oldSMC.numBands
    if verbose == 1
        if mod(i, 5) == 0
%             fprintf( '%d', i );
        else
%             fprintf( '.' );
        end
        if mod(i, 100) == 0
%             fprintf( '\n' );
        end
    end

    D1 = x_flt{1, i};
    D2 = x_flt{2, i};
    [W, D] = myTrainCSP( D1, D2 );
    CSPFilter{i}.W = W( :, [1:numPatterns, end-numPatterns+1:end] );
    Dd = diag(D);
    CSPFilter{i}.D = Dd([1:numPatterns, end-numPatterns+1:end]);
end
% fprintf( '\n' );


function [parameters, ll, ht, VCV, scores, diagnostics] = matrix_garch(data,dataAsym,p,o,q,startingvals,options)
% Estimation of symmetric and asymmetric MATRIX multivariate GARCH models
%
% USAGE:
%  [PARAMETERS,LL,HT,VCV,SCORES,DIAGNOSTICS] = matrix_garch(DATA,DATAASYM,P,O,Q,STARTINGVALS,OPTIONS)
%
% INPUTS:
%   DATA         - A T by K matrix of zero mean residuals -OR-
%                    K by K by T array of covariance estimators (e.g. realized covariance)
%   DATAASYM     - [OPTIONAL] K by K by T array of asymmetric covariance
%                    estimators (e.g. RC scaled by indicator functions)
%   P            - Positive, scalar integer representing the number of lags of the innovation process
%   O            - Non-negative scalar integer representing the number of asymmetric lags to include
%   Q            - Non-negative scalar integer representing the number of lags of conditional covariance
%   STARTINGVALS - [OPTIONAL] INCOMPLETE
%   OPTIONS      - [OPTIONAL] Options to use in the optimization (fminunc)
%
% OUTPUTS:
%   PARAMETERS   - K(K+1)/2*(1+P+O+Q) by 1 vector of parameters of the form
%                    [vech(C)' vech(A(1))' ... vech(A(P))' vech(G(1))' ...
%                    vech(G(O))' vech(B(1))' ... vech(B(Q))']'
%   LL           - The log likelihood at the optimum
%   HT           - A [K K T] dimension matrix of conditional covariances
%   VCV          - A numParams^2 square matrix of robust parameter covariances (A^(-1)*B*A^(-1)*t^(-1))
%   SCORES       - A T by numParams matrix of individual scores
%   DIAGNOSTICS  - Structure containing some diagnostic information
%
% COMMENTS:
%    The conditional variance, H(t), of a MATRIX GARCH is modeled as follows:
%
%      H(t) = CC' + A(1)A(1)'.*r_{t-1}'*r_{t-1} + ... + A(P)A(P)'.*r_{t-P}'*r_{t-P}
%                 + G(1)G(1)'.*n_{t-1}'*n_{t-1} + ... + G(O)G(O)'.*n_{t-P}'*n_{t-P}
%                  + B(1)B(1)'.*H(t-1) +...+ B(Q)B(Q)'.*H(t-q)
%
%    where n_{t} = r_{t} .* (r_{t}<0).  If using realized measures, the
%    RM_{t-1} replaces r_{t-1}'*r_{t-1}, and the asymmetric version
%    replaces n_{t-1}'*n_{t-1}
%
% EXAMPLES:
%     % Estimation of a symmetric MATRIX GARCH(1,0,1) model
%     parameters = matrix_garch(data,[],1,0,1);
%     % Estimation of an asymmetric MATRIX GARCH(1,0,1) model
%     parameters = matrix_garch(data,[],1,1,1);
%     % Estimation of a symmetric MATRIX GARCH(1,0,1) model using realized covariacne
%     data = RC % K by K by T 3D of realized covariacne
%     parameters = matrix_garch(RC,[],1,0,1);

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 10/28/2009

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Argument Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 3
        o=0;
        q=0;
        startingvals=[];
        options = [];
    case 4
        q=0;
        startingvals=[];
        options = [];
    case 5
        startingvals=[];
        options = [];
    case 6
        options = [];
    case 7
    otherwise
        error('Between 2 and 6 arguments required.')
end

%data should be TxK, T>K
if ndims(data)==2
    [t,k]=size(data);
    if ~isempty(dataAsym)
        error('If DATA is a T by K matrix, DATAASYM must be empty.');
    end
    temp = zeros(k,k,t);
    dataAsym = zeros(k,k,t);
    for i=1:t
        temp(:,:,i) = data(i,:)'*data(i,:);
        dataAsym(:,:,i) = (data(i,:).*(data(i,:)<0))'*(data(i,:).*(data(i,:)<0));
    end
    data = temp;
elseif ndims(data)==3
    [k,m,t] = size(data);
    if m~=k
        error('DATA must be K by K by T is a 3D array.');
    end
    if ~isempty(dataAsym)
        if ndims(dataAsym)~=3
            error('DATAASYM must be a 3D array with the same dimensions as DATA');
        end
        [k2,m2,t2]=size(dataAsym);
        if any([k m t]~=[k2 m2 t2])
            error('DATAASYM must be a 3D array with the same dimensions as DATA');
        end
    end
end
k2 = k*(k+1)/2;
if min(t,k)<2 || t<k
    error('DATA must be a T by K matrix or a K by K by T 3D array, T>K>1');
end

%p, o, q much be non-negative scalars
if length(p)>1 || any(p<1) || floor(p)~=p
    error('P must be a positive scalar');
end
if isempty(o)
    o = 0;
end
if length(o)>1 || any(o<0) || floor(o)~=o
    error('O must be a non-negative scalar');
end
if o>0 && isempty(dataAsym)
    error('DATAASYM must be non-empty if O>0.')
end

if isempty(q)
    q = 0;
end
if length(q)>1 || any(q<0) || floor(q)~=q
    error('Q must be a non-negative scalar');
end

% Startingvals must have (k*(k+1)/2)(1+p+o+q) parameters
if  ~isempty(startingvals)
    if size(startingvals,2)>size(startingvals,1)
        startingvals = startingvals';
    end
    if length(startingvals)<(p+o+q)
        error('STARTINGVALS should be a P+O+Q by 1 vector');
    end
    % Only validate if provided
    % FIXME: Fix Validation
    % TODO: Fix Validation
    kappa = 2;
    A=startingvals(1:p);
    G=startingvals(p+1:p+o);
    B=startingvals(p+o+1:p+o+q);
    if (sum(A)+sum(G)/kappa+sum(B))>=.999998
        error('Weighted sum of STATINGVALUES must be less than 1. See Comments.');
    end
    if any(A<0) || any(B<0) || any(G<0)
        error('STARTINGVALS must all be nonnegative.');
    end
end

%Make sure options is a valid option structure
if isempty(options)
    options=optimset('fminunc');
    options.Display='iter';
    options.Diagnostics='on';
    options.LargeScale='off';
    options.MaxFunEvals = 1000*k2*(1+p+o+q);
end
try
    optimset(options);
catch ME
    error('OPTIONS is not a valid options structure');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Argument Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Compute the backCast
backCast = zeros(k);
backCastAsym = zeros(k);
tau = max(ceil(sqrt(t)),k);
weights = .06 * .94.^(0:tau);
weights = weights / sum(weights);
for i=1:tau
    backCast = backCast + weights(i) * data(:,:,i);
    if o>0
        backCastAsym = backCastAsym + weights(i) * dataAsym(:,:,i);
    end
end

%Get starting values from scalar_vt_vech
if isempty(startingvals)
    startingOptions=optimset('fminunc');
    startingOptions.Display='off';
    startingOptions.Diagnostics='off';
    startingOptions.LargeScale='off';
    startingOptions.TolX = 1e-4;
    startingOptions.TolFun = 1e-4;
    [scalarVechStartingvals,~,~,~,~,~,diagnostics] = scalar_vt_vech(data,dataAsym,p,o,q,[],[],startingOptions);
    CpC = diagnostics.intercept;

    startingvals = zeros(k2 * (1+p+o+q),1);
    startingvals(1:k2) = chol2vec(chol(CpC)');
    index = k2;
    for i=1:(p+o+q)
        matrixParameters = scalarVechStartingvals(i)*(.02*eye(k) + .98*ones(k));
        startingvals(index+1:index+k2) = chol2vec(chol(matrixParameters)');
        index = index + k2;
    end
end

warning('off','MATLAB:illConditionedMatrix')
ll0 = matrix_garch_likelihood(startingvals,data,dataAsym,p,o,q,backCast,backCastAsym);
[parameters,ll,exitflag,output]=fminunc('matrix_garch_likelihood',startingvals,options,data,dataAsym,p,o,q,backCast,backCastAsym);

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Estimation Robustification
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if exitflag<=0 &&  ll<ll0
    %Did not converge, but function improved
    options.MaxFunEvals=4*100*(p+q);
    options.MaxIter=2*100*(p+q);
    parameters=fminunc('matrix_garch_likelihood',parameters,options,data,dataAsym,p,o,q,backCast,backCastAsym);
end
warning('on','MATLAB:illConditionedMatrix')
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Estimation Robustification
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargout>1
    [ll,lls,ht]=matrix_garch_likelihood(parameters,data,dataAsym,p,o,q,backCast,backCastAsym);
    ll=-ll;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the VCV
%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargout>3
    [VCV,A,B,scores]=robustvcv('matrix_garch_likelihood',parameters,0,data,dataAsym,p,o,q,backCast,backCastAsym);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the VCV
%%%%%%%%%%%%%%%%%%%%%%%%%%%
diagnostics = [];
diagnostics.EXITFLAG=exitflag;
diagnostics.ITERATIONS=output.iterations;
diagnostics.FUNCCOUNT=output.funcCount;
diagnostics.MESSAGE=output.message;
parameterMatrices = zeros(k,k,1+p+o+q);
index = 0;
for i=1:(1+p+o+1)
    temp = vec2chol(parameters(index+1:index+k2));
    parameterMatrices(:,:,i) = temp*temp';
    index=index+k2;
end
diagnostics.C = parameterMatrices(:,:,1);
diagnostics.A = parameterMatrices(:,:,2:p+1);
if o>0
    diagnostics.G = parameterMatrices(:,:,p+2:p+o+1);
else
    diagnostics.G = [];
end

if q>0
    diagnostics.B = parameterMatrices(:,:,p+o+2:p+o+q+1);
else
    diagnostics.B = [];
end


classdef nnspnorm < nntest
  methods (Test)
   function basic(test)
      h = 13 ;
      w = 17 ;
      d = 4 ;
      n = 5 ;
      param = [3, 3, 0.1, 0.75] ;
      x = test.randn(h,w,d,n,'single') ;
      y = vl_nnspnorm(x, param) ;
      dzdy = test.rand(h, w, d, n) ;
      dzdx = vl_nnspnorm(x, param, dzdy) ;
      test.der(@(x) vl_nnspnorm(x,param), x, dzdy, dzdx, test.range * 1e-3) ;
    end
  end
end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcSharpen(Temp,Event,handles)
handles=guidata(handles.MU_matrix_display);

choice = questdlg('Apply to all slices?','All Slices','No','Yes','No');
if isempty(choice)
    warndlg('Image sharpening is cancelled.');
    return;
end
% Handle response
H =  fspecial('unsharp');
switch choice
    case 'No'
        handles.TMatrix(:,:,handles.V.Slice) = imfilter(handles.BMatrix,H,'replicate');
    case 'Yes'
        if length(handles.V.DimSize)>2
            for i= 1: handles.V.DimSize(3)
                handles.TMatrix(:,:,i) = imfilter(handles.TMatrix(:,:,i),H,'replicate');
                MU_update_waitbar(handles.Progress_axes,i,handles.V.DimSize(3));
            end
        else
            handles.TMatrix = imfilter(handles.BMatrix,H,'replicate');
        end
end

MergeM=get(handles.Matrix_name_edit,'String');
set(handles.Matrix_name_edit,'String',[MergeM '_shp']);

% update current display matrix
handles=MU_update_image(handles.Matrix_display_axes,{handles.TMatrix,handles.Mask},handles,0);
guidata(handles.MU_matrix_display, handles);

end


addpath('./scripts');
%% figures
gam = .8;
T = 25;
noise = .2;
seed = 3;
[y, ~, trueSpikes] = gen_data(gam, noise, T, 1, 0.1, [], 1, seed);

fig2_demo_deconvolveAR1(y, gam, 0.4, false, trueSpikes);

%% video
gam = .8;
T = 130;
noise = .2;
seed = 3;
[y, truth, trueSpikes] = gen_data(gam, noise, T, 1, 0.1, [], 1, seed);
fig2_demo_deconvolveAR1(y, gam, 0.4, true, trueSpikes);


classdef CEC2020_F4 < PROBLEM
% <single> <real>
% Expanded Rosenbrock's plus Griewangk's function

%------------------------------- Reference --------------------------------
% C .T. Yue, K. V. Price, P. N. Suganthan, J. J. Liang, M. Z. Ali, B. Y.
% Qu, N. H. Awad, and P. P Biswas, Problem definitions and evaluation
% criteria for the CEC 2020 special session and competition on single
% objective bound constrained numerical optimization, Zhengzhou University,
% China and Nanyang Technological University, Singapore, 2019.
%------------------------------- 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".
%--------------------------------------------------------------------------

    properties
        O;      % Optimal decision vector
        Mat;	% Rotation matrix
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2020.mat'),'Data');
            obj.O = Data{4}.o;
            obj.M = 1;
            if isempty(obj.D) || obj.D < 10
                obj.D   = 5;
                obj.Mat = Data{4}.M_5;
            elseif obj.D < 15
                obj.D   = 10;
                obj.Mat = Data{4}.M_10;
            elseif obj.D < 20
                obj.D   = 15;
                obj.Mat = Data{4}.M_15;
            else
                obj.D   = 20;
                obj.Mat = Data{4}.M_20;
            end
            obj.lower    = zeros(1,obj.D) - 100;
            obj.upper    = zeros(1,obj.D) + 100;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            Y = 0.05*Z*obj.Mat';
            Z = Y + 1;
            temp   = 100*(Z.^2-Z(:,[2:end,1])).^2 + (Z-1).^2;
            PopObj = 1900 + sum(temp.^2/4000-cos(temp)+1,2);
        end
    end
end


function i4mat_max_test ( )

%*****************************************************************************80
%
%% I4MAT_MAX_TEST tests I4MAT_MAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2014
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 7;
  b = 0;
  c = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4MAT_MAX_TEST\n' );
  fprintf ( 1, '  I4MAT_MAX returns the maximum;\n' );

  seed = 123456789;

  [ a, seed ] = i4mat_uniform_ab ( m, n, b, c, seed );

  i4mat_print ( m, n, a, '  Random array:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Maximum entry = %d', i4mat_max ( m, n, a ) );

  return
end


function x = line_cvt_lloyd ( n, a, b, it_num, header, x )

%*****************************************************************************80
%
%% LINE_CVT_LLOYD carries out the Lloyd algorithm.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of generators.
%
%    Input, real A, B, the left and right endpoints.
%
%    Input, integer IT_NUM, the number of iterations to take.
%
%    Input, string HEADER, an identifying string.
%
%    Input, real X(N), the initial point locations.
%
%    Output, real X(N), the final point locations.
%
  x = x(:);
%
%  Print the initial generators.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial generators:\n' );
  fprintf ( 1, '\n' );
  for k = 1 : n
    fprintf ( 1, '  %4d  %f\n', k, x(k,1) );
  end
%
%  Initialize the plotting arrays.
%
  step = 1 : it_num;
  e = nan ( it_num, 1 );
  xm = nan ( it_num, 1 );

  for it = 1 : it_num

    x_plot ( 1:n, it ) = x(1:n,1);

    x_new = line_cvt_lloyd_step ( n, a, b, x );

    e(it) = line_cvt_energy ( n, a, b, x );
    e(it) = max ( e(it), eps );
%
%  Display the energy.
%
    figure ( 1 )
    plot ( step, log ( e ), 'm-*' )
    title ( 'Log (Energy)' )
    xlabel ( 'Step' )
    ylabel ( 'Energy' )
    grid
%
%  Compute the generator motion.
%
    xm(it,1) = sum ( ( x_new(:) - x(:) ).^2 ) / n;
%
%  Display the generator motion.
%
    figure ( 2 )
    plot ( step, log ( xm ), 'm-*' )
    title ( 'Log (Average generator motion)' )
    xlabel ( 'Step' )
    ylabel ( 'Motion' )
    grid
%
%  Update the generators.
%
    x(1:n,1) = x_new(1:n,1);

  end

  x_plot(1:n,it_num+1) = x(1:n,1);
%
%  Print the current generators.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Current generators:\n' );
  fprintf ( 1, '\n' );
  for k = 1 : n
    fprintf ( 1, '  %4d  %f\n', k, x(k,1) );
  end
%
%  Plot the evolution of the locations of the generators.
%
  figure ( 3 )

  y = ( 0 : it_num );
  for k = 1 : n
    plot ( x_plot(k,1:it_num+1), y )
    hold on;
  end
  grid on
  hold off;

  title ( 'Generator evolution.' );
  xlabel ( 'Generator positions' );
  ylabel ( 'Iterations' );
%
%  Save the plots.
%
  figure ( 1 )
  filename = strcat ( header, '_energy.png' );
  print ( '-dpng', filename );
  figure ( 2 )
  filename = strcat ( header, '_motion.png' );
  print ( '-dpng', filename );
  figure ( 3 )
  filename = strcat ( header, '_evolution.png' );
  print ( '-dpng', filename );

  return
end


function c = tapas_softmax_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the softmax observation model for multinomial responses
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Is the decision based on predictions or posteriors? Comment as appropriate.
c.predorpost = 1; % Predictions
%c.predorpost = 2; % Posteriors

% Model name
c.model = 'softmax';

% Sufficient statistics of Gaussian parameter priors

% Beta
c.logbemu = log(1);
c.logbesa = 4^2;

% Gather prior settings in vectors
c.priormus = [
    c.logbemu,...
         ];

c.priorsas = [
    c.logbesa,...
         ];

% Model filehandle
c.obs_fun = @tapas_softmax;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_softmax_transp;

return;


function [xout,yout] = coords2normfig(x,y,h)

if ~exist('h','var')
  h = gca;
end

if ~strcmpi(get(h,'type'),'axes'),
  error('Handle must correspond to an axis');
end

xax  = get(h,'xlim');
yax = get(h,'ylim');

pos = get(h,'position');

xout = (x - pos(1))/pos(3)*(xax(2)-xax(1)+1) + xax(1);
yout = (y - pos(2))/pos(4)*(yax(2)-yax(1)+1) + yax(1);

yisreversed = strcmpi(get(h,'ydir'),'reverse');
xisreversed = strcmpi(get(h,'xdir'),'reverse');
if xisreversed,
  xout = (1-(x - xax(1))/(xax(2)-xax(1)))*pos(3) + pos(1);
else
  xout = (x - xax(1))/(xax(2)-xax(1))*pos(3) + pos(1);
end
if yisreversed,
  yout = (1-(y - yax(1))/(yax(2)-yax(1)))*pos(4) + pos(2);
else
  yout = (y - yax(1))/(yax(2)-yax(1))*pos(4) + pos(2);
end


clear; close all; clc;

% Datasets = {'TUM', 'KITTI', 'Tanks_and_Temples', 'CPC'};
Datasets = {'TUM'};

Methods = {'SIFT-RT-RANSAC'};

Errors = cell(length(Methods),length(Datasets));
Inlier_rates = cell(length(Methods),length(Datasets));
Numbers = cell(length(Methods),length(Datasets));
for d = 1 : length(Datasets)
    dataset = Datasets{d};
    for m = 1 : length(Methods)
        method = Methods{m};

        results_dir = ['../Results/' dataset '/'];
        filename = [results_dir method '.mat'];
        Results = importdata(filename);

        Error = -ones(length(Results), 1);
        Inlier_rate = -ones(length(Results), 2);
        Number =  zeros(length(Results), 2);

        for idx = 1 : length(Results)

            if Results{idx}.status ~=0
                Results{idx}.sgd_error = -1;
                Results{idx}.inlier_rate = [0,0];
                continue;
            end

            F1 = Results{idx}.F_gt;
            F2 = Results{idx}.F_hat;
            size1 = Results{idx}.size_l;
            size2 = Results{idx}.size_r;
            X1 = Results{idx}.X_l';
            X2 = Results{idx}.X_r';
            inliers = Results{idx}.inliers;

            if isfield(Results{idx}, 'sgd_error') ~= 1 || Results{idx}.sgd_error < 0
                Results{idx}.sgd_error = ComputeNormlizedSGD(F1, F2, size1, size2);
            end
            Error(idx) = Results{idx}.sgd_error;

            if isfield(Results{idx}, 'inlier_rate') ~= 1 || isempty(Results{idx}.inlier_rate) == 1
                Results{idx}.inlier_rate = ComputeInlierRate(F1, X1, X2, inliers, size1, size2, 0.003);
            end
            Inlier_rate(idx,:) = Results{idx}.inlier_rate;
            Number(idx,:) = [length(Results{idx}.inliers), sum(Results{idx}.inliers)];
        end

        save(filename, 'Results');

        mask = Error < 0;
        Error(mask) = [];
        Errors{m, d} = Error;
        Inlier_rate(mask,:) = [];
        Inlier_rates{m, d} = Inlier_rate;
        Number(mask, :) = [];
        Numbers{m, d} = Number;
    end
end

% Recall---(Error)
num_pairs = 1000;
X = linspace(0,0.2,20);
for d = 1 : length(Datasets)
    dataset = Datasets{d};
    Y = zeros(length(Methods), length(X));
    for m = 1 : length(Methods)
       method = Methods{m};
       for t = 1 : length(X)
           Y(m, t) = sum(Errors{m,d} < X(t)) / num_pairs;
       end
    end
    figure;
    h = plot(X,Y,'linewidth',3);
    ylim([0 1]);
    legend(h, Methods, 'Location', 'SouthEast');
    title(dataset);
    xlabel('NSGD Threshold');
    ylabel('Recall');
end

threshold = 0.05;
for d = 1 : length(Datasets)
    dataset = Datasets{d};
    disp(['Dataset : ' dataset]);
    disp('method recall inlier_rate_before inlier_rate_after');

    for m = 1 : length(Methods)
       method = Methods{m};

       recall = sum(Errors{m,d} < threshold) / num_pairs;

       [meanInlierRate] = mean(Inlier_rates{m,d});
       before_rate = meanInlierRate(1);
       after_rate = meanInlierRate(2);

       fprintf(sprintf('%s %f %f %f\n', method, recall, before_rate, after_rate));
    end
end


% DEMROBOTWIRELESSFGPLVM3 Wireless Robot data from University of Washington with dynamics and no back constraints.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'robotWireless';
experimentNo = 3;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('ftc');

latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Add dynamics model.
options = gpOptions('ftc');
options.kern = kernCreate(model.X, {'rbf', 'white'});
options.kern.comp{1}.inverseWidth = 0.2;
% This gives signal to noise of 0.1:1e-3 or 100:1.
options.kern.comp{1}.variance = 0.1^2;
options.kern.comp{2}.variance = 1e-3^2;
model = fgplvmAddDynamics(model, 'gp', options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, lbls, dataSetName, experimentNo);
end

% Load the results and display dynamically.
lvmResultsDynamic(model.type, dataSetName, experimentNo, 'vector')


function [YY,XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);

Ap(2,:) = 0;

for ii = 1:1:length(Ap)-1
    Ap(ii+1,ii) = 1;
end

inx = 1;
UUk = [Uk(inx); 0; 0; 0; 0];
PPk = (Xint_v*Xint_v');
VVk = [Vk(inx); 0; 0; 0; 0];
Qv = V*V';

for ii = 1:1:length(Xint_v)

XKk(ii,1) = Xint_v(ii)^2;                                             % FIRST STEP

end

PPk = Ap*PPk*Ap';                                                   % SECOND STEP

Kk = PPk*C'*inv( (C*PPk*C') + (V*Qv*V') );                          % THIRD STEP

for ii = 1:1:length(Xint_v)

XUPK(ii,1) = XKk(ii)^2 + UUk(ii);                                     % UPPER EQUATIONS.

Zk(ii,1) = cos(XUPK(ii)) +  VVk(ii);                                  % UPPER EQUATIONS.

end

for ii = 1:1:length(XKk)

XBARk(ii,1) = XKk(ii) + Kk(ii)*(Zk(ii) - (cos(XKk(ii)))) ;            % FOURTH STEP

end

II = eye(5,5);

Pk = ( II -  Kk*C)*PPk;                                             % FIFTH STEP

%--------------------------------------------------------------------------
%--------------------------------------------------------------------------

for ii = 1:1:n

UUk = [Uk(ii+1); 0; 0; 0; 0];
PPk = XBARk*XBARk';
VVk = [Vk(ii+1); 0; 0; 0; 0];

XKk = exp(-XBARk);                                                % FIRST STEP

PPkM = Ap*PPk*Ap';                                                 % SECOND STEP

Kk = PPkM*C'*inv( (C*PPkM*C') + (V*Qv*V') );                      % THIRD STEP

for nn = 1:1:length(XBARk)

XUPK(nn) = exp(-XKk(nn)) + UUk(nn);                              % UPPER EQUATIONS.

Zk(nn) = cos(XUPK(nn)) +  VVk(nn);                               % UPPER EQUATIONS.

end

for in = 1:1:length(XUPK)

XNEW(in) = XBARk(in) + Kk(in)*(Zk(in) - cos(XBARk(in)));           % FOURTH STEP

end

II = eye(5,5);

Pk = (II -  Kk*C)*PPkM;                                            % FIFTH STEP

XBARk = XNEW;

OUTX(ii) = XBARk(1,1);
OUTY(ii) = Zk(1,1);

end

YY = OUTY;
XX = OUTX;


function [jrt, jst]=jacprojRTS(j, i, rt, xyz, r0, a)
% symbolic projection function Jacobian
% code automatically generated with maple

  qr0=r0(j*4+1:(j+1)*4);

  t1 = (rt(1) ^ 2);
  t2 = (rt(2) ^ 2);
  t3 = (rt(3) ^ 2);
  t5 = sqrt((1 - t1 - t2 - t3));
  t6 = 0.1e1 / t5;
  t7 = t6 * qr0(2);
  t9 = -t7 * rt(1) + qr0(1);
  t11 = t6 * qr0(3);
  t13 = -t11 * rt(1) - qr0(4);
  t15 = t6 * qr0(4);
  t17 = -t15 * rt(1) + qr0(3);
  t19 = -t9 * xyz(1) - t13 * xyz(2) - t17 * xyz(3);
  t24 = -t5 * qr0(2) - qr0(1) * rt(1) - rt(2) * qr0(4) + rt(3) * qr0(3);
  t31 = t5 * qr0(3) + qr0(1) * rt(2) + rt(3) * qr0(2) - rt(1) * qr0(4);
  t37 = t5 * qr0(4) + qr0(1) * rt(3) + rt(1) * qr0(3) - rt(2) * qr0(2);
  t39 = t24 * xyz(1) - t31 * xyz(2) - t37 * xyz(3);
  t41 = t6 * qr0(1);
  t43 = -t41 * rt(1) - qr0(2);
  t48 = t5 * qr0(1) - rt(1) * qr0(2) - rt(2) * qr0(3) - rt(3) * qr0(4);
  t52 = t48 * xyz(1) + t31 * xyz(3) - t37 * xyz(2);
  t57 = t43 * xyz(1) + t13 * xyz(3) - t17 * xyz(2);
  t62 = t43 * xyz(2) + t17 * xyz(1) - t9 * xyz(3);
  t67 = t48 * xyz(2) + t37 * xyz(1) + t24 * xyz(3);
  t72 = t43 * xyz(3) + t9 * xyz(2) - t13 * xyz(1);
  t77 = t48 * xyz(3) - t24 * xyz(2) - t31 * xyz(1);
  t89 = -t19 * t31 - t39 * t13 + t43 * t67 + t48 * t62 + t72 * t24 - t77 * t9 + t57 * t37 + t52 * t17;
  t99 = -t19 * t37 - t39 * t17 + t43 * t77 + t48 * t72 - t57 * t31 - t52 * t13 - t62 * t24 + t67 * t9;
  t106 = -t39 * t37 + t48 * t77 - t52 * t31 - t67 * t24 + rt(6);
  t107 = 0.1e1 / t106;
  t119 = -t39 * t31 + t48 * t67 + t77 * t24 + t52 * t37 + rt(5);
  t123 = t106 ^ 2;
  t124 = 0.1e1 / t123;
  t125 = (a(1) * (t39 * t24 + t48 * t52 - t67 * t37 + t77 * t31 + rt(4)) + a(2) * t119 + a(3) * t106) * t124;
  t129 = -t7 * rt(2) + qr0(4);
  t132 = -t11 * rt(2) + qr0(1);
  t135 = -t15 * rt(2) - qr0(2);
  t137 = -t129 * xyz(1) - t132 * xyz(2) - t135 * xyz(3);
  t141 = -t41 * rt(2) - qr0(3);
  t146 = t141 * xyz(1) + t132 * xyz(3) - t135 * xyz(2);
  t151 = t141 * xyz(2) + t135 * xyz(1) - t129 * xyz(3);
  t157 = t141 * xyz(3) + t129 * xyz(2) - t132 * xyz(1);
  t170 = -t137 * t31 - t39 * t132 + t141 * t67 + t48 * t151 + t157 * t24 - t77 * t129 + t146 * t37 + t52 * t135;
  t180 = -t137 * t37 - t39 * t135 + t141 * t77 + t48 * t157 - t146 * t31 - t52 * t132 - t151 * t24 + t67 * t129;
  t187 = -t7 * rt(3) - qr0(3);
  t190 = -t11 * rt(3) + qr0(2);
  t193 = -t15 * rt(3) + qr0(1);
  t195 = -t187 * xyz(1) - t190 * xyz(2) - t193 * xyz(3);
  t199 = -t41 * rt(3) - qr0(4);
  t204 = t199 * xyz(1) + t190 * xyz(3) - t193 * xyz(2);
  t209 = t199 * xyz(2) + t193 * xyz(1) - t187 * xyz(3);
  t215 = t199 * xyz(3) + t187 * xyz(2) - t190 * xyz(1);
  t228 = -t195 * t31 - t39 * t190 + t199 * t67 + t48 * t209 + t215 * t24 - t77 * t187 + t204 * t37 + t52 * t193;
  t238 = -t195 * t37 - t39 * t193 + t199 * t77 + t48 * t215 - t204 * t31 - t52 * t190 - t209 * t24 + t67 * t187;
  t255 = (a(4) * t119 + a(5) * t106) * t124;
  jrt(1) = (a(1) * (t19 * t24 - t39 * t9 + t43 * t52 + t48 * t57 - t62 * t37 - t67 * t17 + t72 * t31 + t77 * t13) + a(2) * t89 + a(3) * t99) * t107 - t125 * t99;
  jrt(2) = (a(1) * (t137 * t24 - t39 * t129 + t141 * t52 + t48 * t146 - t151 * t37 - t67 * t135 + t157 * t31 + t77 * t132) + a(2) * t170 + a(3) * t180) * t107 - t125 * t180;
  jrt(3) = (a(1) * (t195 * t24 - t39 * t187 + t199 * t52 + t48 * t204 - t209 * t37 - t67 * t193 + t215 * t31 + t77 * t190) + a(2) * t228 + a(3) * t238) * t107 - t125 * t238;
  jrt(4) = a(1) * t107;
  jrt(5) = a(2) * t107;
  jrt(6) = a(3) * t107 - t125;
  jrt(7) = (a(4) * t89 + a(5) * t99) * t107 - t255 * t99;
  jrt(8) = (a(4) * t170 + a(5) * t180) * t107 - t255 * t180;
  jrt(9) = (a(4) * t228 + a(5) * t238) * t107 - t255 * t238;
  jrt(10) = 0.0e0;
  jrt(11) = a(4) * t107;
  jrt(12) = a(5) * t107 - t255;

  t1 = (rt(1) ^ 2);
  t2 = (rt(2) ^ 2);
  t3 = (rt(3) ^ 2);
  t5 = sqrt((1 - t1 - t2 - t3));
  t10 = -t5 * qr0(2) - qr0(1) * rt(1) - rt(2) * qr0(4) + rt(3) * qr0(3);
  t11 = t10 ^ 2;
  t16 = t5 * qr0(1) - rt(1) * qr0(2) - rt(2) * qr0(3) - rt(3) * qr0(4);
  t17 = t16 ^ 2;
  t22 = t5 * qr0(4) + qr0(1) * rt(3) + rt(1) * qr0(3) - rt(2) * qr0(2);
  t28 = -t5 * qr0(3) - qr0(1) * rt(2) - rt(3) * qr0(2) + rt(1) * qr0(4);
  t29 = t28 ^ 2;
  t32 = t10 * t28;
  t35 = -t16 * t22;
  t36 = 0.2e1 * t32 + t16 * t22 - t35;
  t38 = -t10 * t22;
  t39 = t16 * t28;
  t42 = t38 + 0.2e1 * t39 - t10 * t22;
  t48 = t10 * xyz(1) + t28 * xyz(2) - t22 * xyz(3);
  t53 = t16 * xyz(3) - t10 * xyz(2) + t28 * xyz(1);
  t58 = t16 * xyz(1) - t28 * xyz(3) - t22 * xyz(2);
  t63 = t16 * xyz(2) + t22 * xyz(1) + t10 * xyz(3);
  t65 = -t48 * t22 + t16 * t53 + t58 * t28 - t63 * t10 + rt(6);
  t66 = 0.1e1 / t65;
  t78 = t48 * t28 + t16 * t63 + t53 * t10 + t58 * t22 + rt(5);
  t82 = t65 ^ 2;
  t83 = 0.1e1 / t82;
  t84 = (a(1) * (t48 * t10 + t16 * t58 - t63 * t22 - t53 * t28 + rt(4)) + a(2) * t78 + a(3) * t65) * t83;
  t92 = t22 ^ 2;
  t93 = t29 + t17 - t10 ^ 2 - t92;
  t95 = -t28 * t22;
  t98 = t16 * t10;
  t99 = 0.2e1 * t95 - t16 * t10 - t98;
  t111 = t95 + 0.2e1 * t98 - t28 * t22;
  t114 = t92 + t17 - t28 ^ 2 - t11;
  t127 = (a(4) * t78 + a(5) * t65) * t83;
  jst(1) = (a(1) * (t11 + t17 - t22 ^ 2 - t29) + a(2) * t36 + a(3) * t42) * t66 - t84 * t42;
  jst(2) = (a(1) * (t32 + 0.2e1 * t35 + t10 * t28) + a(2) * t93 + a(3) * t99) * t66 - t84 * t99;
  jst(3) = (a(1) * (0.2e1 * t38 - t16 * t28 - t39) + a(2) * t111 + a(3) * t114) * t66 - t84 * t114;
  jst(4) = (a(4) * t36 + a(5) * t42) * t66 - t127 * t42;
  jst(5) = (a(4) * t93 + a(5) * t99) * t66 - t127 * t99;
  jst(6) = (a(4) * t111 + a(5) * t114) * t66 - t127 * t114;


function engine = kalman_inf_engine(bnet)
% KALMAN_INF_ENGINE Inference engine for Linear-Gaussian state-space models.
% engine = kalman_inf_engine(bnet)
%
% 'onodes' specifies which nodes are observed; these must be leaves.
% The remaining nodes are all hidden. All nodes must have linear-Gaussian CPDs.
% The hidden nodes must be persistent, i.e., they must have children in
% the next time slice. In addition, they may not have any children within the current slice,
% except to the observed leaves. In other words, the topology must be isomorphic to a standard LDS.
%
% There are many derivations of the filtering and smoothing equations for Linear Dynamical
% Systems in the literature. I particularly like the following
% - "From HMMs to LDSs", T. Minka, MIT Tech Report, (no date), available from
%    ftp://vismod.www.media.mit.edu/pub/tpminka/papers/minka-lds-tut.ps.gz

[engine.trans_mat, engine.trans_cov, engine.obs_mat, engine.obs_cov, engine.init_state, engine.init_cov] = ...
    dbn_to_lds(bnet);

% This is where we will store the results between enter_evidence and marginal_nodes
engine.one_slice_marginal = [];
engine.two_slice_marginal = [];

engine = class(engine, 'kalman_inf_engine', inf_engine(bnet));


% @Author: aaronmishkin
% @Date:   2018-07-26T13:13:16-07:00
% @Email:  amishkin@cs.ubc.ca
% @Last modified by:   aaronmishkin
% @Last modified time: 2018-07-26T13:22:15-07:00

% Load the toy example experiment data.
file_name = strcat('./toy_example_experiment_data.mat');
load(file_name)

method = {'Vadam', 'VOGN-1'}
colors = [0 0.8 0.5; 0 0 1; 1 0 0];
f = figure('Position', [50,50,1000,715]); clf;
% exact post
contourf(w1,w2,reshape(post,[n,n]),5);
cbh=colorbar;
colormap(gray);
hold on
% map estimate
h(1) = plot(wmap(1),wmap(2),'+','color', 'k', 'MarkerSize',7, 'linewidth', 8, 'markersize', 7);
h(2) = plot_gaussian_ellipsoid(w_exact_vi, C_exact_vi, 1);
set(h(2), 'color', 0*[1 1 1], 'linestyle', '-.', 'linewidth', 12);

for m = 1:length(method)
    plot(w_all(1,t,m), w_all(2,t,m), 'or', 'color', colors(m,:), 'linewidth', 8, 'markerfacecolor', colors(m,:), 'markersize', 5);
    h(m+2) = plot_gaussian_ellipsoid(w_all(:,t,m), Sigma_all(:,:,t,m), 1);
    set(h(m+2), 'color', colors(m,:), 'linewidth', 12);
end
% mf-exact
axis([0 20 0 12]);
plot(w_exact_vi(1), w_exact_vi(2), 'o', 'color', 0*[1 1 1], 'linewidth', 8, 'markerfacecolor', 0*[1 1 1], 'markersize', 5);
      t = maxIters;

hl = legend(h, {'MAP', 'VI-Exact', 'Vadam', 'VOGN-1'}, 'location', 'northwest');
hx = xlabel('\theta_1');
hy = ylabel('\theta_2');
set(gca, 'fontsize', 24);
set([hx,hy], 'fontsize', 24, 'fontname', 'helvetica');
set(hl, 'fontsize', 24, 'fontname', 'helvetica');
set(gca, 'xtick', [0:5:20], 'ytick', [0:5:10], 'tickdir', 'out');
set(cbh,'YTick',[0:1e-3:5e-3])

h = gcf;

f.Position(3) = 1000;
f.Position(4) = 800;

axesObjs = get(h, 'Children');  %axes handles
dataObjs = get(axesObjs, 'Children'); %handles to low-level graphics objects in axes

cell_obj = dataObjs(3, 1);
cell_obj = cell_obj{1};

vogn_dot = cell_obj(3);
vogn_line = cell_obj(2);

vadam_dot = cell_obj(5);
vadam_line = cell_obj(4);

vi_dot = cell_obj(1);
vi_line = cell_obj(6);
map_dot = cell_obj(7);

linewidth = 12;

set(vogn_dot,'color', 'b')
set(vogn_line,'color', 'b')
set(vogn_line,'linewidth', linewidth)

set(vadam_dot, 'color', 'r')
set(vadam_line,'color', 'r')
set(vadam_line,'linewidth', linewidth)

set(vi_dot,'color', [0, 1.0, 0])
set(vi_line,'color', [0, 1.0, 0])
set(vi_line,'linewidth', linewidth)

set(map_dot, 'markerSize', 22)
set(map_dot, 'color', 'black')

%%%%
fontsize = 30;
ax = gca;
set(ax, 'fontsize', fontsize);

ax.XLim = [0, 20];
ax.YLim = [0, 11];
ax.XLabel.String = 'Weight 1';
ax.YLabel.String = 'Weight 2';
%%%
hLegend = findobj(gcf, 'Type', 'Legend');
hLegend.FontSize = 30;
%set(ax, 'LooseInset', get(ax, 'TightInset'));

hLegend.String{2} = 'MF-Exact';

grid off

set(f,'Units','Inches');
pos = get(f,'Position');
set(f,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])

savefig('figures/figure_two_a.fig')
saveas(f, 'figures/figure_two_a.pdf')
close all


function [VecTot, TOT, time] = rmt(P, Vec, Mat)
%rmt returns a matrix containing the best models for all the paths of the Replacement Method.
%TOT contains all the relative results showing the evolution of the method.
%
%
%	   Input:
%             P             Property vector
%             Vec           Initial descriptors vector
%             Mat           Descriptors matrix with descriptors pool
%
%
%     Returns:
%
%            VecTot           vector containing the best model for all the
%                               paths of the Replacement Method
%            TOT               contains all the relative results
%                               showing the evolution of the method.
%
% Andrew G. Mercader, Pablo R. Duchowicz
% INIFTA, La Plata, Argentina
% Created: 5 March 2007

TOT=[];
VecTot=[];
time=cputime;
warning off

[k_v,n_v]=size(Vec);

for k=1:n_v

Sr=rms(P,Vec,Mat);
TOT(k).A(1,:)=[Sr,Vec];

VecA=rmsr(P, Vec, Mat, k);
Po(1)=k;
VecI=VecA;
if n_v==1
    VecTot=[1,VecI];
    TOT=VecI;
    time=cputime-time
    return
end
VecI(1)=[];
COEF=rmder(P,VecI,Mat);
COER=COEF;
COER(Po)=[];
pos=find(COEF==max(COER));
Po(2)=pos;
TOT(k).A(2,:)=VecA;
for i=2:n_v;
    VecA=rmsr(P, VecI, Mat, pos);
    VecI=VecA;
    TOT(k).A(i+1,:)=VecA;
    VecI(1)=[];
    COEF=rmder(P,VecI,Mat);
    if i==n_v
        Po=[];
        break
    end
    COER=COEF;
    COER(Po)=[];
    pos=find(COEF==max(COER));
    Po(i+1)=pos;
end

for j=1:2;
COER=COEF;
COER(Po)=[];
pos=find(COEF==max(COER));
Po(1)=pos;
for i=1:n_v;
    VecA=rmsr(P, VecI, Mat, pos);
    VecI=VecA;
    TOT(k).A(i+(j*n_v),:)=VecA;
    VecI(1)=[];
    COEF=rmder(P,VecI,Mat);
    if i==n_v
        Po=[];
        break
    end
    COER=COEF;
    COER(Po)=[];
    pos=find(COEF==max(COER));
    Po(i+1)=pos;
end
end

for j=3:100;
COER=COEF;
COER(Po)=[];
pos=find(COEF==max(COER));
Po(1)=pos;
    for i=1:n_v;
    VecA=rmsr(P, VecI, Mat, pos);
    VecI=VecA;
    TOT(k).A(i+(j*n_v),:)=VecA;
    VecI(1)=[];
    COEF=rmder(P,VecI,Mat);
    if i==n_v
        Po=[];
        break
    end
    COER=COEF;
    COER(Po)=[];
    pos=find(COEF==max(COER));
    Po(i+1)=pos;
    end
   if TOT(k).A(i+(j*n_v),:)==TOT(k).A(i+(j*n_v)-(2*n_v),:)
        Po=[];
        break
   end

end
VecP=find(TOT(k).A==min(TOT(k).A(:,1)));
VecTot(k,:)=[k,TOT(k).A(VecP(1),:)];
VecTot=sortrows(VecTot,2);
end
time=cputime-time
warning on

% % End of rmt
%


function timer_tictoc_test04 ( )

%*****************************************************************************80
%
%% TEST04 times the 2D nearest neighbor problem.
%
%  Discussion:
%
%    For the MATLAB implementation of this test, the unvectorized
%    test limits had to be DRASTICALLY reduced.  But then I noticed
%    that some kind of MATLAB cleverness causes all the loops
%    after the first cycle to be computed in almost no time.
%    This test will have to be rethought.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2006
%
%  Author:
%
%    John Burkardt
%
  n_log_min = 10;
  n_log_max = 18;
  n_min = 2^n_log_min;
  n_max = 2^n_log_max;
  n_rep = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  Time the 2D nearest neighbor problem.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Given X(2,N) and Y(2),\n' );
  fprintf ( 1, '    find X(2,*) closest to Y(2).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    for i = 1 : n\n' );
  fprintf ( 1, '      if distance ( x(2,i), y ) < minimum so far\n' );
  fprintf ( 1, '        x_min = x(2,i)\n' );
  fprintf ( 1, '    end\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data vectors will be of minimum size %d\n', n_min );
  fprintf ( 1, '  Data vectors will be of maximum size %d\n', n_max );
  fprintf ( 1, '  Number of repetitions of the operation: %d\n', n_rep );

  x = rand (2,n_max);
  y = rand(1,2);

  for i_rep = 1 : n_rep

    for n_log = n_log_min : n_log_max

      n = 2^n_log;

      tic;

      dist_min = r8_huge ( );
      i_min = 0;
      for i = 1 : n
        dist_i = sum ( ( x(1:2,i) - y(1:2)' ).^2 );
        if ( dist_i < dist_min )
          dist_min = dist_i;
          i_min = i;
        end
      end

      delta(n_log,i_rep) = toc;

    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04 Results:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Vector Size  Rep #1        Rep #2        Rep #3        ' );
  fprintf ( 1, 'Rep #4        Rep #5\n' );
  fprintf ( 1, '\n' );
  for n_log = n_log_min : n_log_max
    n = 2^n_log;
    fprintf ( 1, '%10d', n );
    for j = 1 : n_rep
      fprintf ( 1, '%14f', delta(n_log,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


% Copyright (C) 1993-2014, by Joern Malzahn
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%%begin

%% Getting started with symbolics and code generation
% This is a brief example about how we can derive symbolic robot model
% expressions and how we can generate robot specific functions as well as
% real-time capable Simulink blocks using the |CodeGenerator| class. The
% example uses a reduced version of the Puma 560 arm with the first 3
% links.
%
% A requirement for this demo is that we have the Mathworks Symbolic Toolbox
% installed besides the Robotics Toolbox.
%

%% Instantiate a |CodeGenerator| class object
% We start off with the instantiation of a |CodeGenerator| class object.
% First, we load the |SerialLink| object for which we intend to generate
% code.
mdl_planar3

%%
% After that, we find a |SerialLink| object named p3 in the workspace. This
% object is used to instantiate the CodeGenerator.
cGen = CodeGenerator(p3)

%% Code generation
% By default |CodeGenerator| class objects are configured to generate:
%
% * symbolic expressions
% * m-code
% * Simulink blocks
%
% and they document the CodeGeneration progress on the Matlab console.
% We may modify this behaviour by passing extra arguments to the
% |CodeGenerator| constructor. (Type |help CodeGenerator| for details)
%
% Now let's generate code for the forward kinematics of our reduced Puma
% 560.
symExp = cGen.genfkine

%%
% The text output to the console may be disabled
cGen.verbose = false;
%
% or logged to disk by specifying a log file name
cGen.logfile = 'roblog.txt'

%%
% The output variable |symExp| now contains the symbolic expression for the
% forward kinematics. This expression is the same as would be obtained by
% the following code.
symp3 = p3.sym;
q = symp3.gencoords;
symExpDir = symp3.fkine(q)

% So we have basically two ways for deriving symbolic expressions using the
% Robotics Toolbox.

%%
% The difference is that in addition the functional output the symbolic
% expression has now been saved to disk along with the generated m-code and
% Simulink blocks. The storage directory is given in |cGen.basepath|, which
% we now add to our search path.
addpath(cGen.basepath)
ls(cGen.basepath)
%%
% The m-code is contained in a specialized robot class.
ls(cGen.robjpath)

%% Using the generated m-code
% The |mdl_puma560_3| robot definition script defines some special joint
% configurations:
%
% * qz         zero joint angle configuration
% * qr         vertical 'READY' configuration
% * qstretch   arm is stretched out in the X direction
% * qn         arm is at a nominal non-singular configuration
%
% The use of the symbolic expressions and generated code will be
% exemplified in the following based on the zero joint angle configuration.
%
%
qz
%%
% With the generic version of the fkine function from the |SerialLink|
% class we would compute the forward kinematics as follows:
tic; Tz1 = p3.fkine(qz); t1 = toc
%%
% In order to use the generated robot specific m-functions we add them to
% the search path and instantiate a new robot object.
addpath(cGen.basepath)
specRob = eval(cGen.getrobfname)
tic; Tz2 = specRob.fkine(qz); t2 = toc

%% Speedup
% The specialized robot version of fkine runs a little faster
% because it only performs the computations necessary for the specific robot.
% The speedup of the generated robot specific m-code becomes even more appearent if we
% repeat the comparison of the execution times for dynamics
% functions such as:
%
% * gravload -> cGen.gengravload
% * inertia  -> cGen.geninertia
% * coriolis -> cGen.gencoriolis
% * invdyn   -> cGen.geninvdyn
%
% This way the specialized m-code can be used to decrease simulation times.
%
%%
% We obtain the exact solution without floating point notation if we use
% the symbolic expression as follows:
tic; Tz1 = subs(symExp, {'q1', 'q2', 'q3'},qz); toc
%%
% This is however more time consuming. Most probably we might use the
% symbolic expressions for algorithm development, controller design,
% stability proofs as well as analysis, system identification or teaching.
%
% It is also possible to get the symbolic expressions for the homogenous
% transformations of up to each individual joint. This has been found to be
% useful for example for during derivation of analytical inverse kinematics
% solutions. See the documentation of genfkine for details.
%

%% C-Code generation
% Since Release 9.9 the RTB is able to also generate ready to use C-code.
% You can enable C-code generation by activating the CodeGenerator property
% flag |cGen.genccode|:
cGen.genccode = true;
%%
% Now all higher level generator methods (|cGen.genfkine|, |cGen.geninvdyn|
% etc. ...) also produce .c and .h files. They are written to the directory
% specified by the |cGen.ccodepath| property. You can use the C-files in
% your projects outside the MATLAB world. The header files are documented
% and compatible with Doxygen.
%
% Instead of using the higher level generator methods, you can also
% directly call the C-code generation routine for the model code of your
% choice. In the following we complement the previously generated m-functions
% for the forward kinematics by their C-equivalent:
cGen.genccodefkine;
disp('Generated C-headers:')
ls(fullfile(cGen.ccodepath,'include'))
disp('Generated C-definitions:')
ls(fullfile(cGen.ccodepath,'src'))

%% Generating C-MEX functions
% We can use the generated C-code outside the MATLAB world and use it in
% arbitrary C-applications. In addition we can also benefit from it inside
% the MATLAB world by means of C-MEX functions. The automated generation of
% C-MEX functions is controlled by the CodeGenerator flag properties
% |cGen.genmex| and |cGen.compilemex|:
cGen.genmex = true;
%%
% Now all higher level generator methods (|cGen.genfkine|, |cGen.geninvdyn|
% etc. ...) also produce C-MEX files. The MEX files are stored in the class
% directory |cGen.robjpath| of the specialized robot object also incorporating the
% m-functions we generated before.
%%
% By default the flag compilemex is active. This means that the
% CodeGenerator always compiles the generated MEX function after generation.
% We require an installed C-compiler and our MATLAB MEX environment being
% configured properly. See the MATLAB documentation ond MEX files for
% details. In order to proceed with this demo in the case where we do not
% have this prerequisites, we now deactivate the automatic generation:
cGen.compilemex = false;
%%
% Nevhertheless, we can create the C-MEX code for the forward kinematics
% and inspect the |cGen.robjpath| directory:
cGen.genmexfkine
disp('Robot object directory with new MEX source file fkine.c:')
ls(cGen.robjpath)
%%
% The readily compiled MEX functions will shadow the previously generated
% m-functions. The function calls as such remain identical. Using the
% specialized robot object with MEX files we experience an additional and
% substantial computation speed up compared to the robot specific m-code
% as well as the generic rne functions (both, m and MEX version).
%

%% Inheritance
% Even though we have not yet generated robot specific code for |SerialLink|
% metods other than |fkine|, we can still use all functionality of
% |SerialLink| objects with our new specialized robot object which inherits
% from |SerialLink|.
J01 = p3.jacob0(qz)
J02 = specRob.jacob0(qz)

%% A look at the generated Simulink blocks
% The Simulink blocks are stored in a Simulink library file. By opening the
% generated Simulink library we can investigate the already optimized robot
% specific code within the blocks.
% The usage of these blocks is also accompanied with a noticable speedup
% compared to the blocks based on generic |SerialLink| objects.
eval(cGen.slib);
snapnow;
%%
% Beyond the speedup for simulations all blocks in the generated library
% may be directly compiled for real-time operating systems such as xPC-Target or
% dSpace systems for model based control of real hardware setups.
% This way we avoid tedious and error prone reimplementation of the model
% on the target hardware.
%

bdclose(cGen.slib);

%% Further information
% For further information on symbolics and code generation see the
% documentation of the |SerialLink| and |CodeGenerator| class.
%
% All generated functions come with their own description headers so that information
% about their usage can be found by typing |help funname|.
%
% A list of all available methods provide by the |CodeGenerator| class can be
% displayed.
methods CodeGenerator

%%
% The same applies to the configurable properties:
properties CodeGenerator

%% Cleanup
% If we whish to clean our disk from all the generated code, we can simply
% remove it from the search path
rmpath(cGen.basepath)
%%
% and purge everything.
cGen.purge(1)
snapnow


function DoManualShim

global VCtl
global VObj
global VMag

if ~isfield(VCtl,'Sh_X2_Y2')
    warndlg('Please load Shim tab. Manual Shimming was not performed!');
    return;
end

Mxdims=size(VObj.Rho);
[xgrid,ygrid,zgrid]=meshgrid((-(Mxdims(2)-1)/2)*VObj.XDimRes:VObj.XDimRes:((Mxdims(2)-1)/2)*VObj.XDimRes,...
                            (-(Mxdims(1)-1)/2)*VObj.YDimRes:VObj.YDimRes:((Mxdims(1)-1)/2)*VObj.YDimRes,...
                            (-(Mxdims(3)-1)/2)*VObj.ZDimRes:VObj.ZDimRes:((Mxdims(3)-1)/2)*VObj.ZDimRes);

B0ShimField = VCtl.Sh_X .* xgrid + ...
              VCtl.Sh_Y .* ygrid + ...
              VCtl.Sh_Z .* zgrid + ...
              VCtl.Sh_ZX.* zgrid .* xgrid + ...
              VCtl.Sh_ZY.* zgrid .* ygrid + ...
              VCtl.Sh_Z2.* zgrid .^ 2 + ...
              VCtl.Sh_XYZ.* xgrid .* ygrid .* zgrid+ ...
              VCtl.Sh_X2_Y2.* (xgrid .* ygrid) .^ 2;

VMag.dB0 = VMag.dB0 + B0ShimField;

end


function [s] = ADEM_sample_image(V,o,R)
% samples a (memory mapped) image at displacement o
% FORMAT [s] = ADEM_sample_image(V,o,R)
% FORMAT [s] = ADEM_sample_image(o,h)
%
% V - a structure array containing image volume information
% o - coordinates of foveal sampling:
%   o(1) - oculomotor angle
%   o(2) - oculomotor angle
% R - retinal modulation (n x n)
%
% or
%
% o - coordinates of foveal sampling
% h - vector of coefficients weighting images in STIM.H{:}
%
% s - sensory sample (n x n)
%
% requires a global variable with the following fields:
% STIM.R = contrast modulation matrix that defines resolution
% STIM.W = width of foveal sampling of an image   (default: 1/6)
% STIM.P = image position in retinal  coordinates (default: [0;0])
% STIM.B = basis functions or receptive fields    (default: 1)
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: ADEM_sample_image.m 6932 2016-11-16 12:11:01Z karl $

% retinotopic predictions
%--------------------------------------------------------------------------
global STIM
if nargin < 3
    s     = 0;
    for i = 1:numel(o)
        s = s + o(i)*ADEM_sample_image(STIM.H{i},V,STIM.R);
    end
    return
end

% preliminaries
%--------------------------------------------------------------------------
if ~isfield(STIM,'R'), STIM.R = ones(64,64); end
if ~isfield(STIM,'W'), STIM.W = 1/6;         end
if ~isfield(STIM,'P'), STIM.P = [0;0];       end
if ~isfield(STIM,'B'), STIM.B = 1;           end
if ~isfield(STIM,'A'), STIM.A = 512;         end

% retinotopic sampling
%--------------------------------------------------------------------------
dim = size(R);
dx  = V.dim(1)/dim(1)*STIM.W;

i   = dx*((1:dim(1)) - dim(1)/2) + V.dim(1)/2  + (o(1) + STIM.P(1))*16;
j   = dx*((1:dim(2)) - dim(2)/2) + V.dim(2)/2  + (o(2) + STIM.P(2))*16;
x   = kron(ones(1,dim(2)),i);
y   = kron(j,ones(1,dim(1)));
z   = ones(1,dim(1)*dim(2));

x   = min(max(x,1),V.dim(1));
y   = min(max(y,1),V.dim(2));

s   = spm_sample_vol(V,x,y,z,-2); s(~s) = 1;
s   = reshape(s,dim(1),dim(2)).*R;
s   = STIM.B'*s*STIM.B;

% eccentricity attenuation
%--------------------------------------------------------------------------
s   = s*exp(-o'*o/STIM.A);


classdef MOEADDE < ALGORITHM
% <multi/many> <real/integer>
% MOEA/D based on differential evolution
% delta --- 0.9 --- The probability of choosing parents locally
% nr    ---   2 --- Maximum number of solutions replaced by each offspring

%------------------------------- Reference --------------------------------
% H. Li and Q. Zhang, Multiobjective optimization problems with complicated
% Pareto sets, MOEA/D and NSGA-II, IEEE Transactions on Evolutionary
% Computation, 2009, 13(2): 284-302.
%------------------------------- 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
            [delta,nr] = Algorithm.ParameterSet(0.9,2);

            %% Generate the weight vectors
            [W,Problem.N] = UniformPoint(Problem.N,Problem.M);
            T = ceil(Problem.N/10);

            %% Detect the neighbours of each solution
            B = pdist2(W,W);
            [~,B] = sort(B,2);
            B = B(:,1:T);

            %% Generate random population
            Population = Problem.Initialization();
            Z = min(Population.objs,[],1);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                % For each solution
                for i = 1 : Problem.N
                    % Choose the parents
                    if rand < delta
                        P = B(i,randperm(end));
                    else
                        P = randperm(Problem.N);
                    end

                    % Generate an offspring
                    Offspring = OperatorDE(Problem,Population(i),Population(P(1)),Population(P(2)));

                    % Update the ideal point
                    Z = min(Z,Offspring.obj);

                    % Update the solutions in P by Tchebycheff approach
                    g_old = max(abs(Population(P).objs-repmat(Z,length(P),1)).*W(P,:),[],2);
                    g_new = max(repmat(abs(Offspring.obj-Z),length(P),1).*W(P,:),[],2);
                    Population(P(find(g_old>=g_new,nr))) = Offspring;
                end
            end
        end
    end
end


function output = SSIM(Reference, Clean, Target)

sim_sum=0;

[m1,n1] = size(Reference);
[m2,n2] = size(Target);
[m3,n3] = size(Clean);

r=[m1;m2;m3;];
c=[n1/3;n2/3;n3/3;];

rows=min(r);
cols=min(c);
for i=1:3

    x1=(Reference(1:rows,1:cols,i));
    y=(Target(1:rows,1:cols,i));
    x2=(Clean(1:rows,1:cols,i));

    avg = (x1 + x2)/2;

    [ssimval, ~] = ssim(y,avg);

    sim_sum = sim_sum + ssimval;

end

output = sim_sum/3;


%   getBox:  Given the current state, returns a number from 1 to 162
%             designating the region of the state space encompassing the current state.
%             Returns a value of -1 if a failure state is encountered.

function box = getBox3(theta,thetaDot,x,xDot)
theta = rad2deg(theta);
thetaDot = rad2deg(thetaDot);
if (x < -1 || x > 1  || theta < -8 || theta > 8)
    box = -1;
else

if (theta<-1&&theta>=-8)
	thetaBucket = 1;
elseif (theta<0&&theta>=-1)
	thetaBucket = 2;
elseif (theta<1&&theta>=0)	% zero included
	thetaBucket = 3;
elseif (theta<8&&theta>=1)
	thetaBucket = 4;
end

if (x<-0.8&&x>=-1)
	xBucket = 1;
elseif (x<=0.8&&x>=-0.8)
	xBucket = 2;
elseif (x<=1&&x>0.8)
	xBucket = 3;
end

if (xDot<-0.5)
	xDotBucket = 1;
elseif (xDot>=-0.5&&xDot<=0.5)
	xDotBucket = 2;
else
	xDotBucket = 3;
end

if (thetaDot<-50)
	thetaDotBucket = 1;
elseif (thetaDot>=-50&&thetaDot<=50)
	thetaDotBucket = 2;
else
	thetaDotBucket = 3;
end

box = sub2ind([4,3,3,3],thetaBucket, thetaDotBucket,xBucket,xDotBucket);
end
return;


function [week, sow] = time2weektow(time)

% SYNTAX:
%   [week, sow] = time2weektow(time);
%
% INPUT:
%   time = GPS time (continuous since 6-1-1980)
%
% OUTPUT:
%   week = GPS week
%   sow  = GPS seconds-of-week
%
% DESCRIPTION:
%   Conversion from GPS time in continuous format (similar to datenum) to
%   GPS time in week, seconds-of-week.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

sec_in_week = 7*86400;

sow  = rem(time, sec_in_week);
week = (time - sow) / sec_in_week;


function [im, kspFull] = pocs( ksp, iter, watchProgress )
%Partial-Fourier Reconstruction with POCS
%
% [im, kspFull] = pocs( kspIn, iter, watchProgr )
%
% === Input ===
%
%   kspIn:      Reduced Cartesian MRI Data-Set
%               Any dimension may be reduced,
%               but only one reduction dim. is allowed due to Physics/Math.
%
%               Allowed shapes for kspIn are...
%                 ... Ny x Nx
%                 ... Nc x Ny x Nx
%                 ... Nc x Ny x Nx x Nz
%
%               With Nc == number of receive Channels / Coils.
%
%               kspIn can either be a zero-padded array, so the partial Fourier property is obvious.
%               Or kspIn can be the measured data only, then we try to find k-space centre automagically
%               and create a zero-padded array with the full size, first.
%               Errors are however more likely to occur in the latter case.
%
%
%   iter:       No. of iterations
%   (optional)  default: iter = 20
%               Try on your own if larger iter improves your results!
%
%   watchProgr: true/false; Whether the progress of the reconstruction should
%   (optional)  be monitored in an image window.
%               In 3D data, only the central partition will be shown.
%
%
% === Output ===
%
%   im:         Reconstructed Images (channels not combined)
%
%   kspFull:    Reconstructed full k-space data
%
%
%
% === About the code ===
%
%   (1) We find out whether input data is
%       a) already zero-filled or
%       b) the pure asymmetric dataset, only
%
%       If b) is true, we zero-fill the data ourselves, which means we have to
%       determine the dimension first, in which the partial Fourier reduction was done.
%       We therefor find the position of the max. intensity in k-space which should
%       be identical to k-space centre. If the k-space centre is different from the
%       centre of the matrix, we know the partial Fourier dimension.
%       We then enlarge the matrix to its desired full size and fill the new part
%       with zeros.
%       If a) was true, finding the partial Fourier dimension is easy:
%       It is the dimension with all the zeros. :-)
%
%   (2) We create one low resolution image per channel/coil:
%
%       We need a symmetrically sampled part around the central k-space. Think of a
%       small stripe of phase encoding lines in the central k-space.
%       We only use these symmetric data (setting the rest zero) to reconstruct
%       low-resolution images. In order to avoid Gibbs-Ringing, a Hamming-filter
%       with the width of the stripe is multiplied with the data.
%       Additionally, all the fully sampled dimensions get a Hamming filter, too,
%       since we increase SNR, reduce further Gibbs-ringing and do not lose much
%       resolution.
%
%   (3) The phase of the low-resolution images is saved
%
%       POCS uses the fact that k-space data of real objects (no imaginary part)
%       have a point symmetry:
%           S(-k)  =  S*(k)      with k = (kx, ky, kz)
%       Our MRI objects are always complex, but we assume that phase variations
%       are due to coil sensitivities and B0-inhomgeneities,
%       which are both slowly varying (no high res. required).
%       Small-scale phase pertubations will decrease the reconstruction quality.
%
%   (4) Reference phase is applied in image space
%
%       We...
%       ... transform our zero-filled data to image space (IFFT)
%       ... remove the phase --> abs(image)
%       ... set the phase of our reference phase map --> image .* exp(1i.*phase)
%       ... transform back to k-space (FFT)
%       ... re-insert the measured data (self-consistency!)
%       ... goto "We..."
%
%       Iterating through the above steps fills the missing k-space points
%       with reasonable values.
%       If the phase varies slowly and there is no aliasing, this works very well.
%
% Aliasing artifacts are very challenging for POCS.
% So try to prevent aliasing in the first place (sufficient Field of View).

% =========================================================================
% Original code by Martin Blaimer
% * changed by Uvo Hoelscher
% * changed by Michael Völker
%        -- auto-detect PF dimension
%        -- auto-find centre point/line/partition
%        -- accept zerofilled or "pure" data
%        -- for multichannel or plain 2D data (single-channel)
%        -- 2D and 3D
%        -- error handling
%        -- comments, comments, comments
%        -- added option to monitor progress
%        -- moved code to seperate functions
%        -- smooth transition between acquired signal and
%           reconstructed data
%
% Problems? Suggestions?
%  --> michael.voelker@mr-bavaria.de
% =========================================================================

    % ( ===================================================================
    % Input Handling
    %
        if ~exist( 'ksp', 'var' ) || isempty(ksp) || ~isnumeric(ksp)
            error('pocs:input', 'First input must be Cartesian k-space data.')
        end
        if ~exist('iter','var') || isempty(iter) || numel(iter) ~= 1 || ~isnumeric(iter)
            iter = 20;
        end
        if ~exist('watchProgress','var')  || isempty(watchProgress) ||  numel(watchProgress) ~= 1 || ~isfinite(watchProgress)
            watchProgress = false;
        else
            watchProgress = logical( watchProgress );
        end

        Ndim = ndims( ksp );

        if Ndim > 4 || Ndim < 2
            error('pocs:shape','First input ''kspace'' should have one of these shapes:\n\n\t... Ny x Nx\n\t... Nc x Ny x Nx\n\t... Nc x Ny x Nx x Nz')
        end
        if Ndim == 2    % Ny x Nx
            ksp = reshape( ksp, [1 size(ksp)] );    %  1 x Ny x Nx  --> now we have one channel...
            wasAddedCoilDim = true;
            Ndim = 3;
        else
            wasAddedCoilDim = false;
        end

        % read the properties of the data
        sz   = size( ksp );
        sz   = sz(2:end);           % the (k-)spatial size of the array (i.e. without channels)
        prec = class( ksp );        % single or double precision?
    % ) ===================================================================

    % First: Check the sampling pattern (which parts of input are actually data?)
    smplPtrn = reshape( sum(abs(ksp),1) ~= 0, sz);        % Ny x Nx x Nz

    % ( ===================================================================
    % If input data is not yet zero-filled, do it here
    %
        if nnz(smplPtrn) == numel(smplPtrn)     % only the sampled data were passed / |N|umber of |N|on |Z|ero elements

            [ ksp, pfDim, isUpper, isLower, Nsmp ] = zerofillPFdim( ksp, wasAddedCoilDim );

            sz = size( ksp );
            sz = sz(2:end);     % ignore channels
        else
            [ pfDim, isUpper, isLower, Nsmp ] = detectPFdim( smplPtrn, wasAddedCoilDim );
        end
        clear  smplPtrn
    % ) ===================================================================

    if numel(sz) < 3
        sz(3) = 1;
    end
    Ny = sz(1);
    Nx = sz(2);
    Nz = sz(3);

    % ( ===================================================================
    % Handle ugly problems.
    %
        if ~isUpper && ~isLower
            error('pocs:UnknownErrorFound', 'I thought we are partial Fourier, but things seem to make no sense... :-(')
        end
    % ) ===================================================================

    % =====================================================================
    %
    %        We can now be sure to operate with zero-padded data.
    %
    % =====================================================================

    % initialize a cell of subscripts
    subs = { ':', ':', ':', ':' };      % all channels / all Ny / all Nx / all Nz

    % If the first entries are zero-filled (instead of the trailing ones),
    % flip the entries so we can treat them as if we pf'ed the first half of kspace.
    if isLower
        subs{pfDim+1} = sz(pfDim):-1:1;     % ...esreveR
        ksp           = ksp(subs{:});       % !ecaps-k si sihT
        subs{pfDim+1} = 1:sz(pfDim);        % lalala, we didn't do anything...
    end

    % Find out which point is in the centre and which indices belong to the
    % symmetrically sampled part of k-space.
    [ centreLine, idxSym ] = findSymSampled( ksp, pfDim, Nsmp );

    szSym = numel( idxSym );                % 2 * (Nsmp - centreLine) + 1

    if isUpper
        fprintf('Using %g points around point %g\n', szSym,    centreLine    );
    else
        fprintf('Using %g points around point %g\n', szSym, sz(pfDim)-centreLine+1 );
    end

    % ( ===================================================================
    % build up a symmetric low-pass filter
    %
    filter = cast( 1, prec );
    for d = 1:Ndim-1

        reshRule = ones(1,Ndim);    % how the filter will be reshaped

        if d ~= pfDim   % Each standard dimension gets a simple low-pass filter

            filt1D = hamming( sz(d), 'periodic'  );

        else            % our partial Fourier dimension gets an extra nice filter

            % create a narrow filter and remove everything else
            filt1D          = zeros(sz(d), 1, prec);            % full-size filter
            tmp             = hann( szSym + 2, 'symmetric' );   % a very narrow window
            filt1D(idxSym)  = tmp(2:end-1);                     % cut out the zeros at the edges (we have data there!)

            % take a look:
            %figure, plot(filt1D)
        end

        % reshape the filter according to the dimension it represents
        reshRule(d+1) = sz(d);

        filt1D = reshape( filt1D, reshRule );
        filter = bsxfun( @times, filter, filt1D );      % iteratively build up a multidimensional filter
    end
    % ) ===================================================================

    % Apply the low-pass filter
    kspLowRes = bsxfun( @times, filter, ksp);
    clear  filt1D  filter  reshRule  idxSym

    % ( ===================================================================
    % prerequisites prior to the iteration loop
    %
    %  Set everything up here, do computations that you don't have
    %  to do in the loop, remove no longer needed variables...
    %
        % fftshift everything once before and after for-looping
        %  => less overhead during iteration
        ksp       = cmshiftnd(       ksp, [0  sz/2] );
        kspLowRes = cmshiftnd( kspLowRes, [0  sz/2] );

        % reorder arrays such that the fft-dimensions come first
        % => faster memory access
        ksp       = permute(       ksp, [2 3 4 1] );      % Ny x Nx x Nz x Nc
        kspLowRes = permute( kspLowRes, [2 3 4 1] );      %
        subs      = { subs{2}, subs{3}, subs{4}, subs{1} };

        % calc. initial image and the reference phase map
        im        =  fft( fft( fft( conj(ksp), [], 1), [], 2), [], 3);  % im's phase is wrong now, but we only want it's abs() to be correct
        phase     = ifft(ifft(ifft( kspLowRes, [], 1), [], 2), [], 3);
        phase     = exp(1i * angle(phase));

        % We use a trick in the loop to avoid using ifft (fft is faster).
        % We only need to calculate the factor 1/N ourselves, with N = prod(sz)
        phase = phase ./ prod(sz);      % 1/N is absorbed inside the phase array, once

        % create image with calculated phasemap from low res image
        im = abs(im) .* phase;

        % In the loop, we want to know where we have to copy the
        % measured data to, so we set the subscript of the pf dimension
        % accordingly.
        % We have to do this due to the ifftshift'ing above.
        tmp         = false( 1, sz(pfDim));
        tmp(1:Nsmp) = true;
        subs{pfDim} = find(ifftshift(tmp));

        % release RAM
        clear  tmp  kspLowRes

        % only keep the acquired data in memory
        ksp = ksp(subs{:});
    % ) ===================================================================

    % Helpers for pretty-printing:
    % Such a mess for such beautiful output!
    b = repmat('=',1,80);
    progress_str = 'starting POCS loop...';
    fprintf( '%s\n%s\n%s   %s', b, b(1), b(1), progress_str )
    edging = sprintf( '\n%s\n%s', b(1), b );
    fprintf( edging )

    % ( ===================================================================
    % iterative reconstruction POCS
    %
    tic
    for ii = double(~watchProgress) : iter

        if ii > 0

            % Fourier transform the image to k-space
            im = fft(fft(fft(  im  ,[],1),[],2),[],3);      % "im" is a really bad variable name now
                                                            % but we save a lot of RAM with this
            % Data Consistency:
            % insert original data where we have them
            im(subs{:}) = ksp;                              % "im" is still our reconstructed k-space signal

            % Fourier transform into image domain
            im = conj( im );
            im = fft(fft(fft(  im  ,[],1),[],2),[],3);      % Now, "im" is an image again.

            % create image with calculated phasemap from low res image
            im = abs(im) .* phase;

            prevLength = numel(progress_str) + numel(edging);
            t = toc;
            ETA = (t./ii) * iter  - t;
            progress_str = sprintf( 'Iteration %g/%g, in %g s,  ETA: %g s...', ii, iter, t, ETA );

            fprintf([repmat('\b',1,prevLength) '%s' '%s'], progress_str, edging );

        end % if ii > 0

        % a rough way to monitor the progress
        %
        if watchProgress
            tmp = ifftshift(sqrt(sum(abs(im(:,:,1,:).^2),4)));      % due to fftshift(), the 1st partition is the central one
            maxRange = sort( tmp(:), 'descend' );
            maxRange = maxRange( ceil(0.05 * numel(maxRange)) );    % ignore the "hottest" 5%

            if ~exist('pic','var')
                pic = [tmp tmp zeros(size(tmp),prec)];
                diffScale = 1;
            else
                delta = abs( pic(:,Nx+(1:Nx)) - tmp );
                diffScale = 0.5 * maxRange / median( delta(:) );
                pic(:,  Nx+(1:Nx)) = tmp;
                pic(:,2*Nx+(1:Nx)) = diffScale * delta;
                clear delta
            end

            figure(999)
            imagesc( pic, [0    maxRange ] )
            title(sprintf('\\bfiteration %g\ninitial     |    current     |     abs(previous - current) × %g', ii, diffScale ))
            axis image
            colormap(gray(256))
            drawnow
            clear tmp

            %if Nz == 1          % little pause for 2D (too fast otherwise)
            %    pause(2 / iter)
            %end
        end

    end % for ii = 1:iter
    fprintf([repmat('\b',1, numel(progress_str) + numel(edging)) 'POCS done! (%g s)' '%s\n\n'], t, edging );
    % ) ===================================================================

    clear  phase  pic

    % ( ===================================================================
    % The main part is over. Time for some thoughts.
    %
    % We began with a dataset that had fewer data samples than would be
    % necessary for an unambiguous image reconstruction. As a consequence,
    % an infinite number of images corresponds to the acquired data.
    % The above iteration picks that single image whose abs() fits the data
    % AND whose phase corresponds to the low-resolution phase, obtained
    % using the symmetric part of the data.
    %
    % Viewed in k-space, there is almost always a severe edge at the border
    % between acquired and interpolated data, which is due to imperfections
    % in the assumptions made.
    % Namely, phase often has some high frequency components which cannot be
    % accounted for in the low-resolution map. Additionally, there is noise
    % and we may have changing contrast or trajectory errors in our MRI
    % sequence.
    %
    %         ^
    %         | A A A A A A A A   \
    %         | A A A A A A A A
    %         | A A A A A A A A     acquired signal
    %      k2 | A A A A A A A A
    %         | A A A A A A A A   /
    %         | I I I I I I I I   \
    %         | I I I I I I I I     interpolated data
    %         | I I I I I I I I   /
    %         ----------------->
    %                 k1
    %
    % Empirically, it should be wise to create a smoother transition from
    % the acquired part of the signal to the interpolated data.
    %
        Ntrans = floor( (szSym-1)/3 );      % width of the transition zone

        % Create subscripts where we intend to keep the measured data, only.
        tmp                 = false( 1, sz(pfDim));
        tmp(1:Nsmp-Ntrans)  = true;
        subsPure            = subs;
        subsPure{pfDim}     = find(ifftshift(tmp));

        % Create subscripts where we want to have a smooth transition between
        % measured and phase-corrected data.
        subsTrans           = subs;
        subsTrans{pfDim}    = setdiff( subs{pfDim}, subsPure{pfDim} );

        % build a filter for the transition:
        tmp         = hann( 2*Ntrans+3, 'symmetric');
        filterTrans = tmp( Ntrans+3 : end-1 );
        filterTrans = reshape( filterTrans, [ ones(1,pfDim-1)  Ntrans  1] );

        % Seperate data in unfiltered part and transition zone.
        tmp = zeros( size(im), prec );
        tmp(subs{:}) = ksp;
        kspPure  = tmp(subsPure{:});
        kspTrans = tmp(subsTrans{:});
        clear  tmp  ksp

        im = fft(fft(fft(  im  ,[],1),[],2),[],3);      % "im" becomes k-space signal, again

        im(subsPure{:}) = kspPure;                      % strict data consistency for Nsmp-Ntrans samples
        im(subsTrans{:}) =  bsxfun( @times,   filterTrans,  kspTrans         )     ...
                          + bsxfun( @times, 1-filterTrans,  im(subsTrans{:}) );

        clear  subsPure  subsTrans  filterTrans  kspPure  kspTrans

        if nargout > 1
            kspFull = im;
        else
            kspFull = double.empty([sz 0]);     % kspFull exists, but no memory required
        end

        im = ifft(ifft(ifft(  im  ,[],1),[],2),[],3);   % "im" is an image, again
    % ) ===================================================================

    % ( ===================================================================
    % Undo the prerequisites (--> postrequisites???)
    %
        % undo the permutations
        im      = permute(      im, [4 1 2 3] );
        kspFull = permute( kspFull, [4 1 2 3] );
        subs    = { subs{4}, subs{1}, subs{2}, subs{3} };

        % undo the fftshifts
        im      = cmshiftnd(      im, [0  sz/2] );
        kspFull = cmshiftnd( kspFull, [0  sz/2] );

        % undo flipping
        if isLower
            subs{pfDim+1} = sz(pfDim):-1:1;
            im            = im(subs{:});
            kspFull       = kspFull(subs{:});
        end
    % ) ===================================================================

    if wasAddedCoilDim                              % we initially reshaped a simple 2D raw data matrix to be of size 1 x Ny x Nx
        im      = reshape(      im, Ny, Nx, [] );
        kspFull = reshape( kspFull, Ny, Nx, [] );
    end

end     % of pocs()

% =========================================================================
%                                                                         =
%                      SWAPPED CODE                                       =
%                                                                         =
% =========================================================================

function [ ksp, pfDim, isUpper, isLower, Nsmp ] = zerofillPFdim( ksp, wasAddedCoilDim )
    % Only the acquired data were passed and we have to find the asymmetric
    % dimension. Then we increase the size along this dimension and pad with 0.

    Ndim    = ndims( ksp ) - 1;     % one dimension was for the channels
    sz      = size(  ksp );
    sz      = sz(  2:end );         % ignore channel dimension
    Nc      = size(  ksp, 1 );
    prec    = class( ksp );

    % init some helper variables
    pfDim    = 0;               % partial Fourier reduction dimension
    isUpper  = false;
    isLower  = false;
    isPartialFourier = false(Ndim,1);

    % ( ===============================================================
    % autodetect the Partial Fourier dimension
    %
    for d = 1:Ndim

        centre = floor( sz(d)/2 ) + 1;

        tmp = squeeze( sum(abs(ksp),1) );
        for d2 = 1:Ndim
            if d2 ~= d
                tmp = max(tmp,[],d2);       % keep only the maximum of non-partial data points
            end
        end
        [ dummy, maxPos(d) ] = max( tmp(:) );   %#ok <-- don't use "~", for compatibility

        if abs(maxPos(d) - centre) >= 2      % significant asymmetry ==> partial Fourier acquisition

            isPartialFourier(d) = true;
            pfDim = d;
            Nsmp = sz(d);

            isUpper = maxPos(d) > centre;   % Did we sample the upper matrix part, so the lower part is missing...
            isLower = maxPos(d) < centre;   % ... or are the first data points missing (e.g. asymmetric echo)?
        end
    end % for d = 1:Ndim
    %
    % ) ===== (PF dim detection) ======================================

    switch nnz(isPartialFourier)    % |N|umber of |N|on |Z|ero elements
        case 0
            error( 'pocs:NoPfDim', 'No partial Fourier dimension found.' )
        case 1
            fprintf( 'Found partial Fourier along array dimension %d\n', pfDim + ~wasAddedCoilDim )
        otherwise
            error( 'pocs:TooManyPfDims', 'Partial Fourier only allowed in 1 dimension, but %g were found!', nnz(isPartialFourier) )
    end

    if pfDim == 0   % our init value above
        error('zerofillPF:NoPF','No partial Fourier property found!')
    end

    % initialize a cell of subscripts
    subs = { ':', ':', ':', ':' };      % all channels / all Ny / all Nx / all Nz

    c = maxPos(pfDim);

    if isUpper

        sz(pfDim) = 2 * (c - mod(c,2));         % determine the blown-up size we want to achieve
        subs{pfDim+1} = 1:Nsmp;

    elseif  isLower

        sz(pfDim) = 2 * (Nsmp - c + 1);
        c = floor( sz(pfDim)/2 ) + 1;
        sz(pfDim) = sz(pfDim) + 2*~mod(c,2);    % A hack for Stefan's data... keep an eye on this!
        subs{pfDim+1} = (1:Nsmp) + (sz(pfDim)-Nsmp);

    else
        error( 'zerofillPF:PFdimNotClassified', 'Could not tell how partial Fourier was implemented.' )
    end

    % do the zerofilling
    tmp = zeros( [Nc sz], prec );
    tmp(subs{:}) = ksp;
    ksp = tmp;

end % of zerofillPFdim()

function [ pfDim, isUpper, isLower, Nsmp ] = detectPFdim( smplPtrn, wasAddedCoilDim )
    % User passed already zero-padded data. This was nice, now it's easy
    % to find the partial Fourier dimension!

    Ndim = ndims( smplPtrn );
    sz = size( smplPtrn );

    % init some helper variables
    pfDim    = 0;               % partial Fourier reduction dimension
    isUpper  = false;
    isLower  = false;
    isPartialFourier = false( Ndim, 1 );

    % ( ===============================================================
    % Determine if this is a zerofilled partial Fourier measurement
    % and along which dimension the data is reduced.
    %
    % smplPtrn in Partial Fourier looks like this:
    %
    %     ^
    %     | 1 1 1 1 1 1 1 1     --->  sampling pattern is the same
    %     | 1 1 1 1 1 1 1 1           for all k1 points
    %     | 1 1 1 1 1 1 1 1
    %  k2 | 1 1 1 1 1 1 1 1             i.e. for programming:
    %     | 1 1 1 1 1 1 1 1             smplPtrn == repmat( smplPtrn(:,1,1), [1 Nx Nz] )
    %     | O O O O O 0 0 0
    %     | O O O O O 0 0 0
    %     | O O O O O 0 0 0
    %      ----------------->
    %           k1
    %
    for d = 1:Ndim

        subs = { ones(1,sz(d)),     ... % initialize a cell of subscripts we might be interested in
                 ones(1,sz(d)),     ...
                 ones(1,sz(d))  };
        subs{d} = 1:sz(d);              % we ask for all entries in the d'th dimension

        idx_d = sub2ind( sz, subs{:} ); % convert to linear array indices

        oneCol = smplPtrn( idx_d );     % one column of the d'th dimension

        % create a rule how to reshape oneCol
        reshRule = ones(1,Ndim);
        reshRule(d) = sz(d);                    % e.g. reshRule = [   1   1 128 ]
        oneCol = reshape( oneCol, reshRule);

        % create a rule how to replicate oneCol
        repRule = sz;
        repRule(d) = 1;                         % e.g. repRule  = [ 256 256   1 ]

        % Check if we get the sampling pattern again
        % just by replicating oneCol along the other dimensions
        isPartialFourier(d) = isequal( smplPtrn, repmat( oneCol, repRule ) );

        if isPartialFourier(d)
            pfDim = d;
            Nsmp  = nnz( oneCol );      % how many fully sampled lines do we have?

            % Sampled upper or lower part of k-space matrix?
            isUpper = isequal( oneCol(:).', [ true( 1,Nsmp)          false(1,sz(d)-Nsmp)    ]);
            isLower = isequal( oneCol(:).', [ false(1,sz(d)-Nsmp)    true( 1,Nsmp)          ]);
        end
    end
    % ) ===============================================================

    switch nnz(isPartialFourier)    % |N|umber of |N|on |Z|ero elements
        case 0
            error( 'pocs:NoPfDim', 'No partial Fourier dimension found.' )
        case 1
            fprintf( 'Found partial Fourier along array dimension %d\n', pfDim + ~wasAddedCoilDim )
        otherwise
            error( 'pocs:TooManyPfDims', 'Partial Fourier only allowed in 1 dimension!' )
    end

end % of detectPFdim()

function [ centreLine, idxSym ] = findSymSampled( ksp, pfDim, Nsmp )

    Ndim = ndims( ksp ) - 1;    % one for channels
    sz = size( ksp );
    sz = sz(2:end);

    % autodetect the central k-space line
    %if ~exist('centreLine', 'var') || isempty(centreLine)
        tmp = squeeze( sum(abs(ksp),1) );
        for d = 1:Ndim
           if d ~= pfDim
               tmp = max(tmp,[],d);     % keep only the maximum of non-partial data points
           end
        end
        [ dummy, centreLine] = max( tmp(:) );   %#ok the central line has the max intensity
    %end

    % calculate the size of the symmetric part and the full dataset
    startSym = centreLine - (Nsmp - centreLine);    % start of our symmetric sampling
    endSym   = centreLine + (Nsmp - centreLine);    % end of symmetric part
    idxSym   = startSym : endSym;

    if any(idxSym < 1)    ||   any(idxSym > sz(pfDim))
       error( 'pocs:BadDataProperty' , 'Symmetric part of k-space out of bounds.\nThe maximum k-space intensity is at index %g whereas it should be centred => near %g.\nThe way, zerofilling was done is probably wrong.\nCheck your input k-space.', centreLine, round(sz(pfDim)/2) )
    end

end % of findSymmetricSampled()

function x = cmshiftnd( x, shifts)
%Function to circularly shift N-D arrays

    if nargin < 2 || all(shifts(:) == 0)
       return                       % no shift
    end

    sz      = size( x );
    numDims = ndims(x);             % number of dimensions
    idx = cell(1, numDims);         % creates cell array of empty matrices,
                                    % one cell for each dimension

    for k = 1:numDims

        m = sz(k);
        p = ceil(shifts(k));

        if p < 0
            p = m + p;
        end

        idx{k} = [p+1:m  1:p];
    end

    % Use comma-separated list syntax for N-D indexing.
    x = x(idx{:});

end % of cmshiftnd()

% Avoid the need for the signal toolbox and implement
% hamming() and hann() manually:
%
function w = hamming( N, symFlag )
%Hamming window
%
% w = hamming(L) returns an L-point symmetric Hamming window in the column vector w.
% L should be a positive integer.
%
%  The coefficients of a Hamming window are computed from the following equation:
%
%       w(n) = 0.54  +  0.46 * cos(2*pi*n/N),   0 <= n <= N
%
%
% w = hamming( L, 'symFlag') returns an L-point Hamming window using the window sampling
% specified by 'symFlag', which can be either 'periodic'  or 'symmetric' (the default).
% The 'periodic' flag is useful for DFT/FFT purposes, such as in spectral analysis.
% The DFT/FFT contains an implicit periodic extension and the periodic flag enables a signal
% windowed with a periodic window to have perfect periodic extension.
% When 'periodic' is specified, hamming computes a length L+1 window and returns the first L points.
% When using windows for filter design, the 'symmetric' flag should be used.
%
% --> http://www.mathworks.de/de/help/signal/ref/hamming.html
% --> https://de.wikipedia.org/wiki/Hamming-Fenster

% implemented by Michael.Voelker@mr-bavaria.de, 2012

    if ~exist( 'N', 'var' ) || isempty(N) || numel(N) ~= 1 || ~isnumeric(N)  || ~isfinite(N) || N < 1 || floor(N) ~= N
        error( 'hamming:badSize', 'Window lenght must be a positive integer.' )
    end
    if ~exist( 'symFlag', 'var' ) || isempty(symFlag)
        symFlag = 'symmetric';
    end

    if N == 1
        w = 1;
        return
    end

    switch symFlag
        case 'symmetric'
            L = N-1;
        case 'periodic'
            L = N;
        otherwise
            error('hamming:symFlag', 'Unknown symmetry flag. Try ''symmetric'' (default) or ''periodic''.')
    end

    w = (0:N-1) - L/2;
    w = 0.54  +  0.46 * cos(2*pi * w(:)./L);

end % of hamming()

function w = hann( N, symFlag )
%von-Hann (Hanning) window
%
% w = hann(L) returns an L-point symmetric Hann window in the column vector w.
% L must be a positive integer.
%
% The coefficients of a Hann window are computed from the following equation:
%
%      w(n) = 0.5 * (1 + cos(2*pi*n/N)),   0 <= n <= N
%
% The window length is L = N+1.
%
% w = hann(L,'sflag') returns an L-point Hann window using the window sampling specified by 'sflag',
% which can be either 'periodic' or 'symmetric' (the default). The 'periodic' flag is useful for DFT/FFT purposes,
% such as in spectral analysis.
% The DFT/FFT contains an implicit periodic extension and the periodic flag enables a signal windowed
% with a periodic window to have perfect periodic extension.
% When 'periodic' is specified, hann computes a length L+1 window and returns the first L points.
% When using windows for filter design, the 'symmetric' flag should be used.
%
% --> http://www.mathworks.de/de/help/signal/ref/hann.html
% --> https://de.wikipedia.org/wiki/Hann-Fenster

% implemented by Michael.Voelker@mr-bavaria.de, 2012

    if ~exist( 'N', 'var' ) || isempty(N) || numel(N) ~= 1 || ~isnumeric(N)  || ~isfinite(N) || N < 1 || floor(N) ~= N
        error( 'hann:badSize', 'Window lenght must be a positive integer.' )
    end
    if ~exist( 'symFlag', 'var' ) || isempty(symFlag)
        symFlag = 'symmetric';
    end

    if N == 1
        w = 1;
        return
    end

    switch symFlag
        case 'symmetric'
            L = N-1;
        case 'periodic'
            L = N;
        otherwise
            error('hann:symFlag', 'Unknown symmetry flag. Try ''symmetric'' (default) or ''periodic''.')
    end

    w = (0:N-1) - L/2;
    w = 0.5 * ( 1 + cos(2*pi * w(:)./L) );

end % of hann()


function[total_cost,grad]=softmax(result,docbatch,parameter)
    total_cost=0;
    grad.soft_W=zeroMatrix(size(parameter.soft_W));
    step_size=1;
    num_word=0;
    N=size(docbatch.target_sen_matrix,1);
    zeroState=zeroMatrix([parameter.hidden,N]);
    for ll=1:parameter.layer_num
        grad.source_h{ll,1}=zeroState;
        grad.source_c{ll,1}=zeroState;
        for sen_tt=1:length(result.Target_sen)-1
            grad.target_sen_h{ll,sen_tt}=zeroState;
            grad.target_sen_c{ll,sen_tt}=zeroState;
        end
    end
    for sen_tt=1:length(result.Target_sen)
        Word_List=docbatch.target_word{sen_tt}.Word;
        Word_Mask=docbatch.target_word{sen_tt}.Mask;
        num_word=num_word+length(find(Word_Mask==1));
        N=size(Word_List,1);
        T=size(Word_List,2);
        target_sen=result.Target_sen{sen_tt};
        N_examples=size(Word_List,1)*size(Word_List,2);
        predict_Words=reshape(Word_List,1,N_examples);
        mask=reshape(Word_Mask,1,N_examples);
        h_t=[];
        if sen_tt==1
            h_t=[h_t,result.source_sen{parameter.layer_num,1}];
        else
            dim=size(result.h_t_target_sen);
            h_t=[h_t,result.h_t_target_sen{dim(1),sen_tt-1}];
        end
        dim=size(target_sen.h_t_target_word);
        h_t=[h_t,[target_sen.h_t_target_word{parameter.layer_num,1:dim(2)-1}]];
        [cost,grad_softmax_h]=batchSoftmax(h_t,mask,predict_Words,parameter);
        total_cost=total_cost+cost;
        grad.soft_W=grad.soft_W+grad_softmax_h.soft_W;
        if sen_tt==1
            grad.source_h{parameter.layer_num,1}=grad_softmax_h.h(:,1:N);
        else
            grad.target_sen_h{parameter.layer_num,sen_tt-1}=grad_softmax_h.h(:,1:N);
        end
        for i=1:T-1
            grad.ht{sen_tt}{1,i}=grad_softmax_h.h(:,N*i+1:N*(i+1));
        end
    end
    total_cost=total_cost/N;
    grad.soft_W=grad.soft_W/N;
    clear predict_Words; clear mask;
    clear grad_softmax_h;
end

function[cost,softmax_grad]=batchSoftmax(h_t,mask,predict_Words,parameter)
    unmaskedIds=find(mask==1);
    scores=parameter.soft_W*h_t;
    mx = max(scores,[],1);
    scores=bsxfun(@minus,scores,mx);
    scores=exp(scores);
    norms = sum(scores, 1);
    if length(find(mask==0))==0
        scores=bsxfun(@rdivide, scores, norms);
    else
        scores=bsxfun(@times,scores, mask./norms);
    end
    scoreIndices = sub2ind(size(scores),predict_Words(unmaskedIds),unmaskedIds);
    cost=sum(-log(scores(scoreIndices)));
    scores(scoreIndices) =scores(scoreIndices) - 1;
    softmax_grad.soft_W=scores*h_t';  %(N_word*examples)*(examples*diemsnion)=N_word*diemsnion;
    softmax_grad.h=(scores'*parameter.soft_W)';%(diemsnion*N_word)*(N_word*examples)=dimension*examples
    clear scores;
    clear norms;
end


function inds = meshBoundaryVertexIndices(varargin)
%MESHBOUNDARYVERTEXINDICES Indices of boundary vertices of a mesh.
%
%   INDS = meshBoundaryVertexIndices(V, F)
%   INDS = meshBoundaryVertexIndices(V, E, F)
%
%   Example
%     % create centered icosahedron
%     [v, f] = createIcosahedron;
%     v(:,3) = v(:,3) - mean(v(:,3));
%     % convert to simili-sphere
%     [v2, f2] = subdivideMesh(v, f, 3);
%     v3 = normalizeVector3d(v2);
%     % clip with plane
%     plane = createPlane([0 0 0], [-1 -2 3]);
%     [vc, fc] = clipMeshVertices(v3, f2, plane, 'shape', 'plane');
%     figure; drawMesh(vc, fc); axis equal; view(3);
%     % draw boundary vertices
%     inds = meshBoundaryVertexIndices(vc, fc);
%     hold on; drawPoint3d(vc(inds,:), 'k*');
%
%   See also
%     meshes3d, meshBoundary, meshBoundaryEdgeIndices, meshEdgeFaces

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2019-05-01, using Matlab 8.6.0.267246 (R2015b)
% Copyright 2019-2022 INRA - Cepia Software Platform

[vertices, edges, faces] = parseMeshData(varargin{:});

% Compute edge-vertex map if not specified
if isempty(edges)
    edges = meshEdges(vertices, faces);
end

% compute edges to faces map
edgeFaces = meshEdgeFaces(vertices, edges, faces);

borderEdges = sum(edgeFaces == 0, 2) > 0;

inds = edges(borderEdges, :);
inds = unique(inds(:));


function linpack_z_test18 ( )

%*****************************************************************************80
%
%% TEST18 tests ZPBDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;
  m = 1;
  lda = m+1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST18\n' );
  fprintf ( 1, '  For a double precision complex (C)\n' );
  fprintf ( 1, '  positive definite hermitian band matrix (PB),\n' );
  fprintf ( 1, '  ZPBDI computes the determinant as\n' );
  fprintf ( 1, '    det = MANTISSA * 10**EXPONENT\n' );
  fprintf ( 1, '\n' );
%
%  Set the value of the superdiagonal and diagonal.
%
  a(1,1) = complex ( 0.0000,  0.0000 );
  a(1,2) = complex ( 2.1341, -0.2147 );
  a(1,3) = complex ( 2.0905,  1.1505 );

  a(2,1) = complex ( 4.5281,  0.0000 );
  a(2,2) = complex ( 5.0371,  0.0000 );
  a(2,3) = complex ( 4.7638,  0.0000 );

  [ a, info ] = zpbfa ( a, lda, n, m );

  if ( info ~= 0 )
    fprintf ( 1, '  Error!  ZPBFA returns INFO = %d\n', info );
    return
  end

  det = zpbdi ( a, lda, n, m );

  fprintf ( 1, '  Determinant = %f * 10^(%f)\n', det(1), det(2) );

  return
end