% interp1_fa( x, Y, xi, METHOD )
% -------------------------------------------------------------------------
%
%       One-dimensional data interpolation using flat aperture bin matching
%    (for full flat aperture sampled data). Grids (x and xi values) must be 
%    evenly spaced.
%
%       Same usage as Matlab's interp1.m, except for grid spacing requirements.
%    Yi = interp1( x, Y, xi ) returns a vector Yi containing elements 
%    corresponding to the elements of xi and determined by interpolation 
%    within vectors x and Y. The vector x specifies the points at which 
%    the data Y is given. If Y is a matrix, then the interpolation is 
%    performed for each column of Y and yi will be length(xi)-by-size(Y,2).
%
%
%    Yi = interp1( Y, xi ) assumes x = 1:n, where n is the length(Y) for 
%    vector Y or size( y, 1 ) for matrix Y.
%
% (Out of range values !!![should be]!!!! returned as NaNs. Now it fails.)
%
%   USEAGE:  Yi = interp1( x, Y, xi, type )
%           ??Yi = interp1( Y, xi, type )
%
%   
%
%   NOTE: Proper range for xi is x - dxi/2 + dxi/2 to x + dxi/2 - dxi/2.
%   xi = 1 - .5 + increment/2 : increment : nOriginal + .5 - increment/2;
%
%   NOTE: Currently x's must be integral, from 1:n.
%       xi not restricted to x's range (Should be limited to +- dxi/2).
%       Should distinguish between a discreetly sampled output and
%       an approximately full flat aperture sampled output.
%       
%   METHOD -  The default is linear interpolation.
%               'linear'        linear interpolation
%               'quadratic'     piecewise quadratic interpolation
%

% Mark Dow, April 1999
% Mark Dow, modified March 2007, derived from interp1_bm
% Mark Dow, modified April 11 2007, quadratic and neighbor edge conditions

function Yi = interp1_fa( x, Y, xi, METHOD )

   % First argument can be ignored:  must be 1:n where n is length of vector
   % or number of rows of matrix.  
% To Do: Fix up arguments.
   if nargin == 3
   
      METHOD = xi;
      xi = Y;
      Y = x;
      
      if size(Y,1) == 1
         x = 1:size(Y,2);
      else
         x = 1:size(Y,1);
      end
   
   end

   % Flip single vector to mimic Matlab's interpolation function behavior
   if size( Y, 1 ) == 1
      Y = Y';
   end
   
   % Sanity check: size of grid must match length of data (if Y is a
   % matrix, the number of rows). 
%    if size( Y, 1 ) ~= length(x) 
%    if size( Y, 1 ) ~= size(x,1)
%        iii = 1
%       error('\nThe length of Y (number of rows in matrix Y) does not match length of x. \n');
%       return
%    end
   
   % Output grid
   Yi = zeros( length(xi), size(Y,2) );
   
% Piecewise cubic flat aperture interpolation: 
if isequal( METHOD, 'quadratic' )

% Find control points by convolution with filter.
   
    fa_filter = [ -1/4  3/2  -1/4 ];

    % Find control points of a column by convolution with filter.
    Ymax = max(max(Y));
    Ymin = min(min(Y));
    
    yc( 2 : length(x)+1, : ) = Y;
    % Extrapolate as nearest edge point.
    yc( 1, : ) = yc( 2, : );
    yc( length(x)+2, : ) = yc( length(x)+1, : );
    yct = yc;
    error_max = (Ymax - Ymin)/10000;
    err = 99999;
    while err > error_max
        for i = 2 : length(x) + 1
            for j = 1:size(Y,2)

                yct( i, j ) = fa_filter(1)*yc( i-1, j ) + fa_filter(2)*Y( i-1, j ) + fa_filter(3)*yc( i+1, j );
            end
        end
        % Error is maximum magnitude difference.
        err = max(max(abs(yc - yct)));
        yc = yct;
    end

%     yc( 2 : size(x,1)+1, : ) = ycs;
% 
%     % Linear extrapolation of control endpoints.
% %     yc( 1, : ) = yc( 2, : ) - ( yc( 3, : ) - yc( 2, : ) );
% %     yc( length(x)+2, : ) = yc( length(x)+1, : ) - ( yc( length(x ), : ) - yc( length(x)+1, : ) );
%     yc( 1, : ) = yc( 2, : );
%     yc( size(x,1)+2, : ) = yc( size(x,1)+1, : );

    % Resample to output grid

    dxi_full = xi(2) - xi(1); 
    dxi_half = dxi_full/2;  

    for i = 1 : length(xi)

        dxi_remaining = dxi_full;

        % Get left and right increment range.
        xm_int =  floor( xi(i) - dxi_half + 1/2 + .000001 ) + 1;
        % Avoid upper bound.
        if xm_int == size( yc, 1 )
            xm_int = xm_int - 1;
        end
        xm_frc =       ( xi(i) - dxi_half ) - ( xm_int - 1 );    % [ -1/2, 1/2 ]

        xp_int =  floor( xi(i) + dxi_half + 1/2 ) + 1;
