function [mriT C]=cimec_coregister(cfg)
%cfg=[];
%cfg.datafile='AT_short.fif';
%or
%cfg.datafile=hs; %fieldtrip headshape structure
%
%cfg.mrifile='dicom/Anonymised by dicom_sort_convert-0002-0001-00001.dcm';
%or
%cfg.mrifile=mri; %a fieldtrip MRI structure that perhaps has been coreg in
%                   first pass
%
%mriT=cimec_coregister(cfg)
%
%optional inputs:
% cfg.skipfiducial: 'yes' or 'no' (default);
% cfg.skipfitune 'yes' or 'no' (default);
% cfg.reslice: 'yes' (default) or 'no'
% cfg.segmentedmri: structure after ft_volumesegment
%
%Output:
% mriT: mri structure co-registered to MEG space
% C: optional outputs (e.g. headshape, segmentedmri); useful when doing
%       multiple iterations 

%%
dataFile=cfg.datafile;
mriFile=cfg.mrifile;
C=[];

if isfield(cfg, 'warpmethod')
    warpmeth=cfg.warpmethod;
else
    warpmeth='rigidbody';
end

if isfield(cfg, 'skipfiducial')
    skipfid=cfg.skipfiducial;
else
    skipfid='no';
end

if isfield(cfg, 'skipfinetune')
    skipfine=cfg.skipfinetune;
else
    skipfine='no';
end

if isfield(cfg, 'reslice')
    resl=cfg.reslice;
else
    resl='yes';
end

if isfield(cfg, 'segmentedmri')
    segmented_mri=cfg.segmentedmri;
else
    segmented_mri=[];
end


%%
if ischar(dataFile)
    hs = ft_read_headshape(dataFile);
elseif isstruct(mriFile)
    hs=dataFile;
end
clear dataFile
C.hs=hs;

if ischar(mriFile)
    mri = ft_read_mri(mriFile);
elseif isstruct(mriFile)
    mri=mriFile;
end
C.mriorig=mri;
clear mriFile

%% define fiducials and upper z-point
if strcmpi(skipfid,'no')
    
%% flip MRI so that up is up
if strcmpi(resl, 'yes')
    mri = ft_volumereslice([], mri);
end    

cfg             = [];
cfg.method      = 'interactive';
cfg.coordsys    = 'neuromag';
mri_aligned     = ft_volumerealign(cfg,mri); 
else
    mri_aligned=mri;
end
clear mri


%%
if strcmpi(skipfine,'no')
%%
if isempty(segmented_mri)
cfg=[];
cfg.coordsys       = 'neuromag';
cfg.output = {'brain' 'scalp' 'skull'};
segmented_mri = ft_volumesegment(cfg,mri_aligned);
C.segmentedmri = segmented_mri;
end

%%

segmented_mri.scalp(segmented_mri.skull==1)=0;
segmented_mri.scalp(segmented_mri.brain==1)=0;

%% GET SCALP POINTS
[x y z] = ind2sub(size(segmented_mri.scalp),find(segmented_mri.scalp == 1));

%% CONVERT FROM VOXEL TO ELEKTA COORDINATES
hs_seg_mri=zeros(length(x),3);

for i = 1:length(x)
    ijk = [x(i) y(i) z(i) 1]';
    tmp=segmented_mri.transform * ijk;
    hs_seg_mri(i,:)=tmp(1:3)/10;%convert to cm as polhemus
end

clear x y z



%% remove bad headshape points

kk=0;

while kk==0
	fig=figure; dcm = datacursormode(fig);
	ft_plot_headshape(hs)
	
	tmp=input('Bad point? (0=no, 1=yes)');
	
	if tmp==1
		c_info = getCursorInfo(dcm);
		hs.pnt(c_info.DataIndex,:)
		hs.pnt(c_info.DataIndex,:)=[];
	end
	
	tmp=input('Quit? (0=no, 1=yes)');
	
	if tmp==1
		kk=1;
    end	
    close all
end

%%

[TransMat]=mri2hs_cimec(hs.pnt,hs_seg_mri,warpmeth);
%%
[hs_seg_mri_new]=warp_apply(TransMat, hs_seg_mri);
%%
figure
ft_plot_mesh(hs_seg_mri(1:50:end,:), 'facecolor','skin','vertexcolor','skin', 'edgecolor', 'none','facealpha',.5,'edgealpha',.5)
ft_plot_headshape(hs)
title('Non hs optimized')
%%
figure
ft_plot_mesh(hs_seg_mri_new(1:50:end,:), 'facecolor','skin','vertexcolor','skin', 'facealpha',.2,'edgealpha',.5)
ft_plot_headshape(hs)
title('hs optimized')

%%
whichmri=input('Happy with rigidbody transf.? (1=yes; 2=no): ');
mriT=mri_aligned;

if whichmri==1
    mriT.transform=TransMat*mri_aligned.transform;
    segmented_mri.transform=TransMat*segmented_mri.transform;
    C.segmentedmriNew=segmented_mri;
end
else
    mriT=mri_aligned;
    
end %skipfine

function [TransMat]=mri2hs_cimec(hsCoord_meg,mri_prjpts,meth)

tmp_mri_prjpts=mri_prjpts(1:50:end,:);
P = delaunayn(tmp_mri_prjpts);
[C, D] = dsearchn(tmp_mri_prjpts, P, hsCoord_meg);

mriP=tmp_mri_prjpts(C,:);    


if strcmpi(meth, 'icp')
[R, T] = icp(hsCoord_meg,mriP,[],[],[],3);
R(:,4)=[0 0 0];
R(4,:)=[0 0 0 1];
tmp=eye(4);
tmp(1:3,4)=T;T=tmp;
TransMat=T*R;
end

% 
if strcmpi(meth, 'rigidbody')
fb=0;
[hs_seg_mri_new TransMat]=warp_optim(mriP, hsCoord_meg, 'rigidbody');
TransMat = rigidbody(TransMat);
hs_seg_mri_new=warp_apply(TransMat, mriP);
end