function femIF = genP1IFEM3DFace(mesh,fem,femI)
%% Usage: Generate Quadrature Information on Interface Faces Used in PPIFEM
%         Each interface face (triangle) is cut into 3 small triangles
%            A1                A1
%           D  E      or      E  D         equivalent
%          A2  A3            A3   A2
%
%          p = [A1;D;E;A2;A3];
%          t = [1 2 3; 2 3 4; 3 4 5]
%
% femIF.tL --- global index of left element
% femIF.tR --- global index of right element
% femIF.basL --- 3*nfi-4-4 matrix basis functions of left elem on small tri
% femIF.basR --- 3*nfi-4-4 matrix basis functions of right elem on small tri
% femIF.gx --- Gaussian x nodes on small tri
% femIF.gy --- Gaussian y nodes on small tri
% femIF.gz --- Gaussian z nodes on small tri
% femIF.gw --- Gaussian weights on small tri (3 Gaussian pt for linear IFEM)
% femIF.area --- Areas of all small tri
% femIF.normal --- unit normal vector of each small tri

% Last Modified by Xu Zhang 08/07/2020
%%
nfI = -min(mesh.fLoc); % number of interface faces
intFID = find(mesh.fLoc<0);
nfIB = size(find(mesh.f_t(intFID,2)==0),1); % # of interface faces on Boundary
nfII = nfI - nfIB; % # of internal interface faces: two tetra share it L&R
tL = zeros(3*nfII,4); tR = zeros(3*nfII,4);
basL = zeros(3*nfII,4,4); basR = zeros(3*nfII,4,4);
gx = zeros(3*nfII,3); gy = zeros(3*nfII,3); gz = zeros(3*nfII,3);
A = zeros(3*nfII,1); normal = zeros(3*nfII,3);

tB = zeros(3*nfIB,4); basB = zeros(3*nfIB,4,4);
gxB = zeros(3*nfIB,3); gyB = zeros(3*nfIB,3); gzB = zeros(3*nfIB,3);
AB = zeros(3*nfIB,1); normalB = zeros(3*nfIB,3);

id = 0; idB = 0;
for i = 1:nfI
    %% Form small triangular partition on the interface face
    fID = intFID(i); % face index
    f_e = mesh.f_e(fID,:); % three surrounding edge index
    idE = find(mesh.eLoc(f_e)<0); % find index of two interface edges
    tmp = [mesh.e(f_e(idE(1)),:), mesh.e(f_e(idE(2)),:)];
    nd1 = sum(tmp) - sum(unique(tmp)); % the node of two interface edges
    nd2 = sum(mesh.e(f_e(idE(1)),:)) - nd1;
    nd3 = sum(mesh.e(f_e(idE(2)),:)) - nd1;
    p = [mesh.p(nd1,:); mesh.eIntP(-mesh.eLoc(f_e(idE(1))),:); ...
        mesh.eIntP(-mesh.eLoc(f_e(idE(2))),:); mesh.p(nd2,:); mesh.p(nd3,:)];
    t = [1 2 3; 2 3 4; 3 4 5];

    %% gx gy gz on a triangle with 3 internal point, accurate upto pd = 2
    X1 = p(t(:,1),:);  X2 = p(t(:,2),:);   X3 = p(t(:,3),:);
    G = zeros(3,9);  w1 = 2/3;  w2 = 1/6; % see gaussPtri.m
    G(:,[1,4,7]) = w1*X1 + w2*(X2+X3);
    G(:,[2,5,8]) = w1*X2 + w2*(X1+X3);
    G(:,[3,6,9]) = w1*X3 + w2*(X1+X2);

    %% triangle area on three-dimension.
%     x1 = X1(:,1);  y1 = X1(:,2);   z1 = X1(:,3);
%     x2 = X2(:,1);  y2 = X2(:,2);   z2 = X2(:,3);
%     x3 = X3(:,1);  y3 = X3(:,2);   z3 = X3(:,3);
%     AT = 1/2*(((x1-x3).*(y2-y1) - (x1-x2).*(y3-y1)).^2 + ...
%         ((y1-y3).*(z2-z1) - (y1-y2).*(z3-z1)).^2 + ...
%         ((z1-z3).*(x2-x1) - (z1-z2).*(x3-x1)).^2).^(1/2);
    AT = TriArea3D(X1,X2,X3);

    %% Left and Right Element
    tIDL = mesh.f_t(fID,1); % element index of left element
    tIDR = mesh.f_t(fID,2); % element index of right element
    if tIDR > 0 % Internal Face
        tIDLi = -mesh.tLoc(tIDL); % intf elem index of left element
        tIDRi = -mesh.tLoc(tIDR); % intf elem index of right element

        %% Determine piece
        nd1ID = mesh.pLoc(nd1); tLp = femI.plusPC(tIDLi); tRp = femI.plusPC(tIDRi);
        if (nd1ID < 0 && tLp == 1) || (nd1ID > 0 && tLp == 2)
            basL(id+1,:,:) = femI.bas2(tIDLi,:,:);
            basL(id+2,:,:) = femI.bas1(tIDLi,:,:);
            basL(id+3,:,:) = femI.bas1(tIDLi,:,:);
        elseif (nd1ID < 0 && tLp == 2) || (nd1ID > 0 && tLp == 1)
            basL(id+1,:,:) = femI.bas1(tIDLi,:,:);
            basL(id+2,:,:) = femI.bas2(tIDLi,:,:);
            basL(id+3,:,:) = femI.bas2(tIDLi,:,:);
        end
        if (nd1ID < 0 && tRp == 1) || (nd1ID > 0 && tRp == 2)
            basR(id+1,:,:) = femI.bas2(tIDRi,:,:);
            basR(id+2,:,:) = femI.bas1(tIDRi,:,:);
            basR(id+3,:,:) = femI.bas1(tIDRi,:,:);
        elseif (nd1ID < 0 && tRp == 2) || (nd1ID > 0 && tRp == 1)
            basR(id+1,:,:) = femI.bas1(tIDRi,:,:);
            basR(id+2,:,:) = femI.bas2(tIDRi,:,:);
            basR(id+3,:,:) = femI.bas2(tIDRi,:,:);
        end

        gx(id+1:id+3,:) = G(:,1:3);
        gy(id+1:id+3,:) = G(:,4:6);
        gz(id+1:id+3,:) = G(:,7:9);

        A(id+1:id+3,:) = AT;
        normal(id+1:id+3,:) = repmat(mesh.f_norm(fID,:),3,1);

        %% tL and tR, use locID, b/c index on interface cell is different
        temp = fem.t(tIDL,:);
        temp1 = temp(femI.locID(tIDLi,:));
        tL(id+1:id+3,:) = repmat(temp1,3,1);

        temp = fem.t(tIDR,:);
        temp2 = temp(femI.locID(tIDRi,:));
        tR(id+1:id+3,:) = repmat(temp2,3,1);
        id = id+3;

    elseif tIDR == 0 % Boundary Face
        tIDLi = -mesh.tLoc(tIDL); % intf elem index of left element

        %% Determine piece: only one element.
        nd1ID = mesh.pLoc(nd1); tLp = femI.plusPC(tIDLi);
        if (nd1ID < 0 && tLp == 1) || (nd1ID > 0 && tLp == 2)
            basB(idB+1,:,:) = femI.bas2(tIDLi,:,:);
            basB(idB+2,:,:) = femI.bas1(tIDLi,:,:);
            basB(idB+3,:,:) = femI.bas1(tIDLi,:,:);
        elseif (nd1ID < 0 && tLp == 2) || (nd1ID > 0 && tLp == 1)
            basB(idB+1,:,:) = femI.bas1(tIDLi,:,:);
            basB(idB+2,:,:) = femI.bas2(tIDLi,:,:);
            basB(idB+3,:,:) = femI.bas2(tIDLi,:,:);
        end

        gxB(idB+1:idB+3,:) = G(:,1:3);
        gyB(idB+1:idB+3,:) = G(:,4:6);
        gzB(idB+1:idB+3,:) = G(:,7:9);

        AB(idB+1:idB+3,:) = AT;
        normalB(idB+1:idB+3,:) = repmat(mesh.f_norm(fID,:),3,1);

        %% tB use locID, b/c index on interface cell is different
        temp = fem.t(tIDL,:);
        temp1 = temp(femI.locID(tIDLi,:));
        tB(idB+1:idB+3,:) = repmat(temp1,3,1);
        idB = idB+3;
    end
end

femIF = struct('tL',tL,'tR',tR,'tB',tB,'basL',basL,'basR',basR,'basB',basB, ...
    'gx',gx,'gy',gy,'gz',gz,'gxB',gxB,'gyB',gyB,'gzB',gzB,'area',A,'areaB',AB,...
    'gw',[1/3;1/3;1/3],'normal',normal,'normalB',normalB);


% Implement the back propagation of an LSTM layer.
% Author: Xiong Xiao, Temasek Labs, NTU, Singapore.
% Last modified: 13 Oct 2015
%
function [grad, grad_W, grad_b] = B_LSTM(input, LSTM_layer, future_layers)
W = LSTM_layer.W;
if strcmpi(class(input), 'gpuArray'); useGPU=1; else useGPU = 0; end
precision = class(gather(input(1)));

[dim, nFr, nSeg] = size(input);
input = permute(input, [1 3 2]);

nCell = LSTM_layer.dim(1);  % number of LSTM cells in the layer

useHidden = 1;
usePastState = 1;
usePastStateAsFeature = LSTM_layer.usePastState;

future_layer_grad = 0;
for fli = 1:length(future_layers)
    future_layer_grad = future_layer_grad + future_layers{fli}.grad;      % this is the gradient of LSTM output from the cost function
end
grad_ht_cost = future_layer_grad;
grad_ht_cost = permute(grad_ht_cost,[1 3 2]);

ft = LSTM_layer.ft;
it = LSTM_layer.it;
ot = LSTM_layer.ot;
Ct_raw = LSTM_layer.Ct_raw;
Ct = LSTM_layer.Ct;
ht = LSTM_layer.a;
ht = permute(ht, [1 3 2]);
Ct0 = LSTM_layer.Ct0;
ht0 = LSTM_layer.ht0;

% allocate memory for the gradients of gates and states
if useGPU == 0
    grad_xt = zeros(dim, nSeg, nFr, precision);
    grad_ft = zeros(nCell, nSeg, nFr, precision);     % forget gates
    grad_it = zeros(nCell, nSeg, nFr, precision);     % input gates
    grad_ot = zeros(nCell, nSeg, nFr, precision);     % output gates
    grad_Ct_raw = zeros(nCell, nSeg, nFr, precision); % candidate cell states
    grad_Ct = zeros(nCell, nSeg, nFr, precision);     % cell states
    grad_Ct_future = zeros(nCell, nSeg, nFr, precision);     % cell states
    grad_ht = zeros(nCell, nSeg, nFr, precision);     % hidde layer output, i.e. the output of the LSTM layer
    grad_ht_future = zeros(nCell, nSeg, nFr, precision);     % hidde layer output, i.e. the output of the LSTM layer
    grad_zt = zeros(nCell*4,nSeg, nFr, precision);
else
    grad_xt = gpuArray.zeros(dim, nSeg, nFr, precision);
    grad_ft = gpuArray.zeros(nCell, nSeg, nFr, precision);     % forget gates
    grad_it = gpuArray.zeros(nCell, nSeg, nFr, precision);     % input gates
    grad_ot = gpuArray.zeros(nCell, nSeg, nFr, precision);     % output gates
    grad_Ct_raw = gpuArray.zeros(nCell, nSeg, nFr, precision); % candidate cell states
    grad_Ct = gpuArray.zeros(nCell, nSeg, nFr, precision);     % cell states
    grad_Ct_future = gpuArray.zeros(nCell, nSeg, nFr, precision);     % cell states
    grad_ht = gpuArray.zeros(nCell, nSeg, nFr, precision);     % hidde layer output, i.e. the output of the LSTM layer
    grad_ht_future = gpuArray.zeros(nCell, nSeg, nFr, precision);     % hidde layer output, i.e. the output of the LSTM layer
    grad_zt = gpuArray.zeros(nCell*4,nSeg, nFr, precision);
end

for t = nFr:-1:1
    % compute the gradient of ht and Ct that requires gradients from
    % future. At frame nFr, the future gradients are initialized to 0.
    Ct_raw_curr = Ct_raw(:,:,t);
    if useHidden
        grad_ht_curr = grad_ht_future(:,:,t) + grad_ht_cost(:,:,t);
    else
        grad_ht_curr = grad_ht_cost(:,:,t);
    end
    tanh_Ct = tanh(Ct(:,:,t));
    grad_Ct(:,:,t) = grad_Ct_future(:,:,t) + grad_ht_curr .* (1-tanh_Ct.*tanh_Ct) .* ot(:,:,t);

    % compute the gradient of gates and candidate states
    if usePastState
        if t==1
            grad_ft(:,:,t) = grad_Ct(:,:,t) .* Ct0;
            grad_Ct_future0 = grad_Ct(:,:,t) .* ft(:,:,t);
        else
            grad_ft(:,:,t) = grad_Ct(:,:,t) .* Ct(:,:,t-1);
            grad_Ct_future(:,:,t-1) = grad_Ct(:,:,t) .* ft(:,:,t);
        end
    end
    grad_Ct_raw(:,:,t) = grad_Ct(:,:,t) .* it(:,:,t);
    grad_it(:,:,t) = grad_Ct(:,:,t) .* Ct_raw_curr;
    grad_ot(:,:,t) = grad_ht_curr .* tanh(Ct(:,:,t));

    % compute the gradient of the gates before the activation function.

    grad_zCt_raw = grad_Ct_raw(:,:,t) .* (1-Ct_raw_curr.*Ct_raw_curr);
    if 1
        gates = [ft(:,:,t); it(:,:,t); ot(:,:,t)];
        grad_gates = [grad_ft(:,:,t); grad_it(:,:,t); grad_ot(:,:,t)];
        grad_zgates = grad_gates .* gates .* (1-gates);
        grad_zt_curr = [grad_zgates(1:nCell,:); grad_zCt_raw; grad_zgates(nCell+1:end,:)];
    else
        grad_zft = grad_ft(:,:,t) .* ft(:,:,t) .* (1-ft(:,:,t));
        grad_zit = grad_it(:,:,t) .* it(:,:,t) .* (1-it(:,:,t));
        grad_zot = grad_ot(:,:,t) .* ot(:,:,t) .* (1-ot(:,:,t));
        grad_zt_curr = [grad_zft; grad_zCt_raw; grad_zit; grad_zot];
    end
    grad_zt(:,:,t) = grad_zt_curr;

    % compute the gradient of the W, b, and past hidden, past state, and x
    grad_yt = W' * grad_zt_curr;
    if usePastStateAsFeature
        grad_xt(:,:,t) = grad_yt(nCell*2+1:end,:);
    else
        grad_xt(:,:,t) = grad_yt(nCell+1:end,:);
    end
    if t==1
