function [interp] = megrealign(cfg, data);

% MEGREALIGN interpolates MEG data towards standard gradiometer locations 
% by projecting the individual timelocked data towards a coarse source
% reconstructed representation and computing the magnetic field on
% the standard gradiometer locations.
%
% Use as
%   [interp] = megrealign(cfg, data)
%
% The new gradiometer definition is obtained from a template dataset,
% or can be constructed by averaging the gradiometer positions over
% multiple datasets.
%   cfg.template       = single dataset that serves as template
%   cfg.template(1..N) = datasets that are averaged into the standard
%
% The realignment is done by computing a minumum current estimate on
% the surface of the brain, followed by a forward computation towards
% a planar gradiometer array. This requires the specification of a
% headmodel and of a sourcemodel.
%
% A headmodel must be specified with
%   cfg.hdmfile        = filename for headmodel
% or alternatively manually using 
%   cfg.vol.r          = radius of sphere
%   cfg.vol.o          = [x, y, z] position of origin
%
% A sourcemodel must be specified with
%   cfg.headshape      = filename for headshape or can be 'headmodel'
%   cfg.spheremesh     = number of dipoles on sphere, either 42, 162 or 642
%   cfg.inwardshift    = default is 2.5 cm
%   cfg.pruneratio     = for singular values, default is 1e-3
% 
% This implements the method described by T.R. Knosche, Transformation
% of whole-head MEG recordings between different sensor positions.
% Biomed Tech (Berl). 2002 Mar;47(3):59-62.
% 
% See also MEGINTERPOLATE

% Copyright (C) 2004, Robert Oostenveld
%
% $Log: megrealign.m,v $
% Revision 1.23  2006/02/24 16:37:56  roboos
% only change in whitespace
%
% Revision 1.22  2006/02/23 10:28:16  roboos
% changed dimord strings for consistency, changed toi and foi into time and freq, added fixdimord where neccessary
%
% Revision 1.21  2006/01/30 14:34:59  roboos
% added two comments as a reminder that prepare_dipole_grid should be used in the future
%
% Revision 1.20  2006/01/30 14:09:04  roboos
% replaced ctf specific code by call to read_fcdc_header
%
% Revision 1.19  2005/12/14 14:16:38  roboos
% switched from prepare_brain_surface to headsurface subfunction
%
% Revision 1.18  2005/06/29 12:46:29  roboos
% the following changes were done on a large number of files at the same time, not all of them may apply to this specific file
% - added try-catch around the inclusion of input data configuration
% - moved cfg.version, cfg.previous and the assignment of the output cfg to the end
% - changed help comments around the configuration handling
% - some changes in whitespace
%
% Revision 1.17  2005/06/08 13:40:33  roboos
% replaced the specific call to either meg_leadfield or eeg_leadfield to the generic compute_leadfield
%
% Revision 1.16  2005/06/07 13:03:55  roboos
% fixed bug due to change in read_ctf_res4 and ctf2grad
%
% Revision 1.15  2005/06/02 12:18:41  roboos
% changed handling of input data: All input data that contains averages is converted to raw trials (like the output from preprocessing) prior to further processing. The output data is converted back into a format similar to the original input data using RAW2MEG.
%
% Revision 1.14  2005/05/23 09:31:49  roboos
% now gives error if no template is specified
%
% Revision 1.13  2005/05/17 17:50:37  roboos
% changed all "if" occurences of & and | into && and ||
% this makes the code more compatible with Octave and also seems to be in closer correspondence with Matlab documentation on shortcircuited evaluation of sequential boolean constructs
%
% Revision 1.12  2005/01/17 14:40:38  roboos
% small change in documentation
%
% Revision 1.11  2004/08/20 14:30:27  roboos
% added literatire reference to knosche2002
%
% Revision 1.10  2004/05/05 13:55:20  roberto
% fixed bug that occured when average data was made with keeptrials=yes
%
% Revision 1.9  2004/04/27 13:49:31  roberto
% fixed bug that occurred when template set contained mixture of 150 and 151 channel helmets
%
% Revision 1.8  2004/04/26 09:36:26  roberto
% interp.time was not returned correctly
%
% Revision 1.7  2004/04/22 12:51:19  roberto
% fixed bug that occurred if no rest-channels are present (variable remained undefined)
%
% Revision 1.6  2004/04/13 16:31:09  roberto
% fixed bug in dbstack selection of function filename for Matlab 6.1
%
% Revision 1.5  2004/04/13 14:25:24  roberto
% wrapped code-snippet around mfilename to make it compatible with Matlab 6.1
%
% Revision 1.4  2004/04/08 16:05:19  roberto
% fixed bug in plot3d section where 151 channels were hardcoded
%
% Revision 1.3  2004/04/08 15:45:53  roberto
% added support for datasets with less than 151 MEG channels,
% removed most of the channel selection and now using prepare_vol_sens,
% moved the construction of the brain dipole surface to new function prepare_brain_surface
%

