%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%    EPSON C8.
%
% The author of this script is:
%		  Silvia Moreno Serrano
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

robot.name= '6AXIS_C8';

%Path where everything is stored for this robot
robot.path = 'robots/EPSON/6AXIS_C8';

robot.DH.theta='[q(1)+pi/2 q(2)+pi/2 q(3) q(4) q(5) q(6)]';
robot.DH.d='[0.472 0 0 0.310 0 0.080]';
robot.DH.a='[0.100 0.300 0.03 0 0 0]';
robot.DH.alpha='[pi/2 0 pi/2 -pi/2 pi/2 0]';

robot.J=[];

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

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

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

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-240) deg2rad(240); %Axis 1, minimum, maximum
                deg2rad(-158) deg2rad(65); %Axis 2, minimum, maximum
                deg2rad(-61) deg2rad(202); %Axis 3
                deg2rad(-200) deg2rad(200); %Axis 4: Unlimited (400? default)
                deg2rad(-135) deg2rad(135); %Axis 5
                deg2rad(-360) deg2rad(360)]; %Axis 6: Really Unlimited to (800? default)

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(331); %Axis 1, rad/s
                deg2rad(332); %Axis 2, rad/s
                deg2rad(450); %Axis 3, rad/s
                deg2rad(450); %Axis 4, rad/s
                deg2rad(450); %Axis 5, rad/s
                deg2rad(720)];%Axis 6, rad/s

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

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

%base reference system
robot.T0 = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; %%cambiar para dar la vuelta?

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

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DYNAMIC PARAMETERS
%   WARNING! These parameters do not correspond to the actual IRB 140
%   robot. They have been introduced to demonstrate the necessity of
%   simulating the robot and should be used only for educational purposes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

%consider friction in the computations
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[22 13 6 5 2 1];

%COM of each link with respect to own reference system
robot.dynamics.r_com=[0       0          0; %(rx, ry, rz) link 1
                     -0.05	 0.006	 0.1; %(rx, ry, rz) link 2
                    -0.0203	-0.0141	 0.070;  %(rx, ry, rz) link 3
                     0       0.019       0;%(rx, ry, rz) link 4
                     0       0           0;%(rx, ry, rz) link 5
                     0       0         0.032];%(rx, ry, rz) link 6

%Inertia matrices of each link with respect to its D-H reference system.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, for each row
robot.dynamics.Inertia=[0      0.35	0   	0	0	0;
    .13     .524	.539	0	0	0;
    .066	.086	.0125	0	0	0;
    1.8e-3	1.3e-3	1.8e-3	0	0	0;
    .3e-3	.4e-3	.3e-3	0	0	0;
    .15e-3	.15e-3	.04e-3	0	0	0];

robot.motors=load_motors([5 5 5 4 4 4]);
%Speed reductor at each joint
robot.motors.G=[300 300 300 300 300 300];