%         if xp_int == size( yc, 1 )
%             xp_int = xp_int - 1;
%         end
        xp_frc =       ( xi(i) + dxi_half ) - ( xp_int - 1 );    % [ -1/2, 1/2 ]

        if ( xm_int >= 1 ) & ( xp_int <= size( yc, 1 ) )
      
            bEnd = 0;
            while bEnd == 0
                
                a  =   -1 * yc( xm_int, : ) + ( yc( xm_int + 1, : ) + yc( xm_int - 1, : ) )/2;
                b  =                          ( yc( xm_int + 1, : ) - yc( xm_int - 1, : ) )/2;
                c  =  3/4 * yc( xm_int, : ) + ( yc( xm_int + 1, : ) + yc( xm_int - 1, : ) )/8;
         
                % If right resample point is after next sample mid-point:
                if xp_int + xp_frc - .0000001 > xm_int + 1/2

                    % If next increment is after next sample mid-point:
                    if 1/2 + xm_frc + dxi_remaining > 1
                        
                        % Integrate and update to the sample mid-point.
                        dxi = (1/2 - xm_frc);
                        
                        if dxi == 1 & i > 1
                            Yi( i, : ) =  Yi( i, : ) + c*dxi;
                        else
                            xl = xm_frc;
                            xh = xm_frc + dxi;

                            Yi( i, : ) =  Yi( i, : )...
                                        + 1/3*a*( xh^3 - xl^3 )...
                                        + 1/2*b*( xh^2 - xl^2 )...
                                        +     c*dxi;
                        end
                        
                        % Parameters for next parabolic segment.
                        xm_int = xm_int + 1;
                        xm_frc = -1/2;
                        if ( xm_int >= 1 ) & ( xp_int <= size( yc, 1 ) ) & xm_int < size( yc, 1 ) 

                            a  =   -1 * yc( xm_int, : ) + ( yc( xm_int + 1, : ) + yc( xm_int - 1, : ) )/2;
                            b  =                          ( yc( xm_int + 1, : ) - yc( xm_int - 1, : ) )/2;
                            c  =  3/4 * yc( xm_int, : ) + ( yc( xm_int + 1, : ) + yc( xm_int - 1, : ) )/8;

                            dxi_remaining = dxi_remaining - dxi;
                        else
                            
                            dxi_remaining = 0;
                            bEnd = 1;
                        end
                    
                    % If next increment is before next sample mid-point:
                    else
                        
                        % Integrate and update a full increment.
                        dxi = min( 1, dxi_remaining);
                        
                        xl = xm_frc;
                        xh = xm_frc + dxi;
                        
                        Yi( i, : ) =  Yi( i, : )...
                                    + 1/3*a*( xh^3 - xl^3 )...
                                    + 1/2*b*( xh^2 - xl^2 )...
                                    +     c*dxi;

                        xm_frc = xm_int + dxi;
                        dxi_remaining = dxi_remaining - dxi;
                    end

                % Else next resample point is before next sample point:
                else

                    dxi = dxi_remaining;            
                    xl = xm_frc;
                    xh = xm_frc + dxi;

                    Yi( i, : ) =  Yi( i, : )...
                                + 1/3*a*( xh^3 - xl^3 )...
                                + 1/2*b*( xh^2 - xl^2 )...
                                +     c*dxi;
                    dxi_remaining = 0;
                    bEnd = 1;
                end
            end 
        end
    end
   
    Yi = Yi/dxi_full;   
end


% Linear flat aperture interpolation:
if isequal( METHOD, 'linear' )

    fa_filter = [ -1/6 4/3 -1/6 ];
        
% Find control points of a column by convolution with filter.
    Ymax = max(max(Y));
    Ymin = min(min(Y));
    ycs = Y;
    yct = Y;
    error_max = (Ymax - Ymin)/100;
    err = 99999;
    while err > error_max
        for i = 2 : length(x) - 1
            for j = 1:size(Y,2)

                yct( i, j ) = fa_filter(1)*ycs( i-1, j ) + fa_filter(2)*Y( i, j ) + fa_filter(3)*ycs( i+1, j );
            end
        end
        err = max(max(abs(ycs - yct)));
        ycs = yct;
    end
    
    yc( 2 : length(x)+1, : ) = ycs;
    
    % Linear extrapolation of control endpoints.
    yc( 1, : ) = yc( 2, : ) - ( yc( 3, : ) - yc( 2, : ) );
    yc( length(x)+2, : ) = yc( length(x)+1, : ) - ( yc( length(x ), : ) - yc( length(x)+1, : ) );
    
