% 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 [train_msk, p] = Masks(train, train_lbl, p)
%% Matrices easying the vectorized computation in CostFunction

%% Two types of masks: one is to mask with -Inf values that are superfluous due to sentence length overhead
%% the second is to apply differentiated k-max pooling that depends on the length of each sentene; turning superfluous values to max length of first layer

%Lengths of mask independenct of tot_num_layers
p(25) = p(2)+p(4)-1; %len of masks length+pool layer 1
p(26) = indexMaskLen(p(2), 1, p) + p(6)-1; %len of masks length+pool layer 2
p(27) = indexMaskLen(p(2), 2, p) + p(36)-1; %len of masks length+pool layer 3

if p(10) == 1 %If one layer in total
    train_msk=zeros(size(train,1), 2*p(25));

    train_msk = findMasks(train, train_lbl, train_msk, p);
elseif p(10) == 2
    train_msk=zeros(size(train,1), 2*p(25)+2*p(26));

    train_msk = findMasks(train, train_lbl, train_msk, p);

elseif p(10) == 3
    train_msk=zeros(size(train,1), 2*p(25)+2*p(26)+2*p(27));
    train_msk = findMasks(train, train_lbl, train_msk, p);

end

train_msk = logical(train_msk);

end

function train_msk = findMasks(train, train_lbl, train_msk ,p)
for i=1:length(train)
    if p(60)==1
        sent_len = train_lbl(i,2); %contains length
    else
        sent_len = train_lbl{2}(i); %contains length
    end

    if p(10) == 1
        train_msk(i,sent_len+p(4)-1+1:p(25)) = 1; %layer 1 sent_lengths mask
        train_msk(i,p(25)+p(7)+1:end) = 1; %layer 1 max_pool mask
    elseif p(10) == 2
        train_msk(i,sent_len+p(4)-1+1:p(25)) = 1; %layer 1 sent_lengths mask
        train_msk(i,p(25)+indexMaskLen(sent_len,1,p)+1:2*p(25)) = 1; %layer 1 max_pool mask

        train_msk(i,2*p(25)+indexMaskLen(sent_len,1,p)+p(6)-1+1:2*p(25)+p(26)) = 1; %layer 2 sent_lengths mask
        train_msk(i,2*p(25)+p(26)+p(7)+1:end) = 1; %layer 2 max_pool mask

    elseif p(10) == 3
        train_msk(i,sent_len+p(4)-1+1:p(25)) = 1; %layer 1 sent_lengths mask
        train_msk(i,p(25)+indexMaskLen(sent_len,1,p)+1:2*p(25)) = 1; %layer 1 max_pool mask

        train_msk(i,2*p(25)+indexMaskLen(sent_len,1,p)+p(6)-1+1:2*p(25)+p(26)) = 1; %layer 2 sent_lengths mask
        train_msk(i,2*p(25)+p(26)+indexMaskLen(sent_len,2,p)+1:2*p(25)+2*p(26)) = 1; %layer 2 max_pool mask

        train_msk(i,2*p(25)+2*p(26)+indexMaskLen(sent_len,2,p)+p(36)-1+1:2*p(25)+2*p(26)+p(27)) = 1; %layer 3 sent_lengths mask
        train_msk(i,2*p(25)+2*p(26)+p(27)+p(7)+1:end) = 1; %layer 3 max_pool mask

    end
end

end

function pool_size = indexMaskLen(sent_len, num_conv_layer, p)
%Function computes pooling size for a given layer
if p(10) == 1
    pool_size = p(7); %size of final pooling layer

elseif p(10) == 2
    if num_conv_layer == 1
        pool_size = max(ceil(sent_len/2),p(7)); %half if shallow network
    end

    if num_conv_layer == 2 %not in fact possible
        pool_size = p(7);
    end
elseif p(10) == 3
    if num_conv_layer == 1
        pool_size = max(ceil((sent_len/3)*2),p(7)); %linear function
    end

    if num_conv_layer == 2
        pool_size = max(ceil(sent_len/3),p(7));
    end
else
    pool_size = -1;
end

end


%% SIMPLE FIXED WING AGENT (fixedWing.m) //////////////////////////////////
% This class is designed to contain the placeholder dynamics of a
% fixed-wing aircraft.

% Author: James A. Douthwaite

