function warp_everything(lm_real, lm_phantom, surface_file, mr_file, warped_surface_file,warped_mr_file)
%WARP_EVERYTHING - (summary line here)
% function warp_everything(lm_real, lm_phantom, surface_file, mr_file, warped_surface_file,warped_mr_file)
%
% inputs:
% lm_real = landmarks in real head coordinates, i.e. from a Polhemus or
%   Zebris 3D localizer. Units must be scaled to mm! format is n x 3 (n=#
%   landmarks). The first three entries MUST be Nasion, Left PreAuricular
%   Point and Right preauricular Point in this order!
%   lm_phantom = landmarks in phantom coordinates. see lm_real.
%   surface_file = file name of the surface file to be warped. This MUST be
%   the filename only without the directory name. It is assumed that this
%   function is already in the appropriate directory when called!
% mr_file = mr file name. see surface file for details.
% warped_surface_file = output surface file containing the warped surfaces
% warped_mr_file = output mr file containing the warped MR structure

%<autobegin> ---------------------- 02-Nov-2004 16:16:34 -----------------------
% --------- Automatically Generated Comments Block Using AUTO_COMMENTS ---------
%
% CATEGORY: Visualization
%
% Alphabetical list of external functions (non-Matlab):
%   toolbox\save_fieldnames.m
%
% Subfunctions in this file, in order of occurrence in file:
%   [W,A,e]=warp_transform(p,q)
%   [rw]=warp_lm(r,A,W,p)
%
% At Check-in: $Author: Mosher $  $Revision: 4 $  $Date: 11/02/04 2:38p $
%
% This software is part of BrainStorm Toolbox Version 2.0 (Alpha) 02-Nov-2004
% 
% Principal Investigators and Developers:
% ** Richard M. Leahy, PhD, Signal & Image Processing Institute,
%    University of Southern California, Los Angeles, CA
% ** John C. Mosher, PhD, Biophysics Group,
%    Los Alamos National Laboratory, Los Alamos, NM
% ** Sylvain Baillet, PhD, Cognitive Neuroscience & Brain Imaging Laboratory,
%    CNRS, Hopital de la Salpetriere, Paris, France
% 
% See BrainStorm website at http://neuroimage.usc.edu for further information.
% 
% Copyright (c) 2004 BrainStorm by the University of Southern California
% This software distributed  under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPL
% license can be found at http://www.gnu.org/copyleft/gpl.html .
% 
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%<autoend> ------------------------ 02-Nov-2004 16:16:34 -----------------------



% /---Script Author--------------------------------------\
% |                                                      |
% | *** Felix Darvas, Ph.D                               |
% | University of Southern California                    |
% |                                                      |
% \------------------------------------------------------/

% ----------------------------- Script History ---------------------------------
% 08-Oct-2004 DP  Change input output arguments to integrate into BrainStorm
% JCM 11-Oct-2004 commenting header
% 13-Oct-2004 DP  Moved waitbar command on top for immediate feedback 
% ----------------------------- Script History ---------------------------------

hh=waitbar(0,'transforming coordinates'); 

[W,A,e]=warp_transform(lm_phantom,lm_real); % compute warp transform parameters

surf_in=load(surface_file);
surf_out=surf_in;

for i=1:length(surf_in.Vertices) % warp all surfaces 
    surf_out.Comment{i}=['warped ' surf_in.Comment{i}];
    surf_out.Vertices{i}=(warp_lm(surf_in.Vertices{i}',A,W,lm_phantom)+surf_in.Vertices{i}')';
end
mr_in=load(mr_file);

R=mr_in.SCS.R;
T=mr_in.SCS.T;

lm_phan_mr=(inv(R)*(lm_phantom'*1000-T*ones(1,size(lm_phantom,1))))'; % transform landmarks into MR coordinates
lm_real_mr=(inv(R)*(lm_real'*1000-T*ones(1,size(lm_real,1))))';

[Wmr,Amr]=warp_transform(lm_phan_mr,lm_real_mr); % compute warp transform in MR coordinates

ixc=find(mr_in.Cube>0); % get voxel coordinates. This part is quite memory intensive
[xv,yv,zv]=ind2sub(size(mr_in.Cube),ixc);
rv=[xv';yv';zv'];
clear xv yv zv
val=uint8(mr_in.Cube(ixc));
clear ixc;
sts=10000; % warp only sts coordinates at a time. Doing all at once costs too much memory, doing only 1 at a time costs too much time
rpcw=zeros(size(rv));
kk=0;
ix0=1;
for i=1:ceil(size(rv,2)/sts)
    ix1=ix0-1+sts;
    if(ix1>size(rv,2))
        ix1=size(rv,2);
    end
    kk=kk+1;
    if(kk>ceil(size(rv,2)/sts)/100)
        kk=0;
        waitbar(i/ceil(size(rv,2)/sts));
    end
    rpcw(:,ix0:ix1)=(warp_lm(rv(:,ix0:ix1)',Amr,Wmr,lm_phan_mr)+rv(:,ix0:ix1)')';
    ix0=ix1+1;
end
close(hh)
rpcw=ceil(rpcw);

for i=1:3
    ix=find(rpcw(i,:)<1);
    rpcw(i,ix)=1;
end
% set up new MR structure for BrainStorm
%newCube=uint8(zeros(size(mr_in.Cube)));
newCube=uint8(zeros(max(rpcw')));

ixn=sub2ind(size(newCube),rpcw(1,:)',rpcw(2,:)',rpcw(3,:)');
newCube(ixn)=val;
newMr=mr_in;
newMr.Cube=double(newCube);
clear newCube;
clear ixn;
clear rpcw;
clear rv;
newfid=lm_real_mr(1:3,:)';
origin=(newfid(:,2)+newfid(:,3))/2;
newMr.SCS.mmCubeFiducial=[newfid origin];
save_fieldnames(newMr,warped_mr_file);
save_fieldnames(surf_out,warped_surface_file);





function [W,A,e]=warp_transform(p,q)
% [W,A,e]=warp_transform(p,q)
% calculates nonlinear transformatin coefficents see Ermer's Thesis
% p... = Landmarks in system 1
% q... = landmarks in system 2
% e =warp energy
K=zeros(size(p,1),size(p,1));
for i=1:size(K,1)
    for j=1:size(K,2)
        K(i,j)=norm(p(i,:)-p(j,:));    
    end
end
P=[p ones(size(p,1),1)];
L=[K P;P' zeros(4,4)];
D=[q-p;zeros(4,3)];
H=L\D;
W=H(1:size(p,1),:);
A=H(size(p,1)+1:end,:);
e=sum(diag(W'*K*W));




function [rw]=warp_lm(r,A,W,p)
% function [rw]=warp_lm(r,A,W,p)
% performs warp transformation with linear 3D RFB see Ermer's Thesis
rw=r*A(1:3,1:3)+repmat(A(4,:),size(r,1),1);
for i=1:size(p,1)
  U=sqrt(sum((r-repmat(p(i,:),size(r,1),1)).^2,2));  
  rw=rw+U*W(i,:);
end