function [tissueModel,coreRxnBool,coreMetBool,coreCtrsBool] = fastcore(model, coreRxnInd, epsilon, printLevel)
% Use the FASTCORE algorithm ('Vlassis et al, 2014') to extract a context
% specific model. FASTCORE algorithm defines one set of core
% reactions that is guaranteed to be active in the extracted model and find
% the minimum of reactions possible to support the core.
%
% USAGE:
%
%    tissueModel = fastcore(model, coreRxnInd)
%
% INPUTS:
%    model:             (the following fields are required - others can be supplied)
%                         * S  - `m x 1` Stoichiometric matrix
%                         * lb - `n x 1` Lower bounds
%                         * ub - `n x 1` Upper bounds
%                         * rxns   - `n x 1` cell array of reaction abbreviations
%
%   coreRxnInd:          indices of reactions in cobra model that are part of the
%                        core set of reactions (called 'C' in 'Vlassis et al,
%                        2014')
%
% OPTIONAL INPUTS:
%   epsilon:             smallest flux value that is considered nonzero
%                        (default getCobraSolverParams('LP', 'feasTol')*100)
%   printLevel:          0 = silent, 1 = summary, 2 = debug (default - 0)
%
% OUTPUT:
%
%   tissueModel:         extracted model
%
%   coreRxnBool:         n x 1 boolean vector indicating core reactions
%
% 'Vlassis, Pacheco, Sauter (2014). Fast reconstruction of compact
% context-specific metbolic network models. PLoS Comput. Biol. 10,
% e1003424.'
%
% .. Authors:
%       - Nikos Vlassis, Maria Pires Pacheco, Thomas Sauter, 2013 LCSB / LSRU, University of Luxembourg
%       - Ronan Fleming, commenting of code and inputs/outputs
%       - Anne Richelle, code adaptation to fit with createTissueSpecificModel

if nargin < 4 || ~exist('printLevel','var')
    printLevel = 0;
end
if nargin < 3 || isempty(epsilon)
    epsilon=getCobraSolverParams('LP', 'feasTol')*100;
end

if printLevel > 1
    tic
end

model_orig = model;

[nMets,nRxns] = size(model.S);

LPproblem = buildLPproblemFromModel(model);

%reactions irreversible in the reverse direction
Ir = model.lb < 0 & model.ub<=0;

%flip direction of reactions irreversible in the reverse direction
LPproblem.A(:,Ir) = -LPproblem.A(:,Ir);
tmp = LPproblem.ub(Ir);
LPproblem.ub(Ir) = -LPproblem.lb(Ir);
LPproblem.lb(Ir) = -tmp;

%Find irreversible reactions
irrevRxns = find(model.lb>=0);

A = [];
flipped = false;
singleton = false;

% Find irreversible core reactions
J = intersect(coreRxnInd, irrevRxns);

if printLevel > 0
    fprintf('|J|=%d  ', length(J));
end

%Find all the reactions that are not in the core
nbRxns = 1:nRxns;
% Non Core reactions (penalized)
P = setdiff(nbRxns, coreRxnInd);

% Find the minimum of set reactions from P that need to be included to
% support the irreversible core set of reactions
[Supp, basis] = findSparseMode(J, P, singleton, model, LPproblem, epsilon);

if ~isempty(setdiff(J, Supp))
    warning('fastcore.m Error: Global network is not flux consistent, ignoring the following irreversible core reactions:\n');
    model.rxns(setdiff(J, Supp))
    coreRxnInd = setdiff(coreRxnInd,setdiff(J, Supp));
    %error ('fastcore.m Error: Inconsistent irreversible core reactions.\n');
end

A = Supp;
if printLevel > 0
    fprintf('|A|=%d\n', length(A));
end

% J is the set of core reactions not already in the extracted model
J = setdiff(coreRxnInd, A);
if printLevel > 0
    fprintf('|J|=%d  ', length(J));
end

% Main loop that reduce at each iteration the number of reactions from P that need to be included to
% support the complete core set of reactions
while ~isempty(J)

    P = setdiff(P, A);

    %reuse the basis from the previous solve if it exists
    [Supp, basis] = findSparseMode(J, P, singleton, model, LPproblem, epsilon, basis);

    A = union(A, Supp);
    if printLevel > 0
        fprintf('|A|=%d\n', length(A));
    end

    if ~isempty( intersect(J, A))
        J = setdiff(J, A);
        if printLevel > 0
            fprintf('|J|=%d  ', length(J));
        end
        flipped = false;
    else
        if singleton
            JiRev = setdiff(J(1),irrevRxns);
        else
            JiRev = setdiff(J,irrevRxns);
        end
        if flipped || isempty(JiRev)
            if singleton
                warning('\n fastcore.m: Global network is not flux consistent, ignoring corresponding core reaction:\n');
                disp(model.rxns(J))
                J = [];
                %error('\n fastcore.m Error: Global network is not consistent.\n');
            else
                flipped = false;
                singleton = true;
            end
        else
            LPproblem.A(:,JiRev) = -LPproblem.A(:,JiRev);
            tmp = LPproblem.ub(JiRev);
            LPproblem.ub(JiRev) = -LPproblem.lb(JiRev);
            LPproblem.lb(JiRev) = -tmp;
            flipped = true;

            if printLevel > 0
                fprintf('(flip)  ');
            end
        end
    end
end
if printLevel > 0
    fprintf('|A|=%d\n', length(A)); % A : indices of reactions in the new model
end

if printLevel > 1
    toc
end

coreRxnBool=false(size(model.S,2),1);
coreRxnBool(A)=1;

rxnRemoveList = setdiff(model.rxns,model.rxns(A));

dummyMetBool = contains(model.mets,'dummy_Met_');
dummyRxnBool = contains(model.rxns,'dummy_Rxn_');
dummyRxnList = model.rxns(dummyRxnBool);
if any(dummyMetBool) || any(dummyRxnBool)
    model = destroyDummyModel(model,dummyMetBool,dummyRxnBool);
    rxnRemoveList = setdiff(rxnRemoveList,dummyRxnList);
end

%removes any infeasible coupling constraints also
[tissueModel, metRemoveList, ctrsRemoveList] = removeRxns(model, rxnRemoveList,'metRemoveMethod','exclusive','ctrsRemoveMethod','infeasible');

coreMetBool=~ismember(model_orig.mets,metRemoveList);
if isfield(model,'ctrs')
    coreCtrsBool = ~ismember(model_orig.ctrs,ctrsRemoveList);
else
    coreCtrsBool = ctrsRemoveList;
end

%coreGeneBool
tissueModel = removeUnusedGenes(tissueModel);


function [ T1temp MP2RAGEcorrected] = T1B1correctpackageTFL( B1img,MP2RAGEimg,T1,MP2RAGE,brain,varargin)
% usage
%
% [  T1corr MP2RAGEcorr] = T1B1correctpackage(B1,MP2RAGEimg,T1,MP2RAGE,brain,varargin)
%
% B1 and MP2RAGEimg (and T1) are the nii structures resulting from loading
% the MP2RAGE (MP2RAGEimg, T1) and the result of some B1 mapping technique
%  with load_nii or load_untouch_nii
%
% the variable B1 is compulsory
%
% Only MP2RAGEimg or the T1 have to be loaded (I usually use the MP2RAGEimg)
%
% the variables that are not loaded can be simply left empty []
%
% MP2RAGE  variable contains all the relevant sequence
% information as delailed below
%
% B1 will be given in relative units => 1 if it was correct; values can varie
% from 0-2
%
%     MP2RAGE.B0=7;           % in Tesla
%     MP2RAGE.TR=6;           % MP2RAGE TR in seconds
%     MP2RAGE.TRFLASH=6.7e-3; % TR of the GRE readout
%     MP2RAGE.TIs=[800e-3 2700e-3];% inversion times - time between middle of refocusing pulse and excitatoin of the k-space center encoding
%     MP2RAGE.NZslices=[40 80];% Slices Per Slab * [PartialFourierInSlice-0.5  0.5]
%     MP2RAGE.FlipDegrees=[4 5];% Flip angle of the two readouts in degrees
%
% brain can be an image in the same space as the MP2RAGE and the
% Sa2RAGE that has zeros where there is no need to do any T1/B1 calculation
% (can be a binary mask or not). if left empty the calculation is done
% everywhere
%
% additionally the inversion efficiency of the adiabatic inversion can be
% set as a last optional variable. Ideally it should be 1.
% In the first implementation of the MP2RAGE the inversino efficiency was
% measured to be ~0.96
%
% outputs are:
%  T1corr - T1map corrected for B1 bias
%  MP2RAGEcorr - MP2RAGE image corrected for B1 bias
%
%
% please cite:
% Marques, J.P., Gruetter, R., 2013. New Developments and Applications of the MP2RAGE Sequence - Focusing the Contrast and High Spatial Resolution R1 Mapping. PLoS ONE 8. doi:10.1371/journal.pone.0069294
% Marques, J.P., Kober, T., Krueger, G., van der Zwaag, W., Van de Moortele, P.-F., Gruetter, R., 2010a. MP2RAGE, a self bias-field corrected sequence for improved segmentation and T1-mapping at high field. NeuroImage 49, 1271�1281. doi:10.1016/j.neuroimage.2009.10.002
%

% Check B1 range
b1med = median(prctile(B1img(:),90));

if b1med > 5

     warning(sprintf(['=============== mp2rage::b1correction ==========='...
        '\n B1 data is not in [0-2] range. B1map magnitude will be scaled down.'  ...
        '\n ===========================================================' ...
        ]));

    if b1med > 10 &&  b1med < 500

        B1img = double(B1img)./100;

    elseif b1med>500 && b1med<1500

        B1img = double(B1img)./1000;
    end
end

b1med = median(prctile(B1img(:),90));

if ~(b1med > 0.5) && ~(b1med < 1.5)

    warning(sprintf(['=============== mp2rage::b1correction ==========='...
        '\n B1 data may not be in the required [0-2] range'  ...
        '\n ===========================================================' ...
        ]));

end

if nargin==6

    invEFF=varargin{1};

else

    invEFF=0.96;

end

if isempty(brain)

    if isempty(MP2RAGEimg)

        brain=T1;

        brain.img=ones(size(brain.img));

    else

        brain=MP2RAGEimg;

        brain.img=ones(size(brain.img));

    end;

end;

B1_vector=0.005:0.05:1.9;

T1_vector=0.5:0.05:5.2;

[MP2RAGE.Intensity MP2RAGE.T1vector ]=MP2RAGE_lookuptable(2,MP2RAGE.TR,MP2RAGE.TIs,MP2RAGE.FlipDegrees,MP2RAGE.NZslices,MP2RAGE.TRFLASH,'normal',invEFF);

if isempty(MP2RAGEimg)

    T1.img=double(T1.img)/1000;

    MP2RAGEimg.img=reshape(interp1(MP2RAGE.T1vector,MP2RAGE.Intensity,T1.img(:)),size(B1img));

    MP2RAGEimg.img(isnan(MP2RAGEimg.img))=-0.5;

else

    MP2RAGEimg.img=double(MP2RAGEimg.img)/4095-0.5;

end;

%% now the fun starts

% creates a lookup table of MP2RAGE intensities as a function of B1 and T1

k=0;

for b1val=B1_vector

    k=k+1;

    [Intensity T1vector ]=MP2RAGE_lookuptable(2,MP2RAGE.TR,MP2RAGE.TIs,b1val*MP2RAGE.FlipDegrees,MP2RAGE.NZslices,MP2RAGE.TRFLASH,'normal',invEFF);

    MP2RAGEmatrix(k,:)=interp1(T1vector,Intensity,T1_vector);

end;

k=0;

%% make the matrix  MP2RAGEMatrix into T1_matrix(B1,ratio)

npoints=40;

MP2RAGE_vector=linspace(-0.5,0.5,npoints);

k=0;

for b1val=B1_vector

    k=k+1;

    try

        T1matrix(k,:)=interp1(MP2RAGEmatrix(k,:),T1_vector,MP2RAGE_vector,'pchirp');

    catch

        temp=MP2RAGEmatrix(k,:);

        temp(isnan(temp))=linspace(-0.5-eps,-1,length(find(isnan(temp)==1)));

        temp=interp1(temp,T1_vector,MP2RAGE_vector);

        T1matrix(k,:)=temp;

    end;

end;

%% correcting the estimates of T1 and B1 iteratively

T1temp=MP2RAGEimg;

brain.img(B1img==0)=0;

brain.img(MP2RAGEimg.img==0)=0;

T1temp.img(brain.img==0)=0;

T1temp.img(brain.img==1)=0;

B1img(brain.img==0)=0;

temp=squeeze(T1temp.img(:,end/2,:));

T1temp.img(brain.img~=0)=interp2(MP2RAGE_vector,B1_vector,T1matrix,MP2RAGEimg.img(brain.img~=0),B1img(brain.img~=0));

T1temp.img(isnan(T1temp.img))=4;

temp2=squeeze(T1temp.img(:,end/2,:));

%% creates an MP2RAGEcorrected image and puts both the B1 and T1 in the ms scale

[MP2RAGE.Intensity MP2RAGE.T1vector ]=MP2RAGE_lookuptable(2,MP2RAGE.TR,MP2RAGE.TIs,MP2RAGE.FlipDegrees,MP2RAGE.NZslices,MP2RAGE.TRFLASH,'normal',invEFF);

MP2RAGEcorrected=MP2RAGEimg;

MP2RAGEcorrected.img=reshape(interp1(MP2RAGE.T1vector,MP2RAGE.Intensity,T1temp.img(:)),size(T1temp.img));

MP2RAGEcorrected.img(isnan(MP2RAGEcorrected.img))=-0.5;

MP2RAGEcorrected.img=round(4095*(MP2RAGEcorrected.img+0.5));

%T1temp.img=(T1temp.img)*1000;

end


function [inflMap, colXCoord, rowYCoord, colDividerXCoord, rowDividerYCoord, rowLeafPositions] = getLSInfluenceMapFactorEx(LS,leak,beamIndex)
%"getLSInfluenceMap"
%   Gets an image of the influence generated by the beam described in LS.
%   Use getDICOMLeafPositions to generate LS.
%
%JRA&KZ 02/8/05
%
%Usage:
%   function inflMap = getLSInfluenceMap(LS);
%
% 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/>.

%Maximum precision of leaf position, in mm.
precision = .1;

%Get x max, min and round to precision value.
% xMax = ceil(max(vertcat(LS.xLimits{1}),[],1) / precision) * precision;
% xMin = floor(min(vertcat(LS.xLimits{1}),[],1) / precision) * precision;
% fieldSize.x = max(xMax) - min(xMin);
% fieldLim.x  = [max(xMax) min(xMin)];

%Siemens no X jaws exist
if ~isfield(LS,'xLimits')
    xMax = ceil(max(vertcat(LS.xLeafPositions{:}),[],1) / precision) * precision;
    xMin = floor(min(vertcat(LS.xLeafPositions{:}),[],1) / precision) * precision;
    LS.xLimits{1}(1) = xMin;
    LS.xLimits{1}(2) = xMax;
end

xMax = ceil(max(vertcat(LS.xLimits{:}),[],1) / precision) * precision;
xMin = floor(min(vertcat(LS.xLimits{:}),[],1) / precision) * precision;
fieldSize.x = max(xMax) - min(xMin);
fieldLim.x  = [max(xMax) min(xMin)];

yMax = ceil(max(vertcat(LS.yLimits{:}),[],1) / precision) * precision;
yMin = floor(min(vertcat(LS.yLimits{:}),[],1) / precision) * precision;
fieldSize.y = max(yMax) - min(yMin);
fieldLim.y  = [max(yMax) min(yMin)];

yRes = precision;
nyElements = ceil(fieldSize.y/yRes);
xRes = precision;
nxElements = ceil(fieldSize.x/xRes);

inflMap=zeros(nyElements, nxElements);
colDividerXCoord = linspace(fieldLim.x(2), fieldLim.x(1), nxElements+1);
rowDividerYCoord = linspace(fieldLim.y(2), fieldLim.y(1), nyElements+1);

if isfield(LS, 'yLeafPositions')
    rowLeafPositions = round(interp1(rowDividerYCoord, 1:nyElements+1, LS.yLeafPositions,'linear', 'extrap'));
    rowLeafPositions = clip(rowLeafPositions, 1, nyElements+1, 'limits');
    leafBoundariesToKeep = [diff(rowLeafPositions)>0;logical(1)];
    rowLeafPositions = rowLeafPositions(leafBoundariesToKeep);
    leavesToKeep = leafBoundariesToKeep(1:end-1);
else
    %LS.xLeafPositions{1} = [xMin xMax-precision];
    LS.xLeafPositions{1} = [xMin xMax];
    LS.meterSetWeight = {1};
    rowLeafPositions = [1 nyElements+1];
    leavesToKeep = 1;
end

if length(LS.meterSetWeight) == 1
    doses = LS.meterSetWeight{:};
else
    doses = [0 diff([LS.meterSetWeight{:}])];
end

h = waitbar(0,['Generating Fluence Map For Beam ',num2str(beamIndex)]);

for i=1:length(LS.xLeafPositions)

    nLeaves = length(LS.xLeafPositions{i})/2;

    if length(LS.xLimits) > 1
        jpL = LS.xLimits{i}(1);
        jpR = LS.xLimits{i}(2);
    else
        jpL = LS.xLimits{1}(1);
        jpR = LS.xLimits{1}(2);
    end

    lpL = LS.xLeafPositions{i}(1:nLeaves);
    lpR = LS.xLeafPositions{i}(nLeaves+1:end);
    lpLK = lpL(leavesToKeep);
    lpRK = lpR(leavesToKeep);
    lpLCols = interp1(colDividerXCoord, 1:nxElements+1, lpLK, 'linear', 'extrap');
    lpRCols = interp1(colDividerXCoord, 1:nxElements+1, lpRK, 'linear', 'extrap');

    %Column divider positions of jaws.
    jpLCol = interp1(colDividerXCoord, 1:nxElements+1, jpL, 'linear', 'extrap');
    jpRCol = interp1(colDividerXCoord, 1:nxElements+1, jpR, 'linear', 'extrap');

    lpLCols = clip(lpLCols, jpLCol, jpRCol, 'limits');
    lpRCols = clip(lpRCols, jpLCol, jpRCol, 'limits');

    lpLCols = round(lpLCols);
    lpRCols = round(lpRCols);

    %head scatter radiation parametrs for varian
    a2 = 0.078;
    beta = 1.79;
    lambda = 7.69;

    for j=1:length(lpLCols)
        %HCF from output ratio for MLC fields Zhu, MedPhys
        YMLC = rowDividerYCoord(rowLeafPositions(j)) + abs((rowDividerYCoord(rowLeafPositions(j+1)) - rowDividerYCoord(rowLeafPositions(j))))/2;
        YMLC = YMLC/10;
        sizeLeaf = abs((rowDividerYCoord(rowLeafPositions(j+1)) - rowDividerYCoord(rowLeafPositions(j))));
        sizeLeaf = sizeLeaf/10;
        HCF_UP = 1 + a2*(erf(2*lpLK(j)*beta/(10*lambda)) +  erf(2*lpRK(j)*beta/(10*lambda)))*(erf(2*(YMLC + sizeLeaf/2)/lambda) - erf(2*(YMLC - sizeLeaf/2)/lambda))/4;
        HCF_Down = 1 + a2*(erf(2*jpL/(10*lambda)) +  erf(2*jpR/(10*lambda)))*(erf(2*(YMLC + sizeLeaf/2)/lambda) - erf(2*(YMLC - sizeLeaf/2)/lambda))/4;
        HCF = HCF_UP/HCF_Down;
        inflMap(rowLeafPositions(j):rowLeafPositions(j+1)-1, lpLCols(j):lpRCols(j)-1) = inflMap(rowLeafPositions(j):rowLeafPositions(j+1)-1, lpLCols(j):lpRCols(j)-1) + HCF*doses(i);
        inflMap(rowLeafPositions(j):rowLeafPositions(j+1)-1, jpLCol:lpLCols(j)-1) = inflMap(rowLeafPositions(j):rowLeafPositions(j+1)-1, jpLCol:lpLCols(j)-1) + leak*doses(i);
        inflMap(rowLeafPositions(j):rowLeafPositions(j+1)-1, lpRCols(j):jpRCol-1) = inflMap(rowLeafPositions(j):rowLeafPositions(j+1)-1, lpRCols(j):jpRCol-1) + leak*doses(i);
    end

    waitbar(i/length(LS.xLeafPositions));
end
close(h);
colXCoord = colDividerXCoord(1:end-1) + precision/2;
rowYCoord = rowDividerYCoord(1:end-1) + precision/2;


function f = plus(f, g)
%+   Addition of two TRIGTECH objects.
%   F + G adds F and G, where F and G may be TRIGTECH objects or scalars.
%
%   If F is an array-valued TRIGTECH, then F + C is supported if C is a row
%   vector of doubles with the same number of columns as F.
%
% See also MINUS, UPLUS.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

if ( isempty(f) || isempty(g) ) % TRIGTECH + [] = [].

    f = [];

elseif ( isa(g, 'double') ) % TRIGTECH + double.

    % Update values (use bsxfun() to handle the case in which g is a vector
    % and f is an array-valued TRIGTECH):
    f.values = bsxfun(@plus, f.values, g);

    % Update coeffs:
    if ( (size(g, 2) > 1) && (size(f.coeffs, 2) == 1) )
        % Perform singleton expansion of f:
        f.coeffs = repmat(f.coeffs, 1, size(g, 2));
    end
    N = size(f.coeffs,1);

    % Determine the index in the coefficient vector where the constant term
    % is stored. The way we have arranged the coefficients means it should
    % be in the middle of the array, but that depends on whether the number
    % of coefficients is even or odd.
    if mod(N,2) == 1
        const_index = (N+1)/2;
    else
        const_index = N/2+1;
    end
    f.coeffs(const_index,:) = f.coeffs(const_index,:) + g;

    % Update isReal:
    f.isReal = f.isReal & isreal(g);

elseif ( isa(f, 'double') ) % double + TRIGTECH.

    % Switch argument order and call TRIGTECH/PLUS again:
    f = plus(g, f);

elseif ( isa(f, 'trigtech') && isa(g, 'trigtech') )  % TRIGTECH + TRIGTECH.

    % We will simply add the values together then compute the coefficients
    % of the result. This is probably not the most efficient means of
    % determing the sum.
    nf = size(f.values, 1);
    ng = size(g.values, 1);
    if ( nf > ng )
        % Increase the length of g (via PROLONG):
        g = prolong(g, nf);
    elseif ( nf < ng )
        % Increase the length of f (via PROLONG):
        f = prolong(f, ng);
    end

    % Update values and coefficients:
    f.values = f.values + g.values;
    f.coeffs = f.vals2coeffs(f.values);

    % Update isReal:
    f.isReal = f.isReal & g.isReal;

    % Force the values to be real where f is real.
    f.values(:,f.isReal) = real(f.values(:,f.isReal));

    % Look for a zero output:
    if ( ~any(f.values(:)) || ~any(f.coeffs(:)) )
        % Create a zero TRIGTECH:
        ishappy = f.ishappy && g.ishappy;
        z = zeros(1, size(f.values, 2));
        data.vscale = z;
        f = f.make(z, data);
        f.ishappy = ishappy;
    else
        f.ishappy = f.ishappy && g.ishappy;
    end

else    % Don't know how to do the addition of the objects.

    error('CHEBFUN:TRIGTECH:plus:typeMismatch',['Incompatible operation between objects.\n', ...
    'Make sure functions are of the same type.']);

end

end


function vel = veldim(vel,from,to)
%VELDIM  Convert velocity units
%
%   velOut = VELDIM(velIn, FROM, TO) converts velIn from the units
%   specified by the string FROM to the units specified by the string
%   TO.  FROM and TO are case-insensitive, and may equal any of the
%   following:
%
%        'meters per second', 'mps' or 'm/s'
%        'feet per second', 'ftps' or 'ft/s'   <== U.S. survey feet
%        'kilometers per hour', 'kmph', 'km/hr', or 'km/h'
%        'knots', 'kts', 'nm/hr' or 'nm/h'
%        'miles per hour', 'mph', 'mi/hr', 'mi/h', sm/hr' or 'sm/h' <== statute miles
%
%   Exercise caution with 'feet' and 'miles'
%   ----------------------------------------
%   VELDIM interprets 'feet per second', 'ftps', and 'ft/s' as U.S. survey
%   feet per second, and does not support international feet at all.
%
%   By definition, one international foot is exactly 0.3048 meters and
%   one U.S. survey foot is exactly 1200/3937 meters.  For many
%   applications, the difference is significant.
%
%   Likewise, VELDIM interprets 'miles per hour', 'mph', 'mi/h', and 'sm/h'
%   as statute miles per hour (also known as U.S. survey miles), and does
%   not support international miles per hour at all.  By definition, one
%   international mile is 5280 international feet and one statute mile is
%   5280 survey feet.
%
%   See also  MPS2FTPS, MPS2KMPH,  MPS2KTS,  MPS2MPH,
%             FTPS2MPS, FTPS2KMPH, FTPS2KTS, FTPS2MPH,
%             KMPH2MPS, KMPH2FTPS, KMPH2KTS, KMPH2MPH,
%             KTS2MPS,  KTS2FTPS,  KTS2KMPH, KTS2MPH,
%             MPH2MPS,  MPH2FTPS,  MPH2KMPH, MPH2KTS

error(nargchk(3, 4, nargin, 'struct'))

% Warn and convert to real if VEL is complex.
vel = ignoreComplex(vel, mfilename, 'VEL');

% Convert units only if there's something to change.
if ~strcmp(from, to)
    vel = applyconversion(vel, from, to);
end

function vel = applyconversion(vel, from, to)

from = lower(from);
to = lower(to);

toIsSupported   = true;
fromIsSupported = true;

switch from
    case {'meters per second','mps','m/s'}
        switch to
            case {'feet per second','ftps','ft/s'}
                vel = mps2ftps(vel);
            case {'kilometers per hour','kmph','km/h','km/hr'}
                vel = mps2kmph(vel);
            case {'knots','kts','nm/h','nm/hr'}
                vel = mps2kts(vel);
            case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
                vel = mps2mph(vel);
            otherwise
                toIsSupported = false;
        end
    case {'feet per second','ftps','ft/s'}
        switch to
            case {'meters per second','mps','m/s'}
                vel = ftps2mps(vel);
            case {'kilometers per hour','kmph','km/h','km/hr'}
                vel = ftps2kmph(vel);
            case {'knots','kts','nm/h','nm/hr'}
                vel = ftps2kts(vel);
            case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
                vel = ftps2mph(vel);
            otherwise
                toIsSupported = false;
        end
    case {'kilometers per hour','kmph','km/h','km/hr'}
        switch to
            case {'feet per second','ftps','ft/s'}
                vel = kmph2ftps(vel);
            case {'meters per second','mps','m/s'}
                vel = kmph2mps(vel);
            case {'knots','kts','nm/h','nm/hr'}
                vel = kmph2kts(vel);
            case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
                vel = kmph2mph(vel);
            otherwise
                toIsSupported = false;
        end
    case {'knots','kts','nm/h','nm/hr'}
        switch to
            case {'feet per second','ftps','ft/s'}
                vel = kts2ftps(vel);
            case {'kilometers per hour','kmph','km/h','km/hr'}
                vel = kts2kmph(vel);
            case {'meters per second','mps','m/s'}
                vel = kts2mps(vel);
            case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
                vel = kts2mph(vel);
            otherwise
                toIsSupported = false;
        end
    case {'miles per hour','mph','mi/hr','mi/h','sm/hr','sm/h'}
        switch to
            case {'feet per second','ftps','ft/s'}
                vel = mph2ftps(vel);
            case {'kilometers per hour','kmph','km/h','km/hr'}
                vel = mph2kmph(vel);
            case {'knots','kts','nm/h','nm/hr'}
                vel = mph2kts(vel);
            case {'meters per second','mps','m/s'}
                vel = mph2mps(vel);
            otherwise
                toIsSupported = false;
        end

    otherwise
        fromIsSupported = false;
end

assert(toIsSupported, 'map:distdim:UnsupportedToUnits', ...
    'Unsupported ''TO'' units: %s.', to)

assert(fromIsSupported, 'map:distdim:UnsupportedFromUnits', ...
    'Unsupported ''FROM'' units: %s.', from)


function edgeFaces = meshEdgeFaces(vertices, edges, faces) %#ok<INUSL>
%MESHEDGEFACES Compute index of faces adjacent to each edge of a mesh.
%
%   EF = meshEdgeFaces(V, E, F)
%   Compute index array of faces adjacent to each edge of a mesh.
%   V, E and F define the mesh: V is vertex array, E contains vertex
%   indices of edge extremities, and F contains vertex indices of each
%   face, either as a numerical array or as a cell array.
%   The result EF has as many rows as the number of edges, and two column.
%   The first column contains index of faces located on the left of the
%   corresponding edge, whereas the second column contains index of the
%   face located on the right. Some indices may be 0 if the mesh is not
%   'closed'.
%
%   Note: a faster version is available for triangular meshes.
%
%   Example
%   meshEdgeFaces
%
%   See also
%   meshes3d, trimeshEdgeFaces, meshDihedralAngles, polyhedronMeanBreadth

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2010-10-04, using Matlab 7.9.0.529 (R2009b)
% Copyright 2010-2022 INRA - Cepia Software Platform

Ne = size(edges, 1);

% indices of faces adjacent to each edge
edgeFaces = zeros(Ne, 2);

% different method for extracting current face depending if faces are
% stored as index 2D array or as cell array of 1D arrays.
if isnumeric(faces)
    Nf = size(faces, 1);
    for i = 1:Nf
        face = faces(i, :);
        processFace(face, i)
    end
elseif iscell(faces)
    Nf = length(faces);
    for i = 1:Nf
        face = faces{i};
        processFace(face, i)
    end
end

    function processFace(face, indFace)
        % iterate on face edges
        for j = 1:length(face)
            % build edge: array of vertices
            j2 = mod(j, length(face)) + 1;

            % do not process edges with same vertices
            if face(j) == face(j2)
                continue;
            end

            % vertex indices of current edge
            currentEdge = [face(j) face(j2)];

            % find index of current edge, assuming face is left-located
            b1 = ismember(edges, currentEdge, 'rows');
            indEdge = find(b1);
            if ~isempty(indEdge)
                if edgeFaces(indEdge, 1) ~= 0
                    error('meshes3d:IllegalTopology', ...
                        'Two faces were found on left side of edge %d ', indEdge);
                end

                edgeFaces(indEdge, 1) = indFace;
                continue;
            end

            % otherwise, assume the face is right-located
            b2 = ismember(edges, currentEdge([2 1]), 'rows');
            indEdge = find(b2);
            if ~isempty(indEdge)
                if edgeFaces(indEdge, 2) ~= 0
                    error('meshes3d:IllegalTopology', ...
                        'Two faces were found on left side of edge %d ', indEdge);
                end

                edgeFaces(indEdge, 2) = indFace;
                continue;
            end

            % If face was neither left nor right, error
            warning('meshes3d:IllegalTopology', ...
                'Edge %d of face %d was not found in edge array', ...
                j, indFace);
            continue;
        end
    end

end


%This Matlab script can be used to reproduce Figure 6.4 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Number of BSs
L = 16;

%Range of number of UEs per BS
Krange = [10 20];

%Compute maximum number of UEs
Kmax = max(Krange);

%Number of BS antennas
M = 100;

%Define the range of pilot reuse factors
fRange = [1 2 4];

%Select the number of setups with random UE locations
nbrOfSetups = 1;

%Select the number of channel realizations per setup
nbrOfRealizations = 200;

%Select the range of hardware qualities. The values at the same position in
%the different vectors are considered simultaneously.
kappatUE = [0.9 0.92 0.94 0.96 0.97 0.98 0.99 0.995 1];
kapparBS = kappatUE;

%% Propagation parameters

%Communication bandwidth
B = 20e6;

%Define total uplink transmit power per UE (mW)
p = 100;

%Define noise figure at BS (in dB)
noiseFigure = 7;

%Compute noise power
noiseVariancedBm = -174 + 10*log10(B) + noiseFigure;

%Select length of coherence block
tau_c = 200;

%Use the approximation of the Gaussian local scattering model
accuracy = 2;

%Angular standard deviation in the local scattering model (in degrees)
ASDdeg = 10;

%Prepare to save simulation results
sumSE_MR = zeros(length(kappatUE),length(fRange),nbrOfSetups,length(Krange));
sumSE_RZF = zeros(length(kappatUE),length(fRange),nbrOfSetups,length(Krange));
sumSE_MMMSE = zeros(length(kappatUE),length(fRange),nbrOfSetups,length(Krange));

%% Go through all setups
for n = 1:nbrOfSetups

    %Output simulation progress
    disp([num2str(n) ' setups out of ' num2str(nbrOfSetups)]);

    %Compute channel statistics for one setup
    [R,channelGaindB] = functionExampleSetup(L,Kmax,M,accuracy,ASDdeg);

    %Compute the normalized average channel gain, where the normalization
    %is based on the noise power
    channelGainOverNoise = channelGaindB - noiseVariancedBm;

    %Go through all pilot reuse factors
    for s = 1:length(fRange)

        %Output simulation progress
        disp([num2str(s) ' reuse factors out of ' num2str(length(fRange))]);

        %Extract pilot reuse factor
        f = fRange(s);

        %Go through all hardware qualities
        for r = 1:length(kappatUE)

            for kind = 1:length(Krange)

                %Extract number of UEs
                K = Krange(kind);

                %Generate channel realizations with estimates and estimation
                %error correlation matrices, using LMMSE estimation
                [Hhat,C,tau_p,~,H] = functionChannelEstimates_impairments(R(:,:,1:K,:,:),channelGainOverNoise(1:K,:,:),nbrOfRealizations,M,K,L,p,f,kappatUE(r),kapparBS(r));

                %Compute SEs using Monte Carlo realizations
                [SE_MR,SE_RZF,SE_MMMSE] = functionComputeSE_UL_impairments(H,Hhat,C,tau_c,tau_p,nbrOfRealizations,M,K,L,p,kappatUE(r),kapparBS(r));

                %Save results
                sumSE_MR(r,s,n,kind) = mean(sum(SE_MR,1));
                sumSE_RZF(r,s,n,kind) = mean(sum(SE_RZF,1));
                sumSE_MMMSE(r,s,n,kind) = mean(sum(SE_MMMSE,1));

                %Delete large matrices
                clear Hhat C H;

            end

        end

    end

    %Delete large matrices
    clear R;

end

%% Plot the simulation results
for kind = 1:length(Krange)

    figure;
    hold on; box on;

    plot(kappatUE,max(mean(sumSE_MMMSE(:,:,:,kind),3),[],2),'rd-','LineWidth',1);
    plot(kappatUE,max(mean(sumSE_RZF(:,:,:,kind),3),[],2),'k-.','LineWidth',1);
    plot(kappatUE,max(mean(sumSE_MR(:,:,:,kind),3),[],2),'bs-','LineWidth',1);

    xlabel('Hardware quality');
    ylabel('Average sum SE [bit/s/Hz/cell]');
    legend('M-MMSE','RZF','MR','Location','NorthWest');
    ylim([0 ceil(max(max(mean(sumSE_MMMSE(:,:,:,kind),3),[],2))/10)*10]);

end


%%*******************************************************************
%% HSDNTrhsfun: compute the right-hand side vector of the
%%              Schur complement equation for the NT direction.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*******************************************************************

function [rhs,EinvRc,hRd] = HSDNTrhsfun(blk,At,par,X,Z,rp,Rd,sigmu,hRd,dX,dZ)
global spdensity

m = par.m;
if (nargin > 8)
    corrector = 1;
else
    corrector = 0;
    hRd = zeros(m+2,1);
end
hEinvRc = zeros(m+2,1);
EinvRc  = cell(size(blk,1),1);
if length(sigmu)==1; sigmu = sigmu*ones(1,size(blk,1)); end
%%
for p = 1:size(blk,1)
    pblk = blk(p,:);
    n = sum(pblk{2});  numblk = length(pblk{2});
    if strcmp(pblk{1},'l')
        if (corrector)
            Rq = dX{p}.*dZ{p};
        else
            Rq = sparse(n,1);
            tmp  = par.dd{p}.*Rd{p};
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = sigmu(p)./Z{p}-X{p} -Rq./Z{p};
        tmp2 = mexMatvec(At{p},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'q')
        w = sqrt(par.gamz{p}./par.gamx{p});
        if (corrector)
            hdx = qops(pblk,w,par.ff{p},5,dX{p});
            hdz = qops(pblk,w,par.ff{p},6,dZ{p});
            hdxdz = Arrow(pblk,hdx,hdz);
            vv = qops(pblk,w,par.ff{p},5,X{p});
            Vihdxdz = Arrow(pblk,vv,hdxdz,1);
            Rq = qops(pblk,w,par.ff{p},6,Vihdxdz);
        else
            Rq = sparse(n,1);
            tmp  = par.dd{p}.*Rd{p} + qops(pblk,qops(pblk,Rd{p},par.ee{p},1),par.ee{p},3);
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = qops(pblk,-sigmu(p)./(par.gamz{p}.*par.gamz{p}),Z{p},4)-X{p}-Rq;
        tmp2 = mexMatvec(At{p},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'s')
        n2 = pblk{2}.*(pblk{2}+1)/2;
        if (corrector)
            hdZ = Prod3(pblk,par.G{p},dZ{p},par.G{p}',1);
            hdX = spdiags(-par.sv{p},0,n,n)-hdZ;
            tmp = Prod2(pblk,hdX,hdZ,0);
            tmp = 0.5*(tmp+tmp');
            if (numblk == 1)
                d = par.sv{p};
                e = ones(pblk{2},1);
                Rq = 2*tmp./(d*e'+e*d');
                if (nnz(Rq) <= spdensity*n2); Rq = sparse(Rq); end
            else
                Rq = sparse(n,n);
                s = [0, cumsum(pblk{2})];
                for i = 1:numblk
                    pos = s(i)+1 : s(i+1);
                    d = par.sv{p}(pos); e = ones(length(pos),1);
                    Rq(pos,pos) = 2*tmp(pos,pos)./(d*e' + e*d'); %#ok
                end
            end
        else
            Rq = sparse(n,n);
            EinvRc{p} = Prod3(pblk,par.W{p},Rd{p},par.W{p},1,par.nzlistAy{p});
            tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),EinvRc(p));
            hRd = hRd + tmp2;
        end
        tmp = spdiags(sigmu(p)./par.sv{p} -par.sv{p},0,n,n);
        EinvRc{p} = Prod3(pblk,par.G{p}',tmp-Rq,par.G{p},1);
        tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),EinvRc(p));
        hEinvRc = hEinvRc + tmp2;
    end
