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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defop  Method   @@sym rdivide {(@var{x}, @var{y})}
%% @defopx Operator @@sym {@var{x} ./ @var{y}} {}
%% Element-wise forward slash division of symbolic expressions.
%%
%% Example:
%% @example
%% @group
%% syms x
%% A = sym([1 137; 3 4])
%%   @result{} A = (sym 2×2 matrix)
%%       ⎡1  137⎤
%%       ⎢      ⎥
%%       ⎣3   4 ⎦
%% B = [x pi; 2*x 8]
%%   @result{} B = (sym 2×2 matrix)
%%       ⎡ x   π⎤
%%       ⎢      ⎥
%%       ⎣2⋅x  8⎦
%% A ./ B
%%   @result{} ans = (sym 2×2 matrix)
%%       ⎡ 1   137⎤
%%       ⎢ ─   ───⎥
%%       ⎢ x    π ⎥
%%       ⎢        ⎥
%%       ⎢ 3      ⎥
%%       ⎢───  1/2⎥
%%       ⎣2⋅x     ⎦
%% @end group
%% @end example
%%
%% Either @var{x} or @var{y} can be scalar:
%% @example
%% @group
%% A ./ 2
%%   @result{} ans = (sym 2×2 matrix)
%%       ⎡1/2  137/2⎤
%%       ⎢          ⎥
%%       ⎣3/2    2  ⎦
%% 2 ./ B
%%   @result{} ans = (sym 2×2 matrix)
%%       ⎡2   2 ⎤
%%       ⎢─   ─ ⎥
%%       ⎢x   π ⎥
%%       ⎢      ⎥
%%       ⎢1     ⎥
%%       ⎢─  1/4⎥
%%       ⎣x     ⎦
%% @end group
%% @end example
%%
%% Finally, the can both be scalar:
%% @example
%% @group
%% 2 ./ x
%%   @result{} ans = (sym)
%%       2
%%       ─
%%       x
%% @end group
%% @end example
%% @seealso{@@sym/ldivide, @@sym/mrdivide}
%% @end defop

function z = rdivide(x, y)

  %% 2022-06: TODO cannot simply call hadamard_product for sympy <1.9,
  %% see upstream: https://github.com/sympy/sympy/issues/8557

  cmd = { '(x,y) = _ins'
          'if x.is_Matrix and y.is_Matrix:'
          '    y_eltwise_recip = y.applyfunc(lambda a: 1/a)'
          '    if Version(spver) < Version("1.9"):'
          '        try:'
          '            return x.multiply_elementwise(y_eltwise_recip)'
          '        except:'
          '            pass'
          '    return hadamard_product(x, y_eltwise_recip)'
          'if not x.is_Matrix and y.is_Matrix:'
          '    return y.applyfunc(lambda a: x/a),'
          'else:'
          '    return x/y,' };

  z = pycall_sympy__ (cmd, sym(x), sym(y));

end

%!test
%! % scalar
%! syms x
%! assert (isa (x ./ 1, 'sym'))
%! assert (isa (x ./ x, 'sym'))
%! assert (isequal (x ./ 1, x))
%! assert (isequal (x ./ x, sym(1)))

%!test
%! % matrix-scalar
%! D = 2*[0 1; 2 3];
%! A = sym(D);
%! assert (isequal ( A./2 , D/2  ))
%! assert (isequal ( A./sym(2) , D/2  ))
%! assert (isequal ( D./sym(2) , D/2  ))

%!test
%! % matrix ./ matrix
%! D = [1 2; 3 4];
%! A = sym(D);
%! assert (isequal ( A./A , D./D  ))
%! assert (isequal ( A./D , D./D  ))
%! assert (isequal ( D./A , D./D  ))

%!test
%! % matrix ./ matrix with symbols
%! syms x y
%! A = [x y; x^2 2*y];
%! B = [y x; x y];
%! assert (isequal ( A./A , sym(ones(2,2)) ))
%! assert (isequal ( A./B , [x/y y/x; x 2] ))

%!test
%! % scalar ./ matrix
%! D = [1 2; 3 4];
%! A = sym(D);
%! assert (isequal ( 12./A , 12./D  ))


function mtrPDBScoreHist(filename,chColor,lowThresh,dt6Filename)

% lowThresh is value between 0 and 1, i.e. 0.1 will cut off the bottom 10%
% from the histogram
% chColor can be usual 'r', 'g', 'b', ...

if ieNotDefined('dt6Filename')
  outPathName = pwd;
  [f,p] = uigetfile('*.mat', 'Select dt6 file...', outPathName);
  if(isnumeric(f)) error('User cancelled.'); end
  dt6Filename = fullfile(p,f);
end

if ieNotDefined('lowThresh')
    lowThresh = 0.1;
end

% Load pathways
dt6 = load(dt6Filename,'xformToAcPc');
disp('Loading pathways ...');
fg = mtrImportFibers(filename, dt6.xformToAcPc);

% Find length param
scoreVec = mtrGetFGScoreVec(fg);
scoreVec = sort(scoreVec,'descend');
[n,xout] = hist(scoreVec(1:end*(1-lowThresh)),linspace(-50,150,20));
%bar(xout,n,chColor);
nWithNoZeros = n;
nWithNoZeros(n==0) = 1;
plot(xout,log(nWithNoZeros).*double(n>0),chColor);
xlabel('ln(score)');
ylabel('ln(count)');


function alpha = complexNormalAngle(varargin)
%COMPLEXNORMALANGLE compute normal angle of a vertex of a cellular complex
%
%   ALPHA = complexNormalAngle(NODES, EDGES, FACES, INDEX)
%   ALPHA = complexNormalAngle(NODES, EDGES, FACES, CELLS, INDEX)
%   Compute the nortmal angle of the polyhedral reconstruction defined be
%   nodes NODES, edges EDGES and faces FACES. For 3D reconstructions, it
%   can also contain cells CELLS. INDEX is the index of NODES for which the
%   normal angle ALPHA is computed.
%   Result is normalised between 0 and 2*PI.
%
%   ALPHA = complexNormalAngle(GRAPH, INDEX)
%   Internal data are stored in a structure GRAPH, with fields : 'nodes',
%   'edges', 'faces', and eventually 'cells'.
%
%
%   ALPHA = complexNormalAngle(..., INDICES)
%   If INDICES is an array of indices, the normal angle is computed for
%   each element of NODES(INDICES,:). The result ALPHA has the same size
%   than INDICES.
%
%
% ------
% Author: David Legland
% e-mail: david.legland@jouy.inra.fr
% Created: 2005-12-19
% Copyright 2005 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   2006-04-19 fix bug for small number of faces
%   2006-04-26 returns 0 and not [] for null complexes
%   2006-10-25 revert to return value=[] for null complex
%   2008-08-11 code clean up

cells = [];
if length(varargin)==4
    % no cells in cellular complex
    nodes = varargin{1};
    edges = varargin{2};
    faces = varargin{3};
    ind   = varargin{4};

elseif length(varargin)==5
    % cells are given
    nodes = varargin{1};
    edges = varargin{2};
    faces = varargin{3};
    cells = varargin{4};
    ind   = varargin{5};

elseif length(varargin)==2
    % data stored as structure
    graph = varargin{1};
    nodes = graph.nodes;
    edges = graph.edges;
    faces = graph.faces;
    if isfield(graph, 'cells')
        cells = graph.cells;
    end
    ind   = varargin{2};
else
    error('wrong number of arguments');
end

alpha0 = zeros([length(ind) 1]);
alpha1 = zeros([length(ind) 1]);
alpha2 = zeros([length(ind) 1]);
alpha3 = zeros([length(ind) 1]);
alpha = [];

if size(nodes, 2)==2
    % 2 dimensions

    if iscell(faces)

        % process faces as cell array
        for i=1:length(ind)
            % check that vertex is contained in the complex
            if ind(i)>size(nodes, 1)
                continue;
            end

            % normal angle of vertex
            alpha0(i) = 2*pi;

            % normal angle of edges
            alpha1(i) = length(find(sum(edges==ind(i), 2)))*pi;

            % normal angle of faces
            alpha2(i) = 0;
            for j=1:length(faces)
                face = faces{j};
                indf = find(face==ind(i));

                if ~isempty(indf)
                    alpha2(i) = alpha2(i) + polygonNormalAngle(nodes(face,:), indf);
                end
            end
        end

    else
        % process faces as arrays
        for i=1:length(ind)

            % check that vertex is contained in the complex
            if ind(i)>size(nodes, 1)
                continue;
            end

            % normal angle of vertex
            alpha0(i) = 2*pi;

            % normal angle of edges
            alpha1(i) = length(find(sum(edges==ind(i), 2)))*pi;

            % normal angle of faces
            alpha2(i) = 0;
            for j=1:size(faces, 1)
                face = faces(j,:);
                indf = find(face==ind(i));

                if ~isempty(indf)
                    alpha2(i) = alpha2(i) + polygonNormalAngle(nodes(face,:), indf);
                end
            end

        end
    end

    % compute total normal angle of reconstruction
    alpha = alpha0 - alpha1 + alpha2;

elseif size(nodes, 2)==3
    % 3 dimensions
    for i=1:length(ind)

        % check that vertex is contained in the complex
        if ind(i)>size(nodes, 1)
            continue;
        end

        % normal angle of vertex
        alpha0(i) = 4*pi;

        % normal angle of edges
        alpha1(i) = length(find(sum(edges==ind(i), 2)))*2*pi;

        % normal angle of faces
        alpha2(i) = 0;
        if iscell(faces)
            % process faces as cell array
            for j=1:length(faces)
                face = faces{j};
                indf = find(face==ind(i));

                if ~isempty(indf)
                    alpha2(i) = alpha2(i) + polygon3dNormalAngle(nodes(face,:), indf);
                end
            end

        else
            % process faces as array of double
            for j=1:size(faces, 1)
                face = faces(j,:);
                indf = find(face==ind(i));

                if ~isempty(indf)
                    alpha2(i) = alpha2(i) + polygon3dNormalAngle(nodes(face,:), indf);
                end
            end
        end

        % normal angle of cells
        alpha3(i) = 0;
        for j=1:length(cells)
            cell = cells{j};
            if iscell(faces)
                cellFaces = faces(cell);
            else
                cellFaces = faces(cell, :);
            end

            alpha3(i) = alpha3(i) + polyhedronNormalAngle(nodes, cellFaces, ind(i));
        end

        % compute total normal angle of reconstruction
        alpha = alpha0 - alpha1 + alpha2 - alpha3;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% set of internal functions, for angle computations in 2D and 3D

function theta = polygonNormalAngle(points, ind)
%POLYGONNORMALANGLE compute normal angle at a vertex of the polygon

% number of points
np = size(points, 1);

% number of angles to compute
nv = length(ind);

theta = zeros(nv, 1);

for i=1:nv
    p0 = points(ind(i), :);

    % previous vertex
    if ind(i)==1
        p1 = points(np, :);
    else
        p1 = points(ind(i)-1, :);
    end

    % next vertex
    if ind(i)==np
        p2 = points(1, :);
    else
        p2 = points(ind(i)+1, :);
    end

    % compute angles
    theta1  = mod(atan2(p1(2)-p0(2), p1(1)-p0(1)) + 2*pi, 2*pi);
    theta2  = mod(atan2(p2(2)-p0(2), p2(1)-p0(1)) + 2*pi, 2*pi);
    dtheta  = mod(theta2-theta1+2*pi, 2*pi);

    % use simplification due to the fact that cells are convex
    dtheta  = min(dtheta, 2*pi-dtheta);
    theta(i)= pi - dtheta;
end

return;