%         grad_ht_future0 = grad_yt(nCell+1:nCell*2);
%         grad_Ct_future0 = grad_Ct_future0 + grad_yt(1:nCell);
%         grad_W = grad_W + grad_zt * [Ct0*usePastStateAsFeature; ht0*useHidden; input(:,t)]';
    else
        grad_ht_future(:,:,t-1) = grad_yt(nCell+1:nCell*2,:);
        if usePastStateAsFeature
            grad_Ct_future(:,:,t-1) = grad_Ct_future(:,:,t-1) + grad_yt(1:nCell,:);
        end
%         grad_W = grad_W + grad_zt * [Ct(:,t-1)*usePastStateAsFeature; ht(:,t-1)*useHidden; input(:,t)]';
    end
%     grad_b = grad_b + grad_zt;
    grad_ht(:,:,t) = grad_ht_curr;
end
if usePastStateAsFeature
    tmpMat = [Ct(:,:,1:nFr-1); ht(:,:,1:nFr-1)*useHidden; input(:,:,2:nFr)];
else
    tmpMat = [ht(:,:,1:nFr-1)*useHidden; input(:,:,2:nFr)];
end
tmpMat = reshape(tmpMat, size(tmpMat,1), nSeg*(nFr-1));
tmpMat2 = reshape(grad_zt(:,:,2:nFr), nCell*4, nSeg*(nFr-1));
grad_W = tmpMat2 * tmpMat';
if usePastStateAsFeature
    grad_W = grad_W + grad_zt(:,:,1) * [Ct0; ht0*useHidden; input(:,:,1)]';
else
    grad_W = grad_W + grad_zt(:,:,1) * [ht0*useHidden; input(:,:,1)]';
end

grad_b = sum(sum(grad_zt,3),2);
grad = permute(grad_xt,[1 3 2]);
end


function l = tpl(tab, varargin)
%TPL compute total projection length
%   TPL(IMG) compute the total projection length of structure in image IMG.
%   Projection is computed along the last dimension of image. It
%   correspond to total diameter defined by Serra.
%
%   TPL(IMG, PDIM) specifies dimensions to manage. PDIM= [0 .. 0 1]
%   correspond to the default case. If PDIM contains only one 1, it is a
%   total diameter computation. If PDIM contains 2 ones, it is a total
%   projected surface computation. If PDIM contains only zeros, TPL
%   performs an Euler-Poincare Characteristic computation. If PDIM contains
%   only ones, it performs an area (2D case), volume (3D  case), or
%   Lebesgue measure.
%
%   TPL(IMG, PDIM, CONN) also specifies the neighbourhood configuration to
%   use, which can be 'minimal', or 'maximal'. Default is 'minimal'.
%
%   See also :
%   TPL, MINKOWSKI
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 10/09/2003.
%

%   HISTORY
%   20/04/2004 : 2D : do not call epc, accelerating processing

% remove useless dimensions and convert to binary image
tab = squeeze(tab)~=0;

% input image size
dim = size(tab);

% set default direction for projection : along the last dimension
if length(dim)==2 && (dim(1)==1 || dim(2)==1)
    pdim = 1;                           % dimension 1
else
    pdim = zeros(1, length(dim));       % dimension > 1
    pdim(length(pdim))=1;
end

if ~isempty(varargin)
    pdim = varargin{1};
end

% init
%nc=0;
l = 0;

% dimension 1 -------------------------------------------

if length(dim)==2 && (dim(1)==1 || dim(2)==1)
    if pdim(1)==1
        l = sum(tab);       % total length computation
    else
        l = epc(tab);       % EPC in dimension 1
    end
end

% dimension 2 -------------------------------------------

if length(dim)==2 && dim(1)~=1 && dim(2)~=1
    N1 = dim(1); N2 = dim(2);
    if sum(pdim==[1 0])==2
        % projection along y (first dimension of img)
        l = sum(sum(~tab(:,1:N2-1) & tab(:,2:N2))) + sum(tab(:,1));
    elseif sum(pdim == [0 1])==2
        % projection along x (second dimension of img)
        l = sum(sum(~tab(1:N1-1,:) & tab(2:N1,:))) + sum(tab(1,:));
    elseif sum(pdim == [0 0])==2
        % EPC in dimension 2
        l = epc(tab);
    elseif sum(pdim == [1 1])==2
        % area computation
        l = sum(tab(:));
    end
end

% dimension 3 -------------------------------------------

if length(dim)==3
    N1 = dim(1); N2 = dim(2); N3 = dim(3);

    % three total diameters computations
    if sum(pdim == [1 0 0])==3
        % total diameter in x axis
        n = sum(tab(:));
        n1 = sum(sum(sum(tab(:,1:N2-1,:)&tab(:,2:N2,:))));
        n2 = sum(sum(sum(tab(:,:,1:N3-1)&tab(:,:,2:N3))));
        n12 = sum(sum(sum(tab(:,1:N2-1,1:N3-1) & tab(:,1:N2-1,2:N3) & ...
            tab(:,2:N2,1:N3-1) & tab(:,2:N2,2:N3) )));
        l = n - n1 - n2 + n12;

    elseif sum(pdim == [0 1 0])==3
        % total diameter in y axis
        n = sum(tab(:));
        n1 = sum(sum(sum(tab(1:N1-1,:,:)&tab(2:N1,:,:))));
        n2 = sum(sum(sum(tab(:,:,1:N3-1)&tab(:,:,2:N3))));
        n12 = sum(sum(sum(tab(1:N1-1,:,1:N3-1) & tab(1:N1-1,:,2:N3) & ...
            tab(2:N1,:,1:N3-1) & tab(2:N1,:,2:N3) )));
        l = n - n1 - n2 + n12;

    elseif sum(pdim == [0 0 1])==3
        % total diameter in z axis
        n = sum(tab(:));
        n1 = sum(sum(sum(tab(1:N1-1,:, :)&tab(2:N1,:, :))));
        n2 = sum(sum(sum(tab(:,1:N2-1, :)&tab(:,2:N2, :))));
        n12 = sum(sum(sum(tab(1:N1-1,1:N2-1, :) & tab(1:N1-1,2:N2,:) & ...
            tab(2:N1,1:N2-1,:) & tab(2:N1,2:N2,:) )));
        l = n - n1 - n2 + n12;

        % three total projected area computations
    elseif sum(pdim == [1 1 0])==3
        % projected area on xy plane
        l = sum(sum(sum(~tab(:,:,1:N3-1) & tab(:,:,2:N3)))) + ...
            sum(sum(tab(:, :, 1)));
    elseif sum(pdim == [1 0 1])==3
        % projected area on xz plane
        l = sum(sum(sum(~tab(:,1:N2-1,:) & tab(:,2:N2,:)))) + ...
            sum(sum(tab(:, 1, :)));
    elseif sum(pdim == [0 1 1])==3
        % projected area on yz plane
        l = sum(sum(sum(~tab(1:N1-1,:,:) & tab(2:N1,:,:)))) + ...
            sum(sum(tab(1, :, :)));

    elseif sum(pdim == [0 0 0])==3
        % EPC in dimension 3
        l = epc(tab);

    elseif sum(pdim == [1 1 1])==3
        % volume computation
        l = sum(tab(:));

    end

end


%% retraction of d in T_X(M_r) onto M_r
%   X=U*diag(S)*V'
%   d=U*M*V'+U_p*V'+U*V_p'
%   cf. [Van12]

function [S1,U1,V1]=rtr_fr(d,S,U,V)

n1=size(U,1);
n2=size(V,1);
n3=size(S,1);

sig=0;
if size(S,2)>1, S=diag(S); sig=1; end

eps=1e-3;

% Pu=U*U'; Pv=V*V';

M=U'*d*V;
% U_p=d*V-Pu*d*V; V_p=d'*U-Pv*d'*U;
U_p=d*V-U*M; V_p=d'*U-V*M';

[Q_u,R_u]=qr(U_p,0);
[Q_v,R_v]=qr(V_p,0);
M1=[diag(S)+M, R_v'; R_u, zeros(n3)];
[U1,S1,V1]=svd(M1);
S1=diag(S1); S1=S1(1:n3);
U1=[U,Q_u]*U1(:,1:n3);
V1=[V,Q_v]*V1(:,1:n3);

S1=max(S1,eps);

if sig==1, S1=diag(S1); end


function [ fea, out ] = ex_planestrain1( varargin )
%EX_PLANESTRAIN1 Plane strain analysis of a pressure vessel.
%
%   [ FEA, OUT ] = EX_PLANESTRAIN1( VARARGIN ) Benchmark example for plane strain
%   approximation of a pressure vessel (annular cross section with symmetry).
%
%   Reference.  B. J. Mac Donald, Practical Stress Analysis with Finite Elements (2nd Ed),
%   case study E on page 327, 2007.
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       E           scalar {207e9}         Modulus of elasticity
%       nu          scalar {0.27}          Poissons ratio
%       sfun        string {sflag1}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'E',        207e9; ...
  'nu',       0.27; ...
  'sfun',     'sflag1'; ...
  'iplot',    1; ...
  'igrid',    1; ...
  'tol',      0.1; ...
  'fid',      1 };
[got,opt] = parseopt( cOptDef, varargin{:} );
fid       = opt.fid;

% Geometry and grid.
fea.sdim = { 'x' 'y' };   % Coordinate names.
fea.grid = ringgrid( 12, 216, 100e-3, 120e-3 );
fea.grid = delcells( fea.grid, selcells( fea.grid, '(x<=eps) | (y<=eps)') );
if( opt.igrid~=1 )
  fea.grid = quad2tri( fea.grid );
end
n_bdr = max(fea.grid.b(3,:));   % Number of boundaries.

% Problem definition.
fea = addphys( fea, @planestrain );
fea.phys.psn.eqn.coef{1,end} = { opt.nu };
fea.phys.psn.eqn.coef{2,end} = { opt.E  };
fea.phys.psn.sfun            = { opt.sfun opt.sfun };

% Boundary conditions.
bctype = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bctype{1,4} = 1;
bctype{2,3} = 1;
fea.phys.psn.bdr.coef{1,5} = bctype;

bccoef = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bccoef{1,1} = '-nx*1e4';
bccoef{2,1} = '-ny*1e4';
fea.phys.psn.bdr.coef{1,end} = bccoef;

% Parse and solve problem.
fea       = parsephys( fea );
fea       = parseprob( fea );                          % Check and parse problem struct.
fea.sol.u = solvestat( fea, 'fid', fid, 'icub', 1+str2num(strrep(opt.sfun,'sflag','')) );   % Call to stationary solver.

% Postprocessing.
s_disp = fea.phys.psn.eqn.vars{2,end};
if( opt.iplot>0 )
  figure
  postplot( fea, 'surfexpr', s_disp )
  title( 'Total displacement' )
end

% Error checking.
s_sx     = fea.phys.psn.eqn.vars{5,end};
s_sy     = fea.phys.psn.eqn.vars{6,end};
s_sxy    = fea.phys.psn.eqn.vars{8,end};
s_sp1    = fea.phys.psn.eqn.vars{9,end};
s_sp2    = fea.phys.psn.eqn.vars{10,end};
s_sp3    = fea.phys.psn.eqn.vars{11,end};
v_disp   = evalexpr( s_disp, [100e-3 120e-3-2*sqrt(eps);0 0]+sqrt(eps), fea )';
v_dref   = [2.64e-8 2.41e-8];
[v_sx(1),v_sx(2)] = minmaxsubd( s_sx, fea );
v_sxref  = [-10000 55454];
[v_sy(1),v_sy(2)] = minmaxsubd( s_sy, fea );
v_syref  = [-10000 55454];
[v_sxy(1),v_sxy(2)] = minmaxsubd( s_sxy, fea );
v_sxyref = [-32730 0];
[v_sp1(1),v_sp1(2)] = minmaxsubd( s_sp1, fea );
v_sp1ref = [4.5e4 55454];
[v_sp2(1),v_sp2(2)] = minmaxsubd( s_sp2, fea );
v_sp2ref = [1.227e4 1.227e4];
[v_sp3(1),v_sp3(2)] = minmaxsubd( s_sp3, fea );
v_sp3ref = [-1e4 0];
out.err  = [ abs([v_dref-v_disp])./v_dref ;
             abs([v_sxref-v_sx])./v_sxref ;
             abs([v_syref-v_sy])./v_syref ;
             abs([v_sxyref(1)-v_sxy(1)])./v_sxyref(1) 0 ;
             abs([v_sp1ref(2)-v_sp1(2)])./v_sp1ref(2) 0 ;
             abs([v_sp2ref-v_sp2])./v_sp2ref      ;
             abs([v_sp3ref(1)-v_sp3(1)])./v_sp3ref(1) 0 ];
out.pass = all( out.err(:) <= opt.tol );

if( nargout==0 )
  clear fea out
end


function sgmga_weight_test ( dim_num, importance, level_weight, ...
  level_max_min, level_max_max, rule, growth, np, p, tol )