end
%%
rhs = rp + hRd - hEinvRc;
rhs(m+1) = rhs(m+1) + (par.mu/par.tau - par.kap);
if (corrector)
    rhs(m+1) = rhs(m+1) - par.dtau*par.dkap/par.tau;
end
%%*******************************************************************


function lambda = parlett_eigenvalues ( )

%*****************************************************************************80
%
%% PARLETT_EIGENVALUES returns the eigenvalues of the PARLETT matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real LAMBDA(100,1), the eigenvalues.
%
  n = 100;
  lambda = zeros ( n, 1 );

  for i = 1 : n
    lambda(i,1) = i;
  end

  return
end


function tapas_hgf_categorical_plotTraj(r)
% Plots trajectories estimated by fitModel for the hgf_categorical perceptual model
% Usage:  est = tapas_fitModel(responses, inputs); tapas_hgf_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Check whether we have a configuration structure
if ~isfield(r,'c_prc')
    error('tapas:hgf:ConfigRequired', 'Configuration required: before calling tapas_hgf_categorical_plotTraj, tapas_hgf_categorical_config has to be called.');
end

% Number of outcomes
no = r.c_prc.n_outcomes;

% Define colors
colors = [1 0 0; 0.67 0 1; 0 0.67 1; 0.67 1 0];

