function [results, success, raw] = dcopf_solver(om, mpopt)
%DCOPF_SOLVER  Solves a DC optimal power flow.
%
%   [RESULTS, SUCCESS, RAW] = DCOPF_SOLVER(OM, MPOPT)
%
%   Inputs are an OPF model object and a MATPOWER options struct.
%
%   Outputs are a RESULTS struct, SUCCESS flag and RAW output struct.
%
%   RESULTS is a MATPOWER case struct (mpc) with the usual baseMVA, bus
%   branch, gen, gencost fields, along with the following additional
%   fields:
%       .order      see 'help ext2int' for details of this field
%       .x          final value of optimization variables (internal order)
%       .f          final objective function value
%       .mu         shadow prices on ...
%           .var
%               .l  lower bounds on variables
%               .u  upper bounds on variables
%           .lin
%               .l  lower bounds on linear constraints
%               .u  upper bounds on linear constraints
%
%   SUCCESS     1 if solver converged successfully, 0 otherwise
%
%   RAW         raw output in form returned by MINOS
%       .xr     final value of optimization variables
%       .pimul  constraint multipliers
%       .info   solver specific termination code
%       .output solver specific output information
%
%   See also OPF, OPT_MODEL/SOLVE.

%   MATPOWER
%   Copyright (c) 2000-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%   and Carlos E. Murillo-Sanchez, PSERC Cornell & Universidad Nacional de Colombia
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%%----- initialization -----
%% define named indices into data matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;
[PW_LINEAR, POLYNOMIAL, MODEL, STARTUP, SHUTDOWN, NCOST, COST] = idx_cost;

%% unpack data
mpc = om.get_mpc();
[baseMVA, bus, gen, branch, gencost] = ...
    deal(mpc.baseMVA, mpc.bus, mpc.gen, mpc.branch, mpc.gencost);
cp = om.params_legacy_cost();
Bf = om.get_userdata('Bf');
Pfinj = om.get_userdata('Pfinj');
[vv, ll] = om.get_idx();

%% problem dimensions
nb = size(bus, 1);          %% number of buses
nl = size(branch, 1);       %% number of branches
ny = om.getN('var', 'y');   %% number of piece-wise linear costs

%% options
model = om.problem_type();
opt = mpopt2qpopt(mpopt, model);
if strcmp(opt.alg, 'OSQP')
    opt.x0 = [];    %% disable provided starting point for OSQP
end

%% try to select an interior initial point, unless requested not to
if mpopt.opf.start < 2 && ...
        (strcmp(opt.alg, 'MIPS') || strcmp(opt.alg, 'IPOPT'))
    [x0, xmin, xmax] = om.params_var();     %% init var & bounds
    s = 1;                      %% set init point inside bounds by s
    lb = xmin; ub = xmax;
    lb(xmin == -Inf) = -1e10;   %% replace Inf with numerical proxies
    ub(xmax ==  Inf) =  1e10;
    x0 = (lb + ub) / 2;         %% set x0 mid-way between bounds
    k = find(xmin == -Inf & xmax < Inf);    %% if only bounded above
    x0(k) = xmax(k) - s;                    %% set just below upper bound
    k = find(xmin > -Inf & xmax == Inf);    %% if only bounded below
    x0(k) = xmin(k) + s;                    %% set just above lower bound
    Varefs = bus(bus(:, BUS_TYPE) == REF, VA) * (pi/180);
    x0(vv.i1.Va:vv.iN.Va) = Varefs(1);  %% angles set to first reference angle
    if ny > 0
        ipwl = find(gencost(:, MODEL) == PW_LINEAR);
        c = gencost(sub2ind(size(gencost), ipwl, NCOST+2*gencost(ipwl, NCOST)));    %% largest y-value in CCV data
        x0(vv.i1.y:vv.iN.y) = max(c) + 0.1 * abs(max(c));
    end
    opt.x0 = x0;
end

%%-----  run opf  -----
[x, f, eflag, output, lambda] = om.solve(opt);
success = (eflag == 1);

%%-----  calculate return values  -----
if ~any(isnan(x))
    %% update solution data
    Va = x(vv.i1.Va:vv.iN.Va);
    Pg = x(vv.i1.Pg:vv.iN.Pg);

    %% update voltages & generator outputs
    bus(:, VM) = ones(nb, 1);
    bus(:, VA) = Va * 180/pi;
    gen(:, PG) = Pg * baseMVA;

    %% compute branch flows
    branch(:, [QF, QT]) = zeros(nl, 2);
    branch(:, PF) = (Bf * Va + Pfinj) * baseMVA;
    branch(:, PT) = -branch(:, PF);
end

%% package up results
mu_l = lambda.mu_l;
mu_u = lambda.mu_u;
muLB = lambda.lower;
muUB = lambda.upper;

%% update Lagrange multipliers
il = find(branch(:, RATE_A) ~= 0 & branch(:, RATE_A) < 1e10);
bus(:, [LAM_P, LAM_Q, MU_VMIN, MU_VMAX]) = zeros(nb, 4);
gen(:, [MU_PMIN, MU_PMAX, MU_QMIN, MU_QMAX]) = zeros(size(gen, 1), 4);
branch(:, [MU_SF, MU_ST]) = zeros(nl, 2);
bus(:, LAM_P)       = (mu_u(ll.i1.Pmis:ll.iN.Pmis) - mu_l(ll.i1.Pmis:ll.iN.Pmis)) / baseMVA;
if ~isempty(il)
    branch(il, MU_SF)   = mu_u(ll.i1.Pf:ll.iN.Pf) / baseMVA;
    branch(il, MU_ST)   = mu_l(ll.i1.Pf:ll.iN.Pf) / baseMVA;
end
gen(:, MU_PMIN)     = muLB(vv.i1.Pg:vv.iN.Pg) / baseMVA;
gen(:, MU_PMAX)     = muUB(vv.i1.Pg:vv.iN.Pg) / baseMVA;
pimul = [
  mu_l - mu_u;
 -ones(ny>0, 1);    %% dummy entry corresponding to linear cost row in A (in MINOS)
  muLB - muUB
];

mu = struct( ...
  'var', struct('l', muLB, 'u', muUB), ...
  'lin', struct('l', mu_l, 'u', mu_u) );

results = mpc;
[results.bus, results.branch, results.gen, ...
    results.om, results.x, results.mu, results.f] = ...
        deal(bus, branch, gen, om, x, mu, f);

raw = struct('xr', x, 'pimul', pimul, 'info', eflag, 'output', output);


function cS = chromite

cs_Chr = crystalSymmetry('m3m');
N = Miller({1,1,0},cs_Chr);
dist = [1];
cS = crystalShape(N./dist);


function MRIVertices = ctf_head2mri(HeadVertices,mri)

% ctf_head2mri - convert a CTF head into a voxel coordinate
%
% MRIVertices = ctf_mri2head(HeadVertices,mri)
%
% This function converts CTF head coordinates into the CTF MRI voxel
% coordinates.  The input 'HeadVertices' are head space locations; they are
% Nx3 coordinates in the CTF head space (see below). The 'mri' input
% is a struct returned by ctf_read_mri, which contains the fiducials
% and a transformation matrix between head and MRI voxel coordinates.
%
% CTF MRI volumes are 256^3 voxels, 1mm^3 each. The volume index has
% an origin at the left, anterior, superior voxel, such that:
%
% Sag increases from left to right (+X Right)
% Cor increases from anterior to posterior (+Y Posterior)
% Axi increases from superior to inferior (+Z Inferior)
%
% The head space coordinates are defined in relation to the MRI fiducial
% locations (nasion, left preauricular and right preauricular).  The origin
% lies half way between the left and right preauricular points and the
% coordinate axes are given as:
%
% +X is through the nasion,
% +Y is left
% +Z is superior
%
% CTF head coordinate axes are othogonalized by first taking the cross
% product of the +X and +Y vectors (from the origin through the nasion and
% left preauricular, respectively) to find the +Z vector.  Then, the Y axis
% is orthogonalized to the X/Z plane using the cross product and right hand
% rule.  Hence, +Y can be slightly offset from the left preauricular.
%

% $Revision: 1.1 $ $Date: 2009-01-30 03:49:27 $

% Copyright (C) 2004  Darren L. Weber
%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

% History:  04/2004, Darren.Weber_at_radiology.ucsf.edu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fprintf('in development!'); return

ver = '$Revision: 1.1 $';
fprintf('CTF_HEAD2MRI [v %s]\n',ver(11:15)); tic;

%--------------------------------------------------------------------
% Extract the coordinate transform matrices from the mri struct.  These are
% designed to be left multiplied into the vertices (I don't like this
% because it means the column arrays need to be transposed below)
%T.ctfHEAD2MRI = mri.hdr.transformMatrixHead2MRI;
%T.ctfMRI2HEAD = mri.hdr.transformMatrixMRI2Head;

% OK, so I got fed up with trying to figure out how to use these
% bloody matrices and decided to calculate them myself, so
% that the entries would work in a way that I can understand!
[trans,rot] = calc_tranfer_matrix(mri);

%--------------------------------------------------------------------
fprintf('...converting from cm to mm\n');
MRIVertices = HeadVertices * 10;

%--------------------------------------------------------------------
% Pad out the HeadVertices to Nx4 matrix

Nvertices = size(MRIVertices,1);

right_column = ones( Nvertices, 1 );

MRIVertices = [ MRIVertices right_column ];

%--------------------------------------------------------------------
% Convert CTF head space coordinates into voxel indices

MRIVertices = (-1 * MRIVertices * trans) * rot;

MRIVertices = MRIVertices(:,1:3);

t = toc; fprintf('...done (%6.2f sec).\n\n',t);

return

%-------------------------------------------------------
function [trans,rot] = calc_tranfer_matrix(mri)

% these are the translations!
%ctf.mri.hdr.headOrigin_sagittal: 130
%ctf.mri.hdr.headOrigin_coronal: 123
%ctf.mri.hdr.headOrigin_axial: 152

% This is how they are calculated from the fiducials:

nas(1) = mri.hdr.HeadModel_Info.Nasion_Sag;
nas(2) = mri.hdr.HeadModel_Info.Nasion_Cor;
nas(3) = mri.hdr.HeadModel_Info.Nasion_Axi;
lpa(1) = mri.hdr.HeadModel_Info.LeftEar_Sag;
lpa(2) = mri.hdr.HeadModel_Info.LeftEar_Cor;
lpa(3) = mri.hdr.HeadModel_Info.LeftEar_Axi;
rpa(1) = mri.hdr.HeadModel_Info.RightEar_Sag;
rpa(2) = mri.hdr.HeadModel_Info.RightEar_Cor;
rpa(3) = mri.hdr.HeadModel_Info.RightEar_Axi;

headOffset_Sag = ( rpa(1) - lpa(1) ) / 2;
headOrigin_Sag = lpa(1) + headOffset_Sag;

headOffset_Cor = ( rpa(2) - lpa(2) ) / 2;
headOrigin_Cor = lpa(2) + headOffset_Cor;

headOffset_Axi = ( rpa(3) - lpa(3) ) / 2;
headOrigin_Axi = lpa(3) + headOffset_Axi;

headOrigin = [headOrigin_Sag headOrigin_Cor headOrigin_Axi];

%-------------from here to...

% calculate voxel space vectors, in slices
voxNASvector = nas - headOrigin;
voxLPAvector = lpa - headOrigin;
voxRPAvector = rpa - headOrigin;

% calculate head space vectors, in mm
headNASvector = -1 * [ voxNASvector(2), voxNASvector(1), voxNASvector(3) ];
headLPAvector = -1 * [ voxLPAvector(2), voxLPAvector(1), voxLPAvector(3) ];
headRPAvector = -1 * [ voxRPAvector(2), voxRPAvector(1), voxRPAvector(3) ];

%-------------here; is encapsulated in trans:

trans = eye(3);
trans(1,1) = 0;
trans(2,1) = 1;
trans(1,2) = 1;
trans(2,2) = 0;
trans(4,1:3) = -1 * [ headOrigin(2) headOrigin(1) headOrigin(3) ];

%headVector = -1 * ctfVox * trans;

% At this point, the voxel indices are now in the head space, in
% mm; however, the head space at this point is not orthogonal

%--------------now the head space is orthogonalized

headNASunit = headNASvector / norm(headNASvector);
headLPAunit = headLPAvector / norm(headLPAvector);
headRPAunit = headRPAvector / norm(headRPAvector);

headVERTEXunit = cross( headNASunit, headLPAunit );