function theta = polyhedronNormalAngle(nodes, faces, ind)
%POLYHEDRONNORMALANGLE compute normal angle at a vertex of a 3D polyhedron
%
%   THETA = polyhedronNormalAngle(NODES, FACES, IND);
%   where NODES is a set of 3D points, and FACES a set of faces, whose
%   elements are indices to NODES array, compute the normal angle at the
%   vertex whose index is given by IND.

% number of angles to compute
na = length(ind);

theta = zeros(na, 1);
for i=1:na

    % find faces containing given vertex,
    % and compute normal angle at each face containing vertex
    if iscell(faces)
        for j=1:length(faces)
            if ismember(ind(i), faces{j})
                % create 3D polygon
                face = nodes(faces{j}, :);

                % index of point in polygon
                indp = find(faces{j}==ind(i));

                % compute face angle
                thetaf = [thetaf polygon3dInnerAngle(face, indp)];
            end
        end
    else
        indf = find(sum(ismember(faces, ind(i)), 2));

        thetaf = zeros(length(indf), 1);
        for j=1:length(indf)
            ind2 = faces(indf(j), :);
            face = nodes(ind2, :);
            indp = find(ind2==ind(i));
            thetaf(j) = polygon3dInnerAngle(face, indp);
        end
    end

    % compute normal angle of polyhedron, by use of angle defect formula
    if ~isempty(thetaf)
        theta(i) = 2*pi - sum(thetaf);
    end
end

return;

function theta = polygon3dNormalAngle(points, ind)
%POLYGON3DNORMALANGLE compute normal angle at a vertex of the 3D polygon

theta = 2*pi - 2*polygon3dInnerAngle(points, ind);

return;

function theta = polygon3dInnerAngle(points, ind)
%POLYGON3DNORMALANGLE compute normal angle at a vertex of the 3D polygon

% number of points
np = size(points, 1);

% number of angles to compute
nv = length(ind);

theta = zeros(nv, 1);

for i=1:nv
    p0 = points(ind(i), :);

    % previous vertex
    if ind(i)==1
        p1 = points(np, :);
    else
        p1 = points(ind(i)-1, :);
    end

    % next vertex
    if ind(i)==np
        p2 = points(1, :);
    else
        p2 = points(ind(i)+1, :);
    end

    theta(i) = angle3d(p1, p0, p2);
    theta(i) = min(theta(i), 2*pi-theta(i));
    % todo: solve case for CW oriented polygons
end

return;


classdef HeEMOEA < ALGORITHM
% <multi> <real/integer> <expensive>
% Multiobjective evolutionary algorithm with heterogeneous ensemble based
% infill criterion
% Ke --- 5 --- Number of the solutions to be revaluated

%------------------------------- Reference --------------------------------
% D. Guo, Y. Jin, J. Ding, and T. Chai. Heterogeneous ensemble-based infill
% criterion for evolutionary multiobjective optimization of expensive
% problems. IEEE Transactions on Cybernetics, 2019, 49(3): 1012-1025.
%------------------------------- 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)
            assert(~isempty(ver('nnet')),'The execution of HeE-MOEA requires the Deep Learning Toolbox.');

            %% Parameter setting
            Ke = Algorithm.ParameterSet(5);
            NI = 11*Problem.D-1;
            P  = UniformPoint(NI,Problem.D,'Latin');
            A  = Problem.Evaluation(repmat(Problem.upper-Problem.lower,NI,1).*P+repmat(Problem.lower,NI,1));

            %% Settings of the Ensemble Model
            L    = 11*Problem.D-1+25;
            ADec = A.decs;
            AObj = A.objs;

            Selection = PSOMyself(ADec, AObj(:,1),Problem.D);
            str1={'FE', 'FS', 'NONE'};
            str2={'RBF1','SVM', 'RBF2'};%, 'KNN', 'DTree'
            for i=1:length(str1)
                for j=1:length(str2)
                    str{(i-1)*length(str2)+j, 1}=str1{i};
                    str{(i-1)*length(str2)+j, 2}=str2{j};
                end
            end

            while Algorithm.NotTerminated(A)
                %% Update the model
              % Select the train data
                ADec = A.decs;
                AObj = A.objs;
                Numdata = size(ADec,1);
                if Numdata <=L
                    % fprintf('No training data decrease\n');
                else
                    FrontNo   = NDSort(AObj,Numdata);
                    [~,index] = sort(FrontNo);
                    ADec1 = ADec(index(1:floor(L/2)), :);
                    AObj1 = AObj(index(1:floor(L/2)), :);
                    ADec2 = ADec(index(floor(L/2)+1:end), :);
                    AObj2 = AObj(index(floor(L/2)+1:end), :);
                    index = randperm(size(ADec2,1));
                    ADec  = [ADec1;ADec2(index(1:L-floor(L/2)),:)];
                    AObj  = [AObj1;AObj2(index(1:L-floor(L/2)),:)];
                end

                % Train the model
                Models = TrainModel(ADec,AObj,Selection,str,Problem.M,Problem.D);

                % Optimization
                New    = NSGA2ESelection(ADec,AObj,Models,str,Problem,Ke);
                PopNew = Problem.Evaluation(New);
                A      = [A PopNew];
            end
        end
    end
end


% a basic 3D ZTE/PETRA sequence
% achieves "TE" about 70 us and possibly below

%% high-level sequence parameters

fov=256e-3; dx=2e-3;            % Define FOV and resolution
alpha=4;                        % flip angle
Nr=300;                         % number of readout points (with some oversampling)
R= 8;                           % acceleration/undersampling (for the outer shell)
R_inner= 1;                     % acceleration/undersampling (angular direction of the inner area)
xSpoil=3;%0.6;  %0.6 for 2mm       % the amount of spoiling after the end of the readout (used to ramp to the next point)

Kmax=1/2/dx;
dK=1/fov;

%% more detailed and derived params

rf_duration=10e-6;              % duration of the excitation pulse
ro_duration=300e-6;             % read-out time: controls RO bandwidth and T2-blurring
minRF_to_ADC_time=50e-6;        % the parameter wich defines TE together with ro_discard
%ro_discard=4;                   % how many ADC samples are contaminated by RF switching artifacts and alike
rfSpoilingInc=117;              % RF spoiling increment

% system limits
sys = mr.opts('MaxGrad', 36, 'GradUnit', 'mT/m', ...
    'MaxSlew', 180, 'SlewUnit', 'T/m/s', 'rfRingdownTime', 10e-6, ...
    'rfDeadTime', 100e-6, 'adcDeadTime', 10e-6, 'gamma',42.576e6); % 1H: 42.576e6  23Na: 11.262e6

seq=mr.Sequence(sys);           % Create a new sequence object
seq_sar=mr.Sequence(sys);       % Create an auxillary sequence object for SAR testing

%% create main sequence elements

% %create alpha-degree block pulse
% rf = mr.makeBlockPulse(alpha*pi/180,'Duration',rf_duration,'system',sys);
%or create alpha-degree gaussian pulse
rf = mr.makeGaussPulse(alpha*pi/180,'Duration',rf_duration,'timeBwProduct',3,'system',sys);
Tenc=rf_duration/2+minRF_to_ADC_time+ro_duration; %encoding time
Tg=sys.rfDeadTime+rf_duration/2+Tenc+sys.adcDeadTime; % constant gradient time
Tt=ceil((Tenc*(1+xSpoil)-Tg)/sys.gradRasterTime)*sys.gradRasterTime; % transition time
Ag=Kmax/Tenc; % read gradient grdient amplitude
% % Gr=mr.makeTrapezoid('z','Amplitude',Ag,'flatTime',Tg,'riseTime',0); % this graient has no ramps, I wonder if the Matlab mr library would like it...
% Gr=mr.makeExtendedTrapezoid('z','times',[0 Tg],'amplitudes',[Ag Ag]); % this is a constant graient with no ramps
TR=Tg+Tt;

adc=mr.makeAdc(Nr,'Duration', ro_duration, 'Delay', sys.rfDeadTime+rf_duration+minRF_to_ADC_time);

SamplesBookkeeping=[];

%rfbw=1/rf_duration;
rfbw=mr.calcRfBandwidth(rf);
fprintf('Read gradient amplitude %g mT/m, effective "slice thinckess" %f mm\n', 1e3*Ag/sys.gamma, 1/Ag*rfbw*1e3);
FO=50e-3*Ag;
NF=0; % TR is broken now for NF~=0...

%% generate the sampling set on a surface of a sphere
[phi, theta, im]=spherical_samples(Kmax,dK,R);
Ns=length(phi);
SamplesBookkeeping=[SamplesBookkeeping Ns];

%% main ZTE loop
fprintf('Populating ZTE loop (%d TRs)\n', Ns);
tic;
populate_subsequence(sys,seq,rf,adc,phi,theta,im,Ns,Ag,TR,Tt,NF,FO);
toc

%% SPI loop
KstartZTE=Ag*(rf_duration/2+minRF_to_ADC_time+adc.dwell);
nKspi=floor(KstartZTE/dK);
dKspi=KstartZTE/(nKspi+1);
Tenc=rf_duration/2+minRF_to_ADC_time+adc.dwell/2;
fprintf('Populating SPI loop (%d spheres)\n', nKspi);
tic;
for s=nKspi:-1:1
    % generate the sampling set on a surface of a sphere
    [phi, theta, im]=spherical_samples(dKspi*s,dKspi,R_inner); % normally no acceleration
    Ns=length(phi);
    SamplesBookkeeping=[SamplesBookkeeping Ns];
    % update gradients
    Ag=dKspi*s/Tenc; % read gradient grdient amplitude
    % actually create the next sampling sphere
    fprintf('Populating sphere %d (%d TRs)\n',s,Ns);
    fprintf('Effective "slice thinckess" %f mm\n',  1/Ag*rfbw*1e3);
    populate_subsequence(sys,seq,rf,adc,phi,theta,im,Ns,Ag,TR,Tt,NF,FO);
end
% sample the centere of k-space
seq.addBlock(mr.makeDelay(Tt));
seq.addBlock(rf, adc, mr.makeDelay(TR-Tt));
SamplesBookkeeping=[SamplesBookkeeping 1];
toc
fprintf('Total number of SPI samples: %d; a Cartesian patch would require %d\n', sum(SamplesBookkeeping(2:end)), ceil(nKspi^3*pi*4/3));

% %% OLD!!! SPI loop
% Kspi=ceil(Ag*(rf_duration/2+minRF_to_ADC_time+adc.dwell)/dK);
% rf.delay=rf.delay+Tt;
% Tspi1=mr.calcDuration(rf)+sys.rfRingdownTime;
% adc.delay=adc.delay+Tt-Tspi1;
% Tspi2=TR-Tspi1;
%
% g=mr.makeTrapezoid('x','system',sys,'area',dK*Kspi,'duration',minRF_to_ADC_time);
% gx=g;gx.channel='x';
% gy=g;gy.channel='y';
% gz=g;gz.channel='z';
%
% gxr=g;gxr.channel='x';
% gyr=g;gyr.channel='y';
% gzSpoil=mr.makeTrapezoid('x','system',sys,'area',Kmax*(1+xSpoil),'duration',minRF_to_ADC_time);
%
% fprintf('Populating SPI loop (~%d TRs)\n', round(pi/6*((2*Kspi+1)^3)));
% tic;
%
% % SPI loop itself
% gxr.amplitude=0;
% gyr.amplitude=0;
% for k=-Kspi:Kspi
%     for j=-Kspi:Kspi
%         for i=-Kspi:Kspi
%             if i^2+j^2+k^2>Kspi^2
%                 continue;
%             end
%             % refocusing and spoiling for the previous TR
%             seq.addBlock( rf, gxr, gyr, gzSpoil, mr.makeDelay(Tspi1));
%             % new TR
%             gx.amplitude=g.amplitude/Kspi*i;
%             gy.amplitude=g.amplitude/Kspi*j;
%             gy.amplitude=g.amplitude/Kspi*k;
%             seq.addBlock( rf, adc, gx, gy, gz, mr.makeDelay(Tspi2));
%             gxr.amplitude=-gx.amplitude;
%             gyr.amplitude=-gy.amplitude;
%         end
%     end
% end
% toc