% Set up display
scrsz = get(0,'screenSize');
outerpos = [0.2*scrsz(3),0.2*scrsz(4),0.8*scrsz(3),0.8*scrsz(4)];
figure(...
    'OuterPosition', outerpos,...
    'Name','HGF trajectories');

% Number of trials
t = size(r.u,1);

% Optional plotting of standard deviations (true or false)
plotsd2 = true;
plotsd3 = true;

% Subplots
subplot(3,1,1);

if plotsd3 == true
    upper3prior = r.p_prc.mu3_0 +sqrt(r.p_prc.sa3_0);
    lower3prior = r.p_prc.mu3_0 -sqrt(r.p_prc.sa3_0);
    upper3 = [upper3prior; r.traj.mu(:,3)+sqrt(r.traj.sa(:,3))];
    lower3 = [lower3prior; r.traj.mu(:,3)-sqrt(r.traj.sa(:,3))];

    plot(0, upper3prior, 'ob', 'LineWidth', 1);
    hold all;
    plot(0, lower3prior, 'ob', 'LineWidth', 1);
    fill([0:t, fliplr(0:t)], [(upper3)', fliplr((lower3)')], ...
         'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
plot(0:t, [r.p_prc.mu3_0; r.traj.mu(:,3)], 'b', 'LineWidth', 2);
hold all;
plot(0, r.p_prc.mu3_0, 'ob', 'LineWidth', 2); % prior
xlim([0 t]);
title('Posterior expectation \mu_3 of log-volatility of tendency x_3', 'FontWeight', 'bold');
xlabel('Trial number');
ylabel('\mu_3');

