classdef BoundingBoxGaterX <GaterX
% BOUNDINGBOXGATERX Class
%
% Summary of BoundingBoxGaterX:
% This is a class implementation of a bounding box gater, which gates
% measurements, given in the form of bounding boxes (x,y,w,h), depending on
% their proportional overlap with the estimated target bounding boxes.
%
% BoundingBoxGaterX Properties:
%   + OverlapThresh - The minimum overlap, proportional to the each
%                     target's bounding box.
%
% BoundingBoxGaterX Methods:
%   + BoundingBoxGaterX  - Constructor method
%   + gate - Perform gating and generate the validation matrix
%
% See also SystematicResamplerX
    properties
        OverlapThresh
    end

    properties (SetAccess = immutable)
        NumObsDims
    end

    methods
        function this = BoundingBoxGaterX(varargin)
        % BoundingBoxGaterX Constructor method
        %
        % Parameters
        % ----------
        % OverlapThresh: normalised scalar, optional
        %   The normalised ([0,1]) minimum percentage overlap between the estimated
        %   target bounding box and any given measurement, such that a measurement
        %   should be gated.
        %
        % Usage
        % -----
        % * eg = BoundingBoxGaterX(OverlapThresh) returns an object configured
        %   to exclude any measurements that cover less than OverlapThresh %
        %   of a given target state.
        % * eg = BoundingBoxGaterX('OverlapThresh', OverlapThresh) is also valid
        %
        % See also EllipsoidalGaterX/validate

            parser = inputParser;
            parser.addOptional('OverlapThresh',[]);
            parser.KeepUnmatched = true;
            parser.parse(varargin{:});
            this.OverlapThresh = parser.Results.OverlapThresh;

        end

        function [ValidationMatrix, GateVolumes] = gate(this,varargin)
        % GATE Perform bbox gating to generate the validation
        % matrix indicating the valid associations between Tracks and
        % measurements
        %
        % Parameters
        % ----------
        % TrackList: cell vector, optional
        %   A (1 x NumTracks) cell vector, where each cell contains a TrackX
        %   object. It is required that each TrackX object should have a FilterX
        %   object as a property.
        % TrackPredMeasMeans: matrix, optional
        %   A (NumObsDims x NumTracks) matrix, where each ith column represents
        %   the predicted measurement mean for the ith track. Can be used,
        %   in combination with TrackInnovErrCovars (see below), instead of
        %   a TrackList (see above).
        % TrackInnovErrCovars: matrix, optional
        %   A (NumObsDims x NumObsDims x NumTracks) matrix, where each
        %   (:,:,i) page represents the innovation covariance matrix for
        %   the ith track. Can be used, in combination with TrackInnovErrCovars
        %   (see above), instead of a TrackList (see above).
        % MeasurementList: matrix
        %   A (NumObsDims x NumMeasurements) matrix, where each jth column
        %   corresponds to the jth measurement.
        %
        % Usage
        % -----
        % * [ValidationMatrix] = gate(this,TrackList,MeasurementList)
        %   returns the (NumTracks x NumMeasurements) validation matrix
        %   ValidationMatrix indicating the valid associations between tracks
        %   in TrackList (1 x NumTracks array of TrackX objects) and measurements
        %   in MeasurmentList (NumObsDims x NumMeasurements matrix)
        % * [ValidationMatrix] = gate(this,TrackPredMeasMeans,TrackInnovErrCovars,MeasurementList)
        %   returns the (NumTracks x NumMeasurements) validation matrix
        %   ValidationMatrix indicating the valid associations between tracks
        %   in with predicted measurement means given by the (NumObsDims x NumTracks)
        %   TrackPredMeasMeans matrix, innovation covariances given by the
        %   (NumObsDims x NumObsDims x NumTracks) TrackInnovErrCovars matrix, and
        %   measurements in MeasurmentList (NumObsDims x NumMeasurements matrix), as well as a
        %   (1 x NumTracks) vector GateVolumes, containing the volume of the computed
        %   gate for each track.
        %
        % See also EllipsoidalGaterX/EllipsoidalGaterX

            if(nargin == 3)
                TrackList = varargin{1};
                MeasurementList = varargin{2};
                NumTracks = numel(TrackList);
            else
                TrackPredMeasMeans = varargin{1};
                MeasurementList = varargin{3};
                NumTracks = size(TrackPredMeasMeans,2);
            end

            [~, NumMeasurements] = size(MeasurementList);

            % Pre-allocate memory
            GateVolumes = zeros(1,NumTracks);
            ValidationMatrix = zeros(NumTracks,NumMeasurements);

            % Construct Validation and GateVolumes Matrices
            for trackInd = 1:NumTracks

                if(nargin==3)
                    % Store pointer to track
                    Track = TrackList{trackInd};

                    % Extract predicted measurement and innovation covariance from filter
                    PredMeasMean = Track.Filter.PredMeasMean;
                else
                    % Extract predicted measurement and innovation covariance from filter
                    PredMeasMean = TrackPredMeasMeans(:,trackInd);
                end
                if(NumMeasurements>0)
                    intersect_1 = rectint(PredMeasMean', MeasurementList');
                    intersect_1 = intersect_1./(PredMeasMean(3)*PredMeasMean(4));
                    intersect_2 = rectint(MeasurementList', PredMeasMean')';
                    intersect_2 = intersect_2./(MeasurementList(3,:).*MeasurementList(4,:));
                    ValidationMatrix(trackInd,:) = (intersect_1>this.OverlapThresh | intersect_2>this.OverlapThresh);
                end
                GateVolumes(trackInd) = PredMeasMean(3)*PredMeasMean(4);
            end
            this.ValidationMatrix = ValidationMatrix;
            this.GateVolumes = GateVolumes;
        end
    end
end


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

%initialize q,
q = [0.1 0.1 0.1 0.1 0.1]
T = directkinematic(robot, q)
drawrobot3d(robot, q)


function y = power(x,d)
%POWER (overloaded)

% Vectorize x if d is vector
if numel(x)==1 & (numel(d)>1)
    x = x.*ones(size(d));
end
% Vectorize if x is a vector
if numel(d)==1 & (numel(x)>1)
    d = d.*ones(size(x));
end
if ~isequal(size(d),size(x))
    error('Dimension mismatch in power');
end

% Reuse code
if numel(x)==1 && numel(d)==1
    y = mpower(x,d);
    return
end

if isa(d,'sdpvar')
    % Call helper which vectorizes the elements
    y = powerinternalhelper(d,x);
    if isa(y,'sdpvar')
        y.extra.createTime = definecreationtime;
    end
    return
end

% Sanity Check
if prod(size(d))>1
    if any(size(d)~=size(x))
        error('Matrix dimensions must agree.');
    end
else
    d = ones(x.dim(1),x.dim(2))*d;
end

% Trivial cases
if isnumeric(d)
    if all(all(d==0))
        y = ones(x.dim(1),x.dim(2));
        return
    end
    if all(all(d==1))
        y = x;
        return
    end
    if isnan(d)
        disp('You have NaNs in model (learn to debug)')
        error('NaN power makes no sense.');
    end
end

% Fractional, negative or different powers are
% treated less efficiently using simple code.
fractional = any(any((ceil(d)-d>0)));
negative = any(any(d<0));
different = ~all(all(d==d(1)));
if fractional | negative | different
    if x.dim(1)>1 | x.dim(2)>1
        if isequal(x.basis,[spalloc(prod(x.dim),1,0) speye(prod(x.dim))]) & all(d==d(1))
            % Simple case x.^d
            y = vectorizedUnitPower(x,d);
            y.extra.createTime = definecreationtime;
            return
        end
        [n,m] = size(x);
        y = [];
        for i = 1:n % FIX : Vectorize!
            if m == 1
                temp = extsubsref(x,i,1).^d(i,1);
            else
                temp = [];
                for j = 1:m
                    temp = [temp extsubsref(x,i,j).^d(i,j)];
                end
            end
            y = [y;temp];
        end
        y.extra.createTime = definecreationtime;
        return
    else
        base = getbase(x);
        if isequal(base,[0 1])
            mt = yalmip('monomtable');
            var = getvariables(x);
            previous_var = find((mt(:,var)==d)  & (sum(mt~=0,2)==1));
            if isempty(previous_var)
                mt(end+1,:) = mt(getvariables(x),:)*d;
                yalmip('setmonomtable',mt);
                y = recover(size(mt,1));
            else
                y = recover(previous_var);
            end
        elseif (size(base,2) == 2) & base(1)==0
            % Something like a*t^-d
            y = base(2)^d*recover(getvariables(x))^d;
        else
            error('Only unit scalars can have negative or non-integer powers.');
        end
    end
    y.extra.createTime = definecreationtime;
    return
end

if isequal(x.basis,[spalloc(prod(x.dim),1,0) speye(prod(x.dim))]) & all(d==d(1))
     % Simple case x.^d
     y = vectorizedUnitPower(x,d);
     y.extra.createTime = definecreationtime;
     return
 end

% Back to scalar power...
d = d(1,1);
if x.dim(1)>1 | x.dim(2)>1
    switch d
        case 0
            y = 1;
        case 1
            y = x;
        otherwise
            y = x.*power(x,d-1);
    end
else
    error('This should not appen. Report bug (power does not use mpower)')
end

function y = vectorizedUnitPower(x,d)
d = d(1);
[mt,variabletype,hashM,hash] = yalmip('monomtable');
var = getvariables(x);

usedmt = mt(var,:);
newmt = usedmt*d;
hashV = newmt*hash;
if ~any(ismember(hashV,hashM))
    variabletype = [variabletype newvariabletypegen(newmt)];
    y = size(mt,1) + (1:length(var));
    mt = [mt;newmt];
else
    y = [];
    allnewmt = [];
    newvariables = 0;
    keep = zeros(size(usedmt,1),1);
    for i = 1:length(hashV)
        previous_var = find(abs(hashM - hashV(i)) < 1e-20);
        if isempty(previous_var)
            newmt =  usedmt(i,:)*d;
            variabletype = [variabletype newvariabletypegen(newmt)];
            newvariables = newvariables + 1;
            keep(i) = 1;
            y = [y size(mt,1)+newvariables];
        else
            y = [y previous_var];
        end
    end
    mt = [mt;usedmt(find(keep),:)*d];
end
yalmip('setmonomtable',mt,variabletype);
y = reshape(recover(y),x.dim);


%io_writelcm.m
%Jamie Near, McGill University 2014.
%
% USAGE:
% RF=io_writelcm(in,outfile,te);
%
% DESCRIPTION:
% Takes MRS data in matlab structure format and writes it to a text file
% that can be read by LCModel.
%
% INPUTS:
% in         = input data in matlab structure format.
% outfile    = Desired filename of output text file.
% te         = Echo time of acquisition (in ms).
%
% OUTPUTS:
% RF         = Same as input.  Not used.  The primary output of this
%                function is a text file in LCModel raw format.

function RF=io_writelcm(in,outfile,te);
%function RF=writelcm(in,outfile,te);

if in.flags.isFourSteps
    error('ERROR:  Must first combine four subspecs using op_fourStepCombine');
end

if in.flags.subtracted
    %error('ERROR:  This operation must be done prior to combining subspecs');
end

if ~in.flags.averaged
    disp('WARNING:  Signals must be averaged first');
end

if ~in.flags.addedrcvrs
    error('ERROR:  reciever channels must be combined first');
end

% datsets=1;
% zop=0;
% t0=0;
  Bo=in.Bo;
  hzppm=42.577*Bo;
  dwelltime=in.dwelltime;
% Nuc=0;
% PatName='No Name';
% scanner='TrioTim';
% addinfo='jnear';
seq='PRESS';

RF=zeros(in.sz(in.dims.t),2);
RF(:,1)=real(in.fids(:,1));
RF(:,2)=-imag(in.fids(:,1));

%write to txt file for jmrui
fid=fopen(outfile,'w+');
fprintf(fid,' $SEQPAR');
%fprintf(fid,'\n\nFilename: %s' ,outfile);
%fprintf(fid,'\n\nPointsInDataset: %i',length(data_struct.fids));
%fprintf(fid,'\nDatasetsInFile: %i',datsets);
%fprintf(fid,'\nSamplingInterval: %1.0E',data_struct.dwelltime*1000);
%fprintf(fid,'\nZeroOrderPhase: %1.0E',zop);
%fprintf(fid,'\nBeginTime: %1.0E',t0);
%fprintf(fid,'\nTransmitterFrequency: %2.4E',data_struct.txfrq);
%fprintf(fid,'\nMagneticField: %2.1E',Bo);
%fprintf(fid,'\nTypeOfNucleus: %1.0E',Nuc);
%fprintf(fid,'\nNameOfPatient: %s',PatName);
%fprintf(fid,'\nDateOfExperiment: %i',data_struct.date);
%fprintf(fid,'\nSpectrometer: %s',scanner);
%fprintf(fid,'\nAdditionalInfo: %s\n\n\n',addinfo);
%fprintf(fid,'Signal and FFT\n');
%fprintf(fid,'sig(real)\tsig(imag)\tfft(real)\tfft(imag)\n');
%fprintf(fid,'Signal 1 out of %i in file\n',datsets);
fprintf(fid,'\n echot= %2.2f',te);
fprintf(fid,'\n seq= ''PRESS''');
fprintf(fid,'\n hzpppm= %5.6f',in.txfrq/1e6);
fprintf(fid,'\n NumberOfPoints= %i',in.sz(1));
fprintf(fid,'\n dwellTime= %5.6f' ,dwelltime);
fprintf(fid,'\n $END');
fprintf(fid,'\n $NMID');
fprintf(fid,'\n id=''ANONYMOUS '', fmtdat=''(2E15.6)''');
fprintf(fid,'\n volume=8.0');
fprintf(fid,'\n tramp=1.0');
fprintf(fid,'\n $END\n');
fprintf(fid,'  % 7.6e  % 7.6e\n',RF');
fclose(fid);


filename = 'CantileverBeam_Triangle_Linear_Fine';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1];
constraint = {'volumeConstraint'};
constraint_case = {'EQUALITY'};
optimizer = 'DualNestedInPrimal';
optimizerUnconstrained = 'PROJECTED GRADIENT';
incrementFactor = 1;
kfrac = 1.05;
designVariable = 'Density';
filterType = 'P1';