%% check whether the timing of the sequence is correct
[ok, error_report]=seq.checkTiming;

if (ok)
    fprintf('Timing check passed successfully\n');
else
    fprintf('Timing check failed! Error listing follows:\n');
    fprintf([error_report{:}]);
    fprintf('\n');
end

%%
%seq.plot('TimeRange',[0 1]);

seq.setDefinition('FOV', [fov fov fov]);
seq.setDefinition('Name', 'petra');
seq.setDefinition('SamplesPerShell', SamplesBookkeeping);

seq.write('zte_petra.seq');
return

%% create an RF-only version of the sequence (e.g. for the SAR or signal evolution testing)

tic;
[total_duration, total_numBlocks]=seq.duration();
for iB=1:total_numBlocks
    b=seq.getBlock(iB);
    bd=seq.blockDurations(iB);
    bs={mr.makeDelay(bd)};
    if ~isempty(b.rf)
        bs{end+1}=b.rf;
    end
    if ~isempty(b.adc)
        bs{end+1}=b.adc;
    end
    seq_sar.addBlock(bs);
end
toc
seq_sar.write('zte_petra_sar.seq');
return

% %% test binary storing
%
% seq.writeBinary('zte_petra.bin');
% seq_bin=mr.Sequence();
% seq_bin.readBinary('zte_petra.bin');
% seq_bin.write('zte_petra_bin.seq');
% return

%% visualize the 3D k-space
tic;
[kfa,~,kf]=seq.calculateKspacePP();
toc

%
figure;plot3(kf(1,:),kf(2,:),kf(3,:));
hold on;plot3(kfa(1,:),kfa(2,:),kfa(3,:),'r.');