subplot(3,1,2);
if plotsd2 == true
    for j=1:no
    upper2prior = r.p_prc.mu2_0(j) +sqrt(r.p_prc.sa2_0(j));
    lower2prior = r.p_prc.mu2_0(j) -sqrt(r.p_prc.sa2_0(j));
    upper2 = [upper2prior; r.traj.mu(:,2,j)+sqrt(r.traj.sa(:,2,j))];
    lower2 = [lower2prior; r.traj.mu(:,2,j)-sqrt(r.traj.sa(:,2,j))];

    plot(0, upper2prior, 'o', 'Color', colors(j,:), 'LineWidth', 1);
    hold all;
    plot(0, lower2prior, 'o', 'Color', colors(j,:), 'LineWidth', 1);
    fill([0:t, fliplr(0:t)], [(upper2)', fliplr((lower2)')], ...
         colors(j,:), 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
end
for j=1:no
    plot(0:t, [r.p_prc.mu2_0(j); r.traj.mu(:,2,j)], 'Color', colors(j,:), 'LineWidth', 2);
    hold all;
    plot(0, r.p_prc.mu2_0(j), 'o', 'Color', colors(j,:), 'LineWidth', 2); % prior
end
xlim([0 t]);
title('Posterior expectations \mu_2 of tendencies x_2', 'FontWeight', 'bold');
xlabel({'Trial number', ' '}); % A hack to get the relative subplot sizes right
ylabel('\mu_2');
hold off;

subplot(3,1,3);
for j=1:no
    plot(0:t, [tapas_sgm(r.p_prc.mu2_0(j), 1); tapas_sgm(r.traj.mu(:,2,j), 1)], 'Color', colors(j,:), 'LineWidth', 2);
    hold all;
    plot(0, tapas_sgm(r.p_prc.mu2_0(j), 1), 'o', 'Color', colors(j,:), 'LineWidth', 2); % prior
end
u = r.u(:,1);
for j=1:no
    plot(find(u==j), -0.08*ones([1 length(find(u==j))]), '.', 'Color', colors(j,:)); % inputs
end
if ~isempty(find(strcmp(fieldnames(r),'y'))) && ~isempty(r.y)
    y = r.y(:,1);
    if ~isempty(find(strcmp(fieldnames(r),'irr')))
        y(r.irr) = NaN; % weed out irregular responses
        plot(r.irr,  1.08.*ones([1 length(r.irr)]), 'x', 'Color', [1 0.7 0], 'Markersize', 11, 'LineWidth', 2); % irregular responses
    end
    for j=1:no
        plot(find(y==j), 1.08*ones([1 length(find(y==j))]), '.', 'Color', colors(j,:)); % responses
    end
    title(['Response y (top dot row), input u (bottom dot row), and posterior probability of outcomes s(\mu_2) for \kappa=', ...
           num2str(r.p_prc.ka), ', \omega=', num2str(r.p_prc.om), ', \vartheta=', num2str(r.p_prc.th)], ...
          'FontWeight', 'bold');
    ylabel('y, u, s(\mu_2)');
    axis([0 t -0.1 1.15]);
else
    title(['Input u (bottom dot row) and posterior probability of outcomes s(\mu_2) for \kappa=', ...
           num2str(r.p_prc.ka), ', \omega=', num2str(r.p_prc.om), ', \vartheta=', num2str(r.p_prc.th)], ...
      'FontWeight', 'bold');
    ylabel('u, s(\mu_2)');
    axis([0 t -0.1 1.1]);
end
plot(1:t, 0.5, 'k');
xlabel('Trial number');
hold off;


function [S, mets, badRxns, reversible]=constructS(equations,mets,rxns)
% constructS
%   Constructs a stoichiometric matrix from a cell array of equations
%
%   equations   cell array of equations on the form 'A + 2 B <=> 3 C',
%               where <=> indicates reversible and => irreversible reactions
%   mets        cell array of metabolites. All metabolites in the equations
%               must be present in the list (opt, default generated from
%               the equations)
%   rxns        cell array of reaction ids. This is only used for printing
%               reaction ids instead of equations in warnings/errors (opt,
%               default [])
%
%   S           the resulting stoichiometric matrix mets cell array with
%               metabolites that corresponds to the order in the S matrix
%   badRxns     boolean vector with the reactions that have one or more
%               metabolites as both substrate and product. An example would
%               be the phosphotransferase ATP + ADP <=> ADP + ATP. In the
%               stoichiometric matrix this equals to an empty reaction
%               which can be problematic
%   reversible  boolean vector with true if the equation is reversible
%
%   Usage: [S, mets, badRxns, reversible]=constructS(equations,mets)

equations=convertCharArray(equations);
switch nargin
    case 2
        mets=convertCharArray(mets);
    case 3
        rxns=convertCharArray(rxns);
end

badRxns=false(numel(equations),1);

%Check that no equations are too short to have reversibility data
I=cellfun(@numel,equations);
I=find(I<4,1);
if any(I)
    if isempty(rxns)
        EM=['The following equation does not have reversibility data: ' equations{I} ];
        dispEM(EM);
    else
        EM=['The reaction ' rxns{I} ' does not have reversibility data'];
        dispEM(EM);
    end
end

%Makes life a little easier
equations=strtrim(equations);
equations=fixEquations(equations);

if nargin<2
    mets=parseRxnEqu(equations);
end
if nargin<3
    rxns=[];
end

%Get which reactions are reversible
reversible=cellfun(@any,strfind(equations,' <=> '));

%Make them all reversible. This is not all that neat, but nevermind
equations=strrep(equations,' => ',' <=> ');

