%MATLAB Script File: corecon1.m
%
%   For the Cornea Study, this reconstructs surfaces from
%                               feature matrices
%
format compact ;
disp('MATLAB Script File:  corecon1.m') ;

figure(1) ;
clg reset ;
%disp('Hit any key to continue after clearing graphics') ;
%pause ;

autop = 1 ;   %  1 to create a color postscript file

iimag = 8 ;    %  0 - original test case
              %  1 - 84_1
              %  2 - 85_1
              %  3 - 122_2
              %  4 - 121_2
              %  5 - 55_1
              %  6 - 28_2
              %  7 - 130_1
              %  8 - 139_2
              %  9 - 6_1
              %  10 - 53_1
              %  11 - 43_1

isurf = 3 ;   %  1 - for height map
              %  2 - for height map - sphere
              %  3 - for curvature map


ibas = 1 ;    %  1 - Radial Fourier - Legendre
              %  2 - Diameter Fourier - Legendre
              %            (not working yet)
              %  3 - Zernike
              %            (not working yet)

itest = 0 ;   %  0 - For real images
              %  1 - for tests using basis functions

r0 = 4.2 ;    %  Usual radius of analysis
%spherad = 9.3 ;%  Radius of sphere to subtract
%  now set individually for each image

ngrid = 128 ; %  Construct surfaces as ngrid x ngrid array
ngrid = 64 ; %  Construct surfaces as ngrid x ngrid array

xgrid = linspace(-r0,r0,ngrid) ;
ygrid = xgrid ;
              %  Vectors of image coordinate values 

[mxgrid, mygrid] = meshgrid(xgrid,ygrid) ;
              %  Matrices of image coordinate values 
              
mrgrid = sqrt(mxgrid.^2 + mygrid.^2) ;
              %  Matrices of image polar coordinate radii
mthetag = atan2(mxgrid,mygrid) ;
              %  Matrices of image polar coordinate angles (-pi,pi]
  vind = find(mthetag < 0) ;
mthetag(vind) = mthetag(vind) + 2 * pi ;
              %  Matrices of image polar coordinate angles [0,2pi)
          
mcrop = ones(ngrid,ngrid) ;            
  vind = find(mrgrid > r0) ;
              %  Vector of indices where image is outside circle
mcrop(vind) = nan * ones(length(vind),1) ;
              %  Cropping Matrix, multiply by this to cut off images
              %  outside circle of radius r_0.       


if itest == 1 ;    %  then do some basis functions to check things work
  mfeat = zeros(7,19) ;
%  mfeat(2,1) = 1 ;
%  mfeat(3,1) = 1 ;
%  mfeat(1,2) = 1 ;
%  mfeat(1,3) = 1 ;
%  mfeat(2,2) = 1 ;
%  mfeat(1,4) = 1 ;
%  mfeat(3,2) = 1 ;
  mfeat(3,4) = 1 ;