% revise the LPA unit vector, so it is orthogonal to Nasion
headLPAunit = cross( headVERTEXunit, headNASunit );

% CHECK, these dot products = 0
%dot( headNASunit, headLPAunit )
%dot( headNASunit, headVERTEXunit )
%dot( headLPAunit, headVERTEXunit )

% Note that the LPA/RPA moves!  This has the effect of
% rotating the coordinates in the XY plane.

rot = [ headNASunit; headLPAunit; headVERTEXunit ];

return


function [seq, init_image] = get_sequence_info(seq)

if ~isfield(seq, 'format') || isempty(seq.format)
    if isempty(seq)
        seq.format = 'vot';
    else
        seq.format = 'otb';
    end
end

seq.frame = 0;

if strcmpi(seq.format, 'otb')
    seq.image_files = seq.s_frames;
    seq = rmfield(seq, 's_frames');
    seq.init_sz = [seq.init_rect(1,4), seq.init_rect(1,3)];
    seq.init_pos = [seq.init_rect(1,2), seq.init_rect(1,1)] + (seq.init_sz - 1)/2;
    seq.num_frames = numel(seq.image_files);
    seq.rect_position = zeros(seq.num_frames, 4);
    init_image = imread(seq.image_files{1});
elseif strcmpi(seq.format, 'vot')
    [seq.handle, init_image_file, init_region] = vot('polygon');

    if isempty(init_image_file)
        init_image = [];
        return;
    end

    init_image = imread(init_image_file);

    bb_scale = 1;

    % If the provided region is a polygon ...
    if numel(init_region) > 4
        % Init with an axis aligned bounding box with correct area and center
        % coordinate
        cx = mean(init_region(1:2:end));
        cy = mean(init_region(2:2:end));
        x1 = min(init_region(1:2:end));
        x2 = max(init_region(1:2:end));
        y1 = min(init_region(2:2:end));
        y2 = max(init_region(2:2:end));
        A1 = norm(init_region(1:2) - init_region(3:4)) * norm(init_region(3:4) - init_region(5:6));
        A2 = (x2 - x1) * (y2 - y1);
        s = sqrt(A1/A2);
        w = s * (x2 - x1) + 1;
        h = s * (y2 - y1) + 1;
    else
        cx = init_region(1) + (init_region(3) - 1)/2;
        cy = init_region(2) + (init_region(4) - 1)/2;
        w = init_region(3);
        h = init_region(4);
    end

    init_c = [cy cx];
    init_sz = bb_scale * [h w];

    im_size = size(init_image);

    seq.init_pos = init_c;
    seq.init_sz = min(max(round(init_sz), [1 1]), im_size(1:2));
    seq.num_frames = Inf;
    seq.region = init_region;
else
    error('Uknown sequence format');
end


function [P]=tesSmooth(TES,V,IND_V,cPar)

% function [P]=tesSmooth(TES,V,IND_V,cPar)
% ------------------------------------------------------------------------
%
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 2014/06/02
%------------------------------------------------------------------------

%%

%Get/set method
if isfield(cPar,'Method')
    smoothMethod=cPar.Method;
else
    smoothMethod='LAP'; %DEFAULT
end

%Smooth
switch smoothMethod
    case 'LAP'
        [P]=tesSmooth_LAP(TES,V,IND_V,cPar);
    case 'HC'
        [P]=tesSmooth_HC(TES,V,IND_V,cPar);
    otherwise
        error('Invalid smooth method specified');
end

%%
% _*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/>.


function init_axes2d(axes_handle, grid2d, withinterp, withpml, isswapped)
chkarg(~isempty(axes_handle) && ishandle(axes_handle), '"axes_handle" should be handle.');
chkarg(istypesizeof(grid2d, 'Grid2d'), '"grid2d" should be instance of Grid2d.');
chkarg(istypesizeof(withinterp, 'logical'), '"withinterp" should be logical.');
chkarg(istypesizeof(withpml, 'logical'), '"withpml" should be logical.');
chkarg(istypesizeof(isswapped, 'logical'), '"isswapped" should be logical.');

hold(axes_handle, 'on');

if withinterp
	lplot = grid2d.lplot(GT.prim, withinterp, withpml);
else
	lplot = grid2d.lvoxelbound(GT.prim, withpml);
end

if ~isswapped
	h = Dir.h; v = Dir.v;
else
	h = Dir.v; v = Dir.h;
end

axis(axes_handle, [lplot{h}([1 end]), lplot{v}([1 end])]);
daspect(axes_handle, [1 1 1]);  % axis(axes_handle, 'image');
box(axes_handle, 'on');

hr = rotate3d;
setAllowAxesRotate(hr, axes_handle, false);

% set(axes_handle, 'FontSize', 18, 'FontWeight', 'Bold');
str = [char(grid2d.axis(h)), '  (', char(hex2dec('00D7'))];
if grid2d.unitvalue < 1e5 && grid2d.unitvalue > 1e-3
	str = [str, num2str(grid2d.unitvalue), ')'];
else
	str = [str, num2str(grid2d.unitvalue, '%.2e'), ')'];
end

xlabel(axes_handle, str, 'Rotation', 0, 'FontSize', 15);
% s = [char(grid2d.axis(v)), '  (', char(hex2dec('00D7')), num2str(grid2d.unitvalue, '%e'), ')'];
str = char(grid2d.axis(v));
ylabel(axes_handle, str, 'Rotation', 0, 'FontSize', 15);

set(axes_handle, 'TickDir', 'out');

% L0 = scalar3d.gi.L0;
% dlu = scalar3d.gi.display_length_unit;
% dlu_scale = scalar3d.gi.dlu_scale;
% xlabel(strcat(AxisName(Xx), ' (', char(hex2dec('00D7')), ...  % 00D7 is a unicode character for the multiplication sign.
% 	num2str(L0*dlu_scale), dlu, ')'));
% ylabel(strcat(AxisName(Yy), ' (', char(hex2dec('00D7')), ...  % 00D7 is a unicode character for the multiplication sign.
% 	num2str(L0*dlu_scale), dlu, ')'));
% zlabel(strcat(AxisName(Zz), ' (', char(hex2dec('00D7')), ...  % 00D7 is a unicode character for the multiplication sign.
% 	num2str(L0*dlu_scale), dlu, ')'));
% title(plot_title);


function tests = test_haar
  tests = functiontests(localfunctions);
end

function test_qmf(testCase)
    f = spx.wavelet.haar.quad_mirror_filter();
    verifyTrue(testCase, spx.norm.is_unit_norm_vec(f));
end


%% DEMO_febio_0029_contact_friction_benchmark_boxes
% Below is a demonstration for a FEBio demo presented at the FEBio workshop at
% WCB 2018. Four cubes are stacked on top of each other. Sliding-elastic
% contact with friction is defined between them. The top cube is forced
% downwards in a rotation motion thereby compressing the other cubes and
% also transferring some twist due to frictional forces.
%
% The demo includes
% * Building geometry for 4 cubes with hexahedral elements
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement results

%% Keywords
%
% * febio_spec version 3.0
% * febio, FEBio
% * indentation
% * contact, sliding, sliding-elastic, friction
% * rigid body constraints
% * hexahedral elements, hex8
% * quadrilateral elements, quad4
% * static, solid
% * 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=fullfile(savePath,[febioFebFileNamePart,'.txt']); %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_strainEnergy=[febioFebFileNamePart,'_fsed_out.txt']; %Log file name for exporting strain energy density

%Specifying dimensions and number of elements for slab
sampleHeight=2; %Height
sampleWidth=sampleHeight; %Width
sampleThickness=sampleHeight; %Thickness

numElementsWidth=[4 3 5 4]; %Number of elemens in dir 1
numElementsThickness=numElementsWidth; %Number of elemens in dir 2
numElementsHeight=numElementsWidth; %Number of elemens in dir 3

%Material parameter set
youngsModuli   = [0.3 10  0.3 10 ];
poissonsRatios = [0.4 0.1 0.4 0.1];

% FEA control settings
numTimeSteps=10; %Number of time steps desired
max_refs=25; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=25; %Optimum number of iterations
max_retries=10; %Maximum number of retires
dtmin=(1/numTimeSteps)/10; %Minimum time step size
dtmax=1/numTimeSteps; %Maximum time step size

%Contact parameters
contactInitialOffset=0;
boxOffsets=sampleHeight+contactInitialOffset;

%Prescribed displacement
prescribedDisplacement_Z=-2;
prescribedRotation_Z=pi/2;

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

E=[];
elementMaterialID=[];
V=[];
Fb=[];
Cb=[];
for q=1:1:4

    % Create a box with hexahedral elements
    beamDimensions=[sampleWidth sampleThickness sampleHeight]; %Dimensions
    beamElementNumbers=[numElementsWidth(q) numElementsThickness(q) numElementsHeight(q)]; %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

    V1(:,3)=V1(:,3)+(q-1)*boxOffsets;

    E=[E;E1+size(V,1)];
    Fb=[Fb;Fb1+size(V,1)];
    V=[V;V1];
    colorOffset=max(Cb(:));
    if isempty(colorOffset)
        colorOffset=0;
    end
    Cb=[Cb;Cb1+colorOffset];

    elementMaterialID=[elementMaterialID;q*ones(size(E1,1),1)];

end
V(:,3)=V(:,3)-min(V(:,3)); %Shift so bottom is at 0

%%
% Plotting model boundary surfaces and a cut view

hFig=cFigure;
hold on;
title('Model boundary surfaces and labels','FontSize',fontSize);
gpatch(Fb,V,Cb,'k',faceAlpha1);
colormap(gjet(250)); icolorbar;
axisGeom(gca,fontSize);
drawnow;

%%

logicTops=false(size(Cb,1),4);
logicBottoms=false(size(Cb,1),4);
for q=1:1:4
    logicTops(:,q)=Cb==6+(6*(q-1));
    logicBottoms(:,q)=Cb==5+(6*(q-1));
end

%%
% Plotting model boundary surfaces

hFig=cFigure;
hold on;
title('Contact faces','FontSize',fontSize);
gpatch(Fb,V,'kw','none',0.2);
for q=1:1:size(logicTops,2)-1
    gpatch(Fb(logicTops(:,q),:),V,q*ones(nnz(logicTops(:,q)),1),'g',1);
    gpatch(Fb(logicBottoms(:,q+1),:),V,q*ones(nnz(logicBottoms(:,q+1)),1),'y',1);
end

colormap(gjet(250)); icolorbar;
axisGeom(gca,fontSize);
drawnow;

%% Define boundary conditions

F_support=Fb(logicBottoms(:,1),:);

F_rigidBody=Fb(logicTops(:,end),:);
indNodesRigidBody=unique(F_rigidBody(:));
center_of_mass=mean(V(indNodesRigidBody,:),1);

%Supported nodes
bcSupportList=unique(F_support(:));

%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(Fb,V,'kw','none',faceAlpha2);
hl(1)=gpatch(F_rigidBody,V,'rw','k',1);
hl(2)=plotV(V(bcSupportList,:),'k.','MarkerSize',markerSize);
hl(3)=plotV(center_of_mass,'r.','MarkerSize',50);

legend(hl,{'Rigid body','BC support','Rigid body center of mass'});

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='3.0';

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

%Control section
febio_spec.Control.analysis='STATIC';
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=1/numTimeSteps;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.max_ups=max_ups;
febio_spec.Control.solver.symmetric_stiffness=0;
febio_spec.Control.time_stepper.dtmin=dtmin;
febio_spec.Control.time_stepper.dtmax=dtmax;
febio_spec.Control.time_stepper.max_retries=max_retries;
febio_spec.Control.time_stepper.opt_iter=opt_iter;

%Material section
for q=1:1:numel(youngsModuli)
    materialNameNow=['Material',num2str(q)];
    febio_spec.Material.material{q}.ATTR.name=materialNameNow;
    febio_spec.Material.material{q}.ATTR.type='neo-Hookean';
    febio_spec.Material.material{q}.ATTR.id=q;
    febio_spec.Material.material{q}.E=youngsModuli(q);
    febio_spec.Material.material{q}.v=poissonsRatios(q);
end

