% Function which propagates and adds noise to the 3D points and popoulates
% other variables of the structs passed to the function
% motion = [trans, rot_yaw_theta]
function [detectionsQ, detectionsT] = propagateDetections(detectionsQ, detectionsT, params_2D3D, params_carCuboid, motion, first_time)

%% for all detections in Query propagate the box to 3D box in F2 frame

    for i = 1:length(detectionsQ)

        b1Q = [];
        B1Q = [];
        bvolumeQ1 = [];

        % check if the detection is first time i.e. we have to start with
        % the canonical cuboid. Else we already have the cuboid.

        if(length(detectionsQ{i}.bvolume) == 0 )
            %disp(sprintf('first_time - id :%d\n',detectionsQ{i}.dno));
            canonicalCuboid = getCanonicalCuboid( params_carCuboid.avg_Sz);

            % find center of the bounding box bottom line
            b1Q = [detectionsQ{i}.bbox(1) + (detectionsQ{i}.bbox(3) - detectionsQ{i}.bbox(1))/2 ;
                  detectionsQ{i}.bbox(4);
                  1.0];

            % project it to 3D
            B1Q = (params_2D3D.h * inv(params_2D3D.K) * b1Q) / (params_2D3D.n' * inv(params_2D3D.K) * b1Q);

            % apply offset which is a function of yaw and get car's origin (remember approx.)
            offset_Z = getOffsetBasedOnYaw([params_carCuboid.avg_Sz(1); params_carCuboid.avg_Sz(3)], detectionsQ{i}.yaw);

            % car's origin in Query Frame
            B1Q = B1Q + [0; 0; offset_Z];

            % translate canonical cuboid
            canonicalCuboid = canonicalCuboid + repmat(B1Q', 8,1);

            % BOUNDING VOLUME IN QUERY FRAME
            [bvolumeQ1, k1] = getBoundingVolume(B1Q, canonicalCuboid, detectionsQ{i}.yaw, ...
                                                detectionsQ{i}.sigma_3D, ...
                                                params_carCuboid.sz_ub, params_carCuboid.sz_lb ...
                                               );

            bvolumeQ1 = bvolumeQ1 - repmat([0 params_carCuboid.avg_Sz(2)/2 0], size(bvolumeQ1,1), 1);
        else

            B1Q = detectionsQ{i}.origin;
            bvolumeQ1 = detectionsQ{i}.bvolume;

        end

        % TODO
             % propagate noise and find new noise .... for now same noise
        %

        % car's origin in Train frame
        B2Q = motion(1:3) + B1Q ;
        bvolumeQ1 = bvolumeQ1 + repmat(motion(1:3)', size(bvolumeQ1,1),1);
        % CUBOID IN TRAIN FRAME

        %dummy_sigma = eye(4,4);
        %dumm_sigma(4,4) = 0;

        [bvolumeQ2, k2] = getBoundingVolume(B2Q, bvolumeQ1, motion(4), ...
                                             detectionsQ{i}.sigma_3D, ...
                                             params_carCuboid.sz_ub, params_carCuboid.sz_lb ...
                                            );

       % bvolumeQ2 = bvolumeQ2 - repmat([0 params_carCuboid.avg_Sz(2)/2 0], size(bvolumeQ2,1), 1);    % offset the h/2 as car ar to be on road

        % compute projection of bvolume and store in bvolume_proj variable
        bvolume_proj = (params_2D3D.K*bvolumeQ2')';
        bvolume_proj(:,1:3) = bvolume_proj(:,1:3)./repmat(bvolume_proj(:,3), 1,3);
        kbvolume_proj = convhull(bvolume_proj(:,1:2));
        bvolume_proj = bvolume_proj(kbvolume_proj,:);

        % update the fields of the structure

        detectionsQ{i}.bvolume = bvolumeQ2;
        detectionsQ{i}.bvolume_proj = bvolume_proj;
        detectionsQ{i}.yaw  = detectionsQ{i}.yaw+motion(4);
        detectionsQ{i}.origin  = B2Q;
        detectionsQ{i}.k = k2;

    end

    %% for all detections in Train propagate the box to 3D box in its frame

    for i = 1:length(detectionsT)

        canonicalCuboid = getCanonicalCuboid( params_carCuboid.avg_Sz);

        % find center of the bounding box bottom line
        b1T = [detectionsT{i}.bbox(1) + (detectionsT{i}.bbox(3) - detectionsT{i}.bbox(1))/2 ;
              detectionsT{i}.bbox(4);
              1.0];

        % project it to 3D
        B1T = (params_2D3D.h * inv(params_2D3D.K) * b1T) / (params_2D3D.n' * inv(params_2D3D.K) * b1T);

        % apply offset which is a function of yaw and get car's origin (remember approx.)
        offset_Z = getOffsetBasedOnYaw([params_carCuboid.avg_Sz(1); params_carCuboid.avg_Sz(3)], detectionsT{i}.yaw);

        % car's origin in Query Frame
        B1T = B1T + [0; 0; offset_Z];

        % translate canonical cuboid
        canonicalCuboid = canonicalCuboid + repmat(B1T', 8,1);

        % BOUNDING VOLUME IN QUERY FRAME
        [bvolumeT1, k1] = getBoundingVolume(B1T, canonicalCuboid, detectionsT{i}.yaw, ...
                                            detectionsT{i}.sigma_3D, ...
                                            params_carCuboid.sz_ub, params_carCuboid.sz_lb ...
                                           );
        % update the fields of the structure
        detectionsT{i}.origin  = B1T;
        detectionsT{i}.k = k1;

        detectionsT{i}.bvolume = bvolumeT1 - repmat([0 params_carCuboid.avg_Sz(2)/2 0], size(bvolumeT1,1), 1);    % offset the h/2 as car ar to be on road             ;

        bvolume_proj = detectionsT{i}.bvolume;
        bvolume_proj = (params_2D3D.K*bvolume_proj')';
        bvolume_proj(:,1:3) = bvolume_proj(:,1:3)./repmat(bvolume_proj(:,3), 1,3);
        kbvolume_proj = convhull(bvolume_proj(:,1:2));
        bvolume_proj = bvolume_proj(kbvolume_proj,:);

        detectionsT{i}.bvolume_proj = bvolume_proj;
    end

end


function [warped]= sn2individual(P, input)

% SN2INDIVIDUAL warps the input coordinates (defined as Nx3 matrix) from
% normalised MNI coordinates to individual headspace coordinates, using the
% warp parameters defined in the structure spmparams.
%
% this is modified from code from nutmeg: nut_mni2mri, which was itself
% modified from code originally written by John Ashburner:
% http://www.sph.umich.edu/~nichols/JG2/get_orig_coord2.m

% Copyright (C) 2013-2021, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if isfield(P, 'Tr')
  % this is an old-style representation of the parameters, so it uses the
  % code adjusted from nut_mni2mri

  if numel(P.Tr)==0
    % only an affine transformation has been done
    T      = P.VF.mat*P.Affine/(P.VG.mat);
    warped = ft_warp_apply(T, input);

  else
    % we need the spm_dctmtx function for the nonlinear case
    if ~ft_hastoolbox('spm')
      % add SPM8 or later to the path
      ft_hastoolbox('spm8up', 1);
    end

    dim  = P.VG.dim(1:3);
    xyz  = ft_warp_apply(inv(P.VG.mat), input); % goes into voxel coordinates

    basX = spm_dctmtx(dim(1), size(P.Tr,1), xyz(:,1)-1);
    basY = spm_dctmtx(dim(2), size(P.Tr,2), xyz(:,2)-1);
    basZ = spm_dctmtx(dim(3), size(P.Tr,3), xyz(:,3)-1);

    siz = size(P.Tr);
    Tr1 = reshape(P.Tr(:,:,:,1),siz(1)*siz(2),siz(3));
    Tr2 = reshape(P.Tr(:,:,:,2),siz(1)*siz(2),siz(3));
    Tr3 = reshape(P.Tr(:,:,:,3),siz(1)*siz(2),siz(3));

    xyztmp = zeros(size(xyz));
    for i=1:size(xyz,1)
      bx = basX(i,:);
      by = basY(i,:);
      bz = basZ(i,:);
      tx = reshape(Tr1*bz', siz(1), siz(2) );
      ty = reshape(Tr2*bz', siz(1), siz(2) );
      tz = reshape(Tr3*bz', siz(1), siz(2) );
      xyztmp(i,:) = [bx*tx*by' bx*ty*by' bx*tz*by'];
    end

    T      = P.VF.mat*P.Affine;
    warped = ft_warp_apply(T, xyz+xyztmp);
  end

else
  % the only way I can come up with to do this, is to write a deformation
  % to disk, and to sample this one. This requires spm12 on the path
  ft_hastoolbox('spm12', 1);

  fprintf('creating the deformation field and writing it to a temporary file\n');

  fname  = [tempname,'.nii'];
  V      = nifti;
  V.dat  = file_array(fname, P.image.dim(1:3), [spm_type('float32') spm_platform('bigend')], 0, 1, 0);
  V.mat  = P.image.mat;
  if isfield(P.image, 'mat0')
    V.mat0 = P.image.mat0;
  end
  V.descrip = 'deformation field';
  create(V);
  V.dat(:) = 0; % this is necessary, otherwise SPM fails: image too small

  P.image  = spm_vol(fname);
  spm_preproc_write8(P, zeros(6,4), [0 0], [0 1], 1, 1, nan(2,3), nan);

  [pth,nam,ext] = fileparts(fname);
  V  = nifti(fullfile(pth,['y_',nam,ext]));
  y  = squeeze(V.dat(:,:,:,:,:));

  siz = size(y);
  VT.dim = siz(1:3);
  VT.mat = P.tpm(1).mat;

  % 2b: write the deformation fields in x/y/z direction to temporary files
  V1.fname     = [tempname '.img'];
  V1.dim(1:3)  = VT.dim(1:3);
  V1.pinfo     = [1 0 0]';
  V1.mat       = VT.mat;
  V1.dt        = [64 0];
  V1.descrip   = 'Deformation field';
  spm_write_vol(V1,y(:,:,:,1));

  V2.fname     = [tempname '.img'];
  V2.dim(1:3)  = VT.dim(1:3);
  V2.pinfo     = [1 0 0]';
  V2.mat       = VT.mat;
  V2.dt        = [64 0];
  V2.descrip   = 'Deformation field';
  spm_write_vol(V2,y(:,:,:,2));

  V3.fname     = [tempname '.img'];
  V3.dim(1:3)  = VT.dim(1:3);
  V3.pinfo     = [1 0 0]';
  V3.mat       = VT.mat;
  V3.dt        = [64 0];
  V3.descrip   = 'Deformation field';
  spm_write_vol(V3,y(:,:,:,3));

  % first warp to voxel coordinates
  input_vox = ft_warp_apply(inv(VT.mat), input);  % Express as voxel indices

  % apply the non-linear warp
  warped = cat(2, spm_sample_vol(V1,input_vox(:,1),input_vox(:,2),input_vox(:,3),1), ...
    spm_sample_vol(V2,input_vox(:,1),input_vox(:,2),input_vox(:,3),1), ...
    spm_sample_vol(V3,input_vox(:,1),input_vox(:,2),input_vox(:,3),1));
end


function [Y, optinf] = cbpdn(D, S, lambda, opt)

% cbpdn -- Convolutional Basis Pursuit DeNoising
%
%         argmin_{x_m} (1/2)||\sum_m d_m * x_m - s||_2^2 +
%                           lambda \sum_m ||x_m||_1
%
%         The solution is computed using an ADMM approach (see
%         boyd-2010-distributed) with efficient solution of the main
%         linear systems (see wohlberg-2016-efficient).
%
% Usage:
%       [Y, optinf] = cbpdn(D, S, lambda, opt);
%
% Input:
%       D           Dictionary filter set (3D array)
%       S           Input image
%       lambda      Regularization parameter
%       opt         Algorithm parameters structure
%
% Output:
%       Y           Dictionary coefficient map set (3D array)
%       optinf      Details of optimisation
%
%
% Options structure fields:
%   Verbose          Flag determining whether iteration status is displayed.
%                    Fields are iteration number, functional value,
%                    data fidelity term, l1 regularisation term, and
%                    primal and dual residuals (see Sec. 3.3 of
%                    boyd-2010-distributed). The value of rho is also
%                    displayed if options request that it is automatically
%                    adjusted.
%   MaxMainIter      Maximum main iterations
%   AbsStopTol       Absolute convergence tolerance (see Sec. 3.3.1 of
%                    boyd-2010-distributed)
%   RelStopTol       Relative convergence tolerance (see Sec. 3.3.1 of
%                    boyd-2010-distributed)
%   L1Weight         Weighting array for coefficients in l1 norm of X
%   Y0               Initial value for Y
%   U0               Initial value for U
%   rho              ADMM penalty parameter
%   AutoRho          Flag determining whether rho is automatically updated
%                    (see Sec. 3.4.1 of boyd-2010-distributed)
%   AutoRhoPeriod    Iteration period on which rho is updated
%   RhoRsdlRatio     Primal/dual residual ratio in rho update test
%   RhoScaling       Multiplier applied to rho when updated
%   AutoRhoScaling   Flag determining whether RhoScaling value is
%                    adaptively determined (see wohlberg-2015-adaptive). If
%                    enabled, RhoScaling specifies a maximum allowed
%                    multiplier instead of a fixed multiplier.
%   RhoRsdlTarget    Residual ratio targeted by auto rho update policy.
%   StdResiduals     Flag determining whether standard residual definitions
%                    (see Sec 3.3 of boyd-2010-distributed) are used instead
%                    of normalised residuals (see wohlberg-2015-adaptive)
%   RelaxParam       Relaxation parameter (see Sec. 3.4.3 of
%                    boyd-2010-distributed)
%   NonNegCoef       Flag indicating whether solution should be forced to
%                    be non-negative
%   NoBndryCross     Flag indicating whether all solution coefficients
%                    corresponding to filters crossing the image boundary
%                    should be forced to zero.
%   AuxVarObj        Flag determining whether objective function is computed
%                    using the auxiliary (split) variable
%   HighMemSolve     Use more memory for a slightly faster solution
%
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2015-12-28
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'License' file distributed with
% the library.

if nargin < 4,
  opt = [];
end
checkopt(opt, defaultopts([]));
opt = defaultopts(opt);

% Set up status display for verbose operation
hstr = 'Itn   Fnc       DFid      l1        r         s      ';
sfms = '%4d %9.2e %9.2e %9.2e %9.2e %9.2e';
nsep = 54;
if opt.AutoRho,
  hstr = [hstr '   rho  '];
  sfms = [sfms ' %9.2e'];
  nsep = nsep + 10;
end
if opt.Verbose && opt.MaxMainIter > 0,
  disp(hstr);
  disp(char('-' * ones(1,nsep)));
end

% Collapsing of trailing singleton dimensions greatly complicates
% handling of both SMV and MMV cases. The simplest approach would be
% if S could always be reshaped to 4d, with dimensions consisting of
% image rows, image cols, a single dimensional placeholder for number
% of filters, and number of measurements, but in the single
% measurement case the third dimension is collapsed so that the array
% is only 3d.
if size(S,3) > 1 && size(S,4) == 1,
  xsz = [size(S,1) size(S,2) size(D,3) size(S,3)];
  % Insert singleton 3rd dimension (for number of filters) so that
  % 4th dimension is number of images in input s volume
  S = reshape(S, [size(S,1) size(S,2) 1 size(S,3)]);
else
  xsz = [size(S,1) size(S,2) size(D,3) size(S,4)];
end

% Start timer
tstart = tic;

% Compute filters in DFT domain
Df = fft2(D, size(S,1), size(S,2));
% Convolve-sum and its Hermitian transpose
Dop = @(x) sum(bsxfun(@times, Df, x), 3);
DHop = @(x) bsxfun(@times, conj(Df), x);
% Compute signal in DFT domain
Sf = fft2(S);
% S convolved with all filters in DFT domain
DSf = DHop(Sf);

% Default lambda is 1/10 times the lambda value beyond which the
% solution is a zero vector
if nargin < 3 | isempty(lambda),
  b = ifft2(DHop(Sf), 'symmetric');
  lambda = 0.1*max(vec(abs(b)));
end

% Set up algorithm parameters and initialise variables
rho = opt.rho;
if isempty(rho), rho = 50*lambda+1; end;
if isempty(opt.RhoRsdlTarget),
  if opt.StdResiduals,
    opt.RhoRsdlTarget = 1;
  else
    opt.RhoRsdlTarget = 1 + (18.3).^(log10(lambda) + 1);
  end
end
if opt.HighMemSolve,
  C = bsxfun(@rdivide, Df, sum(Df.*conj(Df), 3) + rho);
else
  C = [];
end
Nx = prod(xsz);
optinf = struct('itstat', [], 'opt', opt);
r = Inf;
s = Inf;
epri = 0;
edua = 0;

% Initialise main working variables
X = [];
if isempty(opt.Y0),
  Y = zeros(xsz, class(S));
else
  Y = opt.Y0;
end
Yprv = Y;
if isempty(opt.U0),
  if isempty(opt.Y0),
    U = zeros(xsz, class(S));
  else
    U = (lambda/rho)*sign(Y);
  end
else
  U = opt.U0;
end

% Main loop
k = 1;
while k <= opt.MaxMainIter && (r > epri | s > edua),
%while k <= opt.MaxMainIter ,
  % Solve X subproblem
  Xf = solvedbi_sm(Df, rho, DSf + rho*fft2(Y - U), C);
  X = ifft2(Xf, 'symmetric');

  % See pg. 21 of boyd-2010-distributed
  if opt.RelaxParam == 1,
    Xr = X;
  else
    Xr = opt.RelaxParam*X + (1-opt.RelaxParam)*Y;
  end

  % Solve Y subproblem
  Y = shrink(Xr + U, (lambda/rho)*opt.L1Weight);
  if opt.NonNegCoef,
    Y(Y < 0) = 0;
  end
  if opt.NoBndryCross,
    Y((end-size(D,1)+2):end,:,:,:) = 0;
    Y(:,(end-size(D,2)+2):end,:,:) = 0;
  end

  % Update dual variable
  U = U + Xr - Y;

  % Compute data fidelity term in Fourier domain (note normalisation)
  if opt.AuxVarObj,
    Yf = fft2(Y); % This represents unnecessary computational cost
    Jdf = sum(vec(abs(sum(bsxfun(@times,Df,Yf),3)-Sf).^2))/(2*xsz(1)*xsz(2));
    Jl1 = sum(abs(vec(bsxfun(@times, opt.L1Weight, Y))));
  else
    Jdf = sum(vec(abs(sum(bsxfun(@times,Df,Xf),3)-Sf).^2))/(2*xsz(1)*xsz(2));
    Jl1 = sum(abs(vec(bsxfun(@times, opt.L1Weight, X))));
  end
  Jfn = Jdf + lambda*Jl1;

  nX = norm(X(:)); nY = norm(Y(:)); nU = norm(U(:));
  if opt.StdResiduals,
    % See pp. 19-20 of boyd-2010-distributed
    r = norm(vec(X - Y));
    s = norm(vec(rho*(Yprv - Y)));
    epri = sqrt(Nx)*opt.AbsStopTol+max(nX,nY)*opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol+rho*nU*opt.RelStopTol;
  else
    % See wohlberg-2015-adaptive
    r = norm(vec(X - Y))/max(nX,nY);
    s = norm(vec(Yprv - Y))/nU;
    epri = sqrt(Nx)*opt.AbsStopTol/max(nX,nY)+opt.RelStopTol;
    edua = sqrt(Nx)*opt.AbsStopTol/(rho*nU)+opt.RelStopTol;
  end

  % Record and display iteration details
  tk = toc(tstart);
  optinf.itstat = [optinf.itstat; [k Jfn Jdf Jl1 r s epri edua rho tk]];
  if opt.Verbose,
    if opt.AutoRho,
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, r, s, rho));
    else
      disp(sprintf(sfms, k, Jfn, Jdf, Jl1, r, s));
    end
  end

  % See wohlberg-2015-adaptive and pp. 20-21 of boyd-2010-distributed
  if opt.AutoRho,
    if k ~= 1 && mod(k, opt.AutoRhoPeriod) == 0,
      if opt.AutoRhoScaling,
        rhomlt = sqrt(r/(s*opt.RhoRsdlTarget));
        if rhomlt < 1, rhomlt = 1/rhomlt; end
        if rhomlt > opt.RhoScaling, rhomlt = opt.RhoScaling; end
      else
        rhomlt = opt.RhoScaling;
      end
      rsf = 1;
      if r > opt.RhoRsdlTarget*opt.RhoRsdlRatio*s, rsf = rhomlt; end
      if s > (opt.RhoRsdlRatio/opt.RhoRsdlTarget)*r, rsf = 1/rhomlt; end
      rho = rsf*rho;
      U = U/rsf;
      if opt.HighMemSolve && rsf ~= 1,
        C = bsxfun(@rdivide, Df, sum(Df.*conj(Df), 3) + rho);
      end
    end
  end

  Yprv = Y;
  k = k + 1;

end

% Record run time and working variables
optinf.runtime = toc(tstart);
optinf.X = X;
optinf.Xf = Xf;
optinf.Y = Y;
optinf.U = U;
optinf.lambda = lambda;
optinf.rho = rho;

% End status display for verbose operation
if opt.Verbose && opt.MaxMainIter > 0,
  disp(char('-' * ones(1,nsep)));
end

return

function u = vec(v)

  u = v(:);

return

function u = shrink(v, lambda)

  if isscalar(lambda),
    u = sign(v).*max(0, abs(v) - lambda);
  else
    u = sign(v).*max(0, bsxfun(@minus, abs(v), lambda));
  end

return

function opt = defaultopts(opt)

  if ~isfield(opt,'Verbose'),
    opt.Verbose = 0;
  end
  if ~isfield(opt,'MaxMainIter'),
    opt.MaxMainIter = 1000;
  end
  if ~isfield(opt,'AbsStopTol'),
    opt.AbsStopTol = 0;
  end
  if ~isfield(opt,'RelStopTol'),
    opt.RelStopTol = 1e-4;
  end
  if ~isfield(opt,'L1Weight'),
    opt.L1Weight = 1;
  end
  if ~isfield(opt,'Y0'),
    opt.Y0 = [];
  end
  if ~isfield(opt,'U0'),
    opt.U0 = [];
  end
  if ~isfield(opt,'rho'),
    opt.rho = [];
  end
  if ~isfield(opt,'AutoRho'),
    opt.AutoRho = 1;
  end
  if ~isfield(opt,'AutoRhoPeriod'),
    opt.AutoRhoPeriod = 1;
  end
  if ~isfield(opt,'RhoRsdlRatio'),
    opt.RhoRsdlRatio = 1.2;
  end
  if ~isfield(opt,'RhoScaling'),
    opt.RhoScaling = 100;
  end
  if ~isfield(opt,'AutoRhoScaling'),
    opt.AutoRhoScaling = 1;
  end
  if ~isfield(opt,'RhoRsdlTarget'),
    opt.RhoRsdlTarget = [];
  end
  if ~isfield(opt,'StdResiduals'),
    opt.StdResiduals = 0;
  end
  if ~isfield(opt,'RelaxParam'),
    opt.RelaxParam = 1.8;
  end
  if ~isfield(opt,'NonNegCoef'),
    opt.NonNegCoef = 0;
  end
  if ~isfield(opt,'NoBndryCross'),
    opt.NoBndryCross = 0;
  end
  if ~isfield(opt,'AuxVarObj'),
    opt.AuxVarObj = 0;
  end
  if ~isfield(opt,'HighMemSolve'),
    opt.HighMemSolve = 0;
  end

return


function Q = cost_allstim(V,t,tc,Run)
%
% Least-squares cost function for the IL model
%
% INPUT:
% Run = stick function
% tc = time course
% t = vector of time points
% V = parameters
%
% OUTPUT:
% Q = cost
%
% By Martin Lindquist and Tor Wager
% Edited 12/12/06
% Further edited by Christian Waugh 2/15/08 to include multiple trialtypes

numstim = length(Run);
len = length(Run{1});
h = zeros(length(t),numstim);
yhatt =zeros(len,numstim);

for k = 1:numstim
    h(:,k) = Get_Logit(V(k*7-6:k*7),t);            % Get IL model corresponding to parameters V
    yhat(:,k) = conv(Run{k}, h(:,k));              % Convolve IL model with stick function
    yhatt(:,k) = yhat(1:len,k);
end

yhat2 = sum(yhatt,2); %Sum models together to get overall estimate

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

return


function [Results, OPTIONS, HeadModel] = bst_wmne_mosher(HeadModel,OPTIONS)
% BST_WMNE: Compute the whitened and weighted minimum-norm operator (wMNE imaging kernel)
%
% USAGE:  [Results,OPTIONS] = bst_wmne(HeadModel, OPTIONS) : Compute mininum operator
%                   OPTIONS = bst_wmne()                   : Return default options
%
% DESCRIPTION:
%     This program computes the whitened and weighted minimum-norm operator,
%     (the wMNE imaging kernel), which is used to compute whitened
%     and weighted minimum-norm estimates (MNE).
%         (e.g., J=wMNEoperator*B; where B is the unwhitened data).
%     It can also compute the whitened and noise-normalized dynamic
%     statistical parametric mapping (dSPM) inverse operator, and/or the
%     whitened standardized low resolution brain electromagnetic tomography
%     (sLORETA) inverse operator, which are used to compute whitened source
%     activity dSPM and sLORETA maps.
%         (e.g., S_dSPM=dSPMoperator*B; where B is the unwhitened data).
%         (e.g., S_sLORETA=sLORETAoperator*B; where B is the unwhitened data).
%
%     The function was written with the goal of providing some of the same
%     functionality of the MNE software written by Matti Hamalainen, but no
%     guarantees are made that it performs all computations in the same
%     exact way. It also provides some functionalities not available in the
%     MNE software.
%
% INPUTS:
%    - HeadModel: Array of Brainstorm head model structures
%         |- Gain       : Forward field matrix for all the channels (unconstrained source orientations)
%         |- GridOrient : Dipole orientation matrix
%         |- area       : Vector with the areas (or possibly volumes) associated with the vertices of the source space.
%    - OPTIONS: structure
%         |- NoiseCov      : NoiseCov is the noise covariance matrix.
%         |- ChannelTypes  : Type of each channel (for each row of the Leadfield and the NoiseCov matrix)
%         |- InverseMethod : {'wmne', 'dspm', 'sloreta'}
%         |- SourceOrient  : String or a cell array of strings specifying the type of orientation constraints for each HeadModel (default: 'fixed')
%         |- SNR        : Signal-to noise ratio defined as in MNE (default: 3).
%         |- diagnoise  : Flag to discard off-diagonal elements of NoiseCov (assuming heteroscedastic uncorrelated noise) (default: 0)
%         |- loose      : Value that weights the source variances of the dipole components defining the tangent space of the cortical surfaces (default: []).
%         |- depth      : Flag to do depth weighting (default: 1).
%         |- weightexp  : Order of the depth weighting. {0=no, 1=full normalization, default=0.8}
%         |- weightlimit: Maximal amount depth weighting (default: 10).
%         |- magreg     : Amount of regularization of the magnetometer noise covariance matrix
%         |- gradreg    : Amount of regularization of the gradiometer noise covariance matrix.
%         |- eegreg     : Amount of regularization of the EEG noise covariance matrix.
%         |- ecogreg    : Amount of regularization of the ECOG noise covariance matrix.
%         |- seegreg    : Amount of regularization of the SEEG noise covariance matrix.
%         |- fMRI       : Vector of fMRI values are the source points.
%         |- fMRIthresh : fMRI threshold. The source variances of source points with OPTIONS.fMRI smaller
%         |               than fMRIthresh will be multiplied by OPTIONS.fMRIoff.
%         |- fMRIoff    : Weight assigned to non-active source points according to fMRI and fMRIthresh.
%
% OUTPUTS:
%    - Results : structure with the wMNE inverse operator and possibly the dSPM
%                and/or sLORETA inverse operators, and other information:

% NOTES:
%     - More leadfield matrices can be used: the solution will combine all
%       leadfield matrices appropriately.
%
%     - This leadfield structure allows to combine surface and volume
%       source spaces with and without dipole orientation constraints,and
%       with and without area or volumetric current density computations.
%       If using a single sphere headmodel, the silent radial
%       component could be eliminated using the SVD (e.g., use
%       bst_remove_silent.m).
%
%     - NoisCov: This should be computed from the pre-stimulus period for
%       averaged ERF data (e.g., using MNE), or from an empty room recording
%       for unaveraged spontaneous or induced data.
%
%     - Orientation constrains for dipoles (.SourceOrient field)
%         - "fixed"     : Dipoles constrained to point normal to cortical surfaces
%         - "free"      : No constraints, dipoles point in x,y,z directions
%         - "loose"     : Source variances of dipole components pointing tangentially to the cortical surfaces are multipled by OPTIONS.loose
%         - "truncated" : An SVD of the gain matrix for each source point is
%                         used to remove the dipole component with least variance, which for
%                         the Single Sphere Head Model, corresponds to the radialsilent component).
%       => For dealing with multiple source spaces with different types of orientation constraints use for example,
%          OPTION.SourceOrient{1}='fixed';
%          OPTION.SourceOrient{2}='free';
%          This has to correspond with the HeadModel Structure
%
%     - The output HeadModel param is used here in return to save LOTS of memory in the bst_wmne function,
%       event if it seems to be absolutely useless. Having a parameter in both input and output have the
%       effect in Matlab of passing them "by referece". So please, do NOT remove it from the function description
%
%     - sLORETA: Output values are multiplied by 1e12 for display in Brainstorm (time series and cortical maps).

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Copyright (C) 2010 - Rey Rene Ramirez
%
% Authors:  Rey Rene Ramirez, Ph.D.   e-mail: rrramirez at mcw.edu
%           Francois Tadel, 2010-2013
%           John Mosher, 2013

%% ===== DEFINE DEFAULT OPTIONS =====
Def_OPTIONS.NoiseCov    = [];
Def_OPTIONS.InverseMethod = 'wmne';
Def_OPTIONS.SNR         = 3;
Def_OPTIONS.diagnoise   = 0;
%Def_OPTIONS.SourceOrient= {'free'};
Def_OPTIONS.SourceOrient= {'fixed'};
Def_OPTIONS.loose       = 0.2;
Def_OPTIONS.depth       = 1;
Def_OPTIONS.weightexp   = 0.5;
Def_OPTIONS.weightlimit = 10;
Def_OPTIONS.regnoise    = 1;
Def_OPTIONS.magreg      = .1;
Def_OPTIONS.gradreg     = .1;
Def_OPTIONS.eegreg      = .1;
Def_OPTIONS.ecogreg     = .1;
Def_OPTIONS.seegreg     = .1;
Def_OPTIONS.fMRI        = [];
Def_OPTIONS.fMRIthresh  = [];
Def_OPTIONS.fMRIoff     = 0.1;
Def_OPTIONS.pca         = 1;
% Return the default options
if (nargin == 0)
    Results = Def_OPTIONS;
    return
end
% Make the default for all the leadfields
numL = size(HeadModel,2);
Def_OPTIONS.SourceOrient = repmat(Def_OPTIONS.SourceOrient, [1 numL]);
% Copy default options to OPTIONS structure (do not replace defined values)
OPTIONS = struct_copy_fields(OPTIONS, Def_OPTIONS, 0);

%% ===== CHECK FOR INVALID VALUES =====
disp(' ');
% Detect if the input noise covariance matrix is or should be diagonal
C_noise = OPTIONS.NoiseCov;
variances = diag(C_noise);
if isequal(C_noise, diag(variances))
    OPTIONS.diagnoise = 1;
    disp('wMNE> Detected diagonal noise covariance: setting diagnoise to 1');
end
% If OPTIONS.diagnoise is 1, then OPTIONS.pca=0
if OPTIONS.diagnoise
    OPTIONS.pca=0;
    disp('wMNE> If using diagonal noise covariance, PCA option should be off. Setting PCA option off.')
end
if isempty(OPTIONS.NoiseCov)
    error('You need to input the noise covariance in the NoiseCov field of OPTIONS.')
end
if (numL ~= length(OPTIONS.SourceOrient))
    error('The number of elements in the HeadModel structure should equal the length of the cell array OPTIONS.SourceOrient.')
end
if ~isempty(OPTIONS.loose) && (OPTIONS.loose>=1 || OPTIONS.loose<=0)
    error('loose value should be smaller than 1 and bigger than 0, or empty for no loose orientations.')
end
if OPTIONS.weightexp>1 || OPTIONS.weightexp<0
    error('weightexp should be a scalar between 0 and 1')
end
if OPTIONS.magreg>1 || OPTIONS.magreg<0
    error('magreg should be a scalar between 0 and 1')
end
if OPTIONS.eegreg>1 || OPTIONS.eegreg<0
    error('eegreg should be a scalar between 0 and 1')
end
if OPTIONS.ecogreg>1 || OPTIONS.ecogreg<0
    error('ecogreg should be a scalar between 0 and 1')
end
if OPTIONS.seegreg>1 || OPTIONS.seegreg<0
    error('seegreg should be a scalar between 0 and 1')
end

%% ===== NOISE COVARIANCE RANK =====
% Get indices of MEG and EEG channels
iMeg  = find(strncmpi(OPTIONS.ChannelTypes,'MEG',3));
iEeg  = find(strncmpi(OPTIONS.ChannelTypes,'EEG',3));
iEcog = find(strncmpi(OPTIONS.ChannelTypes,'ECOG',3));
iSeeg = find(strncmpi(OPTIONS.ChannelTypes,'SEEG',3));
% Diagonal noisecov
if OPTIONS.diagnoise
    C_noise = diag(variances);
    rnkC_noise_meg = length(iMeg);
    rnkC_noise_eeg = length(iEeg);
    rnkC_noise_ecog = length(iEcog);
    rnkC_noise_seeg = length(iSeeg);
    disp('wMNE> Setting off diagonal elements of the noise covariance to zero.');
    disp(['wMNE> Rank of noise covariance is ' num2str(size(C_noise,1))]);
% Full noisecov
else
    % Estimate noise covariance matrix rank separately for sensor types
    if ~isempty(iMeg)
        rnkC_noise_meg = rank(single(C_noise(iMeg,iMeg))); % Rey added this. Separate rank of MEG. 3/23/11
        disp(['wMNE> Rank of MEG part of noise covariance is ' num2str(rnkC_noise_meg)]);
    end
    if ~isempty(iEeg)
        rnkC_noise_eeg = rank(single(C_noise(iEeg,iEeg))); % Rey added this. Separate rank of EEG. 3/23/11
        disp(['wMNE> Rank of EEG part of noise covariance is ' num2str(rnkC_noise_eeg)]);
    end
    if ~isempty(iEcog)
        rnkC_noise_ecog = rank(single(C_noise(iEcog,iEcog)));  % FT added 21-Feb-13
        disp(['wMNE> Rank of ECOG part of noise covariance is ' num2str(rnkC_noise_ecog)]);
    end
    if ~isempty(iSeeg)
        rnkC_noise_seeg = rank(single(C_noise(iSeeg,iSeeg)));  % FT added 21-Feb-13
        disp(['wMNE> Rank of SEEG part of noise covariance is ' num2str(rnkC_noise_seeg)]);
    end
    % Sets off-diagonal terms to zero. Rey added this. 3/23/11
    C_noise_new = 0 * C_noise;
    if ~isempty(iMeg)
        C_noise_new(iMeg,iMeg) = C_noise(iMeg,iMeg);
    end
    if ~isempty(iEeg)
        C_noise_new(iEeg,iEeg) = C_noise(iEeg,iEeg);
    end
    if ~isempty(iEcog)
        C_noise_new(iEcog,iEcog) = C_noise(iEcog,iEcog);
    end
    if ~isempty(iSeeg)
        C_noise_new(iSeeg,iSeeg) = C_noise(iSeeg,iSeeg);
    end
    C_noise = C_noise_new;
end

%% ===== REGULARIZE NOISE COVARIANCE MATRIX =====
% Only if option is selected
if OPTIONS.regnoise
    listTypes = unique(OPTIONS.ChannelTypes);
    % Loop on all the required data types (MEG MAG, MEG GRAD, EEG)
    for iType = 1:length(listTypes)
        % Get channel indices
        iChan = find(strcmpi(OPTIONS.ChannelTypes, listTypes{iType}));
        % Regularize noise covariance matrix
        switch listTypes{iType}
            case 'MEG GRAD', reg = OPTIONS.gradreg;
            case 'MEG MAG',  reg = OPTIONS.magreg;
            case 'MEG',      reg = OPTIONS.gradreg;
            case 'EEG',      reg = OPTIONS.eegreg;
            case 'ECOG',     reg = OPTIONS.ecogreg;
            case 'SEEG',     reg = OPTIONS.seegreg;
        end
        % Original Line 4/5/13:
        % C_noise(iChan,iChan) = C_noise(iChan,iChan) + (reg * mean(variances(iChan)) * eye(length(iChan)));
        % JCM 4/5/13, mods just to be clear
        % mean of the diagonal variances
        % Options could be median, maximum, minimum, etc. Note, this is
        % not the eigenspectrum, but the homoskedastic spectrum.
        % TODO try other forms of matrix norms for noise regualarization
        LAMBDA_REGULARIZER = reg * mean(variances(iChan));
        % Now add this Tikhonov regularizer to the noise diagonal.
        C_noise(iChan,iChan) = C_noise(iChan,iChan) + diag(zeros(length(iChan),1) + LAMBDA_REGULARIZER);

    end
end

%% ===== WHITENING OPERATOR =====
% Rey added all of this, 3/23/11
% Modified FT 21-Feb-2013
% Whitening of each modality separately (MEG,EEG,ECOG,SEEG), which assumes
% zero covariance between them (i.e., a block diagonal noise covariance). This
% was recommended by Matti as EEG does not measure all the signals from the same
% environmental noise sources as MEG.
nChan = size(C_noise,2);
W = zeros(0, nChan);
if ~isempty(iMeg)
    W_meg = CalculateWhitener('MEG', C_noise, iMeg, rnkC_noise_meg, OPTIONS.pca);
    W_tmp = zeros(size(W_meg,1), nChan);
    W_tmp(:,iMeg) = W_meg;
    W = [W; W_tmp];
end
if ~isempty(iEeg)
    W_eeg = CalculateWhitener('EEG', C_noise, iEeg, rnkC_noise_eeg, OPTIONS.pca);
    W_tmp = zeros(size(W_eeg,1), nChan);
    W_tmp(:,iEeg) = W_eeg;
    W = [W; W_tmp];
end
if ~isempty(iEcog)
    W_ecog = CalculateWhitener('ECOG', C_noise, iEcog, rnkC_noise_ecog, OPTIONS.pca);
    W_tmp = zeros(size(W_ecog,1), nChan);
    W_tmp(:,iEcog) = W_ecog;
    W = [W; W_tmp];
end
if ~isempty(iSeeg)
    W_seeg = CalculateWhitener('SEEG', C_noise, iSeeg, rnkC_noise_seeg, OPTIONS.pca);
    W_tmp = zeros(size(W_seeg,1), nChan);
    W_tmp(:,iSeeg) = W_seeg;
    W = [W; W_tmp];
end
% Check for whitener integrity
if any(isnan(W(:))) || any(isinf(W(:)))
    error('Invalid noise covariance matrix.')
end
% Display rank of the whitener
rnkC_noise = size(W,1);
display(['wMNE> Total rank is ' num2str(rnkC_noise) '.'])

%% ===== PROCESSING LEAD FIELD MATRICES, WEIGHTS, AREAS & ORIENTATIONS =====
% Initializing.
spl = zeros(numL,1);
numdipcomp = spl;
for k = 1:numL
    sL = size(HeadModel(k).Gain, 2);
    switch OPTIONS.SourceOrient{k}
        case 'fixed',      numdipcomp(k)=1;
        case 'free',       numdipcomp(k)=3;
        case 'loose',      numdipcomp(k)=3;
        case 'truncated',  numdipcomp(k)=2;
    end
    spl(k) = (sL / 3) * numdipcomp(k); % This is a vector with the total number of dipole components per source space.
end
sspl = sum(spl); % This is the total number of dipole components across all source spaces.
L = zeros(rnkC_noise,sspl);
w = ones(sspl,1);
if isfield(HeadModel, 'area')
    areas = w;
else
    areas = [];
end
itangential = [];
start = 0;
Q_Cortex = [];
for k = 1:numL
    start = start + 1;
    endd = start + spl(k) - 1;
    Lk = HeadModel(k).Gain;
    HeadModel(k).Gain = [];
    %% ===== COMPUTE POWER =====
    szL = size(Lk);
    if OPTIONS.depth
        display(['wMNE> Computing power of gain matrices at each source point for source space ' num2str(k) '.'])
        % Computing power
        % JCM 4/5/2013, this is squared Frobenius norm of the source
        % Options would be the matrix norm (largest singular value, or
        % squared).
        wk = squeeze(sum(sum((reshape(Lk,[szL(1) 3 szL(2)/3])) .^2,1),2));
        wk = repmat(wk',[numdipcomp(k) 1]);
        wk = reshape(wk,[spl(k) 1]);
        w(start:endd) = wk;
        clear wk
    end
    switch OPTIONS.SourceOrient{k}
        case 'fixed'
            display('wMNE> Appying fixed dipole orientations.')
            Lk = bst_gain_orient(Lk,HeadModel(k).GridOrient);
        case 'free'
            display('wMNE> Using free dipole orientations. No constraints.')
        case 'loose'
            display('wMNE> Transforming lead field matrix to cortical coordinate system.')
            [Lk, Q_Cortex] = bst_xyz2lf(Lk, HeadModel(k).GridOrient');
            % Getting indices for tangential dipoles.
            itangentialtmp = start:endd;
            itangentialtmp(1:3:end) = [];
            itangential = [itangential itangentialtmp];  %#ok<AGROW>
        case 'truncated'
            display('wMNE> Truncating the dipole component pointing in the direction with least variance (i.e., silent component for single sphere head model.')
            [Lk, Q_Cortex] = bst_remove_silent(Lk);
    end
    %% ===== WHITEN LEAD FIELD MATRIX =====
    % Whiten lead field.
    display(['wMNE> Whitening lead field matrix for source space ' num2str(k) '.'])
    Lk = W * Lk;
    if isfield(HeadModel(k),'area') && ~isempty(HeadModel(k).area)
        areav = HeadModel(k).area;
        areav = repmat(areav', [numdipcomp(k) 1]);
        areav = reshape(areav, [spl(k) 1]);
        areas(start:endd) = areav;
    end
    L(:,start:endd) = Lk;
    start = endd;
end
% Computing reciprocal of power.
w = 1 ./ w;
% Clear memory
clear Lk endd start itangentialtmp sL szL

%% ===== APPLY AREAS =====
if ~isempty(areas)
    display('wMNE> Applying areas to compute current source density.')
    areas = areas.^2;
    w = w .* areas;
end
clear areas

%% ===== APPLY DEPTH WEIGHTHING =====
if OPTIONS.depth
    % ===== APPLY WEIGHT LIMIT =====
    % Applying weight limit.
    display('wMNE> Applying weight limit.')
    weightlimit2 = OPTIONS.weightlimit .^ 2;
    %limit=min(w(w>min(w)*weightlimit2));  % This is the Matti way.
    limit = min(w) * weightlimit2;  % This is the Rey way (robust to possible weight discontinuity).
    w(w>limit) = limit; %JCM note, 4/5/2013, w = min(w,limit);

    % ===== APPLY WEIGHT EXPONENT =====
    % Applying weight exponent.
    display('wMNE> Applying weight exponent.')
    w = w .^ OPTIONS.weightexp;
    clear limit weightlimit2
end

%% ===== APPLY LOOSE ORIENTATIONS =====
if ~isempty(itangential)
    display(['wMNE> Applying loose dipole orientations. Loose value of ' num2str(OPTIONS.loose) '.'])
    w(itangential) = w(itangential) * (OPTIONS.loose);
end

%% ===== APPLY fMRI PRIORS =====
% Apply fMRI Priors
if ~isempty(OPTIONS.fMRI)
    display('wMNE> Applying fMRI priors.')
    ifmri = (OPTIONS.fMRI < OPTIONS.fMRIthresh);
    w(ifmri) = w(ifmri) * OPTIONS.fMRIoff;
end

%% ===== ADJUSTING SOURCE COVARIANCE MATRIX =====
% Adjusting Source Covariance matrix to make trace of L*C_J*L' equal to number of sensors.
% JCM 4/5/2013, i.e. average signal covariance is one, so trace is number
% of sensors.
display('wMNE> Adjusting source covariance matrix.')
C_J = speye(sspl, sspl);
C_J = spdiags(w, 0, C_J);
trclcl = trace(L * C_J * L');
C_J = C_J * (rnkC_noise / trclcl);
Rc = chol(C_J);
LW = L * Rc;
clear C_J trclcl sspl itangential rnkC_noise

%% BEGIN SOURCE MODELING ROUTINES

%% JCM 4/5/2013 GLS and other routines, using above setup

if any(strcmpi(OPTIONS.InverseMethod, {'gls','gls_p','glsr','glsr_p'})),

   start = 0;
   Kernel = zeros(size(LW,2),size(LW,1)); % note transpose of LW
   lambda2 = OPTIONS.SNR^(-2); % for regularizing the GLS

   for k = 1:numL,
      start = start + 1;
      endd = start + spl(k) - 1;
      Lk = LW(:,start:endd); % this whitened gain matrix
      Rck = Rc(start:endd,start:endd); % covariance priors
      % Now process each source in the gain matrix for it's own inversion
      NumSources = size(Lk,2)/numdipcomp(k); % total number of sources

      for i = 1:NumSources,
         ndx = ((1-numdipcomp(k)):0)+i*numdipcomp(k); % next index
         % SVD the source
         [Ua,Sa,Va] = svd(Lk(:,ndx),0); % svd of this source
         Sa = diag(Sa);
         Tolerance_Source = size(Lk,1)*eps(single(Sa(1)));
         Rank_Source = sum(Sa > Tolerance_Source);
         % Trim decomposition
         Ua = Ua(:,1:Rank_Source);
         Sa = Sa(1:Rank_Source);
         Va = Va(:,1:Rank_Source);

         % calculate the weighted subspace
         Reg_Weights_Source = Sa.^2 ./ (Sa.^2 + lambda2*Sa(1)^2); % regularizer
         % now write the pseudoinverse results back into this same matrix

         switch OPTIONS.InverseMethod
            case 'gls'
               % The true pseudo-inverse
               Lk(:,ndx) = Ua * diag(1./Sa) * Va'*Rck(ndx,ndx);
            case 'gls_p'
               % The model performance
               % Model may be reduced rank
               Lk(:,ndx) = 0; % zero out
               Lk(:,ndx(1:size(Ua,2))) = Ua; % model performance
            case 'glsr'
               % Regularized
               Lk(:,ndx) = Ua * diag(Reg_Weights_Source./Sa) * Va'*Rck(ndx,ndx);
            case 'glsr_p'
               % Regularized model performance
               % Model may be reduced rank
               Lk(:,ndx) = 0; % zero out
               Lk(:,ndx(1:size(Ua,2))) = Ua * diag(sqrt(Reg_Weights_Source)); % model performance
            otherwise
               error('Bad Options String %s',OPTIONS.InverseMethod)
         end
      end

      % Now we have a matrix almost ready for use as an imaging kernel
      % Later, below, the whitener will be added

      Kernel(start:endd,:) = Lk';
      start = endd;
   end

end % JCM GLS routine

if any(strcmpi(OPTIONS.InverseMethod, {'mnej','mnej_p'})), %JCM Min Norm
   % mnej should be identical to Rey, but mnej_p is novel

   start = 0;
   Kernel = zeros(size(LW,2),size(LW,1)); % note transpose of LW
   lambda2 = OPTIONS.SNR^(-2); % for regularizing the MN

   for k = 1:numL,
      start = start + 1;
      endd = start + spl(k) - 1;
      Lk = LW(:,start:endd); % this whitened gain matrix
      Rck = Rc(start:endd,start:endd); % covariance priors

      % Setup the Min Norm
      % First, generate the population data covariance
      wCD = LW*LW' + (diag(zeros(size(LW,1),1) + lambda2)); % whitened data covariance

      % Decompose
      [Ud,Sd] = svd(wCD);

      % Data Whitener
      iWd = Ud*diag(1./sqrt(diag(Sd)))*Ud';

      % Now process each source in the gain matrix for it's own inversion
      NumSources = size(Lk,2)/numdipcomp(k); % total number of sources

      for i = 1:NumSources,
         ndx = ((1-numdipcomp(k)):0)+i*numdipcomp(k); % next index
         % SVD the data whitened source
         [Ua,Sa,Va] = svd(iWd*Lk(:,ndx),0); % svd of this source
         Sa = diag(Sa);
         Tolerance_Source = size(Lk,1)*eps(single(Sa(1)));
         Rank_Source = sum(Sa > Tolerance_Source);
         if Rank_Source < length(Sa),
            fprintf('%.0f ',i);
         end
         % Trim decomposition
         Ua = Ua(:,1:Rank_Source);
         Sa = Sa(1:Rank_Source);
         Va = Va(:,1:Rank_Source);

         SNR_Weights_Source = Sa.^2 ./ (1 - Sa.^2); % SNR
         % now write the pseudoinverse results back into this same matrix

         switch OPTIONS.InverseMethod
            case 'mnej'
               % The true solution
               Lk(:,ndx) = iWd * Ua * diag(Sa) * Va' * Rck(ndx,ndx);
            case 'mnej_p'
               % The model performance
               %Lk(:,ndx) = iWd * Ua * diag(sqrt(SNR_Weights_Source)); % model performance
               Lk(:,ndx) = iWd * Ua; % CHEAT model performance

            otherwise
               error('Bad Options String %s',OPTIONS.InverseMethod)
         end
      end

      % Now we have a matrix almost ready for use as an imaging kernel
      % Later, below, the whitener will be added

      Kernel(start:endd,:) = Lk';
      start = endd;
   end

end %JCM Min norms

%% The dSPM and sLORETA functions rely on 'wmne' being calculated

if any(strcmpi(OPTIONS.InverseMethod, {'wmne','dspm','sloreta'}))

   %% ===== SINGULAR VALUE DECOMPOSITION =====
   % Set regularization parameter based on SNR
   lambda2 = OPTIONS.SNR ^ (-2);
   % Compute SVD.
   display('wMNE> Computing SVD of whitened and weighted lead field matrix.')
   [V,S,U] = svd(LW','econ'); % JCM 4/5/2013 transpose for greater speed
   s = diag(S);
   ss = s ./ (s.^2 + lambda2);
   clear LW lambda2 s

   %% ===== WHITENED MNE IMAGING KERNEL =====
   % Compute whitened MNE operator.
   Kernel = Rc * V * diag(ss) * U';
   clear Rc V ss U
end

%% ===== WHITENED dSPM IMAGING KERNEL =====
% Compute dSPM operator.
if strcmpi(OPTIONS.InverseMethod, 'dspm')
   display('wMNE> Computing dSPM inverse operator.')
   start = 0;
   for k = 1:numL
      start = start+1;
      endd = start + spl(k) - 1;
      dspmdiag = sum(Kernel(start:endd,:) .^2, 2);
      if (numdipcomp(k) == 1)
         dspmdiag = sqrt(dspmdiag);
      elseif (numdipcomp(k)==3 || numdipcomp(k)==2)
         dspmdiag = reshape(dspmdiag, [numdipcomp(k), spl(k)/numdipcomp(k)]);
         dspmdiag = sqrt(sum(dspmdiag)); % Taking trace and sqrt.
         dspmdiag = repmat(dspmdiag, [numdipcomp(k), 1]);
         dspmdiag = reshape(dspmdiag, [spl(k), 1]);
      end
      Kernel(start:endd,:) = bst_bsxfun(@rdivide, Kernel(start:endd,:), dspmdiag);
      start = endd;
   end

   %% ===== WHITENED sLORETA IMAGING KERNEL =====
   % Compute sLORETA operator.
elseif strcmpi(OPTIONS.InverseMethod, 'sloreta')
   display('wMNE> Computing sLORETA inverse operator.')
   start=0;
   for k = 1:numL
      start = start + 1;
      endd = start + spl(k) - 1;
      if (numdipcomp(k) == 1)
         sloretadiag = sqrt(sum(Kernel(start:endd,:) .* L(:,start:endd)', 2));
         Kernel(start:endd,:) = bst_bsxfun(@rdivide, Kernel(start:endd,:), sloretadiag);
      elseif (numdipcomp(k)==3 || numdipcomp(k)==2)
         for spoint = start:numdipcomp(k):endd
            R = Kernel(spoint:spoint+numdipcomp(k)-1,:) * L(:,spoint:spoint+numdipcomp(k)-1);
            SIR = sqrtm(pinv(R));
            Kernel(spoint:spoint+numdipcomp(k)-1,:) = SIR * Kernel(spoint:spoint+numdipcomp(k)-1,:);
         end
      end
      start=endd;
   end
end
disp(' ');

%% JCM, 4/5/2013, this loose orientation may be compatible with GLS and others

%% ===== LOOSE ORIENTATION: RE-ORIENT COMPONENTS =====
% WARNING: Changing the orientations of the dipoles from MNE to Brainstorm
% => Make "Unconstrained" and "loose"/"truncated" models directly comparable
if ~isempty(Q_Cortex)
   % Creating a block diagonal matrix
   N = size(Kernel,1);
   Nout = N / numdipcomp * 3;
   iRow = reshape(repmat(reshape(1:Nout,3,[]), numdipcomp, 1), 1, []);
   iCol = reshape(repmat(1:N,3,[]), 1, []);
   Q_Cortex = sparse(iRow, iCol, Q_Cortex(:));
   % Applying orientations
   Kernel = Q_Cortex * Kernel;
end

%% ===== ASSIGN IMAGING KERNEL =====
% Multiply inverse operator and whitening matrix, so no need to whiten data.
Kernel = Kernel * W;
% Return results structure
Results.ImagingKernel = Kernel;
Results.ImageGridAmp  = [];
Results.Whitener      = W;
if (length(numdipcomp) > 1)
    Results.nComponents = 0;
else
    Results.nComponents = numdipcomp;
end

end

%% ==============================================================================
%  ===== HELPER FUNCTIONS =======================================================
%  ==============================================================================
% TODO JCM 4/5/2013: Cleanup the whitener to use SVD, reduced rank. isPCA
% is actually for truncated SVD of the matrix for small values.
function W = CalculateWhitener(Modality, C_noise, iChannel, rnkC_noise, isPca)
    [V,D] = eig(C_noise(iChannel,iChannel));
    D = diag(D);
    [D,I] = sort(D,'descend');
    V = V(:,I);
    % No PCA case.
    if ~isPca
        display(['wMNE> Not doing PCA for ' Modality '.'])
        D = 1./D;
        W = diag(sqrt(D)) * V';
    % Rey's approach. MNE has been changed to implement this.
    else
        display(['wMNE> Setting small ' Modality ' eigenvalues to zero.'])
        D = 1 ./ D;
        D(rnkC_noise+1:end) = 0;
        W = diag(sqrt(D)) * V';
        W = W(1:rnkC_noise,:); % This line will reduce the actual number of variables in data and leadfield to the true rank. This was not done in the original MNE C code.
    end
end


function [varargout]=mat2wfdb(varargin)
%
% [xbit]=mat2wfdb(X,fname,Fs,bit_res,adu,info,gain,sg_name,baseline,isquant, isdigital)
%
% Convert data from a matlab array into Physionet WFDB format file.
%
% Input Paramater are:
%
% X       -(required)  NxM matrix of M signals with N samples each. The
%                      signals can be of type double.The signals are assumed to be
%                      in physical units already and will be converted to
%                      ADU.
% fname   -(required)  String where the the header (*.hea) and data (*.dat)
%          files will be saved (one single name for both, with no sufix).
% Fs      -(Optional)  1x1 sampling frequency in Hz (all signals must have
%          been sampled at the same frquency). Default is 1 Hz.
% bit_res -(Optional)  1xM (or Mx1):scalar determining the bit depth of the conversion for
%                      each signal.
%                      1x1 : If all the signals should have the same bit depth
%          Options are: 8,  16, and 32 ( all are signed types). 16 is the default.
% adu     -(Optional) Describes the physical units (default is 'mV').
%          Three input formats:
%            - String delimited by forward slashes (e.g. 'V/mV/mmHg'), with
%            M-1 slash characters
%            - Single string (e.g. 'V'), in which case all signals will
%            have the same physical units.
%            - Cell array of strings, where the total units entered has to equal M
%            (number of channels).
% info    -(Optional)  String that will be added to the comment section of the header file.
%           For multi-lined comments, use a cell array of strings. Each
%           cell will be output on a new line. Note that comments in the
%           header file are automatically prefixed with a pound symbol (#)
% gain    -(Required for digital only) Scalar or Mx1 array of floats indicating the difference in sample values
%           that would be observed if a step of one physical unit occurred in the original
%           analog signal. If the 'isdigital' field is 1, this field is mandatory. Otherwise,
%           this field is ignored if present.
% baseline -(Required for digital only) Mx1 array of integers that specifies the sample value for each channel
%           corresponding to 0 physical units. Not to be confused with 'ADC zero' which
%           is currently always taken and written as 0 in this function. If
%           the 'isdigital' field is 1, this field is mandatory. Otherwise,
%           this field is ignored if present.
% sg_name -(Optional) Cell array of strings describing signal names.
%
% isquant   -(Optional) Logical value (default=0). Use this option if the
%           input signal is already quantitized and you want to remove round-off
%           error by mapping the original values to integers prior to fixed
%           point conversion. This field is only used for input physical
%           signals. If 'isdigital' is set to 1, this field is ignored.
%
% isdigital -(Optional) Logical value (default=0). Specifies whether the input signal is
%            digital or physical (default). If it is digital, the signal values will be
%            directly written to the file without scaling. If the signal is physical,
%            the optimal gain and baseline will be calculated and used to digitize the signal
%            to write the WFDB file. This flag also decides the allowed
%            input combinations of the 'gain' and 'baseline' fields.
%            Digital signals must have both, and physical signals must have
%            neither (as the ideal values will be automatically calculated).
%
% Ouput Parameter:
%
% xbit    -(Optional)  NxM the quantitized signals that written to file (possible
%          rescaled if no gain was provided at input). Useful for comparing
%          and estimating quatitization error with the input double signal X
%          (see examples below).
%
%
%  NOTE: The signals can have different amplitudes, they will all be scaled to
%  a reference gain, with the scaling factor saved in the *.hea file.
%
%Written by Ikaro Silva 2010
%Modified by Louis Mayaud 2011, Alistair Johson 2016
% Version 1.0
%
% Since 0.0.1
% See also wrsamp, wfdbdesc
%
%%%%%%%%%%  Example 1 %%%%%%%%%%%%
%
% display('***This example will write a  Ex1.dat and Ex1.hea file to your current directory!')
% s=input('Hit "ctrl + c" to quit or "Enter" to continue!');
%
% %Generate 3 different signals and convert them to signed 16 bit in WFDB format
% clear all;clc;close all
% N=1024;
% Fs=48000;
% tm=[0:1/Fs:(N-1)/Fs]';
% adu='V/mV/V';
% info='Example 1';
%
%
% %First signal a ramp with 2^16 unique levels and is set to (+-) 2^15 (Volts)
% %Thus the header file should have one quant step equal to (2^15-(-2^15))/(2^16) V.
% sig1=double(int16(linspace(-2^15,2^15,N)'));
%
% %Second signal is a sine wave with 2^8 unique levels and set to (+-) 1 (mV)
% %Thus the header file should one quant step equal a (1--1)/(2^8)  adu step
% sig2=double(int8(sin(2*pi*tm*1000).*(2^7)))./(2^7);
%
% %Third signal is a random binary signal set to to (+-) 1 (V) with DC (to be discarded)
% %Thus the header file should have one quant step equal a 1/(2^15) adu step.
% sig3=(rand(N,1) > 0.97)*2 -1 + 2^16;
%
% %Concatenate all signals and convert to WFDB format with default 16 bits (empty brackets)
% sig=[sig1 sig2 sig3];
% mat2wfdb(sig,'Ex1',Fs,[],adu,info)
%
% % %NOTE: If you have WFDB installed you can check the conversion by
% % %uncomenting and this section and running (notice that all signals are scaled
% % %to unit amplitude during conversion, with the header files keeping the gain info):
%
% % !rdsamp -r Ex1 > foo
% % x=dlmread('foo');
% % subplot(211)
% % plot(sig)
% % subplot(212)
% % plot(x(:,1),x(:,2));hold on;plot(x(:,1),x(:,3),'k');plot(x(:,1),x(:,4),'r')
%
%%%%%%%% End of Example 1%%%%%%%%%

%endOfHelp
machine_format='l'; % all wfdb formats are little endian except fmt 61 which this function does not support. Do NOT change this.
skip=0;

% Set default parameters
params={'x','fname','Fs','bit_res','adu','info','gain','sg_name','baseline','isquant', 'isdigital'};
Fs=1;
adu=[];
info=[];
isquant=0;
isdigital=0;
%Use cell array for baseline and gain in case of empty conditions
baseline=[];
gain=[];
sg_name=[];
x=[];
fname=[];
%Used to convert signal from double to appropiate type
bit_res = 16 ;
bit_res_suport=[8 16 32];

for i=1:nargin
    if(~isempty(varargin{i}))
        eval([params{i} '= varargin{i};']);
    end
end

disp(isdigital);
% Check valid gain and baseline combinations depending on whether the input is digital or physical.
if isdigital % digital input signal
    if (isempty(gain) || isempty(baseline))
        error('Input digital signals are directly written to files without scaling. Must also input gain and baseline for correct interpretation of written file.');
    end
    if (~isempty(find(baseline>2147483647))||~isempty(find(baseline<-2147483648))) % baseline stored as int in wfdb library.
        error('Baseline field must lie between 2^-31 and 2^31-1 for this WFDB version'); % Prevent bit overflow
    end
else % physical input signal
    if ( ~isempty(gain) || ~isempty(baseline)) % User inputs gain or baseline to map the physical to digital values.
        % Sorry, we cannot trust that they did it correctly...
        warning('Input gain and baseline fields ignored for physical input signal. This function automatically calculates and applies the ideal values');
    end
end

switch bit_res % Write formats.
    case 8
        fmt='80';
    case 16
        fmt='16';
    case 32
        fmt='32';
end

[N,M]=size(x);

if isempty(adu) % default unit: 'mV'
    adu=repmat({'mV'},[M 1]);
elseif iscell(adu)
    % adu directly input as a cell array of strings
elseif ischar(adu)
    if ~isempty(strfind(adu,'/'))
        adu=regexp(adu,'/','split');
    else
        adu = repmat({adu},[M,1]);
    end
end

% ensure we have the right number of units
if numel(adu) ~= M
    error('adu:wrongNumberOfElements','adu cell array has incorrect number of elements');
end

if(isempty(gain))
    gain=cell(M,1); %Generate empty cells as default
elseif(length(gain)==1)
    gain=repmat(gain,[M 1]);
end
% ensure gain is a cell array
if isnumeric(gain)
    gain=num2cell(gain);
end

if(isempty(sg_name))
    sg_name=repmat({''},[M 1]);
end
if ~isempty(setdiff(bit_res,bit_res_suport))
    error(['Bit res should be one of: ' num2str(bit_res_suport)]);
end
if(isempty(baseline))
    baseline=cell(M,1); %Generate empty cells as default
elseif(length(baseline)==1)
    baseline=repmat(baseline,[M 1]);
end
% ensure baseline is a cell array
if isnumeric(baseline)
    baseline=num2cell(baseline);
end

if isempty(isquant)
    isquant = zeros(M,1);
elseif numel(isquant)==1
    isquant = repmat(isquant,[M,1]);
elseif numel(isquant)~=M
    error('isquant:wrongNumberOfElements','isquant  array has incorrect number of elements');
end

%Head record specification line
head_str=cell(M+1,1);
head_str(1)={[fname ' ' num2str(M) ' ' num2str(Fs) ' ' num2str(N)]};

switch bit_res % Allocate space for digital signals
    case 8
        y=uint8(zeros(N,M));
    case 16
        y=int16(zeros(N,M));
    case 32
        y=int32(zeros(N,M));
end

%Loop through all signals, digitizing them and generating lines in header file
for m=1:M
    nameArray = regexp(fname,'/','split');
    if ~isempty(nameArray)
        fname = nameArray{end};
    end

    [tmp_bit1,bit_gain,baseline_tmp,ck_sum]=quant(x(:,m), ...
        bit_res, gain{m}, baseline{m}, isquant(m), isdigital);

    y(:,m)=tmp_bit1;

    % Header file signal specification lines
    % Should we specify precision of num2str(gain)?
    head_str(m+1)={[fname '.dat ' fmt ' ' num2str(bit_gain) '(' ...
        num2str(baseline_tmp) ')/' adu{m} ' ' num2str(bit_res) ' 0 ' num2str(tmp_bit1(1)) ' ' num2str(ck_sum) ' 0 ' sg_name{m}]};
end
if(length(y)<1)
    error(['Converted data is empty. Exiting without saving file...']);
end

%Write *.dat file
fid = fopen([fname '.dat'],'wb',machine_format);
if(~fid)
    error(['Could not create data file for writing: ' fname]);
end

if (bit_res==8)
    count=fwrite(fid, y','uint8',skip,machine_format);
else
    count=fwrite(fid, y',['int' num2str(bit_res)],skip,machine_format);
end

if(~count)
    fclose(fid);
    error(['Could not data write to file: ' fname]);
end

fprintf(['Generated *.dat file: ' fname '\n']);
fclose(fid);

%Write *.hea file
fid = fopen([fname '.hea'],'w');
for m=1:M+1
    if(~fid)
        error(['Could not create header file for writing: ' fname]);
    end
    fprintf(fid,'%s\n',head_str{m});
end

if(~isempty(info))
    if ischar(info)
        fprintf(fid,'#%s\n',info);
    elseif iscell(info)
        for m=1:numel(info)
            fprintf(fid,'#%s\n',info{m});
        end
    end
end

if(nargout==1)
    varargout(1)={y};
end
fprintf(['Generated *.hea file: ' fname '\n']);
fclose(fid);

end

%%%End of Main %%%%

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

%Helper function
function [y,adc_gain,baseline,check_sum]=quant(x, bit_res, gain, baseline, isquant, isdigital)

min_x=min(x(~isnan(x)));
max_x=max(x(~isnan(x)));
nan_ind=isnan(x);
rg=max_x-min_x;

if(isdigital)
    % Digital input signal. Do not scale or shift the signal. The gain/baseline will only
    % be used to write the header file to interpret the output wfdb record.
    if ((min_x < -2^(bit_res-1)+1) || (max_x > (2^(bit_res-1)-1 )))
        error(['Digital input signal exceeds allowed range of specified output format: {' num2str(-2^(bit_res-1)+1) ' < x < ' num2str(2^(bit_res-1)-1) '}']);
    end
    adc_gain=gain;
    y=x;

else
    % Physical input signal - calculate the gain and baseline to minimize
    % the detail loss during ADC conversion: y = gain*x + baseline. Ignore any input gain or baseline

    % Calculate the adc_gain, baseline, and map the signal to digital
    % Make sure baseline doesn't go beyond 4 byte integer range

    if rg==0 % Flatline signal. Manually set adc_gain or it will be infinite.
          baseline=-(2^(bit_res-1))+1; % Set baseline to minimum value of bit res
          if x(1)==0
            adc_gain=1; % Arbitrary gain=1 for all 0 input signal. All values stored as baseline.
          else
            adc_gain=-baseline/x(1); % Set gain as inverse to store all values as exactly 0.
          end

    else % Non flatline signal: adc_gain = (range of encoding / range of Data) -- remember 1 quant level is for storing NaN
        % Constraint - baseline must be stored as a 4 byte integer for the WFDB library.
        if ((min_x>0) && (bit_res==32)) % All values are +ve, map with baseline = -2^31+1
            adc_gain=((2^32)-2)/max_x;
            baseline=-2147483647;
            if isquant==0 % Only display warning message if not recalculating later
                disp('Due to baseline constraints, output precision may be slightly less than 32 bits for the positive channel.');
            end
        elseif((max_x<0) && (bit_res==32)) % All values are -ve, map with baseline = 2^31-1
            adc_gain=((2^32)-2)/abs(min_x);
            baseline=2147483647;
            if isquant==0 % Only display warning message if not recalculating later
                disp('Due to baseline constraints, output precision may be slightly less than 32 bits for the negative channel.');
            end
        else % Signal has both +ve and -ve values or fmt is not 32. Use full range of bits.
            adc_gain=((2^bit_res)-2)/rg;
            baseline=round(-(2^(bit_res-1))+1-min_x*adc_gain);

        end
        if(isquant)
            % The (non flatline) input signal was already quantitized. Remove round-off error
            % by setting the original values to integers prior to fixed point conversion

            xvalues=sort(unique(x(~isnan(x)))); % All the values of x
            incmin=min(diff(xvalues)); % An estimate of the smallest possible increment in the input signal
            quantlevels=rg/incmin; % The estimated number of quantization levels in the input signal

            % We want to map 1 increment to 1 digital unit. First make sure
            % the full increment range is less than the 2^N-2 increments able to be encoded
            % by the chosen bit resolution. The incmin estimate will always
            % be equal to or larger than the true incmin, so it won't
            % wrongly trigger errors in this validation step.

            if (quantlevels>2^bit_res-2)
                if bit_res==32
                    disp(['The input signal has more quantization levels than 32 bits -1. ' ...
                        'Cannot directly map all input values to integers. Up to 1 bit of roundoff error may occur. Continuing...']);
                    calcquant=0; % Skip the integer matching and keep the old baseline/gain calculated.
                else
                    error(['The input signal has more quantization levels than the chosen bit resolution. ' ...
                        'Please choose a higher resolution or remove the isquant option to allow up to 1 bit of roundoff error']);
                end
            else
                calcquant=1;
            end

            % Calculate gain+offset. Baseline must be stored as a 4 byte integer for the WFDB library.
            if calcquant
                if ((min_x>0) && (bit_res==32)) % 32 bit +ve quant mapping
                    adc_gain=1/incmin;
                    baseline=round(2147483647-adc_gain*max_x); % map max_x to 2^31-1.
                    if (baseline<-2147483647) % Check if baseline goes below -2^31-1. If so, no quant. Recalculate gain and base.
                        adc_gain=((2^32)-2)/max_x;
                        baseline=-2147483647;
                        disp('Due to baseline constraints, the channel will not be quantized. Output precision may be less than 32 bits for the positive channel.');
                    end
                elseif((max_x<0) && (bit_res==32)) % 32 bit -ve quant mapping
                    adc_gain=1/incmin;
                    baseline=round(-2147483647-adc_gain*min_x); % map min_x to -2^31+1.
                    if (baseline>2147483647) % Check if baseline goes above 2^31-1. If so, no quant. Recalculate gain and base.
                        adc_gain=((2^32)-2)/abs(min_x);
                        baseline=2147483647;
                        disp('Due to baseline constraints, channel will not be quantized. Output precision may be less than 32 bits for the negative channel.');
                    end
                else % Signal has both +ve and -ve values or fmt is not 32. Can use full range of bits.
                    adc_gain=1/incmin; % 1 digital unit corresponds to the smallest physical increment.
                    baseline=round(-(2^(bit_res-1))+1-min_x*adc_gain); % xmin still maps to ymin. xmax will not go beyond y limit, baseline should not go beyond 32 bit limits.
                end
            end
        end
        % Check for 8 and 16 bit format 'baseline' field overflow. VERY
        % uncommon situation. Occurs if entire signal is +ve or -ve
        % with very high magnitude.
        if (baseline>2147483647) % Signal is all negative with large magnitude.
            warning('Large offset input channel entered. Output precision may be less than specified format for the negative channel.');
            baseline=2147483647; % Baseline is max int value, min_x maps to min bitres value.
            adc_gain=(-(2^(bit_res-1))+1-baseline)/min_x;
        elseif (baseline<-2147483647) % Signal is all positive with large magnitude.
            warning('Large offset input channel entered. Output precision may be less than specified format for the positive channel.');
            baseline=-2147483647; % Baseline is min int value, max_x maps to max bitres value.
            adc_gain=(2^(bit_res-1)-1-baseline)/max_x;
        end
    end

    y=x*adc_gain+baseline;

end % signal is in digital range. adc_gain and baseline have been calculated.

% Convert signals to appropriate integer type, and shift any WFDB NaN int values to
% a higher value so that they will not be read as NaN's by WFDB
switch bit_res % WFDB will interpret the smallest value as nan.
    case 8
        WFDBNAN=-128;
        y=int8(y);
    case 16
        WFDBNAN=-32768;
        y=int16(y);
    case 32
        WFDBNAN=-2147483648;
        y=int32(y);
end
iswfdbnan=find(y==WFDBNAN);
if(~isempty(iswfdbnan))
    y(iswfdbnan)=WFDBNAN+1;
end

%Set original NaNs to WFDBNAN
y(nan_ind)=WFDBNAN;

%Calculate the 16-bit signed checksum of all samples in the signal
check_sum=sum(y);
M=check_sum/(2^15);
if(M<0)
    check_sum=mod(check_sum,-2^15);
    if(~check_sum && abs(M)<1)
        check_sum=-2^15;
    elseif (mod(ceil(M),2))
        check_sum=2^15 + check_sum;
    end
else
    check_sum=mod(check_sum,2^15);
    if(mod(floor(M),2))
        check_sum=-2^15+check_sum;
    end
end

% Note that checksum must be calculated on actual digital samples for format 80,
% not the shifted ones. Therefore we only convert to real format now.
if bit_res==8
    y=uint8(int16(y)+128); % Convert into unsigned for writing byte offset format.
end

% Signal is ready to be written to dat file.

end

function y=get_names(str,deli)

y={};
old=1;
ind=regexp(str,deli);
ind(end+1)=length(str)+1;
for i=1:length(ind)
    y(end+1)={str(old:ind(i)-1)};
    old=ind(i)+1;
end

end


function varargout = OregonatorGUI(varargin)
% OREGONATORGUI M-file for OregonatorGUI.fig
%      OREGONATORGUI, by itself, creates a new OREGONATORGUI or raises the existing
%      singleton*.
%
%      H = OREGONATORGUI returns the handle to a new OREGONATORGUI or the handle to
%      the existing singleton*.
%
%      OREGONATORGUI('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in OREGONATORGUI.M with the given input arguments.
%
%      OREGONATORGUI('Property','Value',...) creates a new OREGONATORGUI or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before OregonatorGUI_OpeningFunction gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to OregonatorGUI_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Copyright 2002-2003 The MathWorks, Inc.

% Edit the above text to modify the response to help OregonatorGUI

% Last Modified by GUIDE v2.5 02-Dec-2005 23:24:01

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @OregonatorGUI_OpeningFcn, ...
                   'gui_OutputFcn',  @OregonatorGUI_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before OregonatorGUI is made visible.
function OregonatorGUI_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to OregonatorGUI (see VARARGIN)

% Choose default command line output for OregonatorGUI
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes OregonatorGUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = OregonatorGUI_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

x0=[1 1 1];

[t,x]=ode15s(@oregonator,[0 600],x0);

axes(handles.axes1);
plot(t,x(:,1),'r');
axis([300,600,0,100]);
title('figure1');
Xlabel('temps');
Ylabel('x(1)');

axes(handles.axes2);
plot(t,x(:,2),'g');
axis([300,600,0,3]);
title('figure2');
Xlabel('temps');
Ylabel('x(2)');

axes(handles.axes3);
plot(t,x(:,3),'m');
axis([300,600,0,100]);
title('figure3');
Xlabel('temps');
Ylabel('x(3)');


% GCI_creak_postproc.m
% Function to do the post-processing step to remove false positive GCIs
% detected in creaky voice regions
%
% Octave compatible
%
% Description
% Function to do the post-processing step to remove false positive GCIs
% detected in creaky voice regions
%
% Inputs
%  GCI             : [samples] [Mx1] Glottal closure instants
%  creak           : [binary]  [Nx1] Creak decision
%  search_reg      : [integer] [1x1] Search factor
%  rep             : [samples] [Nx1] Resonator output
%  removeThresh    : [integer] [1x1] Threshold for false GCI removal
%  repNum          : [integer] [1x1] number of repititions
%
% Outputs
%  GCI             : [samples] [Mx1] Glottal closure instants
%
% Example
%  GCI = GCI_creak_postproc(GCI,creak,search_reg,rep,removeThresh,repNum)
%
% References
%  [1] Kane, J., Gobl, C., (2013) `Evaluation of glottal closure instant
%       detection in a range of voice qualities', Speech Communication
%       55(2), pp. 295-314.
%
% Copyright (c) 2013 Trinity College Dublin
%
% License
%  This code is a part of the Voice Analysis Toolkit with the following
%  licence:
%  The software product (and any modifications thereof) is distributed under
%  a dual licence: an open source license for individual, non-commercial
%  purposes, and a commercial license. The opensource licence under which
%  the product is distributed is GNU GPL v2. For individual users, this
%  licence suits their use as these are not distributing proprietary
%  modifications, additions to, or derivatives of the product and don't
%  require legal protection of a commercial licence. For commercial users,
%  where open source does not meet their requirements, we offer commercial
%  licensing of the product. A commercial license permits customers to
%  modify, add or produce derivative products without the obligation of
%  making the subsequent code open source. For more information regarding
%  our commercial licence, please contact john.whelan@tcd.ie
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  John Kane kanejo@tcd.ie
%
% $Id <info set by the versioning system> $

function GCI = GCI_creak_postproc(GCI,creak,search_reg,rep,removeThresh,repNum)

% Separate GCIs detected in creaky voice regions from other regions
creak_GCI=GCI(creak(GCI)==1);
GCI(creak(GCI)==1)=[];

for m=1:repNum
    n=2;
    while n < length(creak_GCI)
        cur_rep_max = abs(min(rep(creak_GCI(n)-round(search_reg):creak_GCI(n)+round(search_reg))));

        if mean([abs(rep(creak_GCI(n-1))) abs(rep(creak_GCI(n+1)))])*removeThresh > cur_rep_max
             creak_GCI(n)=NaN;
            n=n+2;
        else n=n+1;
        end
    end
    creak_GCI(isnan(creak_GCI))=[];
end

GCI=sort(unique([GCI(:)' creak_GCI(:)']));


function [x, output] = findExtremePathway(fbaModel, obj)
% Finds an extreme ray
%
% USAGE:
%
%    [x, output] = findExtremePathway(fbaModel, obj)
%
% INPUT:
%    fbaModel:    FBA type model
%
% OPTIONAL INPUT:
%    obj:         default = random vector with size depending on fbaModel.S
%
% OUTPUTS:
%    x:           vector from `result`, where `result` is an output of `solveCobraLP` function
%    output:      `output.objval` contains `result.obj`
%
% .. $Revision: 0.1 $  $Date: 2011/05/01 $

A    = fbaModel.S;
[nmet,nrxn] = size(A);

% Convert model to conic form
if isfield(fbaModel, 'ub') && isfield(fbaModel, 'lb')
    revRxns = fbaModel.lb < 0 & fbaModel.ub > 0;
else
    error('missing fields: revRxns or ub and lb\n');
end

A = [A, -A(:,revRxns)];
[~, n] = size(A);

if nargin < 2
    obj = rand(n,1);
end

% Set required model components
LPProblem = struct();
LPProblem.A = sparse([A; ones(1,n)]);
LPProblem.c   = obj;
LPProblem.csense = repmat('E',nmet+1,1);
LPProblem.b   = [zeros(nmet,1); 1];
% Set optional model components
LPProblem.osense= -1;
LPProblem.lb = 0* ones(n,1);
LPProblem.ub = inf* ones(n,1);

result = solveCobraLP(LPProblem);  % Find extreme ray (be aware, that this can easily be a loop of a reversible reaction.

x = result.full(1:nrxn);
x(revRxns) = x(revRxns) - result.full(nrxn+1:end);

output.objval = result.obj;


classdef AMICO_WATERFREE

properties
    id, name                % id and name of the model
    dPar                    % parallel diffusivity of the tensors [units of mm^2/s]
    dPer                    % perpendicular diffusivities of the tensors [units of mm^2/s]
    dIso                    % isotropic diffusivities [units of mm^2/s]
    OUTPUT_names            % suffix of the output maps
    OUTPUT_descriptions     % description of the output maps
end

methods

    % =================================
    % Setup the parameters of the model
    % =================================
	function obj = AMICO_WATERFREE()
        global CONFIG

        % set the parameters of the model
        obj.id        = 'WATERFREE';
        obj.name      = 'Water free';
        obj.dPar      = 1.7 * 1E-3;
        obj.dIso      = [2.0 3.0] * 1E-3;
        obj.dPer      = linspace(0.1,1.0,10) * 1E-3;

        obj.OUTPUT_names        = { 'ICVF', 'ISOVF' };
        obj.OUTPUT_descriptions = {'Intra-cellular volume fraction', 'Isotropic volume fraction'};

        % set the parameters to fit it
        CONFIG.OPTIMIZATION.SPAMS_param.mode    = 2;
        CONFIG.OPTIMIZATION.SPAMS_param.pos     = true;
        CONFIG.OPTIMIZATION.SPAMS_param.lambda  = 0;    % l1 regularization
        CONFIG.OPTIMIZATION.SPAMS_param.lambda2 = 1e-3; % l2 regularization
    end

    % ==================================================================
    % Generate high-resolution kernels and rotate them in harmonic space
    % ==================================================================
    function GenerateKernels( obj, ATOMS_path, schemeHR, AUX, idx_IN, idx_OUT )
        global CONFIG AMICO_data_path

        % Tensor compartments
        % ===================
        idx = 1;
        for i = 1:numel(obj.dPer)
            TIME = tic();
            fprintf( '\t\t- A_%03d... ', idx );

            % generate
            D = diag( [obj.dPer(i) obj.dPer(i) obj.dPar] );
            signal = obj.TensorSignal( D, schemeHR.camino );

            % rotate and save
            lm = AMICO_RotateKernel( signal, AUX, idx_IN, idx_OUT, false );
            save( fullfile( ATOMS_path, sprintf('A_%03d.mat',idx) ), '-v6', 'lm' )
            idx = idx + 1;

            fprintf( '[%.1f seconds]\n', toc(TIME) );
        end

        % Isotropic compartments
        % ======================
        for i = 1:numel(obj.dIso)
            TIME = tic();
            fprintf( '\t\t- A_%03d... ', idx );

            % generate
            D = diag( [obj.dIso(i) obj.dIso(i) obj.dIso(i)] );
            signal = obj.TensorSignal( D, schemeHR.camino );

            % resample and save
            lm = AMICO_RotateKernel( signal, AUX, idx_IN, idx_OUT, true );
            save( fullfile( ATOMS_path, sprintf('A_%03d.mat',idx) ), '-v6', 'lm' )
            idx = idx + 1;

            fprintf( '[%.1f seconds]\n', toc(TIME) );
        end

    end

    % ==============================================
    % Project kernels from harmonic to subject space
    % ==============================================
    function ResampleKernels( obj, ATOMS_path, idx_OUT, Ylm_OUT )
        global CONFIG AMICO_data_path KERNELS

        % Setup the KERNELS structure
        % ===========================
        n1 = numel(obj.dPer);
        n2 = numel(obj.dIso);
        KERNELS = {};
        KERNELS.nS       = CONFIG.scheme.nS;
        KERNELS.nA       = n1 + n2; % number of atoms
        KERNELS.A1       = zeros( [KERNELS.nS n1 181 181], 'single' );
        KERNELS.A2       = zeros( [KERNELS.nS n2], 'single' );

        % Tensors
        % =======
        idx = 1;
        for i = 1:n1
            TIME = tic();
            fprintf( '\t- A_%03d...  ', idx );

            load( fullfile( ATOMS_path, sprintf('A_%03d.mat',idx) ), 'lm' );
            KERNELS.A1(:,i,:,:) = AMICO_ResampleKernel( lm, idx_OUT, Ylm_OUT, false );
            idx = idx + 1;

            fprintf( '[%.1f seconds]\n', toc(TIME) );
        end

        % Isotropic
        % =========
        for i = 1:n2
            TIME = tic();
            fprintf( '\t- A_%03d...  ', idx );

            load( fullfile( ATOMS_path, sprintf('A_%03d.mat',idx) ), 'lm' );
            KERNELS.A2(:,i,:,:) = AMICO_ResampleKernel( lm, idx_OUT, Ylm_OUT, true );
            idx = idx + 1;

            fprintf( '[%.1f seconds]\n', toc(TIME) );
        end

    end

    % ===========================
    % Fit the model to each voxel
    % ===========================
    function [DIRs, MAPs] = Fit( obj )
        global CONFIG
        global niiSIGNAL niiMASK
        global KERNELS bMATRIX

        % setup the output files
        MAPs         = zeros( [CONFIG.dim(1:3) numel(obj.OUTPUT_names)], 'single' );
        DIRs         = zeros( [CONFIG.dim(1:3) 3], 'single' );
        niiY = niiSIGNAL;

        n1 = numel(obj.dPer);
        n2 = numel(obj.dIso);

        fprintf( '\n-> Fitting "%s" model to data:\n', obj.name );
        TIME = tic();
        for iz = 1:niiSIGNAL.hdr.dime.dim(4)
        for iy = 1:niiSIGNAL.hdr.dime.dim(3)
        for ix = 1:niiSIGNAL.hdr.dime.dim(2)
            if niiMASK.img(ix,iy,iz)==0, continue, end

            % read the signal
            b0 = mean( squeeze( niiSIGNAL.img(ix,iy,iz,CONFIG.scheme.b0_idx) ) );
            if ( b0 < 1e-3 ), continue, end
            y = double( squeeze( niiSIGNAL.img(ix,iy,iz,:) ) ./ ( b0 + eps ) );
            y( y < 0 ) = 0; % [NOTE] this should not happen!

            % build the DICTIONARY
            [ i1, i2 ] = AMICO_Dir2idx( Vt );
            A = double( [ KERNELS.A1(CONFIG.scheme.dwi_idx,:,i1,i2) KERNELS.A2(CONFIG.scheme.dwi_idx,:) ] );

            % fit AMICO
            y = y(CONFIG.scheme.dwi_idx);
            yy = [ 1 ; y ];
            AA = [ ones(1,size(A,2)) ; A ];

            % estimate CSF partial volume and remove it
            x = full( mexLasso( yy, AA, CONFIG.OPTIMIZATION.SPAMS_param ) );

            % STORE results
            DIRs(ix,iy,iz,:) = Vt; % fiber direction

            MAPs(ix,iy,iz,1) = sum( x(1:n1) ) / ( sum(x) + eps ); % intracellular volume fraction

            MAPs(ix,iy,iz,2) = 1 - MAPs(ix,iy,iz,1); % isotropic volume fraction

            x(n1+1:end) = 0;
            niiY.img(ix,iy,iz,CONFIG.scheme.dwi_idx) = AA(2:end,:)*x*b0;

        end
        end
        end
        save_untouch_nii(niiY, 'dwi_fw_corrected.nii');
        TIME = toc(TIME);
        fprintf( '   [ %.0fh %.0fm %.0fs ]\n', floor(TIME/3600), floor(mod(TIME/60,60)), mod(TIME,60) )

        % compute MAPS
        n1 = numel(obj.dPer);
        MAPs    = zeros( [1 numel(obj.OUTPUT_names)], 'single' );
        MAPs(1) = sum( x(1:n1) ) / ( sum(x) + eps ); % intracellular volume fraction
        MAPs(2) = 1 - MAPs(1);                       % isotropic volume fraction
    end

    % ================================================================
    % Simulate signal according to tensor model (1 fiber along z-axis)
    % ================================================================
    function [ signal ] = TensorSignal( obj, D, XYZB )
        nDIR   = size( XYZB, 1 );
        signal = zeros( nDIR, 1 );
        for d = 1:nDIR
            signal(d) = exp(-XYZB(d,4) * XYZB(d,1:3) * D * XYZB(d,1:3)');
        end
    end

end

end


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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