%Script to map a 3D source volume (LM image stack) to a 3D target volume (EM image stack) 
% based on a cloud of corresponding points.
% By Daniel Berger, Oct 2016

clear all;

lmimagenametemplate='..\\LMimages\\DAPI_flipped_backwards\\DAPI_z%02d_flipped_backwards.tif';
targetfolder='h:/lm_in_emcoords_lowres/DAPI2/';
tfilenametemplate='DAPI_LM_as_EM_s%04d.png';

[cemname,cemdata,cemfirstelement]=scanvastcolorfile('emcoords1.txt',0);
[clmname,clmdata,clmfirstelement]=scanvastcolorfile('lmcoords1.txt',0);

emcoords=[cemdata(:,11:13)+1];
lmcoords=[clmdata(:,11:13)+1];
emcoords=emcoords(1:16,:);
lmcoords=lmcoords(1:16,:);

sourcesize=[1989 1990 42];
targetsize=[9804 9660 262]
%targetsize=[12288 12288 259];
nrofcolorchannels=1;

showroughalign=1;
computeaffinealignment=1;
showaffinebox=1;
loadlmimagestack=0;
renderlm2em=0;

%%%%%%%%%%%%%%%% Look at corresponding points in 3D

if (showroughalign==1)
  figure(1);
  for i=1:size(emcoords,1)
    x=[emcoords(i,1) lmcoords(i,1)*41.6-3500.0];
    y=[emcoords(i,2) lmcoords(i,2)*41.6-500.0];
    z=[emcoords(i,3) lmcoords(i,3)*20-200];
    color=[0 0 0];
    if (i>24) color=[0 1 0]; end;
    line(x,y,z,'color',color);
    line(x(1),y(1),z(1),'Marker','.');
  end;
  grid on;
  %axis equal;
end;

%%%%%%%%%%%%%% Compute Affine Transformation