%% local functions
function [phi, theta, im]=spherical_samples(Kr,dK,R)
% the number of samples equals the ceil of the area of the sphere divided
% by the area around every sample
Ns=ceil(4*pi*((Kr/dK)^2)/R);
np=0:(Ns-1);
alpha_gold=pi*(3-sqrt(5));
phi=np*alpha_gold;
%theta=0.5*pi*sqrt(np/Ns); % from Davide Piccini  https://doi.org/10.1002/mrm.22898
theta=acos(1-2*np/(Ns-1));  % from  Anton Semechko (2020). Suite of functions to perform uniform sampling of a sphere (https://github.com/AntonSemechko/S2-Sampling-Toolbox), GitHub. Retrieved October 3, 2020.

xp=sin(theta).*cos(phi);
yp=sin(theta).*sin(phi);
zp=cos(theta);
%figure; sphere; colormap gray;
%hold on; plot3(xp,yp,zp,'.');

% looking for the optimal interleaving factor
nm=round(Ns/2); % middle of the trajetory
sr=round(sqrt(Ns));
v0=[xp(nm); yp(nm); zp(nm)];
v=[xp(nm+(1:sr)); yp(nm+(1:sr)); zp(nm+(1:sr))];
%figure;plot(vecnorm((v-v0(:,ones(size(v,2),1)))));
[dKm,im]=min(vecnorm((v-v0(:,ones(size(v,2),1)))));

% fprintf('requested dK=%g achieved minimal dK=%g, min acceleration: %g, Ns=%d\n', dK/sqrt(R), dKm*Kr, (dKm*Kr/dK)^2,Ns);

% v=[xp; yp; zp]*Kr/dK/sqrt(R)*100;
% md=zeros(1,Ns);
% d=zeros(1,Ns);
% for i=1:Ns
%     d=vecnorm(v(:,i*ones(1,Ns))-v);
%     d(i)=NaN;
%     md(i)=min(d);
% end
% fprintf('dKmin=%g%%, dKmed=%g%% dKmax=%g%%\n', min(md),median(md),max(md));

end

function populate_subsequence(sys,seq,rf,adc,phi,theta,im,Ns,Ag,TR,Tt,FN, FO)
Azc=Ag*(TR-Tt)/(TR+Tt);  %*0.35;
%Azc=0; % for MoCo we need "no-gradient" event blocks to be able to apply updates

Gr=mr.makeExtendedTrapezoid('z','times',[0 TR-Tt],'amplitudes',[Ag Ag]); % this is a constant graient with no ramps

% pre-ramp the gradient to Azc
if abs(Azc)>eps
    Tpr=max(2,ceil(Azc/sys.maxSlew/sys.gradRasterTime))*sys.gradRasterTime;
    assert(Tpr<=TR);
    % this "dummy TR" doe not have an RF pulse, which is not good when it is called for the inner shells... but otherwise there would be no enough spoiling...
    seq.addBlock(mr.align('right',mr.makeDelay(TR),mr.makeExtendedTrapezoid('z','system',sys,'times',[0,Tpr],'amplitudes',[0,Azc])));
end

% the loop itself
for j=1:im
    Glast=struct('x',0,'y',0,'z',Azc);
    for i=j:im:Ns
        Gcr=mr.rotate('z',phi(i),mr.rotate('y',theta(i),Gr));
        Gcurr=struct('x',0,'y',0,'z',0);
        for g=1:length(Gcr)
            Gcurr.(Gcr{g}.channel)=Gcr{g}.waveform(1);
        end
        seq.addBlock( ...
            mr.makeExtendedTrapezoid('x','system',sys,'times',[0,Tt],'amplitudes',[Glast.x,Gcurr.x]), ...
            mr.makeExtendedTrapezoid('y','system',sys,'times',[0,Tt],'amplitudes',[Glast.y,Gcurr.y]), ...
            mr.makeExtendedTrapezoid('z','system',sys,'times',[0,Tt],'amplitudes',[Glast.z,Gcurr.z]));
        for f=-FN:FN % 'slices' loop (frequency offsets)
            rf.freqOffset=f*FO;
            rf.phaseOffset=-2*pi*f*FO*rf.t(end)/2;
            seq.addBlock( [{rf, adc}, Gcr] );
        end
        Glast=Gcurr;
    end
    rf_aux=rf;
    rf_aux.delay=rf.delay+Tt;
    if (j==im)
        Azc=0; % on the last interleave we ramp down to 0
    end
    seq.addBlock( rf_aux, ...
        mr.makeExtendedTrapezoid('x','system',sys,'times',[0,TR],'amplitudes',[Glast.x,0]), ...
        mr.makeExtendedTrapezoid('y','system',sys,'times',[0,TR],'amplitudes',[Glast.y,0]), ...
        mr.makeExtendedTrapezoid('z','system',sys,'times',[0,TR],'amplitudes',[Glast.z,Azc]));
end
end


% L2Norm is the dagnn wrapper which normalizes the features to be norm 1 in
% 2-norm at each location

% Copyright (C) 2015 Tsung-Yu Lin, Aruni RoyChowdhury, Subhransu Maji.
% All rights reserved.
%
% This file is part of the BCNN. It uses MATCONVNET package and is made
% available under the terms of the BSD license (see the COPYING file).

classdef L2Norm < dagnn.ElementWise
  properties
    param = 1e-10;
  end

  methods
    function outputs = forward(obj, inputs, params)
      outputs{1} = vl_nnl2norm(inputs{1}, obj.param) ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      derInputs{1} = vl_nnl2norm(inputs{1}, obj.param, derOutputs{1}) ;
      derParams = {} ;
    end

    function rfs = getReceptiveFields(obj)
      rfs.size = [1 1] ;
      rfs.stride = [1 1] ;
      rfs.offset = [1 1] ;
    end

    function obj = SpatialNorm(varargin)
      obj.load(varargin) ;
    end
  end
end


function [grade,X,SR,improve,AppSet] = LocalSearch3(Problem,X,SR,improve,AppSet)
% Local Search 3 of MTS

%------------------------------- 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".
%--------------------------------------------------------------------------

    grade   = 0;
    SearchL = Problem.lower;
    SearchU = Problem.upper;
    Disp    = (SearchU-SearchL)/10;
    Best    = X;
    while any(Disp<1e-2)
        for i = randperm(length(SR))
            value = SearchL(i) : Disp(i) : SearchU(i);
            Decs  = repmat(Best.dec,length(value),1);
            Decs(:,i) = value;
            Y     = Problem.Evaluation(Decs);
            for j = 1 : length(Y)
                % 'grade', 'SR' and 'improve' will not be updated in local
                % search 3
                [~,~,AppSet] = Grading(Y(j),Best,grade,improve,AppSet,Problem.N);
                if all(Y(j).obj<=Best.obj)
                    Best = Y(j);
                end
            end
            SearchL(i) = max(Best.dec(i)-2*Disp(i),Problem.lower);
            SearchU(i) = min(Best.dec(i)+2*Disp(i),Problem.upper);
            Disp(i)    = (SearchU(i)-SearchL(i))/10;
        end
    end
    X = Best;
end


classdef CylindricalLiftModel < AbstractLiftCoefficientModel
    %CylindricalLiftModel Summary of this class goes here
    %   Detailed explanation goes here

    properties
        cylinderLength(1,1) double = 1; %m
        cylinderRadius(1,1) double = 1; %m

        liftCurves(1,1) LiftCoefficientCurves = LiftCoefficientCurves();
    end

    properties(Constant)
        enum = LiftCoefficientModelEnum.KSPCylinder;
    end

    methods
        function obj = CylindricalLiftModel(cylinderLength, cylinderRadius)
            obj.cylinderLength = cylinderLength;
            obj.cylinderRadius = cylinderRadius;

            obj.liftCurves = LiftCoefficientCurves();
        end

        function [ClS, liftUnitVectInertial] = getLiftCoeffAndDir(obj, ut, rVect, vVect, bodyInfo, mass, altitude, pressure, density, vVectECEF, attState)
            arguments
                obj(1,1) CylindricalLiftModel
                ut(1,1) double
                rVect(3,1) double
                vVect(3,1) double
                bodyInfo(1,1) KSPTOT_BodyInfo
                mass(1,1) double
                altitude(1,1) double
                pressure(1,1) double
                density(1,1) double
                vVectECEF(3,1) double
                attState(1,1) LaunchVehicleAttitudeState
            end

            [~,aoa,sideslip,totalAoA] = attState.getAeroAngles(ut, rVect, vVect, bodyInfo);

            vVectECEFMag = norm(vVectECEF);
            pressurePa = pressure*1000;
            speedSound = sqrt(1.4 * pressurePa / density); %m/s
            speedMS = vVectECEFMag*1000;                   %m/s
            thisMachNum = speedMS / speedSound;

            area = obj.getIncidentArea(totalAoA);

            ClS = obj.liftCurves.bodyLiftGiLift(sin(totalAoA))*obj.liftCurves.bodyLiftMachCurve(thisMachNum)*area;

            %get body centered coordinate systems
            bff = bodyInfo.getBodyFixedFrame();
            bci = bodyInfo.getBodyCenteredInertialFrame();

            %get the rotation matrix from body fixed to body centered
            %inertial
            R_ecef_to_global_inertial = bff.getRotMatToInertialAtTime(ut,[],[]);
            R_bci_to_global_inertial = bci.getRotMatToInertialAtTime(ut,[],[]);
            R_ecef_to_bci = R_bci_to_global_inertial' * R_ecef_to_global_inertial;

            %rotate the body X axis in the inertial frame to the body
            %fixed frame
            %TODO: Is attState.bodyX already in the body centered inertial
            %frame?  Or is it in the global inertial frame?
            R_bci_to_ecef = R_ecef_to_bci';
            bodyXInertial = attState.bodyX;
            bodyXECEF = R_bci_to_ecef*bodyXInertial;

            %get the lift vector in the body centered inertial frame
            v1 = crossARH(vVectECEF, bodyXECEF);
            liftUnitVectECEF = normVector(crossARH(v1, bodyXECEF));
            liftUnitVectInertial = R_ecef_to_bci * liftUnitVectECEF;
        end

        function useTf = openEditDialog(obj, lvdData)
            out = AppDesignerGUIOutput({false});
            lvd_EditKSPCylinderLiftPropertiesGUI_App(obj, lvdData, out);
            useTf = out.output{1};
        end
    end

    methods(Access=private)
        function area = getIncidentArea(obj, totalAoA)
            flatPlateArea = obj.cylinderLength*(2*obj.cylinderRadius);
            circleArea = pi*obj.cylinderRadius^2;

            area = cos(totalAoA)*circleArea + sin(totalAoA)*flatPlateArea;
        end
    end
end


function cout = int(cin, delta)

%tstoolbox/@core/int
%   Syntax:
%     * cout = int(cin, delta)
%
%   Input Arguments:
%     * cin - core object
%     * delta - time period between two data samples
%
%   numerical integration along dimension 1 when data was sampled
%   equidistantly with samplerate = 1/delta
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

tmp = cumsum(data(cin),1)*delta;
tmp = [zeros(1, dlens(cin,2)) ; tmp];
cout = core(tmp);


function y=obj1(x)

global counterf
counterf=counterf+1;

y=x^2+2*x;


function range = p12_range ( )

%*****************************************************************************80
%
%% P12_RANGE returns an interval bounding the root for problem 12.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real RANGE(2), the minimum and maximum values of
%    an interval containing the root.
%
  range(1) = - 4.0;
  range(2) =   4.0;

  return
end


function contour_plot = plot_contour(date, pixel_dir, eddy_indexes, backgroundData, eddy_type)
%PLOT_CONTOUR plot contours for current eddies in the track viewer

temp = load([pixel_dir 'lat.mat']);
lat = temp.lat;
temp = load([pixel_dir 'lon.mat']);
lon = temp.lon;

contour_mask = false(length(lat), length(lon));

if strcmp(eddy_type, 'ant')
    pixel_file = [pixel_dir 'anticyclonic/pixels_' num2str(date)];
else
    pixel_file = [pixel_dir 'cyclonic/pixels_' num2str(date)];
end
temp = load(pixel_file);
pixel_list = temp.data;

for i = 1:length(eddy_indexes)
    contour_mask(pixel_list{eddy_indexes(i)}) = true;
end

contour_mask = bwperim(contour_mask);

contour = nan(length(lat), length(lon));

if strcmp(eddy_type, 'ant')
    if ~isempty(backgroundData)
        contour(contour_mask) = max(backgroundData(:)) + 0.2 * range(backgroundData(:));
    else
        contour(contour_mask) = 1;
    end
elseif strcmp(eddy_type, 'cyc')
    if ~isempty(backgroundData)
        contour(contour_mask) = min(backgroundData(:)) - 0.2 * range(backgroundData(:));
    else
        contour(contour_mask) = 0;
    end
else
    error('Not recognized eddy type');
end

contour_plot = pcolorm(lat, lon, contour);

end


function [I_b_deep,I_d_deep, show_matrix, count_all, I_vector] = vector_deep_latent(img, de_level, L, unit, is_overlap, stride, w_num)

[t1,t2] = size(img);
show_matrix = [];
% I_d = zeros(t1,t2,de_level);
I_b_deep = zeros(t1,t2);
for i=1:de_level
    if i==1
        [temp_b, temp_d, temp_d_v, count_m, I_vector] = vector_decomposition(img, L, unit, is_overlap, stride, w_num);
    else
        [temp_b, temp_d, temp_d_v, count_m, I_vector] = vector_decomposition(temp_b, L, unit, is_overlap, stride, w_num);
    end
    I_d_deep(:,:,i) = temp_d_v(:,:);
    show_matrix = cat(2,show_matrix, temp_d);

end
% show_matrix = cat(2,show_matrix, temp_b);
% figure;imshow(show_matrix);
% imwrite(temp_b,['./fused_vector/features/base_level_',num2str(i),'.png'],'png');

I_b_deep(:,:) = temp_b(:,:);
count_all = count_m;
% I_d_deep = I_d;

end


function Problem = ITERrun_optimizer(Problem,Constraints,Objectives,Spec)

weightings = Objectives.weightings;
if ~any(weightings)
    beep;
    disp('*********** No objective(s) to minimize. ***********')
    return
end

Prob = pre_process(Problem);

GenObjFn = @(X,objtype)...
    EVALobjective(X,Problem,Objectives,Spec,Constraints,objtype);

Prob.nonlcon = @(X) EVALnonlcons(X,Problem,Constraints,Spec);

choice = txtmenu('Choose optimizer to run',...
    {' Return / up menu\n\n\t\tRecommended global search:'
    'Multi-objective genetic algorithm'
    'Genetic algorithm\n\n\t\tRecommended local search:'
    'Nelder-Mead simplex search\n\n\t\tOther search methods:'
    'Steepest descent (fmincon)'
    'Pattern search'
    'Particle swarm'});

if any(choice==[1:2 4:5])
    GUIflag = get_yes_or_no('Leave and open optimization tool GUI? Y/[N]: ',false);
end

if (nnz(weightings) == 1) && (choice == 1) % only one objective for multi
    beep; disp('Only one objective. Using single-objective GA');
    choice = 2;
end

switch choice
    case 0
        return
    case 5 % pattern search
        methodstr = 'Pat. search';
        Prob.objective = @(X) GenObjFn(X,'Single');
        Prob.options = psoptimset(Prob.options,...
            'PlotFcns',{@psplotbestf @psplotmeshsize @psplotbestx});
        Prob.solver = 'patternsearch';
        if GUIflag
            optimtool(Prob);
            disp('Hit F5 to continue when done with GUI');
            keyboard;
        else
            [sol,fval] = patternsearch(Prob);
            Problem = single_solution_postprocess(Problem,sol,fval);
        end

    case 6 % particle swarm
        disp('Not supported')
        return

    case 1 %gamulti
        methodstr = 'MO GA';
        Prob.solver = 'gamultiobj';
        Prob.fitnessfcn = @(X) GenObjFn(X,'MultiPen');
        Prob.options.PlotFcns = {@gaplotstopping @gaplotpareto};

        if GUIflag
            optimtool(Prob);
            disp('Hit F5 to continue when done with GUI')
            keyboard;
        else

            [sol,fval,~,~,population,scores] = ...
                gamultiobj(Prob.fitnessfcn,Prob.nvars,...
                [],[],[],[],Prob.lb,Prob.ub,...
                Prob.options);
            save
            Problem = SELECTind_from_pop(Problem,Constraints,Objectives...
                ,Spec,sol,fval,population,scores);
        end

    case 2 %ga alone
        methodstr = 'GA';
        %set up problem
        Prob.solver = 'ga';
        Prob.fitnessfcn = @(X) GenObjFn(X,'SinglePen');
        Prob.options.PlotFcns = {@gaplotbestf @gaplotbestindiv...
            @gaplotscores @gaplotstopping};

        if GUIflag
            optimtool(Prob);
            disp('Hit F5 to continue when done with GUI')
            keyboard;
        else
            [sol,fval] = ga...
                (Prob.fitnessfcn,Prob.nvars,[],[],[],[],...
                Prob.lb,Prob.ub,[],Prob.options);
            Problem = single_solution_postprocess(Problem,sol,fval);
        end

    case 4 %fmincon
        methodstr = 'fmincon';
        Prob.options = optimset(Prob.options,...
            'TolFun', 1e-5,'TolX', 1e-5,'TolCon',1e-7,...
            'PlotFcns', {@optimplotx @optimplotfval ...
            @optimplotconstrviolation @optimplotstepsize});
        Prob.solver = 'fmincon';
        Prob.objective = @(X) GenObjFn(X,'Single');

        if GUIflag
            optimtool(Prob);
            disp('Hit F5 to continue when done with GUI')
            keyboard;
        else
            [sol,fval] = fmincon(...
                Prob.objective,Prob.x0,...
                [],[],[],[],Prob.lb,Prob.ub,...
                Prob.nonlcon,Prob.options);
            Problem = single_solution_postprocess(Problem,sol,fval);
        end

    case 3 %nelder mead
        methodstr = 'NM';
        Prob.options = optimset(Prob.options,...
            'Tolcon', 1e-6,'TolX', 1e-3,...
            'PlotFcns', {@optimplotx @optimplotfval});
        Prob.objective = @(X) GenObjFn(X,'Single');
        if get_yes_or_no('X0 feasible? [Y]/N: ',true)
            strictness = 'superstrict';
        else strictness = [];
        end

        [sol, fval, ~, optimizer_output] = ...
            optimize(Prob.objective, Prob.x0,...
            Prob.lb, Prob.ub, [],[],[],[],...
            Prob.nonlcon,strictness, Prob.options);

        jnkinfeasible = any(optimizer_output.constrviolation.nonl_ineq{1});
        if jnkinfeasible
            disp('Solution not feasible')
            disp('Constraint violoation:')
            disp(optimizer_output.constrviolation.nonl_ineq{2})
        else
            disp('Solution feasible')
        end

        Problem = single_solution_postprocess(Problem,sol,fval);

end

save(['Autosaves\OptimizerRun' num2str(fix(clock),'%02d') '.mat'])

if get_yes_or_no('Save result to History.xls? [Y]/N: ',true)
    ITERsave_hist(Problem,Constraints,Objectives,Spec,methodstr);
end

end

function Problem = pre_process(Problem)
activeX = Problem.activeX;

Problem.x0 = Problem.x0(activeX);
Problem.nvars = nnz(activeX);
Problem.lb = Problem.lb(activeX);
Problem.ub = Problem.ub(activeX);
Problem.XLabels=Problem.XLabels(activeX);

Problem.Bineq = Problem.bineq;
Problem.Beq = Problem.beq;
Problem.LB = Problem.lb;
Problem.UB = Problem.ub;

%set standard options
Problem.options = gaoptimset('PopulationSize',50,'Generations',50,...
    'TimeLimit',3600*1.5,'StallGenLimit',25,'StallTimeLimit',3600*.5,...
    'TolFun',1e-3,'TolCon',1e-4,...
    'Display','iter',...
    'InitialPopulation',Problem.x0');
Problem.options.TolX = 1e-3;
end

function Problem = single_solution_postprocess(Problem,sol,fval)
% let em know that it's done
beep;pause(.5);beep;pause(.5);beep;pause(.5);beep;pause(.5);beep;pause(.5);

% populate full X vector
solX = Problem.x0;
solX(Problem.activeX) = sol;

fprintf('New objective value: %f\n',fval)
if get_yes_or_no('Replace X0 with optimizer solution? [Y]/N: ',true)
    Problem.x0 = solX;
end
end


% FMRIB1
%
% Files
%   decimate2           - Clone of decimate.m by the mathworks
%   eegplugin_fmrib     - eegplugin_fmrib() -       EEGLAB plugin from the Centre for
%   fastranc            - fastranc.m -  adaptive noise cancellation for fmrib_fastr.m
%   fmrib_fastr         - fmrib_fastr() -  Remove FMRI gradient artifacts from EEG data using
%   fmrib_pas           - fmrib_pas() - Remove pulse
%   fmrib_qrsdetect     - fmrib_qrsdetect() - Detect QRS peaks from ECG channel using combined
%   guifastr            - Application M-file for guifastr.fig
%   guipas              - Application M-file for guifastr.fig
%   pca_calc            - Principal Component Analysis
%   pop_fmrib_fastr     - pop_fmrib_fastr() -  GUI for fmrib_fastr: Remove gradient
%   pop_fmrib_pas       - pop_fmirb_pas() -  GUI for fmrib_pas: Remove hear-related pulse artifacts
%   pop_fmrib_qrsdetect - pop_fmrib_qrsdetect() -  GUI for fmrib_qrsdetect:
%   prcorr2             - - Compute correlation coefficient over all dimentions
%   qrscorrect          - qrscorrect() -  Correct for false positive and negative QRS peaks
%   trigcorrect         - trigcorrect() -   correct for missing FMRI slice triggers


%% DEMO_febio_0007_sphere_sliding
% Below is a demonstration for:
%
% * Building geometry for a slab with hexahedral elements, and a
% triangulated sphere.
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement results

%% Keywords
%
% * febio_spec version 4.0
% * febio, FEBio
% * indentation
% * contact, sliding, sticky, friction
% * rigid body constraints
% * hexahedral elements, hex8
% * triangular elements, tri3
% * slab, block, rectangular
% * sphere
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

%%

clear; close all; clc;

%% Plot settings
fontSize=15;
faceAlpha1=0.8;
faceAlpha2=0.3;
markerSize=40;
markerSize2=20;
lineWidth=3;

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=[febioFebFileNamePart,'.txt']; %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_force=[febioFebFileNamePart,'_force_out.txt']; %Log file name for exporting force
febioLogFileName_stress=[febioFebFileNamePart,'_stress_out.txt']; %Log file name for exporting stress

%Specifying dimensions and number of elements for slab
sampleHeight=5; %Height
sampleWidth=sampleHeight*4; %Width
sampleThickness=sampleHeight*1; %Thickness
pointSpacings=[1 1 1]; %Desired point spacing between nodes
numElementsWidth=round(sampleWidth/pointSpacings(1)); %Number of elemens in dir 1
numElementsThickness=round(sampleThickness/pointSpacings(2)); %Number of elemens in dir 2
numElementsHeight=round(sampleHeight/pointSpacings(3)); %Number of elemens in dir 3

%Sphere parameters
numRefineStepsSphere=3;
sphereRadius=sampleHeight/2;
sphereMeshType='tri3'; % tri3 or quad4

%Define applied displacement
sphereIndentationDisplacement=sphereRadius;
sphereSlideDisplacement=sampleWidth-(sphereRadius*2);

%Material parameter set
c1=1e-3; %Shear-modulus-like parameter
m1=2; %Material parameter setting degree of non-linearity
k_factor=100; %Bulk modulus factor
k=c1*k_factor; %Bulk modulus

% FEA control settings
numTimeSteps1=10; %Number of time steps desired
dtmin1=(1/numTimeSteps1)/100; %Minimum time step size
dtmax1=1/numTimeSteps1; %Maximum time step size

numTimeSteps2=10; %Number of time steps desired
dtmin2=(1/numTimeSteps2)/100; %Minimum time step size
dtmax2=1/numTimeSteps2; %Maximum time step size

max_refs=75; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=20; %Optimum number of iterations
max_retries=5; %Maximum number of retires
symmetric_stiffness=0;

runMode='external';% 'internal' or 'external'

%Contact parameters
contactInitialOffset=0.1;
contactPenalty=20;
laugon=0;
minaug=1;
maxaug=10;
fric_coeff=0.3;

%% Creating model geometry and mesh
% A box is created with tri-linear hexahedral (hex8) elements using the
% |hexMeshBox| function. The function offers the boundary faces with
% seperate labels for the top, bottom, left, right, front, and back sides.
% As such these can be used to define boundary conditions on the exterior.

% Create a box with hexahedral elements
beamDimensions=[sampleWidth sampleThickness sampleHeight]; %Dimensions
beamElementNumbers=[numElementsWidth numElementsThickness numElementsHeight]; %Number of elements
outputStructType=2; %A structure compatible with mesh view
[meshStruct]=hexMeshBox(beamDimensions,beamElementNumbers,outputStructType);

%Access elements, nodes, and faces from the structure
E1=meshStruct.elements; %The elements
V1=meshStruct.nodes; %The nodes (vertices)
Fb1=meshStruct.facesBoundary; %The boundary faces
Cb1=meshStruct.boundaryMarker; %The "colors" or labels for the boundary faces
elementMaterialIndices=ones(size(E1,1),1); %Element material indices

%% Creating triangulated sphere surface model

switch sphereMeshType
    case 'tri3'
        [E2,V2,~]=geoSphere(numRefineStepsSphere,sphereRadius);
    case 'quad4'
        [E2,V2]=quadSphere(numRefineStepsSphere,sphereRadius);
end

%Offset indentor
minV2=min(V2,[],1);
minV1=min(V1,[],1);

V2(:,1)=V2(:,1)-minV2(1)+minV1(1);

V2(:,3)=V2(:,3)-minV2(3)+(sampleHeight/2)+contactInitialOffset;

center_of_mass=mean(V2,1);

%%
% Plotting model boundary surfaces and a cut view

hFig=cFigure;

subplot(1,2,1); hold on;
title('Model boundary surfaces and labels','FontSize',fontSize);
gpatch(Fb1,V1,Cb1,'k',faceAlpha1);
gpatch(E2,V2,'kw','k',faceAlpha1);
colormap(gjet(6)); icolorbar;
axisGeom(gca,fontSize);

hs=subplot(1,2,2); hold on;
title('Cut view of solid mesh','FontSize',fontSize);
optionStruct.hFig=[hFig hs];
gpatch(E2,V2,'kw','k',1);
meshView(meshStruct,optionStruct);
axisGeom(gca,fontSize);

drawnow;

%% Joining node sets
V=[V1;V2;]; %Combined node sets
E2=E2+size(V1,1); %Fixed element indices

%%
% Plotting joined geometry
cFigure;
title('Joined node sets','FontSize',fontSize);
xlabel('X','FontSize',fontSize); ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;
gpatch(Fb1,V,Cb1,'k',faceAlpha1);
gpatch(E2,V,'kw','k',faceAlpha1);
colormap(gjet(6)); icolorbar;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Define contact surfaces

% The rigid master surface of the sphere
F_contact_secondary=E2;

% The deformable slave surface of the slab
logicContactSurf1=Cb1==6;
F_contact_primary=Fb1(logicContactSurf1,:);

% Plotting surface models
cFigure; hold on;
title('Contact sets and normal directions','FontSize',fontSize);

gpatch(Fb1,V,'kw','none',faceAlpha2);
hl(1)=gpatch(F_contact_secondary,V,'g','k',1);
patchNormPlot(F_contact_secondary,V);
hl(2)=gpatch(F_contact_primary,V,'b','k',1);
patchNormPlot(F_contact_primary,V);

legend(hl,{'Secondary','Primary'});

axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Define boundary conditions

%Supported nodes
Fr=Fb1(Cb1==5,:);
bcSupportList=unique(Fr(:));

%%
% Visualize BC's
hf=cFigure;
title('Boundary conditions model','FontSize',fontSize);
xlabel('X','FontSize',fontSize); ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;

gpatch(Fb1,V,'w','none',faceAlpha2);

hl2(1)=gpatch(E2,V,'gw','k',1);
hl2(2)=plotV(V(bcSupportList,:),'k.','MarkerSize',markerSize);

legend(hl2,{'Rigid body sphere','BC support'});

axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='4.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Control section
stepStruct1.Control.analysis='STATIC';
stepStruct1.Control.time_steps=numTimeSteps1;
stepStruct1.Control.step_size=1/numTimeSteps1;
stepStruct1.Control.solver.max_refs=max_refs;
stepStruct1.Control.solver.qn_method.max_ups=max_ups;
stepStruct1.Control.solver.symmetric_stiffness=symmetric_stiffness;
stepStruct1.Control.time_stepper.dtmin=dtmin1;
stepStruct1.Control.time_stepper.dtmax=dtmax1;
stepStruct1.Control.time_stepper.max_retries=max_retries;
stepStruct1.Control.time_stepper.opt_iter=opt_iter;

%Add template based default settings to proposed control section
[stepStruct1.Control]=structComplete(stepStruct1.Control,febio_spec.Control,1); %Complement provided with default if missing

stepStruct2.Control.analysis='STATIC';
stepStruct2.Control.time_steps=numTimeSteps2;
stepStruct2.Control.step_size=1/numTimeSteps2;
stepStruct2.Control.solver.max_refs=max_refs;
stepStruct2.Control.solver.qn_method.max_ups=max_ups;
stepStruct2.Control.solver.symmetric_stiffness=symmetric_stiffness;
stepStruct2.Control.time_stepper.dtmin=dtmin2;
stepStruct2.Control.time_stepper.dtmax=dtmax2;
stepStruct2.Control.time_stepper.max_retries=max_retries;
stepStruct2.Control.time_stepper.opt_iter=opt_iter;

%Add template based default settings to proposed control section
[stepStruct2.Control]=structComplete(stepStruct2.Control,febio_spec.Control,1); %Complement provided with default if missing

%Remove control field (part of template) since step specific control sections are used
febio_spec=rmfield(febio_spec,'Control');

febio_spec.Step.step{1}.Control=stepStruct1.Control;
febio_spec.Step.step{1}.ATTR.id=1;
febio_spec.Step.step{2}.Control=stepStruct2.Control;
febio_spec.Step.step{2}.ATTR.id=2;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='Ogden';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.c1=c1;
febio_spec.Material.material{1}.m1=m1;
febio_spec.Material.material{1}.c2=c1;
febio_spec.Material.material{1}.m2=-m1;
febio_spec.Material.material{1}.k=k;

materialName2='Material2';
febio_spec.Material.material{2}.ATTR.name=materialName2;
febio_spec.Material.material{2}.ATTR.type='rigid body';
febio_spec.Material.material{2}.ATTR.id=2;
febio_spec.Material.material{2}.density=1;
febio_spec.Material.material{2}.center_of_mass=center_of_mass;

%Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='nodeSet_all'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type='hex8'; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E1,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E1; %The element matrix

partName2='Part2';
febio_spec.Mesh.Elements{2}.ATTR.name=partName2; %Name of this part
febio_spec.Mesh.Elements{2}.ATTR.type=sphereMeshType; %Element type
febio_spec.Mesh.Elements{2}.elem.ATTR.id=size(E1,1)+(1:1:size(E2,1))'; %Element id's
febio_spec.Mesh.Elements{2}.elem.VAL=E2; %The element matrix

% -> NodeSets
nodeSetName1='bcSupportList';
febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.VAL=mrow(bcSupportList);

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

febio_spec.MeshDomains.ShellDomain.ATTR.name=partName2;
febio_spec.MeshDomains.ShellDomain.ATTR.mat=materialName2;

% -> Surfaces
surfaceName1='contactSurface1';
febio_spec.Mesh.Surface{1}.ATTR.name=surfaceName1;
febio_spec.Mesh.Surface{1}.(sphereMeshType).ATTR.id=(1:1:size(F_contact_secondary,1))';
febio_spec.Mesh.Surface{1}.(sphereMeshType).VAL=F_contact_secondary;

surfaceName2='contactSurface2';
febio_spec.Mesh.Surface{2}.ATTR.name=surfaceName2;
febio_spec.Mesh.Surface{2}.quad4.ATTR.id=(1:1:size(F_contact_primary,1))';
febio_spec.Mesh.Surface{2}.quad4.VAL=F_contact_primary;

% -> Surface pairs
febio_spec.Mesh.SurfacePair{1}.ATTR.name='Contact1';
febio_spec.Mesh.SurfacePair{1}.primary=surfaceName2;
febio_spec.Mesh.SurfacePair{1}.secondary=surfaceName1;

%Boundary condition section
% -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.name='zero_displacement_x';
febio_spec.Boundary.bc{1}.ATTR.type='zero displacement';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.x_dof=1;
febio_spec.Boundary.bc{1}.y_dof=1;
febio_spec.Boundary.bc{1}.z_dof=1;

%Rigid section
% -> Prescribed rigid body boundary conditions
febio_spec.Step.step{1}.Rigid.rigid_bc{1}.ATTR.name='RigidFix_1';
febio_spec.Step.step{1}.Rigid.rigid_bc{1}.ATTR.type='rigid_fixed';
febio_spec.Step.step{1}.Rigid.rigid_bc{1}.rb=2;
febio_spec.Step.step{1}.Rigid.rigid_bc{1}.Rx_dof=1;
febio_spec.Step.step{1}.Rigid.rigid_bc{1}.Ry_dof=1;
febio_spec.Step.step{1}.Rigid.rigid_bc{1}.Ru_dof=1;
febio_spec.Step.step{1}.Rigid.rigid_bc{1}.Rv_dof=1;
febio_spec.Step.step{1}.Rigid.rigid_bc{1}.Rw_dof=1;

febio_spec.Step.step{1}.Rigid.rigid_bc{2}.ATTR.name='RigidPrescribe';
febio_spec.Step.step{1}.Rigid.rigid_bc{2}.ATTR.type='rigid_displacement';
febio_spec.Step.step{1}.Rigid.rigid_bc{2}.rb=2;
febio_spec.Step.step{1}.Rigid.rigid_bc{2}.dof='z';
febio_spec.Step.step{1}.Rigid.rigid_bc{2}.value.ATTR.lc=1;
febio_spec.Step.step{1}.Rigid.rigid_bc{2}.value.VAL=-(sphereIndentationDisplacement+contactInitialOffset);

febio_spec.Step.step{2}.Rigid.rigid_bc{1}.ATTR.name='RigidFix_1';
febio_spec.Step.step{2}.Rigid.rigid_bc{1}.ATTR.type='rigid_fixed';
febio_spec.Step.step{2}.Rigid.rigid_bc{1}.Ry_dof=1;
febio_spec.Step.step{2}.Rigid.rigid_bc{1}.Rz_dof=1;
febio_spec.Step.step{2}.Rigid.rigid_bc{1}.Ru_dof=1;
febio_spec.Step.step{2}.Rigid.rigid_bc{1}.Rv_dof=1;
febio_spec.Step.step{2}.Rigid.rigid_bc{1}.Rw_dof=1;

febio_spec.Step.step{2}.Rigid.rigid_bc{2}.ATTR.name='RigidPrescribe';
febio_spec.Step.step{2}.Rigid.rigid_bc{2}.ATTR.type='rigid_displacement';
febio_spec.Step.step{2}.Rigid.rigid_bc{2}.rb=2;
febio_spec.Step.step{2}.Rigid.rigid_bc{2}.dof='x';
febio_spec.Step.step{2}.Rigid.rigid_bc{2}.value.ATTR.lc=2;
febio_spec.Step.step{2}.Rigid.rigid_bc{2}.value.VAL=sphereSlideDisplacement;
febio_spec.Step.step{2}.Rigid.rigid_bc{2}.relative=1;

%Contact section
febio_spec.Contact.contact{1}.ATTR.type='sliding-elastic';
febio_spec.Contact.contact{1}.ATTR.surface_pair=febio_spec.Mesh.SurfacePair{1}.ATTR.name;
febio_spec.Contact.contact{1}.two_pass=0;
febio_spec.Contact.contact{1}.laugon=laugon;
febio_spec.Contact.contact{1}.tolerance=0.2;
febio_spec.Contact.contact{1}.gaptol=0;
febio_spec.Contact.contact{1}.minaug=minaug;
febio_spec.Contact.contact{1}.maxaug=maxaug;
febio_spec.Contact.contact{1}.search_tol=0.01;
febio_spec.Contact.contact{1}.search_radius=0.1*sqrt(sum((max(V,[],1)-min(V,[],1)).^2,2));
febio_spec.Contact.contact{1}.symmetric_stiffness=0;
febio_spec.Contact.contact{1}.auto_penalty=1;
febio_spec.Contact.contact{1}.update_penalty=1;
febio_spec.Contact.contact{1}.penalty=contactPenalty;
febio_spec.Contact.contact{1}.fric_coeff=fric_coeff;

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.name='LC_1';
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
%febio_spec.LoadData.load_controller{1}.extend='CONSTANT';
febio_spec.LoadData.load_controller{1}.points.pt.VAL=[0 0; 1 1; 2 1];

febio_spec.LoadData.load_controller{2}.ATTR.name='LC_2';
febio_spec.LoadData.load_controller{2}.ATTR.id=2;
febio_spec.LoadData.load_controller{2}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{2}.interpolate='LINEAR';
%febio_spec.LoadData.load_controller{2}.extend='CONSTANT';
febio_spec.LoadData.load_controller{2}.points.pt.VAL=[0 0; 1 0; 2 1];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{1}.VAL=1:size(V,1);

febio_spec.Output.logfile.node_data{2}.ATTR.file=febioLogFileName_force;
febio_spec.Output.logfile.node_data{2}.ATTR.data='Rx;Ry;Rz';
febio_spec.Output.logfile.node_data{2}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{2}.VAL=1:size(V,1);

febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_stress;
febio_spec.Output.logfile.element_data{1}.ATTR.data='s3';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.element_data{1}.VAL=1:size(E1,1);

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.runMode=runMode;

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    %%
    % Importing nodal displacements from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp),1,1);

    %Access data
    N_disp_mat=dataStruct.data; %Displacement
    timeVec=dataStruct.time; %Time

    %Create deformed coordinate set
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);

    %%
    % Importing element stress from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_stress),1,1);

    %Access data
    E_stress_mat=dataStruct.data;
    E_stress_mat(isnan(E_stress_mat))=0;

        %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    [CV]=faceToVertexMeasure(E1,V,E_stress_mat(:,:,end));

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    title('$\sigma_{3}$ [MPa]','Interpreter','Latex')
    hp=gpatch(Fb1,V_DEF(:,:,end),CV,'k',1); %Add graphics object to animate
    hp.Marker='.';
    hp.MarkerSize=markerSize2;
    hp.FaceColor='interp';

    hp2=gpatch(E2,V_DEF(:,:,end),'w','none',0.5); %Add graphics object to animate

    axisGeom(gca,fontSize);
    colormap(flipud(gjet(250))); colorbar;
    caxis([min(E_stress_mat(:)) max(E_stress_mat(:))]);
    axis(axisLim(V_DEF)); %Set axis limits statically
    camlight headlight;

    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments

        [CV]=faceToVertexMeasure(E1,V,E_stress_mat(:,:,qt));

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp hp2]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData','Vertices'}; %Properties of objects to animate
        animStruct.Set{qt}={V_DEF(:,:,qt),CV,V_DEF(:,:,qt)}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.