% set the default configuration 
if ~isfield(cfg, 'headshape'),     cfg.headshape = 'headmodel';   end
if ~isfield(cfg, 'inwardshift'),   cfg.inwardshift = 2.5;         end
if ~isfield(cfg, 'pruneratio'),    cfg.pruneratio = 1e-3;         end
if ~isfield(cfg, 'spheremesh'),    cfg.spheremesh = 642;          end
if ~isfield(cfg, 'verify'),        cfg.verify = 'yes';            end
if ~isfield(cfg, 'plot3d'),        cfg.plot3d = 'no';             end

if ~isfield(cfg, 'template'), 
  error('you must specify one or more template CTF datasets in cfg.template');
end

% for backward compatibility with old data structures
data = fixdimord(data);

% convert average to raw data using simple helper function
% this allows the same function to work on multiple data types
[data, inputdimord] = data2raw(data);

Ntrials = length(data.trial);

% Retain only the MEG channels in the data and temporarily store
% the rest, these will be added back to the transformed data later.
dataindx = match_str(data.label, data.grad.label);
restindx = setdiff(1:length(data.label),dataindx);
if ~isempty(restindx)
  fprintf('removing %d non-MEG channels from the data\n', length(restindx));
  rest.label = data.label(restindx);    % first remember the rest
  data.label = data.label(dataindx);    % then reduce the data
  for i=1:Ntrials
    rest.trial{i} = data.trial{i}(restindx,:);	% first remember the rest
    data.trial{i} = data.trial{i}(dataindx,:);	% then reduce the data
  end
else
  rest.label = {};
  rest.trial = {};
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% construct the average template gradiometer array
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ntemplate = length(cfg.template);
for i=1:Ntemplate
  hdr = read_fcdc_header(cfg.template{i});
  template(i) = hdr.grad;
end

templ.meanC = [0 0 0];
templ.meanF = [0 0 0];
templ.meanL = [0 0 0];
templ.meanR = [0 0 0];
for i=1:Ntemplate
  % determine the 4 ref sensors for this individual template helmet 
  indxC = strmatch('MZC01', template(i).label); 
  indxF = strmatch('MZF03', template(i).label); 
  indxL = strmatch('MLC21', template(i).label); 
  indxR = strmatch('MRC21', template(i).label); 
  if isempty(indxC) || isempty(indxF) || isempty(indxL) || isempty(indxR)
    error('not all 4 sensors were found that are needed to rotate/translate');
  end
  % add them to the sum, to compute mean location of each ref sensor
  templ.meanC = templ.meanC + template(i).pnt(indxC,:);
  templ.meanF = templ.meanF + template(i).pnt(indxF,:);
  templ.meanL = templ.meanL + template(i).pnt(indxL,:);
  templ.meanR = templ.meanR + template(i).pnt(indxR,:);
end
templ.meanC = templ.meanC / Ntemplate;
templ.meanF = templ.meanF / Ntemplate;
templ.meanL = templ.meanL / Ntemplate;
templ.meanR = templ.meanR / Ntemplate;

% FIXME, there is much more neccessary than this to make it symmetric
% but the helmet does not seem to be symmetric
% if strcmp(cfg.symmetric, 'yes')
%   templ.meanF(2) = templ.meanF(2) - templ.meanC(2);
%   templ.meanL(2) = templ.meanL(2) - templ.meanC(2);
%   templ.meanR(2) = templ.meanR(2) - templ.meanC(2);
%   templ.meanC(2) = 0;
% end

% construct two direction vectors that define the helmet orientation
templ.dirCF = (templ.meanF - templ.meanC);
templ.dirRL = (templ.meanL - templ.meanR);
% construct three orthonormal direction vectors
templ.dirX = normalize(templ.dirCF);
templ.dirY = normalize(templ.dirRL - dot(templ.dirRL, templ.dirX) * templ.dirX);
templ.dirZ = cross(templ.dirX, templ.dirY);
templ.tra = fixedbody(templ.meanC, templ.dirX, templ.dirY, templ.dirZ);