materialNameRigid='RigidMaterial';
matIdRigid=numel(youngsModuli)+1;
febio_spec.Material.material{matIdRigid}.ATTR.name=materialNameRigid;
febio_spec.Material.material{matIdRigid}.ATTR.type='rigid body';
febio_spec.Material.material{matIdRigid}.ATTR.id=numel(youngsModuli)+1;
febio_spec.Material.material{matIdRigid}.density=1;
febio_spec.Material.material{matIdRigid}.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
n=1;
for q=1:1:numel(youngsModuli)
    logicMaterialNow=(elementMaterialID==q);
    partNameNow=['Part',num2str(q)];
    febio_spec.Mesh.Elements{q}.ATTR.name=partNameNow; %Name of this part
    febio_spec.Mesh.Elements{q}.ATTR.type='hex8'; %Element type of this set
    febio_spec.Mesh.Elements{q}.elem.ATTR.id=(n:1:(n-1+nnz(logicMaterialNow)))'; %Element id's
    febio_spec.Mesh.Elements{q}.elem.VAL=E(logicMaterialNow,:);
    n=n+nnz(logicMaterialNow);
end

partNameRigid='RigidPart';
n=max(febio_spec.Mesh.Elements{end}.elem.ATTR.id);
febio_spec.Mesh.Elements{numel(youngsModuli)+1}.ATTR.name=partNameRigid; %Name of this part
febio_spec.Mesh.Elements{numel(youngsModuli)+1}.ATTR.type='quad4'; %Element type of this set
febio_spec.Mesh.Elements{numel(youngsModuli)+1}.ATTR.mat=numel(youngsModuli)+1; %material index for this set
febio_spec.Mesh.Elements{numel(youngsModuli)+1}.elem.ATTR.id=(n:1:(n-1+size(F_rigidBody,1)))'; %Element id's
febio_spec.Mesh.Elements{numel(youngsModuli)+1}.elem.VAL=F_rigidBody;

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

%MeshDomains section
for q=1:1:numel(youngsModuli)
    partNameNow=['Part',num2str(q)];
    materialNameNow=['Material',num2str(q)];
    febio_spec.MeshDomains.SolidDomain{q}.ATTR.name=partNameNow;
    febio_spec.MeshDomains.SolidDomain{q}.ATTR.mat=materialNameNow;
end

febio_spec.MeshDomains.ShellDomain.ATTR.name=partNameRigid;
febio_spec.MeshDomains.ShellDomain.ATTR.mat=materialNameRigid;

% -> Surfaces
febio_spec.Mesh.Surface=[];
for q=1:1:numel(youngsModuli)-1
    F_contact_now=Fb(logicTops(:,q),:);
    c=numel(febio_spec.Mesh.Surface)+1;
    febio_spec.Mesh.Surface{c}.ATTR.name=['contact_',num2str(c)];
    febio_spec.Mesh.Surface{c}.quad4.ATTR.id=(1:1:size(F_contact_now,1))';
    febio_spec.Mesh.Surface{c}.quad4.VAL=F_contact_now;

    F_contact_now=Fb(logicBottoms(:,q+1),:);
    c=numel(febio_spec.Mesh.Surface)+1;
    febio_spec.Mesh.Surface{c}.ATTR.name=['contact_',num2str(c)];
    febio_spec.Mesh.Surface{c}.quad4.ATTR.id=(1:1:size(F_contact_now,1))';
    febio_spec.Mesh.Surface{c}.quad4.VAL=F_contact_now;
end

% -> Surface pairs
surfaceIndices=reshape(1:(2*(numel(youngsModuli)-1)),2,3)'; %Indices for surface pairs
for q=1:1:numel(youngsModuli)-1
    febio_spec.Mesh.SurfacePair{q}.ATTR.name=['Contact_',num2str(q)];

    numElements1=size(febio_spec.Mesh.Surface{surfaceIndices(q,1)}.quad4.VAL,1);
    numElements2=size(febio_spec.Mesh.Surface{surfaceIndices(q,2)}.quad4.VAL,1);
    if numElements1>numElements2
        febio_spec.Mesh.SurfacePair{q}.primary = febio_spec.Mesh.Surface{surfaceIndices(q,1)}.ATTR.name;
        febio_spec.Mesh.SurfacePair{q}.secondary = febio_spec.Mesh.Surface{surfaceIndices(q,2)}.ATTR.name;
    else
        febio_spec.Mesh.SurfacePair{q}.primary = febio_spec.Mesh.Surface{surfaceIndices(q,2)}.ATTR.name;
        febio_spec.Mesh.SurfacePair{q}.secondary = febio_spec.Mesh.Surface{surfaceIndices(q,1)}.ATTR.name;
    end
end

%Boundary condition section
% -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x,y,z';

% %Rigid section
% -> Prescribed rigid body boundary conditions
febio_spec.Rigid.rigid_constraint{1}.ATTR.name='RigidFix_1';
febio_spec.Rigid.rigid_constraint{1}.ATTR.type='fix';
febio_spec.Rigid.rigid_constraint{1}.rb=matIdRigid;
febio_spec.Rigid.rigid_constraint{1}.dofs='Rx,Ry,Ru,Rv';

febio_spec.Rigid.rigid_constraint{2}.ATTR.name='RigidPrescribe';
febio_spec.Rigid.rigid_constraint{2}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{2}.rb=matIdRigid;
febio_spec.Rigid.rigid_constraint{2}.dof='Rz';
febio_spec.Rigid.rigid_constraint{2}.value.ATTR.lc=1;
febio_spec.Rigid.rigid_constraint{2}.value.VAL=prescribedDisplacement_Z;
febio_spec.Rigid.rigid_constraint{2}.relative=0;

febio_spec.Rigid.rigid_constraint{3}.ATTR.name='RigidPrescribe';
febio_spec.Rigid.rigid_constraint{3}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{3}.rb=matIdRigid;
febio_spec.Rigid.rigid_constraint{3}.dof='Rw';
febio_spec.Rigid.rigid_constraint{3}.value.ATTR.lc=1;
febio_spec.Rigid.rigid_constraint{3}.value.VAL=prescribedRotation_Z;
febio_spec.Rigid.rigid_constraint{3}.relative=0;

%Contact section
for q=1:1:numel(youngsModuli)-1
    febio_spec.Contact.contact{q}.ATTR.surface_pair=febio_spec.Mesh.SurfacePair{q}.ATTR.name;
    febio_spec.Contact.contact{q}.ATTR.type='sliding-elastic';
    febio_spec.Contact.contact{q}.two_pass=1;
    febio_spec.Contact.contact{q}.laugon=1;
    febio_spec.Contact.contact{q}.tolerance=0.2;
    febio_spec.Contact.contact{q}.gaptol=0;
    febio_spec.Contact.contact{q}.minaug=1;
    febio_spec.Contact.contact{q}.maxaug=10;
    febio_spec.Contact.contact{q}.search_tol=0.01;
    febio_spec.Contact.contact{q}.search_radius=0.1*sqrt(sum((max(V,[],1)-min(V,[],1)).^2,2));
    febio_spec.Contact.contact{q}.symmetric_stiffness=0;
    switch q
        case 1
            febio_spec.Contact.contact{q}.auto_penalty=1;
            febio_spec.Contact.contact{q}.penalty=5;
            febio_spec.Contact.contact{q}.fric_coeff=1;
        case 2
            febio_spec.Contact.contact{q}.auto_penalty=1;
            febio_spec.Contact.contact{q}.penalty=0.1;
            febio_spec.Contact.contact{q}.fric_coeff=0.08;
        case 3
            febio_spec.Contact.contact{q}.auto_penalty=1;
            febio_spec.Contact.contact{q}.penalty=0.1;
            febio_spec.Contact.contact{q}.fric_coeff=0.08;
    end
end