%****************************************************************************80
%
%% SGMGA_WEIGHT_TEST checks the sum of the quadrature weights.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real IMPORTANCE(DIM_NUM), the anisotropic importance
%    for each dimension.
%
%    Input, real LEVEL_WEIGHT(DIM_NUM), the anisotropic weight
%    for each dimension.
%
%    Input, integer LEVEL_MAX_MIN, LEVEL_MAX_MAX, the minimum and
%    maximum values of LEVEL_MAX.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested.
%     2, "F2",  Fejer Type 2, Open Fully Nested.
%     3, "GP",  Gauss Patterson, Open Fully Nested.
%     4, "GL",  Gauss Legendre, Open Weakly Nested.
%     5, "GH",  Gauss Hermite, Open Weakly Nested.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested.
%     7, "LG",  Gauss Laguerre, Open Non Nested.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested.
%     9, "GJ",  Gauss Jacobi, Open Non Nested.
%    10, "HGK", Hermite Genz-Keister, Open Fully Nested.
%    11, "UO",  User supplied Open, presumably Non Nested.
%    12, "UC",  User supplied Closed, presumably Non Nested.
%
%    Input, integer GROWTH(DIM_NUM), the growth in each dimension.
%    0, "DF", default growth associated with this quadrature rule;
%    1, "SL", slow linear, L+1;
%    2  "SO", slow linear odd, O=1+2((L+1)/2)
%    3, "ML", moderate linear, 2L+1;
%    4, "SE", slow exponential;
%    5, "ME", moderate exponential;
%    6, "FE", full exponential.
%
%    Input, integer NP(DIM_NUM), the number of parameters used by each rule.
%
%    Input, real P(*), the parameters needed by each rule.
%
%    Input, real TOL, a tolerance for point equality.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SGMGA_WEIGHT_TEST:\n' );
  fprintf ( 1, '  Compute the weights of a sparse grid.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Each sparse grid is of spatial dimension DIM_NUM,\n' );
  fprintf ( 1, '  and is made up of product grids of levels up to LEVEL_MAX.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  IMPORTANCE:    ');
  for dim = 1 : dim_num
    fprintf ( 1, '  %14f', importance(dim) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEVEL_WEIGHT:  ');
  for dim = 1 : dim_num
    fprintf ( 1, '  %14f', level_weight(dim) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dimension      Rule       Growth     Parameters\n' );
  fprintf ( 1, '\n' );

  p_index = 1;

  for dim = 1 : dim_num

    if ( rule(dim) == 1 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  2 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  3 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  4 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  5 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  6 )
      fprintf ( 1, '  %8d  %8d  %11d  %14f\n', dim, rule(dim), growth(dim), p(p_index) );
    elseif ( rule(dim) ==  7 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  8 )
      fprintf ( 1, '  %8d  %8d  %11d  %14f\n', dim, rule(dim), growth(dim), p(p_index) );
    elseif ( rule(dim) ==  9 )
      fprintf ( 1, '  %8d  %8d  %11d  %14f  %14f\n', ...
        dim, rule(dim), growth(dim), p(p_index), p(p_index+1) );
    elseif ( rule(dim) == 10 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) == 11 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) == 12 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    end

    p_index = p_index + np(dim);

  end

  weight_sum_exact = 1.0;

  p_index = 1;

  for dim = 1 : dim_num

    if ( rule(dim) == 1 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 2 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 3 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 4 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 5 )
      weight_sum_exact = weight_sum_exact * sqrt ( pi );
    elseif ( rule(dim) == 6 )
      alpha = p(p_index);
      weight_sum_exact = weight_sum_exact * gamma ( 0.5 * ( alpha + 1.0 ) );
    elseif ( rule(dim) == 7 )
      weight_sum_exact = weight_sum_exact * 1.0;
    elseif ( rule(dim) == 8 )
      alpha = p(p_index);
      weight_sum_exact = weight_sum_exact * gamma ( alpha + 1.0 );
    elseif ( rule(dim) == 9 )
      alpha = p(p_index);
      beta = p(p_index+1);
      arg1 = - alpha;
      arg2 = 1.0;
      arg3 = beta + 2.0;
      arg4 = - 1.0;
      value1 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      arg1 = - beta;
      arg2 = 1.0;
      arg3 = alpha + 2.0;
      arg4 = - 1.0;
      value2 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      weight_sum_exact = weight_sum_exact * ( ...
        value1 / ( beta + 1.0 ) + value2 / ( alpha + 1.0 ) );
    elseif ( rule(dim) == 10 )
      weight_sum_exact = weight_sum_exact * sqrt ( pi );
    elseif ( rule(dim) == 11 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SGMGA_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Do not know how to deal with RULE = 11.\n' );
      error ( 'SGMGA_WEIGHT_TEST - Fatal error!' );
    elseif ( rule(dim) == 12 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SGMGA_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Do not know how to deal with RULE = 12.\n' );
      error ( 'SGMGA_WEIGHT_TEST - Fatal error!' );
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SGMGA_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Unexpected value of RULE = %d\n', rule(dim) );
      error ( 'SGMGA_WEIGHT_TEST - Fatal error!' );
    end

    p_index = p_index + np(dim);

  end

  fprintf ( 1, '\n' );
  fprintf ( 1,'  As a simple test, sum these weights.\n' );
  fprintf ( 1, '  They should sum to exactly %f\n', weight_sum_exact );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     Level      Weight sum  Expected sum    Difference\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max

    point_total_num = sgmga_size_total ( dim_num, level_weight, level_max, ...
      rule, growth );

    point_num = sgmga_size ( dim_num, level_weight, level_max, rule, growth, ...
      np, p, tol );

    sparse_unique_index = sgmga_unique_index ( dim_num, level_weight, ...
      level_max, rule, growth, np, p, tol, point_num, point_total_num );

    [ sparse_order, sparse_index ] = sgmga_index ( dim_num, level_weight, ...
      level_max, rule, growth, point_num, point_total_num, sparse_unique_index );

    sparse_weight = sgmga_weight ( dim_num, level_weight, level_max, rule, ...
      growth, np, p, point_num, point_total_num, sparse_unique_index );

    weight_sum = sum ( sparse_weight(1:point_num) );

    weight_sum_error = abs ( weight_sum - weight_sum_exact );

    fprintf ( 1, '  %8d  %14f  %14f  %14e\n', ...
      level_max, weight_sum, weight_sum_exact, weight_sum_error );

  end

  return
end


function K = sdlfmXsdlfmKernCompute(sdlfmKern1, sdlfmKern2, t1, t2, covIC, type)

% SDLFMXSDLFMKERNCOMPUTE Compute a cross kernel between two SDLFM kernels.
% FORMAT
% DESC computes cross kernel terms between two switching dynamical
% LFM kernels for the multiple output kernel.
% ARG sdlfmKern1 : the kernel structure associated with the first SDLFM
% kernel.
% ARG sdlfmKern2 : the kernel structure associated with the second SDLFM
% kernel.
% ARG t : inputs for which kernel is to be computed.
% ARG covIC : covariance for the initial conditions
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between two SDLFM kernels for
% the multiple output kernel.
% ARG sdlfmKern1 : the kernel structure associated with the first SDLFM
% kernel.
% ARG sdlfmKern2 : the kernel structure associated with the second SDLFM
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covIC : covariance for the initial conditions
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between two SDLFM kernels for
% the multiple output kernel. The SDLFM kernels can correspond to Position
% X Position (default), Velocity X Position, Velocity X Velocity,
% Acceleration X Position, Acceleration X Velocity, Acceleration X
% Acceleration. The type of kernel to be computed is specified in 'type'.
% ARG sdlfmKern1 : the kernel structure associated with the first SDLFM
% kernel.
% ARG sdlfmKern2 : the kernel structure associated with the second SDLFM
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covIC : covariance for the initial conditions
% ARG type : specifies the type of kerne to be computed
% RETURN K : block of values from kernel matrix.
%
% SEEALSO : sdlfmKernParamInit, sdlfmKernCompute, sdlfmKernParamInit
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 6
    type = 'PosPos';
    if nargin < 5
        covIC = t2;
        t2 = t1;
    end
end

if size(t1, 2) > 1 || size(t2, 2) > 1
    error('Input can only have one column');
end

compareInverseWidth = sdlfmKern1.inverseWidth == sdlfmKern2.inverseWidth;
if sum(sum(compareInverseWidth))~=(sdlfmKern1.nIntervals*sdlfmKern1.nlfPerInt)
    error('Kernels cannot be cross combined if they have different inverse widths.')
end
compareSwitchingTimes = sdlfmKern1.switchingTimes == sdlfmKern2.switchingTimes;
if sum(sum(compareSwitchingTimes))~=sdlfmKern1.nIntervals
    error('Kernels cannot be cross combined if they have different switching points.')
end

switch type
    case 'PosPos'
        fhandle = 'sdlfmXsdlfmKernComputeBlock';
    case 'VelPos'
        fhandle = 'sdlfmvXsdlfmKernComputeBlock';
    case 'VelVel'
        fhandle = 'sdlfmvXsdlfmvKernComputeBlock';
    case 'AccelPos'
        fhandle = 'sdlfmaXsdlfmKernComputeBlock';
    case 'AccelVel'
        fhandle = 'sdlfmaXsdlfmvKernComputeBlock';
    case 'AccelAccel'
        fhandle = 'sdlfmaXsdlfmaKernComputeBlock';
end

fhandle = str2func(fhandle);

%Form the basic kernels
lfmKern1 = struct();
lfmKern2 = struct();

% Create structures that will make easy the computation of the kernels

spVector = [cumsum(sdlfmKern1.switchingTimes) t1(end)+50];

dim1 = zeros(1, sdlfmKern1.nIntervals); dim2 = zeros(1, sdlfmKern1.nIntervals);

for i=1:sdlfmKern1.nIntervals
    for j =1:sdlfmKern1.nlfPerInt
        % Create the appropriate set of kernel structures
        lfmKern1(i,j).mass = sdlfmKern1.mass;
        lfmKern1(i,j).spring = sdlfmKern1.spring;
        lfmKern1(i,j).damper = sdlfmKern1.damper;
        lfmKern1(i,j).inverseWidth = sdlfmKern1.inverseWidth(j,i);
        lfmKern1(i,j).sensitivity = sdlfmKern1.sensitivity(j,i);
        lfmKern2(i,j).mass = sdlfmKern2.mass;
        lfmKern2(i,j).spring = sdlfmKern2.spring;
        lfmKern2(i,j).damper = sdlfmKern2.damper;
        lfmKern2(i,j).inverseWidth = sdlfmKern2.inverseWidth(j,i);
        lfmKern2(i,j).sensitivity = sdlfmKern2.sensitivity(j,i);
        lfmKern1(i,j).limit = spVector(i+1) - spVector(i);
        lfmKern2(i,j).limit = spVector(i+1) - spVector(i);
        lfmKern1(i,j).isNormalised = sdlfmKern1.isNormalised;
        lfmKern2(i,j).isNormalised = sdlfmKern2.isNormalised;
    end
    newt1 = t1(t1> spVector(i) & t1<spVector(i+1));
    newt2 = t2(t2> spVector(i) & t2<spVector(i+1));
    dim1(i) = length(newt1);
    dim2(i) = length(newt2);
end

if sum(dim1)~=length(t1) || sum(dim2)~=length(t2)
    error('A problem with the dimensions of the switching intervals occured')
end

% Compute some necessary constants

generalConst = sdlfmKernComputeConstant(sdlfmKern1.nIntervals, ...
    lfmKern1(1), lfmKern2(1), spVector);

kyy = zeros(sdlfmKern1.nIntervals);kyv = zeros(sdlfmKern1.nIntervals);
kvy = zeros(sdlfmKern1.nIntervals);kvv = zeros(sdlfmKern1.nIntervals);
kyy(1,1) = covIC(1,1);
kvy(1,1) = covIC(2,1);
kyv(1,1) = covIC(1,2);
kvv(1,1) = covIC(2,2);

%%% Compute initial conditions for intervals (1-2), (2-2) and (2-1)

tInit1 = [spVector(1) - spVector(1);spVector(2) - spVector(1)];
tInit2 = [spVector(1) - spVector(1);spVector(2) - spVector(1)];

% Pos -- Pos
temp = sdlfmXsdlfmKernComputeBlock(lfmKern1(1), lfmKern2(1), ...
    tInit1, tInit2, kyy(1,1), kyv(1,1), kvy(1,1), kvv(1,1), 1, 1, generalConst);
kyy(2,2) = temp(2,2); kyy(1,2) = temp(1,2); kyy(2,1)  = temp(2,1);
% Vel -- Pos
temp = sdlfmvXsdlfmKernComputeBlock(lfmKern1(1), lfmKern2(1), ...
    tInit1, tInit2, kyy(1,1), kyv(1,1), kvy(1,1), kvv(1,1), 1, 1, generalConst);
kvy(2,2) = temp(2,2); kvy(1,2) = temp(1,2); kvy(2,1)  = temp(2,1);
% Pos -- Vel
temp = sdlfmXsdlfmvKernComputeBlock(lfmKern1(1), lfmKern2(1), ...
    tInit1, tInit2, kyy(1,1), kyv(1,1), kvy(1,1), kvv(1,1), 1, 1, generalConst);
kyv(2,2) = temp(2,2); kyv(1,2) = temp(1,2); kyv(2,1)  = temp(2,1);
% Vel -- Vel
temp = sdlfmvXsdlfmvKernComputeBlock(lfmKern1(1), lfmKern2(1), ...
    tInit1, tInit2, kyy(1,1), kyv(1,1), kvy(1,1), kvv(1,1), 1, 1, generalConst);
kvv(2,2) = temp(2,2); kvv(1,2) = temp(1,2); kvv(2,1)  = temp(2,1);

tempPosPos = cell(sdlfmKern1.nIntervals);
tempVelPos = cell(sdlfmKern1.nIntervals);
tempPosVel = cell(sdlfmKern1.nIntervals);
tempVelVel = cell(sdlfmKern1.nIntervals);

startValOne = 1;
endValOne   = 0;

for i=1:sdlfmKern1.nIntervals
    endValOne = endValOne + dim1(i);
    startValThree = 1;
    endValThree = 0;
    for j=1:sdlfmKern1.nIntervals
        if i>j
            lfmKern1Local = lfmKern1(j,:);
            lfmKern2Local = lfmKern2(j,:);
        else
            lfmKern1Local = lfmKern1(i,:);
            lfmKern2Local = lfmKern2(i,:);
        end
        endValThree = endValThree + dim2(j);
        % POS -- POS (Kernel and initial positions)
        K(startValOne:endValOne, startValThree:endValThree) = fhandle(lfmKern1Local, ...
            lfmKern2Local, t1(startValOne:endValOne) - spVector(i), t2(startValThree:endValThree) - spVector(j), ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        % Time vector initial conditions
        tInit1 = [spVector(i) - spVector(i);spVector(i+1) - spVector(i)];
        tInit2 = [spVector(j) - spVector(j);spVector(j+1) - spVector(j)];
        tempPosPos{i,j} = sdlfmXsdlfmKernComputeBlock(lfmKern1Local, ...
            lfmKern2Local, tInit1, tInit2, ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        kyy = organizeIC(kyy, tempPosPos, i, j);
        % VEL -- POS
        tempVelPos{i,j} = sdlfmvXsdlfmKernComputeBlock(lfmKern1Local, ...
            lfmKern2Local, tInit1, tInit2 , ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        kvy = organizeIC(kvy, tempVelPos, i, j);
        % POS -- VEL
        tempPosVel{i,j} = sdlfmXsdlfmvKernComputeBlock(lfmKern1Local, ...
            lfmKern2Local, tInit1, tInit2, ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        kyv = organizeIC(kyv, tempPosVel, i, j);
        % POS -- VEL
        tempVelVel{i,j} = sdlfmvXsdlfmvKernComputeBlock(lfmKern1Local, ...
            lfmKern2Local, tInit1, tInit2, ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        kvv = organizeIC(kvv, tempVelVel, i, j);
        startValThree = endValThree + 1;
    end
    startValOne = endValOne + 1;
end

K = real(K);


function s = SaturationPouli(C, I)
%
%       s = SaturationPouli(C, I)
%
%       This computes the saturation using channel C and I from LCh color
%       space
%
%       input:
%         - C: chroma channel from LCh
%         - I: intensity channel from LCh
%
%       output:
%         - S: saturation channel
%
%     Copyright (C) 2013  Francesco Banterle
%
%     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/>.
%
%     The paper describing this technique is:
%     "Color Correction for Tone Reproduction"
% 	  by Tania Pouli1, Alessandro Artusi, Francesco Banterle,
%     Ahmet Oguz Akyuz, Hans-Peter Seidel and Erik Reinhard
%     in the Twenty-first Color and Imaging Conference (CIC21), Albuquerque, Nov. 2013
%
%

D = sqrt(C.^2 + I.^2);
s = C ./ D;

end


% this is a very naiive and non-optimized cardiac cine GRE sequence

seq=mr.Sequence();              % Create a new sequence object
fov=256e-3; Nx=128; Ny=Nx;      % Define FOV and resolution
alpha=5;                        % flip angle
sliceThickness=5e-3;            % slice
%TE=[7.38 9.84]*1e-3;            % give a vector here to have multiple TEs (e.g. for field mapping)
TE=4.92e-3;
TR=9e-3;                        % only a single value for now

% cardiac features
phases = 8;
hearbeats = 15; % odd numbers of heartbeats / segments work better

% more in-depth parameters
rfSpoilingInc=117;              % RF spoiling increment
rf_duration = 2e-3;
adc_duration = 3.2e-3;
pre_duration = 1e-3;

% set system limits
sys = mr.opts('MaxGrad', 28, 'GradUnit', 'mT/m', ...
    'MaxSlew', 150, 'SlewUnit', 'T/m/s', 'rfRingdownTime', 20e-6, ...
    'rfDeadTime', 100e-6, 'adcDeadTime', 10e-6);

% Create fat-sat pulse
% (in Siemens interpreter from January 2019 duration is limited to 8.192 ms, and although product EPI uses 10.24 ms, 8 ms seems to be sufficient)
% B0=2.89; % 1.5 2.89 3.0
% sat_ppm=-3.45;
% sat_freq=sat_ppm*1e-6*B0*lims.gamma;
% rf_fs = mr.makeGaussPulse(110*pi/180,'system',lims,'Duration',8e-3,...
%     'bandwidth',abs(sat_freq),'freqOffset',sat_freq);
% gz_fs = mr.makeTrapezoid('z',sys,'delay',mr.calcDuration(rf_fs),'Area',1/1e-4); % spoil up to 0.1mm

% define the trigger to play out
trig=mr.makeTrigger('physio1','duration', 2000e-6); % duration after
%trig=mr.makeTriggerPulse('osc0','duration', 4100e-6); % possible channels: 'osc0','osc1','ext1'
trig_out=mr.makeDigitalOutputPulse('ext1','duration', 100e-6,'delay',500e-6); % possible channels: 'osc0','osc1','ext1'

% Create alpha-degree slice selection pulse and gradient
[rf, gz] = mr.makeSincPulse(alpha*pi/180,'Duration',rf_duration,...
    'SliceThickness',sliceThickness,'apodization',0.5,'timeBwProduct',4,'system',sys);

% Define other gradients and ADC events
deltak=1/fov;
gx = mr.makeTrapezoid('x','FlatArea',Nx*deltak,'FlatTime',adc_duration,'system',sys);
adc = mr.makeAdc(Nx,'Duration',gx.flatTime,'Delay',gx.riseTime,'system',sys);
gxPre = mr.makeTrapezoid('x','Area',-gx.area/2,'Duration',pre_duration,'system',sys);
gzReph = mr.makeTrapezoid('z','Area',-gz.area/2,'Duration',pre_duration,'system',sys);

lines_per_segment = round(Ny/hearbeats);
Ns=ceil(Ny/lines_per_segment);
Ny=Ns*lines_per_segment; % it can be that because of the rounding above we measure few more k-space lines...
phaseAreas = ((0:Ny-1)-Ny/2)*deltak;
% now reverse the order in every second segment
phaseAreasSeg=reshape(phaseAreas,lines_per_segment,Ns);
phaseAreasSeg(:,2:2:end)=phaseAreasSeg(end:-1:1,2:2:end);
phaseAreas=phaseAreasSeg(:);

% gradient spoiling
gxSpoil=mr.makeTrapezoid('x','Area',2*Nx*deltak,'system',sys);
gzSpoil=mr.makeTrapezoid('z','Area',4/sliceThickness,'system',sys);

% Calculate timing
delayTE=ceil((TE - mr.calcDuration(gxPre) - gz.fallTime - gz.flatTime/2 ...
    - mr.calcDuration(gx)/2)/seq.gradRasterTime)*seq.gradRasterTime;
delayTR=ceil((TR - mr.calcDuration(gxPre) - mr.calcDuration(gz) ...
    - mr.calcDuration(gx) - delayTE)/seq.gradRasterTime)*seq.gradRasterTime;
assert(all(delayTR>=mr.calcDuration(gxSpoil,gzSpoil)));

fprintf('the sequence will acquire %d lines per segment resulting in a temporal resolution of %g ms per phase\n', lines_per_segment, TR*lines_per_segment*1e3);
fprintf('cardiac acquisition window is: %g ms\n', TR*phases*lines_per_segment*1e3);

rf_phase=0;
rf_inc=0;

% Loop over phase encodes and define sequence blocks
for s=1:Ns
    seq.addBlock(trig); % wait for the cardiac trigger
    for p=1:phases
        for l=1:lines_per_segment
            % restore the line counter
            i=(s-1)*lines_per_segment+l;
            %seq.addBlock(rf_fs,gz_fs); % fat-sat
            rf.phaseOffset=rf_phase/180*pi;
            adc.phaseOffset=rf_phase/180*pi;
            rf_inc=mod(rf_inc+rfSpoilingInc, 360.0);
            rf_phase=mod(rf_phase+rf_inc, 360.0);
            %
            seq.addBlock(rf,gz,trig_out);
            gyPre = mr.makeTrapezoid('y','Area',phaseAreas(i),'Duration',pre_duration,'system',sys);
            seq.addBlock(gxPre,gyPre,gzReph);
            if delayTE>0
                seq.addBlock(mr.makeDelay(delayTE));
            end
            seq.addBlock(gx,adc);
            gyPre.amplitude=-gyPre.amplitude;
            seq.addBlock(mr.makeDelay(delayTR),gxSpoil,gyPre,gzSpoil)
        end
    end
end

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

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

%% prepare sequence export
seq.setDefinition('FOV', [fov fov sliceThickness]);
seq.setDefinition('Name', 'cine-gre');

seq.write('cine_gre.seq')       % Write to pulseq file

%seq.install('siemens');
return

%% plot sequence and k-space diagrams

seq.plot('timeRange', [0 5*TR]);

% new single-function call for trajectory calculation
[ktraj_adc, ktraj, t_excitation, t_refocusing, t_adc] = seq.calculateKspace();

% plot k-spaces
time_axis=(1:(size(ktraj,2)))*sys.gradRasterTime;
figure; plot(time_axis, ktraj'); % plot the entire k-space trajectory
hold; plot(t_adc,ktraj_adc(1,:),'.'); % and sampling points on the kx-axis
figure; plot(ktraj(1,:),ktraj(2,:),'b'); % a 2D plot
axis('equal'); % enforce aspect ratio for the correct trajectory display
hold;plot(ktraj_adc(1,:),ktraj_adc(2,:),'r.'); % plot the sampling points

%% very optional slow step, but useful for testing during development e.g. for the real TE, TR or for staying within slewrate limits

rep = seq.testReport;
fprintf([rep{:}]);


function p=baselineParameters()

p=struct();

p.h = 0.8;
p.gz = 0.001;
p.pi_ss = 1.005;
p.beta = 0.9975;
p.kappa1 = 5;
p.kappa2 = 0.5;
p.epsilon = 6;
p.phi = 10;
p.eta = 2;
p.xi = 0.5;
p.rho_r = 0.8;
p.rho_a = 0.9;
p.sig_r = 0.01;
p.sig_a = 0.01;
p.sig_pi = 0.01;
p.sig_z = 0.01;
p.sigma = 2;
p.d = 1;

end


function [ydum,xdum,breaks]=varprior(nv,nx,lags,mnprior,vprior)
%function [ydum,xdum,breaks]=varprior(nv,nx,lags,mnprior,vprior)
% ydum, xdum:   dummy observation data that implement the prior
% breaks:       vector of points in the dummy data after which new dummy obs's start
%                   Set breaks=T+[0;breaks], ydata=[ydata;ydum], xdum=[xdata;xdum], where
%                   actual data matrix has T rows, in preparing input for rfvar3
% nv,nx,lags: VAR dimensions
% mnprior.tight:Overall tightness of Minnesota prior
% mnprior.decay:Standard deviations of lags shrink as lag^(-decay)
% vprior.sig:   Vector of prior modes for diagonal elements of r.f. covariance matrix
% vprior.w:     Weight on prior on vcv.  1 corresponds to "one dummy observation" weight
%                   Should be an integer, and will be rounded if not.  vprior.sig is needed
%                   to scale the Minnesota prior, even if the prior on sigma is not used itself.
%                   Set vprior.w=0 to achieve this.
% Note:         The original Minnesota prior treats own lags asymmetrically, and therefore
%                   cannot be implemented entirely with dummy observations.  It is also usually
%                   taken to include the sum-of-coefficients and co-persistence components
%                   that are implemented directly in rfvar3.m.  The diagonal prior on v, combined
%                   with sum-of-coefficients and co-persistence components and with the unit own-first-lag
%                   prior mean generates larger prior variances for own than for cross-effects even in
%                   this formulation, but here there is no way to shrink toward a set of unconstrained
%                   univariate AR's.

% Original file downloaded from:
% http://sims.princeton.edu/yftp/VARtools/matlab/varprior.m

if ~isempty(mnprior)
    xdum = zeros(lags+1,nx,lags,nv);
    ydum = zeros(lags+1,nv,lags,nv);
    for il = 1:lags
        ydum(il+1,:,il,:) = il^mnprior.decay*diag(vprior.sig);
    end
    ydum(1,:,1,:) = diag(vprior.sig);
    ydum = mnprior.tight*reshape(ydum,[lags+1,nv,lags*nv]);
    ydum = flipdim(ydum,1);
    xdum = mnprior.tight*reshape(xdum,[lags+1,nx,lags*nv]);
    xdum = flipdim(xdum,1);
    breaks = (lags+1)*[1:(nv*lags)]';
    lbreak = breaks(end);
else
    ydum = [];
    xdum = [];
    breaks = [];
    lbreak = 0;
end
% elle = 0;
% for j1 = 1 : nv
%     for j2  =  1 : lags
%         elle = 1 + elle;
%         ydum(:,:,elle) = ydum(:,:,elle) * mnprior.unit_root_(j1);
%     end
% end
if ~isempty(vprior) && vprior.w>0
    ydum2 = zeros(lags+1,nv,nv);
    xdum2 = zeros(lags+1,nx,nv);
    ydum2(end,:,:) = diag(vprior.sig);
    for i = 1:vprior.w
        ydum = cat(3,ydum,ydum2);
        xdum = cat(3,xdum,xdum2);
        breaks = [breaks;(lags+1)*[1:nv]'+lbreak];
        lbreak = breaks(end);
    end
end
dimy = size(ydum);
ydum = reshape(permute(ydum,[1 3 2]),dimy(1)*dimy(3),nv);
xdum = reshape(permute(xdum,[1 3 2]),dimy(1)*dimy(3),nx);
breaks = breaks(1:(end-1));


function y=v_rhartley(x,n)
%V_RHARTLEY Calculate the Hartley transform of real data Y=(X,N)
% Data is truncated/padded to length N if specified.
% The inverse transformation is x=hartley(y,n)/n

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_rhartley.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin < 2
  y=fft(real(x));
else
  y=fft(real(x),n);
end
y=real(y)-imag(y);


classdef CNSDEDVC < ALGORITHM
% <multi> <real/integer> <robust>
% Constrained nondominated sorting differential evolution based on decision variable classification
% SN     ---     4 --- Number of perturbed solutions
% PN     ---     6 --- Number of perturbations
% TN     ---    15 --- Number of repeated times of perturbation
% theta  --- 0.001 --- Threshold for DVC operation
% eta    --- 0.001 --- Desired level of robustness

%------------------------------- Reference --------------------------------
% W. Du, W. Zhong, Y. Tang, W. Du, and Y. Jin, High-dimensional robust
% multi-objective optimization for order scheduling: A decision variable
% classification approach, IEEE Transactions on Industrial Informatics,
% 15(1): 293-304.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [SN,PN,TN,theta,eta] = Algorithm.ParameterSet(4,6,15,0.001,0.001);

            %% Generate random population
            Population = Problem.Initialization();
            [HR,LR]    = DVC(Problem,Population,SN,PN,TN,theta);
            [~,FrontNo,CrowdDis] = EnvironmentalSelection(Problem,Population,Problem.N,false,eta);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                if ~isempty(HR)
                    for subgen = 1 : 10
                        OffDec       = Population(TournamentSelection(2,end,FrontNo,-CrowdDis)).decs;
                        NewDec       = OperatorDE(Problem,Population.decs,Population(randi(end,1,end)).decs,Population(randi(end,1,end)).decs,{0.9,0.5,1,20});
                        OffDec(:,HR) = NewDec(:,HR);
                        Offspring    = Problem.Evaluation(OffDec);
                        [Population,FrontNo,CrowdDis] = EnvironmentalSelection(Problem,[Population,Offspring],Problem.N,true,eta);
                    end
                end
                if ~isempty(LR)
                    for subgen = 1 : 2
                        OffDec       = Population(TournamentSelection(2,end,FrontNo,-CrowdDis)).decs;
                        NewDec       = OperatorDE(Problem,Population.decs,Population(randi(end,1,end)).decs,Population(randi(end,1,end)).decs,{0.9,0.5,1,20});
                        OffDec(:,LR) = NewDec(:,LR);
                        Offspring    = Problem.Evaluation(OffDec);
                        [Population,FrontNo,CrowdDis] = EnvironmentalSelection(Problem,[Population,Offspring],Problem.N,false,eta);
                    end
                end
            end
        end
    end
end


%function gppca_demo_poster

% Good seeds: 16, 20
seed = 16
randn('state', seed)
rand('state', seed)

inX = 100*(1:200);
inW = 10*rand(2,30);

N = size(inX,2);
M = size(inW,2);

D = 0;

% Trend
D = D + 1;
covfuncX{D} = @gpcov;
logthetaX{D} = log(365*(10+10*rand)); % 10-20 years
initthetaX{D} = logthetaX{D} + 1;%log(365*(10+10*rand));
densityX(D) = 1;

% Periodical (it is difficult to learn from a badly initialized
% periodicity :( )
D = D + 1;
covfuncX{D} = {@gpcovProduct, @gpcov, @gpcovPeriodic};
%logthetaX{D} = log([5*365, 2, 4*365]); % 1 year
logthetaX{D} = log([250*365, 1.6, 3*365]); % decay 100 years, period 3 year
initthetaX{D} = logthetaX{D};%log([300*365, 2, 365*(3.5+1*rand)]);
densityX(D) = 1;

% Short-scale
D = D + 1;
covfuncX{D} = @gpcov;
logthetaX{D} = log(365*1); % 1 year
initthetaX{D} = logthetaX{D} - 0.1;%log(30*(20+20*rand));
densityX(D) = 1;

% Short-scale, almosta noise
D = D + 1;
covfuncX{D} = @gpcovPP;
logthetaX{D} = log(30*4); % 4 months
initthetaX{D} = logthetaX{D} - 0.1;%log(30*(3+6*rand));
densityX(D) = 1;

% Generate latent signals
X = zeros(D,N);
for d=1:D
  X(d,:) = gprnd(inX, logthetaX{d}, covfuncX{d});
end

scales = [2^2 3^2 3^2 1.0^2];
lengthscales = [5 1.5 1.5 0.9];
bias =   [3 0 0 0];
% Working set
% $$$ scales = [2^2 3^2 5^2 1.0^2];
% $$$ lengthscales = [5 2.5 1.5 0.9];
% $$$ bias =   [3 0 0 0];
W = zeros(M,D);
[coord1,coord2] = meshgrid(linspace(0,10,50), linspace(0,10,50));
inWh = [coord1(:)'; coord2(:)'];
W_function = zeros(numel(coord1), D);
for d=1:D
% $$$   covfuncW{d} = {@(logtheta,x1,x2) gpcov(logtheta,x1,x2,@sqdistEuclidean)};
% $$$   logthetaW{d} = [log(2)];
% $$$   initthetaW{d} = [log(6)];
  covfuncW{d} = {@gpcovScale, @(logtheta,x1,x2) gpcov(logtheta,x1,x2,@ ...
                                                    sqdistEuclidean)};
  logthetaW{d} = [log(scales(d)); log(lengthscales(d))];
  initthetaW{d} = logthetaW{d} - 0.1;%[log(5); log(6)];
  densityW(d) = 1;
  W(:,d) = bias(d) + gprnd(inW, logthetaW{d}, covfuncW{d});
% $$$   tmp = bias(d) + gprnd([inW, inWh], logthetaW{d}, covfuncW{d});
% $$$   W(:,d) = tmp(1:M);
% $$$   W_function(:,d) = tmp((M+1):end);
end
for d=1:D
  %tmp = bias(d) + gprnd([inW, inWh], logthetaW{d}, covfuncW{d});
  W_function(:,d) = bias(d) + gppred(inW, W(:,d), 0, inWh, logthetaW{d}, ...
                                     covfuncW{d});
end

% $$$ size_inW = size(inW)
% $$$ size_W = size(W)

% Scale X to unit second moment, so comparison is more valid
scale = sqrt(mean(X.^2,2));
scale = [-1; -1; -1; 1] .* scale; % comparable sign rotation
X = diag(1./scale) * X;
W = W * diag(scale);
W_function = W_function * diag(scale);

%hax = tsplot(inX, X, 'k');
%return

% Data
Y = W*X;
[M,N] = size(Y);

% $$$ variance_noiseless = [(1:M)', std(Y,1,2)]
% $$$ mean_std_noiseless = sqrt(mean(var(Y,1,2)))
% $$$ return

% Noise
Yn = Y + 1*randn(size(Y));

M = size(W,1);
random = randperm(M);
%examples = random([4 9 14 15]);
examples = [19 1 20 5];

% $$$ tsplot(X)
% $$$ tsplot(W')
% $$$ tsplot(Y(examples,:));
% $$$ return

% Missing values
Ynm = Yn;
Itrain = rand(size(Yn))<0.9;
Ynm(Itrain) = nan;
Ytest = Yn;
Ytest(~Itrain) = nan;
Ytest(rand(size(Ytest))<0.87) = nan;

% Create a gap
Ygap = nan*Ynm;
Ynm(:,100:150) = nan;

% $$$ size_Y = size(Ynm)
% $$$ num_of_observations = sum(~isnan(Ynm(:)))
% $$$
% $$$ first_Y = Y(:,1:5)

num_of_observations = sum(sum( ~isnan(Ynm) ))
%return

% $$$ vbpca = false;
% $$$ if vbpca
% $$$   Qvb = pca_full(Ynm,D,'maxiters',50,'rotate2pca',true, 'algorithm','vb');
% $$$
% $$$   % Plot VBPCA latent components
% $$$   varS = zeros(size(Qvb.S));
% $$$   for n=1:N
% $$$     varS(:,n) = diag(Qvb.Sv{n});
% $$$   end
% $$$   hax = tsgpplot(inX', Qvb.S', 2*sqrt(varS)');
% $$$   for i=1:length(hax)
% $$$     set(hax(i), 'xtick', [], 'ytick', []);
% $$$     axes(hax(i));
% $$$     line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
% $$$     line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
% $$$     lab = sprintf('x_{%d}(t)', i);
% $$$     ylabel(lab);
% $$$   end
% $$$   xlabel('time, t');
% $$$   set_figure_size(gcf, 12, 7);
% $$$   set_label_fontsize(hax, 6);
% $$$   print('-depsc', '/home/jluttine/thesis_slides/fig_artificial_vbpca_latent');
% $$$
% $$$   % Plot VBPCA predictive distribution
% $$$   Yvb = bsxfun(@plus, Qvb.A*Qvb.S, Qvb.Mu);
% $$$   varYpvb = zeros(size(Yvb));
% $$$   for m=1:M
% $$$     CovA = Qvb.Av{m};
% $$$     for n=1:N
% $$$       CovS = Qvb.Sv{n};
% $$$       varYvb(m,n) = Qvb.A(m,:)*CovS*Qvb.A(m,:)' + Qvb.S(:,n)'*CovA*Qvb.S(:,n) ...
% $$$           + traceprod(CovS,CovA,true);
% $$$     end
% $$$   end
% $$$   varYvb = bsxfun(@plus, varYvb, Qvb.Muv + Qvb.V);
% $$$   hax = tsgpplot(inX', Yvb(examples,:)', 2*sqrt(varYvb(examples,:))');
% $$$   addtsplot(inX, Yn(examples,:), 'r-');
% $$$   addtsplot(inX, Yvb(examples,:), 'k-'); % draw vbpca mean again..
% $$$   addtsplot(inX, Ynm(examples,:), '+', 'MarkerSize', 7, 'Color', [0 0 1]);
% $$$   for i=1:length(hax)
% $$$     set(hax(i), 'xtick', [], 'ytick', []);
% $$$     axes(hax(i));
% $$$     line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
% $$$     line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
% $$$     lab = sprintf('y_{%d}(t)', examples(i));
% $$$     ylabel(lab);
% $$$   end
% $$$   %xlabel('time, t');
% $$$   set_figure_size(gcf, 12, 10.5);
% $$$   set_label_fontsize(hax, 6);
% $$$   print('-depsc', ['/home/jluttine/papers/2009NIPS/poster/' ...
% $$$                     'fig_artificial_vbpca_predictive']);
% $$$ end

%return

Qgp = vbgppcamv_full(Ynm, D,...
                     inW, inX,...
                     covfuncW, initthetaW,...
                     covfuncX,initthetaX, ...
                     'maxiter',50, ...
                     'pseudodensityx', densityX, ...
                     'pseudodensityw', densityW, ...
                     'loglikelihood', true, ...
                     'updatehyper', [5 10 20 50 80 100], ...
                     'updatepseudox', false, ...
                     'updatepseudow', false, ...
                     'maxsearchx', 3, ...
                     'maxsearchw', 3, ...
                     'checkgradw', false, ...
                     'checkgradx', false);

Ygp = Qgp.W * Qgp.X;
if false
  disp('Using inaccurate prediction variance.');
  varYgp = Qgp.W.^2*Qgp.varX + Qgp.varW*Qgp.X.^2 + Qgp.varW*Qgp.varX + ...
           1/Qgp.tau;
else
  disp('Using accurate prediction variance.')
  varYgp = zeros(size(Ygp));
  for m=1:M
    CovWm = Qgp.CovW(Qgp.indsW(m,:),Qgp.indsW(m,:));
    for n=1:N
      CovXn = Qgp.CovX(Qgp.indsX(:,n),Qgp.indsX(:,n));
      varYgp(m,n) = Qgp.W(m,:)*CovXn*Qgp.W(m,:)' + Qgp.X(:,n)'*CovWm*Qgp.X(:,n) ...
          + traceprod(CovXn,CovWm,true);
    end
  end
  varYgp = varYgp + 1/Qgp.tau;
end

% Plot latent signals
hax = tsgpplot(inX', Qgp.X', 2*sqrt(Qgp.varX)');
for i=1:length(hax)
  set(hax(i), 'xtick', [], 'ytick', []);
  axes(hax(i));
  line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
  line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
  lab = sprintf('x_{%d}(t)', i);
%  ylabel(lab);
end
%xlabel('time, t');
set_subplot_positions(hax, 4, 1, [0.01 0.01 0.01 0.01], [0.02 0.02]);
set_figure_size(gcf, 7, 6);
set_label_fontsize(hax, 6);
set(gcf, 'Color','none');
set(hax, 'Color', 'none');
export_fig('/home/jluttine/thesis/slides/novac2010_artificial_latent', ...
           '-eps');
% $$$ print('-depsc', '/home/jluttine/papers/2009NIPS/poster/fig_artificial_latent');

% $$$ % Plot true latent signals
% $$$ hax = tsplot(inX, X, 'k');
% $$$ hold on
% $$$ yl = max(-min(X,[],2), max(X,[],2))
% $$$ for i=1:length(hax)
% $$$   set(hax(i), 'YLim', [-yl(i) yl(i)]);
% $$$   set(hax(i), 'xtick', [], 'ytick', []);
% $$$   axes(hax(i));
% $$$   line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
% $$$   line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
% $$$   lab = sprintf('x_{%d}(t)', i);
% $$$   ylabel(lab);
% $$$ end
% $$$ xlabel('time, t');
% $$$ set_figure_size(gcf, 7, 6);
% $$$ set_label_fontsize(hax, 6);
% $$$ print('-depsc', '/home/jluttine/papers/2009NIPS/poster/fig_artificial_true_latent');

% Compare latent signals
% $$$ Qgp.X = bsxfun(@minus, Qgp.X, mean(Qgp.X,2));
% $$$ Qgp.varX = bsxfun(@rdivide, Qgp.varX, std(Qgp.X,1,2).^2);
% $$$ Qgp.X = bsxfun(@rdivide, Qgp.X, std(Qgp.X,1,2));
% $$$ X = bsxfun(@minus, X, mean(X,2));
% $$$ X = bsxfun(@rdivide, X, std(X,1,2));
% $$$ tsgpplot(inX', Qgp.X', 2*sqrt(Qgp.varX)');
% $$$ addtsplot(inX, X, 'r')

% Show predictive distribution
hax = tsgpplot(inX', Ygp(examples,:)', 2*sqrt(varYgp(examples,:))');
addtsplot(inX, Yn(examples,:), 'r-');%, 'MarkerSize', 1)
addtsplot(inX, Ygp(examples,:), 'k-') % draw gp mean again..
addtsplot(inX, Ynm(examples,:), '+', 'MarkerSize', 4, 'Color', [0 0 1]);
% $$$ addtsplot(inX, Ytest(examples,:), 'o', 'MarkerSize', 4, 'Color', [0 ...
% $$$                     0 0.8]);
for i=1:length(hax)
  set(hax(i), 'xtick', [], 'ytick', []);
  axes(hax(i));
  line(100*[100, 100], [-1000 1000], 'Color', [0 0 0])
  line(100*[150, 150], [-1000 1000], 'Color', [0 0 0])
% $$$   lab = sprintf('y_{%d}(t)', examples(i));
% $$$   ylabel(lab);
end
%xlabel('time, t');
set_subplot_positions(hax, 4, 1, [0.01 0.01 0.01 0.01], [0.02 0.02]);
set_figure_size(gcf, 7, 6);
set_label_fontsize(hax, 6);
set(gcf, 'Color','none');
set(hax, 'Color', 'none');
export_fig('/home/jluttine/thesis/slides/novac2010_artificial_predictive', ...
           '-eps');
% $$$ print('-depsc', '/home/jluttine/papers/2009NIPS/poster/fig_artificial_predictive');

exp(Qgp.logthetaX{1})
exp(Qgp.logthetaX{2})
exp(Qgp.logthetaX{3})

%return

% Plot spatial components
Mh = size(inWh,2);
Wh = zeros(Mh,D);
varWh = zeros(Mh,D);
fig1 = figure;
% $$$ fig2 = figure;
% $$$ fig3 = figure;
hax = [];
for d=1:D
  %first = (d-1)*M + 1;
  %last = first + M - 1;
  %inds = first:last;
  inds = Qgp.indsW(:,d);
  [Wh(:,d), varWh(:,d)] = gppred(Qgp.inW, Qgp.W(:,d), Qgp.CovW(inds,inds), ...
                                 inWh, Qgp.logthetaW{d}, Qgp.covfuncW{d});

  % Plot mean map
  figure(fig1)
  subplot(2,ceil(D/2),d)
  contourf(coord1,coord2,reshape(Wh(:,d), size(coord1)),20);
  hax(d,1) = gca;
  hold on
  plot(inW(1,:),inW(2,:), 'kx', 'MarkerSize', 8);
  plot(inW(1,examples),inW(2,examples), 'ko', 'MarkerSize', 8);
  %  plot(inW(1,examples),inW(2,examples), 'kx', 'MarkerSize', 10);
  shading('flat');
  cl = get(gca, 'clim');
  cl = max(abs(cl));
  set(gca, 'clim', [-cl cl]);
  mapcolormap;
  set(gca, 'xtick', [], 'ytick', []);
  %pbaspect([1 1 1]);
%  h_cb(d,1) = colorbar('SouthOutside');

% $$$   % Plot uncertainty map
% $$$   figure(fig2)
% $$$ %  subplot(1,D,d);
% $$$   subplot(2,D,D+d);
% $$$   contourf(coord1,coord2,reshape(sqrt(varWh(:,d)), size(coord1)),20);
% $$$   hax(d,2) = gca;
% $$$   hold on
% $$$   plot(inW(1,:),inW(2,:), 'kx', 'MarkerSize', 8);
% $$$   plot(inW(1,examples),inW(2,examples), 'ko', 'MarkerSize', 8);
% $$$   %  plot(inW(1,examples),inW(2,examples), 'kx', 'MarkerSize', 10);
% $$$   shading('flat');
% $$$   cl = get(gca, 'clim');
% $$$   cl = max(abs(cl));
% $$$   set(gca, 'clim', [0 cl]);
% $$$   %mapcolormap;
% $$$   m = colormap('hot');
% $$$   colormap(m(end:-1:1,:));
% $$$   set(gca, 'xtick', [], 'ytick', []);
% $$$   %pbaspect([1 1 1]);
% $$$   h_cb(d,2) = colorbar('SouthOutside');

% $$$   % Plot real map
% $$$   figure(fig3)
% $$$   subplot(1,D,d)
% $$$   contourf(coord1,coord2,reshape(W_function(:,d), size(coord1)),20);
% $$$   hax(d,3) = gca;
% $$$   hold on
% $$$   plot(inW(1,:),inW(2,:), 'kx', 'MarkerSize', 8);
% $$$   plot(inW(1,examples),inW(2,examples), 'ko', 'MarkerSize', 8);
% $$$   %  plot(inW(1,examples),inW(2,examples), 'kx', 'MarkerSize', 10);
% $$$   shading('flat');
% $$$   cl = get(gca, 'clim');
% $$$   cl = max(abs(cl));
% $$$   set(gca, 'clim', [-cl cl]);
% $$$   mapcolormap;
% $$$   set(gca, 'xtick', [], 'ytick', []);
% $$$   %pbaspect([1 1 1]);
% $$$   h_cb(d,3) = colorbar('SouthOutside');

end
cb_height = 0.0;
cb_textheight = 0.0;
cb_sep = 0.0;

for i=1:1
  set_subplot_positions(hax(:,i), 2, 2, [0.01 0.01 0.01 0.01], [0.02 0.02]);
end
for d=1:numel(hax)
  pos_ax = get( hax(d), 'Position');
  cb_pos = pos_ax;
  cb_pos(2) = pos_ax(2) - cb_height - cb_sep;
  cb_pos(4) = cb_height;
  axis(hax(d), 'square')
%  set(h_cb(d), 'Position', cb_pos, 'FontSize', 7);
end

set_figure_size(fig1, 6, 6);
% $$$ set_figure_size(fig2, 12, 4);
% $$$ set_figure_size(fig3, 12, 4);
print(fig1, '-depsc', ['/home/jluttine/thesis/slides/' ...
                    'novac2010_artificial_loadings']);
% $$$ print(fig2, '-depsc', ['/home/jluttine/papers/2009NIPS/poster/' ...
% $$$                     'novac2010_artificial_loadings_uncertainty']);
% $$$ print(fig3, '-depsc', ['/home/jluttine/papers/2009NIPS/poster/' ...
% $$$                     'novac2010_artificial_true_loadings']);


%%*****************************************************************************
%% sqlp: solve an semidefinite-quadratic-linear program
%%       by infeasible path-following method.
%%
%%  [obj,X,y,Z,info,runhist] = sqlp(blk,At,C,b,OPTIONS,X0,y0,Z0);
%%
%%  Input: blk: a cell array describing the block diagonal structure of SQL data.
%%          At: a cell array with At{p} = [svec(Ap1) ... svec(Apm)]
%%         b,C: data for the SQL instance.
%%  (X0,y0,Z0): an initial iterate (if it is not given, the default is used).
%%     OPTIONS: a structure that specifies parameters required in sqlp.m,
%%              (if it is not given, the default in sqlparameters.m is used).
%%
%%  Output: obj  = [<C,X> <b,y>].
%%          (X,y,Z): an approximately optimal solution or a primal or dual
%%                   infeasibility certificate.
%%          info.termcode = termination-code
%%          info.iter     = number of iterations
%%          info.obj      = [primal-obj, dual-obj]
%%          info.cputime  = total-time
%%          info.gap      = gap
%%          info.pinfeas  = primal_infeas
%%          info.dinfeas  = dual_infeas
%%          runhist.pobj    = history of primal objective value.
%%          runhist.dobj    = history of dual   objective value.
%%          runhist.gap     = history of <X,Z>.
%%          runhist.pinfeas = history of primal infeasibility.
%%          runhist.dinfeas = history of dual   infeasibility.
%%          runhist.cputime = history of cputime spent.
%%----------------------------------------------------------------------------
%%  The OPTIONS structure specifies the required parameters:
%%      vers  gam  predcorr  expon  gaptol  inftol  steptol
%%      maxit  printlevel  scale_data ...
%%      (all have default values set in sqlparameters.m).
%%
%%*************************************************************************
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

  function [obj,X,y,Z,info,runhist] = sqlp(blk,At,C,b,OPTIONS,X0,y0,Z0);
%%
%%-----------------------------------------
%% get parameters from the OPTIONS structure.
%%-----------------------------------------
%%
   global matlabversion ispc_hp_ibm
   global spdensity  iter  solve_ok  switch2LU  depconstr
   global cachesize  smallblkdim  printlevel
   global schurfun   schurfun_par

   warning off;
   matlabversion = sscanf(version,'%f');
   matlabversion = matlabversion(1);
   ispc_hp_ibm = strncmp(computer,'PC',2) | strncmp(computer,'HP',2) | ...
                 strncmp(computer,'IBM',3);

   vers        = 1;
   predcorr    = 1;
   gam         = 0;
   expon       = 1;
   gaptol      = 1e-8;
   inftol      = 1e-8;
   steptol     = 1e-6;
   maxit       = 100;
   printlevel  = 3;
   stoplevel   = 1;
   scale_data  = 0;
   spdensity   = 0.5;
   rmdepconstr = 0;
   cachesize   = 256;
   smallblkdim = 15;
   schurfun     = cell(size(blk,1),1);
   schurfun_par = cell(size(blk,1),1);
   if exist('OPTIONS')
      if isfield(OPTIONS,'vers');        vers     = OPTIONS.vers; end
      if isfield(OPTIONS,'predcorr');    predcorr = OPTIONS.predcorr; end
      if isfield(OPTIONS,'gam');         gam      = OPTIONS.gam; end
      if isfield(OPTIONS,'expon');       expon    = OPTIONS.expon; end
      if isfield(OPTIONS,'gaptol');      gaptol   = OPTIONS.gaptol; end
      if isfield(OPTIONS,'inftol');      inftol   = OPTIONS.inftol; end
      if isfield(OPTIONS,'steptol');     steptol  = OPTIONS.steptol; end
      if isfield(OPTIONS,'maxit');       maxit    = OPTIONS.maxit; end
      if isfield(OPTIONS,'printlevel');  printlevel  = OPTIONS.printlevel; end
      if isfield(OPTIONS,'stoplevel');   stoplevel   = OPTIONS.stoplevel; end
      if isfield(OPTIONS,'scale_data');  scale_data  = OPTIONS.scale_data; end
      if isfield(OPTIONS,'spdensity');   spdensity   = OPTIONS.spdensity; end
      if isfield(OPTIONS,'rmdepconstr'); rmdepconstr = OPTIONS.rmdepconstr; end
      if isfield(OPTIONS,'cachesize');   cachesize   = OPTIONS.cachesize; end
      if isfield(OPTIONS,'smallblkdim'); smallblkdim = OPTIONS.smallblkdim; end
      if isfield(OPTIONS,'schurfun');
         schurfun = OPTIONS.schurfun;
         if ~isempty(schurfun); scale_data = 0; end
      end
      if isfield(OPTIONS,'schurfun_par'); schurfun_par = OPTIONS.schurfun_par; end
      if isempty(schurfun); schurfun = cell(size(blk,1),1); end
      if isempty(schurfun_par); schurfun_par = cell(size(blk,1),1); end
   end
%%
   if all(vers-[1 2]); error('*** vers must be 1 or 2 ***'); end;
%%
%%-----------------------------------------
%% convert matrices to cell arrays.
%%-----------------------------------------
%%
   if ~iscell(At); At = {At}; end;
   if ~iscell(C);  C = {C}; end;
   m = length(b);
   if all(size(At) == [size(blk,1), m]);
      convertyes = zeros(size(blk,1),1);
      for p = 1:size(blk,1)
         if strcmp(blk{p,1},'s') & all(size(At{p,1}) == sum(blk{p,2}))
            convertyes(p) = 1;
         end
      end
      if any(convertyes)
         if (printlevel); fprintf('\n sqlp: converting At into required format'); end
         At = svec(blk,At,ones(size(blk,1),1));
      end
   end
   if (nargin <= 5) | (isempty(X0) | isempty(y0) | isempty(Z0));
      [X0,y0,Z0] = infeaspt(blk,At,C,b);
   end
   X = X0; y = y0; Z = Z0;
   if ~iscell(X);  X = {X}; end;
   if ~iscell(Z);  Z = {Z}; end;
%%
%%-----------------------------------------
%% validate SQLP data.
%%-----------------------------------------
%%
   tstart = cputime;
   [blk,At,C,b,dim,numblk,X,Z] = validate(blk,At,C,b,X,y,Z);
   if (printlevel>=2)
      fprintf('\n num. of constraints = %2.0d',length(b));
      if dim(1);
         fprintf('\n dim. of sdp    var  = %2.0d,',dim(1));
         fprintf('   num. of sdp  blk  = %2.0d',numblk(1));
      end
      if dim(2);
         fprintf('\n dim. of socp   var  = %2.0d,',dim(2));
         fprintf('   num. of socp blk  = %2.0d',numblk(2));
      end
      if dim(3); fprintf('\n dim. of linear var  = %2.0d',dim(3)); end
      if dim(4); fprintf('\n dim. of free   var  = %2.0d',dim(4)); end
   end
%%
%%-----------------------------------------
%% convert unrestricted blk to linear blk.
%%-----------------------------------------
%%
   ublkidx = zeros(size(blk,1),1);
   for p = 1:size(blk,1)
      if strcmp(blk{p,1},'u')
         ublkidx(p) = 1;
         n = 2*blk{p,2};
         blk{p,1} = 'l';
         blk{p,2} = n;
         At{p} = [At{p}; -At{p}];
         C{p} = [C{p}; -C{p}];
         b2 = 1 + abs(b');
         normC = 1+norm(C{p});
         normA = 1+sqrt(sum(At{p}.*At{p}));
         X{p} = max(1,max(b2./normA)) *ones(n,1);
         Z{p} = max(1,max([normA,normC])/sqrt(n)) *ones(n,1);
      end
   end
%%
%%-----------------------------------------
%% check whether {A1,...,Am} is
%% linearly independent.
%%-----------------------------------------
%%
   m0 = length(b);
   [At,b,y,indeprows,depconstr,feasible] = checkdepconstr(blk,At,b,y,rmdepconstr);
   if (~feasible)
      fprintf('\n sqlp: SQLP is not feasible'); return;
   end
%%
%%-----------------------------------------
%% scale SQLP data. Note: must be done only
%% after checkdepconstr
%%-----------------------------------------
%%
   normC2 = zeros(length(C),1);
   for p = 1:length(C); normC2(p) = max(max(abs(C{p}))); end
   normC2 = 1+max(normC2);
   normb2 = 1+max(abs(b));
   normX0 = 1+ops(X0,'norm'); normZ0 = 1+ops(Z0,'norm');
   if (scale_data)
      [At,C,b,X,y,Z,normA,normC,normb] = scaling(blk,At,C,b,X,y,Z);
   else
      normA = 1; normC = 1; normb = 1;
   end
%%
%%-----------------------------------------
%% find the combined list of non-zero
%% elements of Aj, j = 1:k, for each k.
%%-----------------------------------------
%%
   m = length(b);
   [At,C,X,Z,par.permA,par.permZ] = sortA(blk,At,C,b,X,Z);
   [par.isspA,par.nzlistA,par.nzlistAsum,par.isspAy,par.nzlistAy] = nzlist(blk,At,m);
%%
%%-----------------------------------------
%% initialization
%%-----------------------------------------
%%
   [Xchol,indef(1)] = blkcholfun(blk,X);
   [Zchol,indef(2)] = blkcholfun(blk,Z);
   if any(indef)
      if (printlevel); fprintf('\n Stop: X, Z are not both positive definite'); end
      termcode = -3;
      return;
   end
   nn = 0;
   for p = 1:size(blk,1);
      pblk = blk(p,:);
      if strcmp(pblk{1},'s') | strcmp(pblk{1},'q') | strcmp(pblk{1},'l')
         nn = nn + sum(pblk{2});
      end
   end
   AX = AXfun(blk,At,par.permA,X);
   rp = b-AX;
   ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
   ZpATynorm = ops(ZpATy,'norm');
   Rd = ops(C,'-',ZpATy);
   obj = (normb*normC)*[blktrace(blk,C,X), b'*y];
   trXZ = blktrace(blk,X,Z);
   gap = (normb*normC)*trXZ;
   mu  = trXZ/nn;
   rel_gap = gap/(1+sum(abs(obj)));
   prim_infeas = norm(rp)/normb2;
   dual_infeas = ops(Rd,'norm')/normC2;
   infeas_meas = max(prim_infeas,dual_infeas);
   if (scale_data)
      infeas_org(1) = norm(normA.*rp)*normb/normb2;
      infeas_org(2) = ops(Rd,'norm')*normC/normC2;
   else
      infeas_org = [0,0];
   end
   termcode = -6;
   pstep = 0; dstep = 0; pred_convg_rate = 1; corr_convg_rate = 1;
   prim_infeas_bad = 0;  homRd = inf; homrp = inf;
   runhist.pobj = obj(1);
   runhist.dobj = obj(2);
   runhist.gap  = gap;
   runhist.relgap  = rel_gap;
   runhist.pinfeas = prim_infeas;
   runhist.dinfeas = dual_infeas;
   runhist.infeas  = infeas_meas;
   runhist.step    = 0;
   runhist.cputime = cputime-tstart;
   ttime.preproc   = runhist.cputime;
   ttime.pred = 0; ttime.pred_pstep = 0; ttime.pred_dstep = 0;
   ttime.corr = 0; ttime.corr_pstep = 0; ttime.corr_dstep = 0;
   ttime.pchol = 0; ttime.dchol = 0; ttime.misc = 0;
%%
%%-----------------------------------------
%% display parameters and initial info
%%-----------------------------------------
%%
   if (printlevel >= 2)
      fprintf('\n********************************************');
      fprintf('***********************\n');
      fprintf('   SDPT3: Infeasible path-following algorithms');
      fprintf('\n********************************************');
      fprintf('***********************\n');
      [hh,mm,ss] = mytimed(ttime.preproc);
      if (printlevel>=3)
         fprintf(' version  predcorr  gam  expon  scale_data\n');
         if (vers == 1); fprintf('   HKM '); elseif (vers == 2); fprintf('    NT '); end
         fprintf('     %1.0f      %4.3f',predcorr,gam);
         fprintf('   %1.0f        %1.0f    %1.0f\n',expon,scale_data);
         fprintf('\nit  pstep dstep p_infeas d_infeas  gap')
         fprintf('     mean(obj)    cputime\n');
         fprintf('------------------------------------------------');
         fprintf('-------------------\n');
         fprintf('%2.0f  %4.3f %4.3f %2.1e %2.1e',0,0,0,prim_infeas,dual_infeas);
         fprintf('  %2.1e %- 7.6e  %d:%d:%d',gap,mean(obj),hh,mm,ss);
      end
   end
%%
%%---------------------------------------------------------------
%% start main loop
%%---------------------------------------------------------------
%%
   param.inftol      = inftol;
   param.normA       = normA;
   param.normC       = normC;
   param.normb       = normb;
   param.normX0      = normX0;
   param.normZ0      = normZ0;
   param.m0          = m0;
   param.indeprows   = indeprows;
   param.gaptol      = gaptol;
   param.inftol      = inftol;
   param.scale_data  = scale_data;
   param.printlevel  = printlevel;
%%
   for iter = 1:maxit;

       update_iter = 0; breakyes = 0; pred_slow = 0; corr_slow = 0; step_short = 0;
       tstart  = cputime;
       timeold = cputime;
%%
%%---------------------------------------------------------------
%% predictor step.
%%---------------------------------------------------------------
%%
       if (predcorr)
          sigma = 0;
       else
          sigma = 1-0.9*min(pstep,dstep);
          if (iter == 1); sigma = 0.5; end;
       end
       sigmu = sigma*mu;

       invXchol = cell(size(blk,1),1);
       invZchol = ops(Zchol,'inv');
       if (vers == 1);
          [par,dX,dy,dZ,coeff,L,hRd] = ...
           HKMpred(blk,At,par,rp,Rd,sigmu,X,Z,invZchol);
       elseif (vers == 2);
          [par,dX,dy,dZ,coeff,L,hRd] = ...
           NTpred(blk,At,par,rp,Rd,sigmu,X,Z,Zchol,invZchol);
       end
       if (solve_ok <= 0)
          fprintf('\n Stop: difficulty in computing predictor directions');
          runhist.cputime(iter+1) = cputime-tstart;
          termcode = -4;
          break;
       end
       timenew = cputime;
       ttime.pred = ttime.pred + timenew-timeold; timeold = timenew;
%%
%%-----------------------------------------
%% step-lengths for predictor step
%%-----------------------------------------
%%
      if (gam == 0)
         gamused = 0.9 + 0.09*min(pstep,dstep);
      else
         gamused = gam;
      end
      [Xstep,invXchol] = steplength(blk,X,dX,Xchol,invXchol);
      pstep = min(1,gamused*Xstep);
      if (Xstep > .99e12) & (blktrace(blk,C,dX) < -1e-3) & (prim_infeas < 1e-3)
         pstep = Xstep;
         if (printlevel); fprintf('\n Predictor: dual seems infeasible.'); end
      end
      timenew = cputime;
      ttime.pred_pstep = ttime.pred_pstep + timenew-timeold; timeold = timenew;
      Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
      dstep = min(1,gamused*Zstep);
      if (Zstep > .99e12) & (b'*dy > 1e-3) & (dual_infeas < 1e-3)
         dstep = Zstep;
         if (printlevel); fprintf('\n Predictor: primal seems infeasible.'); end
      end
      trXZpred = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ) ...
                 + pstep*dstep*blktrace(blk,dX,dZ);
      gappred = (normb*normC)*trXZpred;
      mupred  = trXZpred/nn;
      mupredhist(iter) = mupred;
      timenew = cputime;
      ttime.pred_dstep = ttime.pred_dstep + timenew-timeold; timeold = timenew;
%%
%%-----------------------------------------
%%  stopping criteria for predictor step.
%%-----------------------------------------
%%
      if (min(pstep,dstep) < steptol) & (stoplevel)
         if (printlevel)
            fprintf('\n Stop: steps in predictor too short:');
            fprintf(' pstep = %3.2e,  dstep = %3.2e\n',pstep,dstep);
         end
         runhist.cputime(iter+1) = cputime-tstart;
         termcode = -2;
         breakyes = 1;
      end
      if (iter >= 2)
         idx = [max(2,iter-2) : iter];
         pred_slow = all(mupredhist(idx)./mupredhist(idx-1) > 0.4);
         idx = [max(2,iter-5) : iter];
         pred_convg_rate = mean(mupredhist(idx)./mupredhist(idx-1));
         pred_slow = pred_slow + (mupred/mu > 5*pred_convg_rate);
      end
      if (~predcorr)
         if (max(mu,infeas_meas) < 1e-6) & (pred_slow) & (stoplevel)
            if (printlevel)
               fprintf('\n  Stop: lack of progress in predictor:');
               fprintf(' mupred/mu = %3.2f, pred_convg_rate = %3.2f.',...
                         mupred/mu,pred_convg_rate);
            end
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -1;
            breakyes = 1;
         else
            update_iter = 1;
         end
      end
%%
%%---------------------------------------------------------------
%% corrector step.
%%---------------------------------------------------------------
%%
      if (predcorr) & (~breakyes)
         step_pred = min(pstep,dstep);
         if (mu > 1e-6)
            if (step_pred < 1/sqrt(3));
               expon_used = 1;
            else
               expon_used = max(expon,3*step_pred^2);
            end
         else
            expon_used = max(1,min(expon,3*step_pred^2));
         end
         sigma = min( 1, (mupred/mu)^expon_used );
         sigmu = sigma*mu;
%%
         if (vers == 1)
            [dX,dy,dZ] = HKMcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         elseif (vers == 2)
            [dX,dy,dZ] = NTcorr(blk,At,par,rp,Rd,sigmu,hRd,...
             dX,dZ,coeff,L,X,Z);
         end
         if (solve_ok <= 0)
            fprintf('\n Stop: difficulty in computing corrector directions');
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -4;
            break;
         end
         timenew = cputime;
         ttime.corr = ttime.corr + timenew-timeold; timeold = timenew;
%%
%%-----------------------------------
%% step-lengths for corrector step
%%-----------------------------------
%%
         if (gam == 0)
            gamused = 0.9 + 0.09*min(pstep,dstep);
         else
            gamused = gam;
         end
         Xstep = steplength(blk,X,dX,Xchol,invXchol);
         pstep = min(1,gamused*Xstep);
         if (Xstep > .99e12) & (blktrace(blk,C,dX) < -1e-3) & (prim_infeas < 1e-3)
            pstep = Xstep;
            if (printlevel); fprintf('\n Corrector: dual seems infeasible.'); end
         end
         timenew = cputime;
         ttime.corr_pstep = ttime.corr_pstep + timenew-timeold; timeold = timenew;
         Zstep = steplength(blk,Z,dZ,Zchol,invZchol);
         dstep = min(1,gamused*Zstep);
         if (Zstep > .99e12) & (b'*dy > 1e-3) & (dual_infeas < 1e-3)
            dstep = Zstep;
            if (printlevel); fprintf('\n Corrector: primal seems infeasible.'); end
         end
         trXZcorr = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ)...
                    + pstep*dstep*blktrace(blk,dX,dZ);
         gapcorr = (normb*normC)*trXZcorr;
         mucorr  = trXZcorr/nn;
         timenew = cputime;
         ttime.corr_dstep = ttime.corr_dstep + timenew-timeold; timeold = timenew;
%%
%%-----------------------------------------
%%  stopping criteria for corrector step
%%-----------------------------------------
%%
         if (iter >= 2)
            idx = [max(2,iter-2) : iter];
            corr_slow = all(runhist.gap(idx)./runhist.gap(idx-1) > 0.8);
            idx = [max(2,iter-5) : iter];
            corr_convg_rate = mean(runhist.gap(idx)./runhist.gap(idx-1));
            corr_slow = corr_slow + (mucorr/mu > max(min(1,5*corr_convg_rate),0.8));
         end
	 if (max(mu,infeas_meas) < 1e-6) & (iter > 10) & (corr_slow) & (stoplevel)
   	    if (printlevel)
               fprintf('\n  Stop: lack of progress in corrector:');
               fprintf(' mucorr/mu = %3.2f, corr_convg_rate = %3.2f',...
                         mucorr/mu,corr_convg_rate);
            end
            runhist.cputime(iter+1) = cputime-tstart;
            termcode = -1;
            breakyes = 1;
         else
            update_iter = 1;
         end
      end
%%
%%---------------------------------------------------------------
%% udpate iterate
%%---------------------------------------------------------------
%%
      indef = [1,1];
      if (update_iter)
         for t = 1:5
            [Xchol,indef(1)] = blkcholfun(blk,ops(X,'+',dX,pstep));
            timenew = cputime;
            ttime.pchol = ttime.pchol + timenew-timeold; timeold = timenew;
            if (indef(1)); pstep = 0.8*pstep; else; break; end
         end
	 if (t > 1); pstep = gamused*pstep; end
	 for t = 1:5
            [Zchol,indef(2)] = blkcholfun(blk,ops(Z,'+',dZ,dstep));
            timenew = cputime;
            ttime.dchol = ttime.dchol + timenew-timeold; timeold = timenew;
            if (indef(2)); dstep = 0.8*dstep; else; break; end
         end
	 if (t > 1); dstep = gamused*dstep; end
         AXtmp = AX + pstep*AXfun(blk,At,par.permA,dX);
         prim_infeasnew = norm(b-AXtmp)/normb2;
         if any(indef)
            if (printlevel); fprintf('\n Stop: X, Z not both positive definite'); end
            termcode = -3;
            breakyes = 1;
         elseif (prim_infeasnew > max([rel_gap,20*prim_infeas,1e-8])) ...
	    | (prim_infeasnew > max([1e-4,20*prim_infeas]) & (switch2LU))
            if (stoplevel) & (max(pstep,dstep)<=1)
               if (printlevel)
                  fprintf('\n Stop: primal infeas has deteriorated too much, %2.1e',prim_infeasnew);
               end
               termcode = -7;
               breakyes = 1;
            end
         else
            X = ops(X,'+',dX,pstep);
            y = y+dstep*dy;
            Z = ops(Z,'+',dZ,dstep);
         end
      end
%%---------------------------------------------------------------
%% adjust linear blk arising from unrestricted blk
%%---------------------------------------------------------------
%%
      for p = 1:size(blk,1)
         if (ublkidx(p) == 1)
            len = blk{p,2}/2;
            alpha = 0.8;
            xtmp = min(X{p}([1:len]),X{p}(len+[1:len]));
            X{p}([1:len]) = X{p}([1:len]) - alpha*xtmp;
            X{p}(len+[1:len]) = X{p}(len+[1:len]) - alpha*xtmp;
            if (mu < 1e-8)
               Z{p} = 0.5*mu./max(1,X{p});
	    else
               ztmp = min(1,max(Z{p}([1:len]),Z{p}(len+[1:len])));
               beta1 = xtmp'*(Z{p}([1:len])+Z{p}(len+[1:len]));
               beta2 = (X{p}([1:len])+X{p}(len+[1:len])-2*xtmp)'*ztmp;
               beta = max(0.1,min(beta1/beta2,0.5));
               Z{p}([1:len]) = Z{p}([1:len]) + beta*ztmp;
               Z{p}(len+[1:len]) = Z{p}(len+[1:len]) + beta*ztmp;
            end
         end
      end
%%
%%---------------------------------------------------------------
%% compute rp, Rd, infeasibities, etc.
%%---------------------------------------------------------------
%%
      trXZ = blktrace(blk,X,Z);
      gap = (normb*normC)*trXZ;
      mu  = trXZ/nn;
      AX  = AXfun(blk,At,par.permA,X);
      rp  = b-AX;
      ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
      ZpATynorm = ops(ZpATy,'norm');
      Rd  = ops(C,'-',ZpATy);
      obj = (normb*normC)*[blktrace(blk,C,X), b'*y];
      rel_gap = gap/(1+sum(abs(obj)));
      prim_infeas  = norm(rp)/normb2;
      dual_infeas = ops(Rd,'norm')/normC2;
      if (scale_data)
         infeas_org(1) = norm(normA.*rp)*normb/normb2;
         infeas_org(2) = ops(Rd,'norm')*normC/normC2;
      end
      infeas_meas = max(prim_infeas,dual_infeas);
      if (obj(2) > 0); homRd = ZpATynorm/(obj(2)/(normb*normC)); else; homRd = inf; end
      if (obj(1) < 0); homrp = norm(AX)/(-obj(1)/(normb*normC)); else; homrp = inf; end
      runhist.pobj(iter+1)  = obj(1);
      runhist.dobj(iter+1)  = obj(2);
      runhist.gap(iter+1)   = gap;
      runhist.relgap(iter+1)  = rel_gap;
      runhist.pinfeas(iter+1) = prim_infeas;
      runhist.dinfeas(iter+1) = dual_infeas;
      runhist.infeas(iter+1)  = infeas_meas;
      runhist.step(iter+1)    = min(pstep,dstep);
      runhist.cputime(iter+1) = cputime-tstart;
      timenew = cputime;
      ttime.misc = ttime.misc + timenew-timeold; timeold = timenew;
      [hh,mm,ss] = mytimed(sum(runhist.cputime));
      if (printlevel>=3)
         fprintf('\n%2.0f  %4.3f %4.3f',iter,pstep,dstep);
         fprintf(' %2.1e %2.1e  %2.1e',prim_infeas,dual_infeas,gap);
         fprintf(' %- 7.6e  %d:%d:%d',mean(obj),hh,mm,ss);
      end
%%
%%--------------------------------------------------
%% check convergence.
%%--------------------------------------------------
%%
      param.iter        = iter;
      param.obj         = obj;
      param.rel_gap     = rel_gap;
      param.gap         = gap;
      param.mu          = mu;
      param.prim_infeas = prim_infeas;
      param.dual_infeas = dual_infeas;
      param.homRd       = homRd;
      param.homrp       = homrp;
      param.AX          = AX;
      param.ZpATynorm   = ZpATynorm;
      param.normX       = ops(X,'norm');
      param.normZ       = ops(Z,'norm');
      param.termcode    = termcode;
      param.stoplevel   = stoplevel;
      param.prim_infeas_bad = prim_infeas_bad;
%%
      if (~breakyes)
         [termcode,breakyes,prim_infeas_bad,restart] = sqlpcheckconvg(param,runhist);
      end
      if (breakyes); break; end
      if (restart)
         [X,y,Z] = infeaspt(blk,At,C,b,2,1e5);
         trXZ = blktrace(blk,X,Z);
         gap = (normb*normC)*trXZ;
         mu  = trXZ/nn;
         rp  = b-AXfun(blk,At,par.permA,X);
         ZpATy = ops(Z,'+',Atyfun(blk,At,par.permA,par.isspAy,y));
         Rd  = ops(C,'-',ZpATy);
         prim_infeas  = norm(rp)/normb2;
         dual_infeas = ops(Rd,'norm')/normC2;
         infeas_meas = max(prim_infeas,dual_infeas);
         [Xchol,indef(1)] = blkcholfun(blk,X);
         [Zchol,indef(2)] = blkcholfun(blk,Z);
         stoplevel = 3;
      end
   end
%%
%%---------------------------------------------------------------
%% end of main loop
%%---------------------------------------------------------------
%%
%%---------------------------------------------------------------
%% unscale and produce infeasibility certificates if appropriate
%%---------------------------------------------------------------
%%
   if (iter >= 1)
      param.termcode = termcode;
      [X,y,Z,termcode,resid,reldist] = sqlpmisc(blk,At,C,b,X,y,Z,par.permZ,param);
   end
%%
%%---------------------------------------------------------------
%% recover unrestricted blk from linear blk
%%---------------------------------------------------------------
%%
   for p = 1:size(blk,1)
      if (ublkidx(p) == 1)
         n = blk{p,2}/2;
         X{p} = X{p}(1:n)-X{p}(n+[1:n]);
         Z{p} = Z{p}(1:n);
      end
   end
%%
%%---------------------------------------------------------------
%% print summary
%%---------------------------------------------------------------
%%
   if (scale_data)
      dimacs = [infeas_org(1); 0; infeas_org(2); 0];
   else
      dimacs = [prim_infeas; 0; dual_infeas; 0];
   end
   dimacs = [dimacs; [-diff(obj); gap]/(1+sum(abs(obj)))];
   info.dimacs   = dimacs;
   info.termcode = termcode;
   info.iter     = iter;
   info.obj      = obj;
   info.gap      = gap;
   info.relgap   = rel_gap;
   info.pinfeas  = prim_infeas;
   info.dinfeas  = dual_infeas;
   info.cputime  = sum(runhist.cputime);
   info.resid    = resid;
   info.reldist  = reldist;
%%
   nnorm.b = norm(b); nnorm.C = ops(C,'norm'); nnorm.A = ops(At,'norm');
   nnorm.X = ops(X,'norm'); nnorm.y = norm(y); nnorm.Z = ops(Z,'norm');
   sqlpsummary(info,ttime,infeas_org,nnorm,printlevel);
%%*****************************************************************************


function ALAP_demo_attenuation
% This demonstration illustrates context or state-dependent precision (i.e.
% attention), which is necessary to disambiguate between sensations
% caused exogenously and self-generated sensations. In brief, it is
% necessary to attend away from the sensory consequences of action to
% preclude sensory evidence overriding the prior beliefs that cause
% movement. This necessarily reduced the confidence in self-generated
% sensations and provides a simple (Bayes-optimal) explanation for sensory
% attenuation - in terms of the attention of sensory precision. We
% illustrate this in the setting of the force matching illusion and go on
% to show that increasing the conviction in (precision of) prior beliefs
% abolishes sensory attenuation at the expense of false (delusional)
% posterior beliefs about antagonistic external forces.
%__________________________________________________________________________
% Copyright (C) 2012 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: ALAP_demo_attenuation.m 4826 2012-08-03 16:45:09Z karl $

% process (G) and model (M)
%==========================================================================

% set dimensions for generalised coordinates
%--------------------------------------------------------------------------
G(1).E.d        = 2;                   % approximation order
G(1).E.n        = 4;                   % embedding order

M(1).E.d        = 2;                   % approximation order
M(1).E.n        = 4;                   % embedding order
M(1).E.s        = 1/2;                 % embedding order

M(1).E.method.x = 1;                   % state-dependent noise
M(1).E.method.v = 1;                   % state-dependent noise
M(1).E.method.h = 0;                   % suppress optimisation
M(1).E.method.g = 0;                   % suppress optimisation

G(1).f  = inline('tanh(a) - x/4','x','v','a','P');
G(1).g  = inline('[x; v + x]','x','v','a','P');
G(1).x  = 0;                           % hidden state
G(1).v  = [0; 0];                      % hidden cause (sensory data)
G(1).V  = exp(8);                      % precision (noise)
G(1).W  = exp(8);                      % precision (states)
G(1).U  = [exp(0) 0];                  % precision (action)

% level 2; causes
%--------------------------------------------------------------------------
G(2).v  = 0;                           % exogenous  cause
G(2).a  = 0;                           % endogenous cause (action)
G(2).V  = exp(16);

% state-dependent precision (attentional bias) in generative model (M):
%--------------------------------------------------------------------------
M(1).f  = inline('v - x/4','x','v','P');
M(1).g  = inline('[x(1); sum(x)]','x','v','P');
M(1).x  = [0; 0];                      % hidden states
M(1).v  = [0; 0];                      % hidden causes
M(1).W  = exp(4);                      % precision (states)
M(1).ph = inline('[1; 1]*(8 - h*tanh(v(1) + x(1)))','x','v','h','M');
M(1).hE = 6;

% level 2; causes
%--------------------------------------------------------------------------
M(2).v  = [0; 0];                      % hidden cause
M(2).V  = [exp(6); exp(0)];

% Demonstration of the need for sensory attenuation
%==========================================================================

% hidden cause and prior expectations
%--------------------------------------------------------------------------
N      = 32;
C      = zeros(1,N);
U(1,:) = exp(-((1:N) - N/2).^2/(4.^2))*1;
U(2,:) = zeros(1,N);

% assemble model structure
%--------------------------------------------------------------------------
DEM.M = M;
DEM.G = G;
DEM.C = C;
DEM.U = U;

hE    = (-4:1:6);
for i = 1:length(hE)

    rng('default')

    LAP         = DEM;
    LAP.M(1).hE = hE(i);
    LAP         = spm_ALAP(LAP);

    % true and perceived force exerted (endogenously)
    %----------------------------------------------------------------------
    Px(i) = max(LAP.pU.x{1}(1,:));
    Qx(i) = max(LAP.qU.x{1}(1,:));

    % plot self-generated movement
    %----------------------------------------------------------------------
    if hE(i) == 0

        spm_figure('GetWin','Figure 1: Low attenuation');
        spm_DEM_qU(LAP.qU,LAP.pU)

    elseif hE(i) == 6

        spm_figure('GetWin','Figure 2: High attenuation');
        spm_DEM_qU(LAP.qU,LAP.pU)

    end
end

% adjust axes
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2: High attenuation');
subplot(2,2,2); spm_axis tight, a = axis;
subplot(2,2,1); axis(a);
spm_figure('GetWin','Figure 1: Low attenuation');
subplot(2,2,2); axis(a);
subplot(2,2,1); axis(a);

spm_figure('GetWin','Figure 2: High attenuation');
subplot(2,2,3); spm_axis tight, a = axis;
subplot(2,2,4); axis(a);
spm_figure('GetWin','Figure 1: Low attenuation');
subplot(2,2,3); axis(a);
subplot(2,2,4); axis(a);

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 3');

subplot(2,1,1)
plot(hE,[Px; Qx])
axis square
xlabel('attenuation of sensory precicion','FontSize',12)
ylabel('true and perceived force exerted','FontSize',12)
legend({'true','perceived'})
title('Sensory attenuation and action','FontSize',16)

% Demonstration of sensory attenuation
%==========================================================================

% replay internal force as external force
%--------------------------------------------------------------------------
rng('default')
DEM.C  = [C LAP.pU.x{1}];
DEM.U  = [U sparse(2,N)];
DEM    = spm_ALAP(DEM);

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 4');
spm_DEM_qU(DEM.qU,DEM.pU)

% Force matching
%==========================================================================

% replay perceived (at 90% confidence) internal force
%--------------------------------------------------------------------------
for i = 1:N
    CI(i) = 1.694*sqrt(LAP.qU.S{i}(1,1));
end

DEM.C  = [C (LAP.qU.x{1}(1,:) - CI)];
DEM.U  = [U sparse(2,N)];
DEM    = spm_ALAP(DEM);

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 5');
spm_DEM_qU(DEM.qU,DEM.pU)
subplot(2,2,2); spm_axis tight, a = axis;
subplot(2,2,1); axis(a);
subplot(2,2,3); spm_axis tight, a = axis;
subplot(2,2,4); axis(a);

% Use a range of self-generated forces - with D = 0
%==========================================================================
F     = (1:4)/2;
for i = 1:length(F)

    DEM.C = C;
    DEM.U = U*F(i);
    DEM   = spm_ALAP(DEM);

    % self-generated and matched (inferred) force
    %----------------------------------------------------------------------
    [x j] = max(DEM.pU.x{1}(1,:));
    Sx(i) = x;
    Tx(i) = DEM.qU.x{1}(1,j) - 1.694*sqrt(DEM.qU.S{j}(1,1));

end

% repeat with (delusional) precision D = 2
%--------------------------------------------------------------------------
D           = 2;
DEM.M(1).hE = 6 - D;
DEM.M(1).W  = exp(4 + D);
DEM.M(2).V  = [exp(6 + D); exp(0)];

for i = 1:length(F)

    DEM.C = C;
    DEM.U = U*F(i);
    DEM   = spm_ALAP(DEM);

    % self-generated and matched (inferred) force
    %----------------------------------------------------------------------
    [x j] = max(DEM.pU.x{1}(1,:));
    sx(i) = x;
    tx(i) = DEM.qU.x{1}(1,j) - 1.694*sqrt(DEM.qU.S{j}(1,1));

end

% plot results of force matching
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 6');

subplot(2,1,1)
plot(Tx,Sx,'-'); hold on,
plot(Tx,Sx,'.','MarkerSize',32), hold on
plot(tx,sx,'-.'); hold on,
plot(tx,sx,'.','MarkerSize',16), hold on
plot([0 3],[0 3],':'); hold off

axis square
xlabel('external (perceptually matched) force','FontSize',12)
ylabel('self-generated force','FontSize',12)
title('Force matching illusion','FontSize',16)

% Illustrate false inference with delusional precision
%==========================================================================
D           = 4;
DEM.M(1).hE = 6 - D;
DEM.M(1).W  = exp(4 + D);
DEM.M(2).V  = [exp(6 + D); exp(0)];

% replay perceived (at 90% confidence) internal force
%--------------------------------------------------------------------------
DEM.C = C;
DEM.U = U*2;
DEM   = spm_ALAP(DEM);

for i = 1:N
    CI(i) = 1.694*sqrt(DEM.qU.S{i}(1,1));
end

DEM.C = [C (DEM.qU.x{1}(1,:) - CI)];
DEM.U = [U*2 sparse(2,N)];
DEM   = spm_ALAP(DEM);

% plot false inference under high D
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 7: False (delusional) inference');
spm_DEM_qU(DEM.qU,DEM.pU)