%REFINEDETECTEDMARKERS  Refind not detected markers based on the already detected and the board layout
%
%     [detectedCorners, detectedIds, rejectedCorners] = cv.refineDetectedMarkers(img, board, detectedCorners, detectedIds, rejectedCorners)
%     [detectedCorners, detectedIds, rejectedCorners, recoveredIdxs] = cv.refineDetectedMarkers(...)
%     [...] = cv.refineDetectedMarkers(..., 'OptionName',optionValue, ...)
%
% ## Input
% * __img__ input image (8-bit grayscale or color).
% * __board__ layout of markers in the board.
% * __detectedCorners__ cell array of already detected marker corners
%   `{{[x,y],..}, ..}`.
% * __detectedIds__ vector of already detected marker identifiers (0-based).
% * __rejectedCorners__ cell array of rejected candidates during the marker
%   detection process `{{[x,y],..}, ..}`.
%
% ## Output
% * __detectedCorners__ output refined marker corners.
% * __detectedIds__ output refined marker identifiers
% * __rejectedCorners__ output refined rejected corners.
% * __recoveredIdxs__ Optional array that returns the indexes of the recovered
%   candidates in the original `rejectedCorners` array.
%
% ## Options
% * __CameraMatrix__ optional input 3x3 floating-point camera matrix
%   `A = [fx 0 cx; 0 fy cy; 0 0 1]`.
% * __DistCoeffs__ optional vector of distortion coefficients
%   `[k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4]` of 4, 5, 8 or 12 elements.
% * __MinRepDistance__ minimum distance between the corners of the rejected
%   candidate and the reprojected marker in order to consider it as a
%   correspondence. default 10.0
% * __ErrorCorrectionRate__ rate of allowed erroneous bits respect to the
%   error correction capability of the used dictionary. -1 ignores the error
%   correction step. default 3.0
% * __CheckAllOrders__ Consider the four posible corner orders in the
%   `rejectedCorners` array. If it set to false, only the provided corner
%   order is considered (default true).
% * __DetectorParameters__ marker detection parameters.
%
% This function tries to find markers that were not detected in the basic
% cv.detectMarkers function. First, based on the current detected marker and
% the board layout, the function interpolates the position of the missing
% markers. Then it tries to find correspondence between the reprojected
% markers and the rejected candidates based on the `minRepDistance` and
% `errorCorrectionRate` parameters. If camera parameters and distortion
% coefficients are provided, missing markers are reprojected using
% cv.projectPoints function. If not, missing marker projections are
% interpolated using global homography, and all the marker corners in the
% board must have the same Z coordinate.
%
% See also: cv.detectMarkers, cv.estimatePoseBoard, cv.findHomography,
%  cv.perspectiveTransform
%