%Replace the the plus signs with some weird character that will be used for
%parsing
equations=strrep(equations,' + ', '€');

%Generate the stoichiometric matrix
S=zeros(numel(mets),numel(equations));

%Keep track of coefficients to be added to S-matrix
metsToS = cell(100000,1);
rxnsToS = zeros(100000,1);
coefToS = zeros(100000,1);
newEntry = 1;
%Loop through the equations and add the info to the S matrix
for i=1:numel(equations)
    %Start by finding the position of the (=> or <=>)
    arrowIndex=strfind(equations{i},' <=> ');

    if numel(arrowIndex)~=1
        if isempty(rxns)
            EM=['The following equation does not have reversibility data: ' equations{i} ];
            dispEM(EM);
        else
            EM=['The reaction ' rxns{i} ' does not have reversibility data'];
            dispEM(EM);
        end
    end

    reactants=regexp(equations{i}(1:arrowIndex-1),'€','split');
    products=regexp(equations{i}(arrowIndex+5:end),'€','split');

    %If the splitting character is at the end (if exchange rxns), then an
    %empty string will exist together with the real ones. Remove it
    reactants(cellfun(@isempty,reactants))=[];
    products(cellfun(@isempty,products))=[];

    %A vector where an element is -1 is the corresponding metabolite is a
    %reactant and 1 if it's a product
    multiplyWith=[ones(numel(reactants),1)*-1; ones(numel(products),1)];

    metabolites=strtrim([reactants products]);

    %Now loop through the reactants and see if the metabolite has a
    %coefficient (it will look as 'number name')
    for j=1:numel(metabolites)
        space=strfind(metabolites{j},' ');

        if isempty(space)
            %No coefficient
            coeff=1;
            name=metabolites{j};
        else
            coeff=str2double(metabolites{j}(1:space(1)));

            %If it was not a coefficiant
            if isnan(coeff)
                coeff=1;
                name=strtrim(metabolites{j});
            else
                name=strtrim(metabolites{j}(space+1:end));
            end
        end

        %Find the name in the mets list [a b]=ismember(name,mets);
        metsToS{newEntry}=name;
        rxnsToS(newEntry)=i;
        coefToS(newEntry)=coeff*multiplyWith(j);
        newEntry=newEntry+1;
    end
end
%Remove unused fields
metsToS(newEntry:end)=[];
rxnsToS(newEntry:end)=[];
coefToS(newEntry:end)=[];

%Match to mets array
[metsPresent,metsLoc]=ismember(metsToS,mets);

%Find badRxns
[~,I]=unique([rxnsToS,metsLoc],'rows','stable');
x=1:length(rxnsToS);
x(I)=[];
x=unique(rxnsToS(x));
badRxns(x)=true;

if any(~metsPresent)
    if isempty(rxns)
        error(['Could not find the following metabolites in the metabolite list: ',...
        strjoin(unique(metsToS(~metsPresent)),', ')],'')
    else
        missingMet = find(~metsPresent);
        missingMet = char(strcat(metsToS(missingMet),' (reaction:',rxns(rxnsToS(missingMet)),')\n'));
        error(['Could not find the following metabolites (reaction indicated) in the metabolite list: \n' ...
            missingMet '%s'],'');
    end
end
linearIndices=sub2ind(size(S),metsLoc,rxnsToS);
S(linearIndices)=coefToS;
S=sparse(S);
end

function equ=fixEquations(equ)
%If the equation starts with "=>" or "<=>" then add a space again. This is
%an alternative way to represent uptake reactions. The opposite way for
%producing reactions
equ=equ(:);
for i=1:numel(equ)
    if strcmp(equ{i}(1:2),'=>') || strcmp(equ{i}(1:3),'<=>')
        equ{i}=[' ' equ{i}];
    else
        if strcmp(equ{i}(end-1:end),'=>') || strcmp(equ{i}(end-2:end),'<=>')
            equ{i}=[equ{i} ' '];
        end
    end
end
end


function [media,density,medname,xbound,ybound,zbound] = dicomrt_readegs4phant(patient_position,filename)
% dicomrt_readegs4phant(patient_position,filename)
%
% Read a ct phantom which comply to BEAM/DOSXYZ file format
% Return media matrix and density matrix.
%
% patient_position is a code which correspond to one of the supported
% patient position cases
% Filename is an character string which contain the name of the egs4phantom file (no extension).
%
% Example:
%
% [A,B,medname]=dicomrt_readegs4phant(1,'demo')
%
% will store in A the medium number and in B the density read from the egs4phantom demo.egs4phant;
% Materials name are stored in medname. This information is required when converting
% dose2medium <-> dose2water.
%
% See also dicomrt_ctcreate, dicomrt_getPatientPosition
%
% Copyright (C) 2002 Emiliano Spezi (emiliano.spezi@physics.org)

filename=[filename,'.egs4phant'];
fid=fopen(filename);
nummedia = fscanf(fid,'%i');
medname=cell(nummedia,1);

for i=1:nummedia
    medname{i,1}=fgetl(fid);
end

for i=1:nummedia
    estepe(i,:) = fscanf(fid,'%f',1);
end

global xnum;
global ynum;
global znum;

xnum = fscanf(fid,'%i',1);
ynum = fscanf(fid,'%i',1);
znum = fscanf(fid,'%i',1);

xbound = fscanf(fid,'%f',xnum+1);
ybound = fscanf(fid,'%f',ynum+1);
zbound = fscanf(fid,'%f',znum+1);

xbound = dicomrt_mmdigit(xbound,7,10,'fix');
ybound = dicomrt_mmdigit(ybound,7,10,'fix');
zbound = dicomrt_mmdigit(zbound,7,10,'fix');

if patient_position == 1
    % 1st case: supported Patient Position is HFS
    %load the media matrix
    media=[];
    for k=1:znum
        media_temp = fscanf(fid,'%1i',[xnum, ynum]);
        media(:,:,k) = media_temp';
        line = fgets(fid);
    end
    % load the density matrix
    for k = 1:znum
        density_temp = fscanf(fid,'%f',[xnum, ynum]);
        density(:,:,k) = density_temp';
    end
elseif patient_position == 2
    % 2nd case: supported Patient Position is FFS
    %load the media matrix
    media=[];
    for k=1:znum
        media_temp = fscanf(fid,'%1i',[xnum, ynum]);
        media(:,:,k) = media_temp';
        line = fgets(fid);
    end
    % load the density matrix
    for k = 1:znum
        density_temp = fscanf(fid,'%f',[xnum, ynum]);
        density(:,:,k) = density_temp';
    end
end
fclose(fid);


function Q = cost(V,t,tc,Run)
%
% Least-squares cost function for the IL model
%
% INPUT:
% Run = stick function
% tc = time course
% t = vector of time points
% V = parameters
%
% OUTPUT:
% Q = cost
%
% By Martin Lindquist and Tor Wager
% Edited 12/12/06
%

len = length(Run);

h1 = Get_Logit(V(1:7),t);           % Get IL model corresponding to parameters V
yhat = conv(Run, h1);               % Convolve IL model with stick function
yhat = yhat(1:len);

Q = sum((yhat-tc).^2);              % Calculate cost function

return


function [w]=iwtt(v,wtt_transform)
%Inverse WTT transform with previously computed filters
%   W=IWTT(V,WTT_TRANSFORM) computes the inverse WTT tranformation of a
%   given tensor with filters, stored in the WTT_TRANSFORM structure.
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

u=wtt_transform.u;
r=wtt_transform.rks;
sz=wtt_transform.sz;
w=iwtt_loc(v,u,r,sz);
return
end
function [w]=iwtt_loc(v,u,r,sz)
%[W]=IWTT(V,U,R,SZ)
%Applies Inverse WTT transform to vector V with linear filters U
%and ranks R
%SZ is optional
if ( nargin == 1 )
  sz=size(v);% d=numel(sz);
else
 % d=numel(sz);
end
%Apply one transform back
  N=numel(v);
  v=reshape(v,[r(1)*sz(1),N/(r(1)*sz(1))]);
if ( numel(u) == 1 )
   w=u{1}*v;
    w=reshape(w,[r(1),N/(r(1))]);
else
  %Simplest one is recursion
  w=v;
  w0=v(1:r(2),:);
  unew=u(2:numel(u)); rnew=r(2:numel(r)); sznew=[sz(2),sz(3:numel(sz))];
  w0=iwtt_loc(w0,unew,rnew,sznew);
  w(1:r(2),:)=w0;
  m=size(u{1},1);
  w=reshape(w,[m,N/m]);
  w=u{1}*w;
  w=reshape(w,[r(1),N/r(1)]);
end
  return
end