nsteps = 2;
Vfrac_final = 0.8;
optimality_final =1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
Perimeter_target = 5;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;

% For all tests
plotting = false;
printing = false;
printing_physics = false;
monitoring = false;
maxiter = 5;


function dlap_file_print ( n, nelt, isym, irhs, isoln, ia, ja, a, rhs, ...
  soln, title )

%*****************************************************************************80
%
%% DLAP_FILE_PRINT prints a DLAP linear system that was stored in a file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Mark Seager,
%    A SLAP for the Masses,
%    Lawrence Livermore National Laboratory,
%    Technical Report UCRL-100267, December 1988.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer NELT, the number of non-zeros stored in A.
%
%    Input, integer ISYM, a flag to indicate symmetric
%    storage format.
%    * 0, all nonzero entries of the matrix are stored.
%    * 1, the matrix is symmetric, and only the lower triangle of the
%    matrix is stored.
%
%    Input, integer IRHS, is 1 if a right hand side vector
%    is included.
%
%    Input, integer ISOLN, is 1 if a solution vector is included.
%
%    Input, integer IA(NELT), integer JA(NELT),
%    real A(NELT), the DLAP triad matrix description.
%
%    Input, real RHS(N), the right hand side vector.
%
%    Input, real SOLN(N), the solution to the linear system.
%
%    Input, string TITLE, a title to be printed.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  dlap_header_print ( n, nelt, isym, irhs, isoln );
%
%  Write out the matrix.
%
  r8sp_print ( n, n, nelt, isym, ia, ja, a, '  The sparse matrix' );
%
%  Write the right hand side.
%
  if ( irhs == 1 )
    dlap_rhs_print ( n, rhs );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  No right hand side vector was supplied.\n' );
  end
%
%  Write the solution.
%
  if ( isoln == 1 )
    dlap_soln_print ( n, soln );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  No solution vector was supplied.\n' );
  end

  return
end


function fem = update10nbr(fem_struct,jj)
% update10nbr takes a node with 10 elements connected to it
%  (which defines a region) and addes 3 nodes and 6 elements to
%  that region then regroups so that no node has more than six
%  elements connected to it.  The new nodes and elements are sprung.
%
%  NOTE:  fem_struct.nei must be a component.
%
%  Variables
%  fem_struct -- finite element structure from opnml.
%  jj -- the node number that has 10 elements connected to it.
%  fem -- the updated finite element structure.
%        Note:  Only fem.x,fem.y,fem.z,fem.e, and fem.nei
%               get updated.  fem.bnd does not need to be updated.
%
%  Usage -- fem = update10nbr(fem_struct,jj)
%
%  Name: update10nbr.m
%  Written by: Ben Holladay (SEAP Student, 2004)
%  Date: June 22,2004
%  Modified:  Aug. 30, 2004 -- Chris Massey
% Last Modifed:
%         Sept. 19, 2006 -- Chris Massey, NRL Code 7322, S.S.C. MS 39529
%                      % Add test to make sure the correct nodal neighbor
%                        connectivity existed for the requested update
%                        and added a test to create the nodal neighbor list
%                        if not present.
%         April 9, 2007 -- Chris Massey, NRL Code 7322
%                     Fixed an apparent bug in the code that updates
%                     nodal neighbor list.
%         Aug. 13, 2007 -- Chris Massey, NRL Code 7322
%                     Fixed a bug in the code that updates nodal neighbor
%                     list for nodes getting extra connectivity.
%         July 14, 2008 -- Chris Massey, NRL Code 7322
%               moved test for 4 neighbors to after the test for nei.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Checks if the fem_struct has an nei. If not one is generated.
try
    nei = fem_struct.nei;
catch
    fem_struct.nei = ele2nei(fem_struct.e,fem_struct.x,fem_struct.y);
    nei = fem_struct.nei;
end

tempnc = sum(~ismember((fem_struct.nei(jj,:)),0));
if tempnc ~= 10
    error(['There are ',num2str(tempnc),' nodal neighbors for node number ',...
           num2str(jj),'. This routine only works for 10 nodal neighbors.']);
end

%Sets the intial fem_struct variables.
x = fem_struct.x;
y = fem_struct.y;
z = fem_struct.z;
enodes = fem_struct.e;
vod = [1:10,1:10];

%Adds new nodes and elements and labels the bad node, neighboring nodes,
%and neighboring elements.
[nnodes,nc] = size(nei);
nelems = size(enodes,1);
newnode = nnodes + [1:3];
newelem = nelems + [1:6];
badnode = jj;
nbrnode = nei(jj,1:10);
[nbrelem,~] = find(enodes == badnode);
inrad = max(sqrt((x(nbrnode)-x(badnode)).^2 + (y(nbrnode)-y(badnode)).^2));

%First guess of the new coordinates and bathymetry.
Bx = sum(x([(nbrnode([1:4])),badnode]))./5;
By = sum(y([(nbrnode([1:4])),badnode]))./5;
Bz = sum(z([(nbrnode([1:4])),badnode]))./5;

Dx = sum(x([(nbrnode([6:9])),badnode]))./5;
Dy = sum(y([(nbrnode([6:9])),badnode]))./5;
Dz = sum(z([(nbrnode([6:9])),badnode]))./5;