%BILATERALTEXTUREFILTER  Applies the bilateral texture filter to an image
%
%     dst = cv.bilateralTextureFilter(src)
%     dst = cv.bilateralTextureFilter(src, 'OptionName', optionValue, ...)
%
% ## Input
% * __src__ Source image whose depth is 8-bit `uint8` or 32-bit `single`
%
% ## Output
% * __dst__ Destination image of the same size and type as `src`.
%
% ## Options
% * __FR__ Radius of kernel to be used for filtering. It should be positive
%   integer. default 3
% * __NumIter__ Number of iterations of algorithm, It should be positive
%   integer. default 1
% * __SigmaAlpha__ Controls the sharpness of the weight transition from edges
%   to smooth/texture regions, where a bigger value means sharper transition.
%   When the value is negative, it is automatically calculated. default -1
% * __SigmaAvg__ Range blur parameter for texture blurring. Larger value makes
%   result to be more blurred. When the value is negative, it is automatically
%   calculated as described in the paper. default -1
%
% It performs structure-preserving texture filter. For more details about this
% filter see [Cho2014].
%
% ## References
% [Cho2014]:
% > Hojin Cho, Hyunjoon Lee, Henry Kang, and Seungyong Lee.
% > "Bilateral texture filtering". ACM Transactions on Graphics,
% > 33(4):128:1-128:8, July 2014.
%
% See also: cv.rollingGuidanceFilter, cv.bilateralFilter
%