function [pc, fvecm2, p, c] = gp_predcm(gp,x,y,varargin)
%GP_PREDCM  Corrections for latent marginal posterior
%
%  Description
%    [PC, FVEC, P, C] = GP_PREDCM(GP, X, Y, XT, OPTIONS) Evaluates the
%    corrected marginal posterior of latent variable at given indices
%    of XT or X if XT is empty or not given. Marginal posterior
%    corrections are evaluated in 9 Gauss-Hermite points, after which
%    piecewise cubic Hermite interpolation is used interpolate
%    logarithm of the correction terms to a finer grid. Returns tilted
%    distribution P if XT is empty or equal to X, otherwise predictive
%    distribution, corrected predictive/tilted distribution PC and
%    correction terms C, where PC_i = P_i*C_i for every grid point i
%    in grid FVEC. FVEC is linearly spaced grid from predictive
%    distribution between mean minus/plus 4 standard deviations
%
%
%   OPTIONS is optional parameter-value pair
%      z      - optional observed quantity in triplet (x_i,y_i,z_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, expected value
%               for ith case.
%      ind    - Index vector or scalar defining the indices of data
%               points at which the marginal posterior corrections are
%               done. Default = 1.
%      fcorr  - Method used for evaluating correction terms C. Possible
%               methods are 'fact' (default) for EP and either 'fact'
%               or 'cm2' (default) for Laplace. If method is 'on',
%               the default methods are used.
%      ng     - Number of grid points evaluated from the spline. Default is 50.
%
%   Reference
%     Cseke & Heskes (2011). Approximate Marginals in Latent Gaussian
%     Models. Journal of Machine Learning Research 12 (2011), 417-454
%
%   See also
%     DEMO_IMPROVEDMARGINALS
%
% Copyright (c) 2011,2013 Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

ip=inputParser;
ip.addRequired('gp',@isstruct);
ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
%ip.addRequired('fvec',  @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addOptional('xt', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('ind', 1, @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('ng', 50, @(x) isreal(x) && all(isfinite(x(:))) && x > 1)
ip.addParamValue('fcorr', 'on', @(x) ismember(x, {'fact', 'cm2', 'on','lr'}))
ip.addParamValue('tstind', [], @(x) isempty(x) || iscell(x) ||...
                   (isvector(x) && isreal(x) && all(isfinite(x)&x>0)))
if rem(size(varargin,2), 2) == 0
  ip.parse(gp, x, y, [],varargin{:});
else
  ip.parse(gp, x, y, varargin{:});
end
tstind = ip.Results.tstind;
z = ip.Results.z;
ind = ip.Results.ind;
xt = ip.Results.xt;
ng = ip.Results.ng;
predictive=false;
gplik=gp.lik;
n=size(x,1);
[Ef, Covf] = gp_jpred(gp,x,y,x, 'z', z, 'tstind', 1:n);
Covf=full(Covf);
if ismember('fcorr', ip.UsingDefaults)
  if isequal(gp.latent_method, 'Laplace')
    % Default for Laplace
    fcorr='cm2';
  else
    % Default for EP
    fcorr='fact';
  end
else
  fcorr=ip.Results.fcorr;
end
ind=ind(:);
if ~isempty(xt) && ~isequal(xt, x)
  % Predictive equations if given xt, mind that if xt(ind) is in training
  % input x, predictive equations might not work correctly.
  predictive = true;
  [Ef2, Varf2]=gp_pred(gp,x,y,xt,'z',z,'tstind',tstind);
%   [Ef2, Covf2] = gp_jpred(gp,x,y,xt,'z',z, 'tstind', tstind);
%   Covf2=full(Covf2);
end
nin = 11;
fvecm=zeros(nin,length(ind));
fvecm2=zeros(ng,length(ind));
for i1=1:length(ind)
  i2=ind(i1);
  % Form evaluation points for spline formation & grid points to be
  % evaluated from the spline
  minf = 6;
  maxf = 6;
  if ~predictive
    fvecm(:,i1)=Ef(i2)+[-3.668 -2.783 -2.026 -1.326 -0.657 0 0.657 1.326 2.026 2.783 3.668].*sqrt(Covf(i2,i2));
%     fvecm(:,i1)=Ef(i2)+[-3.191 -2.267 -1.469 -0.724 0 0.724 1.469 2.267 3.191].*sqrt(Covf(i2,i2));
    fvecm2(:,i1)=linspace(Ef(i2)-minf.*sqrt(Covf(i2,i2)), Ef(i2)+maxf.*sqrt(Covf(i2,i2)),ng)';
  else
    fvecm(:,i1)=Ef2(i2)+[-3.668 -2.783 -2.026 -1.326 -0.657 0 0.657 1.326 2.026 2.783 3.668].*sqrt(Varf2(i2));
%     fvecm(:,i1)=Ef2(i2)+[-3.191 -2.267 -1.469 -0.724 0 0.724 1.469 2.267 3.191].*sqrt(Covf2(i2,i2));
    fvecm2(:,i1)=linspace(Ef2(i2)-minf.*sqrt(Varf2(i2)), Ef2(i2)+maxf.*sqrt(Varf2(i2)),ng)';
  end
end
lc = zeros(nin, length(ind));
pc = zeros(ng, size(ind,1)); lp = zeros(ng,size(ind,1)); c = zeros(ng,size(ind,1));
lp2 = zeros(nin,size(ind,1)); p = zeros(ng,size(ind,1));

switch gp.latent_method
  case 'EP'

    if isequal(fcorr, 'lr')
        [Efloo,Varfloo]=gpep_loopred(gp,x,y,'z',z);
    end

    switch fcorr
      case {'fact', 'lr'}
        [tmp, tmp, tmp, param] = gpep_e(gp_pak(gp), gp, x,y,'z',z);
        [tautilde, nutilde, muvec_i, sigm2vec_i] = ...
              deal(param.tautilde, param.nutilde, param.muvec_i, param.sigm2vec_i);

        % Compute tilted moments
        logM02 = gp.lik.fh.tiltedMoments(gp.lik, y, 1:n, sigm2vec_i, muvec_i, z);

        if predictive
          K_ff = gp_trcov(gp, x);
        end

        % Loop through grid indices
        for i1=1:size(ind,1)
          fvec = fvecm(:,i1);
          if ~predictive
            inds=[1:(ind(i1)-1) (ind(i1)+1):n];
            cii = Covf(ind(i1),ind(i1));
            if isempty(z)
              z_ind = [];
            else
              z_ind = z(ind(i1));
            end

            % Here we keep track of normalizing constants so we dont have to
            % normalize distributions at any point.
            %     Z_q = sqrt(2*pi*cii);
%             logM0 = gp.lik.fh.tiltedMoments(gp.lik, y, ind(i1), sigm2vec_i(ind(i1)), muvec_i(ind(i1)), z);
%             Z_p = exp(logM0)*sqrt(2*pi)*sqrt(sigm2vec_i(ind(i1))+1./tautilde(ind(i1)))*exp(0.5*(muvec_i(ind(i1))-nutilde(ind(i1))./tautilde(ind(i1))).^2/(sigm2vec_i(ind(i1))+1./tautilde(ind(i1))));

            % Function handle to marginal distribution without any fcorr parameters
            if isequal(fcorr, 'fact')
                cav = @(f) norm_lpdf(f,Ef(ind(i1)),sqrt(cii)) - norm_lpdf(f, nutilde(ind(i1))/tautilde(ind(i1)), 1/sqrt(tautilde(ind(i1))));
            else
                cav = @(f) norm_lpdf(f, Efloo(ind(i1)), sqrt(Varfloo(ind(i1))));
            end
            fh_p = @(f) (arrayfun(@(a) gplik.fh.ll(gplik, y(ind(i1)), a, z_ind), f)) + cav(f);
          else
            inds=1:n;
            cii = Varf2(ind(i1));
            fh_p = @(f) norm_lpdf(f,Ef2(ind(i1)),sqrt(cii));
          end

          % Loop through grid points
          for i=1:nin

            % Variance and mean for global Gaussian approximation conditioned on
            % other data grid points, q(x_j|x_i) or in predictive case, q(x_j,
            % x_*)
            if ~predictive
              cji = Covf(ind(i1),:);% cji(ind(i1)) = [];
              cjj = Covf;% cjj(ind(i1),:) = []; cjj(:,ind(i1)) = [];
              ci = diag(cjj)-(cji'*(1/cii)).*cji';
              mf = Ef; %mf(ind(i1)) = [];
              mu = mf+cji'./cii.*(fvec(i)-Ef(ind(i1)));
            else
              K_fstar = gp_cov(gp,  x, xt(ind(i1),:));
              cjj = Covf;
              cji = (K_fstar'/K_ff)*cjj;
              ci = diag(cjj)-cji'.*(1/cii).*cji';
              mu = Ef+cji'./cii.*(fvec(i)-Ef2(ind(i1)));
            end
            % Loop through other points in x, exclude point to which current latent grid
            % corresponds to (if not predictive).
            lZtilde=logM02 + log(sqrt(2*pi)) + log(sqrt(sigm2vec_i+1./tautilde)) + 0.5*(muvec_i-nutilde./tautilde).^2./(sigm2vec_i+1./tautilde);
            m1 = nutilde./tautilde;
            s1 = 1./sqrt(tautilde);
            m2 = mu;
            s2 = sqrt(ci);

            s = sqrt(1./(1./s2.^2 - 1./s1.^2));
            m = (m2./s2.^2 - m1./s1.^2).*s.^2;
            lZ = log(s1) - log(s2) - 1./(2*(-s1.^2+s2.^2)).*(m1-m2).^2 +log(sqrt(2*pi*s.^2));
            lc_ii = lZ(inds) - lZtilde(inds) + gp.lik.fh.tiltedMoments(gplik, y(inds), 1:length(inds), s(inds).^2, m(inds), z);
            lc(i,i1) = sum(lc_ii);
            %p(i,i1) = fh_p(fvec(i,i1));

          end
          lp(:,i1) = fh_p(fvecm2(:,i1));
          lp2(:,i1) = fh_p(fvecm(:,i1));
          lp(:,i1) = lp(:,i1)-max(lp(:,i1));
          lp2(:,i1) = lp2(:,i1)-max(lp2(:,i1));
        end
      otherwise
        error('Invalid method for EP, use fact');
    end
  case 'Laplace'

    [tmp, tmp, tmp, param] = gpla_e(gp_pak(gp), gp, x,y,'z',z);
    f_mode = param.f;
    if ~isempty(z)
      ll = arrayfun(@(f,yy, zz) gplik.fh.ll(gplik, yy, f, zz), f_mode, y, z);
    else
      ll = arrayfun(@(f,yy) gplik.fh.ll(gplik, yy, f, z), f_mode, y);
    end
    llg = gplik.fh.llg(gplik, y, f_mode, 'latent', z);
    llg2 = gplik.fh.llg2(gplik, y, f_mode, 'latent', z);
    K_ff = gp_trcov(gp, x);
    if isequal(fcorr, 'lr')
        [Efloo,Varfloo]=gpla_loopred(gp,x,y,'z',z,'method','lrs');
    end

    switch fcorr
      case 'fact'
        % Loop through grid indices
        for i1=1:size(ind,1)
          fvec = fvecm(:,i1);
          if ~predictive
            cii = Covf(ind(i1),ind(i1));
            if isempty(z)
              z_ind = [];
            else
              z_ind = z(ind(i1));
            end

            % Function handle to marginal distribution without any fcorr parameters
            t_tilde = @(f)  (ll(ind(i1)) + (f-f_mode(ind(i1)))*llg(ind(i1)) + 0.5*(f-f_mode(ind(i1))).^2*llg2(ind(i1)));
            fh_p = @(f) (arrayfun(@(a) gplik.fh.ll(gplik, y(ind(i1)), a, z_ind), f)) - t_tilde(f) + norm_lpdf(f,Ef(ind(i1)),sqrt(cii));
          else
            cii = Varf2(ind(i1));
            fh_p = @(f) norm_lpdf(f,Ef2(ind(i1)),sqrt(cii));
          end

          if ~predictive
            cji = Covf(ind(i1),:); %cji(ind(i1)) = [];
            cjj = Covf; %cjj(ind(i1),:) = []; cjj(:,ind(i1)) = [];
            ci = diag(cjj)-(cji'*(1/cii)).*cji';
            mf = Ef; %mf(ind(i1)) = [];
            inds=[1:(ind(i1)-1) (ind(i1)+1):n];
          else
            K_fstar = gp_cov(gp,  x, xt(ind(i1),:));
            cjj = Covf;
            cji = (K_fstar'/K_ff)*cjj;
            ci = diag(cjj)-cji'.*(1/cii).*cji';
            inds=1:n;
          end
          % Loop through grid points
          for i=1:nin

            % Variance and mean for global gaussian approximation conditioned on
            % other data grid poins, q(x_j|x_i) or in predictive case, q(x_j,
            % x_*)
            if ~predictive
              mu = mf+cji'./cii.*(fvec(i)-Ef(ind(i1)));
            else
              mu = Ef+cji'./cii.*(fvec(i)-Ef2(ind(i1)));
            end
            m1 = (f_mode-llg./llg2);
            s1 = sqrt(-1./llg2);
            lC1 = ll+llg2.*f_mode.^2-llg2.*m1.^2 - llg.*f_mode;
            m2 = mu;
            s2 = sqrt(ci);
            lC2 = log(1./sqrt(2*pi*s2.^2));

            s = sqrt(1./(1./s2.^2 - 1./s1.^2));
            m = (m2./s2.^2 - m1./s1.^2).*s.^2;

            lZ = lC1 - lC2 - 1./(2*(-s1.^2+s2.^2)).*(m1-m2).^2 + log(sqrt(2*pi*s.^2));
            lc_ii = lZ(inds) + gp.lik.fh.tiltedMoments(gplik, y(inds), 1:length(inds), s(inds).^2, m(inds), z);

            %c(i,i1) = prod(c_ii);
            lc(i,i1) = sum(lc_ii);
            %p(i,i1) = fh_p(fvec(i,i1));

          end
          lp(:,i1) = fh_p(fvecm2(:,i1));
          lp2(:,i1) = fh_p(fvecm(:,i1));
          lp(:,i1) = lp(:,i1)-max(lp(:,i1));
          lp2(:,i1) = lp2(:,i1)-max(lp2(:,i1));
        end

      case {'cm2', 'lr'}
        % Loop through grid indices
        for i1=1:size(ind,1)
          fvec = fvecm(:,i1);
          if ~predictive
            cii = Covf(ind(i1),ind(i1));
            if isempty(z)
              z_ind = [];
            else
              z_ind = z(ind(i1));
            end

            % Function handle to marginal distribution without any fcorr parameters
            if isequal(fcorr, 'cm2')
%                t_tilde(f) = @(f) (ll(ind(i1)) + (f-f_mode(ind(i1)))*llg(ind(i1)) + 0.5*(f-f_mode(ind(i1))).^2*llg2(ind(i1)));
                cav = @(f) norm_lpdf(f,Ef(ind(i1)),sqrt(cii)) - (ll(ind(i1)) + (f-f_mode(ind(i1)))*llg(ind(i1)) + 0.5*(f-f_mode(ind(i1))).^2*llg2(ind(i1)));
            else
                cav = @(f) norm_lpdf(f, Efloo(ind(i1)), sqrt(Varfloo(ind(i1))));
            end
            fh_p = @(f) arrayfun(@(a) gplik.fh.ll(gplik, y(ind(i1)), a, z_ind), f) + cav(f);
%             fh_p = @(f) (arrayfun(@(a) gplik.fh.ll(gplik, y(ind(i1)), a, z_ind), f)) - t_tilde(f) + norm_lpdf(f,Ef(ind(i1)),sqrt(cii));
          else
            cii = Varf2(ind(i1));
            fh_p = @(f) norm_lpdf(f,Ef2(ind(i1)),sqrt(cii));
          end

          if ~predictive
            cji = Covf(ind(i1),:);
            cji(ind(i1)) = [];
            inds=[1:(ind(i1)-1) (ind(i1)+1):n];
          else
            K_fstar = gp_cov(gp,  x, xt(ind(i1),:));
            cji = (K_fstar'/K_ff)*Covf;
            inds=1:n;
          end
          mf = Ef(inds);
          cjj = Covf(inds,inds);
          y_tmp = y(inds);
          ci = cjj - cji'*(1/cii)*cji;
          f_mode_tmp=f_mode(inds);
          llg2_mode=diag(llg2(inds));
          llg_mode=llg(inds);
          ll_mode=sum(ll(inds));

          lnZ0 = -1/cii - ll_mode + 0.5*log(cii);
          icW = -eye(size(ci))/ci  - llg2_mode;
          % Loop through grid points
          for i=1:nin

            if isempty(z)
              z_tmp = [];
            else
              z_tmp = z(inds);
            end
            % Compute conditional covariance matrices and mean vector
            if ~predictive
              mu = mf+cji'./cii.*(fvec(i)-Ef(ind(i1)));
            else
              mu = mf+cji'./cii.*(fvec(i)-Ef2(ind(i1)));
            end
            W = -diag(gplik.fh.llg2(gplik, y_tmp, mu, 'latent', z_tmp));
            deriv = gplik.fh.llg(gplik, y_tmp, mu, 'latent', z_tmp);
            logll = gplik.fh.ll(gplik,y_tmp, mu, z_tmp);

            % Computation of correction term by integrating the second order Taylor
            % expansion of product of global Gaussian approximation conditioned on latent
            % value x_i, q(x_-i|x_i), and t_-i(x_-i)/ttilde_-i(x_-i)
            mu1=mu-f_mode_tmp;
            lnZ = lnZ0 + logll - mu1'*llg_mode - 0.5*mu1'*llg2_mode*mu1;
            mu2=deriv-llg_mode-(mu1'*llg2_mode)';
            lnZ = lnZ - (0.5*mu2'/(icW - W))*mu2;
            lnZ = lnZ  - evaluate_q(diag(W+llg2_mode), ci);

            lc(i,i1) = lnZ;
            %p(i,i1) = fh_p(fvec(i,i1));

          end
          lp(:,i1) = fh_p(fvecm2(:,i1));
          lp2(:,i1) = fh_p(fvecm(:,i1));
          lp(:,i1) = lp(:,i1)-max(lp(:,i1));
          lp2(:,i1) = lp2(:,i1)-max(lp2(:,i1));

        end
    end

end

for i1=1:length(ind)
  fvec = fvecm(:,i1);

  % Interpolate correction to these grid points
  % using piecewise cubic Hermite interpolation
  fvec2 = fvecm2(:,i1);
  lc(:,i1)=lc(:,i1)-lc(6,i1);

  % Check that the corrected distribution has decreasing tails
  lctmp=lc(:,i1);
  lptmp=(lp2(:,i1)+lc(:,i1));
  fvectmp=fvec;
  while lptmp(end)-lptmp(end-1) > 0
    lctmp=lctmp(1:end-1);
    lptmp=lptmp(1:end-1);
    fvectmp=fvectmp(1:end-1);
  end
  while lptmp(1)-lptmp(2) > 0
    lctmp=lctmp(2:end);
    lptmp=lptmp(2:end);
    fvectmp=fvectmp(2:end);
  end

  if (sum(isnan(lctmp))>0)
    warning('NaNs in moment computations')
    lctmp(isnan(lctmp))=0;
  end
  lc2(:,i1) = interp1(fvectmp, lctmp, fvec2, 'pchip');

  % Make correction
  pc(:,i1)=exp(lc2(:,i1) + lp(:,i1));

  if any(isnan(pc(:,i1))) || any(isinf(pc(:,i1)))
    warning('NaNs in moment computations')
    pc(isnan(pc(:,i1)),i1)=0;
    pc(isinf(pc(:,i1)),i1)=0;
  end
  if any(pc(:,i1)<eps)
    pc(pc(:,i1)<eps,i1)=0;
    iz1=find(pc(1:ceil(0.5*ng),i1)==0,1);
    iz2=ceil(0.5*ng)+find(pc(ceil(0.5*ng):ng,i1)==0,1,'last');
    pc([1:iz1 iz2:ng],i1)=0;
  end

  % Form corrected distribution & normalize
  p(:,i1) = exp(lp(:,i1))./trapz(fvec2,exp(lp(:,i1)));
  pc(:,i1) = pc(:,i1)./trapz(fvec2, pc(:,i1));
  c(:,i1) = pc(:,i1)./p(:,i1);

end
if isequal(fcorr, 'lr')
    pc=p;
end
end

function [lnZ_q,L1,L2]=evaluate_q(tau_q,K)

%%%%%%%%%%%%%%%%
% q-distribution

%   n=length(nu_q);
ii1=find(tau_q>0); n1=length(ii1); W1=sqrt(tau_q(ii1));
ii2=find(tau_q<0); n2=length(ii2); W2=sqrt(abs(tau_q(ii2)));

if ~isempty(ii1)
  % Cholesky decomposition for positive sites
  L1=(W1*W1').*K(ii1,ii1);
  L1(1:n1+1:end)=L1(1:n1+1:end)+1;
  L1=chol(L1);

else
  L1=1;
end

if ~isempty(ii2)
  % Cholesky decomposition for negative sites
  V=bsxfun(@times,K(ii2,ii1),W1')/L1;
  L2=(W2*W2').*(V*V'-K(ii2,ii2));
  L2(1:n2+1:end)=L2(1:n2+1:end)+1;

  L2=chol(L2);

else
  L2=1;
end

% log normalization
lnZ_q = sum(log(diag(L1))) + sum(log(diag(L2)));
end