 function [ob,m,mz] = Gocrf(k,fov,N,mdomswitch,dt,varargin)
%function [ob,m,mz] = Gocrf(k,fov,N,m0,mdomswitch,dt,varargin)
%
% Construct Gocrf object for fast optimal control large-tip-angle
% multidimensional (and parallel) MR RF pulse design. Used in
% conjunction with an optimization routine, this object can be
% used to design small pulse updates that, when summed with an
% underlying pulse, result in an accurate excitation.
% The vectors m and mz are the results of initial Bloch sim at
% object creation.
%
% The object is created by calling:
%    Gocrf = Gocrf( ... );
% and it is then used by commands like:
%    y = Gocrf * x;
% which will evaluate perturbations to the magnetization
% produced by pulse perturbations x. It also passes x with
% unit weighting, so that if G is the sub-object relating
% x to its perturbations, then y = [G;I]*x
%
% in:
%    k            [Nt,d]   excitation k-space trajectory
%                          (= -gam int_t^T g(t') dt')
%                          (inverse fov units)
%    fov          [d,1]    field of view in each dimension
%    N            [d,1]    size of design grid in each
%                          dimension
%    mdomswitch   [1]      switch to evaluate magnetization
%                          or spinors (spinors not tested yet)
%    dt           [1]      RF sampling period
%
% options:
%    m0           [3,1]    initial magnetization state
%                          (e.g., if equilibrium, [0 0 1] (default))
%    'sens'       [prod(N),Ncoils]   transmit (B1+) sensitivities
%    'd'          [[N]]    desired excitation pattern, in
%                          arbitrary flip angle units. this is used
%                          to determine sub-space over which to
%                          simulate
%    'indmask'    [[N]]    mask to choose sub-space points from
%    'baseB1'     [Ncoils*Nt]    baseline RF pulse(s), used for
%                                initial Bloch simulation during
%                                object creation
%    'g'          [Nt,d]   gradient waveforms, for Bloch simulation
%                          (gauss/fov units)
%    'Nsvd'       [1]      # of baseline excitation expansion terms
%                          (default is 4)
%    'a0B','b0B'  [Nt,Nsvd]      baseline temporal basis function
%                                matrices
%    'a0Ct','b0Ct' [prod(N),Nsvd]   baseline spatial coefficient
%                                   matrices
%    'a0','b0'    [prod(N),1]    final baseline spinors
%    'nufft'      cell     nufft arguments (passed to Gmri_SENSE)
%    'RFbasis'    [Nt,Nbasis]    RF pulse basis functions for
%                                parameterized pulse design
%
% options for field-corrected pulse design (untested in this object!):
%    'ti'         [Nt,1]   sample times (0:T in seconds)
%    'we'         [[N]]    field_map (Hz)
%    'L'          [1]      # of approximation terms (default is 4)
%
% After building this object, the user can Bloch-simulate
% a new pulse and update the object using:
%     [ob,m,mz] = feval(ob.arg.update, ob, baseB1);
%
% or a Bloch simulation (without object update) can be performed
% using:
%     [m,mz,a0,b0] = feval(ob.arg.blochsim,baseB1,arg,pos,sens,we);
%
% OR an update can be made (without Bloch sim) using:
%     ob = feval(ob.arg.new_B_Ct,ob,a0B,a0Ct,b0B,b0Ct,a0,b0)
%
% Copyright 2008-10-8, Will Grissom, Stanford University

if nargin < 6, help(mfilename), error(mfilename), end

%
% default object
%
% Gmri objects
arg.a0G = [];arg.b0G = [];
% svd matrices
arg.a0B = [];arg.a0Ct = [];arg.b0B = [];arg.b0Ct = [];
% underlying final alpha0, beta0
arg.a0 = [];arg.b0 = [];
% initial mag (global)
arg.bsq = 0; %
arg.exc = 0; % just return mxy
arg.rfb = 0; % no rf basis
arg.m0 = [0 0 1];
arg.mdomswitch = mdomswitch;
arg.we = [];
arg.nufft_args = {N,6*ones(size(N)),2*N,N/2,'table',2^10,'minmax:kb'};
arg.new_B_Ct = @Gocrf_new_B_Ct;
arg.blochsim = @Gocrf_blochsim;
arg.subindcalc = @Gocrf_subindcalc;
arg.update = @Gocrf_update;
arg.subind = []; % set of points to Bloch sim and do SVD on
arg.nsubbins = 10; % default # of histogram bins to choose sub-points from
arg.simpos = []; % spatial locations for Bloch sim
arg.sens = []; % need sens for Bloch sims
arg.d = []; % desired pattern (for subind calc)
arg.indmask = []; % mask (for subind calc)
arg.simmask = []; % mask for Bloch simulation
arg.dt = dt; % sampling period
arg.ti = []; % time points (for field-map corrected design)
arg.fov = fov;
arg.k = k; % excitation k-space traj
arg.N = N; % spatial dimensions
arg.L = 4;
arg.Nsvd = 4;
arg.subindlen = 15*arg.Nsvd; % default # of sub-points for svd sim
% field map bases and coeffs
arg.B = [];arg.Ct = [];
arg.baseB1 = []; % base B1 pulses (for Bloch sim)
arg.g = []; % gradient waveforms (for Bloch sim)
arg.fmod = []; % modulation freq (for eg dual band designs)
arg.aux1 = 1; % vector to multiply a or mxy by
arg.aux2 = 1; % vector to multiply b or b^2 or mz by
arg.RFbasis = [];
ob.Nbasis = 0;
% Bloch-simulated patterns (will be empty if basis and coeff
% matrices are provided)
m = [];mz = [];

gambar = 4257;             % gamma/2pi in Hz/g
gam = gambar*2*pi;         % gamma in radians/g

arg = vararg_pair(arg, varargin, 'subs', ...
	{'basis', 'basis_args'; 'nufft', 'nufft_args'});

% if not provided, get the subindices for Bloch sim
if isempty(arg.subind) & ~isempty(arg.d) & ~isempty(arg.indmask)
  arg = Gocrf_subindcalc(arg,arg.d,arg.indmask);
elseif isempty(arg.subind)
  disp ['No sub indices or means to calculate them provided. update() ' ...
        'will not work'];
end

ktmp = -[arg.k - repmat(arg.k(1,:)/2,[size(arg.k,1) 1])];
if isempty(arg.we) || sum(abs(arg.we(:))) == 0  % no field map

	if ~isempty(arg.sens)
		arg.a0G = Gmri_SENSE(ktmp,true(N),'fov',fov, ...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',arg.sens*(-1i*gam*arg.dt/2))';
		arg.b0G = Gmri_SENSE(ktmp,true(N),'fov',fov, ...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',arg.sens*(1i*gam*arg.dt/2))';
	else
%{
		arg.a0G = Gmri_SENSE(ktmp,true(N),'fov',fov,'basis',{'dirac'},'nufft',arg.nufft_args, ...
                         'exact',0,'sens',ones(prod(N),1)*(-1i*gam*arg.dt/2))';
		arg.b0G = Gmri_SENSE(ktmp,true(N),'fov',fov,'basis',{'dirac'},'nufft',arg.nufft_args, ...
                         'exact',0,'sens',ones(prod(N),1)*(1i*gam*arg.dt/2))';
%}
		% jf version:
		arg.a0G = (-1i*gam*arg.dt/2)' * ...
			Gmri(ktmp, true(N), 'fov', fov, 'basis', {'dirac'}, ...
				'nufft',arg.nufft_args)';
		arg.b0G = (1i*gam*arg.dt/2)' * ...
			Gmri(ktmp, true(N), 'fov', fov, 'basis', {'dirac'}, ...
				'nufft', arg.nufft_args)';

	end

else % non-zero field map

	if ~isempty(arg.sens)
		arg.a0G = Gmri_SENSE(ktmp,true(N),'fov',fov,...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',arg.sens*(-1i*gam*arg.dt/2), ...
			'ti',-arg.ti+arg.ti(end)/2,...
			'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
		arg.b0G = Gmri_SENSE(ktmp,true(N),'fov',fov,...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',arg.sens*(1i*gam*arg.dt/2), ...
			'ti',-arg.ti+arg.ti(end)/2, ...
			'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
	else
%{
		arg.a0G = Gmri_SENSE(ktmp,true(N),'fov',fov,...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',ones(prod(N),1)*(-1i*gam*arg.dt/2), ...
			'ti',-arg.ti+arg.ti(end)/2, ...
			'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
		arg.b0G = Gmri_SENSE(ktmp,true(N),'fov',fov, ...
			'basis',{'dirac'},'nufft',arg.nufft_args, ...
			'exact',0,'sens',ones(prod(N),1)*(1i*gam*arg.dt/2), ...
 			'ti',-arg.ti+arg.ti(end)/2, ...
			'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
%}
		% jf version:
		arg.a0G =(-1i*gam*arg.dt/2)' * ...
			Gmri(ktmp,true(N),'fov',fov,...
				'basis',{'dirac'},'nufft',arg.nufft_args, ...
				'ti',-arg.ti+arg.ti(end)/2, ...
				'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
		arg.b0G = (1i*gam*arg.dt/2)' * ...
			Gmri(ktmp,true(N),'fov',fov, ...
				'basis',{'dirac'},'nufft',arg.nufft_args, ...
 				'ti',-arg.ti+arg.ti(end)/2, ...
				'zmap',1i*2*pi/2*arg.we,'L',arg.L)';
	end

	arg.B = arg.a0G.arg.B;
	arg.Ct = arg.a0G.arg.Ct;
end

tmp = arg.a0G;
[dim1,dim2] = size(tmp);
if arg.exc
  arg.dim = [dim1 + 2*dim2, 2*dim2];
else
  arg.dim = [2*dim1+2*dim2, 2*dim2];
end

if ~isempty(arg.RFbasis)

  arg.rfb = 1;
  arg.Nbasis = size(arg.RFbasis,2);

  % assuming that there is only one coil when we parameterize the
  % pulse, adjust object dims
  if arg.exc
    arg.dim = [dim1 + 2*arg.Nbasis, 2*arg.Nbasis];
  else
    arg.dim = [2*dim1 + 2*arg.Nbasis, 2*arg.Nbasis];
  end

  % allocate space for basis perturbations
  arg.Gat = zeros(dim1,arg.Nbasis);
  arg.Gbt = zeros(dim1,arg.Nbasis);

end

arg.is.empty = false;

% now build object
%ob = Fatrix(arg.dim, arg, 'caller', mfilename, ...
%	'forw', @Gocrf_forw, 'back', @Gocrf_back);
ob = fatrix2('odim', arg.dim(1), 'idim', arg.dim(2), ...
	'arg', arg, ...
	'forw', @Gocrf_forw, 'back', @Gocrf_back); % jf

% if SVD matrices not supplied, do the necessary Bloch sims
if isempty(ob.arg.a0B)
  % get Bloch simulation grid
  ndgridarg = ['-fov(1)/2:fov(1)/N(1):fov(1)/2-fov(1)/N(1)'];
  posarg = ['x1'];
  for ii = 2:length(N)
    ndgridarg = [ndgridarg sprintf(',-fov(%d)/2:fov(%d)/N(%d):fov(%d)/2-fov(%d)/N(%d)',ii,ii,ii,ii,ii,ii)];
    posarg = [posarg sprintf(',x%d',ii)];
  end
  eval(sprintf('[%s] = ndgrid(%s);',posarg,ndgridarg));
  ob.arg.simpos = [x1(:)];
  for ii = 2:length(N)
    ob.arg.simpos = [ob.arg.simpos eval(sprintf('x%d(:)',ii))];
  end
  % do the Bloch sims and expansion calc
  [ob,m,mz] = Gocrf_update(ob,ob.arg.baseB1);
  % we don't need base B1 anymore
  ob.arg.baseB1 = [];
end

%
% Gocrf_new_B_Ct()
% new values of (svd) bases and coefficients
%
function G = Gocrf_new_B_Ct(G, a0B, a0Ct, b0B, b0Ct, a0, b0)

% apply frequency modulation to basis matrices
if ~isempty(G.arg.fmod)
  t = [0:G.arg.dt:(size(G.arg.k,1)-1)*G.arg.dt] - size(G.arg.k,1)*G.arg.dt/2;
  Amod = repmat(exp(1i*2*pi*G.arg.fmod*t(:)),[1 G.arg.Nsvd]);
  a0B = a0B .* Amod;
  b0B = b0B .* Amod;
end

if ~isempty(G.arg.we) & sum(abs(G.arg.we(:))) ~= 0
  % multiply out new expansion with field map expansion
  newa0B = [];newa0Ct = [];
  newb0B = [];newb0Ct = [];
  for ll = 1:G.arg.L
    newa0B = [newa0B spdiag(G.arg.B(:,ll))*a0B];
    newa0Ct = [newa0Ct spdiag(G.arg.Ct(:,ll))*a0Ct];
    newb0B = [newb0B spdiag(G.arg.B(:,ll))*b0B];
    newb0Ct = [newb0Ct spdiag(G.arg.Ct(:,ll))*b0Ct];
  end
  % update G with provided B and C.' matrices
  G.arg.a0G = feval(G.arg.a0G.arg.new_B_Ct,G.arg.a0G,newa0B,newa0Ct);
  G.arg.b0G = feval(G.arg.b0G.arg.new_B_Ct,G.arg.b0G,newb0B,newb0Ct);
else
  % set the expansion directly
  G.arg.a0G = feval(G.arg.a0G.arg.new_B_Ct,G.arg.a0G,a0B,a0Ct);
  G.arg.b0G = feval(G.arg.b0G.arg.new_B_Ct,G.arg.b0G,b0B,b0Ct);
end

G.arg.a0 = a0;
G.arg.b0 = b0;

%
% Gocrf_forw(): y = G * x
%
function v = Gocrf_forw(arg, u)

% assume the RF vector is [real(db1); imag(db1)]
db1 = u(1:length(u)/2) + 1i*u(length(u)/2+1:end);

if ~arg.rfb
  if any(db1)
    at = arg.b0G * conj(db1);
    bt = arg.a0G * conj(db1);
  else
    at = zeros(size(arg.b0G,1),1);
    bt = zeros(size(arg.a0G,1),1);
  end
else % pulse is parameterized
  if any(db1)
    at = arg.Gat * conj(db1);
    bt = arg.Gbt * conj(db1);
  else
    at = zeros(size(arg.Gat,1),1);
    bt = zeros(size(arg.Gbt,1),1);
  end
end

if arg.mdomswitch
  % dm terms, equilibrium
  dm = 0;dmz = 0;
  if arg.m0(3) ~= 0
    % dm terms, equilibrium
    dm = -2*arg.m0(3)*conj(arg.a0(:).*bt + at.*arg.b0(:));
    % dmz, equilibrium
    dmz = 2*arg.m0(3)*real(arg.a0(:).*conj(at)) - 2*arg.m0(3)*real(arg.b0(:).*conj(bt));
  end
  if any(arg.m0(1:2) ~= 0)
    % dm terms, non-equilibrium
    dm = dm - (arg.m0(1)-1i*arg.m0(2))*conj(2*arg.b0(:).*bt);
    dm = dm + (arg.m0(1)+1i*arg.m0(2))*conj(2*arg.a0(:).*at);
    % dmz, non-equilibrium
    dmz = dmz + 2*real((arg.m0(1)+1i*arg.m0(2))*(conj(arg.a0(:)).*bt + conj(at).*arg.b0(:)));
  end
  if ~arg.exc
    v = [arg.aux1.*dm;arg.aux2.*dmz;u];
  else
    v = [arg.aux1.*dm;u];
  end
else
  if ~arg.bsq
    v = [arg.aux1.*at;arg.aux2.*bt;u];
  else
    v = [2*arg.aux1.*arg.a0(:).*at;2*arg.aux2.*arg.b0(:).*bt;u];
  end
end

%
% Gocrf_back(): x = G' * y
%
function v = Gocrf_back(arg,u)

v = zeros(size(arg.a0G,2),1);
Ns = size(arg.a0G,1);
tmp1 = 0; tmp2 = 0;

% tmp1 contains terms that will multiply by system matrix for at
if arg.mdomswitch
  if arg.m0(3) ~= 0
    if ~arg.exc
      tmp1 = arg.m0(3)*2*conj(-arg.b0(:).*u(1:Ns)+conj(arg.a0(:)).*u(Ns+1:2*Ns));
    else
      tmp1 = arg.m0(3)*2*conj(-arg.b0(:).*u(1:Ns));
    end
  end
  if any(arg.m0(1:2) ~= 0)
    % additional alphat terms from Mxy, if non-equilibrium ic
    tmp1 = tmp1 + 2*conj((arg.m0(1)-1i*arg.m0(2))*arg.a0(:).*u(1:Ns));
    % additional alphat terms from Mz, if non-equilibrium ic
    if ~arg.exc
      tmp1 = tmp1 + 2*(arg.m0(1)+1i*arg.m0(2))*arg.b0(:).*conj(u(Ns+1:2*Ns));
    end
  end
  % tmp2 contains terms that will multiply by system matrix for betat
  if arg.m0(3) ~= 0
    if ~arg.exc
      tmp2 = -arg.m0(3)*2*conj(arg.a0(:).*u(1:Ns)+conj(arg.b0(:)).*u(Ns+1:2*Ns));
    else
      tmp2 = -arg.m0(3)*2*conj(arg.a0(:).*u(1:Ns));
    end
  end
  if any(arg.m0(1:2) ~= 0)
    % additional betat terms from Mxy, if non-equilibrium ic
    tmp2 = tmp2 - 2*conj((arg.m0(1)+1i*arg.m0(2))*arg.b0(:).*u(1:Ns));
    % additional betat terms from Mz, if non-equilibrium ic
    if ~arg.exc
      tmp2 = tmp2 + 2*conj((arg.m0(1)+1i*arg.m0(2))*conj(arg.a0(:)).*u(Ns+1:2*Ns));
    end
  end
else

  if ~arg.bsq
    tmp1 = u(1:Ns);
    tmp2 = u(Ns+1:2*Ns);
  else
    tmp1 = 2*conj(arg.a0(:)).*u(1:Ns);
    tmp2 = 2*conj(arg.b0(:)).*u(Ns+1:2*Ns);
  end
end

if ~arg.rfb
  v = conj(arg.a0G' * (conj(arg.aux2).*tmp2));
  v = v + conj(arg.b0G' * (conj(arg.aux1).*tmp1));
else
  v = conj(arg.Gbt' * (conj(arg.aux2).*tmp2));
  v = v + conj(arg.Gat' * (conj(arg.aux1).*tmp1));
end

if ~arg.exc
  v = [real(v);imag(v)] + u(2*Ns+1:end);
else
  v = [real(v);imag(v)] + u(Ns+1:end);
end

%
% Gocrf_subindcalc(): Determine subset of points to simulate for
%                     SVD evaluation
function arg = Gocrf_subindcalc(arg,d,indmask)

% get bin centers
if min(d(:)) == max(d(:))
  fa = min(d(:))*ones(arg.nsubbins+1,1);
else
  fa = min(d(:)):(max(d(:))-min(d(:)))/arg.nsubbins:max(d(:));
end

dind = 1:prod(size(d));

% arrange indices into bins
indices{1} = dind(col(logical(indmask.*(d >= fa(1) & d <= fa(2)))));
for ii = 2:length(fa)-1
  indices{ii} = dind(col(logical(indmask.*(d > fa(ii) & d <= fa(ii+1)))));
end

% loop through bins again and again, pulling one from each bin
% each time around at random (use rand*size(bin)), until
% index vector is full. This works much better than my
% previous method, which failed for filter-based desired patterns
indvec = [];
indind = 0;
rand('twister',1000);
while length(indvec) < arg.subindlen
  if ~isempty(indices{indind+1})
    binind = ceil(length(indices{indind+1})*rand(1,1));
    indvec = [indvec indices{indind+1}(binind)];
    indices{indind+1} = [indices{indind+1}(1:binind-1) ...
                        indices{indind+1}(binind+1:end)];
  end
  indind = mod(indind + 1,arg.nsubbins);
end

arg.subind = indvec;

%
% Gocrf_update(): Perform Bloch sims to update SVD expansions
%
function [G,m,mz] = Gocrf_update(G,baseB1)

% perform reduced Bloch sim
if isfield(G.arg, 'sens') && ~isempty(G.arg.sens) % jf

  if ~isempty(G.arg.we) & sum(abs(G.arg.we(:))) ~= 0
    [foo,fooz,alpha0,beta0] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos(G.arg.subind,:), ...
                                             G.arg.sens(G.arg.subind,:),G.arg.we(G.arg.subind));
  else
    [foo,fooz,alpha0,beta0] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos(G.arg.subind,:), ...
                                             G.arg.sens(G.arg.subind,:),G.arg.we);
  end

else

  if ~isempty(G.arg.we) & sum(abs(G.arg.we(:))) ~= 0
    [foo,fooz,alpha0,beta0] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos(G.arg.subind,:), ...
                                             ones(size(G.arg.subind)),G.arg.we(G.arg.subind));
  else
    [foo,fooz,alpha0,beta0] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos(G.arg.subind,:), ...
                                             ones(size(G.arg.subind)),G.arg.we);
  end

end

% calculate rotating frame transformation
ktmp = -[G.arg.k - repmat(G.arg.k(1,:),[size(G.arg.k,1) 1])];
Asub = exp(1i*2*pi/2*G.arg.simpos(G.arg.subind,:)*ktmp');
if ~isempty(G.arg.fmod)
  t = 0:G.arg.dt:G.arg.dt*(size(G.arg.k,1)-1);
  Asub = Asub.*exp(-1i*2*pi/2*repmat(G.arg.fmod*t,[length(G.arg.subind) 1]));
end

if ~isempty(G.arg.we) & sum(abs(G.arg.we(:))) ~= 0
  t = 0:G.arg.dt:G.arg.dt*(size(G.arg.k,1)-1);
  Asub = Asub.*exp(-1i*2*pi/2*G.arg.we(G.arg.subind).'*t);
end

% get temporal basis funcs via svd
[foo1,foo2,G.arg.a0B] = svd(conj(alpha0)./Asub,'econ');
[foo1,foo2,G.arg.b0B] = svd(conj(beta0)./Asub,'econ');

% truncate basis
G.arg.a0B = G.arg.a0B(:,1:G.arg.Nsvd);
G.arg.b0B = G.arg.b0B(:,1:G.arg.Nsvd);

% re-simulate to get coeffs at all spatial locs
if ~isempty(G.arg.sens)
  [m,mz,G.arg.a0,G.arg.b0,G.arg.a0Ct,G.arg.b0Ct] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos,G.arg.sens,G.arg.we,G.arg.simmask);
else
  [m,mz,G.arg.a0,G.arg.b0,G.arg.a0Ct,G.arg.b0Ct] = Gocrf_blochsim(baseB1,G.arg,G.arg.simpos,ones(size(G.arg.simpos,1),1),G.arg.we,G.arg.simmask);
end

m = reshape(m,G.arg.N);
mz = reshape(mz,G.arg.N);
G.arg.a0 = reshape(G.arg.a0,G.arg.N);
G.arg.b0 = reshape(G.arg.b0,G.arg.N);

% recalculate subindices based on Bloch sim results, in
% anticipation of the next update
%G.arg = Gocrf_subindcalc(G.arg,real(asin(abs(m))),G.arg.indmask);
jf_tmp = Gocrf_subindcalc(G.arg,real(asin(abs(m))),G.arg.indmask);
G = subsasgn_trick(G, 'arg', jf_tmp); % jf kludge for now

% then stick them into the Gmri objects
G = Gocrf_new_B_Ct(G,G.arg.a0B,G.arg.a0Ct,G.arg.b0B,G.arg.b0Ct,G.arg.a0,G.arg.b0);

if ~isempty(G.arg.RFbasis)
  % evaluate perturbations for basis functions
  for ll = 1:G.arg.Nbasis
    G.arg.Gat(:,ll) = G.arg.b0G * conj(G.arg.RFbasis(:,ll));
    G.arg.Gbt(:,ll) = G.arg.a0G * conj(G.arg.RFbasis(:,ll));
  end
end

%
% Gocrf_blochsim(): Perform a Bloch simulation
%
function [m,mz,a,b,a0Ct,b0Ct] = Gocrf_blochsim(B1,arg,pos,sens,we,simmask)

gambar = 42570000;               % gamma/2pi in Hz/T
gam = gambar*2*pi/10000;         % gamma in radians/g

if nargin == 6
  if ~isempty(simmask)
    % filter out un-sim'd spatial locs
    pos = pos(logical(simmask(:)),:);
    sens = sens(logical(simmask(:)),:);
    if ~isempty(we)
      we = we(logical(simmask));
    end
  end
end

Ns = size(pos,1);
Nt = size(arg.g,1);

statea = ones(Ns,1);
stateb = zeros(Ns,1);

% sum up RF over coils
B1 = reshape(B1,[size(arg.g,1) size(sens,2)]).';
bxy = sens * B1;

% sum up gradient over channels
bz = pos * arg.g';

% add off-resonance
if ~isempty(we) & sum(abs(we(:))) ~= 0
  bz = bz + repmat(we(:)/gam*2*pi,1,Nt);
end

if ~isempty(arg.fmod)
  bz = bz + arg.fmod/gam*2*pi;
end

tmp = zeros(2*Ns,1);

if nargout == 4
  % if we are only calling it with four output args,
  % assume we are doing the subspace sim and return all
  % states. Otherwise, we only return the final state.
  returnallstate = 1;
  a = zeros(Ns,Nt);
  b = zeros(Ns,Nt);
else; returnallstate = 0; end

% check if we need to do the running inner product to get spatial coeffs
if nargout == 6;
  inprod = 1;
  a0Ct = 0;b0Ct = 0;
  tvec = 0:arg.dt:(Nt-1)*arg.dt;
else; inprod = 0; end

ktmp = -[arg.k - repmat(arg.k(1,:),[size(arg.k,1) 1])];

for tt = 1:Nt

  phi = arg.dt*gam*(abs(bxy(:,tt)).^2+bz(:,tt).^2).^0.5;
  normfact = arg.dt*gam*(phi.^(-1));normfact(~isfinite(normfact)) = 0;
  nxy = normfact.*bxy(:,tt);nxy(~isfinite(nxy)) = 0;
  nz = normfact.*bz(:,tt);nz(~isfinite(nz)) = 0;
  cp = cos(phi/2);
  sp = sin(phi/2);
  alpha = cp+1i*nz.*sp;
  beta = 1i*conj(nxy).*sp;

  tmpa = alpha.*statea + beta.*stateb;
  tmpb = -conj(beta).*statea + conj(alpha).*stateb;

  statea = tmpa;stateb = tmpb;
  if any(~isfinite(statea)); keyboard; end;
  if any(~isfinite(stateb)); keyboard; end;
  if returnallstate
    a(:,tt) = statea;
    b(:,tt) = -conj(stateb);
  end

  if inprod
    % build drf transform vector
    Asub = exp(-1i*2*pi/2*pos*ktmp(tt,:).');
    if ~isempty(we) & sum(abs(we(:))) ~= 0
      Asub = Asub.*exp(1i*2*pi/2*we(:)*tvec(tt));
    end
    if ~isempty(arg.fmod)
      Asub = Asub.*exp(1i*2*pi/2*arg.fmod*ones(size(pos,1),1)*tvec(tt));
    end
    % get B matrix coeffs
    a0Ct = a0Ct + conj((conj(statea).*Asub)*arg.a0B(tt,:));
    b0Ct = b0Ct - conj((stateb.*Asub)*arg.b0B(tt,:));
  end

end

% return final alpha, beta if not returning the whole
% state progression
if ~returnallstate
  a = statea;
  b = -conj(stateb);
end

% calculate final magnetization state
mxy0 = arg.m0(1)+1i*arg.m0(2);
mz0 = arg.m0(3);
m = mz0*2*conj(statea).*stateb;
m = m + mxy0*conj(statea).^2;
m = m - conj(mxy0)*stateb.^2;
mz = mz0*(statea.*conj(statea) - stateb.*conj(stateb));
mz = mz + 2*real(mxy0*conj(statea).*-conj(stateb));

% embed results, if we did a masked sim
if nargin == 6
  if ~isempty(simmask)
    tmp = zeros(size(simmask(:)));tmp(logical(simmask)) = a;a = tmp;
    tmp = zeros(size(simmask(:)));tmp(logical(simmask)) = b;b = tmp;
    tmp = zeros(size(simmask(:)));tmp(logical(simmask)) = m;m = tmp;
    tmp = zeros(size(simmask(:)));tmp(logical(simmask)) = mz;mz = tmp;
    tmpa0Ct = zeros(length(simmask(:)),size(arg.a0B,2));
    tmpb0Ct = zeros(length(simmask(:)),size(arg.b0B,2));
    for ii = 1:size(arg.a0B,2)
      tmpa0Ct(logical(simmask),ii) = a0Ct(:,ii);
      tmpb0Ct(logical(simmask),ii) = b0Ct(:,ii);
    end
    a0Ct = tmpa0Ct;b0Ct = tmpb0Ct;
  end
end


report_this_filefun(mfilename('fullpath'));

ttcat = newt2;
mati = maepi(1,3);
%[p,sdp] = mypval(3,mati);
% [p,sdp,c,sdc,dk,sdk,aa,bb]=mypval2(3, mati);

tmin1 = 0.05;

save_aspar2;
do = [ ' ! '  hodi '/aspar/myaspar' ];
eval(do)

load aspar3.out
re = aspar3;

bv = re(2,2);
p =  re(1,2)
c =  re(4,2);
A =  re(3,2);

%[bv magco stan av me mer me2,  pr] =  bvalca3(newt2,1,1);
%[me, bv, si, av] = bmemag(newt2) ;

%A = log10(bv/av)
la = 0;
m0 = maepi(1,6);

m = min(newt2.Magnitude);
dt = 1;

t0 = ( max(newt2.Date) - mati)*365;

c

la = [];ti = [];
for t = c:dt:t0
    la = [la (10^(A + bv*(m0-m)) * (t + c)^(-p))*dt ];
    ti = [ti mati+t/365];
end

figure_w_normalized_uicontrolunits(cum)

l = newt2.Date >= mati + c/365;
tmpn = newt2(l,:);
nu = (1:length(tmpn(:,3))+1); nu(length(tmpn(:,3))+1) = length(tmpn(:,3));

try delete(plc); catch ME, error_handler(ME,@do_nothing);end
try delete(plc2); catch ME, error_handler(ME,@do_nothing);end

hold on
plc = plot([tmpn(:,3) ; teb],nu,'k');

plc2 = plot(ti,cumsum(la),'r');


function []=irfdisp(n,endo,IRFperiods,IRFt,irf_estimates,D_estimates,gamma_estimates,pref,strctident)

% function []=irfdisp(n,endo,IRFperiods,IRFt,irf_estimates,D_estimates,gamma_estimates,datapath)
% plots the results for the impulse response functions
% inputs:  - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - cell 'endo': list of endogenous variables of the model
%          - integer 'IRFperiods': number of periods for IRFs
%          - integer 'IRFt': determines which type of structural decomposition to apply (none, Choleski, triangular factorization)
%          - cell 'irf_estimates': lower bound, point estimates, and upper bound for the IRFs
%          - vector 'D_estimates': point estimate (median) of the structural matrix D, in vectorised form
%          - vector 'gamma_estimates': point estimate (median) of the structural disturbance variance-covariance matrix gamma, in vectorised form
%          - string 'datapath': user-supplied path to excel data spreadsheet
% outputs: none

% transpose the cell of records (required for the plot function in order to obtain correctly ordered plots)
irf_estimates=irf_estimates';

% number of identified shocks
if IRFt==1||IRFt==2||IRFt==3
    identified=n; % fully identified
elseif IRFt==4 || IRFt==6 %if the model is identified by sign restrictions or sign restrictions (+ IV)
    identified=size(strctident.signreslabels_shocks,1); % count the labels provided in the sign res sheet (+ IV)
elseif IRFt==5
    identified=1; % one IV shock
    strctident.signreslabels_shocks{1,1}=strcat('IV Shock (',strctident.Instrument,')'); % and generate the sign res label here
    strctident.signreslabels_shocksindex=1; % first shock
end

count=1;
namecount=1;

if pref.plot==1
% create figure for IRFs
irf=figure('Tag','BEARresults');
set(irf,'Color',[0.9 0.9 0.9]);
%set(irf,'position',[0,0,1920,1080])
   if IRFt==1
   irfname='impulse response functions (no structural identifcation)';
   elseif IRFt==2
   irfname='impulse response functions (structural identification by Cholesky ordering)';
   elseif IRFt==3
   irfname='impulse response functions (structural identification by triangular factorisation)';
   elseif IRFt==4
   irfname=['impulse response functions (structural identification by ',strctident.hbartext_signres,strctident.hbartext_favar_signres,strctident.hbartext_zerores,strctident.hbartext_favar_zerores,strctident.hbartext_magnres,strctident.hbartext_favar_magnres,strctident.hbartext_relmagnres,strctident.hbartext_favar_relmagnres,strctident.hbartext_FEVDres,strctident.hbartext_favar_FEVDres,strctident.hbartext_CorrelInstrumentShock,':::',' restrictions)'];
   irfname=erase(irfname,', :::'); % delete the last ,
   elseif IRFt==5
   irfname=['impulse response functions (structural identification by IV (',strctident.Instrument,'))'];
   elseif IRFt==6
   irfname_temp=[strctident.hbartext_signres,strctident.hbartext_favar_signres,strctident.hbartext_zerores,strctident.hbartext_favar_zerores,strctident.hbartext_magnres,strctident.hbartext_favar_magnres,strctident.hbartext_relmagnres,strctident.hbartext_favar_relmagnres,strctident.hbartext_FEVDres,strctident.hbartext_favar_FEVDres,strctident.hbartext_CorrelInstrumentShock,':::',' restrictions)'];
   irfname_temp=erase(irfname_temp,', :::'); % delete the last ,
   irfname=['impulse response functions (structural identification by IV (',strctident.Instrument,') & ',irfname_temp];
   end
   set(irf,'name',irfname);

if IRFt==1||IRFt==2||IRFt==3
for ii=1:n^2
subplot(n,n,ii);
hold on
Xpatch=[(1:IRFperiods) (IRFperiods:-1:1)];
Ypatch=[irf_estimates{ii}(1,:) fliplr(irf_estimates{ii}(3,:))];
IRFpatch=patch(Xpatch,Ypatch,[0.7 0.78 1]);
set(IRFpatch,'facealpha',0.5);
set(IRFpatch,'edgecolor','none');
plot(irf_estimates{ii}(2,:),'Color',[0.4 0.4 1],'LineWidth',2);
plot([1,IRFperiods],[0 0],'k--');
hold off
minband=min(irf_estimates{ii}(1,:));
maxband=max(irf_estimates{ii}(3,:));
space=maxband-minband;
Ymin=minband-0.2*space;
Ymax=maxband+0.2*space;
set(gca,'XLim',[1 IRFperiods],'YLim',[Ymin Ymax],'FontName','Times New Roman');
% top labels
   if ii<=n
   title(endo{ii,1},'FontWeight','normal','interpreter','none');
   end
% side labels
   if rem((ii-1)/n,1)==0
   ylabel(endo{(ii-1)/n+1,1},'FontWeight','normal','interpreter','none');
   end
end
%% IRFt==4||IRFt==5||IRFt==6
elseif IRFt==4||IRFt==5||IRFt==6
    % subset of irf_estimates and signreslabels that we want to plot
    irf_estimates_shocks=irf_estimates(strctident.signreslabels_shocksindex,:);
    signreslabels_shocks=strctident.signreslabels_shocks;
for jj=1:identified
    for ii=1:n
        count;
        subplot(n,identified,jj+identified*(ii-1));
hold on
Xpatch=[(1:IRFperiods) (IRFperiods:-1:1)];
Ypatch=[irf_estimates_shocks{jj,ii}(1,:) fliplr(irf_estimates_shocks{jj,ii}(3,:))];
IRFpatch=patch(Xpatch,Ypatch,[0.7 0.78 1]);
set(IRFpatch,'facealpha',0.5);
set(IRFpatch,'edgecolor','none');
plot(irf_estimates_shocks{jj,ii}(2,:),'Color',[0.4 0.4 1],'LineWidth',2);
plot([1,IRFperiods],[0 0],'k--');
hold off
minband=min(irf_estimates_shocks{jj,ii}(1,:));
maxband=max(irf_estimates_shocks{jj,ii}(3,:));
space=maxband-minband;
Ymin=minband-0.2*space;
Ymax=maxband+0.2*space;
set(gca,'XLim',[1 IRFperiods],'YLim',[Ymin Ymax],'FontName','Times New Roman');
% top labels
subplotcolumn=jj+identified*(ii-1);
if subplotcolumn <= identified
title(signreslabels_shocks{subplotcolumn,1},'FontWeight','normal','interpreter','none');
end
if count <= n
   ylabel(endo{namecount,1},'FontWeight','normal','interpreter','none');
   namecount=namecount+1;
end
count = count+1;
end
end
end
% top supertitle
ax=axes('Units','Normal','Position',[.11 .075 .85 .88],'Visible','off');
set(get(ax,'Title'),'Visible','on')
title('Shock:','FontSize',11,'FontName','Times New Roman','FontWeight','normal','interpreter','none');
% side supertitle
ylabel('Response of:','FontSize',12,'FontName','Times New Roman','FontWeight','normal','interpreter','none');
set(get(ax,'Ylabel'),'Visible','on')
    set(irf,'PaperPositionMode','Auto')
% % %      PrintName = strcat('IRFs');
% % %      fname = strcat(pref.datapath, '\results\');
% % %      saveas(irf,[fname,PrintName],'epsc')
% % %      saveas(irf,[fname,PrintName],'png')
end

if IRFt==2 || IRFt==3 || IRFt==4 || IRFt==5 || IRFt==6
% then display the results for D and gamma, if a structural decomposition was selected

filelocation=fullfile(pref.results_path, [pref.results_sub '.txt']);
fid=fopen(filelocation,'at');

%print three empty lines
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

%print structural decomposition matrix D
svarinfo1=['D (structural decomposition matrix): posterior estimates'];
fprintf('%s\n',svarinfo1);
fprintf(fid,'%s\n',svarinfo1);

% recover D
D=reshape(D_estimates,n,n);
% calculate the (integer) length of the largest number in D, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(D))))));
% add a separator, a potential minus sign and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:n
temp=[];
   for jj=1:n
   % convert matrix entry into string
   number=num2str(D(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end

%print two empty lines
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

%print structural disturbances covariance matrix gamma
svarinfo2=['gamma (structural disturbances covariance matrix): posterior estimates'];
fprintf('%s\n',svarinfo2);
fprintf(fid,'%s\n',svarinfo2);

% recover gamma
gamma=reshape(gamma_estimates,n,n);
% calculate the (integer) length of the largest number in D, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(gamma))))));
% add a separator, a potential minus sign and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:n
temp=[];
   for jj=1:n
   % convert matrix entry into string
   number=num2str(gamma(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end % pref.plot
fclose(fid);
end

% finally, record results in excel
if pref.results==1
% retranspose the cell of records
irf_estimates=irf_estimates';
bear.data.excelrecord4fcn(identified, IRFperiods, IRFt, endo, strctident, irf_estimates, n, pref)
end


%Vehicle Abstract vehicle class
%
% This abstract class models the kinematics of a mobile robot moving on
% a plane and with a pose in SE(2).  For given steering and velocity inputs it
% updates the true vehicle state and returns noise-corrupted odometry
% readings.
%
% Methods::
%   Vehicle          constructor
%   add_driver       attach a driver object to this vehicle
%   control          generate the control inputs for the vehicle
%   f                predict next state based on odometry
%   init             initialize vehicle state
%   run              run for multiple time steps
%   run2             run with control inputs
%   step             move one time step and return noisy odometry
%   update           update the vehicle state
%
% Plotting/display methods::
%   char             convert to string
%   display          display state/parameters in human readable form
%   plot             plot/animate vehicle on current figure
%   plot_xy          plot the true path of the vehicle
%   Vehicle.plotv    plot/animate a pose on current figure
%
% Properties (read/write)::
%   x               true vehicle state: x, y, theta (3x1)
%   V               odometry covariance (2x2)
%   odometry        distance moved in the last interval (2x1)
%   rdim             dimension of the robot (for drawing)
%   L               length of the vehicle (wheelbase)
%   alphalim        steering wheel limit
%   speedmax        maximum vehicle speed
%   T               sample interval
%   verbose         verbosity
%   x_hist          history of true vehicle state (Nx3)
%   driver          reference to the driver object
%   x0              initial state, restored on init()
%
% Examples::
%
% If veh is an instance of a Vehicle class then we can add a driver object
%      veh.add_driver( RandomPath(10) )
% which will move the vehicle within the region -10<x<10, -10<y<10 which we
% can see by
%      veh.run(1000)
% which shows an animation of the vehicle moving for 1000 time steps
% between randomly selected wayoints.
%
% Notes::
% - Subclass of the MATLAB handle class which means that pass by reference semantics
%   apply.
%
% Reference::
%
%   Robotics, Vision & Control, Chap 6
%   Peter Corke,
%   Springer 2011
%
% See also Bicycle, Unicycle, RandomPath, EKF.

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

classdef Vehicle < handle

    properties
        % state
        x           % true state (x,y,theta)
        x_hist      % x history

        % parameters

        speedmax    % maximum speed
        dim         % dimension of the world -dim -> +dim in x and y
        rdim    % dimension of the robot
        dt           % sample interval
        V           % odometry covariance
        odometry    % distance moved in last interval
        verbose
        driver      % driver object
        x0          % initial state
        options

        vhandle     % handle to vehicle graphics object
        vtrail      % vehicle trail
    end

    methods(Abstract)
        f
    end

    methods

        function veh = Vehicle(varargin)
        %Vehicle Vehicle object constructor
        %
        % V = Vehicle(OPTIONS) creates a Vehicle object that implements the
        % kinematic model of a wheeled vehicle.
        %
        % Options::
        % 'covar',C       specify odometry covariance (2x2) (default 0)
        % 'speedmax',S    Maximum speed (default 1m/s)
        % 'L',L           Wheel base (default 1m)
        % 'x0',x0         Initial state (default (0,0,0) )
        % 'dt',T          Time interval (default 0.1)
        % 'rdim',R        Robot size as fraction of plot window (default 0.2)
        % 'verbose'       Be verbose
        %
        % Notes::
        % - The covariance is used by a "hidden" random number generator within the class.
        % - Subclasses the MATLAB handle class which means that pass by reference semantics
        %   apply.

            % vehicle common
            opt.covar = [];
            opt.rdim = 0.2;
            opt.dt = 0.1;
            opt.x0 = zeros(3,1);
            opt.speedmax = 1;
            opt.vhandle = [];

            [opt,args] = tb_optparse(opt, varargin);

            veh.V = opt.covar;
            veh.rdim = opt.rdim;
            veh.dt = opt.dt;
            veh.x0 = opt.x0(:);
            assert(isvec(veh.x0, 3), 'Initial configuration must be a 3-vector');
            veh.speedmax = opt.speedmax;
            veh.options = args;  % unused options go back to the subclass
            veh.vhandle = opt.vhandle;
            veh.x_hist = [];
        end

        function init(veh, x0)
            %Vehicle.init Reset state
            %
            % V.init() sets the state V.x := V.x0, initializes the driver
            % object (if attached) and clears the history.
            %
            % V.init(X0) as above but the state is initialized to X0.

            % TODO: should this be called from run?

            if nargin > 1
                veh.x = x0(:);
            else
                veh.x = veh.x0;
            end
            veh.x_hist = [];

            if ~isempty(veh.driver)
                veh.driver.init();
            end

            veh.vhandle = [];
        end

        function yy = path(veh, t, u, y0)
            %Vehicle.path Compute path for constant inputs
            %
            % XF = V.path(TF, U) is the final state of the vehicle (3x1) from the initial
            % state (0,0,0) with the control inputs U (vehicle specific).  TF is  a scalar to
            % specify the total integration time.
            %
            % XP = V.path(TV, U) is the trajectory of the vehicle (Nx3) from the initial
            % state (0,0,0) with the control inputs U (vehicle specific).  T is a vector (N) of
            % times for which elements of the trajectory will be computed.
            %
            % XP = V.path(T, U, X0) as above but specify the initial state.
            %
            % Notes::
            % - Integration is performed using ODE45.
            % - The ODE being integrated is given by the deriv method of the vehicle object.
            %
            % See also ODE45.

                if length(t) == 1
                    tt = [0 t];
                else
                    tt = t;
                end

                if nargin < 4
                    y0 = [0 0 0];
                end
                out = ode45( @(t,y) veh.deriv(t, y, u), tt, y0);

                y = out.y';
                if nargout == 0
                    plot(y(:,1), y(:,2));
                    grid on
                    xlabel('X'); ylabel('Y')
                else
                    yy = y;
                    if length(t) == 1
                        % if scalar time given, just return final state
                        yy = yy(end,:);
                    end
                end
        end

        function add_driver(veh, driver)
            %Vehicle.add_driver Add a driver for the vehicle
            %
            % V.add_driver(D) connects a driver object D to the vehicle.  The driver
            % object has one public method:
            %        [speed, steer] = D.demand();
            % that returns a speed and steer angle.
            %
            % Notes::
            % - The Vehicle.step() method invokes the driver if one is attached.
            %
            % See also Vehicle.step, RandomPath.
            veh.driver = driver;
            driver.veh = veh;
        end

        function odo = update(veh, u)
            %Vehicle.update Update the vehicle state
            %
            % ODO = V.update(U) is the true odometry value for
            % motion with U=[speed,steer].
            %
            % Notes::
            % - Appends new state to state history property x_hist.
            % - Odometry is also saved as property odometry.

            xp = veh.x; % previous state
            veh.x(1) = veh.x(1) + u(1)*veh.dt*cos(veh.x(3));
            veh.x(2) = veh.x(2) + u(1)*veh.dt*sin(veh.x(3));
            veh.x(3) = veh.x(3) + u(1)*veh.dt/veh.L * u(2);
            odo = [colnorm(veh.x(1:2)-xp(1:2)) veh.x(3)-xp(3)];
            veh.odometry = odo;

            veh.x_hist = [veh.x_hist; veh.x'];   % maintain history
        end

        function odo = step(veh, varargin)
            %Vehicle.step Advance one timestep
            %
            % ODO = V.step(SPEED, STEER) updates the vehicle state for one timestep
            % of motion at specified SPEED and STEER angle, and returns noisy odometry.
            %
            % ODO = V.step() updates the vehicle state for one timestep of motion and
            % returns noisy odometry.  If a "driver" is attached then its DEMAND() method
            % is invoked to compute speed and steer angle.  If no driver is attached
            % then speed and steer angle are assumed to be zero.
            %
            % Notes::
            % - Noise covariance is the property V.
            %
            % See also Vehicle.control, Vehicle.update, Vehicle.add_driver.

            % get the control input to the vehicle from either passed demand or driver
            u = veh.control(varargin{:});

            % compute the true odometry and update the state
            odo = veh.update(u);

            % add noise to the odometry
            if ~isempty(veh.V)
                odo = veh.odometry + randn(1,2)*sqrtm(veh.V);
            end
        end

        function u = control(veh, speed, steer)
            %Vehicle.control Compute the control input to vehicle
            %
            % U = V.control(SPEED, STEER) is a control input (1x2) = [speed,steer]
            % based on provided controls SPEED,STEER to which speed and steering angle
            % limits have been applied.
            %
            % U = V.control() as above but demand originates with a "driver" object if
            % one is attached, the driver's DEMAND() method is invoked. If no driver is
            % attached then speed and steer angle are assumed to be zero.
            %
            % See also Vehicle.step, RandomPath.
            if nargin < 2
                % if no explicit demand, and a driver is attached, use
                % it to provide demand
                if ~isempty(veh.driver)
                    [speed, steer] = veh.driver.demand();
                else
                    % no demand, do something safe
                    speed = 0;
                    steer = 0;
                end
            end

            % clip the speed
            if isempty(veh.speedmax)
                u(1) = speed;
            else
                u(1) = min(veh.speedmax, max(-veh.speedmax, speed));
            end

            % clip the steering angle
            if isprop(veh, 'steermax') && ~isempty(veh.steermax)
                u(2) = max(-veh.steermax, min(veh.steermax, steer));
            else
                u(2) = steer;
            end
        end

        function p = run(veh, nsteps)
            %Vehicle.run Run the vehicle simulation
            %
            % V.run(N) runs the vehicle model for N timesteps and plots
            % the vehicle pose at each step.
            %
            % P = V.run(N) runs the vehicle simulation for N timesteps and
            % return the state history (Nx3) without plotting.  Each row
            % is (x,y,theta).
            %
            % See also Vehicle.step, Vehicle.run2.

            if nargin < 2
                nsteps = 1000;
            end
            if ~isempty(veh.driver)
                veh.driver.init()
            end
            %veh.clear();
            if ~isempty(veh.driver)
                veh.driver.plot();
            end

            veh.plot();
            for i=1:nsteps
                veh.step();
                if nargout == 0
                    % if no output arguments then plot each step
                    veh.plot();
                    drawnow
                end
            end
            p = veh.x_hist;
        end

        % TODO run and run2 should become superclass methods...

        function p = run2(veh, T, x0, speed, steer)
            %Vehicle.run2 Run the vehicle simulation with control inputs
            %
            % P = V.run2(T, X0, SPEED, STEER) runs the vehicle model for a time T with
            % speed SPEED and steering angle STEER.  P (Nx3) is the path followed and
            % each row is (x,y,theta).
            %
            % Notes::
            % - Faster and more specific version of run() method.
            % - Used by the RRT planner.
            %
            % See also Vehicle.run, Vehicle.step, RRT.
            veh.init(x0);

            for i=1:(T/veh.dt)
                veh.update([speed steer]);
            end
            p = veh.x_hist;
        end

        function h = plot(veh, varargin)
        %Vehicle.plot Plot vehicle
        %
        % The vehicle is depicted graphically as a narrow triangle that travels
        % "point first" and has a length V.rdim.
        %
        % V.plot(OPTIONS) plots the vehicle on the current axes at a pose given by
        % the current robot state.  If the vehicle has been previously plotted its
        % pose is updated.
        %
        % V.plot(X, OPTIONS) as above but the robot pose is given by X (1x3).
        %
        % H = V.plotv(X, OPTIONS) draws a representation of a ground robot as an
        % oriented triangle with pose X (1x3) [x,y,theta].  H is a graphics handle.
        %
        % V.plotv(H, X) as above but updates the pose of the graphic represented
        % by the handle H to pose X.
        %
        % Options::
        % 'scale',S    Draw vehicle with length S x maximum axis dimension
        % 'size',S     Draw vehicle with length S
        % 'color',C    Color of vehicle.
        % 'fill'       Filled
        % 'trail',S    Trail with line style S, use line() name-value pairs
        %
        % Example::
        %          veh.plot('trail', {'Color', 'r', 'Marker', 'o', 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'r', 'MarkerSize', 3})

        % Notes::
        % - The last two calls are useful if animating multiple robots in the same
        %   figure.
        %
        % See also Vehicle.plotv, plot_vehicle.

            if isempty(veh.vhandle)
                veh.vhandle = Vehicle.plotv(veh.x, varargin{:});
            end

            if ~isempty(varargin) && isnumeric(varargin{1})
                % V.plot(X)
                pos = varargin{1}; % use passed value
            else
                % V.plot()
                pos = veh.x;    % use current state
            end

            % animate it
            Vehicle.plotv(veh.vhandle, pos);

            end

        function out = plot_xy(veh, varargin)
            %Vehicle.plot_xy Plots true path followed by vehicle
            %
            % V.plot_xy() plots the true xy-plane path followed by the vehicle.
            %
            % V.plot_xy(LS) as above but the line style arguments LS are passed
            % to plot.
            %
            % Notes::
            % - The path is extracted from the x_hist property.

            xyt = veh.x_hist;
            if nargout == 0
                plot(xyt(:,1), xyt(:,2), varargin{:});
            else
                out = xyt;
            end
        end

        function verbosity(veh, v)
        %Vehicle.verbosity Set verbosity
        %
        % V.verbosity(A) set verbosity to A.  A=0 means silent.
            veh.verbose = v;
        end

        function display(nav)
        %Vehicle.display Display vehicle parameters and state
        %
        % V.display() displays vehicle parameters and state in compact
        % human readable form.
        %
        % Notes::
        % - This method is invoked implicitly at the command line when the result
        %   of an expression is a Vehicle object and the command has no trailing
        %   semicolon.
        %
        % See also Vehicle.char.

            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            disp( char(nav) );
        end % display()

        function s = char(veh)
        %Vehicle.char Convert to string
        %
        % s = V.char() is a string showing vehicle parameters and state in
        % a compact human readable format.
        %
        % See also Vehicle.display.

            s = '  Superclass: Vehicle';
            s = char(s, sprintf(...
            '    max speed=%g, dT=%g, nhist=%d', ...
                veh.speedmax, veh.dt, ...
                numrows(veh.x_hist)));
            if ~isempty(veh.V)
                s = char(s, sprintf(...
                '    V=(%g, %g)', ...
                    veh.V(1,1), veh.V(2,2)));
            end
            s = char(s, sprintf('    configuration: x=%g, y=%g, theta=%g', veh.x));
            if ~isempty(veh.driver)
                s = char(s, '    driven by::');
                s = char(s, [['      '; '      '] char(veh.driver)]);
            end
        end

    end % method

    methods(Static)

        function h = plotv(varargin)
        %Vehicle.plotv Plot ground vehicle pose
        %
        % H = Vehicle.plotv(X, OPTIONS) draws a representation of a ground robot as an
        % oriented triangle with pose X (1x3) [x,y,theta].  H is a graphics handle.
        % If X (Nx3) is a matrix it is considered to represent a trajectory in which case
        % the vehicle graphic is animated.
        %
        % Vehicle.plotv(H, X) as above but updates the pose of the graphic represented
        % by the handle H to pose X.
        %
        % Options::
        % 'scale',S       Draw vehicle with length S x maximum axis dimension
        % 'size',S        Draw vehicle with length S
        % 'fillcolor',C   Color of vehicle.
        % 'fps',F         Frames per second in animation mode (default 10)
        %
        % Example::
        %
        % Generate some path 3xN
        %         p = PRM.plan(start, goal);
        % Set the axis dimensions to stop them rescaling for every point on the path
        %         axis([-5 5 -5 5]);
        %
        % Now invoke the static method
        %         Vehicle.plotv(p);
        %
        % Notes::
        % - This is a class method.
        %
        % See also Vehicle.plot.

        if isstruct(varargin{1})
            plot_vehicle(varargin{2}, 'handle', varargin{1});
        else
            h = plot_vehicle(varargin{1}, 'fillcolor', 'b', 'alpha', 0.5);
        end

        end
    end % static methods

end % classdef


classdef PlaneStressTransformerFactory < handle

    properties (Access = private)
        PSTransformer
        tensor
    end

    methods (Access = public)

        function psTransformer = create(obj,Tensor)
            obj.init(Tensor)
            obj.createPlaneStressTransformer()
            psTransformer = obj.getPlaneStressTransformer();
      end
    end

    methods (Access = private)

        function init(obj,tensor)
            obj.tensor = tensor;
        end

        function createPlaneStressTransformer(obj)
            t = obj.tensor;
            ps = [];
            if obj.isVoigt()
                if obj.isStiffnessTensor()
                    if obj.isInvertible()
                        ps = PST4VoigtFourthOrderTensorNumerically(t.getValue());
                    else
                        ps = PST4VoigtFourthOrderTensorSymbolically(t.getValue());
                    end

                elseif obj.isSecondOrder()
                    ps = PST4VoigtSecondOrderTensor(t);
                end

            elseif obj.isTensor()
                if obj.isSecondOrder()
                    ps =  PlaneStressTransformerForSecondOrderTensor(t);
                end

            end

            if isempty(ps)
                error('Not admitted object to make it Plane Stress')
            else
                obj.PSTransformer = ps;
            end

        end

        function PS = getPlaneStressTransformer(obj)
            PS = obj.PSTransformer;
        end

        function itIs = isVoigt(obj)
            itIs = strcmp(obj.tensor.getRepresentation(),'voigt');
        end

        function itIs = isTensor(obj)
            itIs = strcmp(obj.tensor.getRepresentation(),'tensor');
        end

        function ItIs = isSecondOrder(obj)
            ItIs = strcmp(obj.tensor.getOrder(),'second');
        end

         function itIs = isStiffnessTensor(obj)
            itIs = strcmp(obj.tensor.getFieldName,'stiffness');
        end

        function itIs = isInvertible(obj)
            t = obj.tensor.getValue();
            if obj.isSymbolic(t)
                itIs = isAlways(det(t) == 0);
            else
                itIs = min(abs(eig(t))) > 1e-13;
            end
        end

    end

    methods (Access = private, Static)
        function itIs = isSymbolic(a)
            itIs = isa(a,'sym');
        end
    end

end


function model = ivmOptimiseNoise(model, display, iters);

% IVMOPTIMISENOISE Optimise the noise parameters.
% FORMAT
% DESC optimises the noise parameters in the IVM model.
% ARG model : the model for which the noise parameters are to be
% optimised.
% ARG display : how much to display during optimisation (defaults
% to level 1).
% ARG iters : how many iterations of optimisation to use (defaults
% to 500).
%
% SEEALSO : optimiseParams, defaultOptions, ivmNegLogLikelihood, ivmNegGradientNoise
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% IVM

if nargin < 3
  iters = 500;
  if nargin < 2
    display = 1;
  end
end
options = defaultOptions;
if display
  options(1) = 1;
end
options(14) = iters;

model = optimiseParams('noise', 'scg', 'ivmNegLogLikelihood', ...
                       'ivmNegGradientNoise', options, model);


function [modelPlanePyr, modelRegPyr] = sc_planar_structure_pyramid(scaleImgPyr, modelPlane, modelReg)

%
% SC_PLANAR_STRUCTURE_PYRAMID:
%
% Pre-compute the models of planes and regularity according to the image
% pyramid, rescale parameters according to the image sizes.
%
% Output:
%   - modelPlanePyr: plane model
%   - modelRegPyr: regularity model

numLevel = length(scaleImgPyr);

modelPlanePyr = cell(numLevel, 1);
modelRegPyr = cell(numLevel, 1);

planePostProb = modelPlane.postProb;

for iLvl = 1: numLevel
    % === Update plane model ===
    scaleImgCur = scaleImgPyr{iLvl}.imgScale;
    sizeImgCur = scaleImgPyr{iLvl}.imgSize;

    % Update posterior plane probability
    planePostProbCur = imresize(planePostProb, sizeImgCur, 'bicubic');
    planePostProbCurSum = sum(planePostProbCur, 3);
    planePostProbCur = bsxfun(@rdivide, planePostProbCur, planePostProbCurSum);
    modelPlanePyr{iLvl}.numPlane = modelPlane.numPlane;
    modelPlanePyr{iLvl}.planeProb = planePostProbCur;
    modelPlanePyr{iLvl}.mLogLPlaneProb = -log(planePostProbCur);

    % Update rectification matrix and rotation parameters
    for iPlane = 1: modelPlane.numPlane
        H = eye(3);
        vLine = modelPlane.plane{iPlane}.vLine;
        vLine(1:2) = vLine(1:2)/scaleImgCur;
        H(3,:) = vLine;
        modelPlanePyr{iLvl}.rectMat{iPlane} = H;
        modelPlanePyr{iLvl}.rotPar{iPlane} = modelPlane.plane{iPlane}.rotPar;

        % Construct the rotation matrices
        for iTheta = 1:2
            t = modelPlanePyr{iLvl}.rotPar{iPlane}(iTheta);
            Hr = eye(3);
            Hr(1,1) = cos(t);   Hr(1,2) = - sin(t);
            Hr(2,1) = sin(t);   Hr(2,2) =   cos(t);
            modelPlanePyr{iLvl}.rotMat{iPlane, iTheta} = Hr;
        end
    end

    % === Update reguarlity model ===
    for iPlane = 1: modelPlane.numPlane
        modelRegPyr{iLvl}.dispVec{iPlane} = scaleImgCur*modelReg.plane{iPlane}.dispVec;
        modelRegPyr{iLvl}.numDispVec(iPlane) = modelReg.plane{iPlane}.numDispVec;
    end
end

end


function res = adjSPIRiT(kData, kernel)

%  res = adjSPIRiT(kData, kernel)
%
%	backprojection of the SPIRiT operator on the data
%
%	input:
%		kData: k-space data (sx x sy x ncoils)
%		kernel : SPIRiT kernel
%
%	(c) Michael Lustig 2006

	kernel = conj(kernel(end:-1:1,end:-1:1,:));
	res = kData*0;

	numCoils = size(kernel,3);
	for n=1:numCoils
		res(:,:,n) = filter2(kernel(:,:,n), kData(:,:),'same');
	end


function forest = forestTrain( data, hs, varargin )
% Train random forest classifier.
%
% Dimensions:
%  M - number trees
%  F - number features
%  N - number input vectors
%  H - number classes
%
% USAGE
%  forest = forestTrain( data, hs, [varargin] )
%
% INPUTS
%  data     - [NxF] N length F feature vectors
%  hs       - [Nx1] or {Nx1} target output labels in [1,H]
%  varargin - additional params (struct or name/value pairs)
%   .M          - [1] number of trees to train
%   .H          - [max(hs)] number of classes
%   .N1         - [5*N/M] number of data points for training each tree
%   .F1         - [sqrt(F)] number features to sample for each node split
%   .split      - ['gini'] options include 'gini', 'entropy' and 'twoing'
%   .minCount   - [1] minimum number of data points to allow split
%   .minChild   - [1] minimum number of data points allowed at child nodes
%   .maxDepth   - [64] maximum depth of tree
%   .dWts       - [] weights used for sampling and weighing each data point
%   .fWts       - [] weights used for sampling features
%   .discretize - [] optional function mapping structured to class labels
%                    format: [hsClass,hBest] = discretize(hsStructured,H);
%
% OUTPUTS
%  forest   - learned forest model struct array w the following fields
%   .fids     - [Kx1] feature ids for each node
%   .thrs     - [Kx1] threshold corresponding to each fid
%   .child    - [Kx1] index of child for each node
%   .distr    - [KxH] prob distribution at each node
%   .hs       - [Kx1] or {Kx1} most likely label at each node
%   .count    - [Kx1] number of data points at each node
%   .depth    - [Kx1] depth of each node
%
% EXAMPLE
%  N=10000; H=5; d=2; [xs0,hs0,xs1,hs1]=demoGenData(N,N,H,d,1,1);
%  xs0=single(xs0); xs1=single(xs1);
%  pTrain={'maxDepth',50,'F1',2,'M',150,'minChild',5};
%  tic, forest=forestTrain(xs0,hs0,pTrain{:}); toc
%  hsPr0 = forestApply(xs0,forest);
%  hsPr1 = forestApply(xs1,forest);
%  e0=mean(hsPr0~=hs0); e1=mean(hsPr1~=hs1);
%  fprintf('errors trn=%f tst=%f\n',e0,e1); figure(1);
%  subplot(2,2,1); visualizeData(xs0,2,hs0);
%  subplot(2,2,2); visualizeData(xs0,2,hsPr0);
%  subplot(2,2,3); visualizeData(xs1,2,hs1);
%  subplot(2,2,4); visualizeData(xs1,2,hsPr1);
%
% See also forestApply, fernsClfTrain
%
% Piotr's Computer Vision Matlab Toolbox      Version 3.24
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

% get additional parameters and fill in remaining parameters
dfs={ 'M',1, 'H',[], 'N1',[], 'F1',[], 'split','gini', 'minCount',1, ...
  'minChild',1, 'maxDepth',64, 'dWts',[], 'fWts',[], 'discretize','' };
[M,H,N1,F1,splitStr,minCount,minChild,maxDepth,dWts,fWts,discretize] = ...
  getPrmDflt(varargin,dfs,1);
[N,F]=size(data); assert(length(hs)==N); discr=~isempty(discretize);
minChild=max(1,minChild); minCount=max([1 minCount minChild]);
if(isempty(H)), H=max(hs); end; assert(discr || all(hs>0 & hs<=H));
if(isempty(N1)), N1=round(5*N/M); end; N1=min(N,N1);
if(isempty(F1)), F1=round(sqrt(F)); end; F1=min(F,F1);
if(isempty(dWts)), dWts=ones(1,N,'single'); end; dWts=dWts/sum(dWts);
if(isempty(fWts)), fWts=ones(1,F,'single'); end; fWts=fWts/sum(fWts);
split=find(strcmpi(splitStr,{'gini','entropy','twoing'}))-1;
if(isempty(split)), error('unknown splitting criteria: %s',splitStr); end

% make sure data has correct types
if(~isa(data,'single')), data=single(data); end
if(~isa(hs,'uint32') && ~discr), hs=uint32(hs); end
if(~isa(fWts,'single')), fWts=single(fWts); end
if(~isa(dWts,'single')), dWts=single(dWts); end

% train M random trees on different subsets of data
prmTree = {H,F1,minCount,minChild,maxDepth,fWts,split,discretize};
for i=1:M
  if(N==N1), data1=data; hs1=hs; dWts1=dWts; else
    d=wswor(dWts,N1,4); data1=data(d,:); hs1=hs(d);
    dWts1=dWts(d); dWts1=dWts1/sum(dWts1);
  end
  tree = treeTrain(data1,hs1,dWts1,prmTree);
  if(i==1), forest=tree(ones(M,1)); else forest(i)=tree; end
end

end

function tree = treeTrain( data, hs, dWts, prmTree )
% Train single random tree.
[H,F1,minCount,minChild,maxDepth,fWts,split,discretize]=deal(prmTree{:});
N=size(data,1); K=2*N-1; discr=~isempty(discretize);
thrs=zeros(K,1,'single'); distr=zeros(K,H,'single');
fids=zeros(K,1,'uint32'); child=fids; count=fids; depth=fids;
hsn=cell(K,1); dids=cell(K,1); dids{1}=uint32(1:N); k=1; K=2;
while( k < K )
  % get node data and store distribution
  dids1=dids{k}; dids{k}=[]; hs1=hs(dids1); n1=length(hs1); count(k)=n1;
  if(discr), [hs1,hsn{k}]=feval(discretize,hs1,H); hs1=uint32(hs1); end
  if(discr), assert(all(hs1>0 & hs1<=H)); end; pure=all(hs1(1)==hs1);
  if(~discr), if(pure), distr(k,hs1(1))=1; hsn{k}=hs1(1); else
      distr(k,:)=histc(hs1,1:H)/n1; [~,hsn{k}]=max(distr(k,:)); end; end
  % if pure node or insufficient data don't train split
  if( pure || n1<=minCount || depth(k)>maxDepth ), k=k+1; continue; end
  % train split and continue
  fids1=wswor(fWts,F1,4); data1=data(dids1,fids1);
  [~,order1]=sort(data1); order1=uint32(order1-1);
  [fid,thr,gain]=forestFindThr(data1,hs1,dWts(dids1),order1,H,split);
  fid=fids1(fid); left=data(dids1,fid)<thr; count0=nnz(left);
  if( gain>1e-10 && count0>=minChild && (n1-count0)>=minChild )
    child(k)=K; fids(k)=fid-1; thrs(k)=thr;
    dids{K}=dids1(left); dids{K+1}=dids1(~left);
    depth(K:K+1)=depth(k)+1; K=K+2;
  end; k=k+1;
end
% create output model struct
K=1:K-1; if(discr), hsn={hsn(K)}; else hsn=[hsn{K}]'; end
tree=struct('fids',fids(K),'thrs',thrs(K),'child',child(K),...
  'distr',distr(K,:),'hs',hsn,'count',count(K),'depth',depth(K));
end

function ids = wswor( prob, N, trials )
% Fast weighted sample without replacement. Alternative to:
%  ids=datasample(1:length(prob),N,'weights',prob,'replace',false);
M=length(prob); assert(N<=M); if(N==M), ids=1:N; return; end
if(all(prob(1)==prob)), ids=randperm(M,N); return; end
cumprob=min([0 cumsum(prob)],1); assert(abs(cumprob(end)-1)<.01);
cumprob(end)=1; [~,ids]=histc(rand(N*trials,1),cumprob);
[s,ord]=sort(ids); K(ord)=[1; diff(s)]~=0; ids=ids(K);
if(length(ids)<N), ids=wswor(cumprob,N,trials*2); end
ids=ids(1:N)';
end


function boolean = ExitConditionEvaluation(T_new,T_reference,x_intervals,y_intervals)

boolean = 0;
Tempcount = 0;

for x_index = 2:1:x_intervals-1
    for y_index = 2:1:y_intervals-1
         Tempcount = Tempcount + T_new(x_index,y_index);
    end
end

if(round(Tempcount/((x_intervals-1)*(y_intervals-1))) <= T_reference)
boolean = 1;
end

end


function name = p24_name ( )

%*****************************************************************************80
%
%% P24_NAME returns the name of problem 24.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, string NAME, the name of the problem.
%
  name = '(|4X-2|+c)/(1+c)';

  return
end


% std_topo() - uses topoplot() to get the interpolated Cartesian grid of the
%               specified component topo maps. The topo map grids are saved
%               into a (.icatopo) file and a pointer to the file is stored
%               in the EEG structure. If such a file already exists,
%               loads the information from it.
%
%               Returns the topo map grids of all the requested components. Also
%               returns the EEG sub-structure etc (i.e EEG.etc), which is modified
%               with a pointer to the float file and some information about the file.
% Usage:
%               >> X = std_topo(EEG, components, option);
%
%                  % Returns the ICA topo map grid for a dataset.
%                  % Updates the EEG structure in the Matlab environment and re-saves
% Inputs:
%   EEG        - an EEG dataset structure.
%   components - [numeric vector] components in the EEG structure to compute topo maps
%                      {default|[] -> all}
%   option     - ['gradient'|'laplacian'|'none'] compute gradient or laplacian of
%                the scale topography. This does not acffect the saved file which is
%                always 'none' {default is 'none' = the interpolated topo map}
% Optional inputs
%   'recompute'  - ['on'|'off'] force recomputing topo file even if it is
%                  already on disk.
%   'fileout'    - [string] Path of the folder to save output. The default
%                  is EEG.filepath
% Outputs:
%   X          - the topo map grid of the requested ICA components, each grid is
%                     one ROW of X.
%
% File output: [dataset_name].icatopo
%
% Authors:  Hilit Serby, Arnaud Delorme, SCCN, INC, UCSD, January, 2005
%
% See also  topoplot(), std_erp(), std_ersp(), std_spec(), std_preclust()

% Copyright (C) Hilit Serby, SCCN, INC, UCSD, October 11, 2004, hilit@sccn.ucsd.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [X] = std_topo(EEG, comps, option, varargin)

if nargin < 1
    help std_topo;
    return;
end;
if isfield(EEG,'icaweights')
   numc = size(EEG.icaweights,1);
else
   error('EEG.icaweights not found');
end
if nargin < 2
   comps = 1:numc;
elseif isempty(comps)
   comps = 1:numc;
end

if nargin < 3
    option = 'none';
end;

g = finputcheck( varargin, { 'recompute'   'string'   { 'on','off' }   'off' ;...
                             'fileout'     'string'   []                EEG.filepath},...
                             'std_topo');
% if isstr(g), error(g); end;

% figure; toporeplot(grid,'style', 'both','plotrad', 0.5, 'intrad', 0.5, 'xsurface' ,Xi, 'ysurface',Yi );

% Topo information found in dataset
% ---------------------------------
if exist(fullfile(g.fileout, [ EEG.filename(1:end-3) 'icatopo' ])) && strcmpi(g.recompute, 'off')
    for k = 1:length(comps)
        tmp = std_readtopo( EEG, 1, comps(k));
        if strcmpi(option, 'gradient')
            [tmpx, tmpy]  = gradient(tmp); %Gradient
            tmp = [tmpx(:); tmpy(:)]';
        elseif strcmpi(option, 'laplacian')
            tmp = del2(tmp); %Laplacian
            tmp = tmp(:)';
        else
            tmp = tmp(:)';
        end;

        tmp = tmp(find(~isnan(tmp)));
        if k == 1
            X = zeros(length(comps),length(tmp)) ;
        end
        X(k,:) =  tmp;
    end
    return
end

all_topos = [];
for k = 1:numc

    % compute topo map grid (topoimage)
    % ---------------------------------
    chanlocs = EEG.chanlocs(EEG.icachansind);
    if isempty( [ chanlocs.theta ] )
        error('Channel locations are required for computing scalp topographies');
    end;
    [hfig grid plotrad Xi Yi] = topoplot( EEG.icawinv(:,k), chanlocs, ...
                                          'verbose', 'off',...
                                           'electrodes', 'on' ,'style','both',...
                                           'plotrad',0.55,'intrad',0.55,...
                                           'noplot', 'on', 'chaninfo', EEG.chaninfo);

    all_topos = setfield(all_topos, [ 'comp' int2str(k) '_grid' ], grid);
    all_topos = setfield(all_topos, [ 'comp' int2str(k) '_x' ]   , Xi(:,1));
    all_topos = setfield(all_topos, [ 'comp' int2str(k) '_y' ]   , Yi(:,1));

end

% Save topos in file
% ------------------
all_topos.datatype = 'TOPO';
tmpfile = fullfile( g.fileout, [ EEG.filename(1:end-3) 'icatopo' ]);
std_savedat(tmpfile, all_topos);

for k = 1:length(comps)
    tmp =  getfield(all_topos, [ 'comp' int2str(comps(k)) '_grid' ]);

    if strcmpi(option, 'gradient')
        [tmpx, tmpy]  = gradient(tmp); % Gradient
        tmp = [tmpx(:); tmpy(:)]';
    elseif strcmpi(option, 'laplacian')
        tmp = del2(tmp); % Laplacian
        tmp = tmp(:)';
    else
        tmp = tmp(:)';
    end;

    tmp = tmp(find(~isnan(tmp)));
    if k == 1
        X = zeros(length(comps),length(tmp)) ;
    end
    X(k,:) =  tmp;
end


function imageCha = recon_main(obj, iRep, iAvg)
% main reconstruction function
% prepare constraints and select algorithm depending on image dimensionality
%
% input:
% obj       CS reconstruction object (holding all parameters)
% iRep      current repetition
% iAvg      current average
%
% output:
% imageCha  individual reconstructed channel images
%
% (c) Thomas Kuestner
% -------------------------------------------------------------------------

nCha = obj.measPara.dim(5);
nSlices = size(obj.kSpace,1);
nTime = obj.measPara.dim(4);
imageCha = cell(nSlices,nCha);

if(strcmp(obj.measPara.dimension,'2D'))
% lMask = cellfun(@(x) abs(x) > 0, all_KSpaces, 'UniformOutput', false);

    dispProgress('Slices', 0, nSlices);
    for iSli = 1:nSlices
        % prepare k-space
        % => k_y - k_x - t - cha
        kSpaceL = cell2mat(shiftdim(obj.kSpace(iSli,:,iRep,iAvg),-2));
        if(strcmp(obj.measPara.precision,'double'))
            kSpaceL = double(kSpaceL);
        end
        % same undersampling for all coils and fully sampled in k_x direction
        % => reduce 4D problem (k_y - k_x - t - cha) to 2D problem (k_y - t)
        obj.fullMask = abs(kSpaceL(:,:,:,1)) > 0; % k_y - k_x - t

        if(obj.measPara.oversampling{2,1})
            kSpaceL = fftshift(ifft(ifftshift(kSpaceL, 2),[],2),2); % -> k_y - x - t - cha
            kSpaceL = kSpaceL(:, obj.measPara.oversampling{1,1}, :, :); % anti-aliasing
            obj.fullMask = obj.fullMask(:, obj.measPara.oversampling{1,1}, :, :);
            kSpaceL = fftshift(fft(ifftshift(kSpaceL,2),[],2),2); % -> k_y - k_x - t -cha
            kSpaceL = kSpaceL .* repmat(obj.fullMask,[1 1 1 size(kSpaceL,4)]);
            obj.measPara.dim(2) = size(kSpaceL,2);
        end

        [nPha, nFreq, nTime, nCha] = size(kSpaceL);

        %     obj.fullMask = lMask{1}(:, obj.obj.measPara.oversampling{1}, :); % k_y - k_x - t
        %     mask = abs(sum(permute(fullMask,[3 1 2]),3)) > 0;

        if(iSli == 1)
            if(isempty(obj.calibSize))
                obj.calibSize = cell(1,nTime);
                reempty = true;
            else
                % adjust calibSize due to anti-aliasing
                obj.calibSize(2) = nFreq;
                reempty = false;
            end
        else
            if(reempty)
                obj.calibSize = cell(1,nTime);
            end
        end
        obj.kCalib = cell(1,nTime);
        obj.kernel = obj.kCalib;
        obj.kernelImg = obj.kCalib;
        ESP = obj.kCalib;
%         obj.A = obj.kCalib;

        % convolution with GRAPPA kernel in k-space -> multiplication in image
        % domain
        obj.method = 'fft';

        % prepare k-space => k_y - k_x - cha - t
        kSpaceL = permute(kSpaceL, [1 2 4 3]);

        fprintf('Performing calibration\n');
        for i=1:nTime

            % perform calibration
            % get calibration data from k-space
            if(iscell(obj.calibSize))
                % automatically determine calibration region
                obj.calibSize{i} = obj.calibrationSize(obj.fullMask(:,:,i)); % k_y - k_x (cell: t)
                if(~obj.flagToolbox)
                    obj.kCalib{i} = crop(kSpaceL(:,:,:,i), [obj.calibSize{i}, nCha]); % k_y - k_x - cha (cell: t)
                end
            else
                if(~obj.flagToolbox)
                    % fixed calibration region
                    plonk = [nPha, nFreq];
                    idx = cell(1,length(plonk));
                    for iInner=1:length(plonk)
                        idx{iInner} = floor(plonk(iInner)/2)+1+ceil(-obj.calibSize(iInner)/2):floor(plonk(iInner)/2)+ceil(obj.calibSize(iInner)/2);
                    end
                    obj.kCalib{i} = kSpaceL(idx{1},idx{2},:,i);
                end
            end

            if(~obj.flagToolbox)
    %             % deprecated
    %             obj.kernel{i} = zeros([obj.kernelSize, nCha, nCha]); % k_y - k_x - cha - cha (cell: t)
    %
    %             [AtA,obj.A{i}] = obj.corrMatrix(i);
    %             for n=1:nCha
    %                 obj.kernel{i}(:,:,:,n) = obj.calibrate(AtA,nCha,n);
    %             end

                % compute eigen-value maps
                [obj.kernel{i},S] = dat2Kernel(obj.kCalib{i},obj.kernelSize);
                idx = max(find(S >= S(1)*obj.eigThresh_k));

                % crop kernels and compute eigen-value decomposition in image space to get maps
                [M,W] = kernelEig(obj.kernel{i}(:,:,:,1:idx),[nPha,nFreq]);

                % Compute Soft-SENSE ESPIRiT Maps
                % crop sensitivity maps according to eigenvalues == 1
                % weigth with n maps of eigen-values

                maps = M(:,:,:,end-(obj.n_maps-1):end);

                % Weight the eigenvectors with soft-senses eigen-values
                weights = W(:,:,end-(obj.n_maps-1):end) ;
                weights = (weights - obj.eigThresh_im)./(1-obj.eigThresh_im).* (W(:,:,end-(obj.n_maps-1):end) > obj.eigThresh_im);
                weights = -cos(pi*weights)/2 + 1/2;

                % create and ESPIRiT operator
                ESP{i} = ESPIRiT(maps,weights);
            else
                if(ispc)
                    eval(sprintf('ESP{i} = bart(''ecalib -r %d:%d -k %d:%d -m %d -1'',kSpaceL(:,:,:,i));', obj.calibSize{i}(1), obj.calibSize{i}(2), obj.kernelSize(1), obj.kernelSize(2), obj.n_maps ));

                else
                    % create temporary dir if not yet existing
                    if(~exist([obj.path,filesep,'tmpESPIRiT'],'dir'))
                        mkdir([obj.path,filesep,'tmpESPIRiT']);
                    end
                    writecfl([obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], kSpaceL(:,:,:,i)); % k_y - k_x - cha
                    eval(sprintf('! ecalib -r %d:%d -k %d:%d -m %d %s %s', obj.calibSize{i}(1), obj.calibSize{i}(2), obj.kernelSize(1), obj.kernelSize(2), obj.n_maps, [obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], [obj.path,filesep,'tmpESPIRiT',filesep,'espiritmaps_',num2str(i)]));
                    ESP{i} = [obj.path,filesep,'tmpESPIRiT',filesep,'espiritmaps_',num2str(i)];
                end
            end
        end

        % ensure that empty entries are zero
%         mask = false(nPha,nFreq,nCha,nTime);
        mask = obj.fullMask;
%         for i=1:nTime
%             mask(:,:,:,i) = repmat(obj.fullMask(:,:,i),[1,1,nCha]);
%         end
        kSpaceL(~mask) = 0;

        if(strcmp(obj.solver,'L1') && ~obj.flagToolbox)
            if(strcmp(obj.trafo.trafoType,'wavelet_lab'))
                ssx = 2^ceil(log2(nPha));
                ssy = 2^ceil(log2(nFreq));
                ss = max(ssx, ssy);
                XOP = Wavelet(obj.trafo.waveletFilter,obj.trafo.waveletFilterSize,log2(ss/2^(obj.trafo.waveletStages)));
            end
        end

        image = zeros(nPha,nFreq,obj.n_maps,nTime);
        dispProgress('Time', 0, nTime);
        fprintf('ESPIRiT reconstruction\n');
        for i=1:nTime
            if(~obj.flagToolbox)
                if(strcmp(obj.solver,'L1'))
                    FT = p2DFT(mask(:,:,:,i),[nPha,nFreq,nCha]);
                    if(strcmp(obj.trafo.trafoType,'fft'))
                        XOP = FT;
                    end
                    image(:,:,:,i) = obj.cgL1ESPIRiT(kSpaceL(:,:,:,i), zeros(size(kSpaceL(:,:,:,i))), FT, ESP{i}, obj.iNINNER, XOP, obj.trafo.wavWeight, obj.splitWeight, obj.iNIterSplit); % => y-x-cha-t
                else
                    [~,image(:,:,:,i)] = obj.cgESPIRiT(kSpaceL(:,:,:,i),ESP{i}, obj.iNINNER, obj.calibTyk, zeros(size(kSpaceL(:,:,:,i))));
                end
            else
                if(ispc)
                    if(strcmp(obj.solver,'L1'))
                        eval(sprintf('image(:,:,:,i) = squeeze(bart(''sense -l1 -r %f'', kSpaceL(:,:,:,i), ESP{i}));', obj.trafo.wavWeight));
                    else
                        image(:,:,:,i) = squeeze(bart('sense -l2', kSpaceL(:,:,:,i), ESP{i}));
                    end
                else
                    fftdims = size(kSpaceL);
                    fftdims = fftdims(1:2);
                    if(any(mod(fftdims,2) ~= 0))
                        writecfl([obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], zpad(kSpaceL(:,:,:,i),size(kSpaceL(:,:,:,i)) + [mod(fftdims,2),0])); % k_y - k_z - k_x - cha
                    else
                        writecfl([obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], kSpaceL(:,:,:,i)); % k_y - k_x - cha
                    end
                    if(strcmp(obj.solver,'L1'))
                        eval(sprintf('! sense -l1 -r %f %s %s %s', obj.trafo.wavWeight, [obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], ESP{i}, [obj.path,filesep,'tmpESPIRiT',filesep,'imgOut']));
                    else
                        eval(sprintf('! sense -l2 %s %s %s', [obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], ESP{i}, [obj.path,filesep,'tmpESPIRiT',filesep,'imgOut']));
                    end
    %                 if(obj.n_maps > 1)
    %                     % combine ESPIRiT images into one
    %                    eval(sprintf('! rss 3 %s %s', [obj.path,filesep,'tmpESPIRiT',filesep,'imgOut'], [obj.path,filesep,'tmpESPIRiT',filesep,'imgOutRSS']));
    %                    delete([obj.path,filesep,'tmpESPIRiT',filesep,'imgOut']);
    %                    movefile([obj.path,filesep,'tmpESPIRiT',filesep,'imgOutRSS'], [obj.path,filesep,'tmpESPIRiT',filesep,'imgOut']);
    %                 end
                    image(:,:,:,i) = squeeze(readcfl([obj.path,filesep,'tmpESPIRiT',filesep,'imgOut']));
                end
            end
            dispProgress('Time',i/nTime);
        end
        dispProgress('Time','Close');

        imageCha(iSli,1:obj.n_maps) = shiftdim(mat2cell(permute(image,[1 2 4 3]),nPha,nFreq,nTime,ones(1,obj.n_maps)),2);
        [imageCha{iSli,obj.n_maps+1:end}] = deal(zeros(size(imageCha{iSli,1})));

        dispProgress('Slices', iSli/nSlices);
    end
    dispProgress('Slices', 'Close');

elseif(strcmp(obj.measPara.dimension,'3D'))

    % prepare k-space
    % => k_y - k_z - k_x - cha
    kSpaceL = cell2mat(shiftdim(obj.kSpace(:,:,iRep,iAvg),-2));
    kSpaceL = permute(kSpaceL,[1 3 2 4]);
    if(strcmp(obj.measPara.precision,'double'))
        kSpaceL = double(kSpaceL);
    end
    % same undersampling for all coils and fully sampled in k_x direction
    obj.fullMask = abs(kSpaceL) > 0; % k_y - k_z - k_x
    if(obj.measPara.oversampling{2,1})
        kSpaceL = ifftnshift(kSpaceL, 3); % -> k_y - k_z - x - cha
        kSpaceL = kSpaceL(:, :, obj.measPara.oversampling{1,1}, :); % anti-aliasing
        obj.fullMask = obj.fullMask(:,:, obj.measPara.oversampling{1,1}, :);
        kSpaceL = fftnshift(kSpaceL,3); % -> k_y - k_z - k_x - cha
        kSpaceL = kSpaceL .* obj.fullMask;
        obj.measPara.dim(2) = size(kSpaceL,3);
    end
    [nPha, nZ, nFreq, nCha] = size(kSpaceL);

    if(~isempty(obj.calibSize))
        % adjust calibSize due to anti-aliasing
        obj.calibSize(3) = nFreq;
    end

    % create a 3D kernel fomr the calibration region
    fprintf('Performing calibration\n');
    if(isempty(obj.calibSize))
        % automatically determine calibration region
        obj.calibSize = obj.calibrationSize(obj.fullMask(:,:,:,1)); % k_y - k_z - k_x
        if(size(kSpaceL,3) == 1), obj.calibSize = [obj.calibSize, 1]; obj.kernelSize = obj.kernelSize([1 3 2]); end; % k_y - k_x sparse!
        if(~obj.flagToolbox)
            obj.kCalib = crop(kSpaceL, [obj.calibSize, nCha]); % k_y - k_z - k_x - cha
        end
    else
        if(~obj.flagToolbox)
            % fixed calibration region
            plonk = [nPha, nZ, nFreq];
            idx = cell(1,length(plonk));
            for iInner=1:length(plonk)
                if(mod(obj.calibSize(iInner),2) == 0)
                    idx{iInner} = floor(plonk(iInner)/2)+1+ceil(-obj.calibSize(iInner)/2):floor(plonk(iInner)/2)+ceil(obj.calibSize(iInner)/2);
                else
                    idx{iInner} = floor(plonk(iInner)/2)+ceil(-obj.calibSize(iInner)/2):floor(plonk(iInner)/2)+ceil(obj.calibSize(iInner)/2)-1;
                end

                tmp = [idx{iInner}(1) <= 0, idx{iInner}(end) > plonk(iInner)];
                if(any(tmp))
                    if(all(tmp)), error('crop(): Both index out of bounds'); end;
                    hShift = [abs(idx{iInner}(1)) + 1, idx{iInner}(end) - plonk(iInner)];
                    op = {idx{iInner}(1), idx{iInner}(end); '+', '-'; '<', '>'; plonk(iInner) + 1, 0};
                    eval(sprintf('if(op{1,~helper} %s hShift(helper) %s %d), idx{iInner} = idx{iInner} %s hShift(helper); else idx{iInner} = idx{iInner}(idx{iInner} %s %d);end;',op{2,helper}, op{3,helper}, op{4,helper}, op{2,helper}, op{3,~helper}, op{4,~helper}));
                end
            end
            obj.kCalib = kSpaceL(idx{1},idx{2},idx{3},:); % k_y - k_z - k_x - cha
        end
    end

    if(~obj.flagToolbox)
        % windowing for cut-out region
    %     [~,w1] = window2D(prop.window.type,size(obj.kCalib,1),prop.window.windowOpt{1},prop.window.windowOpt{2});
    %     [~,w2] = window2D(prop.window.type,size(obj.kCalib,2),prop.window.windowOpt{1},prop.window.windowOpt{2});
    %     window =  w1(:) * w2(:).';
        if(obj.window.windowingOn)
            window = windowND(obj.window.type,[size(obj.kCalib,1), size(obj.kCalib,2)],obj.window.windowOpt{1},obj.window.windowOpt{2});
            obj.kCalib = obj.kCalib .* repmat(window,[1 1 size(obj.kCalib,3) nCha]);
        end

        % compute eigen-value maps
        [hybridKernel,S] = dat3Kernel(obj.kCalib,obj.kernelSize);
        hybridKernel = ifftnshift(zpad(hybridKernel(:,:,end:-1:1,:,:),[obj.kernelSize(1),obj.kernelSize(2),nFreq + obj.kernelSize(3)-1,nCha,size(hybridKernel,5)]),3);
        idx = max(find(S >= S(1)*obj.eigThresh_k));

        % compute inverse fourier transform along readout direction
        kSpaceL = ifftnshift(kSpaceL,3); % k_y - k_z - x - cha

        % convolution with GRAPPA kernel in k-space -> multiplication in image
        % domain
        obj.method = 'fft';

        % prepare wavelet transformation
        if(strcmp(obj.solver,'L1'))
            if(strcmp(obj.trafo.trafoType,'wavelet_lab'))
                ssx = 2^ceil(log2(nPha));
                ssy = 2^ceil(log2(nZ));
                ss = max(ssx, ssy);
                XOP = Wavelet(obj.trafo.waveletFilter,obj.trafo.waveletFilterSize,log2(ss/2^(obj.trafo.waveletStages)));
            end
        end

        image = zeros(nPha,nFreq,nZ,obj.n_maps);
    %     AtA_sub = zeros(prod(obj.kernelSize)*nCha); % subtract this matrix from AtA_mask, due to accumulation of previous samples (A'*A multiplication)
    %     A_sub = zeros((size(obj.kCalib,1)-obj.kernelSize(1)+1) * (size(obj.kCalib,2)-obj.kernelSize(2)+1) * (size(obj.kCalib,3)-obj.kernelSize(3)+1),prod(obj.kernelSize)*nCha);

        fprintf('ESPIRiT reconstruction\n');
        dispProgress('Frequency',0,nFreq);
        for iFreq=1:nFreq

            % extract 2D kernel from complete 3D kernel
            obj.kernel = squeeze(hybridKernel(:,:,iFreq,:,:));

            % crop kernels and compute eigen-value decomposition in image space to get maps
            [M,W] = kernelEig(obj.kernel(:,:,:,1:idx),[nPha,nZ]);

            % Compute Soft-SENSE ESPIRiT Maps
            % crop sensitivity maps according to eigenvalues == 1
            % weigth with n maps of eigen-values

            maps = M(:,:,:,end-(obj.n_maps-1):end);

            % Weight the eigenvectors with soft-senses eigen-values
            weights = W(:,:,end-(obj.n_maps-1):end);
            weights = (weights - obj.eigThresh_im)./(1-obj.eigThresh_im).* (W(:,:,end-(obj.n_maps-1):end) > obj.eigThresh_im);
            weights = -cos(pi*weights)/2 + 1/2;

            % create an ESPIRiT operator
            ESP = ESPIRiT(maps,weights);

            if(strcmp(obj.solver,'L1'))
                FT = p2DFT(squeeze(obj.fullMask(:,:,iFreq,:)),[nPha,nZ,nCha]);
                if(strcmp(obj.trafo.trafoType,'fft'))
                    XOP = FT;
                end
                image(:,iFreq,:,:) = permute(obj.cgL1ESPIRiT(squeeze(kSpaceL(:,:,iFreq,:)), zeros([size(squeeze(kSpaceL(:,:,iFreq,1))),obj.n_maps]), FT, ESP, obj.iNINNER, XOP, obj.trafo.wavWeight, obj.splitWeight, obj.iNIterSplit),[1 4 2 3]); % => y-x-z-cha
            else
                [tmpK,tmp] = obj.cgESPIRiT(squeeze(kSpaceL(:,:,iFreq,:)), ESP, obj.iNINNER, obj.calibTyk, zeros(size(squeeze(kSpaceL(:,:,iFreq,:)))));
                image(:,iFreq,:,:) = permute(tmp,[1 4 2 3]);
            end
            dispProgress('Frequency', iFreq/nFreq);
        end
        dispProgress('Frequency','Close');
    %     image = ifftnshift(image,[1,3]);
        imageCha(:,1:obj.n_maps) = shiftdim(mat2cell(image,nPha,nFreq,nZ,ones(1,obj.n_maps)),2);
        [imageCha{:,obj.n_maps+1:end}] = deal(zeros(size(imageCha{1,1})));

    else
        fprintf('ESPIRiT reconstruction\n');
        if(ispc)
            eval(sprintf('ESP = bart(''ecalib -r %d:%d -k %d:%d -m %d'',kSpaceL);', obj.calibSize(1), obj.calibSize(2), obj.kernelSize(1), obj.kernelSize(2), obj.n_maps ));
            if(strcmp(obj.solver,'L1'))
                eval(sprintf('image = permute(squeeze(bart(''pics -l1 -r %f'', kSpaceL, ESP)),[1 3 2 4]);', obj.trafo.wavWeight));
            else
                image = permute(squeeze(bart('pics -l2', kSpaceL, ESP)),[1 3 2 4]);
            end
        else
            % create temporary dir if not yet existing
            if(~exist([obj.path,filesep,'tmpESPIRiT'],'dir'))
                mkdir([obj.path,filesep,'tmpESPIRiT']);
            end
            fftdims = size(kSpaceL);
            fftdims = fftdims(1:3);
            if(any(mod(fftdims,2) ~= 0))
                writecfl([obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], zpad(kSpaceL,size(kSpaceL) + [mod(fftdims,2),0])); % k_y - k_z - k_x - cha
            else
                writecfl([obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], kSpaceL); % k_y - k_z - k_x - cha
            end
            obj.calibSize = obj.calibSize - mod(obj.calibSize,2);
            eval(sprintf('! ecalib -r %d:%d:%d -k %d:%d:%d -m %d %s %s', obj.calibSize(1), obj.calibSize(2), obj.calibSize(3), obj.kernelSize(1), obj.kernelSize(2), obj.kernelSize(3), obj.n_maps, [obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], [obj.path,filesep,'tmpESPIRiT',filesep,'espiritmaps']));
            ESP = [obj.path,filesep,'tmpESPIRiT',filesep,'espiritmaps'];

            if(strcmp(obj.solver,'L1'))
                eval(sprintf('! sense -l1 -r %f %s %s %s', obj.trafo.wavWeight, [obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], ESP, [obj.path,filesep,'tmpESPIRiT',filesep,'imgOut']));
            else
                eval(sprintf('! sense -l2 %s %s %s', [obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], ESP, [obj.path,filesep,'tmpESPIRiT',filesep,'imgOut']));
            end
            image = permute(squeeze(readcfl([obj.path,filesep,'tmpESPIRiT',filesep,'imgOut'])),[1 3 2 4]);
        end
%         % OR:
%         kSpaceL = permute(ifftnshift(kSpaceL,3), [3 1 2 4]); % x - k_y - k_z - cha
%         writecfl([obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], kSpaceL); % x - k_y - k_z - cha
%         eval(sprintf('! ecalib -r %d:%d:%d -k %d:%d:%d -m %d %s %s', obj.calibSize(3), obj.calibSize(1), obj.calibSize(2), obj.kernelSize(3), obj.kernelSize(1), obj.kernelSize(2), obj.n_maps, [obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], [obj.path,filesep,'tmpESPIRiT',filesep,'espiritmaps']));
%         ESP = [obj.path,filesep,'tmpESPIRiT',filesep,'espiritmaps'];
%          if(strcmp(obj.solver,'L1'))
%             eval(sprintf('! rsense -l1 -r %f %s %s %s', obj.trafo.wavWeight, [obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], ESP, [obj.path,filesep,'tmpESPIRiT',filesep,'imgOut']));
%         else
%             eval(sprintf('! rsense -l2 %s %s %s', [obj.path,filesep,'tmpESPIRiT',filesep,'kspace'], ESP, [obj.path,filesep,'tmpESPIRiT',filesep,'imgOut']));
%         end
%         image = permute(squeeze(readcfl([obj.path,filesep,'tmpESPIRiT',filesep,'imgOut'])), [2 1 3 4]);

        image = crop(image,[size(kSpaceL,1),size(kSpaceL,3),size(kSpaceL,2),obj.n_maps]);
        imageCha(:,1:obj.n_maps) = shiftdim(mat2cell(image,nPha,nFreq,nZ,ones(1,obj.n_maps)),2);
        [imageCha{:,obj.n_maps+1:end}] = deal(zeros(size(imageCha{1,1})));
    end

elseif(strcmp(obj.measPara.dimension,'4D'))
    % TODO

end

if(obj.flagToolbox)
    rmdir([obj.path,filesep,'tmpESPIRiT'], 's');
end

end