Cx = sum([(x([(nbrnode([4:6])),badnode]))',Bx,Dx])./6;
Cy = sum([(y([(nbrnode([4:6])),badnode]))',By,Dy])./6;
Cz = sum([(z([(nbrnode([4:6])),badnode]))',Bz,Dz])./6;

Ax = sum([(x([nbrnode([9,10,1])]))',Bx,Dx,Cx])./6;
Ay = sum([(y([nbrnode([9,10,1])]))',By,Dy,Cy])./6;
Az = sum([(z([nbrnode([9,10,1])]))',Bz,Dz,Cz])./6;

x(badnode) = Ax;
y(badnode) = Ay;
z(badnode) = Az;
x(newnode(1)) = Bx;
y(newnode(1)) = By;
z(newnode(1)) = Bz;
x(newnode(2)) = Cx;
y(newnode(2)) = Cy;
z(newnode(2)) = Cz;
x(newnode(3)) = Dx;
y(newnode(3)) = Dy;
z(newnode(3)) = Dz;

%Updates the effected elements. J variables are used to represent which
%elements are connected each node.
i = 1;
while i <= 8
    j1 = ismember((enodes((nbrelem),:)),(nbrnode(vod(i))));
    j2 = ismember((enodes((nbrelem),:)),(nbrnode(vod(i+1))));
    spb = [1,1,1,2,2,3,3,3];
    j = sum([j1,j2],2);
    temp = nbrelem(find (j == 2));
    if ~isempty(temp)
        enodes(temp(1),:) = ([nbrnode(vod(i)),nbrnode(vod(i+1)),newnode(spb(i))]);
    end
    i = i + 1;
    clear j j1 j2 temp;
end

%Addes the 6 new elements.
enodes(newelem(1),:) = [nbrnode(4),newnode(2),newnode(1)];
enodes(newelem(2),:) = [badnode,newnode(1),newnode(2)];
enodes(newelem(3),:) = [nbrnode(1),newnode(1),badnode];
enodes(newelem(4),:) = [nbrnode(6),newnode(3),newnode(2)];
enodes(newelem(5),:) = [badnode,newnode(2),newnode(3)];
enodes(newelem(6),:) = [nbrnode(9),badnode,newnode(3)];

%Springs the region.
M(1,:) = [Ax,Ay,Az];
M(2,:) = [Bx,By,Bz];
M(3,:) = [Cx,Cy,Cz];
M(4,:) = [Dx,Dy,Dz];

imax = 20;
stoptol = 10e-8 * inrad;
i = 1;
tol = stoptol + 10;
while i <= imax && tol > stoptol
    M2(2,1) = sum([(x([(nbrnode([1:4])),badnode]))',M(3,1)]);
    M2(2,2) = sum([(y([(nbrnode([1:4])),badnode]))',M(3,2)]);
    M2(2,3) = sum([(z([(nbrnode([1:4])),badnode]))',M(3,3)]);

    M2(4,1) = sum([(x([(nbrnode([6:9])),badnode]))',M(3,1)]);
    M2(4,2) = sum([(y([(nbrnode([6:9])),badnode]))',M(3,2)]);
    M2(4,3) = sum([(z([(nbrnode([6:9])),badnode]))',M(3,3)]);

    M2(3,1) = sum([(x([(nbrnode([4:6])),badnode]))',M(2,1),M(4,1)]);
    M2(3,2) = sum([(y([(nbrnode([4:6])),badnode]))',M(2,2),M(4,2)]);
    M2(3,3) = sum([(z([(nbrnode([4:6])),badnode]))',M(2,3),M(4,3)]);

    M2(1,1) = sum([(x([nbrnode([9,10,1])]))',M(2,1),M(3,1),M(4,1)]);
    M2(1,2) = sum([(y([nbrnode([9,10,1])]))',M(2,2),M(3,2),M(4,2)]);
    M2(1,3) = sum([(z([nbrnode([9,10,1])]))',M(2,3),M(3,3),M(4,3)]);
    M2 = M2 ./ 6;

    tol = max(sqrt((M2(:,1)-M(:,1)).^2 + (M2(:,2)-M(:,2)).^2));

    M = M2;
    x([badnode,([newnode(1:3)])]) = ([M(:,1)]);
    y([badnode,([newnode(1:3)])]) = ([M(:,2)]);
    z([badnode,([newnode(1:3)])]) = ([M(:,3)]);
    i = i+1;
end

%Creates the output structure.
fem = fem_struct;
fem.e = enodes;
fem.x = x;
fem.y = y;
fem.z = z;
fem.nei = nei;

%Update the connectivity for the bad node and adds the connections for
%the new nodes.
fem.nei(badnode,1:nc) = ([newnode([1:3]),nbrnode([9,10,1]),zeros(1,nc-6)]);
fem.nei(newnode(1),1:nc) = ([badnode,nbrnode(1:4),newnode(2),zeros(1,nc-6)]);
fem.nei(newnode(2),1:nc) = ([badnode,newnode(1),nbrnode(4:6),newnode(3),zeros(1,nc-6)]);
fem.nei(newnode(3),1:nc) = ([badnode,newnode(2),nbrnode(6:9),zeros(1,nc-6)]);

%Updates the nei for neighbor nodes. These nodes get an additional
%connector.
spb = ([1,4,6,9]);
for i = 1:4
    addconn = ([badnode,newnode([1:3]),badnode]);
    tmp = nei(nbrnode(spb(i)),:);
	ij = find(tmp == badnode);
    ik = min((find(tmp == 0)) + 1);
    if isempty(ik)  % TCM 08/13/2007 -- Added this check in case ik was empty.
        ik= size(nei,2)+1;
    end
    fem.nei(nbrnode(spb(i)),1:ik) = ([tmp(1:(ij-1)),addconn(i+1),addconn(i),tmp((ij+1):(ik-1))]);
end

%Updates the nei for neighbor nodes. These nodes get different connector.
spn = ([2,3,5,7,8]);
for i = 1:5
    addnode = ([newnode(1),newnode(1),newnode(2),newnode(3),newnode(3)]);
    tmp = nei(nbrnode(spn(i)),:);
    ij = find(tmp == badnode);
    % TCM 04/09/2007 -- Begin
    tmp2 = ([tmp(1:(ij-1)),addnode(i),tmp((ij+1):end)]);
    fem.nei(nbrnode(spn(i)),:) = 0;  %Zero out the list
    fem.nei(nbrnode(spn(i)),1:length(tmp2)) = tmp2; %Fill in the list
    %fem.nei(nbrnode(spn(i)),1:end) = ([tmp(1:(ij-1)),addnode(i),tmp((ij+1):end)]);
    % TCM 04/09/2007 -- End
end

%Determine the triqual for the final form to see if has very low quality
%elements.
tmp1 = (nbrelem);
tempL3=(fem.x(fem.e(tmp1,1))-fem.x(fem.e(tmp1,2))).^2+(fem.y(fem.e(tmp1,1))-...
   fem.y(fem.e(tmp1,2))).^2;
tempL1=(fem.x(fem.e(tmp1,2))-fem.x(fem.e(tmp1,3))).^2+(fem.y(fem.e(tmp1,2))-...
   fem.y(fem.e(tmp1,3))).^2;
tempL2=(fem.x(fem.e(tmp1,3))-fem.x(fem.e(tmp1,1))).^2+(fem.y(fem.e(tmp1,3))-...
   fem.y(fem.e(tmp1,1))).^2;
tempLs = ([tempL1 + tempL2 + tempL3]);
xnodes = fem.x(fem.e(tmp1,:));
ynodes = fem.y(fem.e(tmp1,:));
temparea = 0.5*(xnodes(:,1).*(ynodes(:,2)-ynodes(:,3))+xnodes(:,2).*...
   (ynodes(:,3)-ynodes(:,1))+xnodes(:,3).*(ynodes(:,1)-ynodes(:,2)));
fem.ar(tmp1) = temparea;
tempq = (4 * sqrt(3) * temparea) ./ tempLs;
clear tempL3 tempL2 tempL1 tempLs xnodes ynodes temparea;

fem1 = fem;
%Identify very low quality elements and attempts to use a line swap to fix.
poor = find(tempq < .1);
nflag = 1;good = 1;
if ~isempty(poor)
   for it = 1:length(poor)
      je = tmp1(poor(it));
      nodes = fem.e(je,:);
      nflag = 0;

      %Determine the angles to see if an edge can be fliped
      a2 = (x(enodes(je,3))-x(enodes(je,2))).^2+(y(enodes(je,3))-y(enodes(je,2))).^2;
      b2 = (x(enodes(je,1))-x(enodes(je,3))).^2+(y(enodes(je,1))-y(enodes(je,3))).^2;
      c2 = (x(enodes(je,2))-x(enodes(je,1))).^2+(y(enodes(je,2))-y(enodes(je,1))).^2;
      A = (180/pi)*acos((b2+c2-a2)./(2*sqrt(b2).*sqrt(c2)));
      B = (180/pi)*acos((c2+a2-b2)./(2*sqrt(c2).*sqrt(a2)));
      C = (180/pi)*acos((a2+b2-c2)./(2*sqrt(a2).*sqrt(b2)));
      [test,ind] = max([A,B,C]);
      if test > 160
         %Find the element numbers to flip the edge.
         nogood = nodes(ind);
         swap = setdiff(nodes,nogood);
         [temp1,tc] = find(enodes == swap(1) | enodes == swap(2));
         [b,j,k] = unique(temp1);
         temp2 = setdiff(1:length(temp1),j);
         swap = temp1(temp2);
         try
            fem1 = line_swap(fem,swap(1),swap(2));
         catch
            fem1 = fem;
         end

         %Spring the new mesh after the line swap.
         for itt = 1:2
             temp = find(fem1.nei(newnode(1),:) ~= 0);
             tempnei = fem1.nei(newnode(1),temp);
             fem1.x(newnode(1)) = mean(fem1.x(tempnei));
             fem1.y(newnode(1)) = mean(fem1.y(tempnei));
             fem1.z(newnode(1)) = mean(fem1.z(tempnei));
             temp = find(fem1.nei(newnode(2),:) ~= 0);
             tempnei = fem1.nei(newnode(2),temp);
             fem1.x(newnode(2)) = mean(fem1.x(tempnei));
             fem1.y(newnode(2)) = mean(fem1.y(tempnei));
             fem1.z(newnode(2)) = mean(fem1.z(tempnei));
             temp = find(fem1.nei(newnode(3),:) ~= 0);
             tempnei = fem1.nei(newnode(3),temp);
             fem1.x(newnode(3)) = mean(fem1.x(tempnei));
             fem1.y(newnode(3)) = mean(fem1.y(tempnei));
             fem1.z(newnode(3)) = mean(fem1.z(tempnei));
             temp = find(fem1.nei(badnode,:) ~= 0);
             tempnei = fem1.nei(badnode,temp);
             fem1.x(badnode) = mean(fem1.x(tempnei));
             fem1.y(badnode) = mean(fem1.y(tempnei));
             fem1.z(badnode) = mean(fem1.z(tempnei));
         end

         %Use triqual to determine if the new mesh is better quality.
         tmp1 = (nbrelem);
         tempL3=(fem1.x(fem1.e(tmp1,1))-fem1.x(fem1.e(tmp1,2))).^2+(fem1.y(fem1.e(tmp1,1))-...
            fem1.y(fem1.e(tmp1,2))).^2;
         tempL1=(fem1.x(fem1.e(tmp1,2))-fem1.x(fem1.e(tmp1,3))).^2+(fem1.y(fem1.e(tmp1,2))-...
            fem1.y(fem1.e(tmp1,3))).^2;
         tempL2=(fem1.x(fem1.e(tmp1,3))-fem1.x(fem1.e(tmp1,1))).^2+(fem1.y(fem1.e(tmp1,3))-...
            fem1.y(fem1.e(tmp1,1))).^2;
         tempLs = ([tempL1 + tempL2 + tempL3]);
         xnodes = fem1.x(fem1.e(tmp1,:));
         ynodes = fem1.y(fem1.e(tmp1,:));
         temparea = 0.5*(xnodes(:,1).*(ynodes(:,2)-ynodes(:,3))+xnodes(:,2).*...
            (ynodes(:,3)-ynodes(:,1))+xnodes(:,3).*(ynodes(:,1)-ynodes(:,2)));
         fem1.ar(tmp1) = temparea;
         tempq = (4 * sqrt(3) * temparea) ./ tempLs;
         clear tempL3 tempL2 tempL1 tempLs xnodes ynodes temparea;
         if min(tempq) > .4
            fem = fem1;
            nflag = 1;
         else
            nflag = 0;
            good = 6;
         end
      end
   end
end

% If problems with new mesh, then retriangulate

%Correct output if line swap failed to run
if sum(nflag) == 0
   fem = patch_update(fem_struct,fem1,jj);
   good = 6;
end

% Correct output if invalid elements were created.
if sum(fem.ar < 0) > 0
   fem = patch_update(fem_struct,fem1,jj);
   good = 6;
end

%Display message if invalid elements where created.
if good == 6
   disp('The nodally updated mesh contained invalid or badly conditioned');
   disp('elements, therefore the patch was retriangulated which should');
   disp('reduce the connecitivity but is not guaranteed to do so.');
end
if sum(fem.ar < 0) > 0
   disp(' ');
   disp('returning original mesh');
   fem = fem_struct;
end

return


function [params, result] = ortho_default(params, W, w)
%  DSS No orthogonalization function
%  Does mere normalisation
%   W = orthof(W)     for symmetric dss
%   w = orthof(W, w)  for deflation dss
%     W Matrix with projection vectors as rows. For deflation
%       algorithm only previously calculated projections are given.
%     w Currently iterated projection. Only for deflation algorithm.

% Copyright (C) 2004, 2005 DSS MATLAB package team (dss@cis.hut.fi).
% Licensed under the Creative Commons Attribution-NonCommercial-ShareAlike
% License. http://creativecommons.org/licenses/by-nc-sa/2.0/.
% Distributed by Laboratory of Computer and Information Science,
% Helsinki University of Technology. http://www.cis.hut.fi/projects/dss/.
% $Id$

if nargin<2
    params.name = 'No orthogonalization';
    params.description = 'Description of this function.';
    return;
end

disp('no orthogonalisation!');
if nargin>2
  % per component orthogonalization
  result = w / norm(w);
else
  % symmetric orthogonalization
  result = diag(1./ sqrt(diag(W * W'))) * W;
end


% demo for creating an SPM M/EEG dataset from arbitrary data using
% conversion of simple Fieldtrip raw data struct.
% SPM8 internal format is quite complex but is transparent to the user
% as meeg object's methods take care of maintaining its consistency.
% The most straightforward way to convert arbitrary data that is available
% as an ASCII file or *.mat file with some variables to SPM8 is to create
% a quite simple Fieldtrip raw data struct and then use SPM's
% spm_eeg_ft2spm to convert this struct to SPM8 file. Missing information
% can then be supplemented using meeg methods and SPM functions.
% Fieldtrip raw struct must contain the following fields:
% .fsample - sampling rate (Hz)
% .trial - cell array of matrices with identical dimensions channels x time
% .time - cell array of time vectors (in sec), the same length as the
%         second dimension of the data. For SPM8 they must be identical.
% .label - cell array of strings, list of channel labels. Same length as
%         the first dimension of the data.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston, Vladimir Litvak
% $Id: spm_lfp_txt2mat.m 2255 2008-09-30 15:36:59Z vladimir $

% load data (in this case a .mat file)
%--------------------------------------------------------------------------
load amy_hc_0501_R1

% define the output file name
%--------------------------------------------------------------------------
fname = 'LFP_example';

% define the sampling rate
%--------------------------------------------------------------------------
fsample = 1000;

% define epochs and create data array - this is specific for this data
% replace with your own code
%--------------------------------------------------------------------------
bins  = find(diff(data(:,3)) > 4);
bins  = bins([3,9]);
nbins = 1000*10;

% Define the channels of interest - in this case only 3,4 and 5
%--------------------------------------------------------------------------
Ic = [3 4 5];

% Create the Fieldtrip raw struct
%--------------------------------------------------------------------------
ftdata = [];

for i = 1:length(bins)
   It            = [1:nbins] + bins(i);
   ftdata.trial{i} = data(It, Ic)';
   ftdata.time{i} = [0:(nbins-1)]./fsample;
end

ftdata.fsample = fsample;
ftdata.label = colheaders(Ic);
ftdata.label = ftdata.label(:);

% Convert the ftdata struct to SPM M\EEG dataset
%--------------------------------------------------------------------------
D = spm_eeg_ft2spm(ftdata, fname);

% Examples of providing additional information in a script
% [] comes instead of an index vector and means that the command
% applies to all channels/all trials.
%--------------------------------------------------------------------------
D = type(D, 'single');             % Sets the dataset type
D = chantype(D, [], 'LFP');        % Sets the channel type
D = conditions(D, [], 'Sound 1');  % Sets the condition label

% save
%--------------------------------------------------------------------------
save(D);


function [gcc, maskGCC] = F_comp_gcc(input_layer, curr_layer)
input = input_layer.a;
[nCh,T,N] = size(input);
frame_len = curr_layer.frame_len;
frame_shift = curr_layer.frame_shift;
overlap = 1 - frame_shift/frame_len;
nFr = enframe_decide_frame_number(T, frame_len, frame_shift, 1-overlap/2);   % get the maximum number of frames of the sentences. It is
        % the same computation as in my_enframe.m
useGPU = IsInGPU(input);
precision = class(gather(input(1)));

gcc_dim = curr_layer.dim(1) / (nCh*(nCh-1)/2);
gcc_bin_range = (gcc_dim-1)/2;

if N==1     % if we have only one sentence in a minibatch
    [gcc]=getCorrelationVector_fast2(input, frame_len, overlap, useGPU);
    gcc = gcc(frame_len/2-gcc_bin_range:frame_len/2+gcc_bin_range,:,:);
    if nCh>2
        gcc = permute(gcc, [1,3,2]);
        [d1,d2,d3] = size(gcc);
        gcc = reshape(gcc, d1*d2, d3);
    end
    maskGCC = [];
else    % if we have multiple sentences in a minibatch
    [mask, variableLength] = getValidFrameMask(input_layer);
    input2 = PadShortTrajectory(input, mask, 0);

    % baseline
    if nCh>2    % loop over the sentences directly if too many channels
        if useGPU
            gcc = gpuArray.zeros(gcc_dim, nFr, nCh*(nCh-1)/2,N);
        else
            gcc = zeros(gcc_dim, nFr, nCh*(nCh-1)/2,N);
        end
        % add or remove samples to avoid partial samples to make enframe
        % faster
        nSampleRequired = nFr * frame_shift + frame_len-frame_shift;
        if nSampleRequired > T
            input2(:,nSampleRequired,:) = 0;
        end
        for i=1:N
            tmpGCC = getCorrelationVector_fast2(input2(:,:,i), frame_len, overlap, useGPU);
            gcc(:,:,:,i) = tmpGCC(frame_len/2-gcc_bin_range:frame_len/2+gcc_bin_range,:,:);
        end
        if nCh>2
            gcc = permute(gcc, [1,3,2,4]);
            [d1,d2,d3,d4] = size(gcc);
            gcc = reshape(gcc, d1*d2, d3,d4);
        else
            gcc = squeeze(gcc);
        end

    else        % put input into a big wavfile and call GCC function only once to save time.
        % just concatenate the sentences including the invalid samples
        % first discard the extra samples that will be discarded by enframe
        nSampleRequired = nFr * frame_shift + frame_len-frame_shift;
        if T<nSampleRequired
            input2(:,nSampleRequired,:) = 0;
        elseif T> nSampleRequired
            input2(:,nSampleRequired+1:end,:) = [];
        end
        % second, we need to append some zeros such that the first samples
        % of the sentences are guaranteed to be the first sample of a
        % frame.
        residual = mod(frame_len/frame_shift,1);
        if residual>0
            nSampleToAppend = (1-residual)*frame_shift; % need to make the residual equal to the frame_shift
            input2(:,end+nSampleToAppend,:) = 0;
        end
        nFrActual = nFr + ceil(frame_len/frame_shift)-1;

        % third, reshape the tensor into a matrix
        input3 = reshape(input2, nCh, size(input2,2)*N);

        [gcc]=getCorrelationVector_fast2(input3, frame_len, overlap, useGPU);
        gcc = gcc(frame_len/2-gcc_bin_range:frame_len/2+gcc_bin_range,:,:);
        if size(gcc,2)<nFrActual*N
            gcc(:,nFrActual*N,:) = 0;
        end

        if nCh>2
            gcc = permute(gcc, [1,3,2]);
            [d1,d2,d3] = size(gcc);
            gcc = reshape(gcc, d1*d2, d3);
        end

        gcc = reshape(gcc, size(gcc,1), nFrActual, N);
        gcc(:,nFr+1:end,:) = [];    % remove extra frames
%     else    % concatenate the valid samples of the sentences without some buffer
        % to be implemented
    end

    % now build a mask for gcc
    nSampleChannel = gather(sum(mask==0));
    if useGPU
        maskGCC = gpuArray.zeros(nFr, N, precision);
    else
        maskGCC = zeros(nFr, N, precision);
    end
    for i=1:N
        nFrChannel = enframe_decide_frame_number(nSampleChannel(i), frame_len, frame_shift, 1-overlap/2);
        maskGCC(nFrChannel+1:end,i) = 1;
    end
    %gcc = PadShortTrajectory(gcc, maskGCC, -1e10);
end

end


function [EI,Pth,Qthth] = lme_mass_RgEI1(X,Zcols,W,CBhat,L,phi,ni,G,GDa,GDb)
% [EI,Pth,Qthth] = lme_mass_RgEI1(X,Zcols,W,CBhat,L,phi,ni,G,GDa,GDb)
%
% Expected information matrix of the restricted log-likelihood of a whole
% region. This is less computationally efficient than lme_mass_RgEI
% but more numerically stable.
%
% Input
% X: Ordered design Matrix (according to time for each subject).
% Zcols: Vector with the indices of the colums of X that will be considered
% as random effects.
% W: Inverses of the estimated temporal covariance matrices for each
% subject stacked in W.
% CBhat: Asymptotic covariance matrix of the fixed effects.
% L: Cholesky factor of the covariance matrix of the random effects (D).
% phi: Within-subject standard deviation of the errors.
% ni: Vector whose entries are the number of repeated measures for each
% subject in the study (ordered according to X).
% G: Spatial covariance matrix.
% GDa: Derivative of the spatial covariance matrix for the first spatial
% parameter.
% GDb: Derivative of the spatial covariance matrix for the second spatial
% parameter (empty for spatial models with a single parameter).
%
% Output
% EI: Expected information matrix.
% Pth,Qthth: Matrices that are useful for inferences on the fixed effects.
%
% $Revision: 1.2 $  $Date: 2015/01/06 17:14:55 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:14:55 $
%    $Revision: 1.2 $
% References: Bernal-Rusiel J.L., Greve D.N., Reuter M., Fischl B., Sabuncu
% M.R., 2012. Statistical Analysis of Longitudinal Neuroimage Data with Linear
% Mixed Effects Models, NeuroImage, doi:10.1016/j.neuroimage.2012.10.065.
%
m = length(ni);
n = sum(ni);
nv = size(G,1);
q = length(Zcols);
nth = q*(q+1)/2+1;
p = size(X,2);
Pth = zeros(nth,p,p);
Qthth = zeros(nth,nth,p,p);
Der = zeros(nth,n,max(ni));
%Computation of the first order derivatives of the temporal covariance matrix
jk = 0;
for k=1:q
    for j=1:k
        jk = jk + 1;
        posi = 1;
        for i=1:m
            posf = posi+ni(i)-1;
            Zi = X(posi:posf, Zcols);
            Zki = Zi(:,k);
            Mjki = Zki*L(j,:)*Zi';
            Mjki = Mjki + Mjki';
            Der(jk,posi:posf,1:ni(i)) = Mjki;
            posi = posf+1;
        end;
    end;
end;
posi = 1;
for i=1:m
    posf = posi+ni(i)-1;
    Der(nth,posi:posf,1:ni(i)) = 2*phi*eye(ni(i));
    posi = posf+1;
end;
%Computation of Pis,Qijs and the expected information matrix EI.
for j=1:nth
    posi = 1; Pj = 0;
    Bj = squeeze(Der(j,:,:));
    for i=1:m
        posf = posi+ni(i)-1;
        Wi = W(posi:posf,1:ni(i));
        Pj = Pj - X(posi:posf,:)'*Wi*Bj(posi:posf,1:ni(i))*Wi*X(posi:posf,:);
        posi = posf+1;
    end;
    Pth(j,:,:) = Pj;
end;
EI = zeros(nth+2,nth+2);
%Expected information among Lijs (including phi)
for k=1:nth
    Bk = squeeze(Der(k,:,:));
    Pk = squeeze(Pth(k,:,:));
    for j=1:k
        Bj = squeeze(Der(j,:,:));
        Pj = squeeze(Pth(j,:,:));
        posi = 1; Qkj = 0;
        traceBkj = 0;
        for i=1:m
            posf = posi+ni(i)-1;
            Wi = W(posi:posf,1:ni(i));
            Bkji = Wi*Bk(posi:posf,1:ni(i))*Wi*Bj(posi:posf,1:ni(i));
            traceBkj = traceBkj + trace(Bkji);
            Bkji = Bkji*Wi;
            Qkji = X(posi:posf,:)'*Bkji*X(posi:posf,:);
            Qkj = Qkj + Qkji;
            posi = posf+1;
        end;
        Qthth(k,j,:,:) = Qkj;
        Qthth(j,k,:,:) = Qkj;
        EI(k,j) = nv*(traceBkj - trace(CBhat*(2*Qkj-Pk*CBhat*Pj)));
        EI(j,k) = EI(k,j);
    end;
end;
%Expected information between Lijs (including phi) and a (first spatial parameter)
Mauxa = G\GDa;
trMauxa = trace(Mauxa);
for k=1:nth
    Bk = squeeze(Der(k,:,:));
    Pk = squeeze(Pth(k,:,:));
    posi = 1;
    traceBk = 0;
    for i=1:m
        posf = posi+ni(i)-1;
        Wi = W(posi:posf,1:ni(i));
        Bki = Wi*Bk(posi:posf,1:ni(i));
        traceBk = traceBk + trace(Bki);
        posi = posf+1;
    end;
    EI(k,nth+1) = trMauxa*(traceBk + trace(CBhat*Pk));
    EI(nth+1,k) = EI(k,nth+1);
end
%Expected information between a and a
EI(nth+1,nth+1) = (n-p)*trace(Mauxa*Mauxa);
if ~isempty(GDb)
    %Expected information between Lijs (including phi) and b (second spatial parameter)
    Mauxb = G\GDb;
    trMauxb = trace(Mauxb);
    for k=1:nth
        Bk = squeeze(Der(k,:,:));
        Pk = squeeze(Pth(k,:,:));
        posi = 1;
        traceBk = 0;
        for i=1:m
            posf = posi+ni(i)-1;
            Wi = W(posi:posf,1:ni(i));
            Bki = Wi*Bk(posi:posf,1:ni(i));
            traceBk = traceBk + trace(Bki);
            posi = posf+1;
        end;
        EI(k,nth+2) = trMauxb*(traceBk + trace(CBhat*Pk));
        EI(nth+2,k) = EI(k,nth+2);
    end
    %Expected information between b and b
    EI(nth+2,nth+2) = (n-p)*trace(Mauxb*Mauxb);
    %Expected information between a and b
    EI(nth+1,nth+2) = (n-p)*trace(Mauxa*Mauxb);
    EI(nth+2,nth+1) = EI(nth+1,nth+2);
else
    EI = EI(1:nth+1,1:nth+1);
end;
EI = 0.5*EI;


filename='Gripping_quad_fine';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'nonadjoint_compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
constraint_case = 'INEQUALITY';
optimizer = 'MMA'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.75;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function dB0=MagUser(p)
%create dB0 field from data file

global VMmg
global VObj

load(p.MagFile); % load dB0 file
Interp=p.Interp;

% Initialize display grid
xgrid=VMmg.xgrid;
ygrid=VMmg.ygrid;
zgrid=VMmg.zgrid;

Mxdims=size(VObj.Rho);
max_xgrid=((Mxdims(2)-1)/2)*VObj.XDimRes;
max_ygrid=((Mxdims(1)-1)/2)*VObj.YDimRes;
max_zgrid=((Mxdims(3)-1)/2)*VObj.ZDimRes;
min_xgrid=(-(Mxdims(2)-1)/2)*VObj.XDimRes;
min_ygrid=(-(Mxdims(1)-1)/2)*VObj.YDimRes;
min_zgrid=(-(Mxdims(3)-1)/2)*VObj.ZDimRes;

[row,col,layer] = size(dB0);

[Magx,Magy,Magz]=meshgrid(linspace(min_xgrid, max_xgrid, col),...
                          linspace(min_ygrid, max_ygrid, row),...
                          linspace(min_zgrid, max_zgrid, layer));

dB0=ba_interp3(Magx,Magy,Magz,dB0,xgrid,ygrid,zgrid,Interp);
dB0(isinf(dB0)) = 0;

end


function pos2 = propmo (tjd1, pos1, vel1, tjd2)

% this function applies proper motion, including foreshortening
% effects, to a star's position.

% input

%  tjd1 = tdb julian date of first epoch

%  pos1 = position vector of star at first epoch

%  vel1 = velocity vector of star at first epoch

%  tjd2 = tdb julian date of second epoch

% output

%  pos2 = position vector of star at second epoch

% ported from NOVAS 3.0

%%%%%%%%%%%%%%%%%%%%%%%

for j = 1:1:3

    pos2(j) = pos1(j) + vel1(j) * (tjd2 - tjd1);

end


function U = som_umat(sMap, varargin)

%SOM_UMAT Compute unified distance matrix of self-organizing map.
%
% U = som_umat(sMap, [argID, value, ...])
%
%  U = som_umat(sMap);
%  U = som_umat(M,sTopol,'median','mask',[1 1 0 1]);
%
%  Input and output arguments ([]'s are optional):
%   sMap     (struct) map struct or
%            (matrix) the codebook matrix of the map
%   [argID,  (string) See below. The values which are unambiguous can
%    value]  (varies) be given without the preceeding argID.
%
%   U        (matrix) u-matrix of the self-organizing map
%
% Here are the valid argument IDs and corresponding values. The values which
% are unambiguous (marked with '*') can be given without the preceeding argID.
%   'mask'       (vector) size dim x 1, weighting factors for different
%                         components (same as BMU search mask)
%   'msize'      (vector) map grid size
%   'topol'     *(struct) topology struct
%   'som_topol','sTopol' = 'topol'
%   'lattice'   *(string) map lattice, 'hexa' or 'rect'
%   'mode'      *(string) 'min','mean','median','max', default is 'median'
%
% NOTE! the U-matrix is always calculated for 'sheet'-shaped map and
% the map grid must be at most 2-dimensional.
%
% For more help, try 'type som_umat' or check out online documentation.
% See also SOM_SHOW, SOM_CPLANE.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% som_umat
%
% PURPOSE
%
% Computes the unified distance matrix of a SOM.
%
% SYNTAX
%
%  U = som_umat(sM)
%  U = som_umat(...,'argID',value,...)
%  U = som_umat(...,value,...)
%
% DESCRIPTION
%
% Compute and return the unified distance matrix of a SOM.
% For example a case of 5x1 -sized map:
%            m(1) m(2) m(3) m(4) m(5)
% where m(i) denotes one map unit. The u-matrix is a 9x1 vector:
%    u(1) u(1,2) u(2) u(2,3) u(3) u(3,4) u(4) u(4,5) u(5)
% where u(i,j) is the distance between map units m(i) and m(j)
% and u(k) is the mean (or minimum, maximum or median) of the
% surrounding values, e.g. u(3) = (u(2,3) + u(3,4))/2.
%
% Note that the u-matrix is always calculated for 'sheet'-shaped map and
% the map grid must be at most 2-dimensional.
%
% REFERENCES
%
% Ultsch, A., Siemon, H.P., "Kohonen's Self-Organizing Feature Maps
%   for Exploratory Data Analysis", in Proc. of INNC'90,
%   International Neural Network Conference, Dordrecht,
%   Netherlands, 1990, pp. 305-308.
% Kohonen, T., "Self-Organizing Map", 2nd ed., Springer-Verlag,
%    Berlin, 1995, pp. 117-119.
% Iivarinen, J., Kohonen, T., Kangas, J., Kaski, S., "Visualizing
%   the Clusters on the Self-Organizing Map", in proceedings of
%   Conference on Artificial Intelligence Research in Finland,
%   Helsinki, Finland, 1994, pp. 122-126.
% Kraaijveld, M.A., Mao, J., Jain, A.K., "A Nonlinear Projection
%   Method Based on Kohonen's Topology Preserving Maps", IEEE
%   Transactions on Neural Networks, vol. 6, no. 3, 1995, pp. 548-559.
%
% REQUIRED INPUT ARGUMENTS
%
%  sM (struct) SOM Toolbox struct or the codebook matrix of the map.
%     (matrix) The matrix may be 3-dimensional in which case the first
%              two dimensions are taken for the map grid dimensions (msize).
%
% OPTIONAL INPUT ARGUMENTS
%
%  argID (string) Argument identifier string (see below).
%  value (varies) Value for the argument (see below).
%
%  The optional arguments are given as 'argID',value -pairs. If the
%  value is unambiguous, it can be given without the preceeding argID.
%  If an argument is given value multiple times, the last one is used.
%
%  Below is the list of valid arguments:
%   'mask'      (vector) mask to be used in calculating
%                        the interunit distances, size [dim  1]. Default is
%                        the one in sM (field sM.mask) or a vector of
%                        ones if only a codebook matrix was given.
%   'topol'     (struct) topology of the map. Default is the one
%                        in sM (field sM.topol).
%   'sTopol','som_topol' (struct) = 'topol'
%   'msize'     (vector) map grid dimensions
%   'lattice'   (string) map lattice 'rect' or 'hexa'
%   'mode'      (string) 'min', 'mean', 'median' or 'max'
%                        Map unit value computation method. In fact,
%                        eval-function is used to evaluate this, so
%                        you can give other computation methods as well.
%                        Default is 'median'.
%
% OUTPUT ARGUMENTS
%
%  U   (matrix) the unified distance matrix of the SOM
%               size 2*n1-1 x 2*n2-1, where n1 = msize(1) and n2 = msize(2)
%
% EXAMPLES
%
%  U = som_umat(sM);
%  U = som_umat(sM.codebook,sM.topol,'median','mask',[1 1 0 1]);
%  U = som_umat(rand(10,10,4),'hexa','rect');
%
% SEE ALSO
%
%  som_show    show the selected component planes and the u-matrix
%  som_cplane  draw a 2D unified distance matrix

% Copyright (c) 1997-2000 by the SOM toolbox programming team.
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 1.0beta juuso 260997
% Version 2.0beta juuso 151199, 151299, 200900

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% check arguments

error(nargchk(1, Inf, nargin));  % check no. of input arguments is correct

% sMap
if isstruct(sMap),
  M = sMap.codebook;
  sTopol = sMap.topol;
  mask = sMap.mask;
elseif isnumeric(sMap),
  M = sMap;
  si = size(M);
  dim = si(end);
  if length(si)>2, msize = si(1:end-1);
  else msize = [si(1) 1];
  end
  munits = prod(msize);
  sTopol = som_set('som_topol','msize',msize,'lattice','rect','shape','sheet');
  mask = ones(dim,1);
  M = reshape(M,[munits,dim]);
end
mode = 'median';

% varargin
i=1;
while i<=length(varargin),
  argok = 1;
  if ischar(varargin{i}),
    switch varargin{i},
      % argument IDs
     case 'mask',       i=i+1; mask = varargin{i};
     case 'msize',      i=i+1; sTopol.msize = varargin{i};
     case 'lattice',    i=i+1; sTopol.lattice = varargin{i};
     case {'topol','som_topol','sTopol'}, i=i+1; sTopol = varargin{i};
     case 'mode',       i=i+1; mode = varargin{i};
      % unambiguous values
     case {'hexa','rect'}, sTopol.lattice = varargin{i};
     case {'min','mean','median','max'}, mode = varargin{i};
     otherwise argok=0;
    end
  elseif isstruct(varargin{i}) && isfield(varargin{i},'type'),
    switch varargin{i}(1).type,
     case 'som_topol', sTopol = varargin{i};
     case 'som_map',   sTopol = varargin{i}.topol;
     otherwise argok=0;
    end
  else
    argok = 0;
  end
  if ~argok,
    disp(['(som_umat) Ignoring invalid argument #' num2str(i+1)]);
  end
  i = i+1;
end

% check
[munits dim] = size(M);
if prod(sTopol.msize)~=munits,
  error('Map grid size does not match the number of map units.')
end
if length(sTopol.msize)>2,
  error('Can only handle 1- and 2-dimensional map grids.')
end
if prod(sTopol.msize)==1,
  warning('Only one codebook vector.'); U = []; return;
end
if ~strcmp(sTopol.shape,'sheet'),
  disp(['The ' sTopol.shape ' shape of the map ignored. Using sheet instead.']);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% initialize variables

y = sTopol.msize(1);
x = sTopol.msize(2);
lattice = sTopol.lattice;
shape = sTopol.shape;
M = reshape(M,[y x dim]);

ux = 2 * x - 1;
uy = 2 * y - 1;
U  = zeros(uy, ux);

calc = sprintf('%s(a)',mode);

if size(mask,2)>1, mask = mask'; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% u-matrix computation

% distances between map units

if strcmp(lattice, 'rect'), % rectangular lattice

  for j=1:y, for i=1:x,
      if i<x,
	dx = (M(j,i,:) - M(j,i+1,:)).^2; % horizontal
	U(2*j-1,2*i) = sqrt(mask'*dx(:));
      end
      if j<y,
	dy = (M(j,i,:) - M(j+1,i,:)).^2; % vertical
	U(2*j,2*i-1) = sqrt(mask'*dy(:));
      end
      if j<y && i<x,
	dz1 = (M(j,i,:) - M(j+1,i+1,:)).^2; % diagonals
	dz2 = (M(j+1,i,:) - M(j,i+1,:)).^2;
	U(2*j,2*i) = (sqrt(mask'*dz1(:))+sqrt(mask'*dz2(:)))/(2 * sqrt(2));
      end
    end
  end

elseif strcmp(lattice, 'hexa') % hexagonal lattice

  for j=1:y,
    for i=1:x,
      if i<x,
	dx = (M(j,i,:) - M(j,i+1,:)).^2; % horizontal
	U(2*j-1,2*i) = sqrt(mask'*dx(:));
      end

      if j<y, % diagonals
	dy = (M(j,i,:) - M(j+1,i,:)).^2;
	U(2*j,2*i-1) = sqrt(mask'*dy(:));

	if rem(j,2)==0 && i<x,
	  dz= (M(j,i,:) - M(j+1,i+1,:)).^2;
	  U(2*j,2*i) = sqrt(mask'*dz(:));
	elseif rem(j,2)==1 && i>1,
	  dz = (M(j,i,:) - M(j+1,i-1,:)).^2;
	  U(2*j,2*i-2) = sqrt(mask'*dz(:));
	end
      end
    end
  end

end

% values on the units

if (uy == 1 || ux == 1),
  % in 1-D case, mean is equal to median

  ma = max([ux uy]);
  for i = 1:2:ma,
    if i>1 && i<ma,
      a = [U(i-1) U(i+1)];
      U(i) = eval(calc);
    elseif i==1, U(i) = U(i+1);
    else U(i) = U(i-1); % i==ma
    end
  end

elseif strcmp(lattice, 'rect')

  for j=1:2:uy,
    for i=1:2:ux,
      if i>1 && j>1 && i<ux && j<uy,    % middle part of the map
	a = [U(j,i-1) U(j,i+1) U(j-1,i) U(j+1,i)];
      elseif j==1 && i>1 && i<ux,        % upper edge
	a = [U(j,i-1) U(j,i+1) U(j+1,i)];
      elseif j==uy && i>1 && i<ux,       % lower edge
	a = [U(j,i-1) U(j,i+1) U(j-1,i)];
      elseif i==1 && j>1 && j<uy,        % left edge
	a = [U(j,i+1) U(j-1,i) U(j+1,i)];
      elseif i==ux && j>1 && j<uy,       % right edge
	a = [U(j,i-1) U(j-1,i) U(j+1,i)];
      elseif i==1 && j==1,              % top left corner
	a = [U(j,i+1) U(j+1,i)];
      elseif i==ux && j==1,             % top right corner
	a = [U(j,i-1) U(j+1,i)];
      elseif i==1 && j==uy,             % bottom left corner
	a = [U(j,i+1) U(j-1,i)];
      elseif i==ux && j==uy,            % bottom right corner
	a = [U(j,i-1) U(j-1,i)];
      else
	a = 0;
      end
      U(j,i) = eval(calc);
    end
  end

elseif strcmp(lattice, 'hexa')

  for j=1:2:uy,
    for i=1:2:ux,
      if i>1 && j>1 && i<ux && j<uy,      % middle part of the map
	a = [U(j,i-1) U(j,i+1)];
	if rem(j-1,4)==0, a = [a, U(j-1,i-1) U(j-1,i) U(j+1,i-1) U(j+1,i)];
	else a = [a, U(j-1,i) U(j-1,i+1) U(j+1,i) U(j+1,i+1)]; end
      elseif j==1 && i>1 && i<ux,        % upper edge
	a = [U(j,i-1) U(j,i+1) U(j+1,i-1) U(j+1,i)];
      elseif j==uy && i>1 && i<ux,       % lower edge
	a = [U(j,i-1) U(j,i+1)];
	if rem(j-1,4)==0, a = [a, U(j-1,i-1) U(j-1,i)];
	else a = [a, U(j-1,i) U(j-1,i+1)]; end
      elseif i==1 && j>1 && j<uy,        % left edge
	a = U(j,i+1);
	if rem(j-1,4)==0, a = [a, U(j-1,i) U(j+1,i)];
	else a = [a, U(j-1,i) U(j-1,i+1) U(j+1,i) U(j+1,i+1)]; end
      elseif i==ux && j>1 && j<uy,       % right edge
	a = U(j,i-1);
	if rem(j-1,4)==0, a=[a, U(j-1,i) U(j-1,i-1) U(j+1,i) U(j+1,i-1)];
	else a = [a, U(j-1,i) U(j+1,i)]; end
      elseif i==1 && j==1,              % top left corner
	a = [U(j,i+1) U(j+1,i)];
      elseif i==ux && j==1,             % top right corner
	a = [U(j,i-1) U(j+1,i-1) U(j+1,i)];
      elseif i==1 && j==uy,             % bottom left corner
	if rem(j-1,4)==0, a = [U(j,i+1) U(j-1,i)];
	else a = [U(j,i+1) U(j-1,i) U(j-1,i+1)]; end
      elseif i==ux && j==uy,            % bottom right corner
	if rem(j-1,4)==0, a = [U(j,i-1) U(j-1,i) U(j-1,i-1)];
	else a = [U(j,i-1) U(j-1,i)]; end
      else
	a=0;
      end
      U(j,i) = eval(calc);
    end
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% normalization between [0,1]

% U = U - min(min(U));
% ma = max(max(U)); if ma > 0, U = U / ma; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


Dataflow {
    TemporalMap(16,16) K;
    SpatialMap(Sz(R),1) Y;
    TemporalMap(Sz(S),1) X;
    TemporalMap(1,1) C;
    Cluster(16, P);
    SpatialMap(1,1) K;
    TemporalMap(Sz(R),1) Y;
    TemporalMap(Sz(S),1) X;
    TemporalMap(Sz(R),7) R;
    TemporalMap(Sz(S),7) S;
}


classdef TestConvertScaleAbs
    %TestConvertScaleAbs

    methods (Static)
        function test_1
            A = rand([10 10], 'single')*200 - 100;
            B = cv.convertScaleAbs(A, 'Alpha',5, 'Beta',3);
            validateattributes(B, {'uint8'}, {'size',size(A)});
        end

        function test_error_argnum
            try
                cv.convertScaleAbs();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = WorstOfCall(S1,S2)
    nsim = size(S1,1);
    nt = size(S1,2);
    A = [(S1(:,nt)-S1(:,1))./S1(:,1) (S2(:,nt)-S2(:,1))./S2(:,1)];
    val = [min(A,[],2) zeros(nsim,1)];
    y = mean(max(val,[],2));
end


function varargout = chopdeal(data,sizes)
% Chop a vector into multiple vectors of varying sizes.
% Outputs... = chopdeal(Data,Sizes)
%
% This function operates simiarly to mat2cell, except that it accepts only vectors and returns
% the results as multiple outputs rather than in single cell array. This makes the function
% equivalent to celldeal(mat2cell(Data,1,Sizes)).
%
% In:
%   Data : a vector to chop (single, double, or cell array).
%
%   Sizes : a double vector of sizes for the chop outputs;
%           the sizes must sum to the length of the data.
%
% Out:
%   Outputs... : one output for each size, each of size [1xSizes(k)].
%
%                                Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                                2010-04-01

% the below implementation is the MATLAB fallback for the mex function of same name.
if nargout ~= length(sizes)
    error('The number of output arguments must match the number of elements in sizes.'); end
varargout = cell(1,length(sizes));
p=0;
for s=1:length(sizes)
    varargout{s} = data(p+(1:sizes(s)));
    p = p+sizes(s);
end


function [infostring warnstring errstring] = est_checkMVARParams(EEG, params)
%
% This function performs a series of sanity checks on the parameters chosen
% for fitting a VAR model. The results are returned in cell vectors
% indicating information bulletins (notifications), warnings, and/or
% errors.
%
% Inputs:
%
%   EEG:            EEG data structure
%   params:         a parameter structure as output from est_fitMVAR
%
% Outputs:
%
%   infostring:     cell array of information bulletins (one per cell)
%   warnstring:     cell array of warning bulletins (one per cell)
%   errstring:      cell array of error bulletins (one per cell)
%
% References:
%
% [1] Korzeniewska, et al (2008). Dynamics of Event-Related Causality in
%     Brain Electrical Activity. Human brain mapping 29:1170?1192
% [2] Schlogl and Supp, (2006). Analyzing event-related EEG data with
%     multivariate AR parameters. Prog. in Brain Researc. Neuper and Klimesh, Eds.
% [3] Mullen T (2010) The Source Information Flow Toolbox (SIFT):
%     Theoretical Handbook and User Manual.
%     Available at: http://www.sccn.ucsd.edu/wiki/Sift
%
% See Also: pop_est_fitMVAR()
%
% Author: Tim Mullen 2010, SCCN/INC, UCSD
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

srate = EEG.srate;
wlen = srate*params.winlen;  % winlen in samples
T = EEG.CAT.trials;
p = params.morder;
M = EEG.CAT.nbchan;

% addtional error checks and info
infostring = {};
warnstring = {};
errstring  = {};

% critical error checks
if params.winstep==0
    error('Step size must be greater than 0\n');
end

if floor(params.winlen*1000) > floor((EEG.xmax-EEG.xmin+1)*1000)
    error('Winlen cannot be greater than the trial length (%0.1f sec)\n',EEG.xmax-EEG.xmin);
end

if round(params.winlen*EEG.srate) <= max(params.morder)
    error('The window length must be greater than the model order. Increase your window to at least %0.2f sec\n',(params.morder(end)+1)/EEG.srate);
end

if length(p)==2
    infostring = [infostring sprintf('Two model orders specified [%d %d]\n',p(1),p(2))];
    warnstring = [warnstring sprintf('\tI assume you are providing a [min max] range for model order selection.\n\tI will use p=(%d) for the remaining checks...\n',p(2))];
    errstring  = [errstring {''}];
    p = p(2);
end

% Check if we have a reasonable ratio of datapoints to free parameters
% Optimal ratio derived from [1]
winrat = ((M^2)*p)/(wlen*T);
infostring = [infostring sprintf('Ratio of number of parameters to datapoints is %0.3f.\n',winrat)];
if winrat > 1
    reclen = ((M^2)*p/T)/srate;
    warnstring = [warnstring sprintf('\tIf using an unconstrained (unregularized) model fitting apprach: The ratio of number of parameters to datapoints must be <= 1.\n\tYour window length must be at least %0.3f sec\n',reclen)];
    errstring  = [errstring {''}];
elseif winrat > 0.1
    reclen = (10*(M^2)*p/T)/srate;
    warnstring = [warnstring sprintf('\tIf using an unconstrained (unregularized) model fitting apprach: For best results, ratio of number of parameters to datapoints should be < 0.1.\n\tI recommend using window length of at least %0.3f sec\n',reclen)];
    errstring  = [errstring {''}];
else
    warnstring = [warnstring {''}];
    errstring  = [errstring {''}];
end

% check if time-frequency principle is violated [2]
TFprod = wlen*sqrt(T);
infostring = [infostring sprintf('Time-Frequency Product is %0.3f. This should be greater than p=%d\n',TFprod,p)];
if wlen<=p/sqrt(T)
    reclen = p/sqrt(T);
    warnstring = [warnstring sprintf('\tTime window does not satisfy the Time-Frequency Uncertainty Principle.\n\tI recommend a window length of at least %0.3f sec\n', reclen)];
    errstring  = [errstring {''}];
else
    warnstring = [warnstring {''}];
    errstring  = [errstring {''}];
end

% notify user of max number of spectral peaks
infostring = [infostring sprintf('Given your model order of p=%d, a maximum of p/2=%0.1f frequency components (spectral peaks) can be estimated for each pair of variables\n',p, p/2)];
warnstring = [warnstring {''}];
errstring  = [errstring {''}];


function idx = argmax(x)

    [~,idx] = max(x);


function [Eft, Varft, lpyt, Eyt, Varyt] = gpep_pred(gp, x, y, varargin)
%GPEP_PRED  Predictions with Gaussian Process EP approximation
%
%  Description
%    [EFT, VARFT] = GPEP_PRED(GP, X, Y, XT, OPTIONS)
%    takes a GP structure together with matrix X of training
%    inputs and vector Y of training targets, and evaluates the
%    predictive distribution at test inputs XT. Returns a posterior
%    mean EFT and variance VARFT of latent variables.
%
%    [EFT, VARFT, LPYT] = GPEP_PRED(GP, X, Y, XT, 'yt', YT, OPTIONS)
%    returns also logarithm of the predictive density LPYT of the
%    observations YT at test input locations XT. This can be used
%    for example in the cross-validation. Here Y has to be a vector.
%
%    [EFT, VARFT, LPYT, EYT, VARYT] = GPEP_PRED(GP, X, Y, XT, OPTIONS)
%    returns also the posterior predictive mean EYT and variance VARYT.
%
%    [EF, VARF, LPY, EY, VARY] = GPEP_PRED(GP, X, Y, OPTIONS)
%    evaluates the predictive distribution at training inputs X
%    and logarithm of the predictive density LPY of the training
%    observations Y.
%
%    OPTIONS is optional parameter-value pair
%      predcf - an index vector telling which covariance functions are
%               used for prediction. Default is all (1:gpcfn).
%               See additional information below.
%      tstind - a vector/cell array defining, which rows of X belong
%               to which training block in *IC type sparse models.
%               Default is []. In case of PIC, a cell array
%               containing index vectors specifying the blocking
%               structure for test data. IN FIC and CS+FIC a
%               vector of length n that points out the test inputs
%               that are also in the training set (if none, set
%               TSTIND = [])
%      yt     - optional observed yt in test points (see below)
%      z      - optional observed quantity in triplet (x_i,y_i,z_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, expected value
%               for ith case.
%      zt     - optional observed quantity in triplet (xt_i,yt_i,zt_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, the expected
%               value for the ith case.
%      fcorr  - Method used for latent marginal posterior corrections.
%               Default is 'off'. For EP possible method is 'fact'.
%               If method is 'on', 'fact' is used for EP.
%
%    NOTE! In case of FIC and PIC sparse approximation the
%    prediction for only some PREDCF covariance functions is just
%    an approximation since the covariance functions are coupled in
%    the approximation and are not strictly speaking additive
%    anymore.
%
%    For example, if you use covariance such as K = K1 + K2 your
%    predictions Eft1 = gpep_pred(GP, X, Y, X, 'predcf', 1) and
%    Eft2 = gpep_pred(gp, x, y, x, 'predcf', 2) should sum up to
%    Eft = gpep_pred(gp, x, y, x). That is Eft = Eft1 + Eft2. With
%    FULL model this is true but with FIC and PIC this is true only
%    approximately. That is Eft \approx Eft1 + Eft2.
%
%    With CS+FIC the predictions are exact if the PREDCF covariance
%    functions are all in the FIC part or if they are CS
%    covariances.
%
%    NOTE! When making predictions with a subset of covariance
%    functions with FIC approximation the predictive variance can
%    in some cases be ill-behaved i.e. negative or unrealistically
%    small. This may happen because of the approximative nature of
%    the prediction.
%
%  See also
%    GPEP_E, GPEP_G, GP_PRED, DEMO_SPATIAL, DEMO_CLASSIFIC

% Copyright (c) 2007-2010 Jarno Vanhatalo
% Copyright (c) 2010      Heikki Peura
% Copyright (c) 2011      Pasi Jylänki
% Copyright (c) 2012 Aki Vehtari

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

  ip=inputParser;
  ip.FunctionName = 'GPEP_PRED';
  ip.addRequired('gp', @isstruct);
  ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addOptional('xt', [], @(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))))
  ip.addParamValue('yt', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('zt', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('predcf', [], @(x) isempty(x) || ...
                   isvector(x) && isreal(x) && all(isfinite(x)&x>0))
  ip.addParamValue('tstind', [], @(x) isempty(x) || iscell(x) ||...
                   (isvector(x) && isreal(x) && all(isfinite(x)&x>0)))
  ip.addParamValue('fcorr', 'off', @(x) ismember(x, {'off', ...
                   'cm2', 'fact', 'on','lr'}))
  if numel(varargin)==0 || isnumeric(varargin{1})
    % inputParser should handle this, but it doesn't
    ip.parse(gp, x, y, varargin{:});
  else
    ip.parse(gp, x, y, [], varargin{:});
  end
  xt=ip.Results.xt;
  yt=ip.Results.yt;
  z=ip.Results.z;
  zt=ip.Results.zt;
  predcf=ip.Results.predcf;
  tstind=ip.Results.tstind;
  fcorr=ip.Results.fcorr;
  if isempty(xt)
    xt=x;
    if isempty(tstind)
      if iscell(gp)
        gptype=gp{1}.type;
      else
        gptype=gp.type;
      end
      switch gptype
        case {'FULL' 'VAR' 'DTC' 'SOR'}
          tstind = [];
        case {'FIC' 'CS+FIC'}
          tstind = 1:size(x,1);
        case 'PIC'
          if iscell(gp)
            tstind = gp{1}.tr_index;
          else
            tstind = gp.tr_index;
          end
      end
    end
    if isempty(yt)
      yt=y;
    end
    if isempty(zt)
      zt=z;
    end
  end

  if isfield(gp, 'monotonic') && gp.monotonic
      [gp,x,y,z,xt,zt] = gp.fh.setUpDataForMonotonic(gp,x,y,z,xt,zt);
  end

  [tn, tnin] = size(x);
  [n, nout] = size(y);

  if isfield(gp.lik, 'nondiagW')
    switch gp.type
      % ============================================================
      % FULL
      % ============================================================
      case 'FULL'
        %[e, edata, eprior, tautilde, nutilde, BKnu, B, cholP, invPBKnu]= gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        [e, edata, eprior, p]= gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        if isnan(e)
            Eft=NaN; Varft=NaN; lpyt=NaN; Eyt=NaN; Varyt=NaN;
            return
        end
        [nutilde, BKnu, B, cholP, invPBKnu]=deal(p.nutilde, p.BKnu, p.B, p.cholP, p.invPBKnu);

        if isfield(gp, 'comp_cf')  % own covariance for each ouput component
          multicf = true;
          if length(gp.comp_cf) ~= nout
            error('GPEP_PRED: the number of component vectors in gp.comp_cf must be the same as number of outputs.')
          end
          if ~isempty(predcf)
            if ~iscell(predcf) || length(predcf)~=nout
              error(['GPEP_PRED: if own covariance for each output component is used,'...
                     'predcf has to be cell array and contain nout (vector) elements.   '])
            end
          else
            predcf = gp.comp_cf;
          end
        else
          multicf = false;
          for i1=1:nout
            predcf2{i1} = predcf;
          end
          predcf=predcf2;
        end

        ntest=size(xt,1);
        % covariances between the training and test latents
        Kt = zeros(ntest,n,nout);
        if multicf
          for i1=1:nout
            Kt(:,:,i1) = gp_cov(gp,xt,x,predcf{i1});
          end
        else
          for i1=1:nout
            Kt(:,:,i1) = gp_cov(gp,xt,x,predcf{i1});
          end
        end

        % full ep with non-diagonal site covariances
        zz=zeros(n*nout,1);
        for k1=1:nout
          zz((1:n)+(k1-1)*n)=BKnu(:,k1)-B(:,:,k1)*invPBKnu;
        end

        %- posterior predictive mean
        Eft=zeros(ntest*nout,1);
        for z1=1:nout
          Eft((1:ntest)+(z1-1)*ntest)=Kt(:,:,z1)*(nutilde(:,z1)-zz((1:n)+(z1-1)*n));
        end

        if nargout > 1
          % posterior predictive covariance
          Covf=zeros(nout, nout, ntest);

          invcholPBKt=zeros(n,ntest,nout);
          for k1=1:nout
            invcholPBKt(:,:,k1)=cholP\(B(:,:,k1)*Kt(:,:,k1)');
          end

          %- update posterior covariance
          for k1=1:nout
            % covariances for the test latents
            kstarstar = gp_trvar(gp,xt,predcf{i1});

            Covf(k1,k1,:)=kstarstar-sum(Kt(:,:,k1)'.*(B(:,:,k1)*Kt(:,:,k1)'))'+sum(invcholPBKt(:,:,k1).*invcholPBKt(:,:,k1))';
            for j1=(k1+1):nout
              Covf(k1,j1,:)=sum(invcholPBKt(:,:,k1).*invcholPBKt(:,:,j1));
              Covf(j1,k1,:)=Covf(k1,j1,:);
            end
          end
          Varft=Covf;

        end

        % ============================================================
        % FIC
        % ============================================================
      case 'FIC'        % Predictions with FIC sparse approximation for GP
                        % ============================================================
                        % PIC
                        % ============================================================
      case {'PIC' 'PIC_BLOCK'}        % Predictions with PIC sparse approximation for GP
                                      % ============================================================
                                      % CS+FIC
                                      % ============================================================
      case 'CS+FIC'        % Predictions with CS+FIC sparse approximation for GP
    end

  else % isfield(gp.lik, 'nondiagW')
    switch gp.type
      % ============================================================
      % FULL
      % ============================================================
      case 'FULL'        % Predictions with FULL GP model
        %[e, edata, eprior, tautilde, nutilde, L] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        if isfield(gp.lik, 'int_likparam')

          [e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
          if isnan(e)
              Eft=NaN; Varft=NaN; lpyt=NaN; Eyt=NaN; Varyt=NaN;
              return
          end
          [tautildee, nutildee, L, L2] = deal(p.tautilde, p.nutilde, p.L, p.La2);

          tautilde=tautildee(:,1);
          nutilde=nutildee(:,1);
          if isfield(gp.lik,'int_likparam') && gp.lik.int_likparam && ~gp.lik.inputparam
             % Give q(theta) to likelihood function to integrate ovet
            zt=[p.mf2 L2'*L2];
          end
          if isfield(gp.lik, 'int_magnitude') && gp.lik.int_magnitude && ~gp.lik.inputmagnitude
            zt=[zt p.mf3 p.La3'*p.La3];
          end
          if (isfield(gp.lik, 'int_likparam') && gp.lik.inputparam) || ...
             (isfield(gp.lik, 'int_magnitude') && gp.lik.inputmagnitude) ...
              || (isfield(gp.lik, 'int_likparam') && isfield(gp, 'comp_cf'))
            [K,C]=gp_trcov(gp,x,gp.comp_cf{1});
            kstarstar = gp_trvar(gp, xt, gp.comp_cf{1});
            K_nf=gp_cov(gp,xt,x,gp.comp_cf{1});
          else
            [K, C]=gp_trcov(gp,x);
            kstarstar = gp_trvar(gp, xt, predcf);
            K_nf=gp_cov(gp,xt,x,predcf);
          end
          ntest=size(xt,1);
          [n,nin] = size(x);

          if size(tautildee,2)==1 && all(tautilde > 0) && ~isequal(gp.latent_opt.optim_method, 'robust-EP')
            % This is the usual case where likelihood is log concave
            % for example, Poisson and probit
            sqrttautilde = sqrt(tautilde(:,1));
            Stildesqroot = sparse(1:n, 1:n, sqrttautilde, n, n);

            if ~isfield(gp,'meanf')
              if issparse(L) % If compact support covariance functions are used
                             % the covariance matrix will be sparse
                zz=Stildesqroot*ldlsolve(L,Stildesqroot*(C*nutilde));
              else
                zz=Stildesqroot*(L'\(L\(Stildesqroot*(C*nutilde))));
              end
              Eft=K_nf*(nutilde-zz);    % The mean, zero mean GP
            else
              zz = Stildesqroot*(L'\(L\(Stildesqroot*(C))));

              Eft_zm=K_nf*(nutilde-zz*nutilde); % The mean, zero mean GP
              Ks = eye(size(zz)) - zz;           % inv(K + S^-1)*S^-1
              Ksy = Ks*nutilde;
              [RB RAR] = mean_predf(gp,x,xt,K_nf',Ks,Ksy,'EP',Stildesqroot.^2);

              Eft = Eft_zm + RB;            % The mean
            end

            % Compute variance
            if nargout > 1
              if issparse(L)
                V = ldlsolve(L, Stildesqroot*K_nf');
                Varft = kstarstar - sum(K_nf.*(Stildesqroot*V)',2);
              else
                V = (L\Stildesqroot)*K_nf';
                Varft = kstarstar - sum(V.^2)';
              end
              if isfield(gp,'meanf')
                Varft = Varft + RAR;
              end
            end
          else
            % We might end up here if the likelihood is not log concave
            % For example Student-t likelihood.

            %{
            zz=tautilde.*(L'*(L*nutilde));
            Eft=K_nf*(nutilde-zz);

            if nargout > 1
              S = diag(tautilde);
              V = K_nf*S*L';
              Varft = kstarstar - sum((K_nf*S).*K_nf,2) + sum(V.^2,2);
            end
            %}

            % An alternative implementation for avoiding negative variances
            [Eft,V]=pred_var(tautilde,K,K_nf,nutilde);
            Varft=kstarstar-V;

          end
          if isfield(gp.lik, 'int_likparam') && gp.lik.int_likparam && gp.lik.inputparam
            tautilde=tautildee(:,2);
            nutilde=nutildee(:,2);
            [K, C]=gp_trcov(gp,x, gp.comp_cf{2});
            kstarstar = gp_trvar(gp, xt, gp.comp_cf{2});
            K_nf=gp_cov(gp,xt,x,gp.comp_cf{2});

            [Eft(:,2),V]=pred_var(tautilde,K,K_nf,nutilde);
            Varft(:,2)=kstarstar-V;
          end
          if isfield(gp.lik, 'int_magnitude') && gp.lik.int_magnitude && gp.lik.inputmagnitude
            tautilde=tautildee(:,end);
            nutilde=nutildee(:,end);
            [K, C]=gp_trcov(gp,x, gp.comp_cf{end});
            kstarstar = gp_trvar(gp, xt, gp.comp_cf{end});
            K_nf=gp_cov(gp,xt,x,gp.comp_cf{end});

            [Eft(:,end+1),V]=pred_var(tautilde,K,K_nf,nutilde);
            Varft(:,end+1)=kstarstar-V;
          end

        else % isfield(gp.lik, 'int_likparam')

          [e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
          if isnan(e)
              Eft=NaN; Varft=NaN; lpyt=NaN; Eyt=NaN; Varyt=NaN;
              return
          end
          [tautilde, nutilde, L] = deal(p.tautilde, p.nutilde, p.L);

          [K, C]=gp_trcov(gp,x);
          kstarstar = gp_trvar(gp, xt, predcf);
          ntest=size(xt,1);
          K_nf=gp_cov(gp,xt,x,predcf);
          [n,nin] = size(x);

          if all(tautilde > 0) ...
                  && ~(isequal(gp.latent_opt.optim_method, 'robust-EP') )
            % This is the usual case where likelihood is log concave
            % for example, Poisson and probit
            sqrttautilde = sqrt(tautilde);
            nstt=length(sqrttautilde);
            Stildesqroot = sparse(1:nstt, 1:nstt, sqrttautilde, nstt,  nstt);

            if ~isfield(gp,'meanf')
              if issparse(L)          % If compact support covariance functions a  re used
                                      % the covariance matrix will be sparse
                zz=Stildesqroot*ldlsolve(L,Stildesqroot*(C*nutilde));
              else
                zz=Stildesqroot*(L'\(L\(Stildesqroot*(C*nutilde))));
              end

              Eft=K_nf*(nutilde-zz);    % The mean, zero mean GP
            else
              zz = Stildesqroot*(L'\(L\(Stildesqroot*(C))));

              Eft_zm=K_nf*(nutilde-zz*nutilde); % The mean, zero mean GP
              Ks = eye(size(zz)) - zz;           % inv(K + S^-1)*S^-1
              Ksy = Ks*nutilde;
              [RB RAR] = mean_predf(gp,x,xt,K_nf',Ks,Ksy,'EP',Stildesqroot.^2);

              Eft = Eft_zm + RB;            % The mean
            end

            % Compute variance
            if nargout > 1
              if issparse(L)
                V = ldlsolve(L, Stildesqroot*K_nf');
                Varft = kstarstar - sum(K_nf.*(Stildesqroot*V)',2);
              else
                V = (L\Stildesqroot)*K_nf';
                Varft = kstarstar - sum(V.^2)';
              end
              if isfield(gp,'meanf')
                Varft = Varft + RAR;
              end
            end
          else
            % We might end up here if the likelihood is not log concave
            % For example Student-t likelihood.

            %{
            zz=tautilde.*(L'*(L*nutilde));
            Eft=K_nf*(nutilde-zz);

            if nargout > 1
              S = diag(tautilde);
              V = K_nf*S*L';
              Varft = kstarstar - sum((K_nf*S).*K_nf,2) + sum(V.^2,2);
            end
            %}

            % An alternative implementation for avoiding negative variances
            [Eft,V]=pred_var(tautilde,K,K_nf,nutilde);
            Varft=kstarstar-V;

          end

        end % isfield(gp.lik, 'int_likparam')
        % ============================================================
        % FIC
        % ============================================================
      case 'FIC'        % Predictions with FIC sparse approximation for GP
                        %[e, edata, eprior, tautilde, nutilde, L, La, b] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        [e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        if isnan(e)
            Eft=NaN; Varft=NaN; lpyt=NaN; Eyt=NaN; Varyt=NaN;
            return
        end
        [tautilde, nutilde, L, La, b] = deal(p.tautilde, p.nutilde, p.L, p.La2, p.b);

        % Here tstind = 1 if the prediction is made for the training set
        if nargin > 6
          if ~isempty(tstind) && length(tstind) ~= size(x,1)
            error('tstind (if provided) has to be of same length as x.')
          end
        else
          tstind = [];
        end

        u = gp.X_u;
        m = size(u,1);

        K_fu = gp_cov(gp,x,u,predcf);          % f x u
        K_nu=gp_cov(gp,xt,u,predcf);
        K_uu = gp_trcov(gp,u,predcf);          % u x u, noiseless covariance K_uu
        K_uu = (K_uu+K_uu')./2;                % ensure the symmetry of K_uu

        kstarstar=gp_trvar(gp,xt,predcf);

        if all(tautilde > 0) && ~isequal(gp.latent_opt.optim_method, 'robust-EP')

          % From this on evaluate the prediction
          % See Snelson and Ghahramani (2007) for details
          %        p=iLaKfu*(A\(iLaKfu'*mutilde));
          p = b';

          ntest=size(xt,1);

          Eft = K_nu*(K_uu\(K_fu'*p));

          % if the prediction is made for training set, evaluate Lav also for prediction points
          if ~isempty(tstind)
            [Kv_ff, Cv_ff] = gp_trvar(gp, xt(tstind,:), predcf);
            Luu = chol(K_uu)';
            B=Luu\(K_fu');
            Qv_ff=sum(B.^2)';
            Lav = Kv_ff-Qv_ff;
            Eft(tstind) = Eft(tstind) + Lav.*p;
          end

          % Compute variance
          if nargout > 1
            %Varft(i1,1)=kstarstar(i1) - (sum(Knf(i1,:).^2./La') - sum((Knf(i1,:)*L).^2));
            Luu = chol(K_uu)';
            B=Luu\(K_fu');
            B2=Luu\(K_nu');
            Varft = kstarstar - sum(B2'.*(B*(repmat(La,1,m).\B')*B2)',2)  + sum((K_nu*(K_uu\(K_fu'*L))).^2, 2);

            % if the prediction is made for training set, evaluate Lav also for prediction points
            if ~isempty(tstind)
              Varft(tstind) = Varft(tstind) - 2.*sum( B2(:,tstind)'.*(repmat((La.\Lav),1,m).*B'),2) ...
                  + 2.*sum( B2(:,tstind)'*(B*L).*(repmat(Lav,1,m).*L), 2)  ...
                  - Lav./La.*Lav + sum((repmat(Lav,1,m).*L).^2,2);
            end
          end

        else
          % Robust-EP
          [Eft,V]=pred_var2(tautilde,nutilde,L,K_uu,K_fu,b,K_nu);
          Varft=kstarstar-V;

        end

        % ============================================================
        % PIC
        % ============================================================
      case {'PIC' 'PIC_BLOCK'}        % Predictions with PIC sparse approximation for GP
                                      % Calculate some help matrices
        u = gp.X_u;
        ind = gp.tr_index;
        %[e, edata, eprior, tautilde, nutilde, L, La, b] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        [e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        if isnan(e)
            Eft=NaN; Varft=NaN; lpyt=NaN; Eyt=NaN; Varyt=NaN;
            return
        end
        [L, La, b] = deal(p.L, p.La2, p.b);

        K_fu = gp_cov(gp, x, u, predcf);         % f x u
        K_nu = gp_cov(gp, xt, u, predcf);         % n x u
        K_uu = gp_trcov(gp, u, predcf);    % u x u, noiseles covariance K_uu

        % From this on evaluate the prediction
        % See Snelson and Ghahramani (2007) for details
        %        p=iLaKfu*(A\(iLaKfu'*mutilde));
        p = b';

        iKuuKuf = K_uu\K_fu';

        w_bu=zeros(length(xt),length(u));
        w_n=zeros(length(xt),1);
        for i=1:length(ind)
          w_bu(tstind{i},:) = repmat((iKuuKuf(:,ind{i})*p(ind{i},:))', length(tstind{i}),1);
          K_nf = gp_cov(gp, xt(tstind{i},:), x(ind{i},:), predcf);              % n x u
          w_n(tstind{i},:) = K_nf*p(ind{i},:);
        end

        Eft = K_nu*(iKuuKuf*p) - sum(K_nu.*w_bu,2) + w_n;

        % Compute variance
        if nargout > 1
          kstarstar = gp_trvar(gp, xt, predcf);
          KnfL = K_nu*(iKuuKuf*L);
          Varft = zeros(length(xt),1);
          for i=1:length(ind)
            v_n = gp_cov(gp, xt(tstind{i},:), x(ind{i},:), predcf);              % n x u
            v_bu = K_nu(tstind{i},:)*iKuuKuf(:,ind{i});
            KnfLa = K_nu*(iKuuKuf(:,ind{i})/chol(La{i}));
            KnfLa(tstind{i},:) = KnfLa(tstind{i},:) - (v_bu + v_n)/chol(La{i});
            Varft = Varft + sum((KnfLa).^2,2);
            KnfL(tstind{i},:) = KnfL(tstind{i},:) - v_bu*L(ind{i},:) + v_n*L(ind{i},:);
          end
          Varft = kstarstar - (Varft - sum((KnfL).^2,2));

        end
        % ============================================================
        % CS+FIC
        % ============================================================
      case 'CS+FIC'        % Predictions with CS+FIC sparse approximation for GP
                           % Here tstind = 1 if the prediction is made for the training set
        if nargin > 6
          if ~isempty(tstind) && length(tstind) ~= size(x,1)
            error('tstind (if provided) has to be of same length as x.')
          end
        else
          tstind = [];
        end

        u = gp.X_u;
        m = length(u);
        n = size(x,1);
        n2 = size(xt,1);

        %[e, edata, eprior, tautilde, nutilde, L, La, b] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        [e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        if isnan(e)
            Eft=NaN; Varft=NaN; lpyt=NaN; Eyt=NaN; Varyt=NaN;
            return
        end
        [L, La, b] = deal(p.L, p.La2, p.b);

        % Indexes to all non-compact support and compact support covariances.
        cf1 = [];
        cf2 = [];
        % Indexes to non-CS and CS covariances, which are used for predictions
        predcf1 = [];
        predcf2 = [];

        ncf = length(gp.cf);
        % Loop through all covariance functions
        for i = 1:ncf
          % Non-CS covariances
          if ~isfield(gp.cf{i},'cs')
            cf1 = [cf1 i];
            % If used for prediction
            if ~isempty(find(predcf==i))
              predcf1 = [predcf1 i];
            end
            % CS-covariances
          else
            cf2 = [cf2 i];
            % If used for prediction
            if ~isempty(find(predcf==i))
              predcf2 = [predcf2 i];
            end
          end
        end
        if isempty(predcf1) && isempty(predcf2)
          predcf1 = cf1;
          predcf2 = cf2;
        end

        % Determine the types of the covariance functions used
        % in making the prediction.
        if ~isempty(predcf1) && isempty(predcf2)       % Only non-CS covariances
          ptype = 1;
          predcf2 = cf2;
        elseif isempty(predcf1) && ~isempty(predcf2)   % Only CS covariances
          ptype = 2;
          predcf1 = cf1;
        else                                           % Both non-CS and CS covariances
          ptype = 3;
        end

        K_fu = gp_cov(gp,x,u,predcf1);   % f x u
        K_uu = gp_trcov(gp,u,predcf1);     % u x u, noiseles covariance K_uu
        K_uu = (K_uu+K_uu')./2;     % ensure the symmetry of K_uu
        K_nu=gp_cov(gp,xt,u,predcf1);

        Kcs_nf = gp_cov(gp, xt, x, predcf2);

        p = b';
        ntest=size(xt,1);

        % Calculate the predictive mean according to the type of
        % covariance functions used for making the prediction
        if ptype == 1
          Eft = K_nu*(K_uu\(K_fu'*p));
        elseif ptype == 2
          Eft = Kcs_nf*p;
        else
          Eft = K_nu*(K_uu\(K_fu'*p)) + Kcs_nf*p;
        end

        % evaluate also Lav if the prediction is made for training set
        if ~isempty(tstind)
          [Kv_ff, Cv_ff] = gp_trvar(gp, xt(tstind,:), predcf1);
          Luu = chol(K_uu)';
          B=Luu\(K_fu');
          Qv_ff=sum(B.^2)';
          Lav = Kv_ff-Qv_ff;
        end

        % Add also Lav if the prediction is made for training set
        % and non-CS covariance function is used for prediction
        if ~isempty(tstind) && (ptype == 1 || ptype == 3)
          Eft(tstind) = Eft(tstind) + Lav.*p;
        end

        % Evaluate the variance
        if nargout > 1
          Knn_v = gp_trvar(gp,xt,predcf);
          Luu = chol(K_uu)';
          B=Luu\(K_fu');
          B2=Luu\(K_nu');
          p = amd(La);
          iLaKfu = La\K_fu;
          % Calculate the predictive variance according to the type
          % covariance functions used for making the prediction
          if ptype == 1 || ptype == 3
            % FIC part of the covariance
            Varft = Knn_v - sum(B2'.*(B*(La\B')*B2)',2) + sum((K_nu*(K_uu\(K_fu'*L))).^2, 2);
            % Add Lav2 if the prediction is made for the training set
            if  ~isempty(tstind)
              % Non-CS covariance
              if ptype == 1
                Kcs_nf = sparse(tstind,1:n,Lav,n2,n);
                % Non-CS and CS covariances
              else
                Kcs_nf = Kcs_nf + sparse(tstind,1:n,Lav,n2,n);
              end
              % Add Lav2 inside Kcs_nf
              Varft = Varft - sum((Kcs_nf(:,p)/chol(La(p,p))).^2,2) + sum((Kcs_nf*L).^2, 2) ...
                      - 2.*sum((Kcs_nf*iLaKfu).*(K_uu\K_nu')',2) + 2.*sum((Kcs_nf*L).*(L'*K_fu*(K_uu\K_nu'))' ,2);
              % In case of both non-CS and CS prediction covariances add
              % only Kcs_nf if the prediction is not done for the training set
            elseif ptype == 3
              Varft = Varft - sum((Kcs_nf(:,p)/chol(La(p,p))).^2,2) + sum((Kcs_nf*L).^2, 2) ...
                      - 2.*sum((Kcs_nf*iLaKfu).*(K_uu\K_nu')',2) + 2.*sum((Kcs_nf*L).*(L'*K_fu*(K_uu\K_nu'))' ,2);
            end
            % Prediction with only CS covariance
          elseif ptype == 2
            Varft = Knn_v - sum((Kcs_nf(:,p)/chol(La(p,p))).^2,2) + sum((Kcs_nf*L).^2, 2) ;
          end
        end
        % ============================================================
        % DTC/(VAR)
        % ============================================================
      case {'DTC' 'VAR' 'SOR'}        % Predictions with DTC or variational sparse approximation for GP
                                      %[e, edata, eprior, tautilde, nutilde, L, La, b] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        [e, edata, eprior, p] = gpep_e(gp_pak(gp), gp, x, y, 'z', z);
        if isnan(e)
            Eft=NaN; Varft=NaN; lpyt=NaN; Eyt=NaN; Varyt=NaN;
            return
        end
        [L, La, b] = deal(p.L, p.La2, p.b);

        % Here tstind = 1 if the prediction is made for the training set
        if nargin > 6
          if ~isempty(tstind) && length(tstind) ~= size(x,1)
            error('tstind (if provided) has to be of same length as x.')
          end
        else
          tstind = [];
        end

        u = gp.X_u;
        m = size(u,1);

        K_fu = gp_cov(gp,x,u,predcf);         % f x u
        K_nu=gp_cov(gp,xt,u,predcf);
        K_uu = gp_trcov(gp,u,predcf);          % u x u, noiseles covariance K_uu
        K_uu = (K_uu+K_uu')./2;          % ensure the symmetry of K_uu

        kstarstar=gp_trvar(gp,xt,predcf);

        % From this on evaluate the prediction
        p = b';

        ntest=size(xt,1);

        Eft = K_nu*(K_uu\(K_fu'*p));

        % if the prediction is made for training set, evaluate Lav also for prediction points
        if ~isempty(tstind)
          [Kv_ff, Cv_ff] = gp_trvar(gp, xt(tstind,:), predcf);
          Luu = chol(K_uu)';
          B=Luu\(K_fu');
          Qv_ff=sum(B.^2)';
          Lav = Kv_ff-Cv_ff;
          Eft(tstind) = Eft(tstind);% + Lav.*p;
        end

        if nargout > 1
          % Compute variances of predictions
          %Varft(i1,1)=kstarstar(i1) - (sum(Knf(i1,:).^2./La') - sum((Knf(i1,:)*L).^2));
          Luu = chol(K_uu)';
          B=Luu\(K_fu');
          B2=Luu\(K_nu');

          Varft = sum(B2'.*(B*(repmat(La,1,m).\B')*B2)',2)  + sum((K_nu*(K_uu\(K_fu'*L))).^2, 2);
          switch gp.type
            case {'VAR' 'DTC'}
              Varft = kstarstar - Varft;
            case 'SOR'
              Varft = sum(B2.^2,1)' - Varft;
          end
        end
    end
  end
  if ~isequal(fcorr, 'off')
    % Do marginal corrections for samples
    [pc_predm, fvecm] = gp_predcm(gp, x, y, xt, 'z', z, 'ind', 1:size(xt,1), 'fcorr', fcorr);
    for i=1:size(xt,1)
      % Remove NaNs and zeros
      pc_pred=pc_predm(:,i);
      dii=isnan(pc_pred)|pc_pred==0;
      pc_pred(dii)=[];
      fvec=fvecm(:,i);
      fvec(dii)=[];
      % Compute mean correction
      Eft(i) = trapz(fvec.*(pc_pred./sum(pc_pred)));
    end
   end

  % ============================================================
  % Evaluate also the predictive mean and variance of new observation(s)
  % ============================================================
  if ~isequal(fcorr, 'off')
    if nargout == 3
      if isempty(yt)
        lpyt=[];
      else
        lpyt = gp.lik.fh.predy(gp.lik, fvecm', pc_predm', yt, zt);
      end
    elseif nargout > 3
      [lpyt, Eyt, Varyt] = gp.lik.fh.predy(gp.lik, fvecm', pc_predm', yt, zt);
    end
  else
    if nargout == 3
      if isempty(yt)
        lpyt=[];
      else
        lpyt = gp.lik.fh.predy(gp.lik, Eft, Varft, yt, zt);
      end
    elseif nargout > 3
      [lpyt, Eyt, Varyt] = gp.lik.fh.predy(gp.lik, Eft, Varft, yt, zt);
    end
  end
end

function [m,S]=pred_var(tau_q,K,A,b)

% helper function for determining
%
% m = A * inv( K+ inv(diag(tau_q)) ) * inv(diag(tau_q)) *b
% S = diag( A * inv( K+ inv(diag(tau_q)) ) * A)
%
% when the site variances tau_q may be negative
%

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

  m=A*b;
  b=K*b;
  S=zeros(size(A,1),1);
  u=0;
  U=0;
  L1=[];
  if ~isempty(ii1)
    % Cholesky decomposition for the positive sites
    L1=(W1*W1').*K(ii1,ii1);
    L1(1:n1+1:end)=L1(1:n1+1:end)+1;
    L1=chol(L1);

    U = bsxfun(@times,A(:,ii1),W1')/L1;
    u = L1'\(W1.*b(ii1));

    m = m-U*u;
    S = S+sum(U.^2,2);
  end

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

    [L2,pd]=chol(L2);
    if pd==0
      U = bsxfun(@minus, bsxfun(@times,A(:,ii2),W2')/L2,U*(bsxfun(@times,V,W2)'/L2));
      u = L2'\(W2.*b(ii2)) -L2'\(bsxfun(@times,V,W2)*u);

      m = m+U*u;
      S = S-sum(U.^2,2);
    else
      fprintf('Posterior covariance is negative definite.\n')
    end
  end

end

function [m_q,S_q]=pred_var2(tautilde,nutilde,L,K_uu,K_fu,D,K_nu)

% function for determining the parameters of the q-distribution
% when site variances tau_q may be negative
%
% q(f) = N(f|0,K)*exp( -0.5*f'*diag(tau_q)*f + nu_q'*f )/Z_q = N(f|m_q,S_q)
%
% S_q = inv(inv(K)+diag(tau_q)) where K is sparse approximation for prior
%       covariance
% m_q = S_q*nu_q;
%
% det(eye(n)+K*diag(tau_q))) = det(L1)^2 * det(L2)^2
% where L1 and L2 are upper triangular
%
% see Expectation consistent approximate inference (Opper & Winther, 2005)

  n=length(nutilde);

  U = K_fu;
  S = 1+tautilde.*D;
  B = tautilde./S;
  BUiL = bsxfun(@times, B, U)/L';
  % iKS = diag(B) - BUiL*BUiL';

  Ktnu = D.*nutilde + U*(K_uu\(U'*nutilde));
  m_q = nutilde - B.*Ktnu + BUiL*(BUiL'*Ktnu);
  kstar = K_nu*(K_uu\K_fu');
  m_q = kstar*m_q;

  S_q = sum(bsxfun(@times,B',kstar.^2),2) - sum((kstar*BUiL).^2,2);
  % S_q = kstar*iKS*kstar';

end