%LoadData section
% -> load_controller
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}.points.point.VAL=[0 0; 1 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.element_data{1}.ATTR.file=febioLogFileName_strainEnergy;
febio_spec.Output.logfile.element_data{1}.ATTR.data='sed';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';

%% 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='external';%'internal';

[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_strainEnergy),1,1);

    %Access data
    E_energy=dataStruct.data;

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

    [CV]=faceToVertexMeasure(E,V,E_energy(:,:,end));

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

    axisGeom(gca,fontSize);
    colormap(gjet(250)); colorbar;
    caxis([min(E_energy(:)) max(E_energy(:))]/2);
    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(E,V,E_energy(:,:,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/>.


function rtk=udion(rtk,tt,bl,ind)

global glc;
GAP_RESION=120;

for i=1:glc.MAXSAT
    j=rtk.ii+i;
    if rtk.x(j)~=0&&rtk.sat(i).outc(1)>GAP_RESION&&rtk.sat(i).outc(2)>GAP_RESION
        rtk.x(j)=0;
    end
end

for i=1:ind.ns
    j=rtk.ii+ind.sat(i);

    if rtk.x(j)==0
        rtk=initx(rtk,1e-6,(rtk.opt.std(2)*bl/1e4)^2,j);
    else
        el=rtk.sat(ind.sat(i)).azel(2);
        fact=cos(el);
        rtk.P(j,j)=rtk.P(j,j)+(rtk.opt.prn(2)*bl/1e4*fact)^2*abs(tt);
    end
end

return


function [Problem,Constraints,Objectives] = ITERoptimoptset(Spec)

LBX0UB = [Spec.MyDesignTypVals,Spec.MyDesignTypVals,Spec.MyDesignTypVals];
Xnames=Spec.MyDesignXnames;

active_decX = false(length(Xnames),1);
active_decX(1:5) = true;

% defaults assume (from hist AC;~ min, median, max):
LBX0UBbase= [   12  40  70  ;% Disc Loading (kg/m2)
    .15     .25     .4  ;% Power to Weight (kW/kg)
    .03     .075    .15 ;% Solidity
    200     220     238 ;% Tip Speed (m/s)
    .08     .17     .275;% Fuel Fraction
    2       5       7   ;% Blades
    -20     -8      0   ;% Twist  (deg)
    0       0       .4  ;% Thrust to Weight
    0       0       2   ;% Sp. Wing Area (m2/tonne)
    6       7       9   ];% Wing AR

nBuiltInVars = size(LBX0UBbase,1);
LBX0UB(1:nBuiltInVars,1) = LBX0UBbase(:,1);
LBX0UB(1:nBuiltInVars,3) = LBX0UBbase(:,3);

X0 = LBX0UB(:,2);
LB = LBX0UB(:,1);
UB = LBX0UB(:,3);

%%
totalncons = Spec.nPPrequirements + Spec.nnonl;
active_constraints = true(totalncons,1);
extra_margins = zeros(totalncons,1);
useBEA = false(Spec.nPPrequirements,1);

nObj = Spec.nObj;
single_objective_weightings = ones(nObj,1);
ObjLabels = Spec.Objlabels;

for ii = 1:nObj
    if strncmpi('Obj_max',func2str(Spec.Objfuncs{ii}),7)
        single_objective_weightings(ii) = -1;
    end
end

nCons = totalncons;

conLabels = [Spec.PPlabels;Spec.nonllabels];

Problem.x0 =    X0;
Problem.Aineq = [];
Problem.bineq = [];
Problem.Aeq =   [];
Problem.beq =   [];
Problem.lb =    LB;
Problem.ub =    UB;
Problem.XLabels = Xnames;
Problem.activeX = active_decX;

Constraints.nCons = nCons;
Constraints.conLabels = conLabels;
Constraints.active_cons = active_constraints;
Constraints.useBEA = useBEA;
Constraints.margins = extra_margins;

Objectives.weightings = single_objective_weightings;
Objectives.nObj = nObj;
Objectives.ObjLabels = ObjLabels;


function [niiC,doseFileNameC] = dose2nii(planC, doseNumV, scanNum, tmpDirPath,reorientFlag)

if ~exist('tmpDirPath','var') || isempty(tmpDirPath) || ~exist(tmpDirPath,'dir')
    tmpDirPath = fullfile(getCERRPath, 'ImageRegistration', 'tmpFiles');
end

if ~exist('reorientFlag','var') || isempty(reorientFlag)
    reorientFlag = 1;
end

[affineMat,~, voxel_size, ~, dose3MC] = getPlanCAffineMat(planC, scanNum, reorientFlag, [], doseNumV);
qOffset = affineMat(1:3,end)';

for i = 1:numel(doseNumV)
    dose3M = dose3MC{i};
    [scanUniqName, ~] = genScanUniqName(planC,scanNum);
    doseFileName = fullfile(tmpDirPath, ['dose_' num2str(doseNumV(i)) '_' scanUniqName '.nii']);
    niiC{i} = vol2nii(dose3M,affineMat,qOffset,voxel_size,doseFileName);
    doseFileNameC{i} = doseFileName;
end


function H=plot(x,varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% KDE plotting function
%
%  plot(kde)            -- plot a KDE with various features.
%  plot(kde,style)      --  style is of the form
%  plot(kde,dim,style)  --    [STR1,STR2,...] where
%
%  style is of the form [STR1 STR2 STR3...] where
%
%  STR1 is the style to plot the line (1D) or kernel locations (2+D).  Note
%     that options must be specified in lowercase, e.g. 'ro' for red circles.
%     Default style is '-b'
%  STRN is a style for various optional plot features:
%    'W' : show kernel weights (by color: black = low, color in STR1 = high)
%    'S' : show relative kernel sizes, as circles around each center
%    'Bs': show KD-tree structure / bounding boxes, 's' is e.g. '-b' for
%             bounding boxes of solid blue lines
%    'Nd':show d levels of the bounding boxes (d an integer) (default all)
%  The default style is '.b'
%    Example styles:   '*kS-b'  -- black centers, blue variance circles
%                      '.gWS-kB-rN3' -- green dots, colored by weights,
%                          with black variances and 3 levels of bounding
%                          boxes in red.
%                      'S-k' -- just plot variances (no centers), in black
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2003 Alexander Ihler; distributable under GPL -- see README.txt

dim = []; args = [];
if (nargin == 1)
  dim = 1:getDim(x);
  if (getDim(x) == 1) args = 'b-'; else args = 'b.'; end;
elseif (nargin == 2)
  dim = [1:getDim(x)];
  args = varargin{1};
else
  dim = varargin{1};
  args = varargin{2};
end;
%
% Separate the arguments into:  [plot info][TAG taginfo][TAG taginfo] ...
%   where TAG is one of 'W','B','S'.
%
F = find(args ~= lower(args)); Fmin = min([F,length(args)+1]);
argsPlot = args(1:Fmin-1);
argsKDE = args(Fmin:length(args));

if (getDim(x) == 1)
  pts = getPoints(x);
  N = 200;   range = [min(pts),max(pts)];
  range(1) = range(1) - .05*(range(2)-range(1));
  range(2) = range(2) + .05*(range(2)-range(1));
  H=draw1D(x,linspace(range(1),range(2),N),argsPlot,argsKDE);
else
  H=drawAllPairs(x,dim,argsPlot,argsKDE);
end;

%
% Internal functions
%
function e=draw1D(x, bins, style, myStyle)
  y = evaluate(x,bins);
  e=plot(bins,y,style);

  mx = max(y);
  wts = getWeights(x);

  if(strfind(myStyle,'W'))
    holdf = ishold; hold on;
    subStyle = extract(myStyle,'W');
    subStyle=subStyle(2:end); if (length(subStyle)==0) subStyle = '^'; end;
    etmp = stem(getPoints(x), wts, subStyle);
    e = [e;etmp];
    if (~holdf) hold off; end;
  end

  if(strfind(myStyle,'S'))
    holdf = ishold; hold on;
    subStyle = extract(myStyle,'S');
    subStyle=subStyle(2:end); if (length(subStyle)==0) subStyle = '--b'; end;

    bw = getBW(x); pts = getPoints(x); type = getType(x);
    for i=1:length(pts)
      xtmp = kde(pts(i), bw(i), 1, type);
      etmp = plot(bins, evaluate(xtmp, bins) * wts(i), subStyle);
      e = [e;etmp];
    end

    % for plotting only gaussian kernels faster:
%    gr = repmat(bins, length(wts), 1);
%    wts = repmat(wts', 1, size(gr, 2));
%    pts = repmat(getPoints(x)', 1, size(gr,2));
%    bw = repmat(getBW(x)', 1, size(gr, 2));
%    etmp = plot( bins, (wts.*(1./(sqrt(2*pi)*bw)).*exp(-(gr-pts).^2./(2*bw.^2)))', subStyle);

    e = [e;etmp];
    if (~holdf) hold off; end;
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function e=drawAllPairs(x,dims,style,myStyle)
  pts = getPoints(x);
  holdf = ishold;
  e = [];
  Nout = length(dims);
  PlotI2 = triu(repmat(dims ,[Nout,1]), 1);
  PlotI1 = triu(repmat((dims)',[1,Nout]), 1);
  PlotI1 = PlotI1(find(PlotI1)); PlotI2 = PlotI2(find(PlotI2));
  Ncol = fix(sqrt(length(PlotI2))); Nrow = ceil(length(PlotI2)/Ncol);
  for iT=1:length(PlotI2)                   %  output all dimension pairs:
    subplot(Nrow,Ncol,iT);
    holdfs = ishold;
    if (holdf) hold on; end;
    etmp = drawPair(x,[PlotI1(iT),PlotI2(iT)],style,myStyle);
    if (~holdfs) hold off; end;
    e = [e;etmp];
  end;
  drawnow;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function etmp = drawPair(x,dims,style,myStyle)
  pts = getPoints(x);
  etmp = [];

  if (strfind(myStyle,'W'))  %plot weight info with color
    subStyle = extract(myStyle,'W');
    %wts = getWeights(x);  wts = wts - min(wts);
    %if (max(wts)==0) wts = wts+1; else wts = .75*wts/max(wts) + .25; end;
    wts = getWeights(x);
    wts = .75*wts/max(wts) + .25;
    holdf = ishold;
    for i=1:size(pts,2)
      etmp2 = plot(pts(dims(1),i),pts(dims(2),i),style);   % plot each location and
      hold on;
      etmp = [etmp;etmp2]; ctmp = get(etmp2,'Color');      %  its color in prop.
      set(etmp2,'Color',[1 1 1] - wts(i)*([1 1 1] - ctmp));%  to its weight
    end;
    if (~holdf) hold off; end;
  else                                                    % otherwise, just plot
    if (length(style)~=0)
      etmp = plot(pts(dims(1),:),pts(dims(2),:),style);   %   all locations
  end; end;

  if (strfind(myStyle,'S'))  %plot BW info with circles
    subStyle = extract(myStyle,'S');
    subStyle=subStyle(2:end); if (length(subStyle)==0) subStyle = '-b'; end;
    pts = getPoints(x); sig=getBW(x);
	meanX = pts(dims(1),:);
	meanY = pts(dims(2),:);
	sigX = sig(dims(1),:);
	sigY = sig(dims(2),:);

	holdf = ishold;	hold on;
	theta = linspace(0,2*pi,100);
	for (i = 1:length(meanX))
      etmp2 = plot(meanX(i)+sigX(i)*cos(theta), meanY(i)+sigY(i)*sin(theta),subStyle);
      etmp = [etmp;etmp2];
	end
	if (~holdf) hold off; end;
  end;

  if (strfind(myStyle,'B'))  %plot bounding box info with rectangles
    levels = [];
    if (isempty(strfind(myStyle,'N'))) levels = ceil(log2(getNpts(x)));
    else
      subStyle = extract(myStyle,'N');              % get # of balls if spec'd
      levels = sscanf(subStyle(2:end),'%d');
      levels = min(ceil(log2(getNpts(x))), levels);
    end;
    subStyle = extract(myStyle,'B');              % get plot style if spec'd
    subStyle=subStyle(2:end);
    if(isempty(subStyle))     subStyle = '-b';    end

    N = getNpts(x);
    indices = []; tmp = [1];
    for i=1:levels
      indices = [indices tmp];
      % get rid of NO_CHILD right children
      rt = double(x.rightch(tmp)) + 1;
      rt = rt .* (rt < N+1) + 1 * (rt > N);
      tmp = [double(x.leftch(tmp))+1   rt];
    end

    rX = x.ranges(dims(1),indices);
    rY = x.ranges(dims(2),indices);
    nodesX = x.centers(dims(1),indices);
    nodesY = x.centers(dims(2),indices);
    leavesX = x.centers(dims(1),N+1:end);
    leavesY = x.centers(dims(2),N+1:end);

    squaresX(1,:) = nodesX + rX;
    squaresX(2,:) = nodesX + rX;
    squaresX(3,:) = nodesX - rX;
    squaresX(4,:) = nodesX - rX;
    squaresX(5,:) = nodesX + rX;

    squaresY(1,:) = nodesY + rY;
    squaresY(2,:) = nodesY - rY;
    squaresY(3,:) = nodesY - rY;
    squaresY(4,:) = nodesY + rY;
    squaresY(5,:) = nodesY + rY;

    holdf = ishold;	hold on;
    for i = 1:length(indices)
      etmp2 = plot(squaresX, squaresY, subStyle);
      etmp = [etmp;etmp2];
    end
%   axis square
    if (~holdf) hold off; end;

  end;

  titlestr = ['Dim ',int2str(dims(1)),' v. ',int2str(dims(2))];
  title(titlestr);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function substr=extract(str,tag)
  substr = [];
  loc = strfind(str,tag);
  if (loc)
    substr = str(loc(1):length(str));
    loc = find(substr(2:end) ~= lower(substr(2:end)));
    if (loc) substr = substr(1:loc(1)); end;
  end;


function [p,priors]=create_parameters12(ncp_shocks,start_at_mode)
if nargin<2
    start_at_mode=false;
    if nargin<1
        ncp_shocks=false;
    end
end

p=struct();

p.beta = 0.99;
p.mu = 0.34;
p.gam = 2;
p.phi_d = 0.001;
p.delta = 0.025;
p.alpha = 0.36;
p.theta = 1.05;
p.omega = 0.10;

if ncp_shocks
    p.mss_H = 0.9997;
    p.mss_F = 0.9997;
    p.m_hf_ss=1;
    p.zss_H = 1.0045;
else
    p.mss_H = 0.9;
    p.mss_F = 0.9;
    p.zss_H = 1.0042;
end

p.zss_F = p.zss_H;
p.vss_H = 1.0004;
p.vss_F = p.vss_H;
p.z_hf_ss = 1;
p.v_hf_ss = 1;
p.gss_H = 0.20;
p.gss_F = 0.20;
p.rho_m_HF = 0;
p.rho_m_FH = 0;
p.rho_z_HF = 0;
p.rho_z_FH = 0;
p.rho_g_HF = 0;
p.rho_g_FH = 0;
p.rho_v_HF = 0;
p.rho_v_FH = 0;

priors=struct();

if start_at_mode
    priors.theta = {1.5709,0,5};

    priors.philtr = {17.113/100,0,3};
    priors.phiktr = {2.971/100,0,3};

    if ncp_shocks
        priors.kappamhtr = {0.0077294,0,3};
        priors.kappa_m_F = {8.0501e-06,0,3};
    end
    priors.kappazhtr = {0.002383,0,3};
    priors.kappa_z_F = {1.6159e-05,0,3};

    priors.kappavhtr = {0.017416,0,3};
    priors.kappa_v_F = {sqrt(eps),0,3};

    priors.rho_m_HH = {0.9671,0,1};
    priors.rho_m_FF = {0.9924,0,1};

    priors.rho_z_HH = {0.1752,0,1};
    priors.rho_z_FF = {0.35979,0,1};

    priors.rho_v_HH = {0.15793,0,1};
    priors.rho_v_FF = {0.9834,0,1};

    priors.rho_g_HH = {0.9695,0,1};
    priors.rho_g_FF = {0.9414,0,1};

    priors.sig_m_H = {0.017084,0,2};
    priors.sig_m_F = {0.0063409,0,2};

    priors.sig_z_H = {0.01176,0,2};
    priors.sig_z_F = {0.0085657,0,2};

    priors.sig_v_H = {0.01436,0,2};
    priors.sig_v_F = {0.00060192,0,2};

    priors.sig_g_H = {0.018465,0,2};
    priors.sig_g_F = {0.0098922,0,2};
else
    thetatr = 1.5709;
    priors.theta = {abs(thetatr),0,5};

    priors.philtr = {17.1310/100,0,3};
    priors.phiktr = {2.7056/100,0,3};

    if ncp_shocks
        kappamftr = 0.0010;
        priors.kappamhtr = {0.0077,0,3};
        priors.kappa_m_F = {abs(kappamftr),0,3};
    end

    priors.kappazhtr = {0.0018,0,3};
    kappazftr = 0.0010;
    priors.kappa_z_F = {abs(kappazftr),0,3};

    priors.kappavhtr = {0.0089,0,3};
    kappavftr = 0.0010;
    priors.kappa_v_F = {abs(kappavftr),0,3};

    rhomhhtr = sqrt(0.9671/(1-0.9671));
    priors.rho_m_HH = {rhomhhtr^2/(1+rhomhhtr^2),0,1};
    rhomfftr = sqrt(0.9924/(1-0.9924));
    priors.rho_m_FF = {rhomfftr^2/(1+rhomfftr^2),0,1};

    rhozhhtr = sqrt(0.1752/(1-0.1752));
    priors.rho_z_HH = {rhozhhtr^2/(1+rhozhhtr^2),0,1};
    rhozfftr = sqrt(0.3598/(1-0.3598));
    priors.rho_z_FF = {rhozfftr^2/(1+rhozfftr^2),0,1};

    rhovhhtr = sqrt(0.1581/(1-0.1581));
    priors.rho_v_HH = {rhovhhtr^2/(1+rhovhhtr^2),0,1};
    rhovfftr = sqrt(0.9834/(1-0.9834));
    priors.rho_v_FF = {rhovfftr^2/(1+rhovfftr^2),0,1};

    rhoghhtr = sqrt(0.9695/(1-0.9695));
    priors.rho_g_HH = {rhoghhtr^2/(1+rhoghhtr^2),0,1};
    rhogfftr = sqrt(0.9414/(1-0.9414));
    priors.rho_g_FF = {rhogfftr^2/(1+rhogfftr^2),0,1};

    sigmhtr = 0.01;
    priors.sig_m_H = {abs(sigmhtr),0,2};
    sigmftr = 0.01;
    priors.sig_m_F = {abs(sigmftr),0,2};

    sigzhtr = 0.01;
    priors.sig_z_H = {abs(sigzhtr),0,2};
    sigzftr = 0.01;
    priors.sig_z_F = {abs(sigzftr),0,2};

    sigvhtr = 0.01;
    priors.sig_v_H = {abs(sigvhtr),0,2};
    sigvftr = 0.01;
    priors.sig_v_F = {abs(sigvftr),0,2};

    sigghtr = 0.01;
    priors.sig_g_H = {abs(sigghtr),0,2};
    siggftr = 0.01;
    priors.sig_g_F = {abs(siggftr),0,2};
end

% add the initial conditions of the priors to the parameters
%------------------------------------------------------------
fields=fieldnames(priors);
for ip=1:numel(fields)
    name=fields{ip};
    p.(name)=priors.(name){1};
end

end


function vw = computeContrastMap2(vw, active, control, contrastName, varargin);
%
%  view = computeContrastMap2(view, active, control, contrastName, [options]);
%
% Compute a statistical contrast map on data for a view, applying
% a GLM if necessary. Updated version, using the rewritten GLM code.
%
% The map is computed off data from the scan group assigned to
% the view's current scan. (You can specify a different scan
% group as an optional argument). The map is added to the
% view's map field, and saved in the view's data dir under
% the specified contrast name.
%
% By default, the map represents the -log10(p) value of a statistical
% T test between the active and control conditions. So, a value of
% 2 indicates that the active conditions are greater than the
% control with a significance of 10^-2, a value of 3 is 10^-3, and
% a value of -2 indicates that control > active with a likelihood of
% 10^-2. Other tests (F test rather than T test) and mapUnits are possible
% using the options specified below.
%
%
%
%
% Options include:
%
%   [add options for varying slice, statistical test, mapUnits]
%
%
% ras 06/06
% ras 02/07: had some 'quickCompute' code in here, but after testing out
% load times, it looks like loading part of the GLM information doesn't
% save time over loading everything. So, kept it simple and removed the
% quickCompute code. Also: saves the mapUnits in a separate variable, will
% update param map code to show mapUnits if they're specified (and maybe if
% the user sets a flag).
% dar 02/07: made the bicolormap assignment conditional on the presence of
% a ui field in view (was crashing on hiddeninplanes).
if nargin<1, help(mfilename); return; end

%%%%% default params
test = 't';

% allow for a user input if the active and control
% conditions aren't specified
if ~exist('active','var') | isempty(active) |  ...
	~exist('control','var') | isempty(control)
   prompt={'Active (+) Conditions:' ...
           'Control (-) Conditions:' ...
           'Contrast Name (if blank, will name based on condition names)'};
   def={'1' '0' ''};
   dlgTitle='Compute Contrast Map 2';

   answer=inputdlg(prompt, dlgTitle, 1, def);

   active = str2num(answer{1});
   control = str2num(answer{2});
   contrastName = answer{3};
end
scan = viewGet(vw,'curScan');
opts = {};
verbose = prefsVerboseCheck;
if verbose,		tic;	end   % time the map computation
tcWeights = [];

%%%%% parse the options
for i = 1:length(varargin)
    switch(lower(varargin{i}))
        case 'f',           test = 'f';
        case 't',           test = 't';
		case 'fdr',		opts = [opts {'mapUnits' 'fdr'}];
        case {'w' 'weights'}  % contrast weights
            opts = [ opts { 'weights' varargin{i+1} } ];
        case {'tcweights'}, tcWeights = varargin{i+1};
        case 'mapunits', opts = [opts {'mapUnits'}];, opts=[opts {varargin{i+1}}];
    end
end

%% auto-add tcWeights option for 'selective averaging' GLMs:
params = er_getParams(vw);
if params.glmHRF==0  % flag for deconvolution
	% what this means is, if the GLM didn't assume an HRF, but instead
	% generated several beta values for each condition (1 per time point in a
	% peri-stimulus time window), we want to specify those betas (=frames in time
	% course) to use for the contrast. This should be the same points selected
	% as the 'peakPeriod' parameter. Formally, tcWeights should support
	% differential weighting across time ponts, but right now the glm_contrast code
	% treats it as a list of frames:
	f1 = fix( min(params.timeWindow) / params.framePeriod );
	f2 = fix( max(params.timeWindow) / params.framePeriod );
	pk1 = fix( min(params.peakPeriod) / params.framePeriod );
	pk2 = fix( max(params.peakPeriod) / params.framePeriod );
	tcWeights = find(ismember(f1:f2, pk1:pk2));
	opts = [opts {'tcWeights' tcWeights}];
end

%% default contrast name
if ~exist('contrastName','var') | isempty(contrastName)
	trials = er_concatParfiles(vw);
    activeNames = ''; controlNames = '';
    for i = 1:length(active)
        j = find(trials.condNums==active(i));
        activeNames = [activeNames trials.condNames{j}];
    end
    for i = 1:length(control)
        j = find(trials.condNums==control(i));
        controlNames = [controlNames trials.condNames{j}];
    end
    contrastName = sprintf('%sV%s', activeNames, controlNames)
end

%% check if a GLM has been run; if not, run one:
checkPath = fullfile(dataDir(vw),sprintf('Scan%i',scan),'glmSlice1.mat');
if ~exist(checkPath,'file')
    % no GLM file found -- ask user if they want to run one
    mrGlobals;
    [scans dt] = er_getScanGroup(vw,scan);
    names = {dataTYPES.name};
    q = 'A GLM wasn''t found for this slice and scan. ';
    q = [q 'Do you want to run one now? '];
    q = [q sprintf('%s scans %s',names{dt},num2str(scans))];
    resp = questdlg(q,'Compute Contrast Map');
    if isequal(resp,'Yes')
        % compute one
        vw = selectDataType(vw,dt);
        vw = setCurScan(vw,scans(1));
        [newDt newScan] = applyGlm(vw, scans);
        vw  = selectDataType(vw, newDt);
        vw = setCurScan(vw, newScan);
    else
        disp('Aborting computeContrastMap2')
        return
    end
end

% initialize map cell (of size nScans):
% If another map with the same name has already been
% computed, load this: we'll just plug in the data for
% the current scan:
mapPath = fullfile(dataDir(vw),[contrastName '.mat']);
if exist(mapPath, 'file')
    load(mapPath, 'map', 'mapName');
else
    mapName = contrastName;
    map = cell(1, viewGet(vw, 'numScans'));
end

% initalize volume for the map
mapVol = NaN*ones(dataSize(vw, scan));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% main loop: load data from each slice and compute map  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if verbose, hwait = mrvWaitbar(0,'Computing Contrast Map'); end

nSlices = viewGet(vw, 'numSlices');
for slice = 1:nSlices
	model  = loadGlmSlice(vw, slice, scan);

	mapVol(:,:,slice) = glm_contrast(model, active, control, ...
									   'size', viewGet(vw, 'sliceDims'), opts);

	if verbose, mrvWaitbar(slice/nSlices, hwait);  end
end

if verbose, close(hwait);	end

%% set new map in view
mapUnits = '-log(p)';  % default
if cellfind(opts, 'mapUnits')
    mapUnits = opts{ cellfind(opts, 'mapUnits') + 1 };
end
if isequal(mapUnits, 'log10p'), mapUnits = '-log(p)'; end   % clarify
map{scan} = mapVol;
vw.mapUnits = mapUnits;
vw = setParameterMap(vw, map, mapName);
if checkfields(vw, 'ui', 'mapMode')
	vw = bicolorCmap(vw);  % take a chance that people will like this
end

% set the variance explained as the coherence
varExpPath = fullfile(dataDir(vw), 'Proportion Variance Explained');
if check4File(varExpPath)
	tmp = load(varExpPath, 'map');
	co = tmp.map;
else
	% this will wipe out any existing co data when the map is loaded:
	% but since we're in the GLMs data type, that should be ok.
	co = cell(1, numScans(vw));
end

%% save results
if exist(mapPath,'file')
    save(mapPath, 'map', 'mapName', 'mapUnits', 'co', '-append');
else
    save(mapPath, 'map', 'mapName', 'co', 'mapUnits');
end
fprintf('Saved Contrast Map in %s.\n', mapPath);

%% refresh screen according to whether we're using mrVista 1 or 2:
global GUI
if ~isempty(GUI)
    sessionGUI_selectDataType;  % mrVista 2
else
    vw = refreshScreen(vw, 1);        % mrVista 1
end

if verbose
	fprintf('Time to compute map: %i min, %3.1f sec.\n\n', ...
		floor(toc/60), mod(toc, 60));
end

return


function plot_sound_field(P,X,Y,Z,x0,conf)
%PLOT_SOUND_FIELD plots a sound field
%
%   Usage: plot_sound_field(P,X,Y,Z,[x0],conf)
%
%   Input parameters:
%       P           - matrix containing the sound field in the format P = P(y,x)
%       X           - x-axis / m; single value or [xmin,xmax] or nD-array
%       Y           - y-axis / m; single value or [ymin,ymax] or nD-array
%       Z           - z-axis / m; single value or [zmin,zmax] or nD-array
%       x,y,z       - vectors for the x-, y- and z-axis
%       x0          - matrix containing the secondary source positions to plot.
%                     Default: plot no secondary sources
%       conf        - configuration struct (see SFS_config)
%
%   PLOT_SOUND_FIELD(P,X,Y,Z,x0,conf) plots the sound field P in dependence of
%   the axes that are non-singleton. You have to provide the axes in the same
%   syntax as for the sound field functions. For a given set x0 of secondary
%   sources the secondary sources are added as dots or loudspeaker symbols
%   depending on your setting of conf.plot.realloudspeakers.
%
%   See also: sound_field_mono, sound_field_imp, draw_loudspeakers

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 5;
nargmax = 6;
narginchk(nargmin,nargmax);
isargnumeric(X,Y,Z,P);
if nargin<nargmax
    conf = x0;
    x0 = [];
end
if isempty(x0)
    conf.plot.loudspeakers = 0;
end
isargstruct(conf);

%% ===== Configuration ==================================================
% Tmp dir
tmpdir = conf.tmpdir;
% Plotting
p.usenormalisation = conf.plot.usenormalisation;
p.cmd = conf.plot.cmd;
p.usedb = conf.plot.usedb;
p.mode = conf.plot.mode;
p.size = conf.plot.size;
p.size_unit = conf.plot.size_unit;
p.caxis = conf.plot.caxis;
p.colormap = conf.plot.colormap;
p.loudspeakers = conf.plot.loudspeakers;
p.realloudspeakers = conf.plot.realloudspeakers;
p.lssize = conf.plot.lssize;
p.usefile = conf.plot.usefile;
p.file = conf.plot.file;

%% ===== Calculation =====================================================
% Handle the given axis and check which should be plotted
[dimensions,x1,x2,x3] = xyz_axes_selection(X,Y,Z);
% Check whether some of the axis have custom size
% NOTE: the grid will always be detected as custom, if x,y,z have more than two
% entries, which could also happen if you perform for example:
% >> [P,x,y,z,x0] = sound_field_mono_wfs(X,Y,Z,[0 -1 0],'pw',1000,conf);
% >> plot_sound_field(P,x,y,z,x0,conf)
% Instead you should do
% >> plot_sound_field(P,X,Y,Z,x0,conf)
% if X,Y,Z are a non-custom grid.
is_custom = is_dim_custom(X,Y,Z);

if any(is_custom) && sum(is_custom) < sum(dimensions)
  error(['%s: it is not possible to combine an axis with regular sampled ' ...
    'grid with an axis with custom grid']);
end

if ~any(dimensions)
    error(['%s: you have only one point in the sound field. ', ...
        'Omitting the plotting.'],upper(mfilename));
elseif ~dimensions(1) && ~dimensions(2)
    p.xlabel = 'z / m';
elseif ~dimensions(1)
    p.xlabel = 'y / m';
    p.ylabel = 'z / m';
elseif ~dimensions(2)
    p.xlabel = 'x / m';
    p.ylabel = 'z / m';
elseif ~dimensions(3)
    p.xlabel = 'x / m';
    p.ylabel = 'y / m';
else
    p.xlabel = 'x / m';
    p.ylabel = 'y / m';
    p.zlabel = 'z / m';
end

% Remove Inf values from sound field
P(isinf(P)) = NaN;

% Normalisation
if p.usenormalisation
    P = norm_sound_field(P,conf);
end

if p.usedb
    % If we have only zeros in the sound field set the field to eps to avoid
    % problems with log(0).
    if max(abs(P(:)))==0
        P(:) = eps;
    end
    % Change default colormap to yellowred
    if strcmp('default',p.colormap)
        conf.plot.colormap = 'yellowred';
    end
    % Calculate sound pressure level in dB
    P = 20*log10(abs(P));
    if p.usenormalisation
        P = P - max(P(:)); % ensure 0 dB max for dB plots
    end
else
    P = real(P);
end

% Check if we should plot loudspeakers symbols
if p.realloudspeakers && size(x0,1)>1000
    warning(['%s: the given number of loudspeaker is >1000. ',...
            'Switching back to non loudspeaker symbols.'],upper(mfilename));
    p.realloudspeakers = 0;
end

% Set the color bar axis to default values if not given otherwise
if p.caxis, else
    if p.usedb
        p.caxis = [-45,0];
    else
        p.caxis = [-1,1];
    end
end

%% ===== Plotting ========================================================

% Create a new figure
figure;
% Set size
figsize(p.size(1),p.size(2),p.size_unit);

switch sum(dimensions)
  case 1
    % === 1D Plot ====
    if any(is_custom)  % custom grid
        plot(x1,P,'o','MarkerFaceColor','blue','MarkerSize',3)
    else % regular grid
        x1 = linspace(x1(1),x1(2),conf.resolution);
        plot(x1,P);
    end
    xlabel(p.xlabel);
    if p.usedb
        ylabel('Amplitude / dB');
    else
        ylabel('Amplitude');
    end
  case 2
    % === 2D Plot ====
    if any(is_custom)  % custom grid
        point_size = [];
        if isoctave, point_size = 2; end % fix scatter plot point size for Octave
        scatter(x1(:),x2(:),point_size,limit_colors(P,p.caxis),'filled');
    else  % regular grid
        imagesc(x1,x2,P,p.caxis);
    end
    % Add color bar
    set_colorbar(conf);
    % Set the y direction in normal mode (imagesc uses the reverse mode by
    % default)
    turn_imagesc;
    % Set the axis to use the same amount of space for the same length (m)
    axis image;
    % Labels etc. for the plot
    xlabel(p.xlabel);
    ylabel(p.ylabel);
    % Add loudspeaker to the plot
    if p.loudspeakers % && dimensions(1) && dimensions(2)
        hold on;
        draw_loudspeakers(x0,dimensions,conf);
        hold off;
    end
  case 3
    if ~any(is_custom)
      [x1,x2,x3] = xyz_grid(X,Y,Z,conf);
    end
    % === 3D Plot ====
    point_size = [];
    if isoctave, point_size = 5; end % fix scatter plot point size for Octave
    scatter3(x1(:),x2(:),x3(:),point_size,limit_colors(P,p.caxis),'filled');
    % Fix perspective of plot in Matlab (https://stackoverflow.com/q/17447404)
    set(gcf,'renderer','opengl');
    % Add color bar
    set_colorbar(conf);
    % Set the axis to use the same amount of space for the same length (m)
    axis image;
    % Labels etc. for the plot
    xlabel(p.xlabel);
    ylabel(p.ylabel);
    zlabel(p.zlabel);
end

% Save as file
if ~isempty(p.file) && strcmp('png',p.file(end-2:end))
    print_png(p.file);
elseif ~isempty(p.file) && strcmp('eps',p.file(end-2:end))
    print_eps(p.file);
end
end

%% ===== Helper functions ================================================
function P = limit_colors(P,caxis)
    % Limits the number of different values in P to 64.
    % This speeds up plotting with the scatter function in octave, see:
    % https://savannah.gnu.org/bugs/?40663
    %
    number_of_colors = 64;
    % First apply the caxis values
    P = min(caxis(2),max(caxis(1),P(:)));
    % Transform to [0...64] and transform to integer
    P = fix(number_of_colors/abs(max(P(:))-min(P(:))) .* (P+abs(min(P(:)))));
    % Transform back to [caxis(1)...caxis(2)]
    P = abs(caxis(2)-caxis(1))/number_of_colors .* P + caxis(1);
end


function res = spm_eeg_artefact_saccade(S)
% Detects eyeblinks in SPM continuous data file
% S              - input structure
% fields of S:
%    S.D         - M/EEG object
%    S.chanind   - vector of indices of channels that this plugin will look at
%    S.threshold - threshold parameter (in stdev)
%
%    Additional parameters can be defined specific for each plugin.
%
% Output:
% res -
%    If no input is provided the plugin returns a cfg branch for itself.
%
%    If input is provided the plugin returns a matrix of size D.nchannels x D.ntrials
%    with zeros for clean channel/trials and ones for artefacts.
%
% A simplified version of a method described by:
% Engbert, R., & Mergenthaler, K. (2006) Microsaccades are triggered by low
% retinal image slip. Proceedings of the National Academy of Sciences of
% the United States of America, 103: 7192-7197.
%__________________________________________________________________________
% Copyright (C) 2008-2017 Wellcome Trust Centre for Neuroimaging

% Markus Bauer, Laurence Hunt
% $Id: spm_eeg_artefact_saccade.m 7132 2017-07-10 16:22:58Z guillaume $

%-This part if for creating a config branch that plugs into spm_cfg_eeg_artefact
% Any parameters can be specified and they are then passed to the plugin
% when it's called.
%--------------------------------------------------------------------------
if nargin == 0
    threshold         = cfg_entry;
    threshold.tag     = 'threshold';
    threshold.name    = 'Threshold';
    threshold.strtype = 'r';
    threshold.val     = {3};
    threshold.num     = [1 1];
    threshold.help    = {'Threshold to reject things that look like saccades but probably aren''t.'};

    excwin         = cfg_entry;
    excwin.tag     = 'excwin';
    excwin.name    = 'Excision window';
    excwin.strtype = 'r';
    excwin.num     = [1 1];
    excwin.val     = {0};
    excwin.help    = {'Window (in ms) to mark as bad around each saccade, 0 to not mark data as bad.'};

    saccade      = cfg_branch;
    saccade.tag  = 'saccade';
    saccade.name = 'Saccades';
    saccade.val  = {threshold, excwin};
    saccade.help = {''};

    res = saccade;

    return
end

SVNrev = '$Rev: 7132 $';

%-Startup
%--------------------------------------------------------------------------
spm('sFnBanner', mfilename, SVNrev);
spm('FigName','M/EEG saccade detection');

if isequal(S.mode, 'reject')
     error('Only mark mode is supported by this plug-in, use event-based rejection to reject.');
end

D = spm_eeg_load(S.D);

chanind = S.chanind;
threshold = S.threshold;

if length(chanind)~=1
    error('More than one channel - not currently supported.')
end

eog_data = reshape(squeeze(D(chanind,:,:)), 1, []);

%-Saccade detection:
% 1) filter the data, saccade duration ~40 ms, filtering at 30 Hz is fine
% even if it may weaken signal a tiny bit, it takes out quite some noise
% 2) calculate the velocity values
%--------------------------------------------------------------------------
eog_data = ft_preproc_lowpassfilter(eog_data, D.fsample, 30);
eog_filt = [eog_data(:,1),diff(eog_data,1,2)];
% eog_filt = ft_preproc_lowpassfilter(eog_filt, D.fsample, 20);

%-Find saccades by thresholding
%--------------------------------------------------------------------------
% robust estimate of standard deviation, suggested by Mark Woolrich
sd_eeg = (spm_percentile(eog_filt,85)-spm_percentile(eog_filt,15))/2;
em_thresh = S.threshold*sd_eeg;

%-Find 'spikes' (putative saccades)
%--------------------------------------------------------------------------
eblength = round(D.fsample/5); %length of saccade(200 ms) in samples;
spikes = [];
for i = eblength:length(eog_filt)-eblength;
    if abs(eog_filt(i))>em_thresh && ... %bigger than threshold
       all(abs(eog_filt(i))>=abs(eog_filt(i-eblength+1:i+eblength))); %biggest in surrounding 400ms
        spikes = [spikes i];
    end
end

if isempty(spikes)
    error('No saccades detected by algorithm. Try a lower threshold.')
end

spikemat = zeros(eblength*2,length(spikes));
for i = 1:length(spikes)
    spikemat(:,i) = eog_filt(spikes(i)-eblength+1:spikes(i)+eblength);
end

% reject spikes whose peak is not within 1 s.d. of the mean
% (gets rid of most artefacts etc. not removed by filtering):
%--------------------------------------------------------------------------
mn_spike = mean(spikemat(eblength,:));
sd_spike = std(spikemat(eblength,:));
spikes(spikemat(eblength,:)>mn_spike+sd_spike | ...
       spikemat(eblength,:)<mn_spike-sd_spike) = [];
spikemat(:,find(spikemat(eblength,:)>mn_spike+sd_spike | ...
       spikemat(eblength,:)<mn_spike-sd_spike)) = [];

disp(['Number of putative saccades detected: ' num2str(length(spikes))]);

num_eb_per_min=(60*D.fsample*length(spikes))/length(eog_data);
disp([num2str(num_eb_per_min) ' saccades per minute '])
if (num_eb_per_min<0.5)
    error(['Only ' num2str(num_eb_per_min) ' saccades per minute detected by algorithm. Try a lower threshold.'])
end
if (num_eb_per_min>60)
    error(['As many as ' num2str(num_eb_per_min) ' saccades per minute detected by algorithm. Try a higher threshold.'])
end

% plot
%--------------------------------------------------------------------------
if ~spm('CmdLine')
    Fgraph = spm_figure('GetWin','Graphics');
    colormap(gray)
    figure(Fgraph)
    clf
    subplot(2, 1 , 1)
    plot(spikes,ones(length(spikes),1)*5*sd_eeg,'r.');
    hold on;
    plot(eog_filt);

    subplot(2, 1 , 2)
    hold on;
    plot(spikemat);plot(mean(spikemat,2),'Color','k','LineWidth',4);
end

% Update the event structure
%--------------------------------------------------------------------------
if ~isempty(spikes)
    for n = 1:D.ntrials
        cspikes   = spikes(spikes>(D.nsamples*(n-1)) & spikes<(D.nsamples*n));
        ctime  = D.trialonset(n)+(cspikes - D.nsamples*(n-1)-1)/D.fsample;
        ctime  = num2cell(ctime);

        ev = events(D, n);

        if iscell(ev)
            ev = ev{1};
        end

        if ~isempty(ev) && ~S.append
            ind1 = strmatch('artefact_saccade', {ev.type}, 'exact');
            if ~isempty(ind1)
                ind2 = strmatch(D.chanlabels(chanind), {ev(ind1).value}, 'exact');
                if ~isempty(ind2)
                    ev(ind1(ind2)) = [];
                end
            end
        end

        Nevents = numel(ev);
        for i=1:numel(ctime)
            if ctime{i} == 0
                %likely to be trial border falsely detected as saccade
                continue;
            end
            ev(Nevents+i).type  = 'artefact_saccade';
            ev(Nevents+i).value = char(D.chanlabels(chanind));
            if S.excwin == 0
                ev(Nevents+i).duration = [];
                ev(Nevents+i).time     = ctime{i};
            else
                ev(Nevents+i).time     = max(D.trialonset(n), ctime{i} - 5e-4*S.excwin);
                ev(Nevents+i).duration = min(1e-3*S.excwin, (D.time(end)-D.time(1))-(ev(Nevents+i).time-D.trialonset(n)))+...
                    min(ctime{i} - 5e-4*S.excwin, 0);
            end
        end

        if ~isempty(ev)
            [dum, I] = sort([ev.time]);
            ev = ev(I);
            D = events(D, n, ev);
        end
    end
else
    warning('No saccade events detected in the selected channel.');
end

res = D;

spm('FigName','M/EEG saccade detection: done');


% Script to report the bug of inconsistent
% Behavior when linear indexing sparse matrix on 32-bit platform
% Bruno Luong <b.luong@fogale.fr>

if ~isempty(strfind(computer(),'64'))
    fprintf('Test valid only on 32-bit machine\n')
    return
end

if datenum(version('-date'))>datenum('03-Apr-2009')
    fprintf('This bug could be already fixed after v.2009A (?).\n')
end

m=66050;
n=65026;
icritical=mod(m*n,2^32)+1;
S=sparse([1 m],[1 n],[1 2],m,n) % Error not occurs for size=3e5

ind=[1 icritical];

disp('ind')
disp(ind)

% Flag to tell if Matlab is buggy
Buggy = 0;

% OK, access both index separately
disp('OK: access both index separately')
disp('S(ind(1))');
disp(S(ind(1))) % 1
disp('S(ind(2))');
disp(S(ind(2))); % 2

% Not OK, access both index at the same time
try
    % ??? Index exceeds matrix dimensions.
    disp('Not OK: access both index at the same time');
    disp('S(ind)');
    disp(S(ind)); % error
catch
    Buggy = 1;
    fprintf('\t-> Error: %s\n\n', lasterr());
end

% OK, acess with large double linear index
disp('OK: acess with large double linear index ');
disp('S(icritical)');
disp(S(icritical)); % 2

disp('OK: acess with "small" double linear index ');
disp('S(icritical-1)');
disp(S(icritical-1)); % 0

% Not OK, acess with large integer linear index
try
    % ??? Index exceeds matrix dimensions.
    disp('Not OK: acess with large integer linear index ');
    disp('S(int32(icritical))');
    disp(S(int32(icritical))); % error
catch
    Buggy = 1;
    fprintf('\t-> Error: %s\n\n', lasterr());
end

if Buggy
    fprintf('You have a matlab that have a bug in linear indexing\n');
else
    fprintf('Congratulation your sparse linear-index is working fine\n');
end


function p = addImpliedSDP(p)
if p.K.q(1)==0 && p.K.s(1) > 0
    % Search for rows where there is a constant, and a diagonal term
    % which thus has to be structly positive, we might leads us to a
    % constraints of the type sum x_i >= 1, if only non-negative integer
    % variables are involved, entering with positive coefficient (and v.s)
    newF = [];
    top = p.K.f + p.K.l + 1;
    for i = 1:length(p.K.s)
        F = p.F_struc(top:top + p.K.s(i)^2-1,:);
        X0 = reshape(F(:,1),p.K.s(i),p.K.s(i));
        candidates = find(any(X0-diag(diag(X0)),2) & (diag(X0)<=0));
        if ~isempty(candidates);
            I = speye(p.K.s(i),p.K.s(i));
            pos = find(I(:));
            for j = candidates(1:end)'
                row = F(pos(j),2:end);
                used = find(row);
                if ~isempty(used)
                    if ~any(row(p.noninteger_variables))
                        if all(row <=0) && all(p.ub(used)<=0)
                            row(find(row)) = 1;
                            newF = [newF;-1 -row];
                        elseif all(row >=0) && all(p.lb(used)>=0)
                            row(find(row)) = 1;
                            newF = [newF;-1 row];
                        end
                    end
                end
            end
        end
        top = top + p.K.s(i)^2;
    end
    if size(newF,1)>0
        p.F_struc = [p.F_struc(1:p.K.f + p.K.l,:);
            newF;
            p.F_struc(1+p.K.f+p.K.l:end,:)];
        p.K.l = p.K.l + size(newF,1);
    end

    % Search for trivial variables entering in diagonal elements alone, and
    % not entering anywhere else in model. Bad modelling?
    % First, they should not appear in LP cone (can be generalized by
    % checking that it acts in the correct direction)
    % TODO: Check for possibility of different signs etc, currently only
    % implemented for typical test cases
    candidates = find(~any(p.F_struc(1:p.K.f + p.K.l,2:end),1));
    fixable = nan(1,length(candidates));

    top = p.K.f + p.K.l + 1;
    for j = 1:length(p.K.s)
        F = p.F_struc(top:top + p.K.s(j)^2-1,:);
        for i = 1:length(candidates(:)')
            X0 = reshape(F(:,1 + candidates(i)),p.K.s(j),p.K.s(j));
            % Diagonal matrix?
            d = diag(X0);
            if nnz((X0-diag(d)))==0
                if all(sign(d)<=0) && p.c(i)>=0 && (isnan(fixable(i)) || fixable(i)==-1)
                    fixable(i) = -1;
                elseif all(sign(d)>=0) && p.c(i)<=0 && (isnan(fixable(i)) || fixable(i)==1)
                    fixable(i) = 1;
                else
                    fixable(i) = 0;
                end
            else
                fixable(i) = 0;
            end
        end
        top = top + p.K.s(j)^2;
    end
    p.adjustable = candidates(find(fixable));
    for i = candidates(find(~isnan(fixable)))
%         if fixable(i) == -1
%             if ~isinf(p.lb(i))
%                 %             p.ub(i) = p.lb(i);
%                 %             p.F_struc(:,1) = p.F_struc(:,1) + p.F_struc(:,i+1)*p.lb(i);
%                 %             p.F_struc(:,i+1) = 0;
%                 %         else
%                 %             % TODO whole row/column should be removed from the LMI!
%             end
%         elseif fixable(i) == 1
%             if ~isinf(p.ub(i))
%                 %             p.ub(i) = p.lb(i);
%                 %             p.F_struc(:,1) = p.F_struc(:,1) + p.F_struc(:,i+1)*p.lb(i);
%                 %             p.F_struc(:,i+1) = 0;
%                 %         else
%                 %             % TODO whole row/column should be removed from the LMI!
%             end
%         end

    end
end


function MDP = DEMO_MDP_maze
% Demo of mixed continuous and discrete state space modelling
%__________________________________________________________________________
%
% This demonstration of active inference focuses on navigation and planning
% in a fairly complicated maze. The idea is to demonstrate how epistemic
% foraging and goal (target) directed behaviour are integrated in the
% minimisation of expected free energy. In this illustration, and 8 x 8
% maze is learned through novelty driven evidence accumulation - to learn
% the likelihood mapping between hidden states (locations in the maze) and
% outcomes (whether the current location is open or closed). This
% accumulated experience is then used to plan a path from a start to an end
% (target location) under a task set specified by prior preferences over
% locations. These priors are based upon a simple diffusion (CF backwards
% induction) heuristic that specifies subgoals. The subgoals (i.e.,
% locations) contain the most paths from the target within the horizon of
% the current policy.
%
% We will first illustrate the novelty driven epistemic foraging that
% efficiently scans the maze to learn its structure. We then simulate
% planning of (shortest path) trajectory to the target under the assumption
% the maze has been previously learned. Finally, we consider exploration
% under prior preferences to simulate behaviour when both epistemic and
% goal directed imperatives are in play. The focus on this demo is on
% behavioural and electrophysiological responses over moves.
%
% A key aspect of this formulation is the  hierarchical decomposition of
% goal directed behaviour into subgoals that are within the horizon of a
% limited policy - here, to moves that correspond to a trial. The prior
% preferences then contextualise each policy or trial to ensure that the
% ultimate goal is achieved.
%
% Empirically, this sort of construction suggests the existence of Path
% cells; namely, cells who report the initial location of any subsequence
% and continue firing until the end of the local path. This is illustrated
% by plotting simulated activity as a function of trajectories during
% exploration.
%
% see also: spm_MPD_VB_X.m
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEMO_MDP_maze.m 7766 2020-01-05 21:37:39Z karl $

% set up and preliminaries: first level
%--------------------------------------------------------------------------
rng('default')

% generative model at the sensory level (DEM): continuous states
%==========================================================================
% the generative model has two outcome modalities; namely, what (open
% versus closed) and where (the current location in a maze). These outcomes
% are generated from a single hidden factor (location), where the structure
% of the maze is encoded in the likelihood of observation mapping (that can
% be learned through experience). Allowable actions  include for moves (up,
% down, left, right) and staying at the current location. These induce five
% transition matrices that play the role of empirical priors. Finally,
% prior preferences are based upon allowable transitions (that are function
% of learned accumulated likelihood), which are used to define attractive
% locations within the horizon of two-move policies. These priors implement
% a task set and are returned by a subfunction below: spm_maze_cost
%--------------------------------------------------------------------------
label.factor     = {'where'};
label.modality   = {'what','where'};
label.outcome{1} = {'open','closed'};

MAZE  = [...
    1 1 1 1 1 1 1 1;
    1 0 0 0 0 0 0 1;
    1 1 1 0 1 1 0 1;
    1 1 0 0 0 1 0 1;
    1 1 0 1 0 0 0 1;
    1 1 0 1 1 1 0 1;
    1 0 0 0 0 0 0 1;
    1 0 1 1 1 1 1 1];
END   = sub2ind(size(MAZE),5,5);                  % goal or target location
START = sub2ind(size(MAZE),8,2);                  % first or start location

% prior beliefs about initial states: D
%--------------------------------------------------------------------------
D{1}  = zeros(numel(MAZE),1);
Ns    = numel(D{1});

% probabilistic mapping from hidden states to outcomes: A
%--------------------------------------------------------------------------
A{1}  = [1 - MAZE(:), MAZE(:)]';                  % what
A{2}  = eye(Ns,Ns);                               % where
Ng    = numel(A);
for g = 1:Ng
    No(g)  = size(A{g},1);
end

% controlled transitions: B (up, down, left, right, stay)
%--------------------------------------------------------------------------
u    = [1 0; -1 0; 0 1; 0 -1; 0 0];               % allowable actions
nu   = size(u,1);                                 % number of actions
B{1} = zeros(Ns,Ns,nu);
[n,m] = size(MAZE);
for i = 1:n
    for j = 1:m

        % allowable transitions from state s to state ss
        %------------------------------------------------------------------
        s     = sub2ind([n,m],i,j);
        for k = 1:nu
            try
                ss = sub2ind([n,m],i + u(k,1),j + u(k,2));
                B{1}(ss,s,k) = 1;
            catch
                B{1}(s, s,k) = 1;
            end
        end
    end
end

% allowable policies (2 moves): V
%--------------------------------------------------------------------------
V     = [];
for i = 1:nu
    for j = 1:nu
        V(:,end + 1) = [i;j];
    end
end

% priors: (negative cost) C:
%--------------------------------------------------------------------------
for g = 1:Ng
    C{g} = zeros(No(g),1);
end

% basic MDP structure
%--------------------------------------------------------------------------
mdp.V = V;                      % allowable policies
mdp.A = A;                      % observation model or likelihood
mdp.B = B;                      % transition probabilities
mdp.C = C;                      % preferred outcomes
mdp.D = D;                      % prior over initial states

mdp.label = label;
mdp       = spm_MDP_check(mdp);

% exploratory (32 trial) sequence (no experience or task set)
%==========================================================================
% These simulations use a subroutine (spm_maze_search) to perform recursive
% variational inversions of the active inference scheme under different
% levels of experience and prior preferences
%--------------------------------------------------------------------------
SDP = spm_maze_search(mdp,32,START,END,0,0);

% show results - behavioural
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1'); clf
spm_maze_plot(SDP,END)

% show results - electrophysiological
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2'); clf
spm_MDP_VB_LFP(SDP);

% exploratory (8 trial) sequence (with experience and task set)
%==========================================================================
MDP = spm_maze_search(mdp,8,START,END,128,1);

% show results in terms of path
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 3'); clf
spm_maze_plot(MDP,END)

% and implicit subgoals
%--------------------------------------------------------------------------
for i = 1:8
    subplot(8,2,(i - 1)*2 + 2);
    c = spm_softmax(MDP(i).C{2}(:,1));
    imagesc(spm_unvec(c,MAZE))
    axis image off, title(sprintf('%s %i','Trial',i))
end

% evidence accumulation under task set
%==========================================================================
MDP   = mdp;
for i = 1:4

    % return to start
    %----------------------------------------------------------------------
    MDP = spm_maze_search(MDP(end),8,START,END,0,1);

    % show results in terms of path
    %----------------------------------------------------------------------
    spm_figure('GetWin',sprintf('%s %i','Search',i)); clf
    spm_maze_plot(MDP,END)

    % and electrophysiological responses
    %----------------------------------------------------------------------
    spm_figure('GetWin',sprintf('%s %i','Responses',i)); clf
    spm_MDP_VB_game(MDP(1:8));
    subplot(6,1,3), set(gca,'YLim',[-1 1]/3);
    subplot(6,1,4), set(gca,'YLim',[0 1]);

end

% in silico psychophysical experiment
%==========================================================================
% Here, we return to the exploratory simulation above and probe each level
% of experience by asking the subject to execute a path to target. The
% behaviour is then assessed in terms of the latency with which the target
% will go is required  - and the number of mistakes or exploratory
% excursions into closed locations.
%--------------------------------------------------------------------------
N     = [];
M     = [];
T     = [];
for i = 1:2:numel(SDP)

    % execute path to target with increasing experience
    %----------------------------------------------------------------------
    MDP = spm_maze_search(SDP(i),8,START,END,0,1);

    % record behaviour
    %----------------------------------------------------------------------
    s   = spm_cat({MDP.s}); s(3:3:end) = [];
    s   = [s,END];
    N   = [N,find(s == END,1)];                      % latency
    M   = [M,sum(MAZE(s))];                          % mistakes
    T   = [T,i/2];                                   % exposure to maze

end

% show results
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 4'); clf
subplot(2,1,1), bar([M;N]'), xlabel('Exposure (seconds)'), axis square
title('Performance','FontSize',16),legend({'Mistakes','Latency'})

% place and direction specific responses
%==========================================================================
spm_figure('GetWin','Figure 5'); clf

% illustrate simulated responses (first eight subsequences)
%--------------------------------------------------------------------------
spm_MDP_VB_LFP(SDP(1:8))

% extract simulated responses
%--------------------------------------------------------------------------
[u,v] = spm_MDP_VB_LFP(SDP);               % responses
E     = kron(eye(32*3),pinv(ones(16,1)));  % expectation matrix
v     = E*spm_cat(v);                      % firing rates (hidden states)
u     = E*spm_cat({SDP.un})';              % firing rates (policies)
s     = spm_cat({SDP.s})';                 % location

% Position
%--------------------------------------------------------------------------
[X,Y] = ndgrid(1:8,1:8);
L     = [X(:),Y(:)];
X     = L(s,:);

% accumulate responses at each location
%--------------------------------------------------------------------------
PC    = zeros(8,8,size(v,2));
for i = 1:size(v,1)
    for j = 1:size(v,2)
       PC(X(i,1),X(i,2),j) = PC(X(i,1),X(i,2),j) + v(i,j);
    end
end
QC    = zeros(8,8,size(u,2));
for i = 1:size(u,1)
    for j = 1:size(u,2)
       QC(X(i,1),X(i,2),j) = QC(X(i,1),X(i,2),j) + u(i,j);
    end
end

% identify path and place cells that fire above threshold (U)
%--------------------------------------------------------------------------
U     = .8;
jpath = [];
jplac = [];
for i = 1:64
    if sum(spm_vec(PC(:,:,i)) > U) > 2
        jpath(end + 1) = i;
    end
end
for i = (1:64) + 128;
    if sum(spm_vec(PC(:,:,i)) > U) > 0
        jplac(end + 1) = i;
    end
end

% and plot as a function of trajectory in space
%--------------------------------------------------------------------------
x     = spm_conv(X + randn(size(X))/8,2,0);
for i = 1:64
    col{i} = spm_softmax(randn(3,1));
end

% path cells
%--------------------------------------------------------------------------
subplot(4,2,3)
plot(x(:,2),x(:,1),'r:'), hold on
for i = 1:size(x,1)
    for j = 1:3:length(jpath)
        if v(i,jpath(j)) > .80
            plot(x(i,2),x(i,1),'.','MarkerSize',32,'Color',col{j})
        end
    end
end
axis([0 9 0 9]), axis ij square, hold off
title('Path cell responses','fontsize',16)

% please cells
%--------------------------------------------------------------------------
subplot(4,2,4)
plot(x(:,2),x(:,1),'r:'), hold on
for i = 1:size(x,1)
    for j = 1:3:length(jpath)
        if v(i,jplac(j)) > .80
            plot(x(i,2),x(i,1),'.','MarkerSize',32,'Color',col{j})
        end
    end
end
axis([0 9 0 9]), axis ij square, hold off
title('Place cell responses','fontsize',16)

return

function MDP = spm_maze_search(mdp,N,START,END,alpha,beta)
% FORMAT MDP = spm_maze_search(mdp,N,START,END,alpha,beta)
% mdp   - MDP structure
% N     - number of trials (i.e., policies: default 8)
% START - index of intial state (default 1)
% END   - index of target state (default 1)
% alpha - prior concentration parameter for likelihood (default 128)
% beta  - precision of prior preference (default 0)
%the argument is
% MDP   - MDP structure array

% preliminaries
%--------------------------------------------------------------------------
try, N;     catch, N     = 8;   end
try, START; catch, START = 1;   end
try, END;   catch, END   = 1;   end
try, alpha; catch, alpha = 128; end
try, beta;  catch, beta  = 0;   end

% initialise concentration parameters: a (if unspecified)
%--------------------------------------------------------------------------
if ~isfield(mdp,'a')
    mdp.a{1} = ones(size(mdp.A{1}))/8 + mdp.A{1}*alpha;
    mdp.a{2} = mdp.A{2}*128;
end
if ~isfield(mdp,'o')
    mdp.o = [];
end
if ~isfield(mdp,'u')
    mdp.u = [];
end
mdp.s = START;

% Evaluate a sequence of moves - recomputing prior preferences at each move
%==========================================================================
for i = 1:N

    % Evaluate preferred states (subgoals) on the basis of current beliefs
    %----------------------------------------------------------------------
    mdp.C{2} = spm_maze_cost(mdp,END)*beta;

    % proceed with subsequent trial
    %----------------------------------------------------------------------
    MDP(i)   = spm_MDP_VB_X(mdp);
    mdp      = MDP(i);
    mdp.s    = mdp.s(:,end);
    mdp.D{1} = MDP(i).X{1}(:,end);
    mdp.o    = [];
    mdp.u    = [];

end

return

function C = spm_maze_cost(MDP,END)
% Evaluate subgoals using graph Laplacian
%==========================================================================
START = MDP.s(1);
if isfield(MDP,'a')
    Q = MDP.a{1};
else
    Q = MDP.A{1};
end
Q   = Q/diag(sum(Q));
Q   = Q(1,:);                                % open states
P   = diag(Q)*any(MDP.B{1},3);               % allowable transitions
ns  = length(Q);                             % number of states
X   = zeros(ns,1);X(START) = 1;              % initial state
Y   = zeros(ns,1);Y(END)   = 1;              % target state

% Preclude transitions to closed states and evaluate graph Laplacian
%--------------------------------------------------------------------------
P   = P - diag(diag(P));
P   = P - diag(sum(P));
P   = expm(P);

% evaluate (negative) cost as a path integral conjunctions
%--------------------------------------------------------------------------
for t = 1:size(MDP.V,1)
    X = P*X;
end
X     = X > exp(-3);
C     = log(X.*(P*Y) + exp(-32));

return

function spm_maze_plot(MDP,END)
% illustrate  search graphically
%--------------------------------------------------------------------------
A  = spm_vec(MDP(1).A{1}(1,:));
ns = numel(A);
ni = sqrt(ns);
A  = reshape(A,ni,ni);
subplot(2,2,1), imagesc(A), axis image
title('Scanpath','fontsize',16);

% Cycle of the trials
%--------------------------------------------------------------------------
h     = [];
MS    = {};
MC    = {};
for p = 1:numel(MDP)

    %  current beliefs and preferences: A likelihood
    %----------------------------------------------------------------------
    if isfield(MDP,'a')
        Q = MDP(p).a{1};
    else
        Q = MDP(p).A{1};
    end
    Q     = Q/diag(sum(Q));
    Q     = Q(1,:);
    a     = reshape(Q(:),ni,ni);
    subplot(2,2,2), imagesc(a), axis image
    title('Likelihood','fontsize',16);

    %  current beliefs and preferences: B transitions
    %----------------------------------------------------------------------
    try
        b = diag(Q)*any(MDP(p).B{1},3);
    catch
        b = diag(Q)*any(MDP(p).B{1},3);
    end
    subplot(2,2,4), imagesc(-b), axis image
    title('Allowable transitions','fontsize',16);

    %  current beliefs and preferences: C preferences
    %----------------------------------------------------------------------
    C     = MDP(p).C{2}(:,1);
    C     = spm_softmax(C);
    C     = reshape(C,ni,ni);
    subplot(2,2,3), imagesc(C), axis image
    title('Preferences','fontsize',16);
    try
        [i,j] = ind2sub([ni,ni],MDP(p).s(1)); hold on
        plot(j,i,'.','MarkerSize',32,'Color','g');
        [i,j] = ind2sub([ni,ni],END);
        plot(j,i,'.','MarkerSize',32,'Color','r'); hold off
    end

    % cycle over  short-term searches
    %----------------------------------------------------------------------
    subplot(2,2,1),hold on
    s     = MDP(p).s;
    for t = 1:numel(s)

        % location
        %------------------------------------------------------------------
        [i,j] = ind2sub([ni,ni],s(t));
        h(end + 1) = plot(j,i,'.','MarkerSize',32,'Color','r');
        try
            set(h(end - 1),'Color','m','MarkerSize',16);
            j = [get(h(end - 1),'Xdata'), get(h(end),'Xdata')];
            i = [get(h(end - 1),'Ydata'), get(h(end),'Ydata')];
            plot(j,i,':r');
        end

        % save
        %------------------------------------------------------------------
        if numel(MS)
            MS(end + 1) = getframe(gca);
        else
            MS = getframe(gca);
        end

    end

    % save
    %----------------------------------------------------------------------
    subplot(2,2,3)
    if numel(MC)
        MC(end + 1) = getframe(gca);
    else
        MC = getframe(gca);
    end

end

% save movie
%--------------------------------------------------------------------------
subplot(2,2,1)
xlabel('click axis for movie')
set(gca,'Userdata',{MS,16})
set(gca,'ButtonDownFcn','spm_DEM_ButtonDownFcn')

subplot(2,2,3)
xlabel('click axis for movie')
set(gca,'Userdata',{MC,16})
set(gca,'ButtonDownFcn','spm_DEM_ButtonDownFcn')


%%*****************************************************************
%% NTdirfun: compute (dX,dZ), given dy, for the NT direction.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

    function [dX,dy,dZ] = NTdirfun(blk,At,par,Rd,EinvRc,xx,m);

    global solve_ok

    dX = cell(size(blk,1),1); dZ = cell(size(blk,1),1); dy = [];
    if (any(isnan(xx)) | any(isinf(xx)))
       solve_ok = 0;
       fprintf('\n  linsysolve: solution contains NaN or inf.');
       return;
    end
%%
    dy = xx(1:m);
    count = m;
%%
    for p=1:size(blk,1)
       pblk = blk(p,:);
       if strcmp(pblk{1},'l')
          %%dZ{p} = Rd{p} - At{p}*dy;
          dZ(p) = ops(Rd(p),'-',Atyfun(pblk,At(p,:),[],[],dy));
          tmp   = par.dd{p}.*dZ{p};
          dX{p} = EinvRc{p} - tmp;
       elseif strcmp(pblk{1},'q')
          %%dZ{p} = Rd{p} - At{p}*dy;
          dZ(p) = ops(Rd(p),'-',Atyfun(pblk,At(p,:),[],[],dy));
          tmp = par.dd{p}.*dZ{p} + qops(pblk,qops(pblk,dZ{p},par.ee{p},1),par.ee{p},3);
          dX{p} = EinvRc{p} - tmp;
       elseif strcmp(pblk{1},'s')
          %%dZ{p} = Rd{p} - smat(pblk,At{p}*dy(par.permA(p,:)),par.isspAy(p));
          dZ(p) = ops(Rd(p),'-',Atyfun(pblk,At(p,:),par.permA(p,:),par.isspAy(p),dy));
          tmp   = Prod3(pblk,par.W{p},dZ{p},par.W{p},1);
          dX{p} = EinvRc{p}-tmp;
       elseif strcmp(pblk{1},'u');
          n = sum(pblk{2});
          dZ{p} = zeros(n,1);
          dX{p} = xx(count+[1:n]);
          count = count + n;
       end
    end
%%*******************************************************************