function [ht,dht,ddht] = corhpp(fm,r,th,r0) 
% CORHPP, For the cornea study, this reconstructs the "heights"
%    function H(r,theta), for the Radial Fourier Legendre basis,
%    and its first two derivatives in the radial direction
% Inputs:
%     fm  - feature matrix (rows ~ Legendre's
%                           cols ~ Fourier's, in cos-sin form
%                                    (so, odd number of these)
%                           for both: "1st entry ~ 0th coeff")
%                           
%     r   - matrix of radii of polar coordinates
%     th  - matrix of angles (in radians) of polar coordinates
%                   (same size as r)
%     r0  - radius of analysis region
% Output:
%     ht   - matrix of reconstructed surface
%     dht  - matrix of radial 1st deriv's of reconstructed surface
%     ddht - matrix of radial 2nd deriv's of reconstructed surface
%                   (same size as r)

ht = zeros(size(r)) ;
dht = zeros(size(r)) ;
ddht = zeros(size(r)) ;

ell0 = size(fm,1) - 1 ;
          %  Highest Legendre order, 
          %  i.e. number rows in feature matrix - 1
m0 = (size(fm,2) - 1) / 2 ;
          %  Highest Fourier order, 
          %  i.e. (number rows in feature matrix -1) / 2 
mpolyarg = (2 * r ./ 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 = fm(1,1) ;
for m = 1:m0 ;
  trigfact = trigfact + fm(1,2*m) .* cos(m * th) ;
  trigfact = trigfact + fm(1,2*m+1) .* sin(m * th) ;
end ;
ht = ht + trigfact .* llleg ;
%dht = dht + trigfact .* lllegp ;
%ddht = ddht + 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 = fm(2,1) ;
for m = 1:m0 ;
  trigfact = trigfact + fm(2,2*m) .* cos(m * th) ;
  trigfact = trigfact + fm(2,2*m+1) .* sin(m * th) ;
end ;
ht = ht + trigfact .* lleg ;
dht = dht + trigfact .* llegp ;
%ddht = ddht + 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 = fm(ell+1,1) ;
  for m = 1:m0 ;
    trigfact = trigfact + fm(ell+1,2*m) .* cos(m * th) ;
    trigfact = trigfact + fm(ell+1,2*m+1) .* sin(m * th) ;
  end ;
  ht = ht + trigfact .* leg ;
  dht = dht + trigfact .* legp ;
  ddht = ddht + trigfact .* legpp ;
      
  llleg = lleg ;
  lleg = leg ;
  lllegp = llegp ;
  llegp = legp ;
  lllegpp = llegpp ;
  llegpp = legpp ;
          %  Update for next run through loop
end ;
dht = dht * (2 ./ r0) ;
ddht = ddht * (2 ./ r0).^2 ;

