%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matlab script: displayMotionField.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Displays point trajectories on specified slices of a 3D volume.
% The volume is loaded in *.mat format and the motion field in binary
% format. 
% Both are available at 
% www.vision.ethz.ch/mdri
% Author: Martin von Siebenthal, 
% Computer Vision Laboratory,
% ETH Zurich, Switzerland
% motion@vision.ee.ethz.ch
%
% This program, the images and the motion data available at
% www.vision.ethz.ch/4dmri
% are provided for academic research purpose only.
% Any publication using this data must cite this url 
% and/or a respective article.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all
close all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% start configuration part
%

% motion field filename
motionFile = 'motionfields/motionfield_subjA_cyc1_ds15.bin'

% volume image filename
% only one per subject!
imgFilename = 'images/exhalation_subjA_cyc3.mat' 

% which planes to show
% x: P->A,   y: I->S,   z: R->L
showX   = [108 128 148];   % [pixel]    AP direction, coronal planes
showY   = [108 128 148];   % [pixel]    SI direction, transversal planes
showZ   = [10 15 20];      % [slice nr] LR direction, sagittal planes

showTol = 3;       % [mm] points up to this distance are shown on a plane

%
% end configuration part
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read image 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read image "img" and 
% pixel dimensions "xySpacing", "zSpacing" from file

load(imgFilename)

% find origin to shift the displacement field
[dimX,dimY,dimZ] = size(img);
shiftY = (dimY - 1)/2 * xySpacing;
shiftX = (dimX - 1)/2 * xySpacing;
shiftZ = (dimZ - 1)/2 * zSpacing;

% calculate interpolation factor to obtain an isotropic voxel size
fx = 1;
fy = 1;
fz = xySpacing/zSpacing;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read motion field
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fid = fopen(motionFile, 'r');

% read a vector of floats
c = fread(fid, 'float');
fclose(fid);  

% each line contains 5 floats
% pointNumber time x y z
nLines = length(c)/5;

% the first line is used to pass some values
nPoints   = c(1); % number of points
dt        = c(2); % normalized time step, 1.0 = duration of one registration
nSteps    = c(3); % number of time steps
dontcare  = c(4);
dontcare  = c(5);

% extract coordinates only (x y z)
ccrop = [];
for li = 2:nLines
  ccrop = [ccrop ; c((li-1)*5+[3:5])];
end
nPoints = (nLines-1)/nSteps

% fill into 'coos' matrix
% dim 1: point number
% dim 2: time step
% dim 3: coordinate (1=x,2=y,3=z)
% e.g. coos(2,5,3) is the z coordinate of the 2nd point after the fourth
% time step (at time instant 5)
coos = zeros(nPoints,nSteps,3);
for p = 1:nPoints
  coos(p,:,:) = reshape(ccrop((p-1)*nSteps*3+[1:nSteps*3]),3,nSteps)';
end

vecT = [0:nSteps-1]*dt;

% convert coordinates to pixel dimensions
coosP = coos;
coosP(:,:,1) = (coos(:,:,1)+shiftX)/xySpacing + 1;
coosP(:,:,2) = (coos(:,:,2)+shiftY)/xySpacing + 1;
coosP(:,:,3) = (coos(:,:,3)+shiftZ)/zSpacing  + 1;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% show vectors on sagittal planes (LR or z direction)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nPlanes = length(showZ);
for p = 1:nPlanes
  pix = showZ(p);

  % find closest points for considered plane
  [v,i]   = min(abs(round(coosP(:,1,3))-pix));
  pixShow = round(coosP(i,1,3));
  % constrain to valid range
  pixShow = min(max(pixShow,1),dimZ);
  
  % interpolate
  zim    = img(:,:,pixShow)';
  xvec   = [1:fx:dimX];
  yvec   = [1:fy:dimY]';
  zimint = interp2(zim,xvec,yvec,'spline');
  
  figure
  imagesc(xvec,yvec,zimint);colormap(gray);

  xlabel('A -> P');
  ylabel('I -> S');
  hold on
  title(['Z = ' int2str(pixShow) ' pixel'])

  % extract points that lie on this plane
  okPointsZ = find(abs(coosP(:,1,3)-pixShow)<showTol);
  coosPlane = coosP(okPointsZ,:,:);

  % show vectors for all time steps
  for t = 1:nSteps-1
    startX = coosPlane(:,t,1);
    startY = coosPlane(:,t,2);
    stopX  = coosPlane(:,t+1,1);
    stopY  = coosPlane(:,t+1,2);
    quiver(startX,startY,stopX-startX,stopY-startY,0,'b')
    if (t==1)
      % mark starting point
      plot(startX,startY,'go');
    end
  end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% show vectors on transversal planes (SI or y direction)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nPlanes = length(showY);
for p = 1:nPlanes
  pix = showY(p);

  % find closest points for considered plane
  [v,i] = min(abs(round(coosP(:,1,2))-pix));
  pixShow = round(coosP(i,1,2));
  
  %interpolate
  yim = squeeze(img(:,pixShow,:))';
  xvec = [1:fx:dimX];
  zvec = [1:fz:dimZ]';
  yimint = interp2(yim,xvec,zvec,'spline');

  figure
  imagesc(zvec,xvec,yimint');colormap(gray);
  set(gca,'YDir','normal')
  xlabel('R -> L');
  ylabel('A -> P');
  hold on
  title(['Y = ' int2str(pixShow) ' pixel'])

  % extract points that lie on this plane
  okPointsY = find(abs(coosP(:,1,2)-pixShow)<showTol);
  coosPlane = coosP(okPointsY,:,:);

  % show vectors for all time steps
  for t = 1:nSteps-1
    startX = coosPlane(:,t,1);
    startZ = coosPlane(:,t,3);
    stopX  = coosPlane(:,t+1,1);
    stopZ  = coosPlane(:,t+1,3);
    quiver(startZ,startX,stopZ-startZ,stopX-startX,0,'b')
    if (t==1)
      % mark starting point
      plot(startZ,startX,'go');
    end
  end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% show vectors on coronal planes (AP or x direction)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nPlanes = length(showX);
for p = 1:nPlanes
  pix = showX(p);

  % find closest points for considered plane
  [v,i] = min(abs(round(coosP(:,1,1))-pix));
  pixShow = round(coosP(i,1,1));
  
  xim = squeeze(img(pixShow,:,:));
  zvec = [1:fz:dimZ];
  yvec = [1:fy:dimY]';
  ximint = interp2(xim,zvec,yvec,'spline');
  
  figure
  imagesc(zvec,yvec,ximint);colormap(gray);
  xlabel('R -> L');
  ylabel('I -> S');
  hold on
  title(['X = ' int2str(pixShow) ' pixel'])

  % extract points that lie on this plane
  okPointsX = find(abs(coosP(:,1,1)-pixShow)<showTol);
  coosPlane = coosP(okPointsX,:,:);

  % show vectors for all time steps
  for t = 1:nSteps-1
    startY = coosPlane(:,t,2);
    startZ = coosPlane(:,t,3);
    stopY  = coosPlane(:,t+1,2);
    stopZ  = coosPlane(:,t+1,3);
    quiver(startZ,startY,stopZ-startZ,stopY-startY,0,'b')
    if (t==1)
      % mark starting point
      plot(startZ,startY,'go');
    end
  end
end

