%% 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 [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 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')


function volumeRendering(command)
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

global planC stateS;
indexS = planC{end};

[origin, spacing, center] = getScanOriginSpacing(planC{indexS.scan}(stateS.scanSet));

scanData = planC{indexS.scan}(stateS.scanSet).scanArray;
%scanData = planC{indexS.dose}(stateS.doseSet).doseArray;
% spacing = [1 1 3];

switch upper(command)

    case 'MIP'
        colormap = [];
%         scanData=GPReduce2(scanData,2,0);
        sampleRatio = 2;
        scanData = scanData(1:sampleRatio:end, 1:sampleRatio:end, 1:sampleRatio:end);
        Render3D_MIP(uint16(permute(scanData,[3,1,2])),colormap', 1, [spacing(3),spacing(1),spacing(2)]*10, 1);

    case 'MIP2'
        colormap = [];
        cData3M = scanDoseFusion();
        cData3M = permute(cData3M, [4,1,2,3]);
        [m,n,o,p] = size(cData3M);
        cData3M = reshape(cData3M, [n,o,p*m]);
        alphaRatio = 30;
        Render3D_MIP(uint16(cData3M),colormap', 2, spacing*10, alphaRatio);

    case 'VR1'
%         colormap = [  0.0     0.0   0.0   0.0   0.0;  ...
%                        200.0   0     0     0.8   0.02;  ...
%                        400.0   0.0   1.0   0.0   .8;  ...
%                        600.0   1.0   1.0   0.0   .6; ...
%                        800.0   1.0   0.0   0.0   .7; ...
%                       1000.0   0.0   1.0   1.0   1; ];

        [strVol, colormap] = generateStructVolume();
        sagOn  = 0;
        axOn   = 1;
        corOn  = 1;
        skinOn = 0;
        boneOn = 1;
        sagPos = 130;
        axPos  = 30;
        corPos = 100;
        tRange1 = 2500;
        tRange2 = 1500;
        tRange3 = 1500;
        skinAlpha = 0.3;
        boneAlpha = 0.8;
        Render3D_S(uint16(strVol(:,:,20:end)),colormap', 2, spacing, ...
            sagOn, axOn, corOn, skinOn, boneOn, sagPos, axPos, corPos, tRange1, tRange2, tRange3, skinAlpha, boneAlpha);

    case 'VR2'
        colormap = [   0    1    1   1    .0  ...
                      255   1    1   1    .0  ...
                      800   .9   .9  .9   .1  ...
                      1000  .9   .9  .3   .1  ...
                      1010  1   1  1   1  ...
                      1020  1   1  1   1  ...
                      1030  1   1  1   1  ...
                      1040  1   1  1   1  ...
                      1050  .6   .0  .6   .7  ...
                      1060  .6   .0  .6   .7  ...
                      1070  .6   .0  .6   .7  ...
                      1080  .0   .9  .0   1  ...
                      1090  .0   .6  .0   1  ...
                      1100  .0   .3  .0   1  ...
                      1150  .0   .8  .0   .1  ...
                      1200  .0   .4  .0   .9  ...
                      1500  .0   .6  .0   .9  ...
                      2000  .0   .8  .8   .9  ...
                      4000  1    1   1    1.0 ];

%         scanData = smooth3(scanData, 'gaussian');
        Render3D_V(uint16(scanData(:,:,40:end)),colormap', 2, spacing);

end


classdef comp2periodz < ZmapHGridFunction
    % COMP2PERIODZ compares seismicity rates for two time periods
    % The differences are as z- and beta-values and as percent change.

    properties
        periodA_start   datetime % start time for period 1
        periodA_end     datetime % end time for period 1
        periodB_start   datetime % start time for period 2
        periodB_end     datetime % end time for period 2
        binsize duration            = ZmapGlobal.Data.bin_dur;
    end

    properties(Constant)
        PlotTag = 'comp2periodz';
        ReturnDetails = cell2table({ ... VariableNames, VariableDescriptions, VariableUnits
            ...
            ... % these are returned by the calculation function
            'z_value',              'z-value',          '';...           #1 'valueMap'
            'pct_change',           'percent change',   'pct';... #2  'per'
            'beta_value',           'Beta value map',   '';...     #3 'beta_map'
            'Number_of_Events_1',   'Number of events in first period', '';... #4
            'Number_of_Events_2',   'Number of events in second period', '';... #5
            ...
            }, 'VariableNames', {'Names','Descriptions','Units'})

        CalcFields={...
            'z_value',              'pct_change',   'beta_value',...
            'Number_of_Events_1',   'Number_of_Events_2'}

        ParameterableProperties = ["periodA_start" "periodA_end" "periodB_start" "periodB_end" "binsize"];

        References="";
    end

    methods
        function obj=comp2periodz(zap, varargin)
            % COMP2PERIODZ compares seismicity rates for two time periods.
            % The differences are as z- and beta-values and as percent change.
            %   Stefan Wiemer 1/95
            %   Rev. R.Z. 4/2001

            obj@ZmapHGridFunction(zap, 'z_value');
            report_this_filefun();

            t0b = min(obj.RawCatalog.Date);
            teb = max(obj.RawCatalog.Date);
            obj.periodA_start = t0b;
            obj.periodB_end = teb;
            obj.periodA_end = t0b + (teb-t0b)/2;
            obj.periodB_start = obj.periodA_end+minutes(0.01);

            obj.parseParameters(varargin);

            obj.StartProcess();

        end

        function InteractiveSetup(obj)

            % get two time periods, along with grid and event parameters
            zdlg=ZmapDialog();
            zdlg.AddHeader('Please define two time periods to compare');
            zdlg.AddEdit('periodA_start','start period 1',  obj.periodA_start,'start time for period 1');
            zdlg.AddEdit('periodA_end',  'end period 1',    obj.periodA_end,'end time for period 1');
            zdlg.AddEdit('periodB_start','start period 2',  obj.periodB_start,'start time for period 2');
            zdlg.AddEdit('periodB_end',  'end period 2',    obj.periodB_end,'end time for period 2');
            zdlg.AddDurationEdit('binsize','Bin Size',      obj.binsize,'number of days in each bin',@days);
            obj.AddDialogOption(zdlg,'EventSelector');
            zdlg.Create('Name', 'Please choose rate change estimation option','WriteToObj',obj,'OkFcn',@obj.doIt);
        end

        function results=Calculate(obj)

            assert(obj.periodA_start < obj.periodA_end,'Period 1 starts before it ends');
            assert(obj.periodB_start < obj.periodB_end,'Period 2 starts before it ends');

            %  make grid, calculate start- endtime etc.  ...

            lt =  (obj.RawCatalog.Date >= obj.periodA_start &  obj.RawCatalog.Date < obj.periodA_end) ...
                | (obj.RawCatalog.Date >= obj.periodB_start &  obj.RawCatalog.Date <= obj.periodB_end);
            obj.RawCatalog = obj.RawCatalog.subset(lt);

            interval1_bins = obj.periodA_start : obj.binsize : obj.periodA_end; % starts
            interval2_bins = obj.periodB_start : obj.binsize : obj.periodB_end; % starts
            interval1_edges = [interval1_bins, interval1_bins(end)+obj.binsize];
            interval2_edges = [interval2_bins, interval2_bins(end)+obj.binsize];

            obj.gridCalculations(@calculation_function);

            obj.Result.period1.dateRange=[obj.periodA_start obj.periodA_end];
            obj.Result.period2.dateRange=[obj.periodB_start obj.periodB_end];

            if nargout
                results=obj.Result.values;
            end
            % save data

            % plot the results
            % old and valueMap (initially ) is the z-value matrix

            %det =  'ast';
            %ZG.shading_style = 'interp';
            % View the b-value map: view_ratecomp.m
            %    which could create a topography overlay ala dramap_z.m
            %
            %

            function out=calculation_function(catalog)
                % calulate values at a single point
                % calculate distance from center point and sort wrt distance

                idx_back =  catalog.Date >= obj.periodA_start &  catalog.Date < obj.periodA_end ;
                [cumu1, ~] = histcounts(catalog.Date(idx_back),interval1_edges);

                idx_after =  catalog.Date >= obj.periodB_start &  catalog.Date <= obj.periodB_end ;
                [cumu2, ~] = histcounts(catalog.Date(idx_after),interval2_edges);

                mean1 = mean(cumu1);        % mean seismicity rate in first interval
                mean2 = mean(cumu2);        % mean seismicity rate in second interval
                sum1 = sum(cumu1);          % number of earthquakes in the first interval
                sum2 = sum(cumu2);          % number of earthquakes in the second interval
                var1 = cov(cumu1);          % variance of cumu1
                var2 = cov(cumu2);          % variance of cumu2
                % remark (db): cov and var calculate the same value when applied to a vector
                ncu1 = length(interval1_bins);         % number of bins in first interval
                ncu2 = length(interval2_bins);         % number of bins in second interval

                % compute the z value "as":
                as = (mean1 - mean2)/ sqrt(var1/ncu1 +var2/ncu2);

                % calculate the percentage
                per = -((mean1-mean2)./mean1)*100;

                % beta nach reasenberg & simpson 1992, time of second interval normalised by time of first interval
                bet = (sum2-sum1*ncu2/ncu1)/sqrt(sum1*(ncu2/ncu1));

                out = [as  per bet sum1 sum2];

            end
        end
        function ModifyGlobals(obj)
            obj.ZG.bvg = obj.Result.values;
        end
    end %methods

    methods(Static)
        function h = AddMenuItem(parent, zapFcn, varargin)
            % create a menu item
            label = 'Compare two periods (z, beta, probabilty)';
            h =  uimenu(parent, 'Label', label,...
                'MenuSelectedFcn', @(~,~)XYfun.comp2periodz(zapFcn()),...
                varargin{:});
        end
    end % static methods
end % classdef


%%% void show_results(matrix,matrix,matrix,matrix,int,int,int)
% I - Input sequence
% L - Low-rank sequence
% S - Sparse sequence
% O - Outliers sequence
% nFrame - Number of frames
% vidHeight - Video height
% vidWidth - Video width
%
function show_results(I,L,S,O,nFrame,vidHeight,vidWidth)
  warning('off','all');
  clf;
  for i = 1:nFrame
    Input = reshape(I(:,i),vidHeight,vidWidth);
    %Input = uint8(Input);

    if(size(L) == size(I))
      LowRank = reshape(L(:,i),vidHeight,vidWidth);
      %LowRank1 = reshape(L(:,i),vidHeight,vidWidth);
      %LowRank = mat2gray(LowRank1);
      %LowRank = im2uint8(LowRank);
    else
      LowRank = uint8(zeros(size(Input)));
    end

    if(size(S) == size(I))
      Sparse = reshape(S(:,i),vidHeight,vidWidth);
      %Sparse1 = reshape(S(:,i),vidHeight,vidWidth);
      %Sparse = mat2gray(Sparse1);
      %Sparse = im2uint8(Sparse);
    else
      Sparse = uint8(zeros(size(Input)));
    end

    if(~isempty(O))
      Outlier = reshape(O(:,i),vidHeight,vidWidth);
      %Outlier1 = reshape(O(:,i),vidHeight,vidWidth);
      %Outlier = mat2gray(Outlier1);
      %Outlier = im2uint8(Outlier);
      %Outlier = medfilt2(Outlier, [5 5]);
    else
      Outlier = uint8(zeros(size(Input)));
    end

    subplot(2,3,1), imshow(Input,[]), title('Input (I)');
    subplot(2,3,2), imshow(LowRank,[]), title('Low Rank (L)');
    subplot(2,3,3), imshow(Sparse,[]), title('Sparse (S)');
    subplot(2,3,4), imshow(Outlier,[]), title('Outliers (O)');
    subplot(2,3,5), imshow(medfilt2(Outlier, [5 5]),[]), title('Filtered Outliers');
    %disp(i);
    pause(0.01);
  end
  warning('on','all');
end


% Simulating Transducer Field Patterns Example
%
% This example demonstrates the use of k-Wave to compute the field pattern
% generated by a curved single element transducer in two dimensions. It
% builds on the Monopole Point Source In A Homogeneous Propagation Medium
% Example.
%
% author: Bradley Treeby
% date: 10th December 2009
% last update: 24th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

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

clear all;

% =========================================================================
% SIMULATION
% =========================================================================

% create the computational grid
Nx = 216;           % number of grid points in the x (row) direction
Ny = 216;           % number of grid points in the y (column) direction
dx = 50e-3/Nx;    	% grid point spacing in the x direction [m]
dy = dx;            % grid point spacing in the y direction [m]
kgrid = makeGrid(Nx, dx, Ny, dy);

% define the properties of the propagation medium
medium.sound_speed = 1500;  % [m/s]
medium.alpha_power = 1.5;   % [dB/(MHz^y cm)]
medium.alpha_coeff = 0.75;  % [dB/(MHz^y cm)]

% create the time array
[kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed);

% define a curved transducer element
source.p_mask = makeCircle(Nx, Ny, 61, 61, 60, pi/2);

% define a time varying sinusoidal source
source_freq = 0.25e6;       % [Hz]
source_mag = 0.5;           % [Pa]
source.p = source_mag*sin(2*pi*source_freq*kgrid.t_array);

% filter the source to remove high frequencies not supported by the grid
source.p = filterTimeSeries(kgrid, medium, source.p);

% create a display mask to display the transducer
display_mask = source.p_mask;

% create a sensor mask covering the entire computational domain using the
% opposing corners of a rectangle
sensor.mask = [1, 1, Nx, Ny].';

% set the record mode capture the final wave-field and the statistics at
% each sensor point
sensor.record = {'p_final', 'p_max', 'p_rms'};

% assign the input options
input_args = {'DisplayMask', display_mask, 'PMLInside', false, 'PlotPML', false};

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

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

% add the source mask onto the recorded wave-field
sensor_data.p_final(source.p_mask ~= 0) = 1;
sensor_data.p_max(source.p_mask ~= 0) = 1;
sensor_data.p_rms(source.p_mask ~= 0) = 1;

% plot the final wave-field
figure;
subplot(1, 3, 1), imagesc(kgrid.y_vec*1e3, kgrid.x_vec*1e3, sensor_data.p_final, [-1 1]);
colormap(getColorMap);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;
title('Final Wave Field');

% plot the maximum recorded pressure
subplot(1, 3, 2), imagesc(kgrid.y_vec*1e3, kgrid.x_vec*1e3, sensor_data.p_max, [-1 1]);
colormap(getColorMap);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;
title('Maximum Pressure');

% plot the rms recorded pressure
subplot(1, 3, 3), imagesc(kgrid.y_vec*1e3, kgrid.x_vec*1e3, sensor_data.p_rms, [-1 1]);
colormap(getColorMap);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;
title('RMS Pressure');
scaleFig(2, 1);


function [afresults, AfAnalysisWindows, AFfile] = PerformAFdetection(subjectID,trr,rr,sqi,HRVparams)
%   PerformAFdetection(subjectID,trr,rr,HRVparams)
%
%	OVERVIEW:
%       Perform Atrial Fibrillation (AF) detection
%
%   INPUT:
%       subjectID : string containing the identifier of the subject to be analyze
%       trr      : (seconds)a single row of time of the rr interval
%                   data
%       rr        : (seconds) a single row of peak to peak interval
%                   data
%       sqi       : Signal Quality Index; Requires a matrix with
%                   at least two columns. Column 1 should be timestamps
%                   should be timestamps of each sqi measure, and Column 2
%                   should be SQI on a scale from 0 to 1.
%       HRVparam  : struct of settings for hrv_toolbox analysis
%
%   OUTPUT:
%       afresults : a single row containing a flag (1) when AF is
%                   dettected in a window and 0 if no AF
%
%   DEPENDENCIES & LIBRARIES:
%       PhysioNet Cardiovascular Signal Toolbox
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%
%   REFERENCE:
%   Vest et al. "An Open Source Benchmarked HRV Toolbox for Cardiovascular
%   Waveform and Interval Analysis" Physiological Measurement (In Press), 2018.
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Giulia Da Poian (giulia.dap@gmail.com) on Sep 6, 2017.
%       Dependent scripts written by various authors
%       (see functions for details)
%	COPYRIGHT (C) 2018
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Modified on 02.14.2018 to include SQI check before evaluating AF; if a
%   windows contains low SQI signal do not compute AF and mark the window
%   as NaN

if isempty(sqi)
     sqi(:,1) = trr;
     sqi(:,2) = ones(length(trr),1);
end

% 1. Calculate AF Features
AfAnalysisWindows = CreateWindowRRintervals(trr,rr,HRVparams,'af');
NNsamps = rr .* HRVparams.Fs;

AFtest = nan(length(AfAnalysisWindows),1);

for idx = 1:length(AfAnalysisWindows)
    tstart = AfAnalysisWindows(idx);

    if ~isnan(tstart)

        idxInWin = find(trr >= tstart & trr< tstart + HRVparams.af.windowlength);

        % Added by Giulia: exclude from the Analysis low quality
        % segments (SQI< SQI_threshold)
        sqiWin = sqi(sqi(:,1) >= tstart & sqi(:,1) < tstart + HRVparams.af.windowlength,2);
        LowQualityIdxs = find(sqiWin < HRVparams.sqi.LowQualityThreshold);

        % If enough data has an adequate SQI, perform the calculations
        cond1 = (numel(LowQualityIdxs)/length(sqiWin)) <  HRVparams.RejectionThreshold;
        % RR interval time series must be > 12 ans <60 to extract feautures
        cond2 = (length(NNsamps(idxInWin)) > 12 && length(NNsamps(idxInWin)) < 60);

        if (cond1 && cond2)
            features_af = AF_features(NNsamps(idxInWin),HRVparams.Fs);
            AFtest(idx) = SVM_AFdetection_withoutTrainingModel(features_af,1);
        end
    end
end

% 2. Export AF Data as CSV File

afresults = AFtest(:);
afcol_titles = {'AFtest'};

outputType = 'AF';
AFfile = SaveHRVoutput(subjectID,AfAnalysisWindows,afresults, ...
    afcol_titles, outputType, HRVparams, trr, rr);


function AAM = build_model_2d_from_files(directory, varargin)
% AAM = build_model_2d_from_files(directory)
% AAM = build_model_2d_from_files(directory, 'triangulation', triangulation)
%
% FIXME: Mostly a cut/paste from build_model_2d, but this
% method was needed to avoid out-of-memory issues when dealing with large training
% sets.
%
% Builds an Active Appearance Model using shape and appearance data retrieved from
% the given directory.
% Based on the 'Active Appearance Models Revisited' paper by Iain Matthews and Simon Baker.
%
%                         PARAMETERS
%
% directory Path to a directory containing shape and appearance data in the appropriate format.
%           See the examples to see what this format is, or use one of the provided datasets.
%
%                     OPTIONAL PROPERTIES
%
% 'triangulation' A matrix property providing a triangular mesh over the
%                 shape (in the same format as the one returned by delaunay).
%                 If not provided, a triangulation will be determined using
%                 delaunay on the mean shape.
%
%
%                        RETURN VALUE
%
% AAM is a structure with the following fields:
%
% 	s0 The mean shape as a column vector [i1 i2 i3 ... j1 j2 j3 ...]'.
%
%   s  The matrix of base shapes with each shape stored on a column vector
%      with the same structure as s0
%
%   s_star The basis for the global normalizing transform. They are four column
%          vectors whose linear combination allow for translation, scale and
%          rotation of the base shape
%
%   shape_eiv The eigenvalues associated to each shape principal
%             component
%
%   warp_map A modelh-by-modelw lookup table for determining to which
%            triangle warp_map(i,j) the pixel (i,j) belongs. If 0, the
%            pixel is outside the mean shape
%
%   shape_mesh The triangles indexed by warp_map. It is a Tx3 matrix
%              containing indices to vertices in the shape. Look at
%              the delaunay function for further information
%
%   alpha_coords An image containing the alpha barycentric coordinate
%                for pixel (i,j) wrt of the triangle given by
%                shape_mesh(warp_map(i,j),:). Value is undefined if
%                warp_map(i,j) is 0
%
%   beta_coords An image containing the beta barycentric coordinate
%               for pixel (i,j) wrt of the triangle given by
%               shape_mesh(warp_map(i,j),:). Value is undefined if
%               warp_map(i,j) is 0
%
%   adj_list Array of N cells containing indices to the triangles sharing
%            a given vertex. Such that for the k-th cell, the list will
%            contain the indices to the triangles in shape_mesh sharing
%            the k-th vertex.
%
%   A0 Mean appearance as a column vector, sorted firstly by color
%      component, then j coordinate and finally i coordinate
%      Example:
%       X1 = [ 1 2; 3 4];
%       X2 = [ 5 6; 7 8];
%       X3 = [ 9 10; 11 12];
%       X(:,:,1)=X1;
%       X(:,:,2)=X2;
%       X(:,:,3)=X3;
%       reshape(X,[],1) =>
%          _________
%        1  j==1
%        3 _____   C1
%        2  j==2
%        4 _________
%        5  j==1
%        7 _____   C2
%        6  j==2
%        8 _________
%        9  j==1
%       11 _____   C3
%       10  j==2
%       12 _________
%
%   A  The matrix of base appearances, with each appearance stored on
%      the colomuns in the same manner as A0
%
%   app_eiv The eigenvalues associated to each appearance principal
%           component
%
%   dA0 The gradient of the mean appearance stored such that for pixel (i,j),
%       dA0(i,j,c,1) gives the i direction of the gradient for color c
%       and dA0(i,j,c,2) gives the j direction of the gradient for color c
%
%   dW_dp Jacobian of the warp with respect to the shape parameters p
%
%   dN_dq Jacobian of the global normalizing warp with respect of the
%         global transform parameters q
%
%   SD Steepest descent images. They are stored as row vectors (of the same length
%      as A0). The first 4 images are associated with the q
%      paramenters, the other are associated with the p parameters
%
%   H The hessian, it's a square NxN matrix where N is the number of steepest
%     descent images (or, equivalently, the sum of the number of p and
%     q parameters)
%
%   invH Inverse hessian, it's a square NxN matrix where N is the number of steepest
%     descent images (or, equivalently, the sum of the number of p and
%     q parameters)
%
%   R Simply the product between the inverse hessian and the steepest descent images,
%     invH * SD. Encodes the linear relationship between the error image and the incremental
%     parameters.
%
% Author: Luca Vezzaro (elvezzaro@gmail.com)

	shape_triangles = zeros(0, 3, 'uint32');

	i = 1;
	while i < numel(varargin)
		if strcmp(varargin{i}, 'triangulation')
			% Be sure the triangle indices are unsigned integers.
			shape_triangles = uint32(varargin{i+1});
		else
			error(sprintf('Unsupported property: %s.', varargin{i}));
		end

		i = i + 2;
	end

	tic;

	landmark_files = dir(sprintf('%s/*.mat', directory));
	disp 'Number of shapes:'
	% size of training set
	ns = numel(landmark_files)

	for j=1:ns
		% Load associated landmarks
		load(sprintf('%s/%s', directory, landmark_files(j).name));
		info = imfinfo(sprintf('%s/%s', directory, landmark_files(j).name(1:end-4)));
		shape_data(:,:,j) = xy2ij(annotations, info.Height);
	end

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

	% use first shape as initial mean
	mean_shape = shape_data(:,:,1);

	reference_shape = mean_shape;

	% Matrix containing the aligned shapes
	aligned_data = shape_data;

	% Less iterations should be enough, but better safe than sorry, efficiency
	% is not an issue here
	for it=1:100
		for i=1:ns
			% Align each shape to the mean
			[ D Y ] = procrustes(mean_shape, aligned_data(:,:,i));
			aligned_data(:,:,i) = Y;
		end

		% New mean shape, store in a different variable so we can compare it to
		% the old one
		new_mean_shape = mean(aligned_data, 3);
		[ D Y ] = procrustes(reference_shape, new_mean_shape);
		new_mean_shape = Y;

		mean_shape = new_mean_shape;
	end

	mean_shape = mean(aligned_data, 3);

	% Determine the region of interest
	mini = min(mean_shape(:,1));
	minj = min(mean_shape(:,2));
	maxi = max(mean_shape(:,1));
	maxj = max(mean_shape(:,2));

	% Place the origin in the upper left corner of the rectangle
	% representing the bounding box of the mean shape. Add a 1
	% pixel offset to avoid artifacts in gradient computations.
	mean_shape = mean_shape - repmat([mini - 2, minj - 2], [np, 1]);

	disp 'Width of model:'
	% Determine width and height of the model, add 1 pixel offset
	% to avoid gradient artifacts
	modelw = ceil(maxj - minj + 3)
	disp 'Height of model:'
	modelh = ceil(maxi - mini + 3)

	disp 'Time required for shape data alignment:'

	toc

	%%
	%% DEBUG STUFF
	%%
%	for i=1:ns
%		plot(aligned_data(:,2,i), aligned_data(:,1,i), 'o');
%		hold on;
%		pause;
%	end
%	plot(mean_shape(:,2), mean_shape(:,1), 'rx');
%	hold off;
%	return;

	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% Definitive mean shape for AAM, stored on a column vector [i1 i2 i3 ... j1 j2 j3 ...]'
	AAM.s0 = reshape(mean_shape, 2*np, 1);

	% Prepare shape data for PCA
	shape_matrix = reshape(aligned_data, [2*np ns]) - repmat(AAM.s0, [1 ns]);
	clear aligned_data;

	% Get modes covering 98% of variation
	[pc eiv] = var_pca(shape_matrix, 0.98);
	clear shape_matrix;

	disp 'Number of Shape Modes:'
	size(pc, 2)

	AAM.shape_eiv = eiv;

	% Build the basis for the global shape transform, we do it here because
	% they are used to orthonormalize the shape principal vectors
	% It is done differently to the paper as we're using a different coordinate
	% frame. Here u -> i, v -> j
	s1_star = AAM.s0;
	s2_star(1:np) = -AAM.s0(np+1:end);
	s2_star(np+1:2*np) = AAM.s0(1:np);
	s3_star(1:np) = ones(np,1);
	s3_star(np+1:2*np) = zeros(np,1);
	s4_star(1:np) = zeros(np,1);
	s4_star(np+1:2*np) = ones(np,1);

	% Stack the basis we found before with the shape basis so
	% we can orthonormalize
	s_star_pc(:,1) = s1_star;
	s_star_pc(:,2) = s2_star;
	s_star_pc(:,3) = s3_star;
	s_star_pc(:,4) = s4_star;

	s_star_pc(:,5:size(pc,2)+4) = pc;

	% Orthogonalize the basis (should already be close to orthogonal)
	s_star_pc = gs_orthonorm(s_star_pc);

	% Basis for the global shape transform
	AAM.s_star = s_star_pc(:,1:4);
	% Basis for the shape model
	AAM.s = s_star_pc(:,5:end);

	disp 'Time required for building shape model:'
	toc

	%%
	%% DEBUG STUFF
	%%

%	disp 'Plotting principal vectors as deformation directions of the mean...'
%
%	plot_s0 = reshape(AAM.s0, [np 2]);
%
%	for i=1:size(AAM.s, 2)
%		hold off;
%		plot(plot_s0(:,2), plot_s0(:,1), 'o');
%		hold on;
%
%		fprintf('Principal vector %d\n.', i);
%
%		delta_s = reshape(AAM.s(:,i), [np 2]);
%		plot_s = plot_s0 + delta_s * 16;
%
%		for j=1:5
%			plot(plot_s(:,2), plot_s(:,1), 'r.');
%			plot_s = plot_s + delta_s * 16;
%		end
%		hold off;
%		disp 'Waiting for keypress...'
%		pause;
%	end
%	return;

	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% Check if a triangulation was already provided
	if size(shape_triangles, 1) == 0
		% Triangle mesh used for warping. We determine it on the base shape.
		% Returned result is double, convert to unsigned integer.
		shape_triangles = delaunay(AAM.s0(np+1:2*np)', AAM.s0(1:np)');
		%triplot(shape_triangles, AAM.s0(1:np)', AAM.s0(np+1:2*np)');
	end

	% Bitmap mapping pixels to the triangle they belong to (the triangle used
	% for warping)
	warp_map = zeros(modelh, modelw, 'uint32');

	% Bitmap containing the first barycentric coordinate of each pixel wrt to the
	% triangle specified by warp_map
	alpha_coords = zeros(modelh, modelw);

	% Bitmap containing the second barycentric coordinate of each pixel wrt to the
	% triangle specified by warp_map
	beta_coords = zeros(modelh, modelw);

	% For each pixel, find the triangle it belongs to
	for j=1:modelw
		for i=1:modelh
			% For each triangle
			for k=1:size(shape_triangles, 1)
				t = shape_triangles(k,:);

				% Vertices of the triangle in the mean shape
				i1 = AAM.s0(t(1));
				j1 = AAM.s0(np + t(1));
				i2 = AAM.s0(t(2));
				j2 = AAM.s0(np + t(2));
				i3 = AAM.s0(t(3));
				j3 = AAM.s0(np + t(3));

				% Compute the two barjcentric coordinates
				den = (i2 - i1) * (j3 - j1) - (j2 - j1) * (i3 - i1);
				alpha = ((i - i1) * (j3 - j1) - (j - j1) * (i3 - i1)) / den;
				beta = ((j - j1) * (i2 - i1) - (i - i1) * (j2 - j1)) / den;

				if alpha >= 0 && beta >= 0 && (alpha + beta) <= 1
					% Found the triangle, save data to the bitmaps and break
					warp_map(i,j) = k;
					alpha_coords(i,j) = alpha;
					beta_coords(i,j) = beta;
					break;
				end
			end
		end
	end

	% Build adjacency list
	AAM.adj_list = cell(np, 1);

	for i=1:size(shape_triangles, 1)
		for j=1:3
			v = shape_triangles(i,j);
			AAM.adj_list{v} = [AAM.adj_list{v} i];
		end
	end

	% IMPORTANT: If the datatype is changed from 'uint32', the mex-file will
	% need to be changed!
	AAM.warp_map = uint32(warp_map);
	AAM.shape_mesh = uint32(shape_triangles);
	AAM.alpha_coords = alpha_coords;
	AAM.beta_coords = beta_coords;

	disp 'Time required for warp pre-computations:'
	toc

	%%
	%% DEBUG STUFF
	%%
%	color_map = rand(max(max(warp_map)), 3);
%	figure(1)
%	imshow(warp_map, color_map);
%
%	figure(2)
%	imshow(alpha_coords);
%
%	figure(3)
%	imshow(beta_coords);
%
%	return;

	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% FIXME: This is somewhat of an hack, but the code is so slow when matrices grow inside loops...
	app = double(imread(sprintf('%s/%s', directory, landmark_files(1).name(1:end-4)))) / 255;
	app_matrix = zeros(modelw*modelh*size(app,3), ns);

	disp 'Color components:'
	nc = size(app, 3)

	for i=1:ns
		% Prepare appearance data
		app = double(imread(sprintf('%s/%s', directory, landmark_files(i).name(1:end-4)))) / 255;
		%app = convn(convn(app, exp(-(-5:5).^2)./sum(exp(-(-5:5).^2)), 'same'), exp(-(-5:5).^2)./sum(exp(-(-5:5).^2))', 'same');

		app_matrix(:,i) = reshape(pa_warp(AAM, shape_data(:,:,i), app), [(modelw*modelh*nc) 1]);
	end

	disp 'Time required for warping all input appearances:'
	toc

	clear app;
	clear landmark_files;

	%%
	%% DEBUG STUFF
	%%

%disp 'Displaying warped images...'
%
%for i=1:ns
%	fprintf('Image %d\n.', i);
%
%	imshow(reshape(app_matrix(:,i), [modelh modelw nc]));
%
%	hold on;
%
%	triplot(shape_triangles, AAM.s0(np+1:2*np)', AAM.s0(1:np)');
%	hold off;
%
%	disp 'Waiting for keypress...'
%	pause;
%end
%hold off;
%return;

	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% FIXME: Should we normalize pixel values like done by Stegmann to
	% improve the generalization of the model?

	% Mean appearance as a column vector, sorted firstly by color
	% component, then j coordinate and finally i coordinate
	%Example:
	%       X1 = [ 1 2; 3 4];
	%       X2 = [ 5 6; 7 8];
	%       X3 = [ 9 10; 11 12];
	%       X(:,:,1)=X1;
	%       X(:,:,2)=X2;
	%       X(:,:,3)=X3;
	%       reshape(X,[],1) =>
	%          _________
	%        1  j==1
	%        3 _____   C1
	%        2  j==2
	%        4 _________
	%        5  j==1
	%        7 _____   C2
	%        6  j==2
	%        8 _________
	%        9  j==1
	%       11 _____   C3
	%       10  j==2
	%       12 _________

	AAM.A0 = mean(app_matrix, 2);
	mean_app = reshape(AAM.A0, [modelh modelw nc]);

	%app_size = modelw * modelh * nc;

	% Prepare apperances for PCA
	app_matrix = app_matrix - repmat(AAM.A0, [1 ns]);

	% FIXME: Goes out of memory without 'econ' flag, look at "An Introduction to Active
	% Shape Models" by Cootes for an optimized PCA
	%[pc eiv] = var_pca(app_matrix, 0.95);
  [pc eiv] = app_pca(app_matrix, 0.95);

	clear app_matrix;

	disp 'Number of Appearance Modes:'
	size(pc, 2)

	AAM.A = pc;
	AAM.app_eiv = eiv;

	disp 'Time required for building apperance model:'
	toc

	%%
	%% DEBUG STUFF
	%%

%  disp 'Displaying results of PCA on appearances...'
%
%	for i=1:size(AAM.A,2)
%   cur_app = reshape(AAM.A(:,i), [modelh modelw nc]);
%
%		fprintf('Showing mean appearance and sums of mean and principal component %d with increasing weight\n.', i);
%
%		subplot(1,3,1), imshow(mean_app)
%		subplot(1,3,2), imshow(mean_app + cur_app*sqrt(eiv(i))*3)
%		subplot(1,3,3), imshow(mean_app - cur_app*sqrt(eiv(i))*3)
%		%subplot(1,4,4), imshow(mean_app + cur_app*sqrt(eiv(i))*3)
%
%		disp 'Waiting for keypress...'
%		pause;
%	end
%	hold off;
%	return;
	%%
	%% END OF DEBUG STUFF
	%%

	tic

	% Now we get into the interesting (and tricky) stuff

	for i=1:nc
		% For each color, estimate the gradient of the mean appearance.
		% dcol is the estimated derivative wrt to columns (which for us is the
		% j coordinate), drow is wrt to rows (which for us is the i coordinate)
		[di dj] = gradient_2d(mean_app(:,:,i), AAM.warp_map);
		mean_app_gradient(:,:,i,1) = di;
		mean_app_gradient(:,:,i,2) = dj;
	end

	AAM.dA0 = mean_app_gradient;

	%%
	%% DEBUG STUFF
	%%
%	figure(1)
%imshow(mean_app);
%	mn = min(min(min(min(AAM.dA0))));
%	mx = max(max(max(max(AAM.dA0))));
%
%	plot_dA0 = (AAM.dA0 - repmat(mn, [modelh modelw nc 2])) / (mx - mn);
%	figure(2)
%	imshow(plot_dA0(:,:,:,1));
%	figure(3);
%	imshow(plot_dA0(:,:,:,2));
%	return;
	%%
	%% END OF DEBUG STUFF
	%%

	disp 'Time required for calculating image gradient:'
	toc

	tic

	dW_dp = zeros(modelh, modelw, 2, size(AAM.s,2));
	dN_dq = zeros(modelh, modelw, 2, 4);

	for j=1:modelw
		for i=1:modelh
			if warp_map(i,j) ~= 0
				% Only the vertices of the triangle containing the pixel are of relevance
				% in determining the Jacobian.
				t = shape_triangles(warp_map(i,j),:);

				% FIXME: this is not how it should be done, there is a way to juggle
				% with the barycentric coordinates we already computed to do the same
				for k=1:3
					% Derivative of vertex 'k' wrt to the shape parameters
					dik_dp = AAM.s(t(k),:);
					djk_dp = AAM.s(t(k)+np,:);

					% Derivative of vertex 'k' wrt to the global transformation parameters
					dik_dq = AAM.s_star(t(k),:);
					djk_dq = AAM.s_star(t(k)+np,:);

					% Now we need the barycentric coordinates of (i,j) computed using
					% point 'k' as the origin. So we rearrange the order of the vertices
					% (it doesn't matter if the result has clockwise or counterclockwise
					% winding)
					t2 = t;
					t2(1) = t(k);
					t2(k) = t(1);

						% Vertices of the triangle in the mean shape
					i1 = AAM.s0(t2(1));
					j1 = AAM.s0(np + t2(1));
					i2 = AAM.s0(t2(2));
					j2 = AAM.s0(np + t2(2));
					i3 = AAM.s0(t2(3));
					j3 = AAM.s0(np + t2(3));

					% Compute the two barycentric coordinates
					den = (i2 - i1) * (j3 - j1) - (j2 - j1) * (i3 - i1);
					alpha = ((i - i1) * (j3 - j1) - (j - j1) * (i3 - i1)) / den;
					beta = ((j - j1) * (i2 - i1) - (i - i1) * (j2 - j1)) / den;

					% Nonzero portion of the Jacobian of the warp wrt to the vertex 'k'.
					% TODO: Isn't this the third barycentric coordinate? It should be
					% easier and faster to compute it directly.
					dW_dij = 1 - alpha - beta;

					% This is how it's formulated on the paper (dW_dq is similar):
					% dW_dp = dW_dp + [dW_dij; 0] * dik_dp + [0; dW_dij] * djk_dp;
					% MATLAB does weird things with dimensions here, so we need squeeze()
					% to make sure the submatrix is actually a 2xn matrix and not a
					% 1x1x2xn
					dW_dp(i,j,:,:) = squeeze(dW_dp(i,j,:,:)) + dW_dij * [dik_dp; djk_dp];
					dN_dq(i,j,:,:) = squeeze(dN_dq(i,j,:,:)) + dW_dij * [dik_dq; djk_dq];
				end
			end
		end
	end

	disp 'Time required for calculating warp Jacobians:'
	toc

	%%
	%% DEBUG STUFF
	%%
%  disp 'Displaying the warp Jacobian...'
%
%	for i=1:size(dW_dp,4)
%	  jacobian_i = dW_dp(:,:,1,i);
%	  jacobian_j = dW_dp(:,:,2,i);
%
%
%		fprintf('Showing warp jacobian i and j components for parameter %d\n.', i);
%
%		subplot(1,2,1), imshow(jacobian_i)
%		subplot(1,2,2), imshow(jacobian_j)
%
%		disp 'Waiting for keypress...'
%		pause;
%	end
%	return;
	%%
	%% END OF DEBUG STUFF
	%%

	tic

	app_modes = reshape(AAM.A, [modelh modelw nc size(AAM.A, 2)]);

	SD = zeros(modelh, modelw, nc, 4 + size(dW_dp, 4));

	% Compute steepest descent images for the 4 global transformation parameters
	% TODO: the loops over c are probably optimizable, but the number of dimensions involved
	% are starting to hurt my head...
	% TODO: test me
	for i=1:4
		prj_diff = zeros(nc, size(AAM.A, 2));
		for j=1:size(AAM.A, 2)
			for c=1:nc
				prj_diff(c,j) = sum(sum(app_modes(:,:,c,j) .* (AAM.dA0(:,:,c,1) .* dN_dq(:,:,1,i) + AAM.dA0(:,:,c,2) .* dN_dq(:,:,2,i))));
			end
		end

		for c=1:nc
			SD(:,:,c,i) = AAM.dA0(:,:,c,1) .* dN_dq(:,:,1,i) + AAM.dA0(:,:,c,2) .* dN_dq(:,:,2,i);
		end

		for j=1:size(AAM.A, 2)
			for c=1:nc
				SD(:,:,c,i) = SD(:,:,c,i) - prj_diff(c,j) * app_modes(:,:,c,j);
			end
		end
	end

	% Compute steepest descent images for the shape parameters
	% TODO: the loops over c are probably optimizable, but the number of dimensions involved
	% are starting to hurt my head...
	% TODO: test me
	for i=1:size(dW_dp, 4)
		prj_diff = zeros(nc, size(AAM.A, 2));
		for j=1:size(AAM.A, 2)
			for c=1:nc
				prj_diff(c,j) = sum(sum(app_modes(:,:,c,j) .* (AAM.dA0(:,:,c,1) .* dW_dp(:,:,1,i) + AAM.dA0(:,:,c,2) .* dW_dp(:,:,2,i))));
			end
		end

		for c=1:nc
			SD(:,:,c,i+4) = AAM.dA0(:,:,c,1) .* dW_dp(:,:,1,i) + AAM.dA0(:,:,c,2) .* dW_dp(:,:,2,i);
		end

		for j=1:size(AAM.A, 2)
			for c=1:nc
				SD(:,:,c,i+4) = SD(:,:,c,i+4) - prj_diff(c,j) * app_modes(:,:,c,j);
			end
		end
	end

	AAM.SD = zeros(size(SD, 4), size(AAM.A,1));

	% FIXME: A posteriori optimization fix, integrate it more nicely with the code.
	for i=1:size(SD, 4)
		AAM.SD(i,:) = reshape(SD(:,:,:,i), 1, []);
	end

	%%
	%% DEBUG STUFF
	%%
%	mn = min(min(min(min(SD))));
%	mx = max(max(max(max(SD))));
%	SD = (SD - repmat(mn, [modelh modelw nc size(SD, 4)])) / (mx - mn);
%	for i=1:size(SD,4)
%		h=figure(i+1);
%		imshow(SD(:,:,:,i));
%	end
	%%
	%% END OF DEBUG STUFF
	%%

	clear SD;

	disp 'Time required for calculating steepest descent images:'
	toc

	tic

	% Compute the Hessian, still not sure if this is the right
	% way to treat the color information (using greyscale images
	% give pretty much the same matching results so it should be ok)
	AAM.H = AAM.SD * AAM.SD';

	AAM.invH = inv(AAM.H);

	AAM.R = AAM.invH * AAM.SD;

	disp 'Time required for determining and inverting Jacobian and calculating invH*SD:'
	toc


% Q = vbfa(D, Y, W_module, X_module, noise_module, ...)
%
% Variational Bayesian (VB) factor analysis (FA) learning algorithm with
% changeable modules for the latent variables.
%
% Optional parameters:
% 'maxiter'             100
% 'update_x'            1
% 'update_w'            1
% 'update_noise'        1
% 'rotate'              1
% 'rotation_checkgrad'  false
% 'rotation_show'       false
% 'rotation_maxiter'    10
% 'debug'               false
% 'autosave'            false
% 'autosavefile'        'vbfa_autosave'

function Q = vbfa(D, Y, W_module, X_module, noise_module, varargin)

[M,N] = size(Y);

options = struct('maxiter', 100, ...
                 'update_x', 1, ...
                 'update_w', 1, ...
                 'update_noise', 1, ...
                 'initialize_x', true, ...
                 'initialize_w', true, ...
                 'initialize_noise', true, ...
                 'rotate', 1, ...
                 'rotation_checkgrad', false, ...
                 'rotation_show', false, ...
                 'rotation_maxiter', 10, ...
                 'loglikelihood', true, ...
                 'debug', false, ...
                 'autosave', false,...
                 'autosavefile', 'vbfa_autosave');

[options, errmsg] = argparse(options, varargin{:});
error(errmsg);

% Initialize
disp('Initializing variables..')
if options.initialize_w
  [Q.W,Q.CovW,Q.rhow] = W_module.initialize(D,M);
else
  QW = W_module.get_struct();
  Q.W = QW.X;
  Q.CovW = QW.CovX;
  Q.rhow = QW.rho;
end
if options.initialize_x
  [Q.X,Q.CovX,Q.rhox] = X_module.initialize(D,N);
else
  QX = W_module.get_struct();
  Q.X = QX.X;
  Q.CovX = QX.CovX;
  Q.rhox = QX.rho;
end
if options.initialize_noise
  [Q.Tau] = noise_module.initialize(M,N);
else
  QTau = noise_module.get_struct();
  Q.Tau = QTau.Tau;
end
%Q.W_module = W_module;
%Q.X_module = X_module;
%Q.noise_module = noise_module;

% Observed values
Obs = ~isnan(Y);
MN_obs = sum(Obs(:));

%%
%% VB learning

loglikelihood_old = -inf;
Q.loglikelihood = nan(options.maxiter,1);

Q.Y = Y;

% Replace missing values with zeros for computational reasons
Y(~Obs) = 0;

KL_W = -inf;
KL_X = -inf;
KL_Tau = -inf;

% Debugging stuff:
klw = nan(options.maxiter,1);
klx = nan(options.maxiter,1);
kltau = nan(options.maxiter,1);
loglike = nan(options.maxiter,1);

%f = zeros(4,options.maxiter);

disp('Starting the VB iteration..')
for ind=1:options.maxiter
  Q.ind = ind;

  startitercpu = cputime;

  %
  % Update variables
  %

  Tau = Q.Tau;
  Tau(~Obs) = 0;

  if index_selected(Q.ind, options.update_x)
    if index_selected(Q.ind, options.debug)
      disp('Update X')
    end
    %[U,Z] = projection(Y,Obs,W,CovW,Tau);
    [Q.X,Q.CovX,Q.rhox,logrhox,KL_X] = X_module.update(Q.ind, Y, Obs, Q.W, ...
                                                      Q.CovW, Q.rhow, Tau);
  end

  if index_selected(Q.ind, options.update_w)
    if index_selected(Q.ind, options.debug)
      disp('Update W')
    end
    %[U,Z] = projection(Y',Obs',X,CovX,Tau');
    [Q.W,Q.CovW,Q.rhow,logrhow,KL_W] = W_module.update(Q.ind, Y', Obs', Q.X, ...
                                                      Q.CovX, Q.rhox, Tau');
  end

  % Compute squared errors <(y_mn-w_m*x_n)^2>
  % (used by noise update and loglikelihood)
  E2 = zeros(size(Y));
  Yh = Q.W'*Q.X;
  E2 = Y.*Y;
  E2 = E2 - 2*Y.*Yh;
  E2 = spdiag(Q.rhow)*E2*spdiag(Q.rhox);
  if ndims(Q.CovW) == 2 && ndims(Q.CovX) == 2
    E2 = E2 ...
         + spdiag(Q.rhow)*(Yh.^2)*spdiag(Q.rhox) ...
         + spdiag(Q.rhow)*(Q.W'.^2)*Q.CovX ...
         + Q.CovW'*(Q.X.^2)*spdiag(Q.rhox) ...
         + Q.CovW'*Q.CovX;
  elseif ndims(Q.CovW) == 3 && ndims(Q.CovX) == 3
    xx = bsxfun(@times, reshape(Q.X*spdiag(Q.rhox),[D,1,N]), reshape(Q.X,[1,D,N])) ...
         + Q.CovX;
    xx = reshape(xx, [D*D,N]);
    ww = bsxfun(@times, reshape(Q.W*spdiag(Q.rhow),[D,1,M]), reshape(Q.W,[1,D,M])) ...
         + Q.CovW;
    ww = reshape(ww, [D*D,M]);
    E2 = E2 + ww'*xx;
  else
    % TODO: Optimize this..
    warning('Optimize this.. and is rho properly here??');
    for m=1:M
      for n=1:N
        if Obs(m,n)
          if ndims(Q.CovW) == 2
            ww = Q.W(:,m)*Q.W(:,m)' + diag(Q.CovW(:,m));
          else
            ww = Q.W(:,m)*Q.W(:,m)' + Q.CovW(:,:,m);
          end
          if ndims(Q.CovX) == 2
            xx = Q.X(:,n)*Q.X(:,n)' + diag(Q.CovX(:,n));
          else
            xx = Q.X(:,n)*Q.X(:,n)' + Q.CovX(:,:,n);
          end
          %WX_WX = WX_WX + traceprod(ww, xx);
          E2(m,n) = E2(m,n) + traceprod(ww, xx);
        end
      end
    end
  end
  E2(~Obs) = 0;

  if index_selected(Q.ind, options.update_noise)
    if index_selected(Q.ind, options.debug)
      disp('Update Tau')
    end

    [Q.Tau,LogTau,KL_Tau] = noise_module.update(Q.ind, E2, Obs);
%    [Tau,lowerbound] = noise_module.update(Y, Obs, v_W, W, CovW, v_X, X, CovX);
  end

  %
  % Rotate
  %

  if index_selected(Q.ind, options.rotate)

    % TODO: You could optimize the hyperparameters at the same time?
    disp('Rotating..')
    A = eye(D);

    if index_selected(Q.ind, options.rotation_checkgrad)
      mycheckgrad(@rotation_cost, A(:) + 0.5*randn(D^2,1), 1e-3, W_module, ...
                  X_module);
    end
    A = minimize(A(:), @rotation_cost, options.rotation_maxiter, W_module, ...
                 X_module);
    A = reshape(A, [D D]);
    if index_selected(Q.ind, options.rotation_show)
      A
    end
    [Q.W, Q.CovW] = W_module.rotate(A);
    [Q.X, Q.CovX] = X_module.rotate(inv(A)');

  end

  %
  % Evaluate VB lower bound
  %

  if index_selected(Q.ind, options.loglikelihood)
  % Likelihood part: <log p(Y|...)>
  logpdf_y = gaussian_logpdf(Q.Tau(Obs)'*E2(Obs), ...
                             0, ...
                             0, ...
                             -sum(LogTau(Obs))-logrhow'*sum(Obs,2)-sum(Obs,1)*logrhox, ...
                             MN_obs);

  % Debugging stuff
  klw(Q.ind) = -KL_W;
  klx(Q.ind) = -KL_X;
  kltau(Q.ind) = -KL_Tau;
  loglike(Q.ind) = logpdf_y;

  % Lower bound
  Q.loglikelihood(Q.ind) = logpdf_y - KL_W - KL_X - KL_Tau;

  if Q.loglikelihood(Q.ind) < loglikelihood_old
    warning(sprintf('Loglikelihood lower bound decreased relatively %e!', ...
                    (loglikelihood_old - Q.loglikelihood(Q.ind)) / ...
                    loglikelihood_old));
    % plot([Q.loglikelihood, klw, klx, kltau, loglike]);
  end

  fprintf('Iteration step %d: loglikelihood=%e (%.2f seconds)\n', Q.ind, ...
          Q.loglikelihood(Q.ind), cputime()-startitercpu);

  loglikelihood_old = Q.loglikelihood(Q.ind);
  end

  if index_selected(Q.ind, options.autosave)
    Q.W_struct = W_module.get_struct();
    Q.X_struct = X_module.get_struct();
    Q.Tau_struct = noise_module.get_struct();
    fprintf('Saving results to %s...', options.autosavefile);
    save(options.autosavefile, '-struct', 'Q');
    fprintf(' done.\n');
  end

end

function [c, dc] = rotation_cost(a, W_module, X_module)
N = sqrt(length(a));
A = reshape(a, N, N);
[U,S,V] = svd(A);
invS = diag(1./diag(S));
invAt = U*invS*V';
[c_w, dc_w] = W_module.rotation_cost(A, U, S, V);
[c_x, dc_x] = X_module.rotation_cost(invAt, U, invS, V);
dc_x = -invAt*dc_x'*invAt;
c = c_w + c_x;
dc = dc_w(:) + dc_x(:);


%Starts from the current decoded state, takes input as minimum distance to
%reach that state and previous state And returns previous state and decoded
%information bit corresponding to that state.

function [prev_state,decoded_bit]=prev_stage(curr_state,distance_prev,metric)
    if(curr_state==1)
        if(distance_prev(1)+metric(1) <= distance_prev(3)+metric(5))
            prev_state=1;decoded_bit=0;
        else
            prev_state=3;decoded_bit=0;
        end
    end

    if(curr_state==2)
        if(distance_prev(1)+metric(2) <= distance_prev(3)+metric(6))
            prev_state=1;decoded_bit=1;
        else
            prev_state=3;decoded_bit=1;
        end
    end

    if(curr_state==3)
        if(distance_prev(2)+metric(3) <= distance_prev(4)+metric(7))
            prev_state=2;decoded_bit=0;
        else
            prev_state=4;decoded_bit=0;
        end
    end

    if(curr_state==4)
        if(distance_prev(2)+metric(4) <= distance_prev(4)+metric(8))
            prev_state=2;decoded_bit=1;
        else
            prev_state=4;decoded_bit=1;
        end
    end

end