%MATLAB Script File: costst1.m
%
%   For the Cornea Study, this tests legendre fit to sphere
%                               
%
format compact ;
running = 'MATLAB Script File:  costst1.m'

itest = 2 ;   %  1,2

r0 = 4.2 ;    %  Usual radius of analysis

ngrid = 27 ;

rgrid = linspace(0,r0,ngrid)' ;

spherad = 9.6 ;
sphht = spherad - sqrt(spherad.^2 - rgrid.^2) ;

%plot(rgrid,sphht) ;

%  Generate matrix of Legendre's
ell0 = 6 ;
mpolyarg = (2 * rgrid ./ r0) - 1 ;

%  Do ell = 0 Legendre
llleg = ones(size(mpolyarg)) ;
mleg = llleg ;
%  Do ell = 1 Legendre
lleg = mpolyarg ;
mleg = [mleg 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
  mleg = [mleg leg] ;
  llleg = lleg ;
  lleg = leg ;
end ;


if itest == 1 ;
  %  Do Least Squares fit of Legendre's to sphere Heights
  avgmse = [] ;
  for iplot = 0:ell0 ;
    x = mleg(:,1:iplot+1) ;
    betahat = inv(x' * x) * x' * sphht ;
    psph = x * betahat ;
    plot(rgrid,[sphht psph]) ;
      title(['Sphere and Fit, Order ' num2str(iplot)]) ;
    disp('Any key for next plot') ;
    pause ;
    plot(rgrid,sphht-psph,[0 r0],[0 0]) ;
      title(['Residuals, Order ' num2str(iplot)]) ;
    disp('Any key for next plot') ;
    pause ;
    avgmse = [avgmse mean((sphht - psph).^2)] ;
  end ;

  plot(log10(avgmse)) ;
    title('log10(avgmse)') ;

  betahat 

elseif itest == 2 ;
    x = mleg ;
    betahat = inv(x' * x) * x' * sphht ;


  mhp = zeros(size(rgrid)) ;
  mhpp = zeros(size(rgrid)) ;
  mpolyarg = (2 * rgrid ./ 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
%    mhp = mhp + betahat(1) * lllegp ;
%    mhpp = mhpp + betahat(1) * 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
    mhp = mhp + betahat(2) * llegp ;
%    mhpp = mhpp + betahat(2) * 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

      mhp = mhp + betahat(ell+1) * legp ;
      mhpp = mhpp + betahat(ell+1) * 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 ;

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

  diary costst1.out ;
    disp('From matlab script costst1.m') ;
    disp('For fitting Legendres to a sphere with:') ;
    spherad
    r0
 
    disp('Vector of Legendre coefficients is:') ;
    betahat 

    disp('Curvatures from Model fit to sphere are:') ;
    mout 
  diary off ;
end ;

