%%%
%%% Error function for non-linear minimization on the smoothness constraint
%%%

function [err] = errfuncperspSmooth( Selevation, S, Sest )

   inc = 0.0001; % distance (unitless)
   x0 = S(1); y0 = S(2); z0 = Selevation;
   x = Sest(1,:); y = Sest(2,:); z = Sest(3,:);
   xx = ones(5,1) * [x0-inc x0-inc/2 x0 x0+inc/2 x0+inc];
   yy = [y0-inc y0-inc/2 y0 y0+inc/2 y0+inc]' * ones(1,5);

   [xi,yi,zi] = griddata( x,y,z, x0-inc,y0, 'cubic' ); Zx(1) = zi;
   [xi,yi,zi] = griddata( x,y,z, x0-inc/2,y0, 'cubic' ); Zx(2) = zi;
   Zx(3)= z0;
   [xi,yi,zi] = griddata( x,y,z, x0+inc/2,y0, 'cubic' ); Zx(4) = zi;
   [xi,yi,zi] = griddata( x,y,z, x0+inc,y0, 'cubic' ); Zx(5) = zi;
   
   [xi,yi,zi] = griddata( x,y,z, x0, y0-inc, 'cubic' ); Zy(1) = zi;
   [xi,yi,zi] = griddata( x,y,z, x0, y0-inc/2, 'cubic' ); Zy(2) = zi;
   Zy(3)= z0;
   [xi,yi,zi] = griddata( x,y,z, x0, y0+inc/2, 'cubic' ); Zy(4) = zi;
   [xi,yi,zi] = griddata( x,y,z, x0, y0+inc, 'cubic' ); Zy(5) = zi;


   ind1 = find(isnan(Zx));
   ind2 = find(isnan(Zy));
   if( isempty(ind1) & isempty(ind2) )
      d2 = [0.236427 0.020404 -0.517610 0.020404 0.236427]; % second derivative
      zx = sum( d2 .* Zx );
      zy = sum( d2 .* Zy );
      err = zx^2 + zy^2;
   else
      err = 0;
   end