% Resample to output grid

%    xi
%    size(xi)
   dxi_full         = xi(2) - xi(1); 
   dxi_half         = dxi_full/2;  
  
   % Initialize right edge   
%    xp_int   =  floor( xi(1) - dxi_half );
%    xp_frc  =       ( xi(1) - dxi_half ) - xp_int;
%    slope_xp =  yc( 2, : ) - yc( 1, : );
    
   for i = 1 : length(xi)
   
      dxi_remaining = dxi_full;
      
      % Get left and right increment range.
      xm_int =  floor( xi(i) - dxi_half ) + 1;
      xm_frc =       ( xi(i) - dxi_half ) - floor( xi(i) - dxi_half );
      
      xp_int =  floor( xi(i) + dxi_half ) + 1;
      xp_frc =       ( xi(i) + dxi_half ) - floor( xi(i) + dxi_half );
      
% xm_int
% xp_int
      if ( xm_int >= 1 ) & ( xp_int <= size( yc, 1 ) )
%       if ( xm_int >= 1 ) & ( xm_int <= length(x)+2 )
          
        bEnd = 0;
        while bEnd == 0
            
            slope_x = yc( xm_int + 1, : ) - yc( xm_int, : );

            % If right resample point is after next sample point:
%             if xp_int + xp_frc - .0001 > xm_int + 1
            if xp_int + xp_frc - .0001 > xm_int + 1

                % If next increment is after next sample point:
                if xm_frc + dxi_remaining > 1
                    
                    % Integrate and update to the sample point.
                    dxi = (1 - xm_frc);
                    Yi( i, : ) =  Yi( i, : )...
                                + dxi*( yc( xm_int, : ) + (xm_frc+dxi/2)*slope_x );
                    xm_int = xm_int + 1;
                    xm_frc = 0;
                    slope_x   =  yc( xm_int + 1, : ) - yc( xm_int, : );
                    dxi_remaining = dxi_remaining - dxi;
                % If next increment is before next sample point:
                else
                    dxi = min( 1, dxi_remaining);
                    Yi( i, : ) =  Yi( i, : )...
                                + dxi*( yc( xm_int, : ) + (xm_frc+dxi/2)*slope_x );
                    xm_int = floor( xm_int + dxi );
                    xm_frc = ( xm_int + dxi ) - xm_int;
                    slope_x   =  yc( xm_int + 1, : ) - yc( xm_int, : );
                    dxi_remaining = dxi_remaining - dxi;
                end
            % Else next resample point is before next sample point:
            else

                dxi = dxi_remaining;            
                Yi( i, : ) =  Yi( i, : )...
                            + dxi*( yc( xm_int, : ) + (xm_frc+dxi/2)*slope_x );
                dxi_remaining = 0;
                bEnd = 1;
            end

    %          slope_xm =  slope_x_p;
    %          slope_x0   =  yc( x0_int + 1, : ) - yc( x0_int, : );
    %          slope_xp =  yc( xp_int + 1, : ) - yc( xp_int, : );
    %          
    %          % Three point estimation of integral over interval.
    %          Yi( i, : ) =  ( yc( x0_int, : ) + x0_frc*( slope_x0   ) )/2 ...
    %                      + ( yc( xm_int, : ) + xm_frc*( slope_xm ) )/4 ...
    %                      + ( yc( xp_int, : ) + xp_frc*( slope_xp ) )/4;

        end
%       else if xm_int <= 1 
%       
%               Yi( i, :) = yc( 2, : ) ...
%                      - (2 - (x0_int + x0_frc)) ...
%                      *( yc( 2, : ) - yc( 1, : ) );
%                      
%            else if xp_int >= x( length(x) )
%             
%                    Yi( i, :) = yc( x0_int, : ) ...
%                      + x0_frc*( yc( x( length(x) ), : ) ... 
%                      - yc( x( length(x) ) - 1, : ) );
%    
%                 end
%            end
      end
      
     end
   
      Yi = Yi/dxi_full;
         
end   
   
   
   % Flip single vector to mimic Matlab's interpolation function behavior
   if size( Yi, 2 ) == 1
      Yi = Yi';
   end