elseif itest == 0 ;   % Then do serious images

  if iimag == 0 ;   %  Then do original test case
    patstr = '1st Test' ;

        %  Read in Amplitude feature matrix
    fid = fopen('\matlab\steve\cornea\000063_2.am1') ;
                %  opens file for reading
    fmamp = fscanf(fid,'%g %g %g %g %g %g %g %g %g %g',[10,7]) ;
                %  formatted read of file, 
                %  fills 10 x 7 matrix in column order
    fmamp = fmamp' ;
                %  puts matrix same as in file
    fclose(fid) ;

        %  Read in Phase feature matrix
    fid = fopen('\matlab\steve\cornea\000063_2.ph1') ;
                %  opens file for reading
    fmpha = fscanf(fid,'%g %g %g %g %g %g %g %g %g %g',[10,7]) ;
                %  formatted read of file, 
                %  fills 10 x 7 matrix in column order
    fmpha = fmpha' ;
                %  puts matrix same as in file
    fmpha = 2 * pi * fmpha / 360 ;
                %  converts degrees to radians
    fclose(fid) ;

  elseif iimag > 0 ;   %  Then do serious cases

    nr = 10 ;
    nc = 7 ;

    if iimag == 1 ;
      patstr = 'Image 84_1' ;
      fmistr = '42000841.ap' ;
      pdist = .123 ;
      pang = 123 ;
      spherad = 7.941 ;  %  Radius of sphere to subtract
      cstep = 6 ;  %  Step size for color scale
    elseif iimag == 2 ;
      patstr = 'Image 85_1' ;
      fmistr = '42000851.ap' ;
      pdist = .254 ;
      pang = 68 ;
      spherad = 7.258 ; %  Radius of sphere to subtract
      cstep = 25 ;  %  Step size for color scale
    elseif iimag == 3 ;
      patstr = 'Image 122_2' ;
      fmistr = '42001222.ap' ;
      pdist = .686 ;
      pang = 89 ;
      spherad = 7.8488 ; %  Radius of sphere to subtract
      cstep = 35 ;  %  Step size for color scale
    elseif iimag == 4 ;
      patstr = 'Image 121_2' ;
      fmistr = '42001212.ap' ;
      pdist = .525 ;
      pang = 108 ;
      spherad = 337.5 / 41 ;  %  Radius of sphere to subtract (Diotpers)
      cstep = 40 ;  %  Step size for color scale
    elseif iimag == 5 ;
      patstr = 'Image 55_1' ;
      fmistr = '42000551.ap' ;
      pdist = .439 ;
      pang = 121 ;
      spherad = 337.5 / 43.5 ;  %  Radius of sphere to subtract (Diotpers)
      cstep = 12 ;  %  Step size for color scale
    elseif iimag == 6 ;
      patstr = 'Image 28_2' ;
      fmistr = '42000282.ap' ;
      pdist = .426 ;
      pang = 115 ;
      spherad = 8.0357 ; %  Radius of sphere to subtract
      cstep = 5 ;  %  Step size for color scale
    elseif iimag == 7 ;
      patstr = 'Image 130_1' ;
      fmistr = '42001301.ap' ;
      pdist = .458 ;
      pang = 112 ;
      spherad = 337.5 / 39.7 ;  %  Radius of sphere to subtract (Diotpers)
      cstep = 10 ;  %  Step size for color scale
