%MATLAB Script File: coltst1.m
%
%   For the Cornea Study, this 2nd deriv of Legendre reconstruction
%
format compact ;
running = 'MATLAB Script File:  coltst1.m'

itest = 3 ;    %  1,2,3

if itest == 1 ;  %  Then do simplest test with Fourier Order = 0 
  mfeat = [1; 0; 0] ;
  mfeat = [0; 1; 0] ;
  mfeat = [0; 0; 1] ;
  mfeat = [0; 0; 0; 1] ;
  mfeat = [0; 0; 0; 0; 1] ;
  ngrid = 16 ; %  Construct surfaces as ngrid x ngrid array
  r0 = 4.2 ;    %  Usual radius of analysis
  xgrid = linspace(0,r0,ngrid) ;
  ygrid = 0 ;
elseif itest == 2 ;  %  Then do numerical diff. with Fourier Order = 0 
  mfeat = [1; 0; 0] ;
  mfeat = [0; 1; 0] ;
  mfeat = [0; 0; 1] ;
  mfeat = [0; 0; 0; 1] ;
  mfeat = [0; 0; 0; 0; 1] ;
  mfeat = [0; 0; 0; 0; 0; 1] ;
  mfeat = [0; 0; 0; 0; 0; 0; 1] ;
  mfeat = [0; 0; 0; 0; 0; 0; 0; 1] ;
  mfeat = [2; -1 ; .5; -2; 1] ;
%  ngrid = 1001 ; %  Construct surfaces as ngrid x ngrid array
  ngrid = 10001 ; %  Construct surfaces as ngrid x ngrid array
  r0 = 4.2 ;    %  Usual radius of analysis
  xgrid = linspace(0,r0,ngrid) ;
  ygrid = 0 ;
elseif itest == 3 ;  %  Very small example, to study with screen writes
  mfeat = [2; -1; .5] ;
  mfeat = [[0 2 0]; [0 0 0]; [0 0 0]] ;
  mfeat = [[0 0 0]; [0 0 2]; [0 0 0]] ;
  mfeat = [[0 0 0]; [2 0 0]] ;
  ngrid = 5 ; %  Construct surfaces as ngrid x ngrid array
  r0 = 4.2 ;    %  Usual radius of analysis
  xgrid = linspace(-r0,r0,ngrid) ;
  ygrid = xgrid ;
end ;
  


%  Next lines were copied from corecon1.m


              %  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)

%mxgrid
%mygrid
%mrgrid
%mthetag
%pause


          
  mout = zeros(size(mxgrid)) ;

    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 ;
%mrgrid
%mpolyarg
%pause

    %  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 ;
trigfact

    mout = mout + trigfact .* llleg ;
mout

    %  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 ;
mout

    %  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 ;
mout
      
      llleg = lleg ;
      lleg = leg ;
              %  Update for next run through loop
    end ;




  mhp = zeros(size(mxgrid)) ;
  mhpp = zeros(size(mxgrid)) ;

    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 ;


if itest == 1 ;
  mrgrid 
  mout 
  mhp
  mhpp
  plot(mrgrid',[mout' mhp' mhpp']) ;
elseif itest == 2 ;
  disp('       rgrid     leg     legp    legpp') ;
  [min(mrgrid') min(mout') min(mhp') min(mhpp')]
  [max(mrgrid') max(mout') max(mhp') max(mhpp')]
  disp(['errors for ngrid = ' num2str(ngrid)]) ;
    nd = (mout(3:ngrid) - mout(1:ngrid-2)) / (mrgrid(3) - mrgrid(1)) ;
    ndd = (mout(3:ngrid) - 2 * mout(2:ngrid-1) + mout(1:ngrid-2)) ...
                               / (mrgrid(2) - mrgrid(1))^2 ;
    npd = (mhp(3:ngrid) - mhp(1:ngrid-2)) / (mrgrid(3) - mrgrid(1)) ;
  max(abs(nd - mhp(2:ngrid-1)))
  max(abs(ndd - mhpp(2:ngrid-1)))
  max(abs(npd - mhpp(2:ngrid-1)))
end ;