if (computeaffinealignment==1)
  %mp1=[[0 0]; [0 8191]; [8191 8191]; [8191 0]];
  %mp2=[[196 347]; [479 269]; [572 604]; [289 683]];
  
  mp1=emcoords;
  mp2=lmcoords;
  
  m=[];
  for i=1:size(emcoords,1)
    %X = [mp1(i,1) mp1(i,2) 1  0  0  0;   0  0 0 mp1(i,1) mp1(i,2)  1 ];
    X = [mp1(i,1) mp1(i,2) mp1(i,3) 1  0 0 0 0  0 0 0 0; ...
      0 0 0 0  mp1(i,1) mp1(i,2) mp1(i,3) 1  0 0 0 0; ...
      0 0 0 0  0 0 0 0  mp1(i,1) mp1(i,2) mp1(i,3) 1];
    m=[m; X];
  end;
  
  %v=[mp2(:,1); mp2(:,2)];
  %v=[196 347 479 269 572 604 289 683]';
  v=mp2'; v=v(:);
  
  a=m\v;
  %lm2em=[reshape(a,[3 2])'; [0 0 1]]; %This affine matrix converts EM coords to LM coords
  em2lm=[reshape(a,[4 3])'; [0 0 0 1]]; %This affine matrix converts EM coords to LM coords
  lm2em=inv(em2lm);
  
  
  figure(2);
  edist=0;
  for i=1:size(lmcoords,1)
    temcoords=lm2em*[lmcoords(i,:)'; 1];
    
    x=[emcoords(i,1)*50 temcoords(1)*50];
    y=[emcoords(i,2)*50 temcoords(2)*50];
    z=[emcoords(i,3)*50 temcoords(3)*50];
    color=[0 0 0];
    if (i>24) color=[0 1 0]; end;
    line(x,y,z,'color',color);
    line(x(1),y(1),z(1),'Marker','.');
    edist=edist+sqrt((x(1)-x(2))*(x(1)-x(2))+(y(1)-y(2))*(y(1)-y(2))+(z(1)-z(2))*(z(1)-z(2)));
  end;
  grid on;
  axis equal;
  
  edist=edist/size(lmcoords,1);
  disp(sprintf('Average distance of transformed support points to targets: %f microns',edist/1000));
end;

if (showaffinebox==1)  
  le=lm2em(1:3,1:3);
  box=[[0 0 0]; [1 0 0]; [1 1 0]; [0 1 0];  [0 0 1]; [1 0 1]; [1 1 1]; [0 1 1]];
  %box=[[0 0 0]; [1 0 0]; [1 1 0]; [0 1 0]; [0 0 0]; [0 0 1]; [1 0 1]; [1 1 1]; [0 1 1]; [0 0 1]; [1 0 0]; [1 0 1]; [1 1 1]; [1 1 0]];
  tbox=box*le;
%   figure(10);
%   plot3(tbox(:,1),tbox(:,2),tbox(:,3));
%   hold on;
%   plot3(box(:,1),box(:,2),box(:,3),'r');
%   grid on;
%   axis equal;
  figure(10);
  plot3(tbox(:,1)*50,tbox(:,2)*50,tbox(:,3)*50);
  hold on;
  plot3(box(:,1)*208,box(:,2)*208,box(:,3)*1000,'r');
  grid on;
  axis equal;
  hold off;
  
  %quads=[[4 3 2 1]; [5 6 7 8]; [1 2 6 5]; [3 4 8 7]; [1 4 8 5]; [2 3 7 6]];
  quads=[[4 3 2 1]; [5 6 7 8]; [1 2 6 5]; [3 4 8 7]; [5 8 4 1]; [2 3 7 6]];
  
  v=box.*(ones(size(box,1),1)*[208 208 1000]);
  f=quads;
  filename='origbox.obj';
  objectname='origbox';
  fid = fopen(filename,'wt');
  %fprintf(fid,'mtllib %s\n',mtlfilename);
  %fprintf(fid,'usemtl %s\n',materialname);
  for i=1:size(v,1)
    fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
  end
  fprintf(fid,'g %s\n',objectname);
  for i=1:size(f,1);
    fprintf(fid,'f %d %d %d %d\n',f(i,1),f(i,2),f(i,3),f(i,4));
  end
  fprintf(fid,'g\n');
  fclose(fid);
  
  v=tbox*50;
  f=quads;
  filename='tbox.obj';
  objectname='tbox';
  fid = fopen(filename,'wt');
  %fprintf(fid,'mtllib %s\n',mtlfilename);
  %fprintf(fid,'usemtl %s\n',materialname);
  for i=1:size(v,1)
    fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
  end
  fprintf(fid,'g %s\n',objectname);
  for i=1:size(f,1);
    fprintf(fid,'f %d %d %d %d\n',f(i,1),f(i,2),f(i,3),f(i,4));
  end
  fprintf(fid,'g\n');
  fclose(fid);
  
  %lbox=box.*(ones(size(box,1),1)*[0.208 0.208 1]);
  lbox=box.*(ones(size(box,1),1)*([sourcesize(2) sourcesize(1) sourcesize(3)])); %%in source pixels
  %lbox=lbox*le; %in target pixels
  lbox=(le*lbox')';
  %lbox=lbox.*(ones(size(box,1),1)*(sourcesize.*[0.208 0.208 1]));
  v=lbox*(50/1000);
  trans=lm2em(1:3,4)*(50/1000);
  v(:,1)=v(:,1)+trans(1);
  v(:,2)=v(:,2)+trans(2);
  v(:,3)=v(:,3)+trans(3);
  
  v2=v;
  v(:,1)=v2(:,2);
  v(:,2)=v2(:,1);
  %v(:,3)=-v(:,3); %Z is negative in the EM stack
  f=quads;
  filename='tlbox.obj';
  objectname='tlbox';
  fid = fopen(filename,'wt');
  %fprintf(fid,'mtllib %s\n',mtlfilename);
  %fprintf(fid,'usemtl %s\n',materialname);
  for i=1:size(v,1)
    fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
  end
  fprintf(fid,'g %s\n',objectname);
  for i=1:size(f,1);
    %fprintf(fid,'f %d %d %d %d\n',f(i,1),f(i,2),f(i,3),f(i,4));
    fprintf(fid,'f %d %d %d %d\n',f(i,4),f(i,3),f(i,2),f(i,1)); %inverted normals
  end
  fprintf(fid,'g\n');
  fclose(fid);
end;

%%%%%%%%%%%%%%%%%%%%%% Load LM image stack
if (loadlmimagestack==1)
  if (nrofcolorchannels==3)
    %lmvolume=zeros(sourcesize,'uint8');
    lmvolume_r=zeros(sourcesize);
    lmvolume_g=zeros(sourcesize);
    lmvolume_b=zeros(sourcesize);
    for i=0:sourcesize(3)-1
      sourcefilename=sprintf(lmimagenametemplate,i);
      img=imread(sourcefilename);
      lmvolume_r(:,:,i+1)=img(:,:,1);
      lmvolume_g(:,:,i+1)=img(:,:,2);
      lmvolume_b(:,:,i+1)=img(:,:,3);
    end;
  end;
  if (nrofcolorchannels==1)
    %lmvolume=zeros(sourcesize,'uint8');
    lmvolume=zeros(sourcesize);
    for i=0:sourcesize(3)-1
      sourcefilename=sprintf(lmimagenametemplate,i);
      img=imread(sourcefilename);
      lmvolume(:,:,i+1)=img;
    end;
  end;
  disp('LM image stack loaded.');
end;

if (renderlm2em==1)
  %%%%%%%%%%%%%%%%%%%%%%% Render LM image stack in EM coordinates
  if (exist(targetfolder, 'dir') == 0)
    [status,message,messageid]=mkdir(targetfolder); %status 0 means error
  end;
  
  x=1:16:targetsize(2);
  y=1:16:targetsize(1);
  [xi,yi]=meshgrid(x,y);
  
  for z=1:targetsize(3)
    %v=[xi(:) yi(:)];
    len=length(xi(:));
    v=[xi(:) yi(:) z*ones(len,1) ones(len,1)];
    tv=em2lm*v';
    tv=tv(1:3,:);
    
    if (nrofcolorchannels==3)
      rvals=interp3(lmvolume_r,tv(1,:),tv(2,:),tv(3,:));
      gvals=interp3(lmvolume_g,tv(1,:),tv(2,:),tv(3,:));
      bvals=interp3(lmvolume_b,tv(1,:),tv(2,:),tv(3,:));
      
      cimg=zeros(length(y),length(x),3);
      cimg(:,:,1)=reshape(rvals,length(y),length(x));
      cimg(:,:,2)=reshape(gvals,length(y),length(x));
      cimg(:,:,3)=reshape(bvals,length(y),length(x));
      
      figure(3);
      imshow(cimg/255);
      
      timg=zeros(targetsize(1),targetsize(2),3,'uint8');
      timg(:,:,1)=uint8(imresize(cimg(:,:,1), [targetsize(1) targetsize(2)],'bilinear'));
      timg(:,:,2)=uint8(imresize(cimg(:,:,2), [targetsize(1) targetsize(2)],'bilinear'));
      timg(:,:,3)=uint8(imresize(cimg(:,:,3), [targetsize(1) targetsize(2)],'bilinear'));
    end;
    if (nrofcolorchannels==1)
      gvals=interp3(lmvolume,tv(1,:),tv(2,:),tv(3,:));
      %cimg=zeros(length(y),length(x));
      cimg=reshape(gvals,length(y),length(x));
      figure(3);
      imshow(cimg/255);
      
      %timg=zeros(12288,12288,'uint8');
      timg=uint8(imresize(cimg, [targetsize(1) targetsize(2)],'bilinear'));
    end;
    
    tfilename=sprintf(tfilenametemplate,z-1);
    disp(['Writing ' targetfolder tfilename '...']);
    imwrite(timg,[targetfolder tfilename]);
  end;
end;

% %load lm stack
% if (1==1)
%   lcuber=zeros(sourcesize);
%   lcubeg=zeros(sourcesize);
%   lcubeb=zeros(sourcesize);
%   for sslice=1:278
%     sfilename=sprintf('./lm/MG_LMEM_Image 12_Stitch_%04d.png',sslice);
%     disp(['Reading ' sfilename '...']);
%     lmimg=imread(sfilename);
%     lcuber(:,:,sslice)=lmimg(:,:,1);
%     lcubeg(:,:,sslice)=lmimg(:,:,2);
%     lcubeb(:,:,sslice)=lmimg(:,:,3);
%   end;
% end;
% 
% % XI=0:16:8191;
% % YI=XI';
% % ZI=0:10:300;
% % sx=griddata(emcoords(:,1),emcoords(:,2),emcoords(:,3),lmcoords(:,1),XI,YI,ZI);
% 
% x=1:16:8192;
% y=x;
% z=1:300; %z=0:10:300;
% [xi,yi,zi]=meshgrid(x,y,z);
% 
% disp('Computing LM->EM mapping for X...');
% Fx=TriScatteredInterp(emcoords(:,1),emcoords(:,2),emcoords(:,3),lmcoords(:,1));
% qx=Fx(xi,yi,zi);
% qx(isnan(qx))=0;
% disp('Computing LM->EM mapping for Y...');
% Fy=TriScatteredInterp(emcoords(:,1),emcoords(:,2),emcoords(:,3),lmcoords(:,2));
% qy=Fy(xi,yi,zi);
% qy(isnan(qy))=0;
% disp('Computing LM->EM mapping for Z...');
% Fz=TriScatteredInterp(emcoords(:,1),emcoords(:,2),emcoords(:,3),lmcoords(:,3));
% qz=Fz(xi,yi,zi);
% qz(isnan(qz))=0;
% 
% % disp('Scaling up to full target volume size in Z...');
% % z2=[0:299]/299*size(qz,3);
% % [xi,yi,zi]=meshgrid(1:size(qx,1),1:size(qy,2),z2);
% % vqx=interp3(qx,xi,yi,zi);
% % vqy=interp3(qy,xi,yi,zi);
% % vqz=interp3(qz,xi,yi,zi);
% vqx=qx; vqy=qy; vqz=qz;
% 
% vqx(isnan(vqx))=0;
% vqy(isnan(vqy))=0;
% vqz(isnan(vqz))=0;
% 
% lmback=[Fx(emcoords(:,1),emcoords(:,2),emcoords(:,3)) Fy(emcoords(:,1),emcoords(:,2),emcoords(:,3)) Fz(emcoords(:,1),emcoords(:,2),emcoords(:,3))];

% for tslice=101:size(vqx,3)
%   disp(sprintf('Slice %d of %d ...',tslice,size(vqx,3)));
%   cx=squeeze(vqx(:,:,tslice));
%   cy=squeeze(vqy(:,:,tslice));
%   cz=squeeze(vqz(:,:,tslice));
%   
%   timgr=interp3(lcuber,cx,cy,cz);
%   timgg=interp3(lcubeg,cx,cy,cz);
%   timgb=interp3(lcubeb,cx,cy,cz);
%   timgr(isnan(timgr))=0;
%   timgg(isnan(timgg))=0;
%   timgb(isnan(timgb))=0;
%   
%   timgr=imresize(timgr,[8192 8192]);
%   timgg=imresize(timgg,[8192 8192]);
%   timgb=imresize(timgb,[8192 8192]);
%   
%   timg=zeros(8192,8192,3);
%   timg(:,:,1)=timgr;
%   timg(:,:,2)=timgg;
%   timg(:,:,3)=timgb;
%   
%   timg=uint8(timg);
%   tfilename=sprintf(tfilenametemplate,tslice);
%   imwrite(timg,tfilename);
% end;