%      spherad = 337.5 / 37.9 ;  %  Radius of sphere to subtract (Diotpers)
    elseif iimag == 8 ;
      patstr = 'Image 139_2' ;
      fmistr = '42001392.ap' ;
      pdist = .417 ;
      pang = 114 ;
      spherad = 8.4375 ; %  Radius of sphere to subtract
      cstep = 10 ;  %  Step size for color scale
    elseif iimag == 9 ;
      patstr = 'Image 6_1' ;
      fmistr = '42000061.ap' ;
      pdist = .454 ;
      pang = 116 ;
      spherad = 7.7586 ; %  Radius of sphere to subtract
      cstep = 6 ;  %  Step size for color scale
    elseif iimag == 10 ;
      patstr = 'Image 53_1' ;
      fmistr = '42000531.ap' ;
      pdist = .375 ;
      pang = 113 ;
      spherad = 8.766 ; %  Radius of sphere to subtract
      cstep = 10 ;  %  Step size for color scale
    elseif iimag == 11 ;
      patstr = 'Image 43_1' ;
      fmistr = '42000431.ap' ;
      pdist = .557 ;
      pang = 91 ;
      spherad = 8.5443 ; %  Radius of sphere to subtract
      cstep = 10 ;  %  Step size for color scale
    end ;


    pangr = (2 * pi * pang) / 360 ;
          %  Convert to radians

  
    fid = fopen(['/matlab/steve/cornea/' fmistr]) ;
    alldat = fscanf(fid,'%he') ;    
          %  Read in as a big column vector
    fclose(fid) ;

    fmamp = alldat(1:(nr*nc)) ;
    fmpha = alldat((nr*nc)+1:(2*nr*nc)) ;
          %  Separate into phase and amplitude column vectors

    fmamp = [reshape(fmamp(1:nr*(nc-1)),nc-1,nr)' fmamp(nr*(nc-1)+1:nr*nc)] ;
    fmpha = [reshape(fmpha(1:nr*(nc-1)),nc-1,nr)' fmpha(nr*(nc-1)+1:nr*nc)] ;
          %  turn from form in Jinting's matlab output to
          %  usual form

%  Use these lines to check Jinting's reconstruction
%fmamp = amp ;
%fmpha = pha ;
%amp(1,1)
%fmamp(1,1) 
%pause ;

    fmpha = 2 * pi * fmpha / 360 ;
                %  converts degrees to radians

    fmamp = fmamp' ;
    fmpha = fmpha' ;
           %  puts in same form as above

  end ;

      %  Convert to c-s form of feature matrix
  mfeat = fmamp(:,1) .* cos(fmpha(:,1)) ;
  for icol = 2:size(fmamp,2) ;   %  Build up c-s feature matrix
    mfeat = [mfeat (fmamp(:,icol) .* cos(fmpha(:,icol)))] ;
    mfeat = [mfeat (fmamp(:,icol) .* sin(fmpha(:,icol)))] ;
  end ;

end ;

disp(['feature matrix has size ' num2str(size(mfeat,1)) ' rows ']) ;
disp(['and ' num2str(size(mfeat,2)) ' cols']) ;



disp('Generating surface') ; 
if isurf == 1 | isurf == 2 ;   %  then work with height map
  surfstr = ' Heights' ;

  mout = zeros(size(mxgrid)) ;
  if ibas == 1 ;     %  then use the radial Fourier - Legendre basis

    ell0 = size(mfeat,1) - 1 ;
              %  Highest Legendre order, 
              %  i.e. number rows in feature matrix - 1
    m0 = (size(mfeat,2) - 1) / 2 ;
              %  Highest Fourier order, 
              %  i.e. (number rows in feature matrix -1) / 2 

    mpolyarg = (2 * mrgrid ./ r0) - 1 ;

    %  Do ell = 0 Legendre
    llleg = ones(size(mpolyarg)) ;
              %  Last, last Legendre for future updating
    trigfact = mfeat(1,1) ;
    for m = 1:m0 ;
      trigfact = trigfact + mfeat(1,2*m) .* cos(m * mthetag) ;
      trigfact = trigfact + mfeat(1,2*m+1) .* sin(m * mthetag) ;
    end ;
    mout = mout + trigfact .* llleg ;

    %  Do ell = 1 Legendre
    lleg = mpolyarg ;
              %  Last Legendre for future updating
    trigfact = mfeat(2,1) ;
    for m = 1:m0 ;
      trigfact = trigfact + mfeat(2,2*m) .* cos(m * mthetag) ;
      trigfact = trigfact + mfeat(2,2*m+1) .* sin(m * mthetag) ;
    end ;
    mout = mout + trigfact .* lleg ;

    %  Loop through rest of Legendre's
    for ell = 2:ell0 ;
      leg = (1/ell) .* ((2*ell-1) .* mpolyarg .* lleg - (ell-1) .* llleg) ;
              %  Legendre update formula

      trigfact = mfeat(ell+1,1) ;
      for m = 1:m0 ;
        trigfact = trigfact + mfeat(ell+1,2*m) .* cos(m * mthetag) ;
        trigfact = trigfact + mfeat(ell+1,2*m+1) .* sin(m * mthetag) ;
      end ;
      mout = mout + trigfact .* leg ;
      
      llleg = lleg ;
      lleg = leg ;
              %  Update for next run through loop
    end ;

  elseif ibas == 2 ;     %  then use the diameter Fourier - Legendre basis

  elseif ibas == 3 ;     %  then use the Zernike basis

  end ;

  if isurf == 2 ;   %  Then need to subtract sphere
    surfstr = [surfstr ' - Sphere'] ;

%    minmout = min(min(mout))
%    maxmout = max(max(mout))
            %  use these lines to decide which sphere to subtract

    sphht = spherad - sqrt(spherad.^2 - mrgrid.^2) ;

%    minsph = min(min(sphht))
%    maxsph = max(max(sphht))
%    pause
            %  use these lines to decide which sphere to subtract


    mout = mout + sphht ;
            %  careful about whether this is + or -
  end ;

elseif isurf == 3 ;   %  then work with curvature map
  surfstr = ' Curvatures' ;

  mhp = zeros(size(mxgrid)) ;
  mhpp = zeros(size(mxgrid)) ;
  if ibas == 1 ;     %  then use the radial Fourier - Legendre basis

    ell0 = size(mfeat,1) - 1 ;
              %  Highest Legendre order, 
              %  i.e. number rows in feature matrix - 1
    m0 = (size(mfeat,2) - 1) / 2 ;
              %  Highest Fourier order, 
              %  i.e. (number rows in feature matrix -1) / 2 

    mpolyarg = (2 * mrgrid ./ r0) - 1 ;

    %  Do ell = 0 Legendre
    llleg = ones(size(mpolyarg)) ;
              %  Last, last Legendre for future updating
    lllegp = zeros(size(mpolyarg)) ;
              %  Last, last Legendre prime for future updating
    lllegpp = zeros(size(mpolyarg)) ;
              %  Last, last Legendre prime, prime for future updating
    trigfact = mfeat(1,1) ;
    for m = 1:m0 ;
      trigfact = trigfact + mfeat(1,2*m) .* cos(m * mthetag) ;
      trigfact = trigfact + mfeat(1,2*m+1) .* sin(m * mthetag) ;
    end ;
%    mhp = mhp + trigfact .* lllegp ;
%    mhpp = mhpp + trigfact .* lllegpp ;
              %  Don't need to actually do these, since Legendre is 0

    %  Do ell = 1 Legendre
    lleg = mpolyarg ;
              %  Last Legendre for future updating
    llegp = ones(size(mpolyarg)) ;
              %  Last Legendre prime for future updating
    llegpp = zeros(size(mpolyarg)) ;
              %  Last Legendre prime, prime for future updating
    trigfact = mfeat(2,1) ;
    for m = 1:m0 ;
      trigfact = trigfact + mfeat(2,2*m) .* cos(m * mthetag) ;
      trigfact = trigfact + mfeat(2,2*m+1) .* sin(m * mthetag) ;
    end ;
    mhp = mhp + trigfact .* llegp ;
%    mhpp = mhpp + trigfact .* llegpp ;
              %  Don't need to actually do this, since Legendre is 0

    %  Loop through rest of Legendre's
    for ell = 2:ell0 ;
      leg = (1/ell) .* ((2*ell-1) .* mpolyarg .* lleg - (ell-1) .* llleg) ;
              %  Legendre update formula
      legp = (1/ell) .* ((2*ell-1) .* (lleg + mpolyarg .* llegp) ...
                                - (ell-1) .* lllegp) ;
              %  Legendre prime update formula
      legpp = (1/ell) .* ((2*ell-1) .* (2 * llegp + mpolyarg .* llegpp) ...
                                - (ell-1) .* lllegpp) ;
              %  Legendre prime, prime update formula

      trigfact = mfeat(ell+1,1) ;
      for m = 1:m0 ;
        trigfact = trigfact + mfeat(ell+1,2*m) .* cos(m * mthetag) ;
        trigfact = trigfact + mfeat(ell+1,2*m+1) .* sin(m * mthetag) ;
      end ;
      mhp = mhp + trigfact .* legp ;
      mhpp = mhpp + trigfact .* legpp ;
      
      llleg = lleg ;
      lleg = leg ;
      lllegp = llegp ;
      llegp = legp ;
      lllegpp = llegpp ;
      llegpp = legpp ;
              %  Update for next run through loop
    end ;
    mhp = mhp * (2 ./ r0) ;
    mhpp = mhpp * (2 ./ r0).^2 ;

  elseif ibas == 2 ;     %  then use the diameter Fourier - Legendre basis

  elseif ibas == 3 ;     %  then use the Zernike basis
  end ;

  mout = -mhpp ./ (1 + mhp.^2).^(3/2) ;
  mout = mout * 337.5 ;
          %  put on scale of diopters

end ;



%  These lines test Jinting's reconstruction
%mout = hh ;
%sphht = spherad - sqrt(spherad.^2 - mrgrid.^2) ;
%mout = mout + sphht ;





if isurf == 2 ;
          %  Then setup Keratron height color map

  %  These use the "full range" Keratron data, from the email message:
  %             cocolora.msg
  %   which were copied into the ASCII file 
  %             cocolor1.dat
  %  for importing into matlab programs
  %    locurve & hicurve are upper and lower ends of the "range"
  %    for each color
  load cornea\cocolor1.dat ;
  cyan = cocolor1(:,3) ;
  magenta = cocolor1(:,4) ;
  yellow = cocolor1(:,5) ;
  black = cocolor1(:,6) ;
  %  Don't use columns 1 and 2, because those are for curvature

  red   = 1 - cyan + black ;
  green = 1 - magenta + black;
  blue  = 1 - yellow + black ;

  comap = [red green blue] ;
  ncomap = size(comap,1) ;
          %  number of rows in the color map ;

  %  make sure values are positive
  %     (since twiddling "black" can leave this range)
  flag = comap < 0 ;
  fflag = find(flag) ;
  comap(fflag) = zeros(length(fflag),1) ;

  %  make sure values are less than 1
  %     (since twiddling "black" can leave this range)
  flag = comap > 1 ;
  fflag = find(flag) ;
  comap(fflag) = ones(length(fflag),1) ;

  colormap(comap) ;

      cstep = cstep / 1000 ;
          %  cstep is "step size in color scale", different for
          %  each image

      u01 = linspace(0,1,ncomap+1) ;
        lbd = -11.5 * cstep ;
        ubd = lbd + ncomap * cstep ;
      colgrid = linspace(lbd,ubd,ncomap+1) ;
          %  equally spaced grid on desired color scale

    vmout = mout(:) ;
          %  vector version for easier manipulation
          %  NOTE: interp1 seems to need vector argument
      indlo = find(vmout < lbd) ;
    vmout(indlo) = lbd * ones(length(indlo),1) ;
      indup = find(vmout > ubd) ;
    vmout(indup) = lbd * ones(length(indup),1) ;
          %  NOTE: interp1 seems to need values within grid

    colcurv = interp1(colgrid, u01, vmout) ;
          %  use this to determine vector of colors representing
          %  curvature, when plotting.  This transforms curvature 
          %  to get Keratron colors, when using comap as colormap.
  mout = reshape(colcurv,size(mout)) ;  

elseif isurf == 3 ;
          %  Then setup Keratron color map

  %  These use the "full range" Keratron data, from the email message:
  %             cocolora.msg
  %   which were copied into the ASCII file 
  %             cocolor1.dat
  %  for importing into matlab programs
  %    locurve & hicurve are upper and lower ends of the "range"
  %    for each color
  load cornea\cocolor1.dat ;
  locurve = cocolor1(:,1) ;
  hicurve = cocolor1(:,2) ;
  cyan = cocolor1(:,3) ;
  magenta = cocolor1(:,4) ;
  yellow = cocolor1(:,5) ;
  black = cocolor1(:,6) ;

  red   = 1 - cyan + black ;
  green = 1 - magenta + black;
  blue  = 1 - yellow + black ;

  comap = [red green blue] ;
  ncomap = size(comap,1) ;
          %  number of rows in the color map ;

  %  make sure values are positive
  flag = comap < 0 ;
  fflag = find(flag) ;
  comap(fflag) = zeros(length(fflag),1) ;

  %  make sure values are less than 1
  flag = comap > 1 ;
  fflag = find(flag) ;
  comap(fflag) = ones(length(fflag),1) ;

    vmout = mout(:) ;
          %  vector version for easier manipulation
          %  NOTE: interp1 seems to need vector argument
      indlo = find(vmout < 0) ;
    vmout(indlo) = 0 * ones(length(indlo),1) ;
      indup = find(vmout > max(hicurve)) ;
    vmout(indup) = max(hicurve) * ones(length(indup),1) ;
          %  NOTE: interp1 seems to need values within grid

  colormap(comap) ;

    u01 = linspace(0,1,ncomap+1) ;
    colcurv = interp1([0; hicurve], u01, vmout) ;
          %  use this to determine vector of colors representing
          %  curvature, when plotting.  This transforms curvature 
          %  to get Keratron colors, when using comap as colormap.
          %  NOTE: interp1 seems to need vector argument
  mout = reshape(colcurv,size(mout)) ;  
end ;



mout = mout .* mcrop ;
            %  Cut off part outside analysis circle

disp('Making Plot') ;


%mesh(mxgrid,mygrid,mout) ;
pcolor(mxgrid,mygrid,mout) ;
%pcolor(mxgrid,mygrid,mout<0) ;
%pcolor(mxgrid,mygrid,mhpp) ;
%pcolor(mxgrid,mygrid,(1 + mhp.^2).^(3/2)) ;
%pcolor(mxgrid,mygrid,mhp) ;
  shading flat ;
  axis('square') ;
  if itest == 0 & iimag >= 1 ;
    title([patstr surfstr]) ;
    if isurf == 2 ;   %  if heightmap - sphere
      caxis([0 1]) ;
      hold on ;
        polar((pi + pangr),pdist,'*k') ;
              %  Show Keratron center
        axis('off') ;
      hold off ;
    elseif isurf == 3 ;   %  if curvature map
      caxis([0 1]) ;
      hold on ;
        polar((pi + pangr),pdist,'*k') ;
              %  Show Keratron center
        axis('off') ;
      hold off ;
    end ;
  end ;


if autop == 1 ;
  if isurf == 2 ;
    if iimag == 1 ;
      print corec1h.ps -dpsc ;
    elseif iimag == 2 ;
      print corec2h.ps -dpsc ;
    elseif iimag == 3 ;
      print corec3h.ps -dpsc ;
    elseif iimag == 4 ;
      print corec4h.ps -dpsc ;
    elseif iimag == 5 ;
      print corec5h.ps -dpsc ;
    elseif iimag == 6 ;
      print corec6h.ps -dpsc ;
    elseif iimag == 7 ;
      print corec7h.ps -dpsc ;
    elseif iimag == 8 ;
      print corec8h.ps -dpsc ;
    elseif iimag == 9 ;
      print corec9h.ps -dpsc ;
    elseif iimag == 10 ;
      print corec10h.ps -dpsc ;
    elseif iimag == 11 ;
      print corec11h.ps -dpsc ;
    end ;
  elseif isurf == 3 ;
    if iimag == 1 ;
      print corec1c.ps -dpsc ;
    elseif iimag == 2 ;
      print corec2c.ps -dpsc ;
    elseif iimag == 3 ;
      print corec3c.ps -dpsc ;
    elseif iimag == 4 ;
      print corec4c.ps -dpsc ;
    elseif iimag == 5 ;
      print corec5c.ps -dpsc ;
    elseif iimag == 6 ;
      print corec6c.ps -dpsc ;
    elseif iimag == 7 ;
      print corec7c.ps -dpsc ;
    elseif iimag == 8 ;
      print corec8c.ps -dpsc ;
    elseif iimag == 9 ;
      print corec9c.ps -dpsc ;
    elseif iimag == 10 ;
      print corec10c.ps -dpsc ;
    elseif iimag == 11 ;
      print corec11c.ps -dpsc ;
    end ;
  end ;
end ;