function r = subsref(a,s)
%SUBSREF      Implements subscripted references for Taylor
%

% written  05/21/09     S.M. Rump
% modified 07/07/10     S.M. Rump  treatment of "end"-index
% modified 08/26/12     S.M. Rump  global variables removed
%

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

  INTLAB_TAYLOR_ORDER = getappdata(0,'INTLAB_TAYLOR_ORDER');

  while 1
    if ~isa(a,'taylor')                  % index reference a.x(i) etc.
      r = subsref(a,s(1));
    elseif strcmp(s(1).type,'()')        % index reference a(i)
      setappdata(0,'INTLAB_TAYLOR_END',0);  % reset INTLAB variable
      index = reshape(1:prod(a.size),a.size);
      index = index(s(1).subs{:});
      r.size = size(index);
      r.t = a.t(:,index(:));
      r = class(r,'taylor');
    elseif strcmp(s(1).type,'{}')        % Taylor coefficient reference a{i}
      INTLAB_TAYLOR_END = getappdata(0,'INTLAB_TAYLOR_END');
      if INTLAB_TAYLOR_END
        setappdata(0,'INTLAB_TAYLOR_END',0); % reset INTLAB variable
        error('"end" cannot be used in an index expression when accessing derivatives by {}')
      end
      setappdata(0,'INTLAB_TAYLOR_END',0);   % reset INTLAB variable
      index = ( 1:INTLAB_TAYLOR_ORDER+1 );
      index = index(s(1).subs{:}+1);
      r = a.t(index,:).';
    elseif strcmp(s(1).type,'.')         % index reference a.t
      if strcmp(s(1).subs,'t')
        r = a.t.';
      elseif strcmp(s(1).subs,'mid')
        r = mid(a);
      else
        error('invalid subscript reference for taylor')
      end
    else
      error('invalid index reference for taylor')
    end
    if length(s)==1
      if rndold
        setround(rndold)
      end
      return
    end
    s = s(2:end);
    a = r;
  end


function []=plot3DDICPPresults(varargin)
%% function for plotting 3D-DIC results in STEP3.
% Plotting the 3D reconstruction of points correlated with Ncorr
% The function opens a selection window for all the possible measures to plot
% After selection, the animation figures are plotted
%
% INPUT options:
% DIC3DPPresults

%%
switch nargin
    case 0 % in case no results were entered
        % ask user to load results from 1 or more camera pairs and turn
        % into a cell array
        [file,path] = uigetfile(pwd,'Select a DIC3DPPresults structure');
        result=load([path file]);
        DIC3DPPresults=result.DIC3DPPresults;
        optStruct=struct;
    case 1
        % use given struct
        DIC3DPPresults=varargin{1};
        optStruct=struct;
    case 2
        % use given struct
        DIC3DPPresults=varargin{1};
        optStruct=varargin{2};
    otherwise
        error('wrong number of input arguments');
end

%% select what to plot
Prompt={'\bf{Select which parameters to plot}';... % 1
    'Surfaces with color as pair index';... % 2
    'Points with color as pair index';... % 3
    'Surfaces with color as combined correlation coefficient';... % 4
    'Points with color as correlation coefficient';... % 5
    'Surfaces with color as dispMgn (Displacement magnitude)';... % 6
    'Points with color as dispMgn (Displacement magnitude)';... % 7
    'Surfaces with color as dispX (X Displacement)';... % 8
    'Points with color as dispX (X Displacement)';... % 9
    'Surfaces with color as dispY (Y Displacement)';... % 10
    'Points with color as dispY (Y Displacement)';... % 11
    'Surfaces with color as dispZ (Z Displacement)';... % 12
    'Points with color as dispZ (Z Displacement)';... % 13
    'Surfaces with color as J (surface area change)';... % 14
    'Surfaces with color as lambda1 (1st principal stretch)';... % 15
    'Surfaces with color as Lambda2 (2nd principal stretch)';... % 16
    'Surfaces with color as Epc1 (1st principal Lagrangian strain)';... % 17
    'Surfaces with color as Epc2 (2nd principal Lagrangian strain)';... % 18
    'Surfaces with color as epc1 (1st principal Almansi strain)';... % 19
    'Surfaces with color as epc2 (2nd principal Almansi strain)';... % 20
    'Surfaces with color as Emgn (Lagrangian strain tensor magnitude)';... % 21
    'Surfaces with color as emgn (Almansi strain tensor magnitude)';... % 22
    'Surfaces with color as Lagrangian equivalent strain';... % 23
    'Surfaces with color as Eulerian equivalent strain';... % 24
    'Surfaces with color as Max Lagrangian shear strain';... % 25
    'Surfaces with color as Max Eulerian shear strain';... % 26
    'Surfaces with color as Lamda1+direction (1st principal stretch value and direction)';... % 27
    'Surfaces with color as Lamda2+direction (2nd principal stretch value and direction)';... % 28
    'Surfaces with color as Epc1+direction (1st principal Lagrangian strain value and direction)';... % 29
    'Surfaces with color as Epc2+direction (2nd principal Lagrangian strain value and direction)';... % 30
    'Surfaces with color as epc1+direction (1st principal Almansi strain value and direction)';... % 31
    'Surfaces with color as epc2+direction (2nd principal Almansi strain value and direction)';... % 32
    'Surfaces with color as Epc1+Epc2+direction (1st and 2nd principal Lagrangian strain values and directions)';... % 33
    'Surfaces with color as epc1+epc2+direction (1st and 2nd principal Almansi strain values and directions)'; % 34
    'Surfaces with color as Max Lagrangian shear strain + directions';... % 35
    'Surfaces with color as Max Eulerian shear strain + directions';... % 36
    'Surfaces with color as Lamda1+Lamda2+direction (1st and 2nd principal stretch values and directions)';... % 37
    'Surfaces with color as FaceIsoInd (triangular face isotropy index)';... % 38
    'Select to remove rigid body motion';... % 39
    'Surfaces with color as FaceColors (grayscale from images)';... % 40
    'Select All'; }; % 41

Title='Select which parameters to plot';

Formats=struct;
Formats(1,1).type='text'; %1
Formats(1,2).type='none';
Formats(2,1).type='check'; %2
Formats(2,2).type='check'; %3
Formats(3,1).type='check'; %4
Formats(3,2).type='check'; %5
Formats(4,1).type='check'; %6
Formats(4,2).type='check'; %7
Formats(5,1).type='check'; %8
Formats(5,2).type='check'; %9
Formats(6,1).type='check'; %10
Formats(6,2).type='check'; %11
Formats(7,1).type='check'; %12
Formats(7,2).type='check'; %13
Formats(8,1).type='check'; %14
Formats(8,2).type='none';
Formats(9,1).type='check'; %15
Formats(9,2).type='check'; %16
Formats(10,1).type='check'; %17
Formats(10,2).type='check'; %18
Formats(11,1).type='check'; %19
Formats(11,2).type='check'; %20
Formats(12,1).type='check'; %21
Formats(12,2).type='check'; %22
Formats(13,1).type='check'; %23
Formats(13,2).type='check'; %24
Formats(14,1).type='check'; %25
Formats(14,2).type='check'; %26
Formats(15,1).type='check'; %27
Formats(15,2).type='check'; %28
Formats(16,1).type='check'; %29
Formats(16,2).type='check'; %30
Formats(17,1).type='check'; %31
Formats(17,2).type='check'; %32
Formats(18,1).type='check'; %33
Formats(18,2).type='check'; %34
Formats(19,1).type='check'; %35
Formats(19,2).type='check'; %36
Formats(20,1).type='check'; %37
Formats(20,2).type='none';
Formats(21,1).type='check'; %38
Formats(21,2).type='check'; %39
Formats(22,1).type='check'; %40
Formats(22,2).type='check'; %41

DefAns=cell(numel(Prompt),1);
DefAns{1}=[];
for ii=2:numel(Prompt)
    DefAns{ii}=false;
end

Options.Resize='on';
Options.FontSize=10;

[Answer,Canceled] = inputsdlg(Prompt, Title, Formats, DefAns, Options);
if Answer{41}
    for ii=[2:38 40]
        Answer{ii}=true;
    end
end
if Answer{39}
    RBMlogic=true;
else
    RBMlogic=false;
end
if Canceled
    return
end
%% Select plotting options
if ~Canceled && sum(cell2mat(Answer(2:end)))>0
    answer = inputdlg({'Enter maximum correlation coefficient to keep points (leave blank for keeping all points)'},'Input',[1,50]);
    CorCoeffCutOff=str2double(answer{1}); % maximal correlation coefficient for display (use [] for default which is max)
    if isnan(CorCoeffCutOff)
        CorCoeffCutOff=[];
    end
else
    return
end

%% create option struct for plotting
% complete the struct fields
if ~isfield(optStruct,'zDirection')
    optStruct.zDirection=1;
end
optStruct.maxCorrCoeff=CorCoeffCutOff;

% optStruct.lineColor='k';
% optStruct.smoothLogic=0;
% optStruct.colorMap=cMap;
% optStruct.supTitleString=pointMeasureString;

%% plot according to answer

if Canceled
    return
end

if Answer{2}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'FacePairInds',RBMlogic,optStruct);
end
if Answer{3}
    anim8_DIC3DPP_pointMeasure(DIC3DPPresults,'pairInd',RBMlogic,optStruct);
end
if Answer{4}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'FaceCorrComb',RBMlogic,optStruct);
end
if Answer{5}
    anim8_DIC3DPP_pointMeasure(DIC3DPPresults,'corrComb',RBMlogic,optStruct);
end
if Answer{6}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'DispMgn',RBMlogic,optStruct);
end
if Answer{7}
    anim8_DIC3DPP_pointMeasure(DIC3DPPresults,'DispMgn',RBMlogic,optStruct);
end
if Answer{8}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'DispX',RBMlogic,optStruct);
end
if Answer{9}
    anim8_DIC3DPP_pointMeasure(DIC3DPPresults,'DispX',RBMlogic,optStruct);
end
if Answer{10}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'DispY',RBMlogic,optStruct);
end
if Answer{11}
    anim8_DIC3DPP_pointMeasure(DIC3DPPresults,'DispY',RBMlogic,optStruct);
end
if Answer{12}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'DispZ',RBMlogic,optStruct);
end
if Answer{13}
    anim8_DIC3DPP_pointMeasure(DIC3DPPresults,'DispZ',RBMlogic,optStruct);
end
if Answer{14}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'J',RBMlogic,optStruct);
end
if Answer{15}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'Lamda1',RBMlogic,optStruct);
end
if Answer{16}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'Lamda2',RBMlogic,optStruct);
end
if Answer{17}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'Epc1',RBMlogic,optStruct);
end
if Answer{18}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'Epc2',RBMlogic,optStruct);
end
if Answer{19}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'epc1',RBMlogic,optStruct);
end
if Answer{20}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'epc2',RBMlogic,optStruct);
end
if Answer{21}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'Emgn',RBMlogic,optStruct);
end
if Answer{22}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'emgn',RBMlogic,optStruct);
end
if Answer{23}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'Eeq',RBMlogic,optStruct);
end
if Answer{24}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'eeq',RBMlogic,optStruct);
end
if Answer{25}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'EShearMax',RBMlogic,optStruct);
end
if Answer{26}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'eShearMax',RBMlogic,optStruct);
end
if Answer{27}
    anim8_DIC3DPP_faceMeasureDirection(DIC3DPPresults,'Lamda1',RBMlogic,optStruct);
end
if Answer{28}
    anim8_DIC3DPP_faceMeasureDirection(DIC3DPPresults,'Lamda2',RBMlogic,optStruct);