classdef fixedWing < agent
    properties

    end
    %% ///////////////////////// MAIN METHODS /////////////////////////////
    methods
        % Constructor
        function [this] = fixedWing(varargin)
            % Call the super class
            this@agent(varargin);                                           % Create the super class 'agent'
            % //////////////// Check for user overrides ///////////////////
            this = this.ApplyUserOverrides(varargin); % Recursive overrides
            % /////////////////////////////////////////////////////////////
        end
        % Setup - 3D DUBLINS CAR STATE [x;y;z;v;psi;theta]
        function [this] = setup(this,localXYZVelocity,localXYZrotations)
            % Infer the initial conditions from the scenario.
            p     = zeros(3,1);            % The position in the local frame
            speed = norm(localXYZVelocity);% The speed of the aircraft
            theta = localXYZrotations(2);  % The elevation of the aircraft
            psi   = 0;                     % The yaw of the aircraft
            % Define the initial state from the scenario
            this.localState = [p(1);p(2);p(3);speed;theta;psi];
            this = this.SetGLOBAL('priorState',this.localState);
        end
        % Main
        function [this] = main(this,ENV,varargin)
            % INPUTS:
            % varargin - Cell array of inputs
            % >dt      - The timestep
            % >objects - The detectable objects cell array of structures
            % OUTPUTS:
            % obj      - The updated project

            % //////////// CHECK FOR NEW INFORMATION UPDATE ///////////////
            % UPDATE THE AGENT WITH THE NEW ENVIRONMENTAL INFORMATION
            %[obj,obstacleSet,agentSet,waypointSet] = obj.getAgentUpdate(varargin{1});                   % IDEAL INFORMATION UPDATE

            u_k = [0.1;0.1;0.1]; % Acceleration, pitch rate, yaw rate
            x_k = this.localState;

            % UPDATE LOCAL STATE
            newState = this.UpdateLocalState(ENV,x_k,u_k);

            % UPDATE THE GLOBAL PROPERTIES OF THE AGENT
            [this] = this.GlobalUpdate(ENV.dt,newState);

            % /////////////// RECORD THE AGENT-SIDE DATA //////////////////
            this.DATA.inputNames = {'$v (m/s)$','$\dot{\theta} (rad/s)$','$\dot{\psi} (rad/s)$'};
            this.DATA.inputs(1:length(this.DATA.inputNames),ENV.currentStep) = newState(4:6);         % Record the control inputs
        end
    end
    methods
        % GET THE STATE UPDATE (USING ODE45)
        function [X] = UpdateLocalState(this,TIME,X0,u)
            % This function computes the state update for the current agent
            % using the ode45 function.

            % Is of the form: u = [v;a;psi_dot;theta_dot];
            X = X0;

            % DETERMINE THE INTEGRATION PERIOD
            if TIME.currentTime == TIME.timeVector(end)
                return
            else
                [~,Xset] = ode45(@(t,X) this.dynamics_nonholonomicDublinsCar(X,u),...
                    [0 TIME.dt],X0,...
                    odeset('RelTol',1e-2,'AbsTol',TIME.dt*1E-2));
                X = Xset(end,:)'; % Pass the state at the end of the sample period
            end
        end
        % DEFINE THE GLOBAL UPDATE PROCEDURE FOR A 3D DUBLINS CAR MODEL
        function [this] = GlobalUpdate(this,dt,eulerState)

            % Retrieve current properties
            p_k	= this.GetGLOBAL('position');
            v_k	= this.GetGLOBAL('velocity');
            q_k	= this.GetGLOBAL('quaternion');
            x_k	= this.GetGLOBAL('priorState');

            % USE THE 'RATE' STATES DIRECTLY
            v_k_plus = [eulerState(4);0;0];                         % Assumed forward in the local axes
            omega_k_plus   = [0;eulerState(5);eulerState(6)];             % Neglect roll only
            localAxisRates = (omega_k_plus - [0;x_k(5:6)])/dt;
            % MAP THE LOCAL RATES TO GLOBAL RATES AND INTEGRATE QUATERNION
            % Previous properties

            % PREVIOUS ROTATION-MATRIX
            %R_k = quat2rotm(quaternion_k');
            R_k = OMAS_geometry.quaternionToRotationMatrix(q_k);
            % THE GLOBAL AXIS RATES
            omega = R_k'*localAxisRates;
            % UPDATE THE QUATERNION POSE
            q_k_plus = OMAS_geometry.integrateQuaternion(q_k,omega,dt);
            % REDEFINE THE ROTATION MATRIX
            R_k_plus = quat2rotm(q_k_plus');
            % MAP THE LOCAL VELOCITY TO THE GLOBAL AXES
            v_k_plus = R_k_plus'*v_k_plus;
            p_k_plus = p_k + dt*v_k;
            % ///////////////// REASSIGN K+1 PARAMETERS ///////////////////
            [this] = this.GlobalUpdate_direct(...
                p_k_plus,...    % Global position at k plius
                v_k_plus,...    % Global velocity at k plus
                q_k_plus);      % Quaternion at k plus
        end
    end
    % STATIC FUNCTIONS
    methods (Static)
        % THE NON-LINEAR HOLONOMIC DUBLINS-CAR MODEL
        function [dX] = dynamics_nonholonomicDublinsCar(X,u)
            % The state:  [dx;dy;dz;dv;dpsi;dtheta]
            % The inputs: [a,theta_dot,psi_dot]
%             dX = zeros(6,1);  % GLOBAL SPACE
%             dX(1) = X(4)*cos(X(6))*cos(X(5));
%             dX(2) = X(4)*cos(X(6))*sin(X(5));
%             dX(3) = X(4)*sin(X(6));
%             dX(4) = a;          % Acceleration
%             dX(5) = psi_dot;    % Yaw rate
%             dX(6) = theta_dot;  % Pitch rate
            dX = zeros(6,1);
            dX(1) = X(4);
            dX(2) = 0;
            dX(3) = 0;
            dX(4) = u(1);  % Acceleration
            dX(5) = u(2);  % Pitch rate
            dX(6) = u(3);  % Yaw rate
        end
    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 test_issue1387

% WALLTIME 00:10:00
% MEM 1gb
% DEPENDENCY ft_read_event ft_read_header ft_preprocessing

%%
% load data and parse the events using '5*nanmedian' threshold on 8 channels
fileName       = dccnpath('/home/common/matlab/fieldtrip/data/test/issue1387/Test_4.edf');
bitChannels    = 3:10;
header         = ft_read_header(fileName);
labels         = header.label(bitChannels);
data_events	   = ft_read_event(fileName,'header',header,'detectflank','up','chanindx',bitChannels,'threshold','5*nanmedian');

%%
% load the raw data
cfg            = [];
cfg.dataset    = fileName;
cfg.continuous = 'yes';
cfg.channel    = 'all';
data           = ft_preprocessing(cfg);

%%
% parse out the event information for the 1st bit channel
idx            = cellfun(@(x)strcmpi(x,labels{1}),{data_events.type});
event.label	   = labels{1};
event.idx      = find(idx==1);
event.evnt     = data_events(event.idx);
event.samples  = [event.evnt.sample];
event.times	   = data.time{1}(event.samples);

%%
% ASSERT: There were 78 TTLs on the first channel, make sure this is the case, error otherwise.
assert(length(event.times)==78,'Number of events should be 78!')

%%
% Plot the events on top of the raw data for visual inspection
tm             = data.time{1};
ch             = data.trial{1}(3,:);
baseline       = median(ch(1:100));
ch             = ch - baseline;
ch             = ch / max(ch);
figure;
plot(tm,ch,'k-');
hold on;
y = repmat(0.5, [1 length(event.times)]);
plot(event.times,y,'ro','MarkerSize',6)
xlim([10 50]);
ylim([-0.05 1.05]);
title('TTL channel 1 should have 78 event markers aligned to the upstates')
xlabel('Time (s)')
ylabel('Normalised Amplitude')


function scannerXform = getScannerXform(rawFile)
% Get the transform matrix from INPLANE coordinates (x,y,z, indices) to
% scanner coordinates (in mm).
%
%  scannerXform = getScannerXform([rawFile])
%
% The transform will yield:
%   scannerCoords = scannerXform * ipCoords;
%
% 8/2009: Siphoned off from RAS's code computeB0DirectionMap.
%
% That code is now shorter. And we can now use this transform for other things
% such as getting a 3-vector for the B0 direction in a scan
%
% Example: scannerXform = getScannerXform;

% Find the raw inplane dicoms
if notDefined('rawFile'),
    rawFile = fullpath('Raw/Anatomy/Inplane');
end

if exist(rawFile, 'dir')
	% allow rawFile to be a directory containing DICOM files
	pattern = fullfile(rawFile, '*.dcm');
	w = dir(pattern);
	if isempty(w)
		pattern = fullfile(rawFile, '*.DCM');
		w = dir(pattern);
	end

	if isempty(w)
		% still not found? We have a problem.
		error(['No DICOM files found in %s. Please provide a ' ...
			'file path if the raw file is not in DICOM format.'], ...
			rawFile);
	end

	% got here? then we have some files. Take the first one.
	rawFile = fullfile(rawFile, w(1).name);

elseif ~exist(rawFile, 'file')
	% no file or directory found with this name? Can't proceed.
	error('%s is not a file or directory.', rawFile);
end

% load the raw file and make sure we have an xform into scanner space
[p f ext] = fileparts(rawFile);
if strncmpi( ext, '.dcm', 4 )
	inplane = mrReadDicom(rawFile);
else
	inplane = mrLoad(rawFile);
end

if ~isfield(inplane, 'spaces') || isempty(inplane.spaces)
	error('No scanner-coordinate information found in the header fields');
end

% find the alignment to the scanner coordinate space
spaces = {inplane.spaces.name};
I = cellfind(spaces, 'Scanner');
if isempty(I)
	error('No scanner-coordinate information found in the header fields');
end

scannerXform = inplane.spaces(I).xform;


function varargout = transpose(varargin)
% .'   SPHEREFUN transpose.
%    F.' is the non-conjugate transpose of a F.
%
% See also CTRANSPOSE.

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

[varargout{1:nargout}] = transpose@separableApprox(varargin{:});

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 [output]=rgbcompression(f)
[F4]= compressimage(f);
F4=uint8(F4);

F4=ycbcr2rgb(F4);
output=F4;
figure;
subplot(1,2,1),imshow(f);
subplot(1,2,2),imshow(F4);


function [D] = spm_DEM_eval_diff(x,v,qp,M,bilinear)
% evaluates derivatives for DEM schemes
% FORMAT [D]       = spm_DEM_eval_diff(x,v,qp,M,bilinear)
% v{i} - casual states
% x(i) - hidden states
% qp - conditional density of parameters
%  qp.p{i} - parameter deviates for i-th level
%  qp.u(i) - basis set
%  qp.x(i) - expansion point ( = prior expectation)
% M        - model structure
% bilinear - optional flag to suppress second-order derivatives
%
% D        - derivatives
%   D.dgdv
%   ...
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_DEM_eval_diff.m 6734 2016-03-02 12:02:46Z peter $

% check for evaluation of bilinear terms
%--------------------------------------------------------------------------
try
    bilinear;
catch
    bilinear = 1;
end

% get dimensions
%==========================================================================
nl    = size(M,2);                       % number of levels
ne    = sum(spm_vec(M.l));               % number of e (errors)
nx    = sum(spm_vec(M.n));               % number of x (hidden states)
np    = sum(spm_vec(M.p));               % number of p (parameters)
ny    = M(1).l;                          % number of y (inputs)
nc    = M(end).l;                        % number of c (prior causes)

% initialise cell arrays for hierarchical structure
%--------------------------------------------------------------------------
df.dv = cell(nl - 1,nl - 1);
df.dx = cell(nl - 1,nl - 1);
df.dp = cell(nl - 1,nl - 1);
dg.dv = cell(nl    ,nl - 1);
dg.dx = cell(nl    ,nl - 1);
dg.dp = cell(nl    ,nl - 1);

for i = 1:(nl - 1)
    dg.dv{i + 1,i} = sparse(M(i).m,M(i).m);
    dg.dx{i + 1,i} = sparse(M(i).m,M(i).n);
    dg.dp{i + 1,i} = sparse(M(i).m,M(i).p);
    dg.dv{i    ,i} = sparse(M(i).l,M(i).m);
    dg.dx{i    ,i} = sparse(M(i).l,M(i).n);
    dg.dp{i    ,i} = sparse(M(i).l,M(i).p);
    df.dv{i    ,i} = sparse(M(i).n,M(i).m);
    df.dx{i    ,i} = sparse(M(i).n,M(i).n);
    df.dp{i    ,i} = sparse(M(i).n,M(i).p);
end

if bilinear
    for i = 1:(nl - 1)
        dg.dvp{i}      = cell(M(i).p,1);
        dg.dxp{i}      = cell(M(i).p,1);
        df.dvp{i}      = cell(M(i).p,1);
        df.dxp{i}      = cell(M(i).p,1);
        [dg.dvp{i}{:}] = deal(dg.dv);
        [dg.dxp{i}{:}] = deal(dg.dx);
        [df.dvp{i}{:}] = deal(df.dv);
        [df.dxp{i}{:}] = deal(df.dx);
    end
end

% Derivatives at each hierarchical level
%==========================================================================

% inline function for evaluating projected parameters
%--------------------------------------------------------------------------
h     = @(f,x,v,q,u,p) f(x,v,spm_unvec(spm_vec(p) + u*q,p));
for i = 1:(nl - 1)

    % states level i
    %----------------------------------------------------------------------
    xvp = {x{i},v{i},qp.p{i},qp.u{i},M(i).pE};

    % 1st and 2nd partial derivatives (states)
    %----------------------------------------------------------------------
    if bilinear && np
        try
            [dgdxp, dgdx] = spm_diff(h,M(i).gx,xvp{:},4,'q');
            [dgdvp, dgdv] = spm_diff(h,M(i).gv,xvp{:},4,'q');
            [dfdxp, dfdx] = spm_diff(h,M(i).fx,xvp{:},4,'q');
            [dfdvp, dfdv] = spm_diff(h,M(i).fv,xvp{:},4,'q');
        catch
            [dgdxp, dgdx] = spm_diff(h,M(i).g,xvp{:},[2 4],'q');
            [dgdvp, dgdv] = spm_diff(h,M(i).g,xvp{:},[3 4],'q');
            [dfdxp, dfdx] = spm_diff(h,M(i).f,xvp{:},[2 4],'q');
            [dfdvp, dfdv] = spm_diff(h,M(i).f,xvp{:},[3 4],'q');
        end
    else
        try
            dgdx = h(M(i).gx,xvp{:});
            dgdv = h(M(i).gv,xvp{:});
            dfdx = h(M(i).fx,xvp{:});
            dfdv = h(M(i).fv,xvp{:});
        catch
            dgdx = spm_diff(h,M(i).g,xvp{:},2);
            dgdv = spm_diff(h,M(i).g,xvp{:},3);
            dfdx = spm_diff(h,M(i).f,xvp{:},2);
            dfdv = spm_diff(h,M(i).f,xvp{:},3);
        end
    end

    % 1st-order partial derivatives (parameters)
    %----------------------------------------------------------------------
    try
        dfdp   = h(M(i).fp,xvp{:});
        dgdp   = h(M(i).gp,xvp{:});
    catch
        dfdp   = spm_diff(h,M(i).f,xvp{:},4);
        dgdp   = spm_diff(h,M(i).g,xvp{:},4);
    end

%     % check which dervatives need to be evaluated
%     %====================================================================
%     D(i).dgdv = nnz(dgdv) + nnz(spm_vec(dgdvp));
%     D(i).dgdx = nnz(dgdx) + nnz(spm_vec(dgdxp));
%     D(i).dfdv = nnz(dfdv) + nnz(spm_vec(dfdvp));
%     D(i).dfdx = nnz(dfdx) + nnz(spm_vec(dfdxp));

    % Constant terms (linking causes over levels)
    %----------------------------------------------------------------------
    dg.dv{i + 1,i} = -speye(M(i).m,M(i).m);

    % place 1st derivatives in array
    %----------------------------------------------------------------------
    dg.dx{i,i} = dgdx;
    dg.dv{i,i} = dgdv;
    df.dx{i,i} = dfdx;
    df.dv{i,i} = dfdv;
    df.dp{i,i} = dfdp;
    dg.dp{i,i} = dgdp;

    % place 2nd derivatives in array
    %----------------------------------------------------------------------
    if bilinear && np
        for j = 1:length(dgdxp)
            dg.dxp{i}{j}{i,i} = dgdxp{j};
            dg.dvp{i}{j}{i,i} = dgdvp{j};
            df.dxp{i}{j}{i,i} = dfdxp{j};
            df.dvp{i}{j}{i,i} = dfdvp{j};
        end
    end
end

% concatenate hierarchical forms
%==========================================================================
D.dgdv  = spm_cat(dg.dv);
D.dgdx  = spm_cat(dg.dx);
D.dfdv  = spm_cat(df.dv);
D.dfdx  = spm_cat(df.dx);
D.dfdp  = spm_cat(df.dp);
D.dgdp  = spm_cat(dg.dp);

% fixed derivatives w.r.t. prediction errors and states
%--------------------------------------------------------------------------
D.dfdy  =  sparse(nx,ny);
D.dfdc  =  sparse(nx,nc);
D.dedy  =  spm_speye(ne,ny);
D.dedc  = -spm_speye(ne,nc,nc - ne);

% bilinear terms if required
%--------------------------------------------------------------------------
if bilinear
    D.dgdvp = {};
    D.dgdxp = {};
    D.dfdvp = {};
    D.dfdxp = {};
    for i = 1:length(dg.dvp)
        for j = 1:length(dg.dvp{i})
            D.dgdvp{end + 1} = spm_cat(dg.dvp{i}{j});
            D.dgdxp{end + 1} = spm_cat(dg.dxp{i}{j});
            D.dfdvp{end + 1} = spm_cat(df.dvp{i}{j});
            D.dfdxp{end + 1} = spm_cat(df.dxp{i}{j});
        end
    end
end


% just run it.
%
% Prompts you for all necessary info, including image to draw on
% and output image file name.
%
% Writes a mask *img file in analyze format that you can view in spm
%
% Functions called:
% c:\tor_scripts\voistatutility\readim2.m
%   read_hdr.m
%
% c:\tor_scripts\roiutility\imgmanip\getvoxels3.m
%   C:\matlabR12\toolbox\matlab\elmat\ind2sub.m
%
% c:\tor_scripts\roiutility\cluster\tor_extract_rois.m
%
% C:\matlabR12\toolbox\matlab\datatypes\squeeze.m

% c:\tor_scripts\voistatutility\nanmean.m
% center_of_mass.m

% C:\matlabR12\toolbox\matlab\elmat\repmat.m
% (calls other spm functions)
%
% c:\tor_scripts\fixedfxutility\voxel2mm.m
%
% spm_vol.m
% spm_read_vols.m
%
% ..
%    by Tor Wager  last modified 9/22/02
% ..
%
% :Note: readim2 works on little endian systems (Linux).
% If you're using Unix, you should modify this script
% to call the function readim2_b.m instead.
%

P = spm_select(1,'image','Choose whole-brain anatomical mask');
%P = spm_get(1,'*.img','Choose whole-brain anatomical mask');

V = spm_vol(P);
vol = spm_read_vols(V);
vol = double(vol);

dosagg = input('Press 1 to choose saggital slices, or 0 for axial.');
% if saggital, use vol, not rvol
% y is not reversed
% ginput x = brain y, ginput y = brainz, slice = x

if dosagg
    [rvol2] = readim2(vol,'p','sagg');
else
    for i = 1:size(vol,3)
        rvol(:,:,i) = rot90(vol(:,:,i));
    end

    [rvol2] = readim2(rvol,'p');
end

colormap gray
wslices = input('Enter range of slices (e.g, 20:30) ');
close

if dosagg
    [rvol2,hdr,h] = readim2(vol,'p','sagg','noflipy',wslices);
    colormap gray
    [voxels,mask] = getvoxels3(h,vol,wslices,'sagg');
else
    [rvol2,hdr,h] = readim2(rvol,'p','ax','noflipy',wslices);
    colormap gray
    [voxels,mask] = getvoxels3(h,vol,wslices);
end

%for i = 1:size(mask,3)
%    rmask(:,:,i) = rot90(mask(:,:,i),3);
%end

V.fname = input('Enter output filename, no quotes: ','s');
spm_write_vol(V,mask);
[d,fname,e] = fileparts(V.fname);
saveas(gcf,fname,'fig')

% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% cluster structure stuff for ROIs
% +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

CLU.M = V.mat;
CLU.voxSize = diag(V.mat)';
CLU.voxSize = CLU.voxSize(1:3);
CLU.VOX = CLU.voxSize;
CLU.XYZ = voxels';
CLU.XYZmm = voxel2mm(voxels',V.mat);
CLU.Z = ones(1,size(CLU.XYZ,2));
CLU.crit_t = 1;
CLU.cl_size = 0;
CLU.u = CLU.crit_t;
CLU.k = CLU.cl_size;
CLU.title = fname;

clusters = tor_extract_rois([],CLU,CLU);
eval(['save ' fname '_clusters CLU clusters'])

disp(' ')
newim = input('Press 1 to choose a display image, or anything else to use your prior mask.');
if newim, P = spm_get(1,'*.img','Choose overlay image');,end

spm_image('init',P)
for i = 1:length(clusters)
    spm_orthviews('AddColouredBlobs',1,clusters(i).XYZ,clusters(i).Z,CLU.M,rand(1,3))
end


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