% determine the 4 ref sensors for the helmet that belongs to this dataset
indxC = strmatch('MZC01', data.grad.label); 
indxF = strmatch('MZF03', data.grad.label); 
indxL = strmatch('MLC21', data.grad.label); 
indxR = strmatch('MRC21', data.grad.label); 
if isempty(indxC) | isempty(indxF) | isempty(indxL) | isempty(indxR)
  error('not all 4 sensors were found that are needed to rotate/translate');
end

% construct two direction vectors that define the helmet orientation
orig.dirCF = data.grad.pnt(indxF,:) - data.grad.pnt(indxC,:);
orig.dirRL = data.grad.pnt(indxL,:) - data.grad.pnt(indxR,:);
% construct three orthonormal direction vectors
orig.dirX = normalize(orig.dirCF);
orig.dirY = normalize(orig.dirRL - dot(orig.dirRL, orig.dirX) * orig.dirX);
orig.dirZ = cross(orig.dirX, orig.dirY);
orig.tra = fixedbody(data.grad.pnt(indxC,:), orig.dirX, orig.dirY, orig.dirZ);

% compute the homogenous transformation matrix and transform the positions
tra = inv(templ.tra) * orig.tra;
pnt = data.grad.pnt;
pnt(:,4) = 1;
pnt = (tra * pnt')';
pnt = pnt(:,1:3);

% remove the translation from the transformation matrix and rotate the orientations
tra(:,4) = [0 0 0 1]';
ori = data.grad.ori;
ori(:,4) = 1;
ori = (tra * ori')';
ori = ori(:,1:3);

% construct the final template gradiometer definition
template = [];
template.grad.pnt = pnt;
template.grad.ori = ori;
% keep the same labels and the linear weights of the coils
template.grad.label = data.grad.label;
template.grad.tra   = data.grad.tra;
template.grad.unit  = data.grad.unit;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PREPARE_VOL_SENS will match the data labels, the gradiometer labels and the 
% volume model labels (in case of a multisphere model) and result in a gradiometer 
% definition that only contains the gradiometers that are present in the data.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tmpdat = [];
tmpdat.label = data.label;
tmpcfg = [];
try
  % the headmodel could be specified as a filename
  tmpcfg.hdmfile = cfg.hdmfile;
end
try
  % or the headmodel itself could be specified in the configuration
  tmpcfg.vol = cfg.vol;
end
tmpdat.grad = template.grad;
[vol, template.grad] = prepare_vol_sens(tmpcfg, tmpdat);
tmpdat.grad  = data.grad;
[vol, data.grad] = prepare_vol_sens(tmpcfg, tmpdat);

% Continue to work with the volume conduction model that matches the
% original data and gradiometer definition. For the template gradiometer
% definition it would not be possible to construct a multi-sphere model.
% For a single sphere model it does not make a difference.

fprintf('mean distance towards template gradiometers is %.2f %s\n', mean(sum((data.grad.pnt-template.grad.pnt).^2, 2).^0.5), template.grad.unit);

% determine the dipole positions on the surface of the cortex
if strcmp(cfg.headshape, 'headmodel')
  shapefile = [];
else
  shapefile = cfg.headshape;
end
% FIXME: in the future this should be moved to the prepare_dipole_grid function
pos = headsurface(vol, data.grad, 'surface', 'cortex', 'headshape', shapefile, 'inwardshift', cfg.inwardshift, 'npnt', cfg.spheremesh);

% compute the forward model for the old and new gradiometer positions
fprintf('computing forward model for %d dipoles\n', size(pos,1));
lfold = compute_leadfield(pos, data.grad, vol);
lfnew = compute_leadfield(pos, template.grad, vol);

% compute this inverse only once, although it is used twice
tmp = prunedinv(lfold, cfg.pruneratio);
% compute the three interpolation matrices
fprintf('computing interpolation matrix #1\n');
realign = lfnew * tmp;
if strcmp(cfg.verify, 'yes')
  fprintf('computing interpolation matrix #2\n');
  noalign = lfold * tmp;
  fprintf('computing interpolation matrix #3\n');
  bkalign = lfold * prunedinv(lfnew, cfg.pruneratio) * realign;
end

% interpolate the data towards the template gradiometers
for i=1:Ntrials
  fprintf('realigning trial %d\n', i);
  data.realign{i} = realign * data.trial{i};
  % also compute the residual variance when interpolating
  rvrealign = rv(data.trial{i}, data.realign{i});
  fprintf('original -> template             RV %.2f %%\n', 100 * mean(rvrealign));
  if strcmp(cfg.verify, 'yes')
    data.noalign{i} = noalign * data.trial{i};
    data.bkalign{i} = bkalign * data.trial{i};
    rvnoalign = rv(data.trial{i}, data.noalign{i});
    rvbkalign = rv(data.trial{i}, data.bkalign{i});
    fprintf('original             -> original RV %.2f %%\n', 100 * mean(rvnoalign));
    fprintf('original -> template -> original RV %.2f %%\n', 100 * mean(rvbkalign)); 
  end
end

% plot the topography before and after the realignment
if strcmp(cfg.plot3d, 'yes')
  warning('showing MEG topography (RMS value over time) in the first trial only');
  Nchan = length(data.grad.label);
  pnt1 = data.grad.pnt(1:Nchan,:);
  pnt2 = template.grad.pnt(1:Nchan,:);
  prj1 = elproj(pnt1); tri1 = delaunay(prj1(:,1), prj1(:,2));
  prj2 = elproj(pnt2); tri2 = delaunay(prj2(:,1), prj2(:,2));
  p1 = sqrt(mean(data.trial{1}.^2, 2));
  p2 = sqrt(mean(data.realign{1}.^2, 2));
  
  figure
  hold
  plot3(pnt1(:,1), pnt1(:,2), pnt1(:,3), 'r.')
  plot3(pnt2(:,1), pnt2(:,2), pnt2(:,3), 'g.')
  X = [pnt1(:,1) pnt2(:,1)]';
  Y = [pnt1(:,2) pnt2(:,2)]';
  Z = [pnt1(:,3) pnt2(:,3)]';
  line(X,Y,Z, 'color', 'black');
  axis equal; axis vis3d
  triplot(pnt1, tri1, p1);
  title('RMS, before realignment')
  view(-90, 90)
  
  figure
  hold
  plot3(pnt1(:,1), pnt1(:,2), pnt1(:,3), 'r.')
  plot3(pnt2(:,1), pnt2(:,2), pnt2(:,3), 'g.')
  X = [pnt1(:,1) pnt2(:,1)]';
  Y = [pnt1(:,2) pnt2(:,2)]';
  Z = [pnt1(:,3) pnt2(:,3)]';
  line(X,Y,Z, 'color', 'black');
  axis equal; axis vis3d
  triplot(pnt2, tri2, p2);
  title('RMS, after realignment')
  view(-90, 90)
end

% store the realigned data in a new structure
interp.label   = template.grad.label;
interp.grad    = template.grad;	  % replace with the template gradiometer array
interp.trial   = data.realign;    % remember the processed data
interp.fsample = data.fsample;
interp.time    = data.time;

% add the rest channels back to the data, these were not interpolated
if ~isempty(rest.label)
  fprintf('adding %d non-MEG channels back to the data\n', length(rest.label));
  for trial=1:length(rest.trial)
    interp.trial{trial} = [interp.trial{trial}; rest.trial{trial}];
  end
  interp.label = [interp.label; rest.label];
end

% convert raw data back to average using simple helper function
% this allows the same function to work on multiple data types
[interp] = raw2data(interp, inputdimord);

% store the configuration of this function call, including that of the previous function call
try
  % get the full name of the function
  cfg.version.name = mfilename('fullpath');
catch
  % required for compatibility with Matlab versions prior to release 13 (6.5)
  [st, i] = dbstack;
  cfg.version.name = st(i);
end
cfg.version.id   = '$Id: megrealign.m,v 1.23 2006/02/24 16:37:56 roboos Exp $';
% remember the configuration details of the input data
try, cfg.previous = data.cfg; end
% remember the exact configuration details in the output 
interp.cfg = cfg;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunction that computes the inverse using a pruned SVD
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [lfi] = prunedinv(lf, r)
[u, s, v] = svd(lf);
p = find(s<(s(1,1)*r) & s~=0);
fprintf('pruning %d out of %d singular values\n', length(p), min(size(s)));
s(p) = 0;
s(find(s~=0)) = 1./s(find(s~=0));
lfi = v * s' * u';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunction that computes the homogenous translation matrix
% corresponding to a fixed body rotation and translation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [h] = fixedbody(center, dirx, diry, dirz);
rot = eye(4);
rot(1:3,1:3) = inv(eye(3) / [dirx; diry; dirz]);
tra = eye(4);
tra(1:4,4)   = [-center 1]';
h = rot * tra;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunction that scales a vector to unit length
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [v] = normalize(v);
v = v / sqrt(v * v');