end
if Answer{29}
    anim8_DIC3DPP_faceMeasureDirection(DIC3DPPresults,'Epc1',RBMlogic,optStruct);
end
if Answer{30}
    anim8_DIC3DPP_faceMeasureDirection(DIC3DPPresults,'Epc2',RBMlogic,optStruct);
end
if Answer{31}
    anim8_DIC3DPP_faceMeasureDirection(DIC3DPPresults,'epc1',RBMlogic,optStruct);
end
if Answer{32}
    anim8_DIC3DPP_faceMeasureDirection(DIC3DPPresults,'epc2',RBMlogic,optStruct);
end
if Answer{33}
    anim8_DIC3DPP_faceMeasureDirection(DIC3DPPresults,{'Epc1','Epc2'},RBMlogic,optStruct);
end
if Answer{34}
    anim8_DIC3DPP_faceMeasureDirection(DIC3DPPresults,{'epc1','epc2'},RBMlogic,optStruct);
end
if Answer{35}
    % develop shear direction plot
    msgbox('Shear directions cannot be plotted. It is under construction...');
end
if Answer{36}
    % develop shear direction plot
    msgbox('Shear directions cannot be plotted. It is under construction...');
end
if Answer{37}
    anim8_DIC3DPP_faceMeasureDirection(DIC3DPPresults,{'Lamda1','Lamda2'},RBMlogic,optStruct);
end
if Answer{38}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'FaceIsoInd',RBMlogic,optStruct);
end
if Answer{40}
    anim8_DIC3DPP_faceMeasure(DIC3DPPresults,'FaceColors',RBMlogic,optStruct);
end

end

%%
% MultiDIC: a MATLAB Toolbox for Multi-View 3D Digital Image Correlation
%
% License: <https://github.com/MultiDIC/MultiDIC/blob/master/LICENSE.txt>
%
% Copyright (C) 2018  Dana Solav
%
% If you use the toolbox/function for your research, please cite our paper:
% <https://engrxiv.org/fv47e>


function [dims, isndarray, maxlevel, count] = nestbracket2dim(str,brackets,testndarray)
%
% [dims, isndarray, maxlevel, count] = nestbracket2dim(str,brackets)
%
% Extracting the dimension vector of a JSON string formatted array
% by analyzing the pairs of opening/closing bracket tokenss; this function
% only returns valid dimension information when the array is an N-D array
%
% authors:Qianqian Fang (q.fang <at> neu.edu)
%
% input:
%      str: a string-formatted JSON array using square-brackets for enclosing
%           elements and comma as separators between elements
%      brackets: (optional), a string of length 2, with the first character
%               being the opening token and the 2nd being the closing token.
%               if not given, brackets is set to '[]' to find matching square-brackets;
%               for example, '{}' looks for a matching closing curly-bracket in
%               the string key(pos(startpos,:end))
%      testndarray: (optional), 1 to test if the input string contains an
%               ND array, i.e. with uniform element lengths (recursively)
%
% output:
%      dims: the speculated dimension vector with the length matching the maximum
%            depth of the embedded bracket pairs. When the input string encodes an
%            N-D array, the dims vector contains all integers; however, returning
%            an all-integer dims vector does not mean the array is
%            rectangular. if testndarray is set to 1, dims returns isndarray
%      isndarray: 1 to indicate the input string contains an ND array,
%            otherwise, 0
%      maxlevel: return the depth of the enclosed brackets in the string, i.e. the
%            length of the dims vector.
%      count: the relative depth from the level 0 - scanning from the left
%            to right of the string, an opening token increases the level by 1
%            and a closing token decreases the level by 1; a zero indicates
%            the positions of a matching bracket of the same level.
%
% example:
%      str='[[ [1,2,3], [4,2,1]], [ [10,1,0], [2,5,10]] ]'; % an N-D array
%      [dim,dep]=nestbracket2dim(str)
%      str='[[ [1,2,3], [4,2,1]], [ [10,1,0], [2,5]] ]'; % an invalid N-D array
%      [dim,dep]=nestbracket2dim(str)

% license:
%     BSD or GPL version 3, see LICENSE_{BSD,GPLv3}.txt files for details
%
% -- this function is part of JSONLab toolbox (http://iso2mesh.sf.net/cgi-bin/index.cgi?jsonlab)
%

if(nargin<2)
    brackets='[]';
end
str=str(str==brackets(1) | str==brackets(2) | str==',');
count=cumsum(str==brackets(1)) - cumsum(str==brackets(2));
isndarray=testuniform(count, max(count));
if(nargin>2 && testndarray)
    dims=isndarray;
    return;
end
maxlevel=max(count);
dims=histc(count,1:maxlevel);
dims(1:end-1)=dims(1:end-1)*0.5;
dims(2:end)=dims(2:end)./dims(1:end-1);
dims=fliplr(dims);

function isndarray=testuniform(count, maxval)
isndarray=false;
if(length(count)>2)
    idx=find(count(2:end)==count(1),1);
    if(idx==length(count)-2 && count(1)<maxval)
        isndarray=testuniform(count(2:end-1), maxval);
        return;
    end
    if(~isempty(idx) && mod(length(count)-1,idx+1)==0)
        count2d=reshape(count(1:end-1),idx+1,[]);
        if(all(diff(count2d')==0))
            isndarray=true;
        end
    end
end


function bpa = tapas_bayesian_parameter_average(varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% This function calculates the Bayesian parameter average for the individual estimates handed  to
% it.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% USAGE:
%     est1 = tapas_fitModel(responses1, inputs);
%     est2 = tapas_fitModel(responses2, inputs);
%     ...
%     estn = tapas_fitModel(responsesn, inputs);
%
%     bpa = tapas_bayesian_parameter_average(est1, est2,..., estn);
%
% INPUT ARGUMENTS:
%     varargin           Estimate structures generated by tapas_fitModel(...). Note that all estimates
%                        must have been made under the same priors.
%
% OUTPUT:
%     bpa.u              Input to agent (i.e., the inputs array from the arguments)
%     bpa.c_prc          Configuration settings for your chosen perceptual model
%                        (see the configuration file of that model for details)
%     bpa.c_obs          Configuration settings for your chosen observation model
%                        (see the configuration file of that model for details)
%     bpa.optim          A place for the optimization algorithm to dump infos of interest to it
%     bpa.p_prc          Bayesian average of estimates of perceptual parameters
%                        (see the configuration file of your chosen perceptual model for details)
%     bpa.p_obs          Bayesian average of estimates of observation parameters
%                        (see the configuration file of your chosen observation model for details)
%     bpa.traj:          Trajectories of the environmental states tracked by the perceptual model
%                        (see the configuration file of that model for details)
%
%
% PLOTTING OF RESULTS:
%     To plot the trajectories of the inferred perceptual states (as implied by the averaged
%     parameters), there is a function <modelname>_plotTraj(...) for each perceptual model. This
%     takes the structure returned by bpa(...) as its only argument.
%
%     Additionally, the function tapas_fit_plotCorr(...) plots the posterior correlation of the
%     averaged parameters. It takes the structure returned by bpa(...) as its only
%     argument. Note that this function only works if the optimization algorithm makes the
%     posterior correlation available in est.optim.Corr for all of the estimate structures handed
%     to bpa(...).
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2020 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/>.

% Number of estimates to average
n = size(varargin,2);

% Inputs
u = varargin{1}.u;

% Determine the models involved
prc_model = varargin{1}.c_prc.model;
obs_model = varargin{1}.c_obs.model;

% Get priors
prc_priormus = varargin{1}.c_prc.priormus;
prc_priorsas = varargin{1}.c_prc.priorsas;
obs_priormus = varargin{1}.c_obs.priormus;
obs_priorsas = varargin{1}.c_obs.priorsas;

% Check whether everything matches up
for i = 2:n
    if ~strcmp(prc_model,varargin{i}.c_prc.model)
        error('tapas:hgf:bpa:PrcModNoMatch', 'Perceptual models do not match.');
    end

    if ~strcmp(obs_model,varargin{i}.c_obs.model)
        error('tapas:hgf:bpa:ObsModNoMatch', 'Observation models do not match.');
    end

    if ~isequalwithequalnans(prc_priormus,varargin{i}.c_prc.priormus) || ~isequalwithequalnans(prc_priorsas,varargin{i}.c_prc.priorsas)
        error('tapas:hgf:bpa:PrcPriorsNoMatch', 'Perceptual priors do not match.');
    end

    if ~isequalwithequalnans(obs_priormus,varargin{i}.c_obs.priormus) || ~isequalwithequalnans(obs_priorsas,varargin{i}.c_obs.priorsas)
        error('tapas:hgf:bpa:ObsPriorsNoMatch', 'Observation priors do not match.');
    end

    if ~isequalwithequalnans(u(:),varargin{i}.u(:))
        disp(['Warning: inputs for argument number ' num2str(i) ' do not match those for first argument.']);
    end
end

% Record configuration
bpa       = struct;
bpa.u     = u;
bpa.ign   = [];
bpa.c_prc = varargin{1}.c_prc;
bpa.c_obs = varargin{1}.c_obs;

% Determine indices of parameters that have been optimized (i.e., those that are not fixed or NaN)
opt_idx = [bpa.c_prc.priorsas, bpa.c_obs.priorsas];
opt_idx(isnan(opt_idx)) = 0;
opt_idx = find(opt_idx);

% Prior precision
priorsas = [prc_priorsas, obs_priorsas];
H0 = diag(1./priorsas(opt_idx));

% Posterior precision and covariance
H = (1-n).*H0;

for i=1:n
    H = H + varargin{i}.optim.H;
end

Sigma = inv(H);
Sigma = tapas_nearest_psd(Sigma);
Corr = tapas_Cov2Corr(Sigma);

% Record results
bpa.optim.H     = H;
bpa.optim.Sigma = Sigma;
bpa.optim.Corr  = Corr;

% Prior mean
priormus = [prc_priormus, obs_priormus]';
mu0 = priormus(opt_idx);

% Posterior mean
mu = (1-n).*H0*mu0;

for i=1:n
    mui = [varargin{i}.p_prc.ptrans, varargin{i}.p_obs.ptrans]';
    mui = mui(opt_idx);
    mu = mu + varargin{i}.optim.H*mui;
end

mu = Sigma*mu;

% Replace optimized values in priormus with averaged values
ptrans = priormus';
ptrans(opt_idx) = mu';

% Separate perceptual and observation parameters
n_prcpars = length(bpa.c_prc.priormus);
ptrans_prc = ptrans(1:n_prcpars);
ptrans_obs = ptrans(n_prcpars+1:end);

% Transform MAP parameters back to their native space
[dummy, bpa.p_prc]   = bpa.c_prc.transp_prc_fun(bpa, ptrans_prc);
[dummy, bpa.p_obs]   = bpa.c_obs.transp_obs_fun(bpa, ptrans_obs);
bpa.p_prc.p      = bpa.c_prc.transp_prc_fun(bpa, ptrans_prc);
bpa.p_obs.p      = bpa.c_obs.transp_obs_fun(bpa, ptrans_obs);

% Store transformed MAP parameters
bpa.p_prc.ptrans = ptrans_prc;
bpa.p_obs.ptrans = ptrans_obs;

% Store representations at MAP estimate
bpa.traj = bpa.c_prc.prc_fun(bpa, bpa.p_prc.p);

% Print results
disp(' ')
disp('Results:');
disp(bpa.p_prc)
disp(bpa.p_obs)

end


function[R]=radearth
%RADEARTH  The radius of the earth in kilometers.
%
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2006 J.M. Lilly --- type 'help jlab_license' for